Merge branch 'sh/nommu' into sh-latest

1612 files changed, 97813 insertions, 57957 deletions

diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index 2672c798272f..7aff6312ce7c 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -596,6 +596,13 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
 		if (ACPI_SUCCESS(status)) {
 			dev_info(root->bus->bridge,
 				"ACPI _OSC control (0x%02x) granted\n", flags);
+			if (acpi_gbl_FADT.boot_flags & ACPI_FADT_NO_ASPM) {
+				/*
+				 * We have ASPM control, but the FADT indicates
+				 * that it's unsupported. Clear it.
+				 */
+				pcie_clear_aspm(root->bus);
+			}
 		} else {
 			dev_info(root->bus->bridge,
 				"ACPI _OSC request failed (%s), "
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index cf047c406d92..6bdedd7cca2c 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -820,7 +820,7 @@ config PATA_PLATFORM
 
 config PATA_OF_PLATFORM
 	tristate "OpenFirmware platform device PATA support"
-	depends on PATA_PLATFORM && OF && OF_IRQ
+	depends on PATA_PLATFORM && OF
 	help
 	  This option enables support for generic directly connected ATA
 	  devices commonly found on embedded systems with OpenFirmware
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index cf26222a93c5..d07bf0366d99 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -52,7 +52,8 @@
 #define DRV_VERSION	"3.0"
 
 enum {
-	AHCI_PCI_BAR		= 5,
+	AHCI_PCI_BAR_STA2X11	= 0,
+	AHCI_PCI_BAR_STANDARD	= 5,
 };
 
 enum board_ids {
@@ -375,6 +376,9 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	{ PCI_VDEVICE(SI, 0x1185), board_ahci },		/* SiS 968 */
 	{ PCI_VDEVICE(SI, 0x0186), board_ahci },		/* SiS 968 */
 
+	/* ST Microelectronics */
+	{ PCI_VDEVICE(STMICRO, 0xCC06), board_ahci },		/* ST ConneXt */
+
 	/* Marvell */
 	{ PCI_VDEVICE(MARVELL, 0x6145), board_ahci_mv },	/* 6145 */
 	{ PCI_VDEVICE(MARVELL, 0x6121), board_ahci_mv },	/* 6121 */
@@ -622,6 +626,13 @@ static int ahci_configure_dma_masks(struct pci_dev *pdev, int using_dac)
 {
 	int rc;
 
+	/*
+	 * If the device fixup already set the dma_mask to some non-standard
+	 * value, don't extend it here. This happens on STA2X11, for example.
+	 */
+	if (pdev->dma_mask && pdev->dma_mask < DMA_BIT_MASK(32))
+		return 0;
+
 	if (using_dac &&
 	    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
 		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
@@ -1026,6 +1037,7 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct ahci_host_priv *hpriv;
 	struct ata_host *host;
 	int n_ports, i, rc;
+	int ahci_pci_bar = AHCI_PCI_BAR_STANDARD;
 
 	VPRINTK("ENTER\n");
 
@@ -1057,6 +1069,10 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev_info(&pdev->dev,
 			 "PDC42819 can only drive SATA devices with this driver\n");
 
+	/* The Connext uses non-standard BAR */
+	if (pdev->vendor == PCI_VENDOR_ID_STMICRO && pdev->device == 0xCC06)
+		ahci_pci_bar = AHCI_PCI_BAR_STA2X11;
+
 	/* acquire resources */
 	rc = pcim_enable_device(pdev);
 	if (rc)
@@ -1065,7 +1081,7 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* AHCI controllers often implement SFF compatible interface.
 	 * Grab all PCI BARs just in case.
 	 */
-	rc = pcim_iomap_regions_request_all(pdev, 1 << AHCI_PCI_BAR, DRV_NAME);
+	rc = pcim_iomap_regions_request_all(pdev, 1 << ahci_pci_bar, DRV_NAME);
 	if (rc == -EBUSY)
 		pcim_pin_device(pdev);
 	if (rc)
@@ -1108,7 +1124,7 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if ((hpriv->flags & AHCI_HFLAG_NO_MSI) || pci_enable_msi(pdev))
 		pci_intx(pdev, 1);
 
-	hpriv->mmio = pcim_iomap_table(pdev)[AHCI_PCI_BAR];
+	hpriv->mmio = pcim_iomap_table(pdev)[ahci_pci_bar];
 
 	/* save initial config */
 	ahci_pci_save_initial_config(pdev, hpriv);
@@ -1172,8 +1188,8 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	for (i = 0; i < host->n_ports; i++) {
 		struct ata_port *ap = host->ports[i];
 
-		ata_port_pbar_desc(ap, AHCI_PCI_BAR, -1, "abar");
-		ata_port_pbar_desc(ap, AHCI_PCI_BAR,
+		ata_port_pbar_desc(ap, ahci_pci_bar, -1, "abar");
+		ata_port_pbar_desc(ap, ahci_pci_bar,
 				   0x100 + ap->port_no * 0x80, "port");
 
 		/* set enclosure management message type */
diff --git a/drivers/ata/ahci_platform.c b/drivers/ata/ahci_platform.c
index 43b875810d1b..48be4e189163 100644
--- a/drivers/ata/ahci_platform.c
+++ b/drivers/ata/ahci_platform.c
@@ -202,6 +202,71 @@ static int __devexit ahci_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int ahci_suspend(struct device *dev)
+{
+	struct ahci_platform_data *pdata = dev_get_platdata(dev);
+	struct ata_host *host = dev_get_drvdata(dev);
+	struct ahci_host_priv *hpriv = host->private_data;
+	void __iomem *mmio = hpriv->mmio;
+	u32 ctl;
+	int rc;
+
+	if (hpriv->flags & AHCI_HFLAG_NO_SUSPEND) {
+		dev_err(dev, "firmware update required for suspend/resume\n");
+		return -EIO;
+	}
+
+	/*
+	 * AHCI spec rev1.1 section 8.3.3:
+	 * Software must disable interrupts prior to requesting a
+	 * transition of the HBA to D3 state.
+	 */
+	ctl = readl(mmio + HOST_CTL);
+	ctl &= ~HOST_IRQ_EN;
+	writel(ctl, mmio + HOST_CTL);
+	readl(mmio + HOST_CTL); /* flush */
+
+	rc = ata_host_suspend(host, PMSG_SUSPEND);
+	if (rc)
+		return rc;
+
+	if (pdata && pdata->suspend)
+		return pdata->suspend(dev);
+	return 0;
+}
+
+static int ahci_resume(struct device *dev)
+{
+	struct ahci_platform_data *pdata = dev_get_platdata(dev);
+	struct ata_host *host = dev_get_drvdata(dev);
+	int rc;
+
+	if (pdata && pdata->resume) {
+		rc = pdata->resume(dev);
+		if (rc)
+			return rc;
+	}
+
+	if (dev->power.power_state.event == PM_EVENT_SUSPEND) {
+		rc = ahci_reset_controller(host);
+		if (rc)
+			return rc;
+
+		ahci_init_controller(host);
+	}
+
+	ata_host_resume(host);
+
+	return 0;
+}
+
+static struct dev_pm_ops ahci_pm_ops = {
+	.suspend		= &ahci_suspend,
+	.resume			= &ahci_resume,
+};
+#endif
+
 static const struct of_device_id ahci_of_match[] = {
 	{ .compatible = "calxeda,hb-ahci", },
 	{},
@@ -214,6 +279,9 @@ static struct platform_driver ahci_driver = {
 		.name = "ahci",
 		.owner = THIS_MODULE,
 		.of_match_table = ahci_of_match,
+#ifdef CONFIG_PM
+		.pm = &ahci_pm_ops,
+#endif
 	},
 	.id_table	= ahci_devtype,
 };
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 3c92dbd751e0..a72bfd0ecfee 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -746,9 +746,6 @@ static void ahci_start_port(struct ata_port *ap)
 	/* enable FIS reception */
 	ahci_start_fis_rx(ap);
 
-	/* enable DMA */
-	ahci_start_engine(ap);
-
 	/* turn on LEDs */
 	if (ap->flags & ATA_FLAG_EM) {
 		ata_for_each_link(link, ap, EDGE) {
@@ -2022,7 +2019,7 @@ static int ahci_port_suspend(struct ata_port *ap, pm_message_t mesg)
 		ahci_power_down(ap);
 	else {
 		ata_port_err(ap, "%s (%d)\n", emsg, rc);
-		ahci_start_port(ap);
+		ata_port_freeze(ap);
 	}
 
 	return rc;
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index c04ad68cb602..11c9aea4f4f7 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -66,6 +66,7 @@
 #include <asm/byteorder.h>
 #include <linux/cdrom.h>
 #include <linux/ratelimit.h>
+#include <linux/pm_runtime.h>
 
 #include "libata.h"
 #include "libata-transport.h"
@@ -3248,10 +3249,10 @@ int ata_do_set_mode(struct ata_link *link, struct ata_device **r_failed_dev)
 		ata_force_xfermask(dev);
 
 		pio_mask = ata_pack_xfermask(dev->pio_mask, 0, 0);
-		dma_mask = ata_pack_xfermask(0, dev->mwdma_mask, dev->udma_mask);
 
 		if (libata_dma_mask & mode_mask)
-			dma_mask = ata_pack_xfermask(0, dev->mwdma_mask, dev->udma_mask);
+			dma_mask = ata_pack_xfermask(0, dev->mwdma_mask,
+						     dev->udma_mask);
 		else
 			dma_mask = 0;
 
@@ -5234,73 +5235,55 @@ bool ata_link_offline(struct ata_link *link)
 }
 
 #ifdef CONFIG_PM
-static int ata_host_request_pm(struct ata_host *host, pm_message_t mesg,
+static int ata_port_request_pm(struct ata_port *ap, pm_message_t mesg,
 			       unsigned int action, unsigned int ehi_flags,
 			       int wait)
 {
+	struct ata_link *link;
 	unsigned long flags;
-	int i, rc;
-
-	for (i = 0; i < host->n_ports; i++) {
-		struct ata_port *ap = host->ports[i];
-		struct ata_link *link;
+	int rc;
 
-		/* Previous resume operation might still be in
-		 * progress.  Wait for PM_PENDING to clear.
-		 */
-		if (ap->pflags & ATA_PFLAG_PM_PENDING) {
-			ata_port_wait_eh(ap);
-			WARN_ON(ap->pflags & ATA_PFLAG_PM_PENDING);
-		}
+	/* Previous resume operation might still be in
+	 * progress.  Wait for PM_PENDING to clear.
+	 */
+	if (ap->pflags & ATA_PFLAG_PM_PENDING) {
+		ata_port_wait_eh(ap);
+		WARN_ON(ap->pflags & ATA_PFLAG_PM_PENDING);
+	}
 
-		/* request PM ops to EH */
-		spin_lock_irqsave(ap->lock, flags);
+	/* request PM ops to EH */
+	spin_lock_irqsave(ap->lock, flags);
 
-		ap->pm_mesg = mesg;
-		if (wait) {
-			rc = 0;
-			ap->pm_result = &rc;
-		}
+	ap->pm_mesg = mesg;
+	if (wait) {
+		rc = 0;
+		ap->pm_result = &rc;
+	}
 
-		ap->pflags |= ATA_PFLAG_PM_PENDING;
-		ata_for_each_link(link, ap, HOST_FIRST) {
-			link->eh_info.action |= action;
-			link->eh_info.flags |= ehi_flags;
-		}
+	ap->pflags |= ATA_PFLAG_PM_PENDING;
+	ata_for_each_link(link, ap, HOST_FIRST) {
+		link->eh_info.action |= action;
+		link->eh_info.flags |= ehi_flags;
+	}
 
-		ata_port_schedule_eh(ap);
+	ata_port_schedule_eh(ap);
 
-		spin_unlock_irqrestore(ap->lock, flags);
+	spin_unlock_irqrestore(ap->lock, flags);
 
-		/* wait and check result */
-		if (wait) {
-			ata_port_wait_eh(ap);
-			WARN_ON(ap->pflags & ATA_PFLAG_PM_PENDING);
-			if (rc)
-				return rc;
-		}
+	/* wait and check result */
+	if (wait) {
+		ata_port_wait_eh(ap);
+		WARN_ON(ap->pflags & ATA_PFLAG_PM_PENDING);
 	}
 
-	return 0;
+	return rc;
 }
 
-/**
- *	ata_host_suspend - suspend host
- *	@host: host to suspend
- *	@mesg: PM message
- *
- *	Suspend @host.  Actual operation is performed by EH.  This
- *	function requests EH to perform PM operations and waits for EH
- *	to finish.
- *
- *	LOCKING:
- *	Kernel thread context (may sleep).
- *
- *	RETURNS:
- *	0 on success, -errno on failure.
- */
-int ata_host_suspend(struct ata_host *host, pm_message_t mesg)
+#define to_ata_port(d) container_of(d, struct ata_port, tdev)
+
+static int ata_port_suspend_common(struct device *dev, pm_message_t mesg)
 {
+	struct ata_port *ap = to_ata_port(dev);
 	unsigned int ehi_flags = ATA_EHI_QUIET;
 	int rc;
 
@@ -5315,31 +5298,108 @@ int ata_host_suspend(struct ata_host *host, pm_message_t mesg)
 	if (mesg.event == PM_EVENT_SUSPEND)
 		ehi_flags |= ATA_EHI_NO_AUTOPSY | ATA_EHI_NO_RECOVERY;
 
-	rc = ata_host_request_pm(host, mesg, 0, ehi_flags, 1);
-	if (rc == 0)
-		host->dev->power.power_state = mesg;
+	rc = ata_port_request_pm(ap, mesg, 0, ehi_flags, 1);
 	return rc;
 }
 
+static int ata_port_suspend(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return ata_port_suspend_common(dev, PMSG_SUSPEND);
+}
+
+static int ata_port_do_freeze(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		pm_runtime_resume(dev);
+
+	return ata_port_suspend_common(dev, PMSG_FREEZE);
+}
+
+static int ata_port_poweroff(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return ata_port_suspend_common(dev, PMSG_HIBERNATE);
+}
+
+static int ata_port_resume_common(struct device *dev)
+{
+	struct ata_port *ap = to_ata_port(dev);
+	int rc;
+
+	rc = ata_port_request_pm(ap, PMSG_ON, ATA_EH_RESET,
+		ATA_EHI_NO_AUTOPSY | ATA_EHI_QUIET, 1);
+	return rc;
+}
+
+static int ata_port_resume(struct device *dev)
+{
+	int rc;
+
+	rc = ata_port_resume_common(dev);
+	if (!rc) {
+		pm_runtime_disable(dev);
+		pm_runtime_set_active(dev);
+		pm_runtime_enable(dev);
+	}
+
+	return rc;
+}
+
+static int ata_port_runtime_idle(struct device *dev)
+{
+	return pm_runtime_suspend(dev);
+}
+
+static const struct dev_pm_ops ata_port_pm_ops = {
+	.suspend = ata_port_suspend,
+	.resume = ata_port_resume,
+	.freeze = ata_port_do_freeze,
+	.thaw = ata_port_resume,
+	.poweroff = ata_port_poweroff,
+	.restore = ata_port_resume,
+
+	.runtime_suspend = ata_port_suspend,
+	.runtime_resume = ata_port_resume_common,
+	.runtime_idle = ata_port_runtime_idle,
+};
+
+/**
+ *	ata_host_suspend - suspend host
+ *	@host: host to suspend
+ *	@mesg: PM message
+ *
+ *	Suspend @host.  Actual operation is performed by port suspend.
+ */
+int ata_host_suspend(struct ata_host *host, pm_message_t mesg)
+{
+	host->dev->power.power_state = mesg;
+	return 0;
+}
+
 /**
  *	ata_host_resume - resume host
  *	@host: host to resume
  *
- *	Resume @host.  Actual operation is performed by EH.  This
- *	function requests EH to perform PM operations and returns.
- *	Note that all resume operations are performed parallelly.
- *
- *	LOCKING:
- *	Kernel thread context (may sleep).
+ *	Resume @host.  Actual operation is performed by port resume.
  */
 void ata_host_resume(struct ata_host *host)
 {
-	ata_host_request_pm(host, PMSG_ON, ATA_EH_RESET,
-			    ATA_EHI_NO_AUTOPSY | ATA_EHI_QUIET, 0);
 	host->dev->power.power_state = PMSG_ON;
 }
 #endif
 
+struct device_type ata_port_type = {
+	.name = "ata_port",
+#ifdef CONFIG_PM
+	.pm = &ata_port_pm_ops,
+#endif
+};
+
 /**
  *	ata_dev_init - Initialize an ata_device structure
  *	@dev: Device structure to initialize
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 2a5412e7e9c1..508a60bfe5c1 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -3381,6 +3381,7 @@ int ata_scsi_add_hosts(struct ata_host *host, struct scsi_host_template *sht)
 		if (!shost)
 			goto err_alloc;
 
+		shost->eh_noresume = 1;
 		*(struct ata_port **)&shost->hostdata[0] = ap;
 		ap->scsi_host = shost;
 
@@ -3398,7 +3399,7 @@ int ata_scsi_add_hosts(struct ata_host *host, struct scsi_host_template *sht)
 		 */
 		shost->max_host_blocked = 1;
 
-		rc = scsi_add_host(ap->scsi_host, ap->host->dev);
+		rc = scsi_add_host(ap->scsi_host, &ap->tdev);
 		if (rc)
 			goto err_add;
 	}
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index 4cadfa28f940..9691dd0966d7 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -929,11 +929,11 @@ static void atapi_pio_bytes(struct ata_queued_cmd *qc)
 	bytes = (bc_hi << 8) | bc_lo;
 
 	/* shall be cleared to zero, indicating xfer of data */
-	if (unlikely(ireason & (1 << 0)))
+	if (unlikely(ireason & ATAPI_COD))
 		goto atapi_check;
 
 	/* make sure transfer direction matches expected */
-	i_write = ((ireason & (1 << 1)) == 0) ? 1 : 0;
+	i_write = ((ireason & ATAPI_IO) == 0) ? 1 : 0;
 	if (unlikely(do_write != i_write))
 		goto atapi_check;
 
diff --git a/drivers/ata/libata-transport.c b/drivers/ata/libata-transport.c
index ce9dc6207f37..9a7f0ea565df 100644
--- a/drivers/ata/libata-transport.c
+++ b/drivers/ata/libata-transport.c
@@ -32,6 +32,7 @@
 #include <linux/libata.h>
 #include <linux/hdreg.h>
 #include <linux/uaccess.h>
+#include <linux/pm_runtime.h>
 
 #include "libata.h"
 #include "libata-transport.h"
@@ -279,6 +280,7 @@ int ata_tport_add(struct device *parent,
 	struct device *dev = &ap->tdev;
 
 	device_initialize(dev);
+	dev->type = &ata_port_type;
 
 	dev->parent = get_device(parent);
 	dev->release = ata_tport_release;
@@ -289,6 +291,9 @@ int ata_tport_add(struct device *parent,
 		goto tport_err;
 	}
 
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+
 	transport_add_device(dev);
 	transport_configure_device(dev);
 
diff --git a/drivers/ata/libata.h b/drivers/ata/libata.h
index 773de97988a2..814486d35c44 100644
--- a/drivers/ata/libata.h
+++ b/drivers/ata/libata.h
@@ -58,6 +58,7 @@ extern int atapi_passthru16;
 extern int libata_fua;
 extern int libata_noacpi;
 extern int libata_allow_tpm;
+extern struct device_type ata_port_type;
 extern struct ata_link *ata_dev_phys_link(struct ata_device *dev);
 extern void ata_force_cbl(struct ata_port *ap);
 extern u64 ata_tf_to_lba(const struct ata_taskfile *tf);
diff --git a/drivers/ata/pata_arasan_cf.c b/drivers/ata/pata_arasan_cf.c
index e8574bba3ee4..048589fad2ca 100644
--- a/drivers/ata/pata_arasan_cf.c
+++ b/drivers/ata/pata_arasan_cf.c
@@ -963,17 +963,7 @@ static struct platform_driver arasan_cf_driver = {
 	},
 };
 
-static int __init arasan_cf_init(void)
-{
-	return platform_driver_register(&arasan_cf_driver);
-}
-module_init(arasan_cf_init);
-
-static void __exit arasan_cf_exit(void)
-{
-	platform_driver_unregister(&arasan_cf_driver);
-}
-module_exit(arasan_cf_exit);
+module_platform_driver(arasan_cf_driver);
 
 MODULE_AUTHOR("Viresh Kumar <viresh.kumar@st.com>");
 MODULE_DESCRIPTION("Arasan ATA Compact Flash driver");
diff --git a/drivers/ata/pata_at91.c b/drivers/ata/pata_at91.c
index a76f24a8e5db..a7d91a72ee35 100644
--- a/drivers/ata/pata_at91.c
+++ b/drivers/ata/pata_at91.c
@@ -360,7 +360,7 @@ static int __devinit pata_at91_probe(struct platform_device *pdev)
 	ap->flags |= ATA_FLAG_SLAVE_POSS;
 	ap->pio_mask = ATA_PIO4;
 
-	if (!irq) {
+	if (!gpio_is_valid(irq)) {
 		ap->flags |= ATA_FLAG_PIO_POLLING;
 		ata_port_desc(ap, "no IRQ, using PIO polling");
 	}
@@ -414,8 +414,8 @@ static int __devinit pata_at91_probe(struct platform_device *pdev)
 
 	host->private_data = info;
 
-	ret = ata_host_activate(host, irq ? gpio_to_irq(irq) : 0,
-			irq ? ata_sff_interrupt : NULL,
+	return ata_host_activate(host, gpio_is_valid(irq) ? gpio_to_irq(irq) : 0,
+			gpio_is_valid(irq) ? ata_sff_interrupt : NULL,
 			irq_flags, &pata_at91_sht);
 
 	if (!ret)
@@ -454,20 +454,7 @@ static struct platform_driver pata_at91_driver = {
 	},
 };
 
-static int __init pata_at91_init(void)
-{
-	return platform_driver_register(&pata_at91_driver);
-}
-
-static void __exit pata_at91_exit(void)
-{
-	platform_driver_unregister(&pata_at91_driver);
-}
-
-
-module_init(pata_at91_init);
-module_exit(pata_at91_exit);
-
+module_platform_driver(pata_at91_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Driver for CF in True IDE mode on AT91SAM9260 SoC");
diff --git a/drivers/ata/pata_bf54x.c b/drivers/ata/pata_bf54x.c
index bd987bb082eb..d6a4677fdf71 100644
--- a/drivers/ata/pata_bf54x.c
+++ b/drivers/ata/pata_bf54x.c
@@ -418,14 +418,6 @@ static void bfin_set_dmamode(struct ata_port *ap, struct ata_device *adev)
 					(tcyc_tdvs<<8 | tdvs));
 				ATAPI_SET_ULTRA_TIM_2(base, (tmli<<8 | tss));
 				ATAPI_SET_ULTRA_TIM_3(base, (trp<<8 | tzah));
-
-				/* Enable host ATAPI Untra DMA interrupts */
-				ATAPI_SET_INT_MASK(base,
-					ATAPI_GET_INT_MASK(base)
-					| UDMAIN_DONE_MASK
-					| UDMAOUT_DONE_MASK
-					| UDMAIN_TERM_MASK
-					| UDMAOUT_TERM_MASK);
 			}
 		}
 	}
@@ -470,10 +462,6 @@ static void bfin_set_dmamode(struct ata_port *ap, struct ata_device *adev)
 			ATAPI_SET_MULTI_TIM_0(base, (tm<<8 | td));
 			ATAPI_SET_MULTI_TIM_1(base, (tkr<<8 | tkw));
 			ATAPI_SET_MULTI_TIM_2(base, (teoc<<8 | th));
-
-			/* Enable host ATAPI Multi DMA interrupts */
-			ATAPI_SET_INT_MASK(base, ATAPI_GET_INT_MASK(base)
-				| MULTI_DONE_MASK | MULTI_TERM_MASK);
 			SSYNC();
 		}
 	}
@@ -1153,15 +1141,11 @@ static unsigned char bfin_bmdma_status(struct ata_port *ap)
 {
 	unsigned char host_stat = 0;
 	void __iomem *base = (void __iomem *)ap->ioaddr.ctl_addr;
-	unsigned short int_status = ATAPI_GET_INT_STATUS(base);
 
-	if (ATAPI_GET_STATUS(base) & (MULTI_XFER_ON|ULTRA_XFER_ON))
+	if (ATAPI_GET_STATUS(base) & (MULTI_XFER_ON | ULTRA_XFER_ON))
 		host_stat |= ATA_DMA_ACTIVE;
-	if (int_status & (MULTI_DONE_INT|UDMAIN_DONE_INT|UDMAOUT_DONE_INT|
-		ATAPI_DEV_INT))
+	if (ATAPI_GET_INT_STATUS(base) & ATAPI_DEV_INT)
 		host_stat |= ATA_DMA_INTR;
-	if (int_status & (MULTI_TERM_INT|UDMAIN_TERM_INT|UDMAOUT_TERM_INT))
-		host_stat |= ATA_DMA_ERR|ATA_DMA_INTR;
 
 	dev_dbg(ap->dev, "ATAPI: host_stat=0x%x\n", host_stat);
 
diff --git a/drivers/ata/pata_cs5536.c b/drivers/ata/pata_cs5536.c
index 628c8fae5937..7a402c75ab90 100644
--- a/drivers/ata/pata_cs5536.c
+++ b/drivers/ata/pata_cs5536.c
@@ -1,6 +1,7 @@
 /*
  * pata_cs5536.c	- CS5536 PATA for new ATA layer
  *			  (C) 2007 Martin K. Petersen <mkp@mkp.net>
+ *			  (C) 2011 Bartlomiej Zolnierkiewicz
  *
  * 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
@@ -55,24 +56,16 @@ MODULE_PARM_DESC(msr, "Force using MSR to configure IDE function (Default: 0)");
 #define DRV_VERSION	"0.0.8"
 
 enum {
-	CFG			= 0,
-	DTC			= 1,
-	CAST			= 2,
-	ETC			= 3,
-
-	MSR_IDE_BASE		= 0x51300000,
-	MSR_IDE_CFG		= (MSR_IDE_BASE + 0x10),
-	MSR_IDE_DTC		= (MSR_IDE_BASE + 0x12),
-	MSR_IDE_CAST		= (MSR_IDE_BASE + 0x13),
-	MSR_IDE_ETC		= (MSR_IDE_BASE + 0x14),
-
+	MSR_IDE_CFG		= 0x51300010,
 	PCI_IDE_CFG		= 0x40,
-	PCI_IDE_DTC		= 0x48,
-	PCI_IDE_CAST		= 0x4c,
-	PCI_IDE_ETC		= 0x50,
 
-	IDE_CFG_CHANEN		= 0x2,
-	IDE_CFG_CABLE		= 0x10000,
+	CFG			= 0,
+	DTC			= 2,
+	CAST			= 3,
+	ETC			= 4,
+
+	IDE_CFG_CHANEN		= (1 << 1),
+	IDE_CFG_CABLE		= (1 << 17) | (1 << 16),
 
 	IDE_D0_SHIFT		= 24,
 	IDE_D1_SHIFT		= 16,
@@ -84,45 +77,50 @@ enum {
 	IDE_CAST_CMD_MASK	= 0xff,
 	IDE_CAST_CMD_SHIFT	= 24,
 
-	IDE_ETC_NODMA		= 0x03,
-};
-
-static const u32 msr_reg[4] = {
-	MSR_IDE_CFG, MSR_IDE_DTC, MSR_IDE_CAST, MSR_IDE_ETC,
-};
-
-static const u8 pci_reg[4] = {
-	PCI_IDE_CFG, PCI_IDE_DTC, PCI_IDE_CAST, PCI_IDE_ETC,
+	IDE_ETC_UDMA_MASK	= 0xc0,
 };
 
-static inline int cs5536_read(struct pci_dev *pdev, int reg, u32 *val)
+static int cs5536_read(struct pci_dev *pdev, int reg, u32 *val)
 {
 	if (unlikely(use_msr)) {
 		u32 dummy __maybe_unused;
 
-		rdmsr(msr_reg[reg], *val, dummy);
+		rdmsr(MSR_IDE_CFG + reg, *val, dummy);
 		return 0;
 	}
 
-	return pci_read_config_dword(pdev, pci_reg[reg], val);
+	return pci_read_config_dword(pdev, PCI_IDE_CFG + reg * 4, val);
 }
 
-static inline int cs5536_write(struct pci_dev *pdev, int reg, int val)
+static int cs5536_write(struct pci_dev *pdev, int reg, int val)
 {
 	if (unlikely(use_msr)) {
-		wrmsr(msr_reg[reg], val, 0);
+		wrmsr(MSR_IDE_CFG + reg, val, 0);
 		return 0;
 	}
 
-	return pci_write_config_dword(pdev, pci_reg[reg], val);
+	return pci_write_config_dword(pdev, PCI_IDE_CFG + reg * 4, val);
+}
+
+static void cs5536_program_dtc(struct ata_device *adev, u8 tim)
+{
+	struct pci_dev *pdev = to_pci_dev(adev->link->ap->host->dev);
+	int dshift = adev->devno ? IDE_D1_SHIFT : IDE_D0_SHIFT;
+	u32 dtc;
+
+	cs5536_read(pdev, DTC, &dtc);
+	dtc &= ~(IDE_DRV_MASK << dshift);
+	dtc |= tim << dshift;
+	cs5536_write(pdev, DTC, dtc);
 }
 
 /**
  *	cs5536_cable_detect	-	detect cable type
  *	@ap: Port to detect on
  *
- *	Perform cable detection for ATA66 capable cable. Return a libata
- *	cable type.
+ *	Perform cable detection for ATA66 capable cable.
+ *
+ *	Returns a cable type.
  */
 
 static int cs5536_cable_detect(struct ata_port *ap)
@@ -132,7 +130,7 @@ static int cs5536_cable_detect(struct ata_port *ap)
 
 	cs5536_read(pdev, CFG, &cfg);
 
-	if (cfg & (IDE_CFG_CABLE << ap->port_no))
+	if (cfg & IDE_CFG_CABLE)
 		return ATA_CBL_PATA80;
 	else
 		return ATA_CBL_PATA40;
@@ -162,19 +160,15 @@ static void cs5536_set_piomode(struct ata_port *ap, struct ata_device *adev)
 	struct ata_device *pair = ata_dev_pair(adev);
 	int mode = adev->pio_mode - XFER_PIO_0;
 	int cmdmode = mode;
-	int dshift = adev->devno ? IDE_D1_SHIFT : IDE_D0_SHIFT;
 	int cshift = adev->devno ? IDE_CAST_D1_SHIFT : IDE_CAST_D0_SHIFT;
-	u32 dtc, cast, etc;
+	u32 cast;
 
 	if (pair)
 		cmdmode = min(mode, pair->pio_mode - XFER_PIO_0);
 
-	cs5536_read(pdev, DTC, &dtc);
-	cs5536_read(pdev, CAST, &cast);
-	cs5536_read(pdev, ETC, &etc);
+	cs5536_program_dtc(adev, drv_timings[mode]);
 
-	dtc &= ~(IDE_DRV_MASK << dshift);
-	dtc |= drv_timings[mode] << dshift;
+	cs5536_read(pdev, CAST, &cast);
 
 	cast &= ~(IDE_CAST_DRV_MASK << cshift);
 	cast |= addr_timings[mode] << cshift;
@@ -182,12 +176,7 @@ static void cs5536_set_piomode(struct ata_port *ap, struct ata_device *adev)
 	cast &= ~(IDE_CAST_CMD_MASK << IDE_CAST_CMD_SHIFT);
 	cast |= cmd_timings[cmdmode] << IDE_CAST_CMD_SHIFT;
 
-	etc &= ~(IDE_DRV_MASK << dshift);
-	etc |= IDE_ETC_NODMA << dshift;
-
-	cs5536_write(pdev, DTC, dtc);
 	cs5536_write(pdev, CAST, cast);
-	cs5536_write(pdev, ETC, etc);
 }
 
 /**
@@ -208,25 +197,21 @@ static void cs5536_set_dmamode(struct ata_port *ap, struct ata_device *adev)
 	};
 
 	struct pci_dev *pdev = to_pci_dev(ap->host->dev);
-	u32 dtc, etc;
+	u32 etc;
 	int mode = adev->dma_mode;
 	int dshift = adev->devno ? IDE_D1_SHIFT : IDE_D0_SHIFT;
 
-	if (mode >= XFER_UDMA_0) {
-		cs5536_read(pdev, ETC, &etc);
+	cs5536_read(pdev, ETC, &etc);
 
+	if (mode >= XFER_UDMA_0) {
 		etc &= ~(IDE_DRV_MASK << dshift);
 		etc |= udma_timings[mode - XFER_UDMA_0] << dshift;
-
-		cs5536_write(pdev, ETC, etc);
 	} else { /* MWDMA */
-		cs5536_read(pdev, DTC, &dtc);
-
-		dtc &= ~(IDE_DRV_MASK << dshift);
-		dtc |= mwdma_timings[mode - XFER_MW_DMA_0] << dshift;
-
-		cs5536_write(pdev, DTC, dtc);
+		etc &= ~(IDE_ETC_UDMA_MASK << dshift);
+		cs5536_program_dtc(adev, mwdma_timings[mode - XFER_MW_DMA_0]);
 	}
+
+	cs5536_write(pdev, ETC, etc);
 }
 
 static struct scsi_host_template cs5536_sht = {
diff --git a/drivers/ata/pata_imx.c b/drivers/ata/pata_imx.c
index ca9d9caedfa3..c5af97f5107b 100644
--- a/drivers/ata/pata_imx.c
+++ b/drivers/ata/pata_imx.c
@@ -235,17 +235,7 @@ static struct platform_driver pata_imx_driver = {
 	},
 };
 
-static int __init pata_imx_init(void)
-{
-	return platform_driver_register(&pata_imx_driver);
-}
-
-static void __exit pata_imx_exit(void)
-{
-	platform_driver_unregister(&pata_imx_driver);
-}
-module_init(pata_imx_init);
-module_exit(pata_imx_exit);
+module_platform_driver(pata_imx_driver);
 
 MODULE_AUTHOR("Arnaud Patard <arnaud.patard@rtp-net.org>");
 MODULE_DESCRIPTION("low-level driver for iMX PATA");
diff --git a/drivers/ata/pata_ixp4xx_cf.c b/drivers/ata/pata_ixp4xx_cf.c
index 15b64311fe0a..badb1789a918 100644
--- a/drivers/ata/pata_ixp4xx_cf.c
+++ b/drivers/ata/pata_ixp4xx_cf.c
@@ -205,21 +205,10 @@ static struct platform_driver ixp4xx_pata_platform_driver = {
 	.remove		= __devexit_p(ixp4xx_pata_remove),
 };
 
-static int __init ixp4xx_pata_init(void)
-{
-	return platform_driver_register(&ixp4xx_pata_platform_driver);
-}
-
-static void __exit ixp4xx_pata_exit(void)
-{
-	platform_driver_unregister(&ixp4xx_pata_platform_driver);
-}
+module_platform_driver(ixp4xx_pata_platform_driver);
 
 MODULE_AUTHOR("Alessandro Zummo <a.zummo@towertech.it>");
 MODULE_DESCRIPTION("low-level driver for ixp4xx Compact Flash PATA");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 MODULE_ALIAS("platform:" DRV_NAME);
-
-module_init(ixp4xx_pata_init);
-module_exit(ixp4xx_pata_exit);
diff --git a/drivers/ata/pata_mpc52xx.c b/drivers/ata/pata_mpc52xx.c
index 3e1746314f22..00748ae1a016 100644
--- a/drivers/ata/pata_mpc52xx.c
+++ b/drivers/ata/pata_mpc52xx.c
@@ -897,26 +897,7 @@ static struct platform_driver mpc52xx_ata_of_platform_driver = {
 	},
 };
 
-
-/* ======================================================================== */
-/* Module                                                                   */
-/* ======================================================================== */
-
-static int __init
-mpc52xx_ata_init(void)
-{
-	printk(KERN_INFO "ata: MPC52xx IDE/ATA libata driver\n");
-	return platform_driver_register(&mpc52xx_ata_of_platform_driver);
-}
-
-static void __exit
-mpc52xx_ata_exit(void)
-{
-	platform_driver_unregister(&mpc52xx_ata_of_platform_driver);
-}
-
-module_init(mpc52xx_ata_init);
-module_exit(mpc52xx_ata_exit);
+module_platform_driver(mpc52xx_ata_of_platform_driver);
 
 MODULE_AUTHOR("Sylvain Munaut <tnt@246tNt.com>");
 MODULE_DESCRIPTION("Freescale MPC52xx IDE/ATA libata driver");
diff --git a/drivers/ata/pata_of_platform.c b/drivers/ata/pata_of_platform.c
index 2a472c5bb7db..1654dc27e7f8 100644
--- a/drivers/ata/pata_of_platform.c
+++ b/drivers/ata/pata_of_platform.c
@@ -12,8 +12,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/of_address.h>
-#include <linux/of_irq.h>
-#include <linux/of_platform.h>
+#include <linux/platform_device.h>
 #include <linux/ata_platform.h>
 
 static int __devinit pata_of_platform_probe(struct platform_device *ofdev)
@@ -22,7 +21,7 @@ static int __devinit pata_of_platform_probe(struct platform_device *ofdev)
 	struct device_node *dn = ofdev->dev.of_node;
 	struct resource io_res;
 	struct resource ctl_res;
-	struct resource irq_res;
+	struct resource *irq_res;
 	unsigned int reg_shift = 0;
 	int pio_mode = 0;
 	int pio_mask;
@@ -51,11 +50,9 @@ static int __devinit pata_of_platform_probe(struct platform_device *ofdev)
 		}
 	}
 
-	ret = of_irq_to_resource(dn, 0, &irq_res);
-	if (!ret)
-		irq_res.start = irq_res.end = 0;
-	else
-		irq_res.flags = 0;
+	irq_res = platform_get_resource(ofdev, IORESOURCE_IRQ, 0);
+	if (irq_res)
+		irq_res->flags = 0;
 
 	prop = of_get_property(dn, "reg-shift", NULL);
 	if (prop)
@@ -75,7 +72,7 @@ static int __devinit pata_of_platform_probe(struct platform_device *ofdev)
 	pio_mask = 1 << pio_mode;
 	pio_mask |= (1 << pio_mode) - 1;
 
-	return __pata_platform_probe(&ofdev->dev, &io_res, &ctl_res, &irq_res,
+	return __pata_platform_probe(&ofdev->dev, &io_res, &ctl_res, irq_res,
 				     reg_shift, pio_mask);
 }
 
@@ -101,17 +98,7 @@ static struct platform_driver pata_of_platform_driver = {
 	.remove		= __devexit_p(pata_of_platform_remove),
 };
 
-static int __init pata_of_platform_init(void)
-{
-	return platform_driver_register(&pata_of_platform_driver);
-}
-module_init(pata_of_platform_init);
-
-static void __exit pata_of_platform_exit(void)
-{
-	platform_driver_unregister(&pata_of_platform_driver);
-}
-module_exit(pata_of_platform_exit);
+module_platform_driver(pata_of_platform_driver);
 
 MODULE_DESCRIPTION("OF-platform PATA driver");
 MODULE_AUTHOR("Anton Vorontsov <avorontsov@ru.mvista.com>");
diff --git a/drivers/ata/pata_palmld.c b/drivers/ata/pata_palmld.c
index b86d7e22595e..5ff31b68135c 100644
--- a/drivers/ata/pata_palmld.c
+++ b/drivers/ata/pata_palmld.c
@@ -132,20 +132,9 @@ static struct platform_driver palmld_pata_platform_driver = {
 	.remove		= __devexit_p(palmld_pata_remove),
 };
 
-static int __init palmld_pata_init(void)
-{
-	return platform_driver_register(&palmld_pata_platform_driver);
-}
-
-static void __exit palmld_pata_exit(void)
-{
-	platform_driver_unregister(&palmld_pata_platform_driver);
-}
+module_platform_driver(palmld_pata_platform_driver);
 
 MODULE_AUTHOR("Marek Vasut <marek.vasut@gmail.com>");
 MODULE_DESCRIPTION("PalmLD PATA driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:" DRV_NAME);
-
-module_init(palmld_pata_init);
-module_exit(palmld_pata_exit);
diff --git a/drivers/ata/pata_platform.c b/drivers/ata/pata_platform.c
index 2067308f683f..f1848aeda783 100644
--- a/drivers/ata/pata_platform.c
+++ b/drivers/ata/pata_platform.c
@@ -256,17 +256,7 @@ static struct platform_driver pata_platform_driver = {
 	},
 };
 
-static int __init pata_platform_init(void)
-{
-	return platform_driver_register(&pata_platform_driver);
-}
-
-static void __exit pata_platform_exit(void)
-{
-	platform_driver_unregister(&pata_platform_driver);
-}
-module_init(pata_platform_init);
-module_exit(pata_platform_exit);
+module_platform_driver(pata_platform_driver);
 
 module_param(pio_mask, int, 0);
 
diff --git a/drivers/ata/pata_pxa.c b/drivers/ata/pata_pxa.c
index b4ede40f8ae1..0bb0fb7b26bc 100644
--- a/drivers/ata/pata_pxa.c
+++ b/drivers/ata/pata_pxa.c
@@ -390,18 +390,7 @@ static struct platform_driver pxa_ata_driver = {
 	},
 };
 
-static int __init pxa_ata_init(void)
-{
-	return platform_driver_register(&pxa_ata_driver);
-}
-
-static void __exit pxa_ata_exit(void)
-{
-	platform_driver_unregister(&pxa_ata_driver);
-}
-
-module_init(pxa_ata_init);
-module_exit(pxa_ata_exit);
+module_platform_driver(pxa_ata_driver);
 
 MODULE_AUTHOR("Marek Vasut <marek.vasut@gmail.com>");
 MODULE_DESCRIPTION("DMA-capable driver for PATA on PXA CPU");
diff --git a/drivers/ata/pata_rb532_cf.c b/drivers/ata/pata_rb532_cf.c
index 1b9d10d9c5d9..9417101bd5ca 100644
--- a/drivers/ata/pata_rb532_cf.c
+++ b/drivers/ata/pata_rb532_cf.c
@@ -188,9 +188,6 @@ static __devexit int rb532_pata_driver_remove(struct platform_device *pdev)
 	return 0;
 }
 
-/* work with hotplug and coldplug */
-MODULE_ALIAS("platform:" DRV_NAME);
-
 static struct platform_driver rb532_pata_platform_driver = {
 	.probe		= rb532_pata_driver_probe,
 	.remove		= __devexit_p(rb532_pata_driver_remove),
@@ -200,27 +197,13 @@ static struct platform_driver rb532_pata_platform_driver = {
 	},
 };
 
-/* ------------------------------------------------------------------------ */
-
 #define DRV_INFO DRV_DESC " version " DRV_VERSION
 
-static int __init rb532_pata_module_init(void)
-{
-	printk(KERN_INFO DRV_INFO "\n");
-
-	return platform_driver_register(&rb532_pata_platform_driver);
-}
-
-static void __exit rb532_pata_module_exit(void)
-{
-	platform_driver_unregister(&rb532_pata_platform_driver);
-}
+module_platform_driver(rb532_pata_platform_driver);
 
 MODULE_AUTHOR("Gabor Juhos <juhosg at openwrt.org>");
 MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
 MODULE_DESCRIPTION(DRV_DESC);
 MODULE_VERSION(DRV_VERSION);
 MODULE_LICENSE("GPL");
-
-module_init(rb532_pata_module_init);
-module_exit(rb532_pata_module_exit);
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/ata/sata_dwc_460ex.c b/drivers/ata/sata_dwc_460ex.c
index 5c4237452f50..69f7cde49c6b 100644
--- a/drivers/ata/sata_dwc_460ex.c
+++ b/drivers/ata/sata_dwc_460ex.c
@@ -1777,18 +1777,7 @@ static struct platform_driver sata_dwc_driver = {
 	.remove = sata_dwc_remove,
 };
 
-static int __init sata_dwc_init(void)
-{
-	return platform_driver_register(&sata_dwc_driver);
-}
-
-static void __exit sata_dwc_exit(void)
-{
-	platform_driver_unregister(&sata_dwc_driver);
-}
-
-module_init(sata_dwc_init);
-module_exit(sata_dwc_exit);
+module_platform_driver(sata_dwc_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Mark Miesfeld <mmiesfeld@amcc.com>");
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index 78ae7b67b09e..5a2c95ba050a 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -1452,21 +1452,9 @@ static struct platform_driver fsl_sata_driver = {
 #endif
 };
 
-static int __init sata_fsl_init(void)
-{
-	platform_driver_register(&fsl_sata_driver);
-	return 0;
-}
-
-static void __exit sata_fsl_exit(void)
-{
-	platform_driver_unregister(&fsl_sata_driver);
-}
+module_platform_driver(fsl_sata_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Ashish Kalra, Freescale Semiconductor");
 MODULE_DESCRIPTION("Freescale 3.0Gbps SATA controller low level driver");
 MODULE_VERSION("1.10");
-
-module_init(sata_fsl_init);
-module_exit(sata_fsl_exit);
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index 0b8b8b488ee8..38950ea8398a 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -3988,7 +3988,7 @@ static int mv_create_dma_pools(struct mv_host_priv *hpriv, struct device *dev)
 }
 
 static void mv_conf_mbus_windows(struct mv_host_priv *hpriv,
-				 struct mbus_dram_target_info *dram)
+				 const struct mbus_dram_target_info *dram)
 {
 	int i;
 
@@ -3998,7 +3998,7 @@ static void mv_conf_mbus_windows(struct mv_host_priv *hpriv,
 	}
 
 	for (i = 0; i < dram->num_cs; i++) {
-		struct mbus_dram_window *cs = dram->cs + i;
+		const struct mbus_dram_window *cs = dram->cs + i;
 
 		writel(((cs->size - 1) & 0xffff0000) |
 			(cs->mbus_attr << 8) |
@@ -4019,6 +4019,7 @@ static void mv_conf_mbus_windows(struct mv_host_priv *hpriv,
 static int mv_platform_probe(struct platform_device *pdev)
 {
 	const struct mv_sata_platform_data *mv_platform_data;
+	const struct mbus_dram_target_info *dram;
 	const struct ata_port_info *ppi[] =
 	    { &mv_port_info[chip_soc], NULL };
 	struct ata_host *host;
@@ -4072,8 +4073,9 @@ static int mv_platform_probe(struct platform_device *pdev)
 	/*
 	 * (Re-)program MBUS remapping windows if we are asked to.
 	 */
-	if (mv_platform_data->dram != NULL)
-		mv_conf_mbus_windows(hpriv, mv_platform_data->dram);
+	dram = mv_mbus_dram_info();
+	if (dram)
+		mv_conf_mbus_windows(hpriv, dram);
 
 	rc = mv_create_dma_pools(hpriv, &pdev->dev);
 	if (rc)
@@ -4141,17 +4143,18 @@ static int mv_platform_suspend(struct platform_device *pdev, pm_message_t state)
 static int mv_platform_resume(struct platform_device *pdev)
 {
 	struct ata_host *host = platform_get_drvdata(pdev);
+	const struct mbus_dram_target_info *dram;
 	int ret;
 
 	if (host) {
 		struct mv_host_priv *hpriv = host->private_data;
-		const struct mv_sata_platform_data *mv_platform_data = \
-			pdev->dev.platform_data;
+
 		/*
 		 * (Re-)program MBUS remapping windows if we are asked to.
 		 */
-		if (mv_platform_data->dram != NULL)
-			mv_conf_mbus_windows(hpriv, mv_platform_data->dram);
+		dram = mv_mbus_dram_info();
+		if (dram)
+			mv_conf_mbus_windows(hpriv, dram);
 
 		/* initialize adapter */
 		ret = mv_init_host(host);
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 8069322e4c9e..37c794d31264 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -787,17 +787,14 @@ static const struct xenbus_device_id xen_blkbk_ids[] = {
 };
 
 
-static struct xenbus_driver xen_blkbk = {
-	.name = "vbd",
-	.owner = THIS_MODULE,
-	.ids = xen_blkbk_ids,
+static DEFINE_XENBUS_DRIVER(xen_blkbk, ,
 	.probe = xen_blkbk_probe,
 	.remove = xen_blkbk_remove,
 	.otherend_changed = frontend_changed
-};
+);
 
 
 int xen_blkif_xenbus_init(void)
 {
-	return xenbus_register_backend(&xen_blkbk);
+	return xenbus_register_backend(&xen_blkbk_driver);
 }
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index 7b2ec5908413..9fd3ee203b1e 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -1437,16 +1437,13 @@ static const struct xenbus_device_id blkfront_ids[] = {
 	{ "" }
 };
 
-static struct xenbus_driver blkfront = {
-	.name = "vbd",
-	.owner = THIS_MODULE,
-	.ids = blkfront_ids,
+static DEFINE_XENBUS_DRIVER(blkfront, ,
 	.probe = blkfront_probe,
 	.remove = blkfront_remove,
 	.resume = blkfront_resume,
 	.otherend_changed = blkback_changed,
 	.is_ready = blkfront_is_ready,
-};
+);
 
 static int __init xlblk_init(void)
 {
@@ -1461,7 +1458,7 @@ static int __init xlblk_init(void)
 		return -ENODEV;
 	}
 
-	ret = xenbus_register_frontend(&blkfront);
+	ret = xenbus_register_frontend(&blkfront_driver);
 	if (ret) {
 		unregister_blkdev(XENVBD_MAJOR, DEV_NAME);
 		return ret;
@@ -1474,7 +1471,7 @@ module_init(xlblk_init);
 
 static void __exit xlblk_exit(void)
 {
-	return xenbus_unregister_driver(&blkfront);
+	return xenbus_unregister_driver(&blkfront_driver);
 }
 module_exit(xlblk_exit);
 
diff --git a/drivers/block/xsysace.c b/drivers/block/xsysace.c
index fb1975d82a73..1a17e338735e 100644
--- a/drivers/block/xsysace.c
+++ b/drivers/block/xsysace.c
@@ -456,7 +456,7 @@ static inline void ace_fsm_yieldirq(struct ace_device *ace)
 {
 	dev_dbg(ace->dev, "ace_fsm_yieldirq()\n");
 
-	if (ace->irq == NO_IRQ)
+	if (!ace->irq)
 		/* No IRQ assigned, so need to poll */
 		tasklet_schedule(&ace->fsm_tasklet);
 	ace->fsm_continue_flag = 0;
@@ -1034,12 +1034,12 @@ static int __devinit ace_setup(struct ace_device *ace)
 		ACE_CTRL_DATABUFRDYIRQ | ACE_CTRL_ERRORIRQ);
 
 	/* Now we can hook up the irq handler */
-	if (ace->irq != NO_IRQ) {
+	if (ace->irq) {
 		rc = request_irq(ace->irq, ace_interrupt, 0, "systemace", ace);
 		if (rc) {
 			/* Failure - fall back to polled mode */
 			dev_err(ace->dev, "request_irq failed\n");
-			ace->irq = NO_IRQ;
+			ace->irq = 0;
 		}
 	}
 
@@ -1086,7 +1086,7 @@ static void __devexit ace_teardown(struct ace_device *ace)
 
 	tasklet_kill(&ace->fsm_tasklet);
 
-	if (ace->irq != NO_IRQ)
+	if (ace->irq)
 		free_irq(ace->irq, ace);
 
 	iounmap(ace->baseaddr);
@@ -1156,7 +1156,7 @@ static int __devinit ace_probe(struct platform_device *dev)
 	resource_size_t physaddr = 0;
 	int bus_width = ACE_BUS_WIDTH_16; /* FIXME: should not be hard coded */
 	u32 id = dev->id;
-	int irq = NO_IRQ;
+	int irq = 0;
 	int i;
 
 	dev_dbg(&dev->dev, "ace_probe(%p)\n", dev);
diff --git a/drivers/char/agp/generic.c b/drivers/char/agp/generic.c
index b072648dc3f6..17e05d1076b3 100644
--- a/drivers/char/agp/generic.c
+++ b/drivers/char/agp/generic.c
@@ -514,12 +514,12 @@ static void agp_v2_parse_one(u32 *requested_mode, u32 *bridge_agpstat, u32 *vga_
 	switch (*bridge_agpstat & 7) {
 	case 4:
 		*bridge_agpstat |= (AGPSTAT2_2X | AGPSTAT2_1X);
-		printk(KERN_INFO PFX "BIOS bug. AGP bridge claims to only support x4 rate"
+		printk(KERN_INFO PFX "BIOS bug. AGP bridge claims to only support x4 rate. "
 			"Fixing up support for x2 & x1\n");
 		break;
 	case 2:
 		*bridge_agpstat |= AGPSTAT2_1X;
-		printk(KERN_INFO PFX "BIOS bug. AGP bridge claims to only support x2 rate"
+		printk(KERN_INFO PFX "BIOS bug. AGP bridge claims to only support x2 rate. "
 			"Fixing up support for x1\n");
 		break;
 	default:
@@ -693,7 +693,7 @@ static void agp_v3_parse_one(u32 *requested_mode, u32 *bridge_agpstat, u32 *vga_
 			*bridge_agpstat &= ~(AGPSTAT3_4X | AGPSTAT3_RSVD);
 			*vga_agpstat &= ~(AGPSTAT3_4X | AGPSTAT3_RSVD);
 		} else {
-			printk(KERN_INFO PFX "Fell back to AGPx4 mode because");
+			printk(KERN_INFO PFX "Fell back to AGPx4 mode because ");
 			if (!(*bridge_agpstat & AGPSTAT3_8X)) {
 				printk(KERN_INFO PFX "bridge couldn't do x8. bridge_agpstat:%x (orig=%x)\n",
 					*bridge_agpstat, origbridge);
@@ -956,7 +956,7 @@ int agp_generic_create_gatt_table(struct agp_bridge_data *bridge)
 	bridge->driver->cache_flush();
 #ifdef CONFIG_X86
 	if (set_memory_uc((unsigned long)table, 1 << page_order))
-		printk(KERN_WARNING "Could not set GATT table memory to UC!");
+		printk(KERN_WARNING "Could not set GATT table memory to UC!\n");
 
 	bridge->gatt_table = (void *)table;
 #else
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 35309274ad68..9b3cd08cd0ed 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -3,5 +3,8 @@ config CLKDEV_LOOKUP
 	bool
 	select HAVE_CLK
 
+config HAVE_CLK_PREPARE
+	bool
+
 config HAVE_MACH_CLKDEV
 	bool
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index 9a353c2216d0..e779b434af45 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -1250,7 +1250,7 @@ static int __devinit mv_xor_probe(struct platform_device *pdev)
 
 static void
 mv_xor_conf_mbus_windows(struct mv_xor_shared_private *msp,
-			 struct mbus_dram_target_info *dram)
+			 const struct mbus_dram_target_info *dram)
 {
 	void __iomem *base = msp->xor_base;
 	u32 win_enable = 0;
@@ -1264,7 +1264,7 @@ mv_xor_conf_mbus_windows(struct mv_xor_shared_private *msp,
 	}
 
 	for (i = 0; i < dram->num_cs; i++) {
-		struct mbus_dram_window *cs = dram->cs + i;
+		const struct mbus_dram_window *cs = dram->cs + i;
 
 		writel((cs->base & 0xffff0000) |
 		       (cs->mbus_attr << 8) |
@@ -1290,7 +1290,7 @@ static struct platform_driver mv_xor_driver = {
 
 static int mv_xor_shared_probe(struct platform_device *pdev)
 {
-	struct mv_xor_platform_shared_data *msd = pdev->dev.platform_data;
+	const struct mbus_dram_target_info *dram;
 	struct mv_xor_shared_private *msp;
 	struct resource *res;
 
@@ -1323,8 +1323,9 @@ static int mv_xor_shared_probe(struct platform_device *pdev)
 	/*
 	 * (Re-)program MBUS remapping windows if we are asked to.
 	 */
-	if (msd != NULL && msd->dram != NULL)
-		mv_xor_conf_mbus_windows(msp, msd->dram);
+	dram = mv_mbus_dram_info();
+	if (dram)
+		mv_xor_conf_mbus_windows(msp, dram);
 
 	return 0;
 }
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index b4588bdd98bb..fc903c0ed234 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -334,7 +334,7 @@ static int mxs_dma_alloc_chan_resources(struct dma_chan *chan)
 			goto err_irq;
 	}
 
-	ret = clk_enable(mxs_dma->clk);
+	ret = clk_prepare_enable(mxs_dma->clk);
 	if (ret)
 		goto err_clk;
 
@@ -372,7 +372,7 @@ static void mxs_dma_free_chan_resources(struct dma_chan *chan)
 	dma_free_coherent(mxs_dma->dma_device.dev, PAGE_SIZE,
 			mxs_chan->ccw, mxs_chan->ccw_phys);
 
-	clk_disable(mxs_dma->clk);
+	clk_disable_unprepare(mxs_dma->clk);
 }
 
 static struct dma_async_tx_descriptor *mxs_dma_prep_slave_sg(
@@ -578,7 +578,7 @@ static int __init mxs_dma_init(struct mxs_dma_engine *mxs_dma)
 {
 	int ret;
 
-	ret = clk_enable(mxs_dma->clk);
+	ret = clk_prepare_enable(mxs_dma->clk);
 	if (ret)
 		goto err_out;
 
@@ -604,7 +604,7 @@ static int __init mxs_dma_init(struct mxs_dma_engine *mxs_dma)
 	writel(MXS_DMA_CHANNELS_MASK << MXS_DMA_CHANNELS,
 		mxs_dma->base + HW_APBHX_CTRL1 + MXS_SET_ADDR);
 
-	clk_disable(mxs_dma->clk);
+	clk_disable_unprepare(mxs_dma->clk);
 
 	return 0;
 
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 2d8d1b041d95..09adcfcd953e 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -19,6 +19,7 @@
 #include <linux/amba/pl330.h>
 #include <linux/pm_runtime.h>
 #include <linux/scatterlist.h>
+#include <linux/of.h>
 
 #define NR_DEFAULT_DESC	16
 
@@ -116,6 +117,9 @@ struct dma_pl330_desc {
 	struct dma_pl330_chan *pchan;
 };
 
+/* forward declaration */
+static struct amba_driver pl330_driver;
+
 static inline struct dma_pl330_chan *
 to_pchan(struct dma_chan *ch)
 {
@@ -267,6 +271,32 @@ static void dma_pl330_rqcb(void *token, enum pl330_op_err err)
 	tasklet_schedule(&pch->task);
 }
 
+bool pl330_filter(struct dma_chan *chan, void *param)
+{
+	u8 *peri_id;
+
+	if (chan->device->dev->driver != &pl330_driver.drv)
+		return false;
+
+#ifdef CONFIG_OF
+	if (chan->device->dev->of_node) {
+		const __be32 *prop_value;
+		phandle phandle;
+		struct device_node *node;
+
+		prop_value = ((struct property *)param)->value;
+		phandle = be32_to_cpup(prop_value++);
+		node = of_find_node_by_phandle(phandle);
+		return ((chan->private == node) &&
+				(chan->chan_id == be32_to_cpup(prop_value)));
+	}
+#endif
+
+	peri_id = chan->private;
+	return *peri_id == (unsigned)param;
+}
+EXPORT_SYMBOL(pl330_filter);
+
 static int pl330_alloc_chan_resources(struct dma_chan *chan)
 {
 	struct dma_pl330_chan *pch = to_pchan(chan);
@@ -497,7 +527,7 @@ pluck_desc(struct dma_pl330_dmac *pdmac)
 static struct dma_pl330_desc *pl330_get_desc(struct dma_pl330_chan *pch)
 {
 	struct dma_pl330_dmac *pdmac = pch->dmac;
-	struct dma_pl330_peri *peri = pch->chan.private;
+	u8 *peri_id = pch->chan.private;
 	struct dma_pl330_desc *desc;
 
 	/* Pluck one desc from the pool of DMAC */
@@ -522,13 +552,7 @@ static struct dma_pl330_desc *pl330_get_desc(struct dma_pl330_chan *pch)
 	desc->txd.cookie = 0;
 	async_tx_ack(&desc->txd);
 
-	if (peri) {
-		desc->req.rqtype = peri->rqtype;
-		desc->req.peri = pch->chan.chan_id;
-	} else {
-		desc->req.rqtype = MEMTOMEM;
-		desc->req.peri = 0;
-	}
+	desc->req.peri = peri_id ? pch->chan.chan_id : 0;
 
 	dma_async_tx_descriptor_init(&desc->txd, &pch->chan);
 
@@ -615,12 +639,14 @@ static struct dma_async_tx_descriptor *pl330_prep_dma_cyclic(
 	case DMA_TO_DEVICE:
 		desc->rqcfg.src_inc = 1;
 		desc->rqcfg.dst_inc = 0;
+		desc->req.rqtype = MEMTODEV;
 		src = dma_addr;
 		dst = pch->fifo_addr;
 		break;
 	case DMA_FROM_DEVICE:
 		desc->rqcfg.src_inc = 0;
 		desc->rqcfg.dst_inc = 1;
+		desc->req.rqtype = DEVTOMEM;
 		src = pch->fifo_addr;
 		dst = dma_addr;
 		break;
@@ -646,16 +672,12 @@ pl330_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dst,
 {
 	struct dma_pl330_desc *desc;
 	struct dma_pl330_chan *pch = to_pchan(chan);
-	struct dma_pl330_peri *peri = chan->private;
 	struct pl330_info *pi;
 	int burst;
 
 	if (unlikely(!pch || !len))
 		return NULL;
 
-	if (peri && peri->rqtype != MEMTOMEM)
-		return NULL;
-
 	pi = &pch->dmac->pif;
 
 	desc = __pl330_prep_dma_memcpy(pch, dst, src, len);
@@ -664,6 +686,7 @@ pl330_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dst,
 
 	desc->rqcfg.src_inc = 1;
 	desc->rqcfg.dst_inc = 1;
+	desc->req.rqtype = MEMTOMEM;
 
 	/* Select max possible burst size */
 	burst = pi->pcfg.data_bus_width / 8;
@@ -692,25 +715,14 @@ pl330_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 {
 	struct dma_pl330_desc *first, *desc = NULL;
 	struct dma_pl330_chan *pch = to_pchan(chan);
-	struct dma_pl330_peri *peri = chan->private;
 	struct scatterlist *sg;
 	unsigned long flags;
 	int i;
 	dma_addr_t addr;
 
-	if (unlikely(!pch || !sgl || !sg_len || !peri))
+	if (unlikely(!pch || !sgl || !sg_len))
 		return NULL;
 
-	/* Make sure the direction is consistent */
-	if ((direction == DMA_TO_DEVICE &&
-				peri->rqtype != MEMTODEV) ||
-			(direction == DMA_FROM_DEVICE &&
-				peri->rqtype != DEVTOMEM)) {
-		dev_err(pch->dmac->pif.dev, "%s:%d Invalid Direction\n",
-				__func__, __LINE__);
-		return NULL;
-	}
-
 	addr = pch->fifo_addr;
 
 	first = NULL;
@@ -750,11 +762,13 @@ pl330_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		if (direction == DMA_TO_DEVICE) {
 			desc->rqcfg.src_inc = 1;
 			desc->rqcfg.dst_inc = 0;
+			desc->req.rqtype = MEMTODEV;
 			fill_px(&desc->px,
 				addr, sg_dma_address(sg), sg_dma_len(sg));
 		} else {
 			desc->rqcfg.src_inc = 0;
 			desc->rqcfg.dst_inc = 1;
+			desc->req.rqtype = DEVTOMEM;
 			fill_px(&desc->px,
 				sg_dma_address(sg), addr, sg_dma_len(sg));
 		}
@@ -856,32 +870,16 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 	INIT_LIST_HEAD(&pd->channels);
 
 	/* Initialize channel parameters */
-	num_chan = max(pdat ? pdat->nr_valid_peri : 0, (u8)pi->pcfg.num_chan);
+	num_chan = max(pdat ? pdat->nr_valid_peri : (u8)pi->pcfg.num_peri,
+			(u8)pi->pcfg.num_chan);
 	pdmac->peripherals = kzalloc(num_chan * sizeof(*pch), GFP_KERNEL);
 
 	for (i = 0; i < num_chan; i++) {
 		pch = &pdmac->peripherals[i];
-		if (pdat) {
-			struct dma_pl330_peri *peri = &pdat->peri[i];
-
-			switch (peri->rqtype) {
-			case MEMTOMEM:
-				dma_cap_set(DMA_MEMCPY, pd->cap_mask);
-				break;
-			case MEMTODEV:
-			case DEVTOMEM:
-				dma_cap_set(DMA_SLAVE, pd->cap_mask);
-				dma_cap_set(DMA_CYCLIC, pd->cap_mask);
-				break;
-			default:
-				dev_err(&adev->dev, "DEVTODEV Not Supported\n");
-				continue;
-			}
-			pch->chan.private = peri;
-		} else {
-			dma_cap_set(DMA_MEMCPY, pd->cap_mask);
-			pch->chan.private = NULL;
-		}
+		if (!adev->dev.of_node)
+			pch->chan.private = pdat ? &pdat->peri_id[i] : NULL;
+		else
+			pch->chan.private = adev->dev.of_node;
 
 		INIT_LIST_HEAD(&pch->work_list);
 		spin_lock_init(&pch->lock);
@@ -894,6 +892,15 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 	}
 
 	pd->dev = &adev->dev;
+	if (pdat) {
+		pd->cap_mask = pdat->cap_mask;
+	} else {
+		dma_cap_set(DMA_MEMCPY, pd->cap_mask);
+		if (pi->pcfg.num_peri) {
+			dma_cap_set(DMA_SLAVE, pd->cap_mask);
+			dma_cap_set(DMA_CYCLIC, pd->cap_mask);
+		}
+	}
 
 	pd->device_alloc_chan_resources = pl330_alloc_chan_resources;
 	pd->device_free_chan_resources = pl330_free_chan_resources;
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 5099681cf503..573532f7553e 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -141,6 +141,12 @@ config GPIO_PL061
 	help
 	  Say yes here to support the PrimeCell PL061 GPIO device
 
+config GPIO_PXA
+	bool "PXA GPIO support"
+	depends on ARCH_PXA || ARCH_MMP
+	help
+	  Say yes here to support the PXA GPIO device
+
 config GPIO_XILINX
 	bool "Xilinx GPIO support"
 	depends on PPC_OF || MICROBLAZE
@@ -170,15 +176,6 @@ config GPIO_SCH
 	  The Intel Tunnel Creek processor has 5 GPIOs powered by the
 	  core power rail and 9 from suspend power supply.
 
-config GPIO_U300
-	bool "ST-Ericsson U300 COH 901 335/571 GPIO"
-	depends on GPIOLIB && ARCH_U300
-	help
-	  Say yes here to support GPIO interface on ST-Ericsson U300.
-	  The names of the two IP block variants supported are
-	  COH 901 335 and COH 901 571/3. They contain 3, 5 or 7
-	  ports of 8 GPIO pins each.
-
 config GPIO_VX855
 	tristate "VIA VX855/VX875 GPIO"
 	depends on PCI
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 4e018d6a7639..62e641e79e8f 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -40,7 +40,7 @@ obj-$(CONFIG_GPIO_PCA953X)	+= gpio-pca953x.o
 obj-$(CONFIG_GPIO_PCF857X)	+= gpio-pcf857x.o
 obj-$(CONFIG_GPIO_PCH)		+= gpio-pch.o
 obj-$(CONFIG_GPIO_PL061)	+= gpio-pl061.o
-obj-$(CONFIG_PLAT_PXA)		+= gpio-pxa.o
+obj-$(CONFIG_GPIO_PXA)		+= gpio-pxa.o
 obj-$(CONFIG_GPIO_RDC321X)	+= gpio-rdc321x.o
 obj-$(CONFIG_PLAT_SAMSUNG)	+= gpio-samsung.o
 obj-$(CONFIG_ARCH_SA1100)	+= gpio-sa1100.o
@@ -54,7 +54,6 @@ obj-$(CONFIG_ARCH_DAVINCI_TNETV107X) += gpio-tnetv107x.o
 obj-$(CONFIG_GPIO_TPS65910)	+= gpio-tps65910.o
 obj-$(CONFIG_GPIO_TPS65912)	+= gpio-tps65912.o
 obj-$(CONFIG_GPIO_TWL4030)	+= gpio-twl4030.o
-obj-$(CONFIG_MACH_U300)		+= gpio-u300.o
 obj-$(CONFIG_GPIO_UCB1400)	+= gpio-ucb1400.o
 obj-$(CONFIG_GPIO_VR41XX)	+= gpio-vr41xx.o
 obj-$(CONFIG_GPIO_VX855)	+= gpio-vx855.o
diff --git a/drivers/gpio/gpio-pxa.c b/drivers/gpio/gpio-pxa.c
index ee137712f9db..b2d3ee1d183a 100644
--- a/drivers/gpio/gpio-pxa.c
+++ b/drivers/gpio/gpio-pxa.c
@@ -11,14 +11,46 @@
  *  it under the terms of the GNU General Public License version 2 as
  *  published by the Free Software Foundation.
  */
+#include <linux/clk.h>
+#include <linux/err.h>
 #include <linux/gpio.h>
+#include <linux/gpio-pxa.h>
 #include <linux/init.h>
 #include <linux/irq.h>
 #include <linux/io.h>
+#include <linux/platform_device.h>
 #include <linux/syscore_ops.h>
 #include <linux/slab.h>
 
-#include <mach/gpio-pxa.h>
+/*
+ * We handle the GPIOs by banks, each bank covers up to 32 GPIOs with
+ * one set of registers. The register offsets are organized below:
+ *
+ *           GPLR    GPDR    GPSR    GPCR    GRER    GFER    GEDR
+ * BANK 0 - 0x0000  0x000C  0x0018  0x0024  0x0030  0x003C  0x0048
+ * BANK 1 - 0x0004  0x0010  0x001C  0x0028  0x0034  0x0040  0x004C
+ * BANK 2 - 0x0008  0x0014  0x0020  0x002C  0x0038  0x0044  0x0050
+ *
+ * BANK 3 - 0x0100  0x010C  0x0118  0x0124  0x0130  0x013C  0x0148
+ * BANK 4 - 0x0104  0x0110  0x011C  0x0128  0x0134  0x0140  0x014C
+ * BANK 5 - 0x0108  0x0114  0x0120  0x012C  0x0138  0x0144  0x0150
+ *
+ * NOTE:
+ *   BANK 3 is only available on PXA27x and later processors.
+ *   BANK 4 and 5 are only available on PXA935
+ */
+
+#define GPLR_OFFSET	0x00
+#define GPDR_OFFSET	0x0C
+#define GPSR_OFFSET	0x18
+#define GPCR_OFFSET	0x24
+#define GRER_OFFSET	0x30
+#define GFER_OFFSET	0x3C
+#define GEDR_OFFSET	0x48
+#define GAFR_OFFSET	0x54
+#define ED_MASK_OFFSET	0x9C	/* GPIO edge detection for AP side */
+
+#define BANK_OFF(n)	(((n) < 3) ? (n) << 2 : 0x100 + (((n) - 3) << 2))
 
 int pxa_last_gpio;
 
@@ -39,8 +71,20 @@ struct pxa_gpio_chip {
 #endif
 };
 
+enum {
+	PXA25X_GPIO = 0,
+	PXA26X_GPIO,
+	PXA27X_GPIO,
+	PXA3XX_GPIO,
+	PXA93X_GPIO,
+	MMP_GPIO = 0x10,
+	MMP2_GPIO,
+};
+
 static DEFINE_SPINLOCK(gpio_lock);
 static struct pxa_gpio_chip *pxa_gpio_chips;
+static int gpio_type;
+static void __iomem *gpio_reg_base;
 
 #define for_each_gpio_chip(i, c)			\
 	for (i = 0, c = &pxa_gpio_chips[0]; i <= pxa_last_gpio; i += 32, c++)
@@ -55,6 +99,122 @@ static inline struct pxa_gpio_chip *gpio_to_pxachip(unsigned gpio)
 	return &pxa_gpio_chips[gpio_to_bank(gpio)];
 }
 
+static inline int gpio_is_pxa_type(int type)
+{
+	return (type & MMP_GPIO) == 0;
+}
+
+static inline int gpio_is_mmp_type(int type)
+{
+	return (type & MMP_GPIO) != 0;
+}
+
+/* GPIO86/87/88/89 on PXA26x have their direction bits in PXA_GPDR(2 inverted,
+ * as well as their Alternate Function value being '1' for GPIO in GAFRx.
+ */
+static inline int __gpio_is_inverted(int gpio)
+{
+	if ((gpio_type == PXA26X_GPIO) && (gpio > 85))
+		return 1;
+	return 0;
+}
+
+/*
+ * On PXA25x and PXA27x, GAFRx and GPDRx together decide the alternate
+ * function of a GPIO, and GPDRx cannot be altered once configured. It
+ * is attributed as "occupied" here (I know this terminology isn't
+ * accurate, you are welcome to propose a better one :-)
+ */
+static inline int __gpio_is_occupied(unsigned gpio)
+{
+	struct pxa_gpio_chip *pxachip;
+	void __iomem *base;
+	unsigned long gafr = 0, gpdr = 0;
+	int ret, af = 0, dir = 0;
+
+	pxachip = gpio_to_pxachip(gpio);
+	base = gpio_chip_base(&pxachip->chip);
+	gpdr = readl_relaxed(base + GPDR_OFFSET);
+
+	switch (gpio_type) {
+	case PXA25X_GPIO:
+	case PXA26X_GPIO:
+	case PXA27X_GPIO:
+		gafr = readl_relaxed(base + GAFR_OFFSET);
+		af = (gafr >> ((gpio & 0xf) * 2)) & 0x3;
+		dir = gpdr & GPIO_bit(gpio);
+
+		if (__gpio_is_inverted(gpio))
+			ret = (af != 1) || (dir == 0);
+		else
+			ret = (af != 0) || (dir != 0);
+		break;
+	default:
+		ret = gpdr & GPIO_bit(gpio);
+		break;
+	}
+	return ret;
+}
+
+#ifdef CONFIG_ARCH_PXA
+static inline int __pxa_gpio_to_irq(int gpio)
+{
+	if (gpio_is_pxa_type(gpio_type))
+		return PXA_GPIO_TO_IRQ(gpio);
+	return -1;
+}
+
+static inline int __pxa_irq_to_gpio(int irq)
+{
+	if (gpio_is_pxa_type(gpio_type))
+		return irq - PXA_GPIO_TO_IRQ(0);
+	return -1;
+}
+#else
+static inline int __pxa_gpio_to_irq(int gpio) { return -1; }
+static inline int __pxa_irq_to_gpio(int irq) { return -1; }
+#endif
+
+#ifdef CONFIG_ARCH_MMP
+static inline int __mmp_gpio_to_irq(int gpio)
+{
+	if (gpio_is_mmp_type(gpio_type))
+		return MMP_GPIO_TO_IRQ(gpio);
+	return -1;
+}
+
+static inline int __mmp_irq_to_gpio(int irq)
+{
+	if (gpio_is_mmp_type(gpio_type))
+		return irq - MMP_GPIO_TO_IRQ(0);
+	return -1;
+}
+#else
+static inline int __mmp_gpio_to_irq(int gpio) { return -1; }
+static inline int __mmp_irq_to_gpio(int irq) { return -1; }
+#endif
+
+static int pxa_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+	int gpio, ret;
+
+	gpio = chip->base + offset;
+	ret = __pxa_gpio_to_irq(gpio);
+	if (ret >= 0)
+		return ret;
+	return __mmp_gpio_to_irq(gpio);
+}
+
+int pxa_irq_to_gpio(int irq)
+{
+	int ret;
+
+	ret = __pxa_irq_to_gpio(irq);
+	if (ret >= 0)
+		return ret;
+	return __mmp_irq_to_gpio(irq);
+}
+
 static int pxa_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
 	void __iomem *base = gpio_chip_base(chip);
@@ -63,12 +223,12 @@ static int pxa_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 
 	spin_lock_irqsave(&gpio_lock, flags);
 
-	value = __raw_readl(base + GPDR_OFFSET);
+	value = readl_relaxed(base + GPDR_OFFSET);
 	if (__gpio_is_inverted(chip->base + offset))
 		value |= mask;
 	else
 		value &= ~mask;
-	__raw_writel(value, base + GPDR_OFFSET);
+	writel_relaxed(value, base + GPDR_OFFSET);
 
 	spin_unlock_irqrestore(&gpio_lock, flags);
 	return 0;
@@ -81,16 +241,16 @@ static int pxa_gpio_direction_output(struct gpio_chip *chip,
 	uint32_t tmp, mask = 1 << offset;
 	unsigned long flags;
 
-	__raw_writel(mask, base + (value ? GPSR_OFFSET : GPCR_OFFSET));
+	writel_relaxed(mask, base + (value ? GPSR_OFFSET : GPCR_OFFSET));
 
 	spin_lock_irqsave(&gpio_lock, flags);
 
-	tmp = __raw_readl(base + GPDR_OFFSET);
+	tmp = readl_relaxed(base + GPDR_OFFSET);
 	if (__gpio_is_inverted(chip->base + offset))
 		tmp &= ~mask;
 	else
 		tmp |= mask;
-	__raw_writel(tmp, base + GPDR_OFFSET);
+	writel_relaxed(tmp, base + GPDR_OFFSET);
 
 	spin_unlock_irqrestore(&gpio_lock, flags);
 	return 0;
@@ -98,16 +258,16 @@ static int pxa_gpio_direction_output(struct gpio_chip *chip,
 
 static int pxa_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	return __raw_readl(gpio_chip_base(chip) + GPLR_OFFSET) & (1 << offset);
+	return readl_relaxed(gpio_chip_base(chip) + GPLR_OFFSET) & (1 << offset);
 }
 
 static void pxa_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	__raw_writel(1 << offset, gpio_chip_base(chip) +
+	writel_relaxed(1 << offset, gpio_chip_base(chip) +
 				(value ? GPSR_OFFSET : GPCR_OFFSET));
 }
 
-static int __init pxa_init_gpio_chip(int gpio_end)
+static int __devinit pxa_init_gpio_chip(int gpio_end)
 {
 	int i, gpio, nbanks = gpio_to_bank(gpio_end) + 1;
 	struct pxa_gpio_chip *chips;
@@ -122,7 +282,7 @@ static int __init pxa_init_gpio_chip(int gpio_end)
 		struct gpio_chip *c = &chips[i].chip;
 
 		sprintf(chips[i].label, "gpio-%d", i);
-		chips[i].regbase = GPIO_BANK(i);
+		chips[i].regbase = gpio_reg_base + BANK_OFF(i);
 
 		c->base  = gpio;
 		c->label = chips[i].label;
@@ -131,6 +291,7 @@ static int __init pxa_init_gpio_chip(int gpio_end)
 		c->direction_output = pxa_gpio_direction_output;
 		c->get = pxa_gpio_get;
 		c->set = pxa_gpio_set;
+		c->to_irq = pxa_gpio_to_irq;
 
 		/* number of GPIOs on last bank may be less than 32 */
 		c->ngpio = (gpio + 31 > gpio_end) ? (gpio_end - gpio + 1) : 32;
@@ -147,18 +308,18 @@ static inline void update_edge_detect(struct pxa_gpio_chip *c)
 {
 	uint32_t grer, gfer;
 
-	grer = __raw_readl(c->regbase + GRER_OFFSET) & ~c->irq_mask;
-	gfer = __raw_readl(c->regbase + GFER_OFFSET) & ~c->irq_mask;
+	grer = readl_relaxed(c->regbase + GRER_OFFSET) & ~c->irq_mask;
+	gfer = readl_relaxed(c->regbase + GFER_OFFSET) & ~c->irq_mask;
 	grer |= c->irq_edge_rise & c->irq_mask;
 	gfer |= c->irq_edge_fall & c->irq_mask;
-	__raw_writel(grer, c->regbase + GRER_OFFSET);
-	__raw_writel(gfer, c->regbase + GFER_OFFSET);
+	writel_relaxed(grer, c->regbase + GRER_OFFSET);
+	writel_relaxed(gfer, c->regbase + GFER_OFFSET);
 }
 
 static int pxa_gpio_irq_type(struct irq_data *d, unsigned int type)
 {
 	struct pxa_gpio_chip *c;
-	int gpio = irq_to_gpio(d->irq);
+	int gpio = pxa_irq_to_gpio(d->irq);
 	unsigned long gpdr, mask = GPIO_bit(gpio);
 
 	c = gpio_to_pxachip(gpio);
@@ -176,12 +337,12 @@ static int pxa_gpio_irq_type(struct irq_data *d, unsigned int type)
 		type = IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING;
 	}
 
-	gpdr = __raw_readl(c->regbase + GPDR_OFFSET);
+	gpdr = readl_relaxed(c->regbase + GPDR_OFFSET);
 
 	if (__gpio_is_inverted(gpio))
-		__raw_writel(gpdr | mask,  c->regbase + GPDR_OFFSET);
+		writel_relaxed(gpdr | mask,  c->regbase + GPDR_OFFSET);
 	else
-		__raw_writel(gpdr & ~mask, c->regbase + GPDR_OFFSET);
+		writel_relaxed(gpdr & ~mask, c->regbase + GPDR_OFFSET);
 
 	if (type & IRQ_TYPE_EDGE_RISING)
 		c->irq_edge_rise |= mask;
@@ -212,9 +373,9 @@ static void pxa_gpio_demux_handler(unsigned int irq, struct irq_desc *desc)
 		for_each_gpio_chip(gpio, c) {
 			gpio_base = c->chip.base;
 
-			gedr = __raw_readl(c->regbase + GEDR_OFFSET);
+			gedr = readl_relaxed(c->regbase + GEDR_OFFSET);
 			gedr = gedr & c->irq_mask;
-			__raw_writel(gedr, c->regbase + GEDR_OFFSET);
+			writel_relaxed(gedr, c->regbase + GEDR_OFFSET);
 
 			n = find_first_bit(&gedr, BITS_PER_LONG);
 			while (n < BITS_PER_LONG) {
@@ -229,29 +390,29 @@ static void pxa_gpio_demux_handler(unsigned int irq, struct irq_desc *desc)
 
 static void pxa_ack_muxed_gpio(struct irq_data *d)
 {
-	int gpio = irq_to_gpio(d->irq);
+	int gpio = pxa_irq_to_gpio(d->irq);
 	struct pxa_gpio_chip *c = gpio_to_pxachip(gpio);
 
-	__raw_writel(GPIO_bit(gpio), c->regbase + GEDR_OFFSET);
+	writel_relaxed(GPIO_bit(gpio), c->regbase + GEDR_OFFSET);
 }
 
 static void pxa_mask_muxed_gpio(struct irq_data *d)
 {
-	int gpio = irq_to_gpio(d->irq);
+	int gpio = pxa_irq_to_gpio(d->irq);
 	struct pxa_gpio_chip *c = gpio_to_pxachip(gpio);
 	uint32_t grer, gfer;
 
 	c->irq_mask &= ~GPIO_bit(gpio);
 
-	grer = __raw_readl(c->regbase + GRER_OFFSET) & ~GPIO_bit(gpio);
-	gfer = __raw_readl(c->regbase + GFER_OFFSET) & ~GPIO_bit(gpio);
-	__raw_writel(grer, c->regbase + GRER_OFFSET);
-	__raw_writel(gfer, c->regbase + GFER_OFFSET);
+	grer = readl_relaxed(c->regbase + GRER_OFFSET) & ~GPIO_bit(gpio);
+	gfer = readl_relaxed(c->regbase + GFER_OFFSET) & ~GPIO_bit(gpio);
+	writel_relaxed(grer, c->regbase + GRER_OFFSET);
+	writel_relaxed(gfer, c->regbase + GFER_OFFSET);
 }
 
 static void pxa_unmask_muxed_gpio(struct irq_data *d)
 {
-	int gpio = irq_to_gpio(d->irq);
+	int gpio = pxa_irq_to_gpio(d->irq);
 	struct pxa_gpio_chip *c = gpio_to_pxachip(gpio);
 
 	c->irq_mask |= GPIO_bit(gpio);
@@ -266,34 +427,143 @@ static struct irq_chip pxa_muxed_gpio_chip = {
 	.irq_set_type	= pxa_gpio_irq_type,
 };
 
-void __init pxa_init_gpio(int mux_irq, int start, int end, set_wake_t fn)
+static int pxa_gpio_nums(void)
 {
-	struct pxa_gpio_chip *c;
-	int gpio, irq;
+	int count = 0;
+
+#ifdef CONFIG_ARCH_PXA
+	if (cpu_is_pxa25x()) {
+#ifdef CONFIG_CPU_PXA26x
+		count = 89;
+		gpio_type = PXA26X_GPIO;
+#elif defined(CONFIG_PXA25x)
+		count = 84;
+		gpio_type = PXA26X_GPIO;
+#endif /* CONFIG_CPU_PXA26x */
+	} else if (cpu_is_pxa27x()) {
+		count = 120;
+		gpio_type = PXA27X_GPIO;
+	} else if (cpu_is_pxa93x() || cpu_is_pxa95x()) {
+		count = 191;
+		gpio_type = PXA93X_GPIO;
+	} else if (cpu_is_pxa3xx()) {
+		count = 127;
+		gpio_type = PXA3XX_GPIO;
+	}
+#endif /* CONFIG_ARCH_PXA */
+
+#ifdef CONFIG_ARCH_MMP
+	if (cpu_is_pxa168() || cpu_is_pxa910()) {
+		count = 127;
+		gpio_type = MMP_GPIO;
+	} else if (cpu_is_mmp2()) {
+		count = 191;
+		gpio_type = MMP2_GPIO;
+	}
+#endif /* CONFIG_ARCH_MMP */
+	return count;
+}
 
-	pxa_last_gpio = end;
+static int __devinit pxa_gpio_probe(struct platform_device *pdev)
+{
+	struct pxa_gpio_chip *c;
+	struct resource *res;
+	struct clk *clk;
+	int gpio, irq, ret;
+	int irq0 = 0, irq1 = 0, irq_mux, gpio_offset = 0;
+
+	pxa_last_gpio = pxa_gpio_nums();
+	if (!pxa_last_gpio)
+		return -EINVAL;
+
+	irq0 = platform_get_irq_byname(pdev, "gpio0");
+	irq1 = platform_get_irq_byname(pdev, "gpio1");
+	irq_mux = platform_get_irq_byname(pdev, "gpio_mux");
+	if ((irq0 > 0 && irq1 <= 0) || (irq0 <= 0 && irq1 > 0)
+		|| (irq_mux <= 0))
+		return -EINVAL;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -EINVAL;
+	gpio_reg_base = ioremap(res->start, resource_size(res));
+	if (!gpio_reg_base)
+		return -EINVAL;
+
+	if (irq0 > 0)
+		gpio_offset = 2;
+
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "Error %ld to get gpio clock\n",
+			PTR_ERR(clk));
+		iounmap(gpio_reg_base);
+		return PTR_ERR(clk);
+	}
+	ret = clk_prepare(clk);
+	if (ret) {
+		clk_put(clk);
+		iounmap(gpio_reg_base);
+		return ret;
+	}
+	ret = clk_enable(clk);
+	if (ret) {
+		clk_unprepare(clk);
+		clk_put(clk);
+		iounmap(gpio_reg_base);
+		return ret;
+	}
 
 	/* Initialize GPIO chips */
-	pxa_init_gpio_chip(end);
+	pxa_init_gpio_chip(pxa_last_gpio);
 
 	/* clear all GPIO edge detects */
 	for_each_gpio_chip(gpio, c) {
-		__raw_writel(0, c->regbase + GFER_OFFSET);
-		__raw_writel(0, c->regbase + GRER_OFFSET);
-		__raw_writel(~0,c->regbase + GEDR_OFFSET);
+		writel_relaxed(0, c->regbase + GFER_OFFSET);
+		writel_relaxed(0, c->regbase + GRER_OFFSET);
+		writel_relaxed(~0,c->regbase + GEDR_OFFSET);
+		/* unmask GPIO edge detect for AP side */
+		if (gpio_is_mmp_type(gpio_type))
+			writel_relaxed(~0, c->regbase + ED_MASK_OFFSET);
 	}
 
-	for (irq  = gpio_to_irq(start); irq <= gpio_to_irq(end); irq++) {
+#ifdef CONFIG_ARCH_PXA
+	irq = gpio_to_irq(0);
+	irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
+				 handle_edge_irq);
+	set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
+	irq_set_chained_handler(IRQ_GPIO0, pxa_gpio_demux_handler);
+
+	irq = gpio_to_irq(1);
+	irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
+				 handle_edge_irq);
+	set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
+	irq_set_chained_handler(IRQ_GPIO1, pxa_gpio_demux_handler);
+#endif
+
+	for (irq  = gpio_to_irq(gpio_offset);
+		irq <= gpio_to_irq(pxa_last_gpio); irq++) {
 		irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
 					 handle_edge_irq);
 		set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
 	}
 
-	/* Install handler for GPIO>=2 edge detect interrupts */
-	irq_set_chained_handler(mux_irq, pxa_gpio_demux_handler);
-	pxa_muxed_gpio_chip.irq_set_wake = fn;
+	irq_set_chained_handler(irq_mux, pxa_gpio_demux_handler);
+	return 0;
 }
 
+static struct platform_driver pxa_gpio_driver = {
+	.probe		= pxa_gpio_probe,
+	.driver		= {
+		.name	= "pxa-gpio",
+	},
+};
+
+static int __init pxa_gpio_init(void)
+{
+	return platform_driver_register(&pxa_gpio_driver);
+}
+postcore_initcall(pxa_gpio_init);
+
 #ifdef CONFIG_PM
 static int pxa_gpio_suspend(void)
 {
@@ -301,13 +571,13 @@ static int pxa_gpio_suspend(void)
 	int gpio;
 
 	for_each_gpio_chip(gpio, c) {
-		c->saved_gplr = __raw_readl(c->regbase + GPLR_OFFSET);
-		c->saved_gpdr = __raw_readl(c->regbase + GPDR_OFFSET);
-		c->saved_grer = __raw_readl(c->regbase + GRER_OFFSET);
-		c->saved_gfer = __raw_readl(c->regbase + GFER_OFFSET);
+		c->saved_gplr = readl_relaxed(c->regbase + GPLR_OFFSET);
+		c->saved_gpdr = readl_relaxed(c->regbase + GPDR_OFFSET);
+		c->saved_grer = readl_relaxed(c->regbase + GRER_OFFSET);
+		c->saved_gfer = readl_relaxed(c->regbase + GFER_OFFSET);
 
 		/* Clear GPIO transition detect bits */
-		__raw_writel(0xffffffff, c->regbase + GEDR_OFFSET);
+		writel_relaxed(0xffffffff, c->regbase + GEDR_OFFSET);
 	}
 	return 0;
 }
@@ -319,12 +589,12 @@ static void pxa_gpio_resume(void)
 
 	for_each_gpio_chip(gpio, c) {
 		/* restore level with set/clear */
-		__raw_writel( c->saved_gplr, c->regbase + GPSR_OFFSET);
-		__raw_writel(~c->saved_gplr, c->regbase + GPCR_OFFSET);
+		writel_relaxed( c->saved_gplr, c->regbase + GPSR_OFFSET);
+		writel_relaxed(~c->saved_gplr, c->regbase + GPCR_OFFSET);
 
-		__raw_writel(c->saved_grer, c->regbase + GRER_OFFSET);
-		__raw_writel(c->saved_gfer, c->regbase + GFER_OFFSET);
-		__raw_writel(c->saved_gpdr, c->regbase + GPDR_OFFSET);
+		writel_relaxed(c->saved_grer, c->regbase + GRER_OFFSET);
+		writel_relaxed(c->saved_gfer, c->regbase + GFER_OFFSET);
+		writel_relaxed(c->saved_gpdr, c->regbase + GPDR_OFFSET);
 	}
 }
 #else
@@ -336,3 +606,10 @@ struct syscore_ops pxa_gpio_syscore_ops = {
 	.suspend	= pxa_gpio_suspend,
 	.resume		= pxa_gpio_resume,
 };
+
+static int __init pxa_gpio_sysinit(void)
+{
+	register_syscore_ops(&pxa_gpio_syscore_ops);
+	return 0;
+}
+postcore_initcall(pxa_gpio_sysinit);
diff --git a/drivers/gpio/gpio-samsung.c b/drivers/gpio/gpio-samsung.c
index ab098ba9f1dd..a7661773c052 100644
--- a/drivers/gpio/gpio-samsung.c
+++ b/drivers/gpio/gpio-samsung.c
@@ -24,6 +24,9 @@
 #include <linux/interrupt.h>
 #include <linux/device.h>
 #include <linux/ioport.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/of_address.h>
 
 #include <asm/irq.h>
 
@@ -2383,6 +2386,63 @@ static struct samsung_gpio_chip exynos4_gpios_3[] = {
 #endif
 };
 
+#if defined(CONFIG_ARCH_EXYNOS4) && defined(CONFIG_OF)
+static int exynos4_gpio_xlate(struct gpio_chip *gc, struct device_node *np,
+			      const void *gpio_spec, u32 *flags)
+{
+	const __be32 *gpio = gpio_spec;
+	const u32 n = be32_to_cpup(gpio);
+	unsigned int pin = gc->base + be32_to_cpu(gpio[0]);
+
+	if (WARN_ON(gc->of_gpio_n_cells < 4))
+		return -EINVAL;
+
+	if (n > gc->ngpio)
+		return -EINVAL;
+
+	if (s3c_gpio_cfgpin(pin, S3C_GPIO_SFN(be32_to_cpu(gpio[1]))))
+		pr_warn("gpio_xlate: failed to set pin function\n");
+	if (s3c_gpio_setpull(pin, be32_to_cpu(gpio[2])))
+		pr_warn("gpio_xlate: failed to set pin pull up/down\n");
+	if (s5p_gpio_set_drvstr(pin, be32_to_cpu(gpio[3])))
+		pr_warn("gpio_xlate: failed to set pin drive strength\n");
+
+	return n;
+}
+
+static const struct of_device_id exynos4_gpio_dt_match[] __initdata = {
+	{ .compatible = "samsung,exynos4-gpio", },
+	{}
+};
+
+static __init void exynos4_gpiolib_attach_ofnode(struct samsung_gpio_chip *chip,
+						 u64 base, u64 offset)
+{
+	struct gpio_chip *gc =  &chip->chip;
+	u64 address;
+
+	if (!of_have_populated_dt())
+		return;
+
+	address = chip->base ? base + ((u32)chip->base & 0xfff) : base + offset;
+	gc->of_node = of_find_matching_node_by_address(NULL,
+			exynos4_gpio_dt_match, address);
+	if (!gc->of_node) {
+		pr_info("gpio: device tree node not found for gpio controller"
+			" with base address %08llx\n", address);
+		return;
+	}
+	gc->of_gpio_n_cells = 4;
+	gc->of_xlate = exynos4_gpio_xlate;
+}
+#elif defined(CONFIG_ARCH_EXYNOS4)
+static __init void exynos4_gpiolib_attach_ofnode(struct samsung_gpio_chip *chip,
+						 u64 base, u64 offset)
+{
+	return;
+}
+#endif /* defined(CONFIG_ARCH_EXYNOS4) && defined(CONFIG_OF) */
+
 /* TODO: cleanup soc_is_* */
 static __init int samsung_gpiolib_init(void)
 {
@@ -2464,6 +2524,10 @@ static __init int samsung_gpiolib_init(void)
 				chip->config = &exynos4_gpio_cfg;
 				chip->group = group++;
 			}
+#ifdef CONFIG_CPU_EXYNOS4210
+			exynos4_gpiolib_attach_ofnode(chip,
+					EXYNOS4_PA_GPIO1, i * 0x20);
+#endif
 		}
 		samsung_gpiolib_add_4bit_chips(exynos4_gpios_1, nr_chips, S5P_VA_GPIO1);
 
@@ -2476,6 +2540,10 @@ static __init int samsung_gpiolib_init(void)
 				chip->config = &exynos4_gpio_cfg;
 				chip->group = group++;
 			}
+#ifdef CONFIG_CPU_EXYNOS4210
+			exynos4_gpiolib_attach_ofnode(chip,
+					EXYNOS4_PA_GPIO2, i * 0x20);
+#endif
 		}
 		samsung_gpiolib_add_4bit_chips(exynos4_gpios_2, nr_chips, S5P_VA_GPIO2);
 
@@ -2488,6 +2556,10 @@ static __init int samsung_gpiolib_init(void)
 				chip->config = &exynos4_gpio_cfg;
 				chip->group = group++;
 			}
+#ifdef CONFIG_CPU_EXYNOS4210
+			exynos4_gpiolib_attach_ofnode(chip,
+					EXYNOS4_PA_GPIO3, i * 0x20);
+#endif
 		}
 		samsung_gpiolib_add_4bit_chips(exynos4_gpios_3, nr_chips, S5P_VA_GPIO3);
 
diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
index 1368826ef284..2418429a9836 100644
--- a/drivers/gpu/drm/Kconfig
+++ b/drivers/gpu/drm/Kconfig
@@ -162,3 +162,6 @@ config DRM_SAVAGE
 source "drivers/gpu/drm/exynos/Kconfig"
 
 source "drivers/gpu/drm/vmwgfx/Kconfig"
+
+source "drivers/gpu/drm/gma500/Kconfig"
+
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index c0496f660707..0cde1b80fdb1 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -9,7 +9,7 @@ drm-y       :=	drm_auth.o drm_buffer.o drm_bufs.o drm_cache.o \
 		drm_drv.o drm_fops.o drm_gem.o drm_ioctl.o drm_irq.o \
 		drm_lock.o drm_memory.o drm_proc.o drm_stub.o drm_vm.o \
 		drm_agpsupport.o drm_scatter.o ati_pcigart.o drm_pci.o \
-		drm_platform.o drm_sysfs.o drm_hashtab.o drm_sman.o drm_mm.o \
+		drm_platform.o drm_sysfs.o drm_hashtab.o drm_mm.o \
 		drm_crtc.o drm_modes.o drm_edid.o \
 		drm_info.o drm_debugfs.o drm_encoder_slave.o \
 		drm_trace_points.o drm_global.o drm_usb.o
@@ -36,4 +36,5 @@ obj-$(CONFIG_DRM_VMWGFX)+= vmwgfx/
 obj-$(CONFIG_DRM_VIA)	+=via/
 obj-$(CONFIG_DRM_NOUVEAU) +=nouveau/
 obj-$(CONFIG_DRM_EXYNOS) +=exynos/
+obj-$(CONFIG_DRM_GMA500) += gma500/
 obj-y			+= i2c/
diff --git a/drivers/gpu/drm/drm_context.c b/drivers/gpu/drm/drm_context.c
index 6d440fb894cf..325365f6d355 100644
--- a/drivers/gpu/drm/drm_context.c
+++ b/drivers/gpu/drm/drm_context.c
@@ -154,8 +154,6 @@ int drm_getsareactx(struct drm_device *dev, void *data,
 		return -EINVAL;
 	}
 
-	mutex_unlock(&dev->struct_mutex);
-
 	request->handle = NULL;
 	list_for_each_entry(_entry, &dev->maplist, head) {
 		if (_entry->map == map) {
@@ -164,6 +162,9 @@ int drm_getsareactx(struct drm_device *dev, void *data,
 			break;
 		}
 	}
+
+	mutex_unlock(&dev->struct_mutex);
+
 	if (request->handle == NULL)
 		return -EINVAL;
 
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index 8323fc389840..5e818a808ace 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -36,6 +36,7 @@
 #include "drmP.h"
 #include "drm_crtc.h"
 #include "drm_edid.h"
+#include "drm_fourcc.h"
 
 struct drm_prop_enum_list {
 	int type;
@@ -324,6 +325,7 @@ void drm_framebuffer_cleanup(struct drm_framebuffer *fb)
 {
 	struct drm_device *dev = fb->dev;
 	struct drm_crtc *crtc;
+	struct drm_plane *plane;
 	struct drm_mode_set set;
 	int ret;
 
@@ -340,6 +342,18 @@ void drm_framebuffer_cleanup(struct drm_framebuffer *fb)
 		}
 	}
 
+	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
+		if (plane->fb == fb) {
+			/* should turn off the crtc */
+			ret = plane->funcs->disable_plane(plane);
+			if (ret)
+				DRM_ERROR("failed to disable plane with busy fb\n");
+			/* disconnect the plane from the fb and crtc: */
+			plane->fb = NULL;
+			plane->crtc = NULL;
+		}
+	}
+
 	drm_mode_object_put(dev, &fb->base);
 	list_del(&fb->head);
 	dev->mode_config.num_fb--;
@@ -540,6 +554,63 @@ void drm_encoder_cleanup(struct drm_encoder *encoder)
 }
 EXPORT_SYMBOL(drm_encoder_cleanup);
 
+int drm_plane_init(struct drm_device *dev, struct drm_plane *plane,
+		   unsigned long possible_crtcs,
+		   const struct drm_plane_funcs *funcs,
+		   const uint32_t *formats, uint32_t format_count,
+		   bool priv)
+{
+	mutex_lock(&dev->mode_config.mutex);
+
+	plane->dev = dev;
+	drm_mode_object_get(dev, &plane->base, DRM_MODE_OBJECT_PLANE);
+	plane->funcs = funcs;
+	plane->format_types = kmalloc(sizeof(uint32_t) * format_count,
+				      GFP_KERNEL);
+	if (!plane->format_types) {
+		DRM_DEBUG_KMS("out of memory when allocating plane\n");
+		drm_mode_object_put(dev, &plane->base);
+		mutex_unlock(&dev->mode_config.mutex);
+		return -ENOMEM;
+	}
+
+	memcpy(plane->format_types, formats, format_count * sizeof(uint32_t));
+	plane->format_count = format_count;
+	plane->possible_crtcs = possible_crtcs;
+
+	/* private planes are not exposed to userspace, but depending on
+	 * display hardware, might be convenient to allow sharing programming
+	 * for the scanout engine with the crtc implementation.
+	 */
+	if (!priv) {
+		list_add_tail(&plane->head, &dev->mode_config.plane_list);
+		dev->mode_config.num_plane++;
+	} else {
+		INIT_LIST_HEAD(&plane->head);
+	}
+
+	mutex_unlock(&dev->mode_config.mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(drm_plane_init);
+
+void drm_plane_cleanup(struct drm_plane *plane)
+{
+	struct drm_device *dev = plane->dev;
+
+	mutex_lock(&dev->mode_config.mutex);
+	kfree(plane->format_types);
+	drm_mode_object_put(dev, &plane->base);
+	/* if not added to a list, it must be a private plane */
+	if (!list_empty(&plane->head)) {
+		list_del(&plane->head);
+		dev->mode_config.num_plane--;
+	}
+	mutex_unlock(&dev->mode_config.mutex);
+}
+EXPORT_SYMBOL(drm_plane_cleanup);
+
 /**
  * drm_mode_create - create a new display mode
  * @dev: DRM device
@@ -871,6 +942,7 @@ void drm_mode_config_init(struct drm_device *dev)
 	INIT_LIST_HEAD(&dev->mode_config.encoder_list);
 	INIT_LIST_HEAD(&dev->mode_config.property_list);
 	INIT_LIST_HEAD(&dev->mode_config.property_blob_list);
+	INIT_LIST_HEAD(&dev->mode_config.plane_list);
 	idr_init(&dev->mode_config.crtc_idr);
 
 	mutex_lock(&dev->mode_config.mutex);
@@ -947,6 +1019,7 @@ void drm_mode_config_cleanup(struct drm_device *dev)
 	struct drm_encoder *encoder, *enct;
 	struct drm_framebuffer *fb, *fbt;
 	struct drm_property *property, *pt;
+	struct drm_plane *plane, *plt;
 
 	list_for_each_entry_safe(encoder, enct, &dev->mode_config.encoder_list,
 				 head) {
@@ -971,6 +1044,10 @@ void drm_mode_config_cleanup(struct drm_device *dev)
 		crtc->funcs->destroy(crtc);
 	}
 
+	list_for_each_entry_safe(plane, plt, &dev->mode_config.plane_list,
+				 head) {
+		plane->funcs->destroy(plane);
+	}
 }
 EXPORT_SYMBOL(drm_mode_config_cleanup);
 
@@ -1379,7 +1456,7 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 	 */
 	if ((out_resp->count_modes >= mode_count) && mode_count) {
 		copied = 0;
-		mode_ptr = (struct drm_mode_modeinfo *)(unsigned long)out_resp->modes_ptr;
+		mode_ptr = (struct drm_mode_modeinfo __user *)(unsigned long)out_resp->modes_ptr;
 		list_for_each_entry(mode, &connector->modes, head) {
 			drm_crtc_convert_to_umode(&u_mode, mode);
 			if (copy_to_user(mode_ptr + copied,
@@ -1394,8 +1471,8 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 
 	if ((out_resp->count_props >= props_count) && props_count) {
 		copied = 0;
-		prop_ptr = (uint32_t *)(unsigned long)(out_resp->props_ptr);
-		prop_values = (uint64_t *)(unsigned long)(out_resp->prop_values_ptr);
+		prop_ptr = (uint32_t __user *)(unsigned long)(out_resp->props_ptr);
+		prop_values = (uint64_t __user *)(unsigned long)(out_resp->prop_values_ptr);
 		for (i = 0; i < DRM_CONNECTOR_MAX_PROPERTY; i++) {
 			if (connector->property_ids[i] != 0) {
 				if (put_user(connector->property_ids[i],
@@ -1417,7 +1494,7 @@ int drm_mode_getconnector(struct drm_device *dev, void *data,
 
 	if ((out_resp->count_encoders >= encoders_count) && encoders_count) {
 		copied = 0;
-		encoder_ptr = (uint32_t *)(unsigned long)(out_resp->encoders_ptr);
+		encoder_ptr = (uint32_t __user *)(unsigned long)(out_resp->encoders_ptr);
 		for (i = 0; i < DRM_CONNECTOR_MAX_ENCODER; i++) {
 			if (connector->encoder_ids[i] != 0) {
 				if (put_user(connector->encoder_ids[i],
@@ -1471,6 +1548,245 @@ out:
 }
 
 /**
+ * drm_mode_getplane_res - get plane info
+ * @dev: DRM device
+ * @data: ioctl data
+ * @file_priv: DRM file info
+ *
+ * Return an plane count and set of IDs.
+ */
+int drm_mode_getplane_res(struct drm_device *dev, void *data,
+			    struct drm_file *file_priv)
+{
+	struct drm_mode_get_plane_res *plane_resp = data;
+	struct drm_mode_config *config;
+	struct drm_plane *plane;
+	uint32_t __user *plane_ptr;
+	int copied = 0, ret = 0;
+
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -EINVAL;
+
+	mutex_lock(&dev->mode_config.mutex);
+	config = &dev->mode_config;
+
+	/*
+	 * This ioctl is called twice, once to determine how much space is
+	 * needed, and the 2nd time to fill it.
+	 */
+	if (config->num_plane &&
+	    (plane_resp->count_planes >= config->num_plane)) {
+		plane_ptr = (uint32_t __user *)(unsigned long)plane_resp->plane_id_ptr;
+
+		list_for_each_entry(plane, &config->plane_list, head) {
+			if (put_user(plane->base.id, plane_ptr + copied)) {
+				ret = -EFAULT;
+				goto out;
+			}
+			copied++;
+		}
+	}
+	plane_resp->count_planes = config->num_plane;
+
+out:
+	mutex_unlock(&dev->mode_config.mutex);
+	return ret;
+}
+
+/**
+ * drm_mode_getplane - get plane info
+ * @dev: DRM device
+ * @data: ioctl data
+ * @file_priv: DRM file info
+ *
+ * Return plane info, including formats supported, gamma size, any
+ * current fb, etc.
+ */
+int drm_mode_getplane(struct drm_device *dev, void *data,
+			struct drm_file *file_priv)
+{
+	struct drm_mode_get_plane *plane_resp = data;
+	struct drm_mode_object *obj;
+	struct drm_plane *plane;
+	uint32_t __user *format_ptr;
+	int ret = 0;
+
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -EINVAL;
+
+	mutex_lock(&dev->mode_config.mutex);
+	obj = drm_mode_object_find(dev, plane_resp->plane_id,
+				   DRM_MODE_OBJECT_PLANE);
+	if (!obj) {
+		ret = -ENOENT;
+		goto out;
+	}
+	plane = obj_to_plane(obj);
+
+	if (plane->crtc)
+		plane_resp->crtc_id = plane->crtc->base.id;
+	else
+		plane_resp->crtc_id = 0;
+
+	if (plane->fb)
+		plane_resp->fb_id = plane->fb->base.id;
+	else
+		plane_resp->fb_id = 0;
+
+	plane_resp->plane_id = plane->base.id;
+	plane_resp->possible_crtcs = plane->possible_crtcs;
+	plane_resp->gamma_size = plane->gamma_size;
+
+	/*
+	 * This ioctl is called twice, once to determine how much space is
+	 * needed, and the 2nd time to fill it.
+	 */
+	if (plane->format_count &&
+	    (plane_resp->count_format_types >= plane->format_count)) {
+		format_ptr = (uint32_t __user *)(unsigned long)plane_resp->format_type_ptr;
+		if (copy_to_user(format_ptr,
+				 plane->format_types,
+				 sizeof(uint32_t) * plane->format_count)) {
+			ret = -EFAULT;
+			goto out;
+		}
+	}
+	plane_resp->count_format_types = plane->format_count;
+
+out:
+	mutex_unlock(&dev->mode_config.mutex);
+	return ret;
+}
+
+/**
+ * drm_mode_setplane - set up or tear down an plane
+ * @dev: DRM device
+ * @data: ioctl data*
+ * @file_prive: DRM file info
+ *
+ * Set plane info, including placement, fb, scaling, and other factors.
+ * Or pass a NULL fb to disable.
+ */
+int drm_mode_setplane(struct drm_device *dev, void *data,
+			struct drm_file *file_priv)
+{
+	struct drm_mode_set_plane *plane_req = data;
+	struct drm_mode_object *obj;
+	struct drm_plane *plane;
+	struct drm_crtc *crtc;
+	struct drm_framebuffer *fb;
+	int ret = 0;
+	unsigned int fb_width, fb_height;
+	int i;
+
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -EINVAL;
+
+	mutex_lock(&dev->mode_config.mutex);
+
+	/*
+	 * First, find the plane, crtc, and fb objects.  If not available,
+	 * we don't bother to call the driver.
+	 */
+	obj = drm_mode_object_find(dev, plane_req->plane_id,
+				   DRM_MODE_OBJECT_PLANE);
+	if (!obj) {
+		DRM_DEBUG_KMS("Unknown plane ID %d\n",
+			      plane_req->plane_id);
+		ret = -ENOENT;
+		goto out;
+	}
+	plane = obj_to_plane(obj);
+
+	/* No fb means shut it down */
+	if (!plane_req->fb_id) {
+		plane->funcs->disable_plane(plane);
+		plane->crtc = NULL;
+		plane->fb = NULL;
+		goto out;
+	}
+
+	obj = drm_mode_object_find(dev, plane_req->crtc_id,
+				   DRM_MODE_OBJECT_CRTC);
+	if (!obj) {
+		DRM_DEBUG_KMS("Unknown crtc ID %d\n",
+			      plane_req->crtc_id);
+		ret = -ENOENT;
+		goto out;
+	}
+	crtc = obj_to_crtc(obj);
+
+	obj = drm_mode_object_find(dev, plane_req->fb_id,
+				   DRM_MODE_OBJECT_FB);
+	if (!obj) {
+		DRM_DEBUG_KMS("Unknown framebuffer ID %d\n",
+			      plane_req->fb_id);
+		ret = -ENOENT;
+		goto out;
+	}
+	fb = obj_to_fb(obj);
+
+	/* Check whether this plane supports the fb pixel format. */
+	for (i = 0; i < plane->format_count; i++)
+		if (fb->pixel_format == plane->format_types[i])
+			break;
+	if (i == plane->format_count) {
+		DRM_DEBUG_KMS("Invalid pixel format 0x%08x\n", fb->pixel_format);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	fb_width = fb->width << 16;
+	fb_height = fb->height << 16;
+
+	/* Make sure source coordinates are inside the fb. */
+	if (plane_req->src_w > fb_width ||
+	    plane_req->src_x > fb_width - plane_req->src_w ||
+	    plane_req->src_h > fb_height ||
+	    plane_req->src_y > fb_height - plane_req->src_h) {
+		DRM_DEBUG_KMS("Invalid source coordinates "
+			      "%u.%06ux%u.%06u+%u.%06u+%u.%06u\n",
+			      plane_req->src_w >> 16,
+			      ((plane_req->src_w & 0xffff) * 15625) >> 10,
+			      plane_req->src_h >> 16,
+			      ((plane_req->src_h & 0xffff) * 15625) >> 10,
+			      plane_req->src_x >> 16,
+			      ((plane_req->src_x & 0xffff) * 15625) >> 10,
+			      plane_req->src_y >> 16,
+			      ((plane_req->src_y & 0xffff) * 15625) >> 10);
+		ret = -ENOSPC;
+		goto out;
+	}
+
+	/* Give drivers some help against integer overflows */
+	if (plane_req->crtc_w > INT_MAX ||
+	    plane_req->crtc_x > INT_MAX - (int32_t) plane_req->crtc_w ||
+	    plane_req->crtc_h > INT_MAX ||
+	    plane_req->crtc_y > INT_MAX - (int32_t) plane_req->crtc_h) {
+		DRM_DEBUG_KMS("Invalid CRTC coordinates %ux%u+%d+%d\n",
+			      plane_req->crtc_w, plane_req->crtc_h,
+			      plane_req->crtc_x, plane_req->crtc_y);
+		ret = -ERANGE;
+		goto out;
+	}
+
+	ret = plane->funcs->update_plane(plane, crtc, fb,
+					 plane_req->crtc_x, plane_req->crtc_y,
+					 plane_req->crtc_w, plane_req->crtc_h,
+					 plane_req->src_x, plane_req->src_y,
+					 plane_req->src_w, plane_req->src_h);
+	if (!ret) {
+		plane->crtc = crtc;
+		plane->fb = fb;
+	}
+
+out:
+	mutex_unlock(&dev->mode_config.mutex);
+
+	return ret;
+}
+
+/**
  * drm_mode_setcrtc - set CRTC configuration
  * @inode: inode from the ioctl
  * @filp: file * from the ioctl
@@ -1576,7 +1892,7 @@ int drm_mode_setcrtc(struct drm_device *dev, void *data,
 		}
 
 		for (i = 0; i < crtc_req->count_connectors; i++) {
-			set_connectors_ptr = (uint32_t *)(unsigned long)crtc_req->set_connectors_ptr;
+			set_connectors_ptr = (uint32_t __user *)(unsigned long)crtc_req->set_connectors_ptr;
 			if (get_user(out_id, &set_connectors_ptr[i])) {
 				ret = -EFAULT;
 				goto out;
@@ -1625,10 +1941,8 @@ int drm_mode_cursor_ioctl(struct drm_device *dev,
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
 
-	if (!req->flags) {
-		DRM_ERROR("no operation set\n");
+	if (!req->flags)
 		return -EINVAL;
-	}
 
 	mutex_lock(&dev->mode_config.mutex);
 	obj = drm_mode_object_find(dev, req->crtc_id, DRM_MODE_OBJECT_CRTC);
@@ -1641,7 +1955,6 @@ int drm_mode_cursor_ioctl(struct drm_device *dev,
 
 	if (req->flags & DRM_MODE_CURSOR_BO) {
 		if (!crtc->funcs->cursor_set) {
-			DRM_ERROR("crtc does not support cursor\n");
 			ret = -ENXIO;
 			goto out;
 		}
@@ -1654,7 +1967,6 @@ int drm_mode_cursor_ioctl(struct drm_device *dev,
 		if (crtc->funcs->cursor_move) {
 			ret = crtc->funcs->cursor_move(crtc, req->x, req->y);
 		} else {
-			DRM_ERROR("crtc does not support cursor\n");
 			ret = -EFAULT;
 			goto out;
 		}
@@ -1664,6 +1976,42 @@ out:
 	return ret;
 }
 
+/* Original addfb only supported RGB formats, so figure out which one */
+uint32_t drm_mode_legacy_fb_format(uint32_t bpp, uint32_t depth)
+{
+	uint32_t fmt;
+
+	switch (bpp) {
+	case 8:
+		fmt = DRM_FORMAT_RGB332;
+		break;
+	case 16:
+		if (depth == 15)
+			fmt = DRM_FORMAT_XRGB1555;
+		else
+			fmt = DRM_FORMAT_RGB565;
+		break;
+	case 24:
+		fmt = DRM_FORMAT_RGB888;
+		break;
+	case 32:
+		if (depth == 24)
+			fmt = DRM_FORMAT_XRGB8888;
+		else if (depth == 30)
+			fmt = DRM_FORMAT_XRGB2101010;
+		else
+			fmt = DRM_FORMAT_ARGB8888;
+		break;
+	default:
+		DRM_ERROR("bad bpp, assuming x8r8g8b8 pixel format\n");
+		fmt = DRM_FORMAT_XRGB8888;
+		break;
+	}
+
+	return fmt;
+}
+EXPORT_SYMBOL(drm_mode_legacy_fb_format);
+
 /**
  * drm_mode_addfb - add an FB to the graphics configuration
  * @inode: inode from the ioctl
@@ -1684,7 +2032,140 @@ out:
 int drm_mode_addfb(struct drm_device *dev,
 		   void *data, struct drm_file *file_priv)
 {
-	struct drm_mode_fb_cmd *r = data;
+	struct drm_mode_fb_cmd *or = data;
+	struct drm_mode_fb_cmd2 r = {};
+	struct drm_mode_config *config = &dev->mode_config;
+	struct drm_framebuffer *fb;
+	int ret = 0;
+
+	/* Use new struct with format internally */
+	r.fb_id = or->fb_id;
+	r.width = or->width;
+	r.height = or->height;
+	r.pitches[0] = or->pitch;
+	r.pixel_format = drm_mode_legacy_fb_format(or->bpp, or->depth);
+	r.handles[0] = or->handle;
+
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -EINVAL;
+
+	if ((config->min_width > r.width) || (r.width > config->max_width))
+		return -EINVAL;
+
+	if ((config->min_height > r.height) || (r.height > config->max_height))
+		return -EINVAL;
+
+	mutex_lock(&dev->mode_config.mutex);
+
+	/* TODO check buffer is sufficiently large */
+	/* TODO setup destructor callback */
+
+	fb = dev->mode_config.funcs->fb_create(dev, file_priv, &r);
+	if (IS_ERR(fb)) {
+		DRM_ERROR("could not create framebuffer\n");
+		ret = PTR_ERR(fb);
+		goto out;
+	}
+
+	or->fb_id = fb->base.id;
+	list_add(&fb->filp_head, &file_priv->fbs);
+	DRM_DEBUG_KMS("[FB:%d]\n", fb->base.id);
+
+out:
+	mutex_unlock(&dev->mode_config.mutex);
+	return ret;
+}
+
+static int format_check(struct drm_mode_fb_cmd2 *r)
+{
+	uint32_t format = r->pixel_format & ~DRM_FORMAT_BIG_ENDIAN;
+
+	switch (format) {
+	case DRM_FORMAT_C8:
+	case DRM_FORMAT_RGB332:
+	case DRM_FORMAT_BGR233:
+	case DRM_FORMAT_XRGB4444:
+	case DRM_FORMAT_XBGR4444:
+	case DRM_FORMAT_RGBX4444:
+	case DRM_FORMAT_BGRX4444:
+	case DRM_FORMAT_ARGB4444:
+	case DRM_FORMAT_ABGR4444:
+	case DRM_FORMAT_RGBA4444:
+	case DRM_FORMAT_BGRA4444:
+	case DRM_FORMAT_XRGB1555:
+	case DRM_FORMAT_XBGR1555:
+	case DRM_FORMAT_RGBX5551:
+	case DRM_FORMAT_BGRX5551:
+	case DRM_FORMAT_ARGB1555:
+	case DRM_FORMAT_ABGR1555:
+	case DRM_FORMAT_RGBA5551:
+	case DRM_FORMAT_BGRA5551:
+	case DRM_FORMAT_RGB565:
+	case DRM_FORMAT_BGR565:
+	case DRM_FORMAT_RGB888:
+	case DRM_FORMAT_BGR888:
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_XBGR8888:
+	case DRM_FORMAT_RGBX8888:
+	case DRM_FORMAT_BGRX8888:
+	case DRM_FORMAT_ARGB8888:
+	case DRM_FORMAT_ABGR8888:
+	case DRM_FORMAT_RGBA8888:
+	case DRM_FORMAT_BGRA8888:
+	case DRM_FORMAT_XRGB2101010:
+	case DRM_FORMAT_XBGR2101010:
+	case DRM_FORMAT_RGBX1010102:
+	case DRM_FORMAT_BGRX1010102:
+	case DRM_FORMAT_ARGB2101010:
+	case DRM_FORMAT_ABGR2101010:
+	case DRM_FORMAT_RGBA1010102:
+	case DRM_FORMAT_BGRA1010102:
+	case DRM_FORMAT_YUYV:
+	case DRM_FORMAT_YVYU:
+	case DRM_FORMAT_UYVY:
+	case DRM_FORMAT_VYUY:
+	case DRM_FORMAT_AYUV:
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+	case DRM_FORMAT_YUV411:
+	case DRM_FORMAT_YVU411:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_YUV444:
+	case DRM_FORMAT_YVU444:
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+/**
+ * drm_mode_addfb2 - add an FB to the graphics configuration
+ * @inode: inode from the ioctl
+ * @filp: file * from the ioctl
+ * @cmd: cmd from ioctl
+ * @arg: arg from ioctl
+ *
+ * LOCKING:
+ * Takes mode config lock.
+ *
+ * Add a new FB to the specified CRTC, given a user request with format.
+ *
+ * Called by the user via ioctl.
+ *
+ * RETURNS:
+ * Zero on success, errno on failure.
+ */
+int drm_mode_addfb2(struct drm_device *dev,
+		    void *data, struct drm_file *file_priv)
+{
+	struct drm_mode_fb_cmd2 *r = data;
 	struct drm_mode_config *config = &dev->mode_config;
 	struct drm_framebuffer *fb;
 	int ret = 0;
@@ -1693,18 +2174,23 @@ int drm_mode_addfb(struct drm_device *dev,
 		return -EINVAL;
 
 	if ((config->min_width > r->width) || (r->width > config->max_width)) {
-		DRM_ERROR("mode new framebuffer width not within limits\n");
+		DRM_ERROR("bad framebuffer width %d, should be >= %d && <= %d\n",
+			  r->width, config->min_width, config->max_width);
 		return -EINVAL;
 	}
 	if ((config->min_height > r->height) || (r->height > config->max_height)) {
-		DRM_ERROR("mode new framebuffer height not within limits\n");
+		DRM_ERROR("bad framebuffer height %d, should be >= %d && <= %d\n",
+			  r->height, config->min_height, config->max_height);
 		return -EINVAL;
 	}
 
-	mutex_lock(&dev->mode_config.mutex);
+	ret = format_check(r);
+	if (ret) {
+		DRM_ERROR("bad framebuffer format 0x%08x\n", r->pixel_format);
+		return ret;
+	}
 
-	/* TODO check buffer is sufficiently large */
-	/* TODO setup destructor callback */
+	mutex_lock(&dev->mode_config.mutex);
 
 	fb = dev->mode_config.funcs->fb_create(dev, file_priv, r);
 	if (IS_ERR(fb)) {
@@ -1756,7 +2242,6 @@ int drm_mode_rmfb(struct drm_device *dev,
 	obj = drm_mode_object_find(dev, *id, DRM_MODE_OBJECT_FB);
 	/* TODO check that we really get a framebuffer back. */
 	if (!obj) {
-		DRM_ERROR("mode invalid framebuffer id\n");
 		ret = -EINVAL;
 		goto out;
 	}
@@ -1767,7 +2252,6 @@ int drm_mode_rmfb(struct drm_device *dev,
 			found = 1;
 
 	if (!found) {
-		DRM_ERROR("tried to remove a fb that we didn't own\n");
 		ret = -EINVAL;
 		goto out;
 	}
@@ -1814,7 +2298,6 @@ int drm_mode_getfb(struct drm_device *dev,
 	mutex_lock(&dev->mode_config.mutex);
 	obj = drm_mode_object_find(dev, r->fb_id, DRM_MODE_OBJECT_FB);
 	if (!obj) {
-		DRM_ERROR("invalid framebuffer id\n");
 		ret = -EINVAL;
 		goto out;
 	}
@@ -1824,7 +2307,7 @@ int drm_mode_getfb(struct drm_device *dev,
 	r->width = fb->width;
 	r->depth = fb->depth;
 	r->bpp = fb->bits_per_pixel;
-	r->pitch = fb->pitch;
+	r->pitch = fb->pitches[0];
 	fb->funcs->create_handle(fb, file_priv, &r->handle);
 
 out:
@@ -1850,14 +2333,13 @@ int drm_mode_dirtyfb_ioctl(struct drm_device *dev,
 	mutex_lock(&dev->mode_config.mutex);
 	obj = drm_mode_object_find(dev, r->fb_id, DRM_MODE_OBJECT_FB);
 	if (!obj) {
-		DRM_ERROR("invalid framebuffer id\n");
 		ret = -EINVAL;
 		goto out_err1;
 	}
 	fb = obj_to_fb(obj);
 
 	num_clips = r->num_clips;
-	clips_ptr = (struct drm_clip_rect *)(unsigned long)r->clips_ptr;
+	clips_ptr = (struct drm_clip_rect __user *)(unsigned long)r->clips_ptr;
 
 	if (!num_clips != !clips_ptr) {
 		ret = -EINVAL;
@@ -2253,7 +2735,7 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	struct drm_property_enum *prop_enum;
 	struct drm_mode_property_enum __user *enum_ptr;
 	struct drm_property_blob *prop_blob;
-	uint32_t *blob_id_ptr;
+	uint32_t __user *blob_id_ptr;
 	uint64_t __user *values_ptr;
 	uint32_t __user *blob_length_ptr;
 
@@ -2283,7 +2765,7 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	out_resp->flags = property->flags;
 
 	if ((out_resp->count_values >= value_count) && value_count) {
-		values_ptr = (uint64_t *)(unsigned long)out_resp->values_ptr;
+		values_ptr = (uint64_t __user *)(unsigned long)out_resp->values_ptr;
 		for (i = 0; i < value_count; i++) {
 			if (copy_to_user(values_ptr + i, &property->values[i], sizeof(uint64_t))) {
 				ret = -EFAULT;
@@ -2296,7 +2778,7 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	if (property->flags & DRM_MODE_PROP_ENUM) {
 		if ((out_resp->count_enum_blobs >= enum_count) && enum_count) {
 			copied = 0;
-			enum_ptr = (struct drm_mode_property_enum *)(unsigned long)out_resp->enum_blob_ptr;
+			enum_ptr = (struct drm_mode_property_enum __user *)(unsigned long)out_resp->enum_blob_ptr;
 			list_for_each_entry(prop_enum, &property->enum_blob_list, head) {
 
 				if (copy_to_user(&enum_ptr[copied].value, &prop_enum->value, sizeof(uint64_t))) {
@@ -2318,8 +2800,8 @@ int drm_mode_getproperty_ioctl(struct drm_device *dev,
 	if (property->flags & DRM_MODE_PROP_BLOB) {
 		if ((out_resp->count_enum_blobs >= blob_count) && blob_count) {
 			copied = 0;
-			blob_id_ptr = (uint32_t *)(unsigned long)out_resp->enum_blob_ptr;
-			blob_length_ptr = (uint32_t *)(unsigned long)out_resp->values_ptr;
+			blob_id_ptr = (uint32_t __user *)(unsigned long)out_resp->enum_blob_ptr;
+			blob_length_ptr = (uint32_t __user *)(unsigned long)out_resp->values_ptr;
 
 			list_for_each_entry(prop_blob, &property->enum_blob_list, head) {
 				if (put_user(prop_blob->base.id, blob_id_ptr + copied)) {
@@ -2380,7 +2862,7 @@ int drm_mode_getblob_ioctl(struct drm_device *dev,
 	struct drm_mode_get_blob *out_resp = data;
 	struct drm_property_blob *blob;
 	int ret = 0;
-	void *blob_ptr;
+	void __user *blob_ptr;
 
 	if (!drm_core_check_feature(dev, DRIVER_MODESET))
 		return -EINVAL;
@@ -2394,7 +2876,7 @@ int drm_mode_getblob_ioctl(struct drm_device *dev,
 	blob = obj_to_blob(obj);
 
 	if (out_resp->length == blob->length) {
-		blob_ptr = (void *)(unsigned long)out_resp->data;
+		blob_ptr = (void __user *)(unsigned long)out_resp->data;
 		if (copy_to_user(blob_ptr, blob->data, blob->length)){
 			ret = -EFAULT;
 			goto done;
@@ -2788,3 +3270,71 @@ int drm_mode_destroy_dumb_ioctl(struct drm_device *dev,
 
 	return dev->driver->dumb_destroy(file_priv, dev, args->handle);
 }
+
+/*
+ * Just need to support RGB formats here for compat with code that doesn't
+ * use pixel formats directly yet.
+ */
+void drm_fb_get_bpp_depth(uint32_t format, unsigned int *depth,
+			  int *bpp)
+{
+	switch (format) {
+	case DRM_FORMAT_RGB332:
+	case DRM_FORMAT_BGR233:
+		*depth = 8;
+		*bpp = 8;
+		break;
+	case DRM_FORMAT_XRGB1555:
+	case DRM_FORMAT_XBGR1555:
+	case DRM_FORMAT_RGBX5551:
+	case DRM_FORMAT_BGRX5551:
+	case DRM_FORMAT_ARGB1555:
+	case DRM_FORMAT_ABGR1555:
+	case DRM_FORMAT_RGBA5551:
+	case DRM_FORMAT_BGRA5551:
+		*depth = 15;
+		*bpp = 16;
+		break;
+	case DRM_FORMAT_RGB565:
+	case DRM_FORMAT_BGR565:
+		*depth = 16;
+		*bpp = 16;
+		break;
+	case DRM_FORMAT_RGB888:
+	case DRM_FORMAT_BGR888:
+		*depth = 24;
+		*bpp = 24;
+		break;
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_XBGR8888:
+	case DRM_FORMAT_RGBX8888:
+	case DRM_FORMAT_BGRX8888:
+		*depth = 24;
+		*bpp = 32;
+		break;
+	case DRM_FORMAT_XRGB2101010:
+	case DRM_FORMAT_XBGR2101010:
+	case DRM_FORMAT_RGBX1010102:
+	case DRM_FORMAT_BGRX1010102:
+	case DRM_FORMAT_ARGB2101010:
+	case DRM_FORMAT_ABGR2101010:
+	case DRM_FORMAT_RGBA1010102:
+	case DRM_FORMAT_BGRA1010102:
+		*depth = 30;
+		*bpp = 32;
+		break;
+	case DRM_FORMAT_ARGB8888:
+	case DRM_FORMAT_ABGR8888:
+	case DRM_FORMAT_RGBA8888:
+	case DRM_FORMAT_BGRA8888:
+		*depth = 32;
+		*bpp = 32;
+		break;
+	default:
+		DRM_DEBUG_KMS("unsupported pixel format\n");
+		*depth = 0;
+		*bpp = 0;
+		break;
+	}
+}
+EXPORT_SYMBOL(drm_fb_get_bpp_depth);
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index d2619d72cece..84a4a809793f 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -34,6 +34,7 @@
 
 #include "drmP.h"
 #include "drm_crtc.h"
+#include "drm_fourcc.h"
 #include "drm_crtc_helper.h"
 #include "drm_fb_helper.h"
 
@@ -710,7 +711,7 @@ int drm_crtc_helper_set_config(struct drm_mode_set *set)
 			for (i = 0; i < set->num_connectors; i++) {
 				DRM_DEBUG_KMS("\t[CONNECTOR:%d:%s] set DPMS on\n", set->connectors[i]->base.id,
 					      drm_get_connector_name(set->connectors[i]));
-				set->connectors[i]->dpms = DRM_MODE_DPMS_ON;
+				set->connectors[i]->funcs->dpms(set->connectors[i], DRM_MODE_DPMS_ON);
 			}
 		}
 		drm_helper_disable_unused_functions(dev);
@@ -847,13 +848,19 @@ void drm_helper_connector_dpms(struct drm_connector *connector, int mode)
 EXPORT_SYMBOL(drm_helper_connector_dpms);
 
 int drm_helper_mode_fill_fb_struct(struct drm_framebuffer *fb,
-				   struct drm_mode_fb_cmd *mode_cmd)
+				   struct drm_mode_fb_cmd2 *mode_cmd)
 {
+	int i;
+
 	fb->width = mode_cmd->width;
 	fb->height = mode_cmd->height;
-	fb->pitch = mode_cmd->pitch;
-	fb->bits_per_pixel = mode_cmd->bpp;
-	fb->depth = mode_cmd->depth;
+	for (i = 0; i < 4; i++) {
+		fb->pitches[i] = mode_cmd->pitches[i];
+		fb->offsets[i] = mode_cmd->offsets[i];
+	}
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &fb->depth,
+				    &fb->bits_per_pixel);
+	fb->pixel_format = mode_cmd->pixel_format;
 
 	return 0;
 }
@@ -1008,3 +1015,36 @@ void drm_helper_hpd_irq_event(struct drm_device *dev)
 		queue_delayed_work(system_nrt_wq, &dev->mode_config.output_poll_work, 0);
 }
 EXPORT_SYMBOL(drm_helper_hpd_irq_event);
+
+
+/**
+ * drm_format_num_planes - get the number of planes for format
+ * @format: pixel format (DRM_FORMAT_*)
+ *
+ * RETURNS:
+ * The number of planes used by the specified pixel format.
+ */
+int drm_format_num_planes(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_YUV410:
+	case DRM_FORMAT_YVU410:
+	case DRM_FORMAT_YUV411:
+	case DRM_FORMAT_YVU411:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_YUV444:
+	case DRM_FORMAT_YVU444:
+		return 3;
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+		return 2;
+	default:
+		return 1;
+	}
+}
+EXPORT_SYMBOL(drm_format_num_planes);
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 40c187c60f44..ebf7d3f68fc4 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -61,14 +61,14 @@ static int drm_version(struct drm_device *dev, void *data,
 
 /** Ioctl table */
 static struct drm_ioctl_desc drm_ioctls[] = {
-	DRM_IOCTL_DEF(DRM_IOCTL_VERSION, drm_version, 0),
+	DRM_IOCTL_DEF(DRM_IOCTL_VERSION, drm_version, DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_GET_UNIQUE, drm_getunique, 0),
 	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAGIC, drm_getmagic, 0),
 	DRM_IOCTL_DEF(DRM_IOCTL_IRQ_BUSID, drm_irq_by_busid, DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAP, drm_getmap, 0),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_CLIENT, drm_getclient, 0),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_STATS, drm_getstats, 0),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_CAP, drm_getcap, 0),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAP, drm_getmap, DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_CLIENT, drm_getclient, DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_STATS, drm_getstats, DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_CAP, drm_getcap, DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_SET_VERSION, drm_setversion, DRM_MASTER),
 
 	DRM_IOCTL_DEF(DRM_IOCTL_SET_UNIQUE, drm_setunique, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
@@ -136,8 +136,11 @@ static struct drm_ioctl_desc drm_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_IOCTL_GEM_OPEN, drm_gem_open_ioctl, DRM_AUTH|DRM_UNLOCKED),
 
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETRESOURCES, drm_mode_getresources, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPLANERESOURCES, drm_mode_getplane_res, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETCRTC, drm_mode_getcrtc, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETCRTC, drm_mode_setcrtc, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPLANE, drm_mode_getplane, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETPLANE, drm_mode_setplane, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CURSOR, drm_mode_cursor_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETGAMMA, drm_mode_gamma_get_ioctl, DRM_MASTER|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETGAMMA, drm_mode_gamma_set_ioctl, DRM_MASTER|DRM_UNLOCKED),
@@ -150,6 +153,7 @@ static struct drm_ioctl_desc drm_ioctls[] = {
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPROPBLOB, drm_mode_getblob_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETFB, drm_mode_getfb, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ADDFB, drm_mode_addfb, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ADDFB2, drm_mode_addfb2, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_RMFB, drm_mode_rmfb, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_PAGE_FLIP, drm_mode_page_flip_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DIRTYFB, drm_mode_dirtyfb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index 3e927ce7557d..ece03fc2d386 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -508,25 +508,10 @@ static void
 cea_for_each_detailed_block(u8 *ext, detailed_cb *cb, void *closure)
 {
 	int i, n = 0;
-	u8 rev = ext[0x01], d = ext[0x02];
+	u8 d = ext[0x02];
 	u8 *det_base = ext + d;
 
-	switch (rev) {
-	case 0:
-		/* can't happen */
-		return;
-	case 1:
-		/* have to infer how many blocks we have, check pixel clock */
-		for (i = 0; i < 6; i++)
-			if (det_base[18*i] || det_base[18*i+1])
-				n++;
-		break;
-	default:
-		/* explicit count */
-		n = min(ext[0x03] & 0x0f, 6);
-		break;
-	}
-
+	n = (127 - d) / 18;
 	for (i = 0; i < n; i++)
 		cb((struct detailed_timing *)(det_base + 18 * i), closure);
 }
@@ -1319,6 +1304,7 @@ add_detailed_modes(struct drm_connector *connector, struct edid *edid,
 
 #define HDMI_IDENTIFIER 0x000C03
 #define AUDIO_BLOCK	0x01
+#define VIDEO_BLOCK     0x02
 #define VENDOR_BLOCK    0x03
 #define SPEAKER_BLOCK	0x04
 #define EDID_BASIC_AUDIO	(1 << 6)
@@ -1349,6 +1335,47 @@ u8 *drm_find_cea_extension(struct edid *edid)
 }
 EXPORT_SYMBOL(drm_find_cea_extension);
 
+static int
+do_cea_modes (struct drm_connector *connector, u8 *db, u8 len)
+{
+	struct drm_device *dev = connector->dev;
+	u8 * mode, cea_mode;
+	int modes = 0;
+
+	for (mode = db; mode < db + len; mode++) {
+		cea_mode = (*mode & 127) - 1; /* CEA modes are numbered 1..127 */
+		if (cea_mode < drm_num_cea_modes) {
+			struct drm_display_mode *newmode;
+			newmode = drm_mode_duplicate(dev,
+						     &edid_cea_modes[cea_mode]);
+			if (newmode) {
+				drm_mode_probed_add(connector, newmode);
+				modes++;
+			}
+		}
+	}
+
+	return modes;
+}
+
+static int
+add_cea_modes(struct drm_connector *connector, struct edid *edid)
+{
+	u8 * cea = drm_find_cea_extension(edid);
+	u8 * db, dbl;
+	int modes = 0;
+
+	if (cea && cea[1] >= 3) {
+		for (db = cea + 4; db < cea + cea[2]; db += dbl + 1) {
+			dbl = db[0] & 0x1f;
+			if (((db[0] & 0xe0) >> 5) == VIDEO_BLOCK)
+				modes += do_cea_modes (connector, db+1, dbl);
+		}
+	}
+
+	return modes;
+}
+
 static void
 parse_hdmi_vsdb(struct drm_connector *connector, uint8_t *db)
 {
@@ -1432,26 +1459,29 @@ void drm_edid_to_eld(struct drm_connector *connector, struct edid *edid)
 	eld[18] = edid->prod_code[0];
 	eld[19] = edid->prod_code[1];
 
-	for (db = cea + 4; db < cea + cea[2]; db += dbl + 1) {
-		dbl = db[0] & 0x1f;
-
-		switch ((db[0] & 0xe0) >> 5) {
-		case AUDIO_BLOCK:	/* Audio Data Block, contains SADs */
-			sad_count = dbl / 3;
-			memcpy(eld + 20 + mnl, &db[1], dbl);
-			break;
-		case SPEAKER_BLOCK:	/* Speaker Allocation Data Block */
-			eld[7] = db[1];
-			break;
-		case VENDOR_BLOCK:
-			/* HDMI Vendor-Specific Data Block */
-			if (db[1] == 0x03 && db[2] == 0x0c && db[3] == 0)
-				parse_hdmi_vsdb(connector, db);
-			break;
-		default:
-			break;
+	if (cea[1] >= 3)
+		for (db = cea + 4; db < cea + cea[2]; db += dbl + 1) {
+			dbl = db[0] & 0x1f;
+			
+			switch ((db[0] & 0xe0) >> 5) {
+			case AUDIO_BLOCK:
+				/* Audio Data Block, contains SADs */
+				sad_count = dbl / 3;
+				memcpy(eld + 20 + mnl, &db[1], dbl);
+				break;
+			case SPEAKER_BLOCK:
+                                /* Speaker Allocation Data Block */
+				eld[7] = db[1];
+				break;
+			case VENDOR_BLOCK:
+				/* HDMI Vendor-Specific Data Block */
+				if (db[1] == 0x03 && db[2] == 0x0c && db[3] == 0)
+					parse_hdmi_vsdb(connector, db);
+				break;
+			default:
+				break;
+			}
 		}
-	}
 	eld[5] |= sad_count << 4;
 	eld[2] = (20 + mnl + sad_count * 3 + 3) / 4;
 
@@ -1722,6 +1752,7 @@ int drm_add_edid_modes(struct drm_connector *connector, struct edid *edid)
 	num_modes += add_standard_modes(connector, edid);
 	num_modes += add_established_modes(connector, edid);
 	num_modes += add_inferred_modes(connector, edid);
+	num_modes += add_cea_modes(connector, edid);
 
 	if (quirks & (EDID_QUIRK_PREFER_LARGE_60 | EDID_QUIRK_PREFER_LARGE_75))
 		edid_fixup_preferred(connector, quirks);
diff --git a/drivers/gpu/drm/drm_edid_modes.h b/drivers/gpu/drm/drm_edid_modes.h
index 5f2064489fd5..a91ffb117220 100644
--- a/drivers/gpu/drm/drm_edid_modes.h
+++ b/drivers/gpu/drm/drm_edid_modes.h
@@ -378,3 +378,287 @@ static const struct {
 	{ 1920, 1440, 75, 0 },
 };
 static const int num_est3_modes = sizeof(est3_modes) / sizeof(est3_modes[0]);
+
+/*
+ * Probably taken from CEA-861 spec.
+ * This table is converted from xorg's hw/xfree86/modes/xf86EdidModes.c.
+ */
+static const struct drm_display_mode edid_cea_modes[] = {
+	/* 640x480@60Hz */
+	{ DRM_MODE("640x480", DRM_MODE_TYPE_DRIVER, 25175, 640, 656,
+		   752, 800, 0, 480, 490, 492, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 720x480@60Hz */
+	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 27000, 720, 736,
+		   798, 858, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 720x480@60Hz */
+	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 27000, 720, 736,
+		   798, 858, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1280x720@60Hz */
+	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 1390,
+		   1430, 1650, 0, 720, 725, 730, 750, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1080i@60Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2008,
+		   2052, 2200, 0, 1080, 1084, 1094, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x480i@60Hz */
+	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
+		   1602, 1716, 0, 480, 488, 494, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x480i@60Hz */
+	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
+		   1602, 1716, 0, 480, 488, 494, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x240@60Hz */
+	{ DRM_MODE("1440x240", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
+		   1602, 1716, 0, 240, 244, 247, 262, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x240@60Hz */
+	{ DRM_MODE("1440x240", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
+		   1602, 1716, 0, 240, 244, 247, 262, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2880x480i@60Hz */
+	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
+		   3204, 3432, 0, 480, 488, 494, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 2880x480i@60Hz */
+	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
+		   3204, 3432, 0, 480, 488, 494, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 2880x240@60Hz */
+	{ DRM_MODE("2880x240", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
+		   3204, 3432, 0, 240, 244, 247, 262, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2880x240@60Hz */
+	{ DRM_MODE("2880x240", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2956,
+		   3204, 3432, 0, 240, 244, 247, 262, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x480@60Hz */
+	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1472,
+		   1596, 1716, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x480@60Hz */
+	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1472,
+		   1596, 1716, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1920x1080@60Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2008,
+		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 720x576@50Hz */
+	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 27000, 720, 732,
+		   796, 864, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 720x576@50Hz */
+	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 27000, 720, 732,
+		   796, 864, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1280x720@50Hz */
+	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 1720,
+		   1760, 1980, 0, 720, 725, 730, 750, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1080i@50Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2448,
+		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x576i@50Hz */
+	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
+		   1590, 1728, 0, 576, 580, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x576i@50Hz */
+	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
+		   1590, 1728, 0, 576, 580, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x288@50Hz */
+	{ DRM_MODE("1440x288", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
+		   1590, 1728, 0, 288, 290, 293, 312, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x288@50Hz */
+	{ DRM_MODE("1440x288", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
+		   1590, 1728, 0, 288, 290, 293, 312, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2880x576i@50Hz */
+	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
+		   3180, 3456, 0, 576, 580, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 2880x576i@50Hz */
+	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
+		   3180, 3456, 0, 576, 580, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 2880x288@50Hz */
+	{ DRM_MODE("2880x288", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
+		   3180, 3456, 0, 288, 290, 293, 312, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2880x288@50Hz */
+	{ DRM_MODE("2880x288", DRM_MODE_TYPE_DRIVER, 54000, 2880, 2928,
+		   3180, 3456, 0, 288, 290, 293, 312, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x576@50Hz */
+	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
+		   1592, 1728, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x576@50Hz */
+	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
+		   1592, 1728, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1920x1080@50Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2448,
+		   2492, 2640, 0, 1080, 1084, 1089, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1080@24Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2558,
+		   2602, 2750, 0, 1080, 1084, 1089, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1080@25Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2448,
+		   2492, 2640, 0, 1080, 1084, 1089, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1080@30Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 74250, 1920, 2008,
+		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 2880x480@60Hz */
+	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2944,
+		   3192, 3432, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2880x480@60Hz */
+	{ DRM_MODE("2880x480", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2944,
+		   3192, 3432, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2880x576@50Hz */
+	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2928,
+		   3184, 3456, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 2880x576@50Hz */
+	{ DRM_MODE("2880x576", DRM_MODE_TYPE_DRIVER, 108000, 2880, 2928,
+		   3184, 3456, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1920x1080i@50Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 72000, 1920, 1952,
+		   2120, 2304, 0, 1080, 1126, 1136, 1250, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1920x1080i@100Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2448,
+		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1280x720@100Hz */
+	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 148500, 1280, 1720,
+		   1760, 1980, 0, 720, 725, 730, 750, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 720x576@100Hz */
+	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 54000, 720, 732,
+		   796, 864, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 720x576@100Hz */
+	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 54000, 720, 732,
+		   796, 864, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x576i@100Hz */
+	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
+		   1590, 1728, 0, 576, 580, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x576i@100Hz */
+	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
+		   1590, 1728, 0, 576, 580, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1920x1080i@120Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2008,
+		   2052, 2200, 0, 1080, 1084, 1094, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1280x720@120Hz */
+	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 148500, 1280, 1390,
+		   1430, 1650, 0, 720, 725, 730, 750, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 720x480@120Hz */
+	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 54000, 720, 736,
+		   798, 858, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 720x480@120Hz */
+	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 54000, 720, 736,
+		   798, 858, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x480i@120Hz */
+	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1478,
+		   1602, 1716, 0, 480, 488, 494, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x480i@120Hz */
+	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1478,
+		   1602, 1716, 0, 480, 488, 494, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 720x576@200Hz */
+	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 108000, 720, 732,
+		   796, 864, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 720x576@200Hz */
+	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 108000, 720, 732,
+		   796, 864, 0, 576, 581, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x576i@200Hz */
+	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1464,
+		   1590, 1728, 0, 576, 580, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x576i@200Hz */
+	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1464,
+		   1590, 1728, 0, 576, 580, 586, 625, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 720x480@240Hz */
+	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 108000, 720, 736,
+		   798, 858, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 720x480@240Hz */
+	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 108000, 720, 736,
+		   798, 858, 0, 480, 489, 495, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
+	/* 1440x480i@240 */
+	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1478,
+		   1602, 1716, 0, 480, 488, 494, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1440x480i@240 */
+	{ DRM_MODE("1440x480", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1478,
+		   1602, 1716, 0, 480, 488, 494, 525, 0,
+		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
+			DRM_MODE_FLAG_INTERLACE) },
+	/* 1280x720@24Hz */
+	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 59400, 1280, 3040,
+		   3080, 3300, 0, 720, 725, 730, 750, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x720@25Hz */
+	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 3700,
+		   3740, 3960, 0, 720, 725, 730, 750, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1280x720@30Hz */
+	{ DRM_MODE("1280x720", DRM_MODE_TYPE_DRIVER, 74250, 1280, 3040,
+		   3080, 3300, 0, 720, 725, 730, 750, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1080@120Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 297000, 1920, 2008,
+		   2052, 2200, 0, 1080, 1084, 1089, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	/* 1920x1080@100Hz */
+	{ DRM_MODE("1920x1080", DRM_MODE_TYPE_DRIVER, 297000, 1920, 2448,
+		   2492, 2640, 0, 1080, 1084, 1094, 1125, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+};
+static const int drm_num_cea_modes =
+	sizeof (edid_cea_modes) / sizeof (edid_cea_modes[0]);
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index 80fe39d98b0c..aada26f63dec 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -255,6 +255,13 @@ bool drm_fb_helper_force_kernel_mode(void)
 int drm_fb_helper_panic(struct notifier_block *n, unsigned long ununsed,
 			void *panic_str)
 {
+	/*
+	 * It's a waste of time and effort to switch back to text console
+	 * if the kernel should reboot before panic messages can be seen.
+	 */
+	if (panic_timeout < 0)
+		return 0;
+
 	printk(KERN_ERR "panic occurred, switching back to text console\n");
 	return drm_fb_helper_force_kernel_mode();
 }
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index 4911e1d1dcf2..c00cf154cc0b 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -182,7 +182,7 @@ int drm_stub_open(struct inode *inode, struct file *filp)
 		goto out;
 
 	old_fops = filp->f_op;
-	filp->f_op = fops_get(&dev->driver->fops);
+	filp->f_op = fops_get(dev->driver->fops);
 	if (filp->f_op == NULL) {
 		filp->f_op = old_fops;
 		goto out;
diff --git a/drivers/gpu/drm/drm_ioctl.c b/drivers/gpu/drm/drm_ioctl.c
index 904d7e9c8e47..956fd38d7c9e 100644
--- a/drivers/gpu/drm/drm_ioctl.c
+++ b/drivers/gpu/drm/drm_ioctl.c
@@ -158,14 +158,11 @@ int drm_getmap(struct drm_device *dev, void *data,
 	int i;
 
 	idx = map->offset;
-
-	mutex_lock(&dev->struct_mutex);
-	if (idx < 0) {
-		mutex_unlock(&dev->struct_mutex);
+	if (idx < 0)
 		return -EINVAL;
-	}
 
 	i = 0;
+	mutex_lock(&dev->struct_mutex);
 	list_for_each(list, &dev->maplist) {
 		if (i == idx) {
 			r_list = list_entry(list, struct drm_map_list, head);
@@ -211,9 +208,9 @@ int drm_getclient(struct drm_device *dev, void *data,
 	int i;
 
 	idx = client->idx;
-	mutex_lock(&dev->struct_mutex);
-
 	i = 0;
+
+	mutex_lock(&dev->struct_mutex);
 	list_for_each_entry(pt, &dev->filelist, lhead) {
 		if (i++ >= idx) {
 			client->auth = pt->authenticated;
@@ -249,8 +246,6 @@ int drm_getstats(struct drm_device *dev, void *data,
 
 	memset(stats, 0, sizeof(*stats));
 
-	mutex_lock(&dev->struct_mutex);
-
 	for (i = 0; i < dev->counters; i++) {
 		if (dev->types[i] == _DRM_STAT_LOCK)
 			stats->data[i].value =
@@ -262,8 +257,6 @@ int drm_getstats(struct drm_device *dev, void *data,
 
 	stats->count = dev->counters;
 
-	mutex_unlock(&dev->struct_mutex);
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/drm_lock.c b/drivers/gpu/drm/drm_lock.c
index 632ae243ede0..c79c713eeba0 100644
--- a/drivers/gpu/drm/drm_lock.c
+++ b/drivers/gpu/drm/drm_lock.c
@@ -33,6 +33,7 @@
  * OTHER DEALINGS IN THE SOFTWARE.
  */
 
+#include <linux/export.h>
 #include "drmP.h"
 
 static int drm_notifier(void *priv);
@@ -345,6 +346,7 @@ void drm_idlelock_take(struct drm_lock_data *lock_data)
 	}
 	spin_unlock_bh(&lock_data->spinlock);
 }
+EXPORT_SYMBOL(drm_idlelock_take);
 
 void drm_idlelock_release(struct drm_lock_data *lock_data)
 {
@@ -364,6 +366,7 @@ void drm_idlelock_release(struct drm_lock_data *lock_data)
 	}
 	spin_unlock_bh(&lock_data->spinlock);
 }
+EXPORT_SYMBOL(drm_idlelock_release);
 
 int drm_i_have_hw_lock(struct drm_device *dev, struct drm_file *file_priv)
 {
diff --git a/drivers/gpu/drm/drm_sman.c b/drivers/gpu/drm/drm_sman.c
deleted file mode 100644
index cebce45f4429..000000000000
--- a/drivers/gpu/drm/drm_sman.c
+++ /dev/null
@@ -1,351 +0,0 @@
-/**************************************************************************
- *
- * Copyright 2006 Tungsten Graphics, Inc., Bismarck., ND., USA.
- * All Rights Reserved.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the
- * "Software"), to deal in the Software without restriction, including
- * without limitation the rights to use, copy, modify, merge, publish,
- * distribute, sub license, and/or sell copies of the Software, and to
- * permit persons to whom the Software is furnished to do so, subject to
- * the following conditions:
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
- * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
- * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
- * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
- * USE OR OTHER DEALINGS IN THE SOFTWARE.
- *
- * The above copyright notice and this permission notice (including the
- * next paragraph) shall be included in all copies or substantial portions
- * of the Software.
- *
- *
- **************************************************************************/
-/*
- * Simple memory manager interface that keeps track on allocate regions on a
- * per "owner" basis. All regions associated with an "owner" can be released
- * with a simple call. Typically if the "owner" exists. The owner is any
- * "unsigned long" identifier. Can typically be a pointer to a file private
- * struct or a context identifier.
- *
- * Authors:
- * Thomas Hellström <thomas-at-tungstengraphics-dot-com>
- */
-
-#include <linux/export.h>
-#include "drm_sman.h"
-
-struct drm_owner_item {
-	struct drm_hash_item owner_hash;
-	struct list_head sman_list;
-	struct list_head mem_blocks;
-};
-
-void drm_sman_takedown(struct drm_sman * sman)
-{
-	drm_ht_remove(&sman->user_hash_tab);
-	drm_ht_remove(&sman->owner_hash_tab);
-	kfree(sman->mm);
-}
-
-EXPORT_SYMBOL(drm_sman_takedown);
-
-int
-drm_sman_init(struct drm_sman * sman, unsigned int num_managers,
-	      unsigned int user_order, unsigned int owner_order)
-{
-	int ret = 0;
-
-	sman->mm = kcalloc(num_managers, sizeof(*sman->mm), GFP_KERNEL);
-	if (!sman->mm) {
-		ret = -ENOMEM;
-		goto out;
-	}
-	sman->num_managers = num_managers;
-	INIT_LIST_HEAD(&sman->owner_items);
-	ret = drm_ht_create(&sman->owner_hash_tab, owner_order);
-	if (ret)
-		goto out1;
-	ret = drm_ht_create(&sman->user_hash_tab, user_order);
-	if (!ret)
-		goto out;
-
-	drm_ht_remove(&sman->owner_hash_tab);
-out1:
-	kfree(sman->mm);
-out:
-	return ret;
-}
-
-EXPORT_SYMBOL(drm_sman_init);
-
-static void *drm_sman_mm_allocate(void *private, unsigned long size,
-				  unsigned alignment)
-{
-	struct drm_mm *mm = (struct drm_mm *) private;
-	struct drm_mm_node *tmp;
-
-	tmp = drm_mm_search_free(mm, size, alignment, 1);
-	if (!tmp) {
-		return NULL;
-	}
-	tmp = drm_mm_get_block(tmp, size, alignment);
-	return tmp;
-}
-
-static void drm_sman_mm_free(void *private, void *ref)
-{
-	struct drm_mm_node *node = (struct drm_mm_node *) ref;
-
-	drm_mm_put_block(node);
-}
-
-static void drm_sman_mm_destroy(void *private)
-{
-	struct drm_mm *mm = (struct drm_mm *) private;
-	drm_mm_takedown(mm);
-	kfree(mm);
-}
-
-static unsigned long drm_sman_mm_offset(void *private, void *ref)
-{
-	struct drm_mm_node *node = (struct drm_mm_node *) ref;
-	return node->start;
-}
-
-int
-drm_sman_set_range(struct drm_sman * sman, unsigned int manager,
-		   unsigned long start, unsigned long size)
-{
-	struct drm_sman_mm *sman_mm;
-	struct drm_mm *mm;
-	int ret;
-
-	BUG_ON(manager >= sman->num_managers);
-
-	sman_mm = &sman->mm[manager];
-	mm = kzalloc(sizeof(*mm), GFP_KERNEL);
-	if (!mm) {
-		return -ENOMEM;
-	}
-	sman_mm->private = mm;
-	ret = drm_mm_init(mm, start, size);
-
-	if (ret) {
-		kfree(mm);
-		return ret;
-	}
-
-	sman_mm->allocate = drm_sman_mm_allocate;
-	sman_mm->free = drm_sman_mm_free;
-	sman_mm->destroy = drm_sman_mm_destroy;
-	sman_mm->offset = drm_sman_mm_offset;
-
-	return 0;
-}
-
-EXPORT_SYMBOL(drm_sman_set_range);
-
-int
-drm_sman_set_manager(struct drm_sman * sman, unsigned int manager,
-		     struct drm_sman_mm * allocator)
-{
-	BUG_ON(manager >= sman->num_managers);
-	sman->mm[manager] = *allocator;
-
-	return 0;
-}
-EXPORT_SYMBOL(drm_sman_set_manager);
-
-static struct drm_owner_item *drm_sman_get_owner_item(struct drm_sman * sman,
-						 unsigned long owner)
-{
-	int ret;
-	struct drm_hash_item *owner_hash_item;
-	struct drm_owner_item *owner_item;
-
-	ret = drm_ht_find_item(&sman->owner_hash_tab, owner, &owner_hash_item);
-	if (!ret) {
-		return drm_hash_entry(owner_hash_item, struct drm_owner_item,
-				      owner_hash);
-	}
-
-	owner_item = kzalloc(sizeof(*owner_item), GFP_KERNEL);
-	if (!owner_item)
-		goto out;
-
-	INIT_LIST_HEAD(&owner_item->mem_blocks);
-	owner_item->owner_hash.key = owner;
-	if (drm_ht_insert_item(&sman->owner_hash_tab, &owner_item->owner_hash))
-		goto out1;
-
-	list_add_tail(&owner_item->sman_list, &sman->owner_items);
-	return owner_item;
-
-out1:
-	kfree(owner_item);
-out:
-	return NULL;
-}
-
-struct drm_memblock_item *drm_sman_alloc(struct drm_sman *sman, unsigned int manager,
-				    unsigned long size, unsigned alignment,
-				    unsigned long owner)
-{
-	void *tmp;
-	struct drm_sman_mm *sman_mm;
-	struct drm_owner_item *owner_item;
-	struct drm_memblock_item *memblock;
-
-	BUG_ON(manager >= sman->num_managers);
-
-	sman_mm = &sman->mm[manager];
-	tmp = sman_mm->allocate(sman_mm->private, size, alignment);
-
-	if (!tmp) {
-		return NULL;
-	}
-
-	memblock = kzalloc(sizeof(*memblock), GFP_KERNEL);
-
-	if (!memblock)
-		goto out;
-
-	memblock->mm_info = tmp;
-	memblock->mm = sman_mm;
-	memblock->sman = sman;
-
-	if (drm_ht_just_insert_please
-	    (&sman->user_hash_tab, &memblock->user_hash,
-	     (unsigned long)memblock, 32, 0, 0))
-		goto out1;
-
-	owner_item = drm_sman_get_owner_item(sman, owner);
-	if (!owner_item)
-		goto out2;
-
-	list_add_tail(&memblock->owner_list, &owner_item->mem_blocks);
-
-	return memblock;
-
-out2:
-	drm_ht_remove_item(&sman->user_hash_tab, &memblock->user_hash);
-out1:
-	kfree(memblock);
-out:
-	sman_mm->free(sman_mm->private, tmp);
-
-	return NULL;
-}
-
-EXPORT_SYMBOL(drm_sman_alloc);
-
-static void drm_sman_free(struct drm_memblock_item *item)
-{
-	struct drm_sman *sman = item->sman;
-
-	list_del(&item->owner_list);
-	drm_ht_remove_item(&sman->user_hash_tab, &item->user_hash);
-	item->mm->free(item->mm->private, item->mm_info);
-	kfree(item);
-}
-
-int drm_sman_free_key(struct drm_sman *sman, unsigned int key)
-{
-	struct drm_hash_item *hash_item;
-	struct drm_memblock_item *memblock_item;
-
-	if (drm_ht_find_item(&sman->user_hash_tab, key, &hash_item))
-		return -EINVAL;
-
-	memblock_item = drm_hash_entry(hash_item, struct drm_memblock_item,
-				       user_hash);
-	drm_sman_free(memblock_item);
-	return 0;
-}
-
-EXPORT_SYMBOL(drm_sman_free_key);
-
-static void drm_sman_remove_owner(struct drm_sman *sman,
-				  struct drm_owner_item *owner_item)
-{
-	list_del(&owner_item->sman_list);
-	drm_ht_remove_item(&sman->owner_hash_tab, &owner_item->owner_hash);
-	kfree(owner_item);
-}
-
-int drm_sman_owner_clean(struct drm_sman *sman, unsigned long owner)
-{
-
-	struct drm_hash_item *hash_item;
-	struct drm_owner_item *owner_item;
-
-	if (drm_ht_find_item(&sman->owner_hash_tab, owner, &hash_item)) {
-		return -1;
-	}
-
-	owner_item = drm_hash_entry(hash_item, struct drm_owner_item, owner_hash);
-	if (owner_item->mem_blocks.next == &owner_item->mem_blocks) {
-		drm_sman_remove_owner(sman, owner_item);
-		return -1;
-	}
-
-	return 0;
-}
-
-EXPORT_SYMBOL(drm_sman_owner_clean);
-
-static void drm_sman_do_owner_cleanup(struct drm_sman *sman,
-				      struct drm_owner_item *owner_item)
-{
-	struct drm_memblock_item *entry, *next;
-
-	list_for_each_entry_safe(entry, next, &owner_item->mem_blocks,
-				 owner_list) {
-		drm_sman_free(entry);
-	}
-	drm_sman_remove_owner(sman, owner_item);
-}
-
-void drm_sman_owner_cleanup(struct drm_sman *sman, unsigned long owner)
-{
-
-	struct drm_hash_item *hash_item;
-	struct drm_owner_item *owner_item;
-
-	if (drm_ht_find_item(&sman->owner_hash_tab, owner, &hash_item)) {
-
-		return;
-	}
-
-	owner_item = drm_hash_entry(hash_item, struct drm_owner_item, owner_hash);
-	drm_sman_do_owner_cleanup(sman, owner_item);
-}
-
-EXPORT_SYMBOL(drm_sman_owner_cleanup);
-
-void drm_sman_cleanup(struct drm_sman *sman)
-{
-	struct drm_owner_item *entry, *next;
-	unsigned int i;
-	struct drm_sman_mm *sman_mm;
-
-	list_for_each_entry_safe(entry, next, &sman->owner_items, sman_list) {
-		drm_sman_do_owner_cleanup(sman, entry);
-	}
-	if (sman->mm) {
-		for (i = 0; i < sman->num_managers; ++i) {
-			sman_mm = &sman->mm[i];
-			if (sman_mm->private) {
-				sman_mm->destroy(sman_mm->private);
-				sman_mm->private = NULL;
-			}
-		}
-	}
-}
-
-EXPORT_SYMBOL(drm_sman_cleanup);
diff --git a/drivers/gpu/drm/exynos/Kconfig b/drivers/gpu/drm/exynos/Kconfig
index 847466aab435..f9aaa56eae07 100644
--- a/drivers/gpu/drm/exynos/Kconfig
+++ b/drivers/gpu/drm/exynos/Kconfig
@@ -18,3 +18,10 @@ config DRM_EXYNOS_FIMD
 	help
 	  Choose this option if you want to use Exynos FIMD for DRM.
 	  If M is selected, the module will be called exynos_drm_fimd
+
+config DRM_EXYNOS_HDMI
+	tristate "Exynos DRM HDMI"
+	depends on DRM_EXYNOS
+	help
+	  Choose this option if you want to use Exynos HDMI for DRM.
+	  If M is selected, the module will be called exynos_drm_hdmi
diff --git a/drivers/gpu/drm/exynos/Makefile b/drivers/gpu/drm/exynos/Makefile
index 0496d3ff2683..395e69c9a96e 100644
--- a/drivers/gpu/drm/exynos/Makefile
+++ b/drivers/gpu/drm/exynos/Makefile
@@ -5,7 +5,10 @@
 ccflags-y := -Iinclude/drm -Idrivers/gpu/drm/exynos
 exynosdrm-y := exynos_drm_drv.o exynos_drm_encoder.o exynos_drm_connector.o \
 		exynos_drm_crtc.o exynos_drm_fbdev.o exynos_drm_fb.o \
-		exynos_drm_buf.o exynos_drm_gem.o exynos_drm_core.o
+		exynos_drm_buf.o exynos_drm_gem.o exynos_drm_core.o \
+		exynos_drm_plane.o
 
 obj-$(CONFIG_DRM_EXYNOS) += exynosdrm.o
 obj-$(CONFIG_DRM_EXYNOS_FIMD) += exynos_drm_fimd.o
+obj-$(CONFIG_DRM_EXYNOS_HDMI) += exynos_hdmi.o exynos_mixer.o exynos_ddc.o \
+				 exynos_hdmiphy.o exynos_drm_hdmi.o
diff --git a/drivers/gpu/drm/exynos/exynos_ddc.c b/drivers/gpu/drm/exynos/exynos_ddc.c
new file mode 100644
index 000000000000..84b614fe26fd
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_ddc.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Authors:
+ *	Seung-Woo Kim <sw0312.kim@samsung.com>
+ *	Inki Dae <inki.dae@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include "drmP.h"
+
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+
+
+#include "exynos_drm_drv.h"
+#include "exynos_hdmi.h"
+
+static int s5p_ddc_probe(struct i2c_client *client,
+			const struct i2c_device_id *dev_id)
+{
+	hdmi_attach_ddc_client(client);
+
+	dev_info(&client->adapter->dev, "attached s5p_ddc "
+		"into i2c adapter successfully\n");
+
+	return 0;
+}
+
+static int s5p_ddc_remove(struct i2c_client *client)
+{
+	dev_info(&client->adapter->dev, "detached s5p_ddc "
+		"from i2c adapter successfully\n");
+
+	return 0;
+}
+
+static struct i2c_device_id ddc_idtable[] = {
+	{"s5p_ddc", 0},
+	{ },
+};
+
+struct i2c_driver ddc_driver = {
+	.driver = {
+		.name = "s5p_ddc",
+		.owner = THIS_MODULE,
+	},
+	.id_table	= ddc_idtable,
+	.probe		= s5p_ddc_probe,
+	.remove		= __devexit_p(s5p_ddc_remove),
+	.command		= NULL,
+};
+EXPORT_SYMBOL(ddc_driver);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_buf.c b/drivers/gpu/drm/exynos/exynos_drm_buf.c
index 2bb07bca511a..3cf785c58186 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_buf.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_buf.c
@@ -73,7 +73,7 @@ struct exynos_drm_gem_buf *exynos_drm_buf_create(struct drm_device *dev,
 	buffer = kzalloc(sizeof(*buffer), GFP_KERNEL);
 	if (!buffer) {
 		DRM_ERROR("failed to allocate exynos_drm_gem_buf.\n");
-		return ERR_PTR(-ENOMEM);
+		return NULL;
 	}
 
 	buffer->size = size;
@@ -84,8 +84,7 @@ struct exynos_drm_gem_buf *exynos_drm_buf_create(struct drm_device *dev,
 	 */
 	if (lowlevel_buffer_allocate(dev, buffer) < 0) {
 		kfree(buffer);
-		buffer = NULL;
-		return ERR_PTR(-ENOMEM);
+		return NULL;
 	}
 
 	return buffer;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_buf.h b/drivers/gpu/drm/exynos/exynos_drm_buf.h
index 6e91f9caa5db..c913f2bad760 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_buf.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_buf.h
@@ -30,9 +30,6 @@
 struct exynos_drm_gem_buf *exynos_drm_buf_create(struct drm_device *dev,
 		unsigned int size);
 
-/* get memory information of a drm framebuffer. */
-struct exynos_drm_gem_buf *exynos_drm_fb_get_buf(struct drm_framebuffer *fb);
-
 /* remove allocated physical memory. */
 void exynos_drm_buf_destroy(struct drm_device *dev,
 		struct exynos_drm_gem_buf *buffer);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.c b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
index ee43cc220853..e3861ac49295 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
@@ -34,7 +34,6 @@
 #include "exynos_drm_fb.h"
 #include "exynos_drm_encoder.h"
 #include "exynos_drm_gem.h"
-#include "exynos_drm_buf.h"
 
 #define to_exynos_crtc(x)	container_of(x, struct exynos_drm_crtc,\
 				drm_crtc)
@@ -52,11 +51,13 @@
  *	drm framework doesn't support multiple irq yet.
  *	we can refer to the crtc to current hardware interrupt occured through
  *	this pipe value.
+ * @dpms: store the crtc dpms value
  */
 struct exynos_drm_crtc {
 	struct drm_crtc			drm_crtc;
 	struct exynos_drm_overlay	overlay;
 	unsigned int			pipe;
+	unsigned int			dpms;
 };
 
 static void exynos_drm_crtc_apply(struct drm_crtc *crtc)
@@ -78,19 +79,23 @@ int exynos_drm_overlay_update(struct exynos_drm_overlay *overlay,
 	struct exynos_drm_gem_buf *buffer;
 	unsigned int actual_w;
 	unsigned int actual_h;
+	int nr = exynos_drm_format_num_buffers(fb->pixel_format);
+	int i;
+
+	for (i = 0; i < nr; i++) {
+		buffer = exynos_drm_fb_buffer(fb, i);
+		if (!buffer) {
+			DRM_LOG_KMS("buffer is null\n");
+			return -EFAULT;
+		}
 
-	buffer = exynos_drm_fb_get_buf(fb);
-	if (!buffer) {
-		DRM_LOG_KMS("buffer is null.\n");
-		return -EFAULT;
-	}
-
-	overlay->dma_addr = buffer->dma_addr;
-	overlay->vaddr = buffer->kvaddr;
+		overlay->dma_addr[i] = buffer->dma_addr;
+		overlay->vaddr[i] = buffer->kvaddr;
 
-	DRM_DEBUG_KMS("vaddr = 0x%lx, dma_addr = 0x%lx\n",
-			(unsigned long)overlay->vaddr,
-			(unsigned long)overlay->dma_addr);
+		DRM_DEBUG_KMS("buffer: %d, vaddr = 0x%lx, dma_addr = 0x%lx\n",
+				i, (unsigned long)overlay->vaddr[i],
+				(unsigned long)overlay->dma_addr[i]);
+	}
 
 	actual_w = min((mode->hdisplay - pos->crtc_x), pos->crtc_w);
 	actual_h = min((mode->vdisplay - pos->crtc_y), pos->crtc_h);
@@ -101,7 +106,8 @@ int exynos_drm_overlay_update(struct exynos_drm_overlay *overlay,
 	overlay->fb_width = fb->width;
 	overlay->fb_height = fb->height;
 	overlay->bpp = fb->bits_per_pixel;
-	overlay->pitch = fb->pitch;
+	overlay->pitch = fb->pitches[0];
+	overlay->pixel_format = fb->pixel_format;
 
 	/* set overlay range to be displayed. */
 	overlay->crtc_x = pos->crtc_x;
@@ -153,26 +159,37 @@ static int exynos_drm_crtc_update(struct drm_crtc *crtc)
 
 static void exynos_drm_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
+	struct drm_device *dev = crtc->dev;
 	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
 
 	DRM_DEBUG_KMS("crtc[%d] mode[%d]\n", crtc->base.id, mode);
 
+	if (exynos_crtc->dpms == mode) {
+		DRM_DEBUG_KMS("desired dpms mode is same as previous one.\n");
+		return;
+	}
+
+	mutex_lock(&dev->struct_mutex);
+
 	switch (mode) {
 	case DRM_MODE_DPMS_ON:
-		exynos_drm_fn_encoder(crtc, &exynos_crtc->pipe,
-				exynos_drm_encoder_crtc_commit);
+		exynos_drm_fn_encoder(crtc, &mode,
+				exynos_drm_encoder_crtc_dpms);
+		exynos_crtc->dpms = mode;
 		break;
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 	case DRM_MODE_DPMS_OFF:
-		/* TODO */
-		exynos_drm_fn_encoder(crtc, NULL,
-				exynos_drm_encoder_crtc_disable);
+		exynos_drm_fn_encoder(crtc, &mode,
+				exynos_drm_encoder_crtc_dpms);
+		exynos_crtc->dpms = mode;
 		break;
 	default:
-		DRM_DEBUG_KMS("unspecified mode %d\n", mode);
+		DRM_ERROR("unspecified mode %d\n", mode);
 		break;
 	}
+
+	mutex_unlock(&dev->struct_mutex);
 }
 
 static void exynos_drm_crtc_prepare(struct drm_crtc *crtc)
@@ -188,6 +205,28 @@ static void exynos_drm_crtc_commit(struct drm_crtc *crtc)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	/*
+	 * when set_crtc is requested from user or at booting time,
+	 * crtc->commit would be called without dpms call so if dpms is
+	 * no power on then crtc->dpms should be called
+	 * with DRM_MODE_DPMS_ON for the hardware power to be on.
+	 */
+	if (exynos_crtc->dpms != DRM_MODE_DPMS_ON) {
+		int mode = DRM_MODE_DPMS_ON;
+
+		/*
+		 * enable hardware(power on) to all encoders hdmi connected
+		 * to current crtc.
+		 */
+		exynos_drm_crtc_dpms(crtc, mode);
+		/*
+		 * enable dma to all encoders connected to current crtc and
+		 * lcd panel.
+		 */
+		exynos_drm_fn_encoder(crtc, &mode,
+					exynos_drm_encoder_dpms_from_crtc);
+	}
+
 	exynos_drm_fn_encoder(crtc, &exynos_crtc->pipe,
 			exynos_drm_encoder_crtc_commit);
 }
@@ -344,6 +383,8 @@ int exynos_drm_crtc_create(struct drm_device *dev, unsigned int nr)
 	}
 
 	exynos_crtc->pipe = nr;
+	exynos_crtc->dpms = DRM_MODE_DPMS_OFF;
+	exynos_crtc->overlay.zpos = DEFAULT_ZPOS;
 	crtc = &exynos_crtc->drm_crtc;
 
 	private->crtc[nr] = crtc;
@@ -357,9 +398,14 @@ int exynos_drm_crtc_create(struct drm_device *dev, unsigned int nr)
 int exynos_drm_crtc_enable_vblank(struct drm_device *dev, int crtc)
 {
 	struct exynos_drm_private *private = dev->dev_private;
+	struct exynos_drm_crtc *exynos_crtc =
+		to_exynos_crtc(private->crtc[crtc]);
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (exynos_crtc->dpms != DRM_MODE_DPMS_ON)
+		return -EPERM;
+
 	exynos_drm_fn_encoder(private->crtc[crtc], &crtc,
 			exynos_drm_enable_vblank);
 
@@ -369,9 +415,14 @@ int exynos_drm_crtc_enable_vblank(struct drm_device *dev, int crtc)
 void exynos_drm_crtc_disable_vblank(struct drm_device *dev, int crtc)
 {
 	struct exynos_drm_private *private = dev->dev_private;
+	struct exynos_drm_crtc *exynos_crtc =
+		to_exynos_crtc(private->crtc[crtc]);
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (exynos_crtc->dpms != DRM_MODE_DPMS_ON)
+		return;
+
 	exynos_drm_fn_encoder(private->crtc[crtc], &crtc,
 			exynos_drm_disable_vblank);
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.c b/drivers/gpu/drm/exynos/exynos_drm_drv.c
index 53e2216de61d..35889ca255e9 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.c
@@ -36,13 +36,16 @@
 #include "exynos_drm_fbdev.h"
 #include "exynos_drm_fb.h"
 #include "exynos_drm_gem.h"
+#include "exynos_drm_plane.h"
 
-#define DRIVER_NAME	"exynos-drm"
+#define DRIVER_NAME	"exynos"
 #define DRIVER_DESC	"Samsung SoC DRM"
 #define DRIVER_DATE	"20110530"
 #define DRIVER_MAJOR	1
 #define DRIVER_MINOR	0
 
+#define VBLANK_OFF_DELAY	50000
+
 static int exynos_drm_load(struct drm_device *dev, unsigned long flags)
 {
 	struct exynos_drm_private *private;
@@ -77,6 +80,12 @@ static int exynos_drm_load(struct drm_device *dev, unsigned long flags)
 			goto err_crtc;
 	}
 
+	for (nr = 0; nr < MAX_PLANE; nr++) {
+		ret = exynos_plane_init(dev, nr);
+		if (ret)
+			goto err_crtc;
+	}
+
 	ret = drm_vblank_init(dev, MAX_CRTC);
 	if (ret)
 		goto err_crtc;
@@ -100,6 +109,8 @@ static int exynos_drm_load(struct drm_device *dev, unsigned long flags)
 		goto err_drm_device;
 	}
 
+	drm_vblank_offdelay = VBLANK_OFF_DELAY;
+
 	return 0;
 
 err_drm_device:
@@ -163,6 +174,18 @@ static struct drm_ioctl_desc exynos_ioctls[] = {
 			DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_MMAP,
 			exynos_drm_gem_mmap_ioctl, DRM_UNLOCKED | DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(EXYNOS_PLANE_SET_ZPOS, exynos_plane_set_zpos_ioctl,
+			DRM_UNLOCKED | DRM_AUTH),
+};
+
+static const struct file_operations exynos_drm_driver_fops = {
+	.owner		= THIS_MODULE,
+	.open		= drm_open,
+	.mmap		= exynos_drm_gem_mmap,
+	.poll		= drm_poll,
+	.read		= drm_read,
+	.unlocked_ioctl	= drm_ioctl,
+	.release	= drm_release,
 };
 
 static struct drm_driver exynos_drm_driver = {
@@ -182,15 +205,7 @@ static struct drm_driver exynos_drm_driver = {
 	.dumb_map_offset	= exynos_drm_gem_dumb_map_offset,
 	.dumb_destroy		= exynos_drm_gem_dumb_destroy,
 	.ioctls			= exynos_ioctls,
-	.fops = {
-		.owner		= THIS_MODULE,
-		.open		= drm_open,
-		.mmap		= exynos_drm_gem_mmap,
-		.poll		= drm_poll,
-		.read		= drm_read,
-		.unlocked_ioctl	= drm_ioctl,
-		.release	= drm_release,
-	},
+	.fops			= &exynos_drm_driver_fops,
 	.name	= DRIVER_NAME,
 	.desc	= DRIVER_DESC,
 	.date	= DRIVER_DATE,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index 5e02e6ecc2e0..e685e1e33055 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -33,11 +33,16 @@
 #include "drm.h"
 
 #define MAX_CRTC	2
+#define MAX_PLANE	5
+#define MAX_FB_BUFFER	3
+#define DEFAULT_ZPOS	-1
 
 struct drm_device;
 struct exynos_drm_overlay;
 struct drm_connector;
 
+extern unsigned int drm_vblank_offdelay;
+
 /* this enumerates display type. */
 enum exynos_drm_output_type {
 	EXYNOS_DISPLAY_TYPE_NONE,
@@ -57,8 +62,8 @@ enum exynos_drm_output_type {
 struct exynos_drm_overlay_ops {
 	void (*mode_set)(struct device *subdrv_dev,
 			 struct exynos_drm_overlay *overlay);
-	void (*commit)(struct device *subdrv_dev);
-	void (*disable)(struct device *subdrv_dev);
+	void (*commit)(struct device *subdrv_dev, int zpos);
+	void (*disable)(struct device *subdrv_dev, int zpos);
 };
 
 /*
@@ -80,9 +85,11 @@ struct exynos_drm_overlay_ops {
  * @scan_flag: interlace or progressive way.
  *	(it could be DRM_MODE_FLAG_*)
  * @bpp: pixel size.(in bit)
- * @dma_addr: bus(accessed by dma) address to the memory region allocated
- *	for a overlay.
- * @vaddr: virtual memory addresss to this overlay.
+ * @pixel_format: fourcc pixel format of this overlay
+ * @dma_addr: array of bus(accessed by dma) address to the memory region
+ *	      allocated for a overlay.
+ * @vaddr: array of virtual memory addresss to this overlay.
+ * @zpos: order of overlay layer(z position).
  * @default_win: a window to be enabled.
  * @color_key: color key on or off.
  * @index_color: if using color key feature then this value would be used
@@ -109,8 +116,10 @@ struct exynos_drm_overlay {
 	unsigned int scan_flag;
 	unsigned int bpp;
 	unsigned int pitch;
-	dma_addr_t dma_addr;
-	void __iomem *vaddr;
+	uint32_t pixel_format;
+	dma_addr_t dma_addr[MAX_FB_BUFFER];
+	void __iomem *vaddr[MAX_FB_BUFFER];
+	int zpos;
 
 	bool default_win;
 	bool color_key;
@@ -144,17 +153,19 @@ struct exynos_drm_display_ops {
 /*
  * Exynos drm manager ops
  *
+ * @dpms: control device power.
+ * @apply: set timing, vblank and overlay data to registers.
  * @mode_set: convert drm_display_mode to hw specific display mode and
  *	      would be called by encoder->mode_set().
  * @commit: set current hw specific display mode to hw.
- * @disable: disable hardware specific display mode.
  * @enable_vblank: specific driver callback for enabling vblank interrupt.
  * @disable_vblank: specific driver callback for disabling vblank interrupt.
  */
 struct exynos_drm_manager_ops {
+	void (*dpms)(struct device *subdrv_dev, int mode);
+	void (*apply)(struct device *subdrv_dev);
 	void (*mode_set)(struct device *subdrv_dev, void *mode);
 	void (*commit)(struct device *subdrv_dev);
-	void (*disable)(struct device *subdrv_dev);
 	int (*enable_vblank)(struct device *subdrv_dev);
 	void (*disable_vblank)(struct device *subdrv_dev);
 };
diff --git a/drivers/gpu/drm/exynos/exynos_drm_encoder.c b/drivers/gpu/drm/exynos/exynos_drm_encoder.c
index 153061415baf..86b93dde219a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_encoder.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_encoder.c
@@ -42,49 +42,68 @@
  * @drm_encoder: encoder object.
  * @manager: specific encoder has its own manager to control a hardware
  *	appropriately and we can access a hardware drawing on this manager.
+ * @dpms: store the encoder dpms value.
  */
 struct exynos_drm_encoder {
 	struct drm_encoder		drm_encoder;
 	struct exynos_drm_manager	*manager;
+	int dpms;
 };
 
-static void exynos_drm_encoder_dpms(struct drm_encoder *encoder, int mode)
+static void exynos_drm_display_power(struct drm_encoder *encoder, int mode)
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_connector *connector;
 	struct exynos_drm_manager *manager = exynos_drm_get_manager(encoder);
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		if (connector->encoder == encoder) {
+			struct exynos_drm_display_ops *display_ops =
+							manager->display_ops;
+
+			DRM_DEBUG_KMS("connector[%d] dpms[%d]\n",
+					connector->base.id, mode);
+			if (display_ops && display_ops->power_on)
+				display_ops->power_on(manager->dev, mode);
+		}
+	}
+}
+
+static void exynos_drm_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct exynos_drm_manager *manager = exynos_drm_get_manager(encoder);
 	struct exynos_drm_manager_ops *manager_ops = manager->ops;
+	struct exynos_drm_encoder *exynos_encoder = to_exynos_encoder(encoder);
 
 	DRM_DEBUG_KMS("%s, encoder dpms: %d\n", __FILE__, mode);
 
+	if (exynos_encoder->dpms == mode) {
+		DRM_DEBUG_KMS("desired dpms mode is same as previous one.\n");
+		return;
+	}
+
+	mutex_lock(&dev->struct_mutex);
+
 	switch (mode) {
 	case DRM_MODE_DPMS_ON:
-		if (manager_ops && manager_ops->commit)
-			manager_ops->commit(manager->dev);
+		if (manager_ops && manager_ops->apply)
+			manager_ops->apply(manager->dev);
+		exynos_drm_display_power(encoder, mode);
+		exynos_encoder->dpms = mode;
 		break;
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 	case DRM_MODE_DPMS_OFF:
-		/* TODO */
-		if (manager_ops && manager_ops->disable)
-			manager_ops->disable(manager->dev);
+		exynos_drm_display_power(encoder, mode);
+		exynos_encoder->dpms = mode;
 		break;
 	default:
 		DRM_ERROR("unspecified mode %d\n", mode);
 		break;
 	}
 
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		if (connector->encoder == encoder) {
-			struct exynos_drm_display_ops *display_ops =
-							manager->display_ops;
-
-			DRM_DEBUG_KMS("connector[%d] dpms[%d]\n",
-					connector->base.id, mode);
-			if (display_ops && display_ops->power_on)
-				display_ops->power_on(manager->dev, mode);
-		}
-	}
+	mutex_unlock(&dev->struct_mutex);
 }
 
 static bool
@@ -169,7 +188,6 @@ static void exynos_drm_encoder_destroy(struct drm_encoder *encoder)
 	exynos_encoder->manager->pipe = -1;
 
 	drm_encoder_cleanup(encoder);
-	encoder->dev->mode_config.num_encoder--;
 	kfree(exynos_encoder);
 }
 
@@ -199,6 +217,7 @@ exynos_drm_encoder_create(struct drm_device *dev,
 		return NULL;
 	}
 
+	exynos_encoder->dpms = DRM_MODE_DPMS_OFF;
 	exynos_encoder->manager = manager;
 	encoder = &exynos_encoder->drm_encoder;
 	encoder->possible_crtcs = possible_crtcs;
@@ -275,12 +294,27 @@ void exynos_drm_disable_vblank(struct drm_encoder *encoder, void *data)
 		manager_ops->disable_vblank(manager->dev);
 }
 
-void exynos_drm_encoder_crtc_commit(struct drm_encoder *encoder, void *data)
+void exynos_drm_encoder_crtc_plane_commit(struct drm_encoder *encoder,
+					  void *data)
 {
 	struct exynos_drm_manager *manager =
 		to_exynos_encoder(encoder)->manager;
 	struct exynos_drm_overlay_ops *overlay_ops = manager->overlay_ops;
+	int zpos = DEFAULT_ZPOS;
+
+	if (data)
+		zpos = *(int *)data;
+
+	if (overlay_ops && overlay_ops->commit)
+		overlay_ops->commit(manager->dev, zpos);
+}
+
+void exynos_drm_encoder_crtc_commit(struct drm_encoder *encoder, void *data)
+{
+	struct exynos_drm_manager *manager =
+		to_exynos_encoder(encoder)->manager;
 	int crtc = *(int *)data;
+	int zpos = DEFAULT_ZPOS;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
@@ -290,8 +324,53 @@ void exynos_drm_encoder_crtc_commit(struct drm_encoder *encoder, void *data)
 	 */
 	manager->pipe = crtc;
 
-	if (overlay_ops && overlay_ops->commit)
-		overlay_ops->commit(manager->dev);
+	exynos_drm_encoder_crtc_plane_commit(encoder, &zpos);
+}
+
+void exynos_drm_encoder_dpms_from_crtc(struct drm_encoder *encoder, void *data)
+{
+	struct exynos_drm_encoder *exynos_encoder = to_exynos_encoder(encoder);
+	int mode = *(int *)data;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	exynos_drm_encoder_dpms(encoder, mode);
+
+	exynos_encoder->dpms = mode;
+}
+
+void exynos_drm_encoder_crtc_dpms(struct drm_encoder *encoder, void *data)
+{
+	struct drm_device *dev = encoder->dev;
+	struct exynos_drm_encoder *exynos_encoder = to_exynos_encoder(encoder);
+	struct exynos_drm_manager *manager = exynos_encoder->manager;
+	struct exynos_drm_manager_ops *manager_ops = manager->ops;
+	struct drm_connector *connector;
+	int mode = *(int *)data;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (manager_ops && manager_ops->dpms)
+		manager_ops->dpms(manager->dev, mode);
+
+	/*
+	 * set current dpms mode to the connector connected to
+	 * current encoder. connector->dpms would be checked
+	 * at drm_helper_connector_dpms()
+	 */
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
+		if (connector->encoder == encoder)
+			connector->dpms = mode;
+
+	/*
+	 * if this condition is ok then it means that the crtc is already
+	 * detached from encoder and last function for detaching is properly
+	 * done, so clear pipe from manager to prevent repeated call.
+	 */
+	if (mode > DRM_MODE_DPMS_ON) {
+		if (!encoder->crtc)
+			manager->pipe = -1;
+	}
 }
 
 void exynos_drm_encoder_crtc_mode_set(struct drm_encoder *encoder, void *data)
@@ -310,19 +389,15 @@ void exynos_drm_encoder_crtc_disable(struct drm_encoder *encoder, void *data)
 	struct exynos_drm_manager *manager =
 		to_exynos_encoder(encoder)->manager;
 	struct exynos_drm_overlay_ops *overlay_ops = manager->overlay_ops;
+	int zpos = DEFAULT_ZPOS;
 
 	DRM_DEBUG_KMS("\n");
 
-	if (overlay_ops && overlay_ops->disable)
-		overlay_ops->disable(manager->dev);
+	if (data)
+		zpos = *(int *)data;
 
-	/*
-	 * crtc is already detached from encoder and last
-	 * function for detaching is properly done, so
-	 * clear pipe from manager to prevent repeated call
-	 */
-	if (!encoder->crtc)
-		manager->pipe = -1;
+	if (overlay_ops && overlay_ops->disable)
+		overlay_ops->disable(manager->dev, zpos);
 }
 
 MODULE_AUTHOR("Inki Dae <inki.dae@samsung.com>");
diff --git a/drivers/gpu/drm/exynos/exynos_drm_encoder.h b/drivers/gpu/drm/exynos/exynos_drm_encoder.h
index a22acfbf0e4e..97b087a51cb6 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_encoder.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_encoder.h
@@ -39,7 +39,12 @@ void exynos_drm_fn_encoder(struct drm_crtc *crtc, void *data,
 			    void (*fn)(struct drm_encoder *, void *));
 void exynos_drm_enable_vblank(struct drm_encoder *encoder, void *data);
 void exynos_drm_disable_vblank(struct drm_encoder *encoder, void *data);
+void exynos_drm_encoder_crtc_plane_commit(struct drm_encoder *encoder,
+					  void *data);
 void exynos_drm_encoder_crtc_commit(struct drm_encoder *encoder, void *data);
+void exynos_drm_encoder_dpms_from_crtc(struct drm_encoder *encoder,
+					void *data);
+void exynos_drm_encoder_crtc_dpms(struct drm_encoder *encoder, void *data);
 void exynos_drm_encoder_crtc_mode_set(struct drm_encoder *encoder, void *data);
 void exynos_drm_encoder_crtc_disable(struct drm_encoder *encoder, void *data);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.c b/drivers/gpu/drm/exynos/exynos_drm_fb.c
index 5bf4a1ac7f82..3733fe6723d3 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.c
@@ -33,7 +33,6 @@
 
 #include "exynos_drm_drv.h"
 #include "exynos_drm_fb.h"
-#include "exynos_drm_buf.h"
 #include "exynos_drm_gem.h"
 
 #define to_exynos_fb(x)	container_of(x, struct exynos_drm_fb, fb)
@@ -42,15 +41,11 @@
  * exynos specific framebuffer structure.
  *
  * @fb: drm framebuffer obejct.
- * @exynos_gem_obj: exynos specific gem object containing a gem object.
- * @buffer: pointer to exynos_drm_gem_buffer object.
- *	- contain the memory information to memory region allocated
- *	at default framebuffer creation.
+ * @exynos_gem_obj: array of exynos specific gem object containing a gem object.
  */
 struct exynos_drm_fb {
 	struct drm_framebuffer		fb;
-	struct exynos_drm_gem_obj	*exynos_gem_obj;
-	struct exynos_drm_gem_buf	*buffer;
+	struct exynos_drm_gem_obj	*exynos_gem_obj[MAX_FB_BUFFER];
 };
 
 static void exynos_drm_fb_destroy(struct drm_framebuffer *fb)
@@ -61,13 +56,6 @@ static void exynos_drm_fb_destroy(struct drm_framebuffer *fb)
 
 	drm_framebuffer_cleanup(fb);
 
-	/*
-	 * default framebuffer has no gem object so
-	 * a buffer of the default framebuffer should be released at here.
-	 */
-	if (!exynos_fb->exynos_gem_obj && exynos_fb->buffer)
-		exynos_drm_buf_destroy(fb->dev, exynos_fb->buffer);
-
 	kfree(exynos_fb);
 	exynos_fb = NULL;
 }
@@ -81,7 +69,7 @@ static int exynos_drm_fb_create_handle(struct drm_framebuffer *fb,
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
 	return drm_gem_handle_create(file_priv,
-			&exynos_fb->exynos_gem_obj->base, handle);
+			&exynos_fb->exynos_gem_obj[0]->base, handle);
 }
 
 static int exynos_drm_fb_dirty(struct drm_framebuffer *fb,
@@ -102,134 +90,88 @@ static struct drm_framebuffer_funcs exynos_drm_fb_funcs = {
 	.dirty		= exynos_drm_fb_dirty,
 };
 
-static struct drm_framebuffer *
-exynos_drm_fb_init(struct drm_file *file_priv, struct drm_device *dev,
-		    struct drm_mode_fb_cmd *mode_cmd)
+struct drm_framebuffer *
+exynos_drm_framebuffer_init(struct drm_device *dev,
+			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    struct drm_gem_object *obj)
 {
 	struct exynos_drm_fb *exynos_fb;
-	struct drm_framebuffer *fb;
-	struct exynos_drm_gem_obj *exynos_gem_obj = NULL;
-	struct drm_gem_object *obj;
-	unsigned int size;
 	int ret;
 
-	DRM_DEBUG_KMS("%s\n", __FILE__);
-
-	mode_cmd->pitch = max(mode_cmd->pitch,
-			mode_cmd->width * (mode_cmd->bpp >> 3));
-
-	DRM_LOG_KMS("drm fb create(%dx%d)\n",
-			mode_cmd->width, mode_cmd->height);
-
 	exynos_fb = kzalloc(sizeof(*exynos_fb), GFP_KERNEL);
 	if (!exynos_fb) {
-		DRM_ERROR("failed to allocate exynos drm framebuffer.\n");
+		DRM_ERROR("failed to allocate exynos drm framebuffer\n");
 		return ERR_PTR(-ENOMEM);
 	}
 
-	fb = &exynos_fb->fb;
-	ret = drm_framebuffer_init(dev, fb, &exynos_drm_fb_funcs);
+	ret = drm_framebuffer_init(dev, &exynos_fb->fb, &exynos_drm_fb_funcs);
 	if (ret) {
-		DRM_ERROR("failed to initialize framebuffer.\n");
-		goto err_init;
+		DRM_ERROR("failed to initialize framebuffer\n");
+		return ERR_PTR(ret);
 	}
 
-	DRM_LOG_KMS("create: fb id: %d\n", fb->base.id);
+	drm_helper_mode_fill_fb_struct(&exynos_fb->fb, mode_cmd);
+	exynos_fb->exynos_gem_obj[0] = to_exynos_gem_obj(obj);
 
-	size = mode_cmd->pitch * mode_cmd->height;
+	return &exynos_fb->fb;
+}
 
-	/*
-	 * mode_cmd->handle could be NULL at booting time or
-	 * with user request. if NULL, a new buffer or a gem object
-	 * would be allocated.
-	 */
-	if (!mode_cmd->handle) {
-		if (!file_priv) {
-			struct exynos_drm_gem_buf *buffer;
-
-			/*
-			 * in case that file_priv is NULL, it allocates
-			 * only buffer and this buffer would be used
-			 * for default framebuffer.
-			 */
-			buffer = exynos_drm_buf_create(dev, size);
-			if (IS_ERR(buffer)) {
-				ret = PTR_ERR(buffer);
-				goto err_buffer;
-			}
-
-			exynos_fb->buffer = buffer;
-
-			DRM_LOG_KMS("default: dma_addr = 0x%lx, size = 0x%x\n",
-					(unsigned long)buffer->dma_addr, size);
-
-			goto out;
-		} else {
-			exynos_gem_obj = exynos_drm_gem_create(dev, file_priv,
-							&mode_cmd->handle,
-							size);
-			if (IS_ERR(exynos_gem_obj)) {
-				ret = PTR_ERR(exynos_gem_obj);
-				goto err_buffer;
-			}
-		}
-	} else {
-		obj = drm_gem_object_lookup(dev, file_priv, mode_cmd->handle);
-		if (!obj) {
-			DRM_ERROR("failed to lookup gem object.\n");
-			goto err_buffer;
-		}
+static struct drm_framebuffer *
+exynos_user_fb_create(struct drm_device *dev, struct drm_file *file_priv,
+		      struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	struct drm_gem_object *obj;
+	struct drm_framebuffer *fb;
+	struct exynos_drm_fb *exynos_fb;
+	int nr;
+	int i;
 
-		exynos_gem_obj = to_exynos_gem_obj(obj);
+	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-		drm_gem_object_unreference_unlocked(obj);
+	obj = drm_gem_object_lookup(dev, file_priv, mode_cmd->handles[0]);
+	if (!obj) {
+		DRM_ERROR("failed to lookup gem object\n");
+		return ERR_PTR(-ENOENT);
 	}
 
-	/*
-	 * if got a exynos_gem_obj from either a handle or
-	 * a new creation then exynos_fb->exynos_gem_obj is NULL
-	 * so that default framebuffer has no its own gem object,
-	 * only its own buffer object.
-	 */
-	exynos_fb->buffer = exynos_gem_obj->buffer;
-
-	DRM_LOG_KMS("dma_addr = 0x%lx, size = 0x%x, gem object = 0x%x\n",
-			(unsigned long)exynos_fb->buffer->dma_addr, size,
-			(unsigned int)&exynos_gem_obj->base);
+	drm_gem_object_unreference_unlocked(obj);
 
-out:
-	exynos_fb->exynos_gem_obj = exynos_gem_obj;
+	fb = exynos_drm_framebuffer_init(dev, mode_cmd, obj);
+	if (IS_ERR(fb))
+		return fb;
 
-	drm_helper_mode_fill_fb_struct(fb, mode_cmd);
+	exynos_fb = to_exynos_fb(fb);
+	nr = exynos_drm_format_num_buffers(fb->pixel_format);
 
-	return fb;
-
-err_buffer:
-	drm_framebuffer_cleanup(fb);
-
-err_init:
-	kfree(exynos_fb);
+	for (i = 1; i < nr; i++) {
+		obj = drm_gem_object_lookup(dev, file_priv,
+				mode_cmd->handles[i]);
+		if (!obj) {
+			DRM_ERROR("failed to lookup gem object\n");
+			exynos_drm_fb_destroy(fb);
+			return ERR_PTR(-ENOENT);
+		}
 
-	return ERR_PTR(ret);
-}
+		drm_gem_object_unreference_unlocked(obj);
 
-struct drm_framebuffer *exynos_drm_fb_create(struct drm_device *dev,
-					      struct drm_file *file_priv,
-					      struct drm_mode_fb_cmd *mode_cmd)
-{
-	DRM_DEBUG_KMS("%s\n", __FILE__);
+		exynos_fb->exynos_gem_obj[i] = to_exynos_gem_obj(obj);
+	}
 
-	return exynos_drm_fb_init(file_priv, dev, mode_cmd);
+	return fb;
 }
 
-struct exynos_drm_gem_buf *exynos_drm_fb_get_buf(struct drm_framebuffer *fb)
+struct exynos_drm_gem_buf *exynos_drm_fb_buffer(struct drm_framebuffer *fb,
+						int index)
 {
 	struct exynos_drm_fb *exynos_fb = to_exynos_fb(fb);
 	struct exynos_drm_gem_buf *buffer;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	buffer = exynos_fb->buffer;
+	if (index >= MAX_FB_BUFFER)
+		return NULL;
+
+	buffer = exynos_fb->exynos_gem_obj[index]->buffer;
 	if (!buffer)
 		return NULL;
 
@@ -250,7 +192,7 @@ static void exynos_drm_output_poll_changed(struct drm_device *dev)
 }
 
 static struct drm_mode_config_funcs exynos_drm_mode_config_funcs = {
-	.fb_create = exynos_drm_fb_create,
+	.fb_create = exynos_user_fb_create,
 	.output_poll_changed = exynos_drm_output_poll_changed,
 };
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.h b/drivers/gpu/drm/exynos/exynos_drm_fb.h
index eb35931d302c..3ecb30d93552 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.h
@@ -28,9 +28,27 @@
 #ifndef _EXYNOS_DRM_FB_H_
 #define _EXYNOS_DRM_FB_H
 
-struct drm_framebuffer *exynos_drm_fb_create(struct drm_device *dev,
-					      struct drm_file *filp,
-					      struct drm_mode_fb_cmd *mode_cmd);
+static inline int exynos_drm_format_num_buffers(uint32_t format)
+{
+	switch (format) {
+	case DRM_FORMAT_NV12M:
+	case DRM_FORMAT_NV12MT:
+		return 2;
+	case DRM_FORMAT_YUV420M:
+		return 3;
+	default:
+		return 1;
+	}
+}
+
+struct drm_framebuffer *
+exynos_drm_framebuffer_init(struct drm_device *dev,
+			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    struct drm_gem_object *obj);
+
+/* get memory information of a drm framebuffer */
+struct exynos_drm_gem_buf *exynos_drm_fb_buffer(struct drm_framebuffer *fb,
+						 int index);
 
 void exynos_drm_mode_config_init(struct drm_device *dev);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fbdev.c b/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
index 836f41008187..d7ae29d2f3d6 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
@@ -34,7 +34,6 @@
 #include "exynos_drm_drv.h"
 #include "exynos_drm_fb.h"
 #include "exynos_drm_gem.h"
-#include "exynos_drm_buf.h"
 
 #define MAX_CONNECTOR		4
 #define PREFERRED_BPP		32
@@ -43,8 +42,8 @@
 				drm_fb_helper)
 
 struct exynos_drm_fbdev {
-	struct drm_fb_helper	drm_fb_helper;
-	struct drm_framebuffer	*fb;
+	struct drm_fb_helper		drm_fb_helper;
+	struct exynos_drm_gem_obj	*exynos_gem_obj;
 };
 
 static int exynos_drm_fbdev_set_par(struct fb_info *info)
@@ -90,26 +89,24 @@ static int exynos_drm_fbdev_update(struct drm_fb_helper *helper,
 {
 	struct fb_info *fbi = helper->fbdev;
 	struct drm_device *dev = helper->dev;
-	struct exynos_drm_fbdev *exynos_fb = to_exynos_fbdev(helper);
 	struct exynos_drm_gem_buf *buffer;
 	unsigned int size = fb->width * fb->height * (fb->bits_per_pixel >> 3);
 	unsigned long offset;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	exynos_fb->fb = fb;
-
-	drm_fb_helper_fill_fix(fbi, fb->pitch, fb->depth);
+	drm_fb_helper_fill_fix(fbi, fb->pitches[0], fb->depth);
 	drm_fb_helper_fill_var(fbi, helper, fb->width, fb->height);
 
-	buffer = exynos_drm_fb_get_buf(fb);
+	/* RGB formats use only one buffer */
+	buffer = exynos_drm_fb_buffer(fb, 0);
 	if (!buffer) {
 		DRM_LOG_KMS("buffer is null.\n");
 		return -EFAULT;
 	}
 
 	offset = fbi->var.xoffset * (fb->bits_per_pixel >> 3);
-	offset += fbi->var.yoffset * fb->pitch;
+	offset += fbi->var.yoffset * fb->pitches[0];
 
 	dev->mode_config.fb_base = (resource_size_t)buffer->dma_addr;
 	fbi->screen_base = buffer->kvaddr + offset;
@@ -124,10 +121,12 @@ static int exynos_drm_fbdev_create(struct drm_fb_helper *helper,
 				    struct drm_fb_helper_surface_size *sizes)
 {
 	struct exynos_drm_fbdev *exynos_fbdev = to_exynos_fbdev(helper);
+	struct exynos_drm_gem_obj *exynos_gem_obj;
 	struct drm_device *dev = helper->dev;
 	struct fb_info *fbi;
-	struct drm_mode_fb_cmd mode_cmd = { 0 };
+	struct drm_mode_fb_cmd2 mode_cmd = { 0 };
 	struct platform_device *pdev = dev->platformdev;
+	unsigned long size;
 	int ret;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -138,8 +137,9 @@ static int exynos_drm_fbdev_create(struct drm_fb_helper *helper,
 
 	mode_cmd.width = sizes->surface_width;
 	mode_cmd.height = sizes->surface_height;
-	mode_cmd.bpp = sizes->surface_bpp;
-	mode_cmd.depth = sizes->surface_depth;
+	mode_cmd.pitches[0] = sizes->surface_width * (sizes->surface_bpp >> 3);
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
 
 	mutex_lock(&dev->struct_mutex);
 
@@ -150,14 +150,23 @@ static int exynos_drm_fbdev_create(struct drm_fb_helper *helper,
 		goto out;
 	}
 
-	exynos_fbdev->fb = exynos_drm_fb_create(dev, NULL, &mode_cmd);
-	if (IS_ERR_OR_NULL(exynos_fbdev->fb)) {
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+	exynos_gem_obj = exynos_drm_gem_create(dev, size);
+	if (IS_ERR(exynos_gem_obj)) {
+		ret = PTR_ERR(exynos_gem_obj);
+		goto out;
+	}
+
+	exynos_fbdev->exynos_gem_obj = exynos_gem_obj;
+
+	helper->fb = exynos_drm_framebuffer_init(dev, &mode_cmd,
+			&exynos_gem_obj->base);
+	if (IS_ERR_OR_NULL(helper->fb)) {
 		DRM_ERROR("failed to create drm framebuffer.\n");
-		ret = PTR_ERR(exynos_fbdev->fb);
+		ret = PTR_ERR(helper->fb);
 		goto out;
 	}
 
-	helper->fb = exynos_fbdev->fb;
 	helper->fbdev = fbi;
 
 	fbi->par = helper;
@@ -171,8 +180,10 @@ static int exynos_drm_fbdev_create(struct drm_fb_helper *helper,
 	}
 
 	ret = exynos_drm_fbdev_update(helper, helper->fb);
-	if (ret < 0)
+	if (ret < 0) {
 		fb_dealloc_cmap(&fbi->cmap);
+		goto out;
+	}
 
 /*
  * if failed, all resources allocated above would be released by
@@ -205,34 +216,42 @@ static int exynos_drm_fbdev_recreate(struct drm_fb_helper *helper,
 {
 	struct drm_device *dev = helper->dev;
 	struct exynos_drm_fbdev *exynos_fbdev = to_exynos_fbdev(helper);
-	struct drm_framebuffer *fb = exynos_fbdev->fb;
-	struct drm_mode_fb_cmd mode_cmd = { 0 };
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	struct drm_framebuffer *fb = helper->fb;
+	struct drm_mode_fb_cmd2 mode_cmd = { 0 };
+	unsigned long size;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	if (helper->fb != fb) {
-		DRM_ERROR("drm framebuffer is different\n");
-		return -EINVAL;
-	}
-
 	if (exynos_drm_fbdev_is_samefb(fb, sizes))
 		return 0;
 
 	mode_cmd.width = sizes->surface_width;
 	mode_cmd.height = sizes->surface_height;
-	mode_cmd.bpp = sizes->surface_bpp;
-	mode_cmd.depth = sizes->surface_depth;
+	mode_cmd.pitches[0] = sizes->surface_width * (sizes->surface_bpp >> 3);
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
+
+	if (exynos_fbdev->exynos_gem_obj)
+		exynos_drm_gem_destroy(exynos_fbdev->exynos_gem_obj);
 
 	if (fb->funcs->destroy)
 		fb->funcs->destroy(fb);
 
-	exynos_fbdev->fb = exynos_drm_fb_create(dev, NULL, &mode_cmd);
-	if (IS_ERR(exynos_fbdev->fb)) {
-		DRM_ERROR("failed to allocate fb.\n");
-		return PTR_ERR(exynos_fbdev->fb);
+	size = mode_cmd.pitches[0] * mode_cmd.height;
+	exynos_gem_obj = exynos_drm_gem_create(dev, size);
+	if (IS_ERR(exynos_gem_obj))
+		return PTR_ERR(exynos_gem_obj);
+
+	exynos_fbdev->exynos_gem_obj = exynos_gem_obj;
+
+	helper->fb = exynos_drm_framebuffer_init(dev, &mode_cmd,
+			&exynos_gem_obj->base);
+	if (IS_ERR_OR_NULL(helper->fb)) {
+		DRM_ERROR("failed to create drm framebuffer.\n");
+		return PTR_ERR(helper->fb);
 	}
 
-	helper->fb = exynos_fbdev->fb;
 	return exynos_drm_fbdev_update(helper, helper->fb);
 }
 
@@ -366,6 +385,9 @@ void exynos_drm_fbdev_fini(struct drm_device *dev)
 
 	fbdev = to_exynos_fbdev(private->fb_helper);
 
+	if (fbdev->exynos_gem_obj)
+		exynos_drm_gem_destroy(fbdev->exynos_gem_obj);
+
 	exynos_drm_fbdev_destroy(dev, private->fb_helper);
 	kfree(fbdev);
 	private->fb_helper = NULL;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index db3b3d9e731d..ca83139cd309 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -17,6 +17,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
+#include <linux/pm_runtime.h>
 
 #include <drm/exynos_drm.h>
 #include <plat/regs-fb-v4.h>
@@ -68,6 +69,7 @@ struct fimd_win_data {
 	void __iomem		*vaddr;
 	unsigned int		buf_offsize;
 	unsigned int		line_size;	/* bytes */
+	bool			enabled;
 };
 
 struct fimd_context {
@@ -84,6 +86,8 @@ struct fimd_context {
 	unsigned long			irq_flags;
 	u32				vidcon0;
 	u32				vidcon1;
+	bool				suspended;
+	struct mutex			lock;
 
 	struct fb_videomode		*timing;
 };
@@ -119,7 +123,7 @@ static int fimd_display_power_on(struct device *dev, int mode)
 {
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	/* TODO. */
+	/* TODO */
 
 	return 0;
 }
@@ -132,12 +136,68 @@ static struct exynos_drm_display_ops fimd_display_ops = {
 	.power_on = fimd_display_power_on,
 };
 
+static void fimd_dpms(struct device *subdrv_dev, int mode)
+{
+	struct fimd_context *ctx = get_fimd_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s, %d\n", __FILE__, mode);
+
+	mutex_lock(&ctx->lock);
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		/*
+		 * enable fimd hardware only if suspended status.
+		 *
+		 * P.S. fimd_dpms function would be called at booting time so
+		 * clk_enable could be called double time.
+		 */
+		if (ctx->suspended)
+			pm_runtime_get_sync(subdrv_dev);
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		pm_runtime_put_sync(subdrv_dev);
+		break;
+	default:
+		DRM_DEBUG_KMS("unspecified mode %d\n", mode);
+		break;
+	}
+
+	mutex_unlock(&ctx->lock);
+}
+
+static void fimd_apply(struct device *subdrv_dev)
+{
+	struct fimd_context *ctx = get_fimd_context(subdrv_dev);
+	struct exynos_drm_manager *mgr = &ctx->subdrv.manager;
+	struct exynos_drm_manager_ops *mgr_ops = mgr->ops;
+	struct exynos_drm_overlay_ops *ovl_ops = mgr->overlay_ops;
+	struct fimd_win_data *win_data;
+	int i;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	for (i = 0; i < WINDOWS_NR; i++) {
+		win_data = &ctx->win_data[i];
+		if (win_data->enabled && (ovl_ops && ovl_ops->commit))
+			ovl_ops->commit(subdrv_dev, i);
+	}
+
+	if (mgr_ops && mgr_ops->commit)
+		mgr_ops->commit(subdrv_dev);
+}
+
 static void fimd_commit(struct device *dev)
 {
 	struct fimd_context *ctx = get_fimd_context(dev);
 	struct fb_videomode *timing = ctx->timing;
 	u32 val;
 
+	if (ctx->suspended)
+		return;
+
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
 	/* setup polarity values from machine code. */
@@ -177,40 +237,6 @@ static void fimd_commit(struct device *dev)
 	writel(val, ctx->regs + VIDCON0);
 }
 
-static void fimd_disable(struct device *dev)
-{
-	struct fimd_context *ctx = get_fimd_context(dev);
-	struct exynos_drm_subdrv *subdrv = &ctx->subdrv;
-	struct drm_device *drm_dev = subdrv->drm_dev;
-	struct exynos_drm_manager *manager = &subdrv->manager;
-	u32 val;
-
-	DRM_DEBUG_KMS("%s\n", __FILE__);
-
-	/* fimd dma off */
-	val = readl(ctx->regs + VIDCON0);
-	val &= ~(VIDCON0_ENVID | VIDCON0_ENVID_F);
-	writel(val, ctx->regs + VIDCON0);
-
-	/*
-	 * if vblank is enabled status with dma off then
-	 * it disables vsync interrupt.
-	 */
-	if (drm_dev->vblank_enabled[manager->pipe] &&
-		atomic_read(&drm_dev->vblank_refcount[manager->pipe])) {
-		drm_vblank_put(drm_dev, manager->pipe);
-
-		/*
-		 * if vblank_disable_allowed is 0 then disable
-		 * vsync interrupt right now else the vsync interrupt
-		 * would be disabled by drm timer once a current process
-		 * gives up ownershop of vblank event.
-		 */
-		if (!drm_dev->vblank_disable_allowed)
-			drm_vblank_off(drm_dev, manager->pipe);
-	}
-}
-
 static int fimd_enable_vblank(struct device *dev)
 {
 	struct fimd_context *ctx = get_fimd_context(dev);
@@ -218,6 +244,9 @@ static int fimd_enable_vblank(struct device *dev)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (ctx->suspended)
+		return -EPERM;
+
 	if (!test_and_set_bit(0, &ctx->irq_flags)) {
 		val = readl(ctx->regs + VIDINTCON0);
 
@@ -242,6 +271,9 @@ static void fimd_disable_vblank(struct device *dev)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (ctx->suspended)
+		return;
+
 	if (test_and_clear_bit(0, &ctx->irq_flags)) {
 		val = readl(ctx->regs + VIDINTCON0);
 
@@ -253,8 +285,9 @@ static void fimd_disable_vblank(struct device *dev)
 }
 
 static struct exynos_drm_manager_ops fimd_manager_ops = {
+	.dpms = fimd_dpms,
+	.apply = fimd_apply,
 	.commit = fimd_commit,
-	.disable = fimd_disable,
 	.enable_vblank = fimd_enable_vblank,
 	.disable_vblank = fimd_disable_vblank,
 };
@@ -264,6 +297,7 @@ static void fimd_win_mode_set(struct device *dev,
 {
 	struct fimd_context *ctx = get_fimd_context(dev);
 	struct fimd_win_data *win_data;
+	int win;
 	unsigned long offset;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -273,12 +307,19 @@ static void fimd_win_mode_set(struct device *dev,
 		return;
 	}
 
+	win = overlay->zpos;
+	if (win == DEFAULT_ZPOS)
+		win = ctx->default_win;
+
+	if (win < 0 || win > WINDOWS_NR)
+		return;
+
 	offset = overlay->fb_x * (overlay->bpp >> 3);
 	offset += overlay->fb_y * overlay->pitch;
 
 	DRM_DEBUG_KMS("offset = 0x%lx, pitch = %x\n", offset, overlay->pitch);
 
-	win_data = &ctx->win_data[ctx->default_win];
+	win_data = &ctx->win_data[win];
 
 	win_data->offset_x = overlay->crtc_x;
 	win_data->offset_y = overlay->crtc_y;
@@ -286,8 +327,8 @@ static void fimd_win_mode_set(struct device *dev,
 	win_data->ovl_height = overlay->crtc_height;
 	win_data->fb_width = overlay->fb_width;
 	win_data->fb_height = overlay->fb_height;
-	win_data->dma_addr = overlay->dma_addr + offset;
-	win_data->vaddr = overlay->vaddr + offset;
+	win_data->dma_addr = overlay->dma_addr[0] + offset;
+	win_data->vaddr = overlay->vaddr[0] + offset;
 	win_data->bpp = overlay->bpp;
 	win_data->buf_offsize = (overlay->fb_width - overlay->crtc_width) *
 				(overlay->bpp >> 3);
@@ -381,15 +422,21 @@ static void fimd_win_set_colkey(struct device *dev, unsigned int win)
 	writel(keycon1, ctx->regs + WKEYCON1_BASE(win));
 }
 
-static void fimd_win_commit(struct device *dev)
+static void fimd_win_commit(struct device *dev, int zpos)
 {
 	struct fimd_context *ctx = get_fimd_context(dev);
 	struct fimd_win_data *win_data;
-	int win = ctx->default_win;
+	int win = zpos;
 	unsigned long val, alpha, size;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (ctx->suspended)
+		return;
+
+	if (win == DEFAULT_ZPOS)
+		win = ctx->default_win;
+
 	if (win < 0 || win > WINDOWS_NR)
 		return;
 
@@ -472,24 +519,37 @@ static void fimd_win_commit(struct device *dev)
 	if (win != 0)
 		fimd_win_set_colkey(dev, win);
 
+	/* wincon */
+	val = readl(ctx->regs + WINCON(win));
+	val |= WINCONx_ENWIN;
+	writel(val, ctx->regs + WINCON(win));
+
 	/* Enable DMA channel and unprotect windows */
 	val = readl(ctx->regs + SHADOWCON);
 	val |= SHADOWCON_CHx_ENABLE(win);
 	val &= ~SHADOWCON_WINx_PROTECT(win);
 	writel(val, ctx->regs + SHADOWCON);
+
+	win_data->enabled = true;
 }
 
-static void fimd_win_disable(struct device *dev)
+static void fimd_win_disable(struct device *dev, int zpos)
 {
 	struct fimd_context *ctx = get_fimd_context(dev);
-	int win = ctx->default_win;
+	struct fimd_win_data *win_data;
+	int win = zpos;
 	u32 val;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	if (win == DEFAULT_ZPOS)
+		win = ctx->default_win;
+
 	if (win < 0 || win > WINDOWS_NR)
 		return;
 
+	win_data = &ctx->win_data[win];
+
 	/* protect windows */
 	val = readl(ctx->regs + SHADOWCON);
 	val |= SHADOWCON_WINx_PROTECT(win);
@@ -505,6 +565,8 @@ static void fimd_win_disable(struct device *dev)
 	val &= ~SHADOWCON_CHx_ENABLE(win);
 	val &= ~SHADOWCON_WINx_PROTECT(win);
 	writel(val, ctx->regs + SHADOWCON);
+
+	win_data->enabled = false;
 }
 
 static struct exynos_drm_overlay_ops fimd_overlay_ops = {
@@ -540,9 +602,17 @@ static void fimd_finish_pageflip(struct drm_device *drm_dev, int crtc)
 		wake_up_interruptible(&e->base.file_priv->event_wait);
 	}
 
-	if (is_checked)
+	if (is_checked) {
 		drm_vblank_put(drm_dev, crtc);
 
+		/*
+		 * don't off vblank if vblank_disable_allowed is 1,
+		 * because vblank would be off by timer handler.
+		 */
+		if (!drm_dev->vblank_disable_allowed)
+			drm_vblank_off(drm_dev, crtc);
+	}
+
 	spin_unlock_irqrestore(&drm_dev->event_lock, flags);
 }
 
@@ -560,19 +630,14 @@ static irqreturn_t fimd_irq_handler(int irq, void *dev_id)
 		/* VSYNC interrupt */
 		writel(VIDINTCON1_INT_FRAME, ctx->regs + VIDINTCON1);
 
-	/*
-	 * in case that vblank_disable_allowed is 1, it could induce
-	 * the problem that manager->pipe could be -1 because with
-	 * disable callback, vsync interrupt isn't disabled and at this moment,
-	 * vsync interrupt could occur. the vsync interrupt would be disabled
-	 * by timer handler later.
-	 */
-	if (manager->pipe == -1)
-		return IRQ_HANDLED;
+	/* check the crtc is detached already from encoder */
+	if (manager->pipe < 0)
+		goto out;
 
 	drm_handle_vblank(drm_dev, manager->pipe);
 	fimd_finish_pageflip(drm_dev, manager->pipe);
 
+out:
 	return IRQ_HANDLED;
 }
 
@@ -590,6 +655,13 @@ static int fimd_subdrv_probe(struct drm_device *drm_dev, struct device *dev)
 	 */
 	drm_dev->irq_enabled = 1;
 
+	/*
+	 * with vblank_disable_allowed = 1, vblank interrupt will be disabled
+	 * by drm timer once a current process gives up ownership of
+	 * vblank event.(after drm_vblank_put function is called)
+	 */
+	drm_dev->vblank_disable_allowed = 1;
+
 	return 0;
 }
 
@@ -739,9 +811,6 @@ static int __devinit fimd_probe(struct platform_device *pdev)
 
 	ctx->irq = res->start;
 
-	for (win = 0; win < WINDOWS_NR; win++)
-		fimd_clear_win(ctx, win);
-
 	ret = request_irq(ctx->irq, fimd_irq_handler, 0, "drm_fimd", ctx);
 	if (ret < 0) {
 		dev_err(dev, "irq request failed.\n");
@@ -769,7 +838,17 @@ static int __devinit fimd_probe(struct platform_device *pdev)
 	subdrv->manager.display_ops = &fimd_display_ops;
 	subdrv->manager.dev = dev;
 
+	mutex_init(&ctx->lock);
+
 	platform_set_drvdata(pdev, ctx);
+
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+
+	for (win = 0; win < WINDOWS_NR; win++)
+		fimd_clear_win(ctx, win);
+
 	exynos_drm_subdrv_register(subdrv);
 
 	return 0;
@@ -797,14 +876,25 @@ err_clk_get:
 
 static int __devexit fimd_remove(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct fimd_context *ctx = platform_get_drvdata(pdev);
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
 	exynos_drm_subdrv_unregister(&ctx->subdrv);
 
+	if (ctx->suspended)
+		goto out;
+
 	clk_disable(ctx->lcd_clk);
 	clk_disable(ctx->bus_clk);
+
+	pm_runtime_set_suspended(dev);
+	pm_runtime_put_sync(dev);
+
+out:
+	pm_runtime_disable(dev);
+
 	clk_put(ctx->lcd_clk);
 	clk_put(ctx->bus_clk);
 
@@ -818,12 +908,102 @@ static int __devexit fimd_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int fimd_suspend(struct device *dev)
+{
+	int ret;
+
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	ret = pm_runtime_suspend(dev);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int fimd_resume(struct device *dev)
+{
+	int ret;
+
+	ret = pm_runtime_resume(dev);
+	if (ret < 0) {
+		DRM_ERROR("failed to resume runtime pm.\n");
+		return ret;
+	}
+
+	pm_runtime_disable(dev);
+
+	ret = pm_runtime_set_active(dev);
+	if (ret < 0) {
+		DRM_ERROR("failed to active runtime pm.\n");
+		pm_runtime_enable(dev);
+		pm_runtime_suspend(dev);
+		return ret;
+	}
+
+	pm_runtime_enable(dev);
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_PM_RUNTIME
+static int fimd_runtime_suspend(struct device *dev)
+{
+	struct fimd_context *ctx = get_fimd_context(dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	clk_disable(ctx->lcd_clk);
+	clk_disable(ctx->bus_clk);
+
+	ctx->suspended = true;
+	return 0;
+}
+
+static int fimd_runtime_resume(struct device *dev)
+{
+	struct fimd_context *ctx = get_fimd_context(dev);
+	int ret;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	ret = clk_enable(ctx->bus_clk);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_enable(ctx->lcd_clk);
+	if  (ret < 0) {
+		clk_disable(ctx->bus_clk);
+		return ret;
+	}
+
+	ctx->suspended = false;
+
+	/* if vblank was enabled status, enable it again. */
+	if (test_and_clear_bit(0, &ctx->irq_flags))
+		fimd_enable_vblank(dev);
+
+	fimd_apply(dev);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops fimd_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fimd_suspend, fimd_resume)
+	SET_RUNTIME_PM_OPS(fimd_runtime_suspend, fimd_runtime_resume, NULL)
+};
+
 static struct platform_driver fimd_driver = {
 	.probe		= fimd_probe,
 	.remove		= __devexit_p(fimd_remove),
 	.driver		= {
 		.name	= "exynos4-fb",
 		.owner	= THIS_MODULE,
+		.pm	= &fimd_pm_ops,
 	},
 };
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index aba0fe47f7ea..025abb3e3b67 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -55,17 +55,54 @@ static unsigned int convert_to_vm_err_msg(int msg)
 	return out_msg;
 }
 
-static unsigned int get_gem_mmap_offset(struct drm_gem_object *obj)
+static int exynos_drm_gem_handle_create(struct drm_gem_object *obj,
+					struct drm_file *file_priv,
+					unsigned int *handle)
 {
+	int ret;
+
+	/*
+	 * allocate a id of idr table where the obj is registered
+	 * and handle has the id what user can see.
+	 */
+	ret = drm_gem_handle_create(file_priv, obj, handle);
+	if (ret)
+		return ret;
+
+	DRM_DEBUG_KMS("gem handle = 0x%x\n", *handle);
+
+	/* drop reference from allocate - handle holds it now. */
+	drm_gem_object_unreference_unlocked(obj);
+
+	return 0;
+}
+
+void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj)
+{
+	struct drm_gem_object *obj;
+
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	return (unsigned int)obj->map_list.hash.key << PAGE_SHIFT;
+	if (!exynos_gem_obj)
+		return;
+
+	obj = &exynos_gem_obj->base;
+
+	DRM_DEBUG_KMS("handle count = %d\n", atomic_read(&obj->handle_count));
+
+	exynos_drm_buf_destroy(obj->dev, exynos_gem_obj->buffer);
+
+	if (obj->map_list.map)
+		drm_gem_free_mmap_offset(obj);
+
+	/* release file pointer to gem object. */
+	drm_gem_object_release(obj);
+
+	kfree(exynos_gem_obj);
 }
 
-static struct exynos_drm_gem_obj
-		*exynos_drm_gem_init(struct drm_device *drm_dev,
-			struct drm_file *file_priv, unsigned int *handle,
-			unsigned int size)
+static struct exynos_drm_gem_obj *exynos_drm_gem_init(struct drm_device *dev,
+						      unsigned long size)
 {
 	struct exynos_drm_gem_obj *exynos_gem_obj;
 	struct drm_gem_object *obj;
@@ -73,75 +110,41 @@ static struct exynos_drm_gem_obj
 
 	exynos_gem_obj = kzalloc(sizeof(*exynos_gem_obj), GFP_KERNEL);
 	if (!exynos_gem_obj) {
-		DRM_ERROR("failed to allocate exynos gem object.\n");
-		return ERR_PTR(-ENOMEM);
+		DRM_ERROR("failed to allocate exynos gem object\n");
+		return NULL;
 	}
 
 	obj = &exynos_gem_obj->base;
 
-	ret = drm_gem_object_init(drm_dev, obj, size);
+	ret = drm_gem_object_init(dev, obj, size);
 	if (ret < 0) {
-		DRM_ERROR("failed to initialize gem object.\n");
-		ret = -EINVAL;
-		goto err_object_init;
+		DRM_ERROR("failed to initialize gem object\n");
+		kfree(exynos_gem_obj);
+		return NULL;
 	}
 
 	DRM_DEBUG_KMS("created file object = 0x%x\n", (unsigned int)obj->filp);
 
-	ret = drm_gem_create_mmap_offset(obj);
-	if (ret < 0) {
-		DRM_ERROR("failed to allocate mmap offset.\n");
-		goto err_create_mmap_offset;
-	}
-
-	/*
-	 * allocate a id of idr table where the obj is registered
-	 * and handle has the id what user can see.
-	 */
-	ret = drm_gem_handle_create(file_priv, obj, handle);
-	if (ret)
-		goto err_handle_create;
-
-	DRM_DEBUG_KMS("gem handle = 0x%x\n", *handle);
-
-	/* drop reference from allocate - handle holds it now. */
-	drm_gem_object_unreference_unlocked(obj);
-
 	return exynos_gem_obj;
-
-err_handle_create:
-	drm_gem_free_mmap_offset(obj);
-
-err_create_mmap_offset:
-	drm_gem_object_release(obj);
-
-err_object_init:
-	kfree(exynos_gem_obj);
-
-	return ERR_PTR(ret);
 }
 
 struct exynos_drm_gem_obj *exynos_drm_gem_create(struct drm_device *dev,
-				struct drm_file *file_priv,
-				unsigned int *handle, unsigned long size)
+						 unsigned long size)
 {
-
-	struct exynos_drm_gem_obj *exynos_gem_obj = NULL;
 	struct exynos_drm_gem_buf *buffer;
+	struct exynos_drm_gem_obj *exynos_gem_obj;
 
 	size = roundup(size, PAGE_SIZE);
-
 	DRM_DEBUG_KMS("%s: size = 0x%lx\n", __FILE__, size);
 
 	buffer = exynos_drm_buf_create(dev, size);
-	if (IS_ERR(buffer)) {
-		return ERR_CAST(buffer);
-	}
+	if (!buffer)
+		return ERR_PTR(-ENOMEM);
 
-	exynos_gem_obj = exynos_drm_gem_init(dev, file_priv, handle, size);
-	if (IS_ERR(exynos_gem_obj)) {
+	exynos_gem_obj = exynos_drm_gem_init(dev, size);
+	if (!exynos_gem_obj) {
 		exynos_drm_buf_destroy(dev, buffer);
-		return exynos_gem_obj;
+		return ERR_PTR(-ENOMEM);
 	}
 
 	exynos_gem_obj->buffer = buffer;
@@ -150,23 +153,30 @@ struct exynos_drm_gem_obj *exynos_drm_gem_create(struct drm_device *dev,
 }
 
 int exynos_drm_gem_create_ioctl(struct drm_device *dev, void *data,
-					struct drm_file *file_priv)
+				struct drm_file *file_priv)
 {
 	struct drm_exynos_gem_create *args = data;
-	struct exynos_drm_gem_obj *exynos_gem_obj = NULL;
+	struct exynos_drm_gem_obj *exynos_gem_obj;
+	int ret;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	exynos_gem_obj = exynos_drm_gem_create(dev, file_priv,
-						&args->handle, args->size);
+	exynos_gem_obj = exynos_drm_gem_create(dev, args->size);
 	if (IS_ERR(exynos_gem_obj))
 		return PTR_ERR(exynos_gem_obj);
 
+	ret = exynos_drm_gem_handle_create(&exynos_gem_obj->base, file_priv,
+			&args->handle);
+	if (ret) {
+		exynos_drm_gem_destroy(exynos_gem_obj);
+		return ret;
+	}
+
 	return 0;
 }
 
 int exynos_drm_gem_map_offset_ioctl(struct drm_device *dev, void *data,
-		struct drm_file *file_priv)
+				    struct drm_file *file_priv)
 {
 	struct drm_exynos_gem_map_off *args = data;
 
@@ -185,7 +195,7 @@ int exynos_drm_gem_map_offset_ioctl(struct drm_device *dev, void *data,
 }
 
 static int exynos_drm_gem_mmap_buffer(struct file *filp,
-		struct vm_area_struct *vma)
+				      struct vm_area_struct *vma)
 {
 	struct drm_gem_object *obj = filp->private_data;
 	struct exynos_drm_gem_obj *exynos_gem_obj = to_exynos_gem_obj(obj);
@@ -196,6 +206,7 @@ static int exynos_drm_gem_mmap_buffer(struct file *filp,
 
 	vma->vm_flags |= (VM_IO | VM_RESERVED);
 
+	/* in case of direct mapping, always having non-cachable attribute */
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 	vma->vm_file = filp;
 
@@ -232,7 +243,7 @@ static const struct file_operations exynos_drm_gem_fops = {
 };
 
 int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
-		struct drm_file *file_priv)
+			      struct drm_file *file_priv)
 {
 	struct drm_exynos_gem_mmap *args = data;
 	struct drm_gem_object *obj;
@@ -278,32 +289,19 @@ int exynos_drm_gem_init_object(struct drm_gem_object *obj)
 	return 0;
 }
 
-void exynos_drm_gem_free_object(struct drm_gem_object *gem_obj)
+void exynos_drm_gem_free_object(struct drm_gem_object *obj)
 {
-	struct exynos_drm_gem_obj *exynos_gem_obj;
-
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	DRM_DEBUG_KMS("handle count = %d\n",
-			atomic_read(&gem_obj->handle_count));
-
-	if (gem_obj->map_list.map)
-		drm_gem_free_mmap_offset(gem_obj);
-
-	/* release file pointer to gem object. */
-	drm_gem_object_release(gem_obj);
-
-	exynos_gem_obj = to_exynos_gem_obj(gem_obj);
-
-	exynos_drm_buf_destroy(gem_obj->dev, exynos_gem_obj->buffer);
-
-	kfree(exynos_gem_obj);
+	exynos_drm_gem_destroy(to_exynos_gem_obj(obj));
 }
 
 int exynos_drm_gem_dumb_create(struct drm_file *file_priv,
-		struct drm_device *dev, struct drm_mode_create_dumb *args)
+			       struct drm_device *dev,
+			       struct drm_mode_create_dumb *args)
 {
 	struct exynos_drm_gem_obj *exynos_gem_obj;
+	int ret;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
@@ -316,19 +314,27 @@ int exynos_drm_gem_dumb_create(struct drm_file *file_priv,
 	args->pitch = args->width * args->bpp >> 3;
 	args->size = args->pitch * args->height;
 
-	exynos_gem_obj = exynos_drm_gem_create(dev, file_priv, &args->handle,
-							args->size);
+	exynos_gem_obj = exynos_drm_gem_create(dev, args->size);
 	if (IS_ERR(exynos_gem_obj))
 		return PTR_ERR(exynos_gem_obj);
 
+	ret = exynos_drm_gem_handle_create(&exynos_gem_obj->base, file_priv,
+			&args->handle);
+	if (ret) {
+		exynos_drm_gem_destroy(exynos_gem_obj);
+		return ret;
+	}
+
 	return 0;
 }
 
 int exynos_drm_gem_dumb_map_offset(struct drm_file *file_priv,
-		struct drm_device *dev, uint32_t handle, uint64_t *offset)
+				   struct drm_device *dev, uint32_t handle,
+				   uint64_t *offset)
 {
 	struct exynos_drm_gem_obj *exynos_gem_obj;
 	struct drm_gem_object *obj;
+	int ret = 0;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
@@ -343,19 +349,46 @@ int exynos_drm_gem_dumb_map_offset(struct drm_file *file_priv,
 	obj = drm_gem_object_lookup(dev, file_priv, handle);
 	if (!obj) {
 		DRM_ERROR("failed to lookup gem object.\n");
-		mutex_unlock(&dev->struct_mutex);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto unlock;
 	}
 
 	exynos_gem_obj = to_exynos_gem_obj(obj);
 
-	*offset = get_gem_mmap_offset(&exynos_gem_obj->base);
-
-	drm_gem_object_unreference(obj);
+	if (!exynos_gem_obj->base.map_list.map) {
+		ret = drm_gem_create_mmap_offset(&exynos_gem_obj->base);
+		if (ret)
+			goto out;
+	}
 
+	*offset = (u64)exynos_gem_obj->base.map_list.hash.key << PAGE_SHIFT;
 	DRM_DEBUG_KMS("offset = 0x%lx\n", (unsigned long)*offset);
 
+out:
+	drm_gem_object_unreference(obj);
+unlock:
 	mutex_unlock(&dev->struct_mutex);
+	return ret;
+}
+
+int exynos_drm_gem_dumb_destroy(struct drm_file *file_priv,
+				struct drm_device *dev,
+				unsigned int handle)
+{
+	int ret;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	/*
+	 * obj->refcount and obj->handle_count are decreased and
+	 * if both them are 0 then exynos_drm_gem_free_object()
+	 * would be called by callback to release resources.
+	 */
+	ret = drm_gem_handle_delete(file_priv, handle);
+	if (ret < 0) {
+		DRM_ERROR("failed to delete drm_gem_handle.\n");
+		return ret;
+	}
 
 	return 0;
 }
@@ -403,28 +436,6 @@ int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 	return ret;
 }
 
-
-int exynos_drm_gem_dumb_destroy(struct drm_file *file_priv,
-		struct drm_device *dev, unsigned int handle)
-{
-	int ret;
-
-	DRM_DEBUG_KMS("%s\n", __FILE__);
-
-	/*
-	 * obj->refcount and obj->handle_count are decreased and
-	 * if both them are 0 then exynos_drm_gem_free_object()
-	 * would be called by callback to release resources.
-	 */
-	ret = drm_gem_handle_delete(file_priv, handle);
-	if (ret < 0) {
-		DRM_ERROR("failed to delete drm_gem_handle.\n");
-		return ret;
-	}
-
-	return 0;
-}
-
 MODULE_AUTHOR("Inki Dae <inki.dae@samsung.com>");
 MODULE_DESCRIPTION("Samsung SoC DRM GEM Module");
 MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.h b/drivers/gpu/drm/exynos/exynos_drm_gem.h
index ef8797334e6d..67cdc9168708 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.h
@@ -60,14 +60,16 @@ struct exynos_drm_gem_buf {
  *	user can access the buffer through kms_bo.handle.
  */
 struct exynos_drm_gem_obj {
-	struct drm_gem_object base;
-	struct exynos_drm_gem_buf *buffer;
+	struct drm_gem_object		base;
+	struct exynos_drm_gem_buf	*buffer;
 };
 
-/* create a new buffer and get a new gem handle. */
+/* destroy a buffer with gem object */
+void exynos_drm_gem_destroy(struct exynos_drm_gem_obj *exynos_gem_obj);
+
+/* create a new buffer with gem object */
 struct exynos_drm_gem_obj *exynos_drm_gem_create(struct drm_device *dev,
-		struct drm_file *file_priv,
-		unsigned int *handle, unsigned long size);
+						 unsigned long size);
 
 /*
  * request gem object creation and buffer allocation as the size
@@ -75,15 +77,18 @@ struct exynos_drm_gem_obj *exynos_drm_gem_create(struct drm_device *dev,
  * height and bpp.
  */
 int exynos_drm_gem_create_ioctl(struct drm_device *dev, void *data,
-		struct drm_file *file_priv);
+				struct drm_file *file_priv);
 
 /* get buffer offset to map to user space. */
 int exynos_drm_gem_map_offset_ioctl(struct drm_device *dev, void *data,
-		struct drm_file *file_priv);
+				    struct drm_file *file_priv);
 
-/* unmap a buffer from user space. */
-int exynos_drm_gem_munmap_ioctl(struct drm_device *dev, void *data,
-		struct drm_file *file_priv);
+/*
+ * mmap the physically continuous memory that a gem object contains
+ * to user space.
+ */
+int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
+			      struct drm_file *file_priv);
 
 /* initialize gem object. */
 int exynos_drm_gem_init_object(struct drm_gem_object *obj);
@@ -93,24 +98,13 @@ void exynos_drm_gem_free_object(struct drm_gem_object *gem_obj);
 
 /* create memory region for drm framebuffer. */
 int exynos_drm_gem_dumb_create(struct drm_file *file_priv,
-		struct drm_device *dev, struct drm_mode_create_dumb *args);
+			       struct drm_device *dev,
+			       struct drm_mode_create_dumb *args);
 
 /* map memory region for drm framebuffer to user space. */
 int exynos_drm_gem_dumb_map_offset(struct drm_file *file_priv,
-		struct drm_device *dev, uint32_t handle, uint64_t *offset);
-
-/* page fault handler and mmap fault address(virtual) to physical memory. */
-int exynos_drm_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
-
-/*
- * mmap the physically continuous memory that a gem object contains
- * to user space.
- */
-int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
-		struct drm_file *file_priv);
-
-/* set vm_flags and we can change the vm attribute to other one at here. */
-int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma);
+				   struct drm_device *dev, uint32_t handle,
+				   uint64_t *offset);
 
 /*
  * destroy memory region allocated.
@@ -118,6 +112,13 @@ int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma);
  *	would be released by drm_gem_handle_delete().
  */
 int exynos_drm_gem_dumb_destroy(struct drm_file *file_priv,
-		struct drm_device *dev, unsigned int handle);
+				struct drm_device *dev,
+				unsigned int handle);
+
+/* page fault handler and mmap fault address(virtual) to physical memory. */
+int exynos_drm_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
+
+/* set vm_flags and we can change the vm attribute to other one at here. */
+int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma);
 
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
new file mode 100644
index 000000000000..ed8a319ed84b
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
@@ -0,0 +1,439 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Authors:
+ *	Inki Dae <inki.dae@samsung.com>
+ *	Seung-Woo Kim <sw0312.kim@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include "drmP.h"
+
+#include <linux/kernel.h>
+#include <linux/wait.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+#include <drm/exynos_drm.h>
+
+#include "exynos_drm_drv.h"
+#include "exynos_drm_hdmi.h"
+
+#define to_context(dev)		platform_get_drvdata(to_platform_device(dev))
+#define to_subdrv(dev)		to_context(dev)
+#define get_ctx_from_subdrv(subdrv)	container_of(subdrv,\
+					struct drm_hdmi_context, subdrv);
+
+/* these callback points shoud be set by specific drivers. */
+static struct exynos_hdmi_display_ops *hdmi_display_ops;
+static struct exynos_hdmi_manager_ops *hdmi_manager_ops;
+static struct exynos_hdmi_overlay_ops *hdmi_overlay_ops;
+
+struct drm_hdmi_context {
+	struct exynos_drm_subdrv	subdrv;
+	struct exynos_drm_hdmi_context	*hdmi_ctx;
+	struct exynos_drm_hdmi_context	*mixer_ctx;
+	struct work_struct		work;
+};
+
+void exynos_drm_display_ops_register(struct exynos_hdmi_display_ops
+					*display_ops)
+{
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (display_ops)
+		hdmi_display_ops = display_ops;
+}
+EXPORT_SYMBOL(exynos_drm_display_ops_register);
+
+void exynos_drm_manager_ops_register(struct exynos_hdmi_manager_ops
+					*manager_ops)
+{
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (manager_ops)
+		hdmi_manager_ops = manager_ops;
+}
+EXPORT_SYMBOL(exynos_drm_manager_ops_register);
+
+void exynos_drm_overlay_ops_register(struct exynos_hdmi_overlay_ops
+					*overlay_ops)
+{
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (overlay_ops)
+		hdmi_overlay_ops = overlay_ops;
+}
+EXPORT_SYMBOL(exynos_drm_overlay_ops_register);
+
+static bool drm_hdmi_is_connected(struct device *dev)
+{
+	struct drm_hdmi_context *ctx = to_context(dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_display_ops && hdmi_display_ops->is_connected)
+		return hdmi_display_ops->is_connected(ctx->hdmi_ctx->ctx);
+
+	return false;
+}
+
+static int drm_hdmi_get_edid(struct device *dev,
+		struct drm_connector *connector, u8 *edid, int len)
+{
+	struct drm_hdmi_context *ctx = to_context(dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_display_ops && hdmi_display_ops->get_edid)
+		return hdmi_display_ops->get_edid(ctx->hdmi_ctx->ctx,
+				connector, edid, len);
+
+	return 0;
+}
+
+static int drm_hdmi_check_timing(struct device *dev, void *timing)
+{
+	struct drm_hdmi_context *ctx = to_context(dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_display_ops && hdmi_display_ops->check_timing)
+		return hdmi_display_ops->check_timing(ctx->hdmi_ctx->ctx,
+				timing);
+
+	return 0;
+}
+
+static int drm_hdmi_power_on(struct device *dev, int mode)
+{
+	struct drm_hdmi_context *ctx = to_context(dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_display_ops && hdmi_display_ops->power_on)
+		return hdmi_display_ops->power_on(ctx->hdmi_ctx->ctx, mode);
+
+	return 0;
+}
+
+static struct exynos_drm_display_ops drm_hdmi_display_ops = {
+	.type = EXYNOS_DISPLAY_TYPE_HDMI,
+	.is_connected = drm_hdmi_is_connected,
+	.get_edid = drm_hdmi_get_edid,
+	.check_timing = drm_hdmi_check_timing,
+	.power_on = drm_hdmi_power_on,
+};
+
+static int drm_hdmi_enable_vblank(struct device *subdrv_dev)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+	struct exynos_drm_subdrv *subdrv = &ctx->subdrv;
+	struct exynos_drm_manager *manager = &subdrv->manager;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_overlay_ops && hdmi_overlay_ops->enable_vblank)
+		return hdmi_overlay_ops->enable_vblank(ctx->mixer_ctx->ctx,
+							manager->pipe);
+
+	return 0;
+}
+
+static void drm_hdmi_disable_vblank(struct device *subdrv_dev)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_overlay_ops && hdmi_overlay_ops->disable_vblank)
+		return hdmi_overlay_ops->disable_vblank(ctx->mixer_ctx->ctx);
+}
+
+static void drm_hdmi_mode_set(struct device *subdrv_dev, void *mode)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_manager_ops && hdmi_manager_ops->mode_set)
+		hdmi_manager_ops->mode_set(ctx->hdmi_ctx->ctx, mode);
+}
+
+static void drm_hdmi_commit(struct device *subdrv_dev)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_manager_ops && hdmi_manager_ops->commit)
+		hdmi_manager_ops->commit(ctx->hdmi_ctx->ctx);
+}
+
+static void drm_hdmi_dpms(struct device *subdrv_dev, int mode)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+	case DRM_MODE_DPMS_OFF:
+		if (hdmi_manager_ops && hdmi_manager_ops->disable)
+			hdmi_manager_ops->disable(ctx->hdmi_ctx->ctx);
+		break;
+	default:
+		DRM_DEBUG_KMS("unkown dps mode: %d\n", mode);
+		break;
+	}
+}
+
+static struct exynos_drm_manager_ops drm_hdmi_manager_ops = {
+	.dpms = drm_hdmi_dpms,
+	.enable_vblank = drm_hdmi_enable_vblank,
+	.disable_vblank = drm_hdmi_disable_vblank,
+	.mode_set = drm_hdmi_mode_set,
+	.commit = drm_hdmi_commit,
+};
+
+static void drm_mixer_mode_set(struct device *subdrv_dev,
+		struct exynos_drm_overlay *overlay)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_overlay_ops && hdmi_overlay_ops->win_mode_set)
+		hdmi_overlay_ops->win_mode_set(ctx->mixer_ctx->ctx, overlay);
+}
+
+static void drm_mixer_commit(struct device *subdrv_dev, int zpos)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_overlay_ops && hdmi_overlay_ops->win_commit)
+		hdmi_overlay_ops->win_commit(ctx->mixer_ctx->ctx, zpos);
+}
+
+static void drm_mixer_disable(struct device *subdrv_dev, int zpos)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (hdmi_overlay_ops && hdmi_overlay_ops->win_disable)
+		hdmi_overlay_ops->win_disable(ctx->mixer_ctx->ctx, zpos);
+}
+
+static struct exynos_drm_overlay_ops drm_hdmi_overlay_ops = {
+	.mode_set = drm_mixer_mode_set,
+	.commit = drm_mixer_commit,
+	.disable = drm_mixer_disable,
+};
+
+
+static int hdmi_subdrv_probe(struct drm_device *drm_dev,
+		struct device *dev)
+{
+	struct exynos_drm_subdrv *subdrv = to_subdrv(dev);
+	struct drm_hdmi_context *ctx;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct exynos_drm_common_hdmi_pd *pd;
+	int ret;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	pd = pdev->dev.platform_data;
+
+	if (!pd) {
+		DRM_DEBUG_KMS("platform data is null.\n");
+		return -EFAULT;
+	}
+
+	if (!pd->hdmi_dev) {
+		DRM_DEBUG_KMS("hdmi device is null.\n");
+		return -EFAULT;
+	}
+
+	if (!pd->mixer_dev) {
+		DRM_DEBUG_KMS("mixer device is null.\n");
+		return -EFAULT;
+	}
+
+	ret = platform_driver_register(&hdmi_driver);
+	if (ret) {
+		DRM_DEBUG_KMS("failed to register hdmi driver.\n");
+		return ret;
+	}
+
+	ret = platform_driver_register(&mixer_driver);
+	if (ret) {
+		DRM_DEBUG_KMS("failed to register mixer driver.\n");
+		goto err_hdmidrv;
+	}
+
+	ctx = get_ctx_from_subdrv(subdrv);
+
+	ctx->hdmi_ctx = (struct exynos_drm_hdmi_context *)
+				to_context(pd->hdmi_dev);
+	if (!ctx->hdmi_ctx) {
+		DRM_DEBUG_KMS("hdmi context is null.\n");
+		ret = -EFAULT;
+		goto err_mixerdrv;
+	}
+
+	ctx->hdmi_ctx->drm_dev = drm_dev;
+
+	ctx->mixer_ctx = (struct exynos_drm_hdmi_context *)
+				to_context(pd->mixer_dev);
+	if (!ctx->mixer_ctx) {
+		DRM_DEBUG_KMS("mixer context is null.\n");
+		ret = -EFAULT;
+		goto err_mixerdrv;
+	}
+
+	ctx->mixer_ctx->drm_dev = drm_dev;
+
+	return 0;
+
+err_mixerdrv:
+	platform_driver_unregister(&mixer_driver);
+err_hdmidrv:
+	platform_driver_unregister(&hdmi_driver);
+	return ret;
+}
+
+static void hdmi_subdrv_remove(struct drm_device *drm_dev)
+{
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	platform_driver_unregister(&hdmi_driver);
+	platform_driver_unregister(&mixer_driver);
+}
+
+static void exynos_drm_hdmi_late_probe(struct work_struct *work)
+{
+	struct drm_hdmi_context *ctx = container_of(work,
+				struct drm_hdmi_context, work);
+
+	/*
+	 * this function calls subdrv->probe() so this must be called
+	 * after probe context.
+	 *
+	 * PS. subdrv->probe() will call platform_driver_register() to probe
+	 * hdmi and mixer driver.
+	 */
+	exynos_drm_subdrv_register(&ctx->subdrv);
+}
+
+static int __devinit exynos_drm_hdmi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct exynos_drm_subdrv *subdrv;
+	struct drm_hdmi_context *ctx;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		DRM_LOG_KMS("failed to alloc common hdmi context.\n");
+		return -ENOMEM;
+	}
+
+	subdrv = &ctx->subdrv;
+
+	subdrv->probe = hdmi_subdrv_probe;
+	subdrv->remove = hdmi_subdrv_remove;
+	subdrv->manager.pipe = -1;
+	subdrv->manager.ops = &drm_hdmi_manager_ops;
+	subdrv->manager.overlay_ops = &drm_hdmi_overlay_ops;
+	subdrv->manager.display_ops = &drm_hdmi_display_ops;
+	subdrv->manager.dev = dev;
+
+	platform_set_drvdata(pdev, subdrv);
+
+	INIT_WORK(&ctx->work, exynos_drm_hdmi_late_probe);
+
+	schedule_work(&ctx->work);
+
+	return 0;
+}
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	return 0;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+	.runtime_suspend = hdmi_runtime_suspend,
+	.runtime_resume	 = hdmi_runtime_resume,
+};
+
+static int __devexit exynos_drm_hdmi_remove(struct platform_device *pdev)
+{
+	struct drm_hdmi_context *ctx = platform_get_drvdata(pdev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	exynos_drm_subdrv_unregister(&ctx->subdrv);
+	kfree(ctx);
+
+	return 0;
+}
+
+static struct platform_driver exynos_drm_common_hdmi_driver = {
+	.probe		= exynos_drm_hdmi_probe,
+	.remove		= __devexit_p(exynos_drm_hdmi_remove),
+	.driver		= {
+		.name	= "exynos-drm-hdmi",
+		.owner	= THIS_MODULE,
+		.pm = &hdmi_pm_ops,
+	},
+};
+
+static int __init exynos_drm_hdmi_init(void)
+{
+	int ret;
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	ret = platform_driver_register(&exynos_drm_common_hdmi_driver);
+	if (ret) {
+		DRM_DEBUG_KMS("failed to register hdmi common driver.\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static void __exit exynos_drm_hdmi_exit(void)
+{
+	platform_driver_unregister(&exynos_drm_common_hdmi_driver);
+}
+
+module_init(exynos_drm_hdmi_init);
+module_exit(exynos_drm_hdmi_exit);
+
+MODULE_AUTHOR("Inki Dae <inki.dae@samsung.com>");
+MODULE_AUTHOR("Seung-Woo Kim, <sw0312.kim@samsung.com>");
+MODULE_DESCRIPTION("Samsung SoC DRM HDMI Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
new file mode 100644
index 000000000000..3c29f790ee45
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
@@ -0,0 +1,73 @@
+/* exynos_drm_hdmi.h
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Authoer: Inki Dae <inki.dae@samsung.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * VA LINUX SYSTEMS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _EXYNOS_DRM_HDMI_H_
+#define _EXYNOS_DRM_HDMI_H_
+
+/*
+ * exynos hdmi common context structure.
+ *
+ * @drm_dev: pointer to drm_device.
+ * @ctx: pointer to the context of specific device driver.
+ *	this context should be hdmi_context or mixer_context.
+ */
+struct exynos_drm_hdmi_context {
+	struct drm_device	*drm_dev;
+	void			*ctx;
+};
+
+struct exynos_hdmi_display_ops {
+	bool (*is_connected)(void *ctx);
+	int (*get_edid)(void *ctx, struct drm_connector *connector,
+			u8 *edid, int len);
+	int (*check_timing)(void *ctx, void *timing);
+	int (*power_on)(void *ctx, int mode);
+};
+
+struct exynos_hdmi_manager_ops {
+	void (*mode_set)(void *ctx, void *mode);
+	void (*commit)(void *ctx);
+	void (*disable)(void *ctx);
+};
+
+struct exynos_hdmi_overlay_ops {
+	int (*enable_vblank)(void *ctx, int pipe);
+	void (*disable_vblank)(void *ctx);
+	void (*win_mode_set)(void *ctx, struct exynos_drm_overlay *overlay);
+	void (*win_commit)(void *ctx, int zpos);
+	void (*win_disable)(void *ctx, int zpos);
+};
+
+extern struct platform_driver hdmi_driver;
+extern struct platform_driver mixer_driver;
+
+void exynos_drm_display_ops_register(struct exynos_hdmi_display_ops
+					*display_ops);
+void exynos_drm_manager_ops_register(struct exynos_hdmi_manager_ops
+					*manager_ops);
+void exynos_drm_overlay_ops_register(struct exynos_hdmi_overlay_ops
+					*overlay_ops);
+
+#endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.c b/drivers/gpu/drm/exynos/exynos_drm_plane.c
new file mode 100644
index 000000000000..bdcf770aa22e
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.c
@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Authors: Joonyoung Shim <jy0922.shim@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include "drmP.h"
+
+#include "exynos_drm.h"
+#include "exynos_drm_crtc.h"
+#include "exynos_drm_drv.h"
+#include "exynos_drm_encoder.h"
+
+struct exynos_plane {
+	struct drm_plane		base;
+	struct exynos_drm_overlay	overlay;
+	bool				enabled;
+};
+
+static int
+exynos_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
+		     struct drm_framebuffer *fb, int crtc_x, int crtc_y,
+		     unsigned int crtc_w, unsigned int crtc_h,
+		     uint32_t src_x, uint32_t src_y,
+		     uint32_t src_w, uint32_t src_h)
+{
+	struct exynos_plane *exynos_plane =
+		container_of(plane, struct exynos_plane, base);
+	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
+	struct exynos_drm_crtc_pos pos;
+	unsigned int x = src_x >> 16;
+	unsigned int y = src_y >> 16;
+	int ret;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	memset(&pos, 0, sizeof(struct exynos_drm_crtc_pos));
+	pos.crtc_x = crtc_x;
+	pos.crtc_y = crtc_y;
+	pos.crtc_w = crtc_w;
+	pos.crtc_h = crtc_h;
+
+	pos.fb_x = x;
+	pos.fb_y = y;
+
+	/* TODO: scale feature */
+	ret = exynos_drm_overlay_update(overlay, fb, &crtc->mode, &pos);
+	if (ret < 0)
+		return ret;
+
+	exynos_drm_fn_encoder(crtc, overlay,
+			exynos_drm_encoder_crtc_mode_set);
+	exynos_drm_fn_encoder(crtc, &overlay->zpos,
+			exynos_drm_encoder_crtc_plane_commit);
+
+	exynos_plane->enabled = true;
+
+	return 0;
+}
+
+static int exynos_disable_plane(struct drm_plane *plane)
+{
+	struct exynos_plane *exynos_plane =
+		container_of(plane, struct exynos_plane, base);
+	struct exynos_drm_overlay *overlay = &exynos_plane->overlay;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	if (!exynos_plane->enabled)
+		return 0;
+
+	exynos_drm_fn_encoder(plane->crtc, &overlay->zpos,
+			exynos_drm_encoder_crtc_disable);
+
+	exynos_plane->enabled = false;
+	exynos_plane->overlay.zpos = DEFAULT_ZPOS;
+
+	return 0;
+}
+
+static void exynos_plane_destroy(struct drm_plane *plane)
+{
+	struct exynos_plane *exynos_plane =
+		container_of(plane, struct exynos_plane, base);
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	exynos_disable_plane(plane);
+	drm_plane_cleanup(plane);
+	kfree(exynos_plane);
+}
+
+static struct drm_plane_funcs exynos_plane_funcs = {
+	.update_plane	= exynos_update_plane,
+	.disable_plane	= exynos_disable_plane,
+	.destroy	= exynos_plane_destroy,
+};
+
+int exynos_plane_init(struct drm_device *dev, unsigned int nr)
+{
+	struct exynos_plane *exynos_plane;
+	uint32_t possible_crtcs;
+
+	exynos_plane = kzalloc(sizeof(struct exynos_plane), GFP_KERNEL);
+	if (!exynos_plane)
+		return -ENOMEM;
+
+	/* all CRTCs are available */
+	possible_crtcs = (1 << MAX_CRTC) - 1;
+
+	exynos_plane->overlay.zpos = DEFAULT_ZPOS;
+
+	/* TODO: format */
+	return drm_plane_init(dev, &exynos_plane->base, possible_crtcs,
+			      &exynos_plane_funcs, NULL, 0, false);
+}
+
+int exynos_plane_set_zpos_ioctl(struct drm_device *dev, void *data,
+				struct drm_file *file_priv)
+{
+	struct drm_exynos_plane_set_zpos *zpos_req = data;
+	struct drm_mode_object *obj;
+	struct drm_plane *plane;
+	struct exynos_plane *exynos_plane;
+	int ret = 0;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	if (!drm_core_check_feature(dev, DRIVER_MODESET))
+		return -EINVAL;
+
+	if (zpos_req->zpos < 0 || zpos_req->zpos >= MAX_PLANE) {
+		if (zpos_req->zpos != DEFAULT_ZPOS) {
+			DRM_ERROR("zpos not within limits\n");
+			return -EINVAL;
+		}
+	}
+
+	mutex_lock(&dev->mode_config.mutex);
+
+	obj = drm_mode_object_find(dev, zpos_req->plane_id,
+			DRM_MODE_OBJECT_PLANE);
+	if (!obj) {
+		DRM_DEBUG_KMS("Unknown plane ID %d\n",
+			      zpos_req->plane_id);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	plane = obj_to_plane(obj);
+	exynos_plane = container_of(plane, struct exynos_plane, base);
+
+	exynos_plane->overlay.zpos = zpos_req->zpos;
+
+out:
+	mutex_unlock(&dev->mode_config.mutex);
+	return ret;
+}
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.h b/drivers/gpu/drm/exynos/exynos_drm_plane.h
new file mode 100644
index 000000000000..16b71f8217e7
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Authors: Joonyoung Shim <jy0922.shim@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+int exynos_plane_init(struct drm_device *dev, unsigned int nr);
+int exynos_plane_set_zpos_ioctl(struct drm_device *dev, void *data,
+				struct drm_file *file_priv);
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
new file mode 100644
index 000000000000..f48f7ce92f5f
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -0,0 +1,1176 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Authors:
+ * Seung-Woo Kim <sw0312.kim@samsung.com>
+ *	Inki Dae <inki.dae@samsung.com>
+ *	Joonyoung Shim <jy0922.shim@samsung.com>
+ *
+ * Based on drivers/media/video/s5p-tv/hdmi_drv.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;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include "drmP.h"
+#include "drm_edid.h"
+#include "drm_crtc_helper.h"
+
+#include "regs-hdmi.h"
+
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include <drm/exynos_drm.h>
+
+#include "exynos_drm_drv.h"
+#include "exynos_drm_hdmi.h"
+
+#include "exynos_hdmi.h"
+
+#define HDMI_OVERLAY_NUMBER	3
+#define get_hdmi_context(dev)	platform_get_drvdata(to_platform_device(dev))
+
+static const u8 hdmiphy_conf27[32] = {
+	0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
+	0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
+	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+	0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const u8 hdmiphy_conf27_027[32] = {
+	0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
+	0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
+	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+	0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const u8 hdmiphy_conf74_175[32] = {
+	0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xef, 0x5B,
+	0x6D, 0x10, 0x01, 0x51, 0xef, 0xF3, 0x54, 0xb9,
+	0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+	0x22, 0x40, 0xa5, 0x26, 0x01, 0x00, 0x00, 0x00,
+};
+
+static const u8 hdmiphy_conf74_25[32] = {
+	0x01, 0x05, 0x00, 0xd8, 0x10, 0x9c, 0xf8, 0x40,
+	0x6a, 0x10, 0x01, 0x51, 0xff, 0xf1, 0x54, 0xba,
+	0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xe0,
+	0x22, 0x40, 0xa4, 0x26, 0x01, 0x00, 0x00, 0x00,
+};
+
+static const u8 hdmiphy_conf148_5[32] = {
+	0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xf8, 0x40,
+	0x6A, 0x18, 0x00, 0x51, 0xff, 0xF1, 0x54, 0xba,
+	0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+	0x22, 0x40, 0xa4, 0x26, 0x02, 0x00, 0x00, 0x00,
+};
+
+struct hdmi_tg_regs {
+	u8 cmd;
+	u8 h_fsz_l;
+	u8 h_fsz_h;
+	u8 hact_st_l;
+	u8 hact_st_h;
+	u8 hact_sz_l;
+	u8 hact_sz_h;
+	u8 v_fsz_l;
+	u8 v_fsz_h;
+	u8 vsync_l;
+	u8 vsync_h;
+	u8 vsync2_l;
+	u8 vsync2_h;
+	u8 vact_st_l;
+	u8 vact_st_h;
+	u8 vact_sz_l;
+	u8 vact_sz_h;
+	u8 field_chg_l;
+	u8 field_chg_h;
+	u8 vact_st2_l;
+	u8 vact_st2_h;
+	u8 vsync_top_hdmi_l;
+	u8 vsync_top_hdmi_h;
+	u8 vsync_bot_hdmi_l;
+	u8 vsync_bot_hdmi_h;
+	u8 field_top_hdmi_l;
+	u8 field_top_hdmi_h;
+	u8 field_bot_hdmi_l;
+	u8 field_bot_hdmi_h;
+};
+
+struct hdmi_core_regs {
+	u8 h_blank[2];
+	u8 v_blank[3];
+	u8 h_v_line[3];
+	u8 vsync_pol[1];
+	u8 int_pro_mode[1];
+	u8 v_blank_f[3];
+	u8 h_sync_gen[3];
+	u8 v_sync_gen1[3];
+	u8 v_sync_gen2[3];
+	u8 v_sync_gen3[3];
+};
+
+struct hdmi_preset_conf {
+	struct hdmi_core_regs core;
+	struct hdmi_tg_regs tg;
+};
+
+static const struct hdmi_preset_conf hdmi_conf_480p = {
+	.core = {
+		.h_blank = {0x8a, 0x00},
+		.v_blank = {0x0d, 0x6a, 0x01},
+		.h_v_line = {0x0d, 0xa2, 0x35},
+		.vsync_pol = {0x01},
+		.int_pro_mode = {0x00},
+		.v_blank_f = {0x00, 0x00, 0x00},
+		.h_sync_gen = {0x0e, 0x30, 0x11},
+		.v_sync_gen1 = {0x0f, 0x90, 0x00},
+		/* other don't care */
+	},
+	.tg = {
+		0x00, /* cmd */
+		0x5a, 0x03, /* h_fsz */
+		0x8a, 0x00, 0xd0, 0x02, /* hact */
+		0x0d, 0x02, /* v_fsz */
+		0x01, 0x00, 0x33, 0x02, /* vsync */
+		0x2d, 0x00, 0xe0, 0x01, /* vact */
+		0x33, 0x02, /* field_chg */
+		0x49, 0x02, /* vact_st2 */
+		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
+		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+	},
+};
+
+static const struct hdmi_preset_conf hdmi_conf_720p60 = {
+	.core = {
+		.h_blank = {0x72, 0x01},
+		.v_blank = {0xee, 0xf2, 0x00},
+		.h_v_line = {0xee, 0x22, 0x67},
+		.vsync_pol = {0x00},
+		.int_pro_mode = {0x00},
+		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
+		.h_sync_gen = {0x6c, 0x50, 0x02},
+		.v_sync_gen1 = {0x0a, 0x50, 0x00},
+		.v_sync_gen2 = {0x01, 0x10, 0x00},
+		.v_sync_gen3 = {0x01, 0x10, 0x00},
+		/* other don't care */
+	},
+	.tg = {
+		0x00, /* cmd */
+		0x72, 0x06, /* h_fsz */
+		0x71, 0x01, 0x01, 0x05, /* hact */
+		0xee, 0x02, /* v_fsz */
+		0x01, 0x00, 0x33, 0x02, /* vsync */
+		0x1e, 0x00, 0xd0, 0x02, /* vact */
+		0x33, 0x02, /* field_chg */
+		0x49, 0x02, /* vact_st2 */
+		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
+		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+	},
+};
+
+static const struct hdmi_preset_conf hdmi_conf_1080i50 = {
+	.core = {
+		.h_blank = {0xd0, 0x02},
+		.v_blank = {0x32, 0xB2, 0x00},
+		.h_v_line = {0x65, 0x04, 0xa5},
+		.vsync_pol = {0x00},
+		.int_pro_mode = {0x01},
+		.v_blank_f = {0x49, 0x2A, 0x23},
+		.h_sync_gen = {0x0E, 0xEA, 0x08},
+		.v_sync_gen1 = {0x07, 0x20, 0x00},
+		.v_sync_gen2 = {0x39, 0x42, 0x23},
+		.v_sync_gen3 = {0x38, 0x87, 0x73},
+		/* other don't care */
+	},
+	.tg = {
+		0x00, /* cmd */
+		0x50, 0x0A, /* h_fsz */
+		0xCF, 0x02, 0x81, 0x07, /* hact */
+		0x65, 0x04, /* v_fsz */
+		0x01, 0x00, 0x33, 0x02, /* vsync */
+		0x16, 0x00, 0x1c, 0x02, /* vact */
+		0x33, 0x02, /* field_chg */
+		0x49, 0x02, /* vact_st2 */
+		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
+		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+	},
+};
+
+static const struct hdmi_preset_conf hdmi_conf_1080p50 = {
+	.core = {
+		.h_blank = {0xd0, 0x02},
+		.v_blank = {0x65, 0x6c, 0x01},
+		.h_v_line = {0x65, 0x04, 0xa5},
+		.vsync_pol = {0x00},
+		.int_pro_mode = {0x00},
+		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
+		.h_sync_gen = {0x0e, 0xea, 0x08},
+		.v_sync_gen1 = {0x09, 0x40, 0x00},
+		.v_sync_gen2 = {0x01, 0x10, 0x00},
+		.v_sync_gen3 = {0x01, 0x10, 0x00},
+		/* other don't care */
+	},
+	.tg = {
+		0x00, /* cmd */
+		0x50, 0x0A, /* h_fsz */
+		0xCF, 0x02, 0x81, 0x07, /* hact */
+		0x65, 0x04, /* v_fsz */
+		0x01, 0x00, 0x33, 0x02, /* vsync */
+		0x2d, 0x00, 0x38, 0x04, /* vact */
+		0x33, 0x02, /* field_chg */
+		0x48, 0x02, /* vact_st2 */
+		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
+		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+	},
+};
+
+static const struct hdmi_preset_conf hdmi_conf_1080i60 = {
+	.core = {
+		.h_blank = {0x18, 0x01},
+		.v_blank = {0x32, 0xB2, 0x00},
+		.h_v_line = {0x65, 0x84, 0x89},
+		.vsync_pol = {0x00},
+		.int_pro_mode = {0x01},
+		.v_blank_f = {0x49, 0x2A, 0x23},
+		.h_sync_gen = {0x56, 0x08, 0x02},
+		.v_sync_gen1 = {0x07, 0x20, 0x00},
+		.v_sync_gen2 = {0x39, 0x42, 0x23},
+		.v_sync_gen3 = {0xa4, 0x44, 0x4a},
+		/* other don't care */
+	},
+	.tg = {
+		0x00, /* cmd */
+		0x98, 0x08, /* h_fsz */
+		0x17, 0x01, 0x81, 0x07, /* hact */
+		0x65, 0x04, /* v_fsz */
+		0x01, 0x00, 0x33, 0x02, /* vsync */
+		0x16, 0x00, 0x1c, 0x02, /* vact */
+		0x33, 0x02, /* field_chg */
+		0x49, 0x02, /* vact_st2 */
+		0x01, 0x00, 0x33, 0x02, /* vsync top/bot */
+		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+	},
+};
+
+static const struct hdmi_preset_conf hdmi_conf_1080p60 = {
+	.core = {
+		.h_blank = {0x18, 0x01},
+		.v_blank = {0x65, 0x6c, 0x01},
+		.h_v_line = {0x65, 0x84, 0x89},
+		.vsync_pol = {0x00},
+		.int_pro_mode = {0x00},
+		.v_blank_f = {0x00, 0x00, 0x00}, /* don't care */
+		.h_sync_gen = {0x56, 0x08, 0x02},
+		.v_sync_gen1 = {0x09, 0x40, 0x00},
+		.v_sync_gen2 = {0x01, 0x10, 0x00},
+		.v_sync_gen3 = {0x01, 0x10, 0x00},
+		/* other don't care */
+	},
+	.tg = {
+		0x00, /* cmd */
+		0x98, 0x08, /* h_fsz */
+		0x17, 0x01, 0x81, 0x07, /* hact */
+		0x65, 0x04, /* v_fsz */
+		0x01, 0x00, 0x33, 0x02, /* vsync */
+		0x2d, 0x00, 0x38, 0x04, /* vact */
+		0x33, 0x02, /* field_chg */
+		0x48, 0x02, /* vact_st2 */
+		0x01, 0x00, 0x01, 0x00, /* vsync top/bot */
+		0x01, 0x00, 0x33, 0x02, /* field top/bot */
+	},
+};
+
+static const struct hdmi_conf hdmi_confs[] = {
+	{ 1280, 720, 60, false, hdmiphy_conf74_25, &hdmi_conf_720p60 },
+	{ 1280, 720, 50, false, hdmiphy_conf74_25, &hdmi_conf_720p60 },
+	{ 720, 480, 60, false, hdmiphy_conf27_027, &hdmi_conf_480p },
+	{ 1920, 1080, 50, true, hdmiphy_conf74_25, &hdmi_conf_1080i50 },
+	{ 1920, 1080, 50, false, hdmiphy_conf148_5, &hdmi_conf_1080p50 },
+	{ 1920, 1080, 60, true, hdmiphy_conf74_25, &hdmi_conf_1080i60 },
+	{ 1920, 1080, 60, false, hdmiphy_conf148_5, &hdmi_conf_1080p60 },
+};
+
+
+static inline u32 hdmi_reg_read(struct hdmi_context *hdata, u32 reg_id)
+{
+	return readl(hdata->regs + reg_id);
+}
+
+static inline void hdmi_reg_writeb(struct hdmi_context *hdata,
+				 u32 reg_id, u8 value)
+{
+	writeb(value, hdata->regs + reg_id);
+}
+
+static inline void hdmi_reg_writemask(struct hdmi_context *hdata,
+				 u32 reg_id, u32 value, u32 mask)
+{
+	u32 old = readl(hdata->regs + reg_id);
+	value = (value & mask) | (old & ~mask);
+	writel(value, hdata->regs + reg_id);
+}
+
+static void hdmi_regs_dump(struct hdmi_context *hdata, char *prefix)
+{
+#define DUMPREG(reg_id) \
+	DRM_DEBUG_KMS("%s:" #reg_id " = %08x\n", prefix, \
+	readl(hdata->regs + reg_id))
+	DRM_DEBUG_KMS("%s: ---- CONTROL REGISTERS ----\n", prefix);
+	DUMPREG(HDMI_INTC_FLAG);
+	DUMPREG(HDMI_INTC_CON);
+	DUMPREG(HDMI_HPD_STATUS);
+	DUMPREG(HDMI_PHY_RSTOUT);
+	DUMPREG(HDMI_PHY_VPLL);
+	DUMPREG(HDMI_PHY_CMU);
+	DUMPREG(HDMI_CORE_RSTOUT);
+
+	DRM_DEBUG_KMS("%s: ---- CORE REGISTERS ----\n", prefix);
+	DUMPREG(HDMI_CON_0);
+	DUMPREG(HDMI_CON_1);
+	DUMPREG(HDMI_CON_2);
+	DUMPREG(HDMI_SYS_STATUS);
+	DUMPREG(HDMI_PHY_STATUS);
+	DUMPREG(HDMI_STATUS_EN);
+	DUMPREG(HDMI_HPD);
+	DUMPREG(HDMI_MODE_SEL);
+	DUMPREG(HDMI_HPD_GEN);
+	DUMPREG(HDMI_DC_CONTROL);
+	DUMPREG(HDMI_VIDEO_PATTERN_GEN);
+
+	DRM_DEBUG_KMS("%s: ---- CORE SYNC REGISTERS ----\n", prefix);
+	DUMPREG(HDMI_H_BLANK_0);
+	DUMPREG(HDMI_H_BLANK_1);
+	DUMPREG(HDMI_V_BLANK_0);
+	DUMPREG(HDMI_V_BLANK_1);
+	DUMPREG(HDMI_V_BLANK_2);
+	DUMPREG(HDMI_H_V_LINE_0);
+	DUMPREG(HDMI_H_V_LINE_1);
+	DUMPREG(HDMI_H_V_LINE_2);
+	DUMPREG(HDMI_VSYNC_POL);
+	DUMPREG(HDMI_INT_PRO_MODE);
+	DUMPREG(HDMI_V_BLANK_F_0);
+	DUMPREG(HDMI_V_BLANK_F_1);
+	DUMPREG(HDMI_V_BLANK_F_2);
+	DUMPREG(HDMI_H_SYNC_GEN_0);
+	DUMPREG(HDMI_H_SYNC_GEN_1);
+	DUMPREG(HDMI_H_SYNC_GEN_2);
+	DUMPREG(HDMI_V_SYNC_GEN_1_0);
+	DUMPREG(HDMI_V_SYNC_GEN_1_1);
+	DUMPREG(HDMI_V_SYNC_GEN_1_2);
+	DUMPREG(HDMI_V_SYNC_GEN_2_0);
+	DUMPREG(HDMI_V_SYNC_GEN_2_1);
+	DUMPREG(HDMI_V_SYNC_GEN_2_2);
+	DUMPREG(HDMI_V_SYNC_GEN_3_0);
+	DUMPREG(HDMI_V_SYNC_GEN_3_1);
+	DUMPREG(HDMI_V_SYNC_GEN_3_2);
+
+	DRM_DEBUG_KMS("%s: ---- TG REGISTERS ----\n", prefix);
+	DUMPREG(HDMI_TG_CMD);
+	DUMPREG(HDMI_TG_H_FSZ_L);
+	DUMPREG(HDMI_TG_H_FSZ_H);
+	DUMPREG(HDMI_TG_HACT_ST_L);
+	DUMPREG(HDMI_TG_HACT_ST_H);
+	DUMPREG(HDMI_TG_HACT_SZ_L);
+	DUMPREG(HDMI_TG_HACT_SZ_H);
+	DUMPREG(HDMI_TG_V_FSZ_L);
+	DUMPREG(HDMI_TG_V_FSZ_H);
+	DUMPREG(HDMI_TG_VSYNC_L);
+	DUMPREG(HDMI_TG_VSYNC_H);
+	DUMPREG(HDMI_TG_VSYNC2_L);
+	DUMPREG(HDMI_TG_VSYNC2_H);
+	DUMPREG(HDMI_TG_VACT_ST_L);
+	DUMPREG(HDMI_TG_VACT_ST_H);
+	DUMPREG(HDMI_TG_VACT_SZ_L);
+	DUMPREG(HDMI_TG_VACT_SZ_H);
+	DUMPREG(HDMI_TG_FIELD_CHG_L);
+	DUMPREG(HDMI_TG_FIELD_CHG_H);
+	DUMPREG(HDMI_TG_VACT_ST2_L);
+	DUMPREG(HDMI_TG_VACT_ST2_H);
+	DUMPREG(HDMI_TG_VSYNC_TOP_HDMI_L);
+	DUMPREG(HDMI_TG_VSYNC_TOP_HDMI_H);
+	DUMPREG(HDMI_TG_VSYNC_BOT_HDMI_L);
+	DUMPREG(HDMI_TG_VSYNC_BOT_HDMI_H);
+	DUMPREG(HDMI_TG_FIELD_TOP_HDMI_L);
+	DUMPREG(HDMI_TG_FIELD_TOP_HDMI_H);
+	DUMPREG(HDMI_TG_FIELD_BOT_HDMI_L);
+	DUMPREG(HDMI_TG_FIELD_BOT_HDMI_H);
+#undef DUMPREG
+}
+
+static int hdmi_conf_index(struct drm_display_mode *mode)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hdmi_confs); ++i)
+		if (hdmi_confs[i].width == mode->hdisplay &&
+				hdmi_confs[i].height == mode->vdisplay &&
+				hdmi_confs[i].vrefresh == mode->vrefresh &&
+				hdmi_confs[i].interlace ==
+				((mode->flags & DRM_MODE_FLAG_INTERLACE) ?
+				 true : false))
+			return i;
+
+	return -1;
+}
+
+static bool hdmi_is_connected(void *ctx)
+{
+	struct hdmi_context *hdata = (struct hdmi_context *)ctx;
+	u32 val = hdmi_reg_read(hdata, HDMI_HPD_STATUS);
+
+	if (val)
+		return true;
+
+	return false;
+}
+
+static int hdmi_get_edid(void *ctx, struct drm_connector *connector,
+				u8 *edid, int len)
+{
+	struct edid *raw_edid;
+	struct hdmi_context *hdata = (struct hdmi_context *)ctx;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	if (!hdata->ddc_port)
+		return -ENODEV;
+
+	raw_edid = drm_get_edid(connector, hdata->ddc_port->adapter);
+	if (raw_edid) {
+		memcpy(edid, raw_edid, min((1 + raw_edid->extensions)
+					* EDID_LENGTH, len));
+		DRM_DEBUG_KMS("width[%d] x height[%d]\n",
+				raw_edid->width_cm, raw_edid->height_cm);
+	} else {
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int hdmi_check_timing(void *ctx, void *timing)
+{
+	struct fb_videomode *check_timing = timing;
+	int i;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	DRM_DEBUG_KMS("[%d]x[%d] [%d]Hz [%x]\n", check_timing->xres,
+			check_timing->yres, check_timing->refresh,
+			check_timing->vmode);
+
+	for (i = 0; i < ARRAY_SIZE(hdmi_confs); ++i)
+		if (hdmi_confs[i].width == check_timing->xres &&
+			hdmi_confs[i].height == check_timing->yres &&
+			hdmi_confs[i].vrefresh == check_timing->refresh &&
+			hdmi_confs[i].interlace ==
+			((check_timing->vmode & FB_VMODE_INTERLACED) ?
+			 true : false))
+			return 0;
+
+	return -EINVAL;
+}
+
+static int hdmi_display_power_on(void *ctx, int mode)
+{
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		DRM_DEBUG_KMS("hdmi [on]\n");
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+		break;
+	case DRM_MODE_DPMS_SUSPEND:
+		break;
+	case DRM_MODE_DPMS_OFF:
+		DRM_DEBUG_KMS("hdmi [off]\n");
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static struct exynos_hdmi_display_ops display_ops = {
+	.is_connected	= hdmi_is_connected,
+	.get_edid	= hdmi_get_edid,
+	.check_timing	= hdmi_check_timing,
+	.power_on	= hdmi_display_power_on,
+};
+
+static void hdmi_conf_reset(struct hdmi_context *hdata)
+{
+	/* disable hpd handle for drm */
+	hdata->hpd_handle = false;
+
+	/* resetting HDMI core */
+	hdmi_reg_writemask(hdata, HDMI_CORE_RSTOUT,  0, HDMI_CORE_SW_RSTOUT);
+	mdelay(10);
+	hdmi_reg_writemask(hdata, HDMI_CORE_RSTOUT, ~0, HDMI_CORE_SW_RSTOUT);
+	mdelay(10);
+
+	/* enable hpd handle for drm */
+	hdata->hpd_handle = true;
+}
+
+static void hdmi_conf_init(struct hdmi_context *hdata)
+{
+	/* disable hpd handle for drm */
+	hdata->hpd_handle = false;
+
+	/* enable HPD interrupts */
+	hdmi_reg_writemask(hdata, HDMI_INTC_CON, 0, HDMI_INTC_EN_GLOBAL |
+		HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
+	mdelay(10);
+	hdmi_reg_writemask(hdata, HDMI_INTC_CON, ~0, HDMI_INTC_EN_GLOBAL |
+		HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
+
+	/* choose HDMI mode */
+	hdmi_reg_writemask(hdata, HDMI_MODE_SEL,
+		HDMI_MODE_HDMI_EN, HDMI_MODE_MASK);
+	/* disable bluescreen */
+	hdmi_reg_writemask(hdata, HDMI_CON_0, 0, HDMI_BLUE_SCR_EN);
+	/* choose bluescreen (fecal) color */
+	hdmi_reg_writeb(hdata, HDMI_BLUE_SCREEN_0, 0x12);
+	hdmi_reg_writeb(hdata, HDMI_BLUE_SCREEN_1, 0x34);
+	hdmi_reg_writeb(hdata, HDMI_BLUE_SCREEN_2, 0x56);
+	/* enable AVI packet every vsync, fixes purple line problem */
+	hdmi_reg_writeb(hdata, HDMI_AVI_CON, 0x02);
+	/* force RGB, look to CEA-861-D, table 7 for more detail */
+	hdmi_reg_writeb(hdata, HDMI_AVI_BYTE(0), 0 << 5);
+	hdmi_reg_writemask(hdata, HDMI_CON_1, 0x10 << 5, 0x11 << 5);
+
+	hdmi_reg_writeb(hdata, HDMI_SPD_CON, 0x02);
+	hdmi_reg_writeb(hdata, HDMI_AUI_CON, 0x02);
+	hdmi_reg_writeb(hdata, HDMI_ACR_CON, 0x04);
+
+	/* enable hpd handle for drm */
+	hdata->hpd_handle = true;
+}
+
+static void hdmi_timing_apply(struct hdmi_context *hdata,
+				 const struct hdmi_preset_conf *conf)
+{
+	const struct hdmi_core_regs *core = &conf->core;
+	const struct hdmi_tg_regs *tg = &conf->tg;
+	int tries;
+
+	/* setting core registers */
+	hdmi_reg_writeb(hdata, HDMI_H_BLANK_0, core->h_blank[0]);
+	hdmi_reg_writeb(hdata, HDMI_H_BLANK_1, core->h_blank[1]);
+	hdmi_reg_writeb(hdata, HDMI_V_BLANK_0, core->v_blank[0]);
+	hdmi_reg_writeb(hdata, HDMI_V_BLANK_1, core->v_blank[1]);
+	hdmi_reg_writeb(hdata, HDMI_V_BLANK_2, core->v_blank[2]);
+	hdmi_reg_writeb(hdata, HDMI_H_V_LINE_0, core->h_v_line[0]);
+	hdmi_reg_writeb(hdata, HDMI_H_V_LINE_1, core->h_v_line[1]);
+	hdmi_reg_writeb(hdata, HDMI_H_V_LINE_2, core->h_v_line[2]);
+	hdmi_reg_writeb(hdata, HDMI_VSYNC_POL, core->vsync_pol[0]);
+	hdmi_reg_writeb(hdata, HDMI_INT_PRO_MODE, core->int_pro_mode[0]);
+	hdmi_reg_writeb(hdata, HDMI_V_BLANK_F_0, core->v_blank_f[0]);
+	hdmi_reg_writeb(hdata, HDMI_V_BLANK_F_1, core->v_blank_f[1]);
+	hdmi_reg_writeb(hdata, HDMI_V_BLANK_F_2, core->v_blank_f[2]);
+	hdmi_reg_writeb(hdata, HDMI_H_SYNC_GEN_0, core->h_sync_gen[0]);
+	hdmi_reg_writeb(hdata, HDMI_H_SYNC_GEN_1, core->h_sync_gen[1]);
+	hdmi_reg_writeb(hdata, HDMI_H_SYNC_GEN_2, core->h_sync_gen[2]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_1_0, core->v_sync_gen1[0]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_1_1, core->v_sync_gen1[1]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_1_2, core->v_sync_gen1[2]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_2_0, core->v_sync_gen2[0]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_2_1, core->v_sync_gen2[1]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_2_2, core->v_sync_gen2[2]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_3_0, core->v_sync_gen3[0]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_3_1, core->v_sync_gen3[1]);
+	hdmi_reg_writeb(hdata, HDMI_V_SYNC_GEN_3_2, core->v_sync_gen3[2]);
+	/* Timing generator registers */
+	hdmi_reg_writeb(hdata, HDMI_TG_H_FSZ_L, tg->h_fsz_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_H_FSZ_H, tg->h_fsz_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_HACT_ST_L, tg->hact_st_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_HACT_ST_H, tg->hact_st_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_HACT_SZ_L, tg->hact_sz_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_HACT_SZ_H, tg->hact_sz_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_V_FSZ_L, tg->v_fsz_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_V_FSZ_H, tg->v_fsz_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_L, tg->vsync_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_H, tg->vsync_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC2_L, tg->vsync2_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC2_H, tg->vsync2_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST_L, tg->vact_st_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST_H, tg->vact_st_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_SZ_L, tg->vact_sz_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_SZ_H, tg->vact_sz_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_CHG_L, tg->field_chg_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_CHG_H, tg->field_chg_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST2_L, tg->vact_st2_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_VACT_ST2_H, tg->vact_st2_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_TOP_HDMI_L, tg->vsync_top_hdmi_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_TOP_HDMI_H, tg->vsync_top_hdmi_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_BOT_HDMI_L, tg->vsync_bot_hdmi_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_VSYNC_BOT_HDMI_H, tg->vsync_bot_hdmi_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_TOP_HDMI_L, tg->field_top_hdmi_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_TOP_HDMI_H, tg->field_top_hdmi_h);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_BOT_HDMI_L, tg->field_bot_hdmi_l);
+	hdmi_reg_writeb(hdata, HDMI_TG_FIELD_BOT_HDMI_H, tg->field_bot_hdmi_h);
+
+	/* waiting for HDMIPHY's PLL to get to steady state */
+	for (tries = 100; tries; --tries) {
+		u32 val = hdmi_reg_read(hdata, HDMI_PHY_STATUS);
+		if (val & HDMI_PHY_STATUS_READY)
+			break;
+		mdelay(1);
+	}
+	/* steady state not achieved */
+	if (tries == 0) {
+		DRM_ERROR("hdmiphy's pll could not reach steady state.\n");
+		hdmi_regs_dump(hdata, "timing apply");
+	}
+
+	clk_disable(hdata->res.sclk_hdmi);
+	clk_set_parent(hdata->res.sclk_hdmi, hdata->res.sclk_hdmiphy);
+	clk_enable(hdata->res.sclk_hdmi);
+
+	/* enable HDMI and timing generator */
+	hdmi_reg_writemask(hdata, HDMI_CON_0, ~0, HDMI_EN);
+	if (core->int_pro_mode[0])
+		hdmi_reg_writemask(hdata, HDMI_TG_CMD, ~0, HDMI_TG_EN |
+				HDMI_FIELD_EN);
+	else
+		hdmi_reg_writemask(hdata, HDMI_TG_CMD, ~0, HDMI_TG_EN);
+}
+
+static void hdmiphy_conf_reset(struct hdmi_context *hdata)
+{
+	u8 buffer[2];
+
+	clk_disable(hdata->res.sclk_hdmi);
+	clk_set_parent(hdata->res.sclk_hdmi, hdata->res.sclk_pixel);
+	clk_enable(hdata->res.sclk_hdmi);
+
+	/* operation mode */
+	buffer[0] = 0x1f;
+	buffer[1] = 0x00;
+
+	if (hdata->hdmiphy_port)
+		i2c_master_send(hdata->hdmiphy_port, buffer, 2);
+
+	/* reset hdmiphy */
+	hdmi_reg_writemask(hdata, HDMI_PHY_RSTOUT, ~0, HDMI_PHY_SW_RSTOUT);
+	mdelay(10);
+	hdmi_reg_writemask(hdata, HDMI_PHY_RSTOUT,  0, HDMI_PHY_SW_RSTOUT);
+	mdelay(10);
+}
+
+static void hdmiphy_conf_apply(struct hdmi_context *hdata)
+{
+	u8 buffer[32];
+	u8 operation[2];
+	u8 read_buffer[32] = {0, };
+	int ret;
+	int i;
+
+	if (!hdata->hdmiphy_port) {
+		DRM_ERROR("hdmiphy is not attached\n");
+		return;
+	}
+
+	/* pixel clock */
+	memcpy(buffer, hdmi_confs[hdata->cur_conf].hdmiphy_data, 32);
+	ret = i2c_master_send(hdata->hdmiphy_port, buffer, 32);
+	if (ret != 32) {
+		DRM_ERROR("failed to configure HDMIPHY via I2C\n");
+		return;
+	}
+
+	mdelay(10);
+
+	/* operation mode */
+	operation[0] = 0x1f;
+	operation[1] = 0x80;
+
+	ret = i2c_master_send(hdata->hdmiphy_port, operation, 2);
+	if (ret != 2) {
+		DRM_ERROR("failed to enable hdmiphy\n");
+		return;
+	}
+
+	ret = i2c_master_recv(hdata->hdmiphy_port, read_buffer, 32);
+	if (ret < 0) {
+		DRM_ERROR("failed to read hdmiphy config\n");
+		return;
+	}
+
+	for (i = 0; i < ret; i++)
+		DRM_DEBUG_KMS("hdmiphy[0x%02x] write[0x%02x] - "
+			"recv [0x%02x]\n", i, buffer[i], read_buffer[i]);
+}
+
+static void hdmi_conf_apply(struct hdmi_context *hdata)
+{
+	const struct hdmi_preset_conf *conf =
+		  hdmi_confs[hdata->cur_conf].conf;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	hdmiphy_conf_reset(hdata);
+	hdmiphy_conf_apply(hdata);
+
+	hdmi_conf_reset(hdata);
+	hdmi_conf_init(hdata);
+
+	/* setting core registers */
+	hdmi_timing_apply(hdata, conf);
+
+	hdmi_regs_dump(hdata, "start");
+}
+
+static void hdmi_mode_set(void *ctx, void *mode)
+{
+	struct hdmi_context *hdata = (struct hdmi_context *)ctx;
+	int conf_idx;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	conf_idx = hdmi_conf_index(mode);
+	if (conf_idx >= 0 && conf_idx < ARRAY_SIZE(hdmi_confs))
+		hdata->cur_conf = conf_idx;
+	else
+		DRM_DEBUG_KMS("not supported mode\n");
+}
+
+static void hdmi_commit(void *ctx)
+{
+	struct hdmi_context *hdata = (struct hdmi_context *)ctx;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	hdmi_conf_apply(hdata);
+
+	hdata->enabled = true;
+}
+
+static void hdmi_disable(void *ctx)
+{
+	struct hdmi_context *hdata = (struct hdmi_context *)ctx;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	if (hdata->enabled) {
+		hdmiphy_conf_reset(hdata);
+		hdmi_conf_reset(hdata);
+	}
+}
+
+static struct exynos_hdmi_manager_ops manager_ops = {
+	.mode_set	= hdmi_mode_set,
+	.commit		= hdmi_commit,
+	.disable	= hdmi_disable,
+};
+
+/*
+ * Handle hotplug events outside the interrupt handler proper.
+ */
+static void hdmi_hotplug_func(struct work_struct *work)
+{
+	struct hdmi_context *hdata =
+		container_of(work, struct hdmi_context, hotplug_work);
+	struct exynos_drm_hdmi_context *ctx =
+		(struct exynos_drm_hdmi_context *)hdata->parent_ctx;
+
+	drm_helper_hpd_irq_event(ctx->drm_dev);
+}
+
+static irqreturn_t hdmi_irq_handler(int irq, void *arg)
+{
+	struct exynos_drm_hdmi_context *ctx = arg;
+	struct hdmi_context *hdata = (struct hdmi_context *)ctx->ctx;
+	u32 intc_flag;
+
+	intc_flag = hdmi_reg_read(hdata, HDMI_INTC_FLAG);
+	/* clearing flags for HPD plug/unplug */
+	if (intc_flag & HDMI_INTC_FLAG_HPD_UNPLUG) {
+		DRM_DEBUG_KMS("unplugged, handling:%d\n", hdata->hpd_handle);
+		hdmi_reg_writemask(hdata, HDMI_INTC_FLAG, ~0,
+			HDMI_INTC_FLAG_HPD_UNPLUG);
+	}
+	if (intc_flag & HDMI_INTC_FLAG_HPD_PLUG) {
+		DRM_DEBUG_KMS("plugged, handling:%d\n", hdata->hpd_handle);
+		hdmi_reg_writemask(hdata, HDMI_INTC_FLAG, ~0,
+			HDMI_INTC_FLAG_HPD_PLUG);
+	}
+
+	if (ctx->drm_dev && hdata->hpd_handle)
+		queue_work(hdata->wq, &hdata->hotplug_work);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit hdmi_resources_init(struct hdmi_context *hdata)
+{
+	struct device *dev = hdata->dev;
+	struct hdmi_resources *res = &hdata->res;
+	static char *supply[] = {
+		"hdmi-en",
+		"vdd",
+		"vdd_osc",
+		"vdd_pll",
+	};
+	int i, ret;
+
+	DRM_DEBUG_KMS("HDMI resource init\n");
+
+	memset(res, 0, sizeof *res);
+
+	/* get clocks, power */
+	res->hdmi = clk_get(dev, "hdmi");
+	if (IS_ERR_OR_NULL(res->hdmi)) {
+		DRM_ERROR("failed to get clock 'hdmi'\n");
+		goto fail;
+	}
+	res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
+	if (IS_ERR_OR_NULL(res->sclk_hdmi)) {
+		DRM_ERROR("failed to get clock 'sclk_hdmi'\n");
+		goto fail;
+	}
+	res->sclk_pixel = clk_get(dev, "sclk_pixel");
+	if (IS_ERR_OR_NULL(res->sclk_pixel)) {
+		DRM_ERROR("failed to get clock 'sclk_pixel'\n");
+		goto fail;
+	}
+	res->sclk_hdmiphy = clk_get(dev, "sclk_hdmiphy");
+	if (IS_ERR_OR_NULL(res->sclk_hdmiphy)) {
+		DRM_ERROR("failed to get clock 'sclk_hdmiphy'\n");
+		goto fail;
+	}
+	res->hdmiphy = clk_get(dev, "hdmiphy");
+	if (IS_ERR_OR_NULL(res->hdmiphy)) {
+		DRM_ERROR("failed to get clock 'hdmiphy'\n");
+		goto fail;
+	}
+
+	clk_set_parent(res->sclk_hdmi, res->sclk_pixel);
+
+	res->regul_bulk = kzalloc(ARRAY_SIZE(supply) *
+		sizeof res->regul_bulk[0], GFP_KERNEL);
+	if (!res->regul_bulk) {
+		DRM_ERROR("failed to get memory for regulators\n");
+		goto fail;
+	}
+	for (i = 0; i < ARRAY_SIZE(supply); ++i) {
+		res->regul_bulk[i].supply = supply[i];
+		res->regul_bulk[i].consumer = NULL;
+	}
+	ret = regulator_bulk_get(dev, ARRAY_SIZE(supply), res->regul_bulk);
+	if (ret) {
+		DRM_ERROR("failed to get regulators\n");
+		goto fail;
+	}
+	res->regul_count = ARRAY_SIZE(supply);
+
+	return 0;
+fail:
+	DRM_ERROR("HDMI resource init - failed\n");
+	return -ENODEV;
+}
+
+static int hdmi_resources_cleanup(struct hdmi_context *hdata)
+{
+	struct hdmi_resources *res = &hdata->res;
+
+	regulator_bulk_free(res->regul_count, res->regul_bulk);
+	/* kfree is NULL-safe */
+	kfree(res->regul_bulk);
+	if (!IS_ERR_OR_NULL(res->hdmiphy))
+		clk_put(res->hdmiphy);
+	if (!IS_ERR_OR_NULL(res->sclk_hdmiphy))
+		clk_put(res->sclk_hdmiphy);
+	if (!IS_ERR_OR_NULL(res->sclk_pixel))
+		clk_put(res->sclk_pixel);
+	if (!IS_ERR_OR_NULL(res->sclk_hdmi))
+		clk_put(res->sclk_hdmi);
+	if (!IS_ERR_OR_NULL(res->hdmi))
+		clk_put(res->hdmi);
+	memset(res, 0, sizeof *res);
+
+	return 0;
+}
+
+static void hdmi_resource_poweron(struct hdmi_context *hdata)
+{
+	struct hdmi_resources *res = &hdata->res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	/* turn HDMI power on */
+	regulator_bulk_enable(res->regul_count, res->regul_bulk);
+	/* power-on hdmi physical interface */
+	clk_enable(res->hdmiphy);
+	/* turn clocks on */
+	clk_enable(res->hdmi);
+	clk_enable(res->sclk_hdmi);
+
+	hdmiphy_conf_reset(hdata);
+	hdmi_conf_reset(hdata);
+	hdmi_conf_init(hdata);
+
+}
+
+static void hdmi_resource_poweroff(struct hdmi_context *hdata)
+{
+	struct hdmi_resources *res = &hdata->res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	/* turn clocks off */
+	clk_disable(res->sclk_hdmi);
+	clk_disable(res->hdmi);
+	/* power-off hdmiphy */
+	clk_disable(res->hdmiphy);
+	/* turn HDMI power off */
+	regulator_bulk_disable(res->regul_count, res->regul_bulk);
+}
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
+
+	DRM_DEBUG_KMS("%s\n", __func__);
+
+	hdmi_resource_poweroff((struct hdmi_context *)ctx->ctx);
+
+	return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *ctx = get_hdmi_context(dev);
+
+	DRM_DEBUG_KMS("%s\n", __func__);
+
+	hdmi_resource_poweron((struct hdmi_context *)ctx->ctx);
+
+	return 0;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+	.runtime_suspend = hdmi_runtime_suspend,
+	.runtime_resume	 = hdmi_runtime_resume,
+};
+
+static struct i2c_client *hdmi_ddc, *hdmi_hdmiphy;
+
+void hdmi_attach_ddc_client(struct i2c_client *ddc)
+{
+	if (ddc)
+		hdmi_ddc = ddc;
+}
+EXPORT_SYMBOL(hdmi_attach_ddc_client);
+
+void hdmi_attach_hdmiphy_client(struct i2c_client *hdmiphy)
+{
+	if (hdmiphy)
+		hdmi_hdmiphy = hdmiphy;
+}
+EXPORT_SYMBOL(hdmi_attach_hdmiphy_client);
+
+static int __devinit hdmi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct exynos_drm_hdmi_context *drm_hdmi_ctx;
+	struct hdmi_context *hdata;
+	struct exynos_drm_hdmi_pdata *pdata;
+	struct resource *res;
+	int ret;
+
+	DRM_DEBUG_KMS("[%d]\n", __LINE__);
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		DRM_ERROR("no platform data specified\n");
+		return -EINVAL;
+	}
+
+	drm_hdmi_ctx = kzalloc(sizeof(*drm_hdmi_ctx), GFP_KERNEL);
+	if (!drm_hdmi_ctx) {
+		DRM_ERROR("failed to allocate common hdmi context.\n");
+		return -ENOMEM;
+	}
+
+	hdata = kzalloc(sizeof(struct hdmi_context), GFP_KERNEL);
+	if (!hdata) {
+		DRM_ERROR("out of memory\n");
+		kfree(drm_hdmi_ctx);
+		return -ENOMEM;
+	}
+
+	drm_hdmi_ctx->ctx = (void *)hdata;
+	hdata->parent_ctx = (void *)drm_hdmi_ctx;
+
+	platform_set_drvdata(pdev, drm_hdmi_ctx);
+
+	hdata->default_win = pdata->default_win;
+	hdata->default_timing = &pdata->timing;
+	hdata->default_bpp = pdata->bpp;
+	hdata->dev = dev;
+
+	ret = hdmi_resources_init(hdata);
+	if (ret) {
+		ret = -EINVAL;
+		goto err_data;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		DRM_ERROR("failed to find registers\n");
+		ret = -ENOENT;
+		goto err_resource;
+	}
+
+	hdata->regs_res = request_mem_region(res->start, resource_size(res),
+					   dev_name(dev));
+	if (!hdata->regs_res) {
+		DRM_ERROR("failed to claim register region\n");
+		ret = -ENOENT;
+		goto err_resource;
+	}
+
+	hdata->regs = ioremap(res->start, resource_size(res));
+	if (!hdata->regs) {
+		DRM_ERROR("failed to map registers\n");
+		ret = -ENXIO;
+		goto err_req_region;
+	}
+
+	/* DDC i2c driver */
+	if (i2c_add_driver(&ddc_driver)) {
+		DRM_ERROR("failed to register ddc i2c driver\n");
+		ret = -ENOENT;
+		goto err_iomap;
+	}
+
+	hdata->ddc_port = hdmi_ddc;
+
+	/* hdmiphy i2c driver */
+	if (i2c_add_driver(&hdmiphy_driver)) {
+		DRM_ERROR("failed to register hdmiphy i2c driver\n");
+		ret = -ENOENT;
+		goto err_ddc;
+	}
+
+	hdata->hdmiphy_port = hdmi_hdmiphy;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		DRM_ERROR("get interrupt resource failed.\n");
+		ret = -ENXIO;
+		goto err_hdmiphy;
+	}
+
+	/* create workqueue and hotplug work */
+	hdata->wq = alloc_workqueue("exynos-drm-hdmi",
+			WQ_UNBOUND | WQ_NON_REENTRANT, 1);
+	if (hdata->wq == NULL) {
+		DRM_ERROR("Failed to create workqueue.\n");
+		ret = -ENOMEM;
+		goto err_hdmiphy;
+	}
+	INIT_WORK(&hdata->hotplug_work, hdmi_hotplug_func);
+
+	/* register hpd interrupt */
+	ret = request_irq(res->start, hdmi_irq_handler, 0, "drm_hdmi",
+				drm_hdmi_ctx);
+	if (ret) {
+		DRM_ERROR("request interrupt failed.\n");
+		goto err_workqueue;
+	}
+	hdata->irq = res->start;
+
+	/* register specific callbacks to common hdmi. */
+	exynos_drm_display_ops_register(&display_ops);
+	exynos_drm_manager_ops_register(&manager_ops);
+
+	hdmi_resource_poweron(hdata);
+
+	return 0;
+
+err_workqueue:
+	destroy_workqueue(hdata->wq);
+err_hdmiphy:
+	i2c_del_driver(&hdmiphy_driver);
+err_ddc:
+	i2c_del_driver(&ddc_driver);
+err_iomap:
+	iounmap(hdata->regs);
+err_req_region:
+	release_resource(hdata->regs_res);
+	kfree(hdata->regs_res);
+err_resource:
+	hdmi_resources_cleanup(hdata);
+err_data:
+	kfree(hdata);
+	kfree(drm_hdmi_ctx);
+	return ret;
+}
+
+static int __devexit hdmi_remove(struct platform_device *pdev)
+{
+	struct exynos_drm_hdmi_context *ctx = platform_get_drvdata(pdev);
+	struct hdmi_context *hdata = (struct hdmi_context *)ctx->ctx;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	hdmi_resource_poweroff(hdata);
+
+	disable_irq(hdata->irq);
+	free_irq(hdata->irq, hdata);
+
+	cancel_work_sync(&hdata->hotplug_work);
+	destroy_workqueue(hdata->wq);
+
+	hdmi_resources_cleanup(hdata);
+
+	iounmap(hdata->regs);
+
+	release_resource(hdata->regs_res);
+	kfree(hdata->regs_res);
+
+	/* hdmiphy i2c driver */
+	i2c_del_driver(&hdmiphy_driver);
+	/* DDC i2c driver */
+	i2c_del_driver(&ddc_driver);
+
+	kfree(hdata);
+
+	return 0;
+}
+
+struct platform_driver hdmi_driver = {
+	.probe		= hdmi_probe,
+	.remove		= __devexit_p(hdmi_remove),
+	.driver		= {
+		.name	= "exynos4-hdmi",
+		.owner	= THIS_MODULE,
+		.pm = &hdmi_pm_ops,
+	},
+};
+EXPORT_SYMBOL(hdmi_driver);
+
+MODULE_AUTHOR("Seung-Woo Kim, <sw0312.kim@samsung.com>");
+MODULE_AUTHOR("Inki Dae <inki.dae@samsung.com>");
+MODULE_AUTHOR("Joonyoung Shim <jy0922.shim@samsung.com>");
+MODULE_DESCRIPTION("Samsung DRM HDMI core Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.h b/drivers/gpu/drm/exynos/exynos_hdmi.h
new file mode 100644
index 000000000000..31d6cf84c1aa
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.h
@@ -0,0 +1,87 @@
+/*
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Authors:
+ *	Inki Dae <inki.dae@samsung.com>
+ *	Seung-Woo Kim <sw0312.kim@samsung.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * VA LINUX SYSTEMS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _EXYNOS_HDMI_H_
+#define _EXYNOS_HDMI_H_
+
+struct hdmi_conf {
+	int width;
+	int height;
+	int vrefresh;
+	bool interlace;
+	const u8 *hdmiphy_data;
+	const struct hdmi_preset_conf *conf;
+};
+
+struct hdmi_resources {
+	struct clk *hdmi;
+	struct clk *sclk_hdmi;
+	struct clk *sclk_pixel;
+	struct clk *sclk_hdmiphy;
+	struct clk *hdmiphy;
+	struct regulator_bulk_data *regul_bulk;
+	int regul_count;
+};
+
+struct hdmi_context {
+	struct device			*dev;
+	struct drm_device		*drm_dev;
+	struct fb_videomode		*default_timing;
+	unsigned int			default_win;
+	unsigned int			default_bpp;
+	bool				hpd_handle;
+	bool				enabled;
+
+	struct resource			*regs_res;
+	/** base address of HDMI registers */
+	void __iomem *regs;
+	/** HDMI hotplug interrupt */
+	unsigned int irq;
+	/** workqueue for delayed work */
+	struct workqueue_struct *wq;
+	/** hotplug handling work */
+	struct work_struct hotplug_work;
+
+	struct i2c_client *ddc_port;
+	struct i2c_client *hdmiphy_port;
+
+	/** current hdmiphy conf index */
+	int cur_conf;
+	/** other resources */
+	struct hdmi_resources res;
+
+	void *parent_ctx;
+};
+
+
+void hdmi_attach_ddc_client(struct i2c_client *ddc);
+void hdmi_attach_hdmiphy_client(struct i2c_client *hdmiphy);
+
+extern struct i2c_driver hdmiphy_driver;
+extern struct i2c_driver ddc_driver;
+
+#endif
diff --git a/drivers/gpu/drm/exynos/exynos_hdmiphy.c b/drivers/gpu/drm/exynos/exynos_hdmiphy.c
new file mode 100644
index 000000000000..9fe2995ab9f9
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_hdmiphy.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Authors:
+ *	Seung-Woo Kim <sw0312.kim@samsung.com>
+ *	Inki Dae <inki.dae@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include "drmP.h"
+
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+
+#include "exynos_drm_drv.h"
+#include "exynos_hdmi.h"
+
+
+static int hdmiphy_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	hdmi_attach_hdmiphy_client(client);
+
+	dev_info(&client->adapter->dev, "attached s5p_hdmiphy "
+		"into i2c adapter successfully\n");
+
+	return 0;
+}
+
+static int hdmiphy_remove(struct i2c_client *client)
+{
+	dev_info(&client->adapter->dev, "detached s5p_hdmiphy "
+		"from i2c adapter successfully\n");
+
+	return 0;
+}
+
+static const struct i2c_device_id hdmiphy_id[] = {
+	{ "s5p_hdmiphy", 0 },
+	{ },
+};
+
+struct i2c_driver hdmiphy_driver = {
+	.driver = {
+		.name	= "s5p-hdmiphy",
+		.owner	= THIS_MODULE,
+	},
+	.id_table = hdmiphy_id,
+	.probe		= hdmiphy_probe,
+	.remove		= __devexit_p(hdmiphy_remove),
+	.command		= NULL,
+};
+EXPORT_SYMBOL(hdmiphy_driver);
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
new file mode 100644
index 000000000000..ac24cff39775
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -0,0 +1,1070 @@
+/*
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Authors:
+ * Seung-Woo Kim <sw0312.kim@samsung.com>
+ *	Inki Dae <inki.dae@samsung.com>
+ *	Joonyoung Shim <jy0922.shim@samsung.com>
+ *
+ * Based on drivers/media/video/s5p-tv/mixer_reg.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;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include "drmP.h"
+
+#include "regs-mixer.h"
+#include "regs-vp.h"
+
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include <drm/exynos_drm.h>
+
+#include "exynos_drm_drv.h"
+#include "exynos_drm_hdmi.h"
+#include "exynos_hdmi.h"
+#include "exynos_mixer.h"
+
+#define get_mixer_context(dev)	platform_get_drvdata(to_platform_device(dev))
+
+static const u8 filter_y_horiz_tap8[] = {
+	0,	-1,	-1,	-1,	-1,	-1,	-1,	-1,
+	-1,	-1,	-1,	-1,	-1,	0,	0,	0,
+	0,	2,	4,	5,	6,	6,	6,	6,
+	6,	5,	5,	4,	3,	2,	1,	1,
+	0,	-6,	-12,	-16,	-18,	-20,	-21,	-20,
+	-20,	-18,	-16,	-13,	-10,	-8,	-5,	-2,
+	127,	126,	125,	121,	114,	107,	99,	89,
+	79,	68,	57,	46,	35,	25,	16,	8,
+};
+
+static const u8 filter_y_vert_tap4[] = {
+	0,	-3,	-6,	-8,	-8,	-8,	-8,	-7,
+	-6,	-5,	-4,	-3,	-2,	-1,	-1,	0,
+	127,	126,	124,	118,	111,	102,	92,	81,
+	70,	59,	48,	37,	27,	19,	11,	5,
+	0,	5,	11,	19,	27,	37,	48,	59,
+	70,	81,	92,	102,	111,	118,	124,	126,
+	0,	0,	-1,	-1,	-2,	-3,	-4,	-5,
+	-6,	-7,	-8,	-8,	-8,	-8,	-6,	-3,
+};
+
+static const u8 filter_cr_horiz_tap4[] = {
+	0,	-3,	-6,	-8,	-8,	-8,	-8,	-7,
+	-6,	-5,	-4,	-3,	-2,	-1,	-1,	0,
+	127,	126,	124,	118,	111,	102,	92,	81,
+	70,	59,	48,	37,	27,	19,	11,	5,
+};
+
+static inline u32 vp_reg_read(struct mixer_resources *res, u32 reg_id)
+{
+	return readl(res->vp_regs + reg_id);
+}
+
+static inline void vp_reg_write(struct mixer_resources *res, u32 reg_id,
+				 u32 val)
+{
+	writel(val, res->vp_regs + reg_id);
+}
+
+static inline void vp_reg_writemask(struct mixer_resources *res, u32 reg_id,
+				 u32 val, u32 mask)
+{
+	u32 old = vp_reg_read(res, reg_id);
+
+	val = (val & mask) | (old & ~mask);
+	writel(val, res->vp_regs + reg_id);
+}
+
+static inline u32 mixer_reg_read(struct mixer_resources *res, u32 reg_id)
+{
+	return readl(res->mixer_regs + reg_id);
+}
+
+static inline void mixer_reg_write(struct mixer_resources *res, u32 reg_id,
+				 u32 val)
+{
+	writel(val, res->mixer_regs + reg_id);
+}
+
+static inline void mixer_reg_writemask(struct mixer_resources *res,
+				 u32 reg_id, u32 val, u32 mask)
+{
+	u32 old = mixer_reg_read(res, reg_id);
+
+	val = (val & mask) | (old & ~mask);
+	writel(val, res->mixer_regs + reg_id);
+}
+
+static void mixer_regs_dump(struct mixer_context *ctx)
+{
+#define DUMPREG(reg_id) \
+do { \
+	DRM_DEBUG_KMS(#reg_id " = %08x\n", \
+		(u32)readl(ctx->mixer_res.mixer_regs + reg_id)); \
+} while (0)
+
+	DUMPREG(MXR_STATUS);
+	DUMPREG(MXR_CFG);
+	DUMPREG(MXR_INT_EN);
+	DUMPREG(MXR_INT_STATUS);
+
+	DUMPREG(MXR_LAYER_CFG);
+	DUMPREG(MXR_VIDEO_CFG);
+
+	DUMPREG(MXR_GRAPHIC0_CFG);
+	DUMPREG(MXR_GRAPHIC0_BASE);
+	DUMPREG(MXR_GRAPHIC0_SPAN);
+	DUMPREG(MXR_GRAPHIC0_WH);
+	DUMPREG(MXR_GRAPHIC0_SXY);
+	DUMPREG(MXR_GRAPHIC0_DXY);
+
+	DUMPREG(MXR_GRAPHIC1_CFG);
+	DUMPREG(MXR_GRAPHIC1_BASE);
+	DUMPREG(MXR_GRAPHIC1_SPAN);
+	DUMPREG(MXR_GRAPHIC1_WH);
+	DUMPREG(MXR_GRAPHIC1_SXY);
+	DUMPREG(MXR_GRAPHIC1_DXY);
+#undef DUMPREG
+}
+
+static void vp_regs_dump(struct mixer_context *ctx)
+{
+#define DUMPREG(reg_id) \
+do { \
+	DRM_DEBUG_KMS(#reg_id " = %08x\n", \
+		(u32) readl(ctx->mixer_res.vp_regs + reg_id)); \
+} while (0)
+
+	DUMPREG(VP_ENABLE);
+	DUMPREG(VP_SRESET);
+	DUMPREG(VP_SHADOW_UPDATE);
+	DUMPREG(VP_FIELD_ID);
+	DUMPREG(VP_MODE);
+	DUMPREG(VP_IMG_SIZE_Y);
+	DUMPREG(VP_IMG_SIZE_C);
+	DUMPREG(VP_PER_RATE_CTRL);
+	DUMPREG(VP_TOP_Y_PTR);
+	DUMPREG(VP_BOT_Y_PTR);
+	DUMPREG(VP_TOP_C_PTR);
+	DUMPREG(VP_BOT_C_PTR);
+	DUMPREG(VP_ENDIAN_MODE);
+	DUMPREG(VP_SRC_H_POSITION);
+	DUMPREG(VP_SRC_V_POSITION);
+	DUMPREG(VP_SRC_WIDTH);
+	DUMPREG(VP_SRC_HEIGHT);
+	DUMPREG(VP_DST_H_POSITION);
+	DUMPREG(VP_DST_V_POSITION);
+	DUMPREG(VP_DST_WIDTH);
+	DUMPREG(VP_DST_HEIGHT);
+	DUMPREG(VP_H_RATIO);
+	DUMPREG(VP_V_RATIO);
+
+#undef DUMPREG
+}
+
+static inline void vp_filter_set(struct mixer_resources *res,
+		int reg_id, const u8 *data, unsigned int size)
+{
+	/* assure 4-byte align */
+	BUG_ON(size & 3);
+	for (; size; size -= 4, reg_id += 4, data += 4) {
+		u32 val = (data[0] << 24) |  (data[1] << 16) |
+			(data[2] << 8) | data[3];
+		vp_reg_write(res, reg_id, val);
+	}
+}
+
+static void vp_default_filter(struct mixer_resources *res)
+{
+	vp_filter_set(res, VP_POLY8_Y0_LL,
+		filter_y_horiz_tap8, sizeof filter_y_horiz_tap8);
+	vp_filter_set(res, VP_POLY4_Y0_LL,
+		filter_y_vert_tap4, sizeof filter_y_vert_tap4);
+	vp_filter_set(res, VP_POLY4_C0_LL,
+		filter_cr_horiz_tap4, sizeof filter_cr_horiz_tap4);
+}
+
+static void mixer_vsync_set_update(struct mixer_context *ctx, bool enable)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	/* block update on vsync */
+	mixer_reg_writemask(res, MXR_STATUS, enable ?
+			MXR_STATUS_SYNC_ENABLE : 0, MXR_STATUS_SYNC_ENABLE);
+
+	vp_reg_write(res, VP_SHADOW_UPDATE, enable ?
+			VP_SHADOW_UPDATE_ENABLE : 0);
+}
+
+static void mixer_cfg_scan(struct mixer_context *ctx, unsigned int height)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	u32 val;
+
+	/* choosing between interlace and progressive mode */
+	val = (ctx->interlace ? MXR_CFG_SCAN_INTERLACE :
+				MXR_CFG_SCAN_PROGRASSIVE);
+
+	/* choosing between porper HD and SD mode */
+	if (height == 480)
+		val |= MXR_CFG_SCAN_NTSC | MXR_CFG_SCAN_SD;
+	else if (height == 576)
+		val |= MXR_CFG_SCAN_PAL | MXR_CFG_SCAN_SD;
+	else if (height == 720)
+		val |= MXR_CFG_SCAN_HD_720 | MXR_CFG_SCAN_HD;
+	else if (height == 1080)
+		val |= MXR_CFG_SCAN_HD_1080 | MXR_CFG_SCAN_HD;
+	else
+		val |= MXR_CFG_SCAN_HD_720 | MXR_CFG_SCAN_HD;
+
+	mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_SCAN_MASK);
+}
+
+static void mixer_cfg_rgb_fmt(struct mixer_context *ctx, unsigned int height)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	u32 val;
+
+	if (height == 480) {
+		val = MXR_CFG_RGB601_0_255;
+	} else if (height == 576) {
+		val = MXR_CFG_RGB601_0_255;
+	} else if (height == 720) {
+		val = MXR_CFG_RGB709_16_235;
+		mixer_reg_write(res, MXR_CM_COEFF_Y,
+				(1 << 30) | (94 << 20) | (314 << 10) |
+				(32 << 0));
+		mixer_reg_write(res, MXR_CM_COEFF_CB,
+				(972 << 20) | (851 << 10) | (225 << 0));
+		mixer_reg_write(res, MXR_CM_COEFF_CR,
+				(225 << 20) | (820 << 10) | (1004 << 0));
+	} else if (height == 1080) {
+		val = MXR_CFG_RGB709_16_235;
+		mixer_reg_write(res, MXR_CM_COEFF_Y,
+				(1 << 30) | (94 << 20) | (314 << 10) |
+				(32 << 0));
+		mixer_reg_write(res, MXR_CM_COEFF_CB,
+				(972 << 20) | (851 << 10) | (225 << 0));
+		mixer_reg_write(res, MXR_CM_COEFF_CR,
+				(225 << 20) | (820 << 10) | (1004 << 0));
+	} else {
+		val = MXR_CFG_RGB709_16_235;
+		mixer_reg_write(res, MXR_CM_COEFF_Y,
+				(1 << 30) | (94 << 20) | (314 << 10) |
+				(32 << 0));
+		mixer_reg_write(res, MXR_CM_COEFF_CB,
+				(972 << 20) | (851 << 10) | (225 << 0));
+		mixer_reg_write(res, MXR_CM_COEFF_CR,
+				(225 << 20) | (820 << 10) | (1004 << 0));
+	}
+
+	mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_RGB_FMT_MASK);
+}
+
+static void mixer_cfg_layer(struct mixer_context *ctx, int win, bool enable)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	u32 val = enable ? ~0 : 0;
+
+	switch (win) {
+	case 0:
+		mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_GRP0_ENABLE);
+		break;
+	case 1:
+		mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_GRP1_ENABLE);
+		break;
+	case 2:
+		vp_reg_writemask(res, VP_ENABLE, val, VP_ENABLE_ON);
+		mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_VP_ENABLE);
+		break;
+	}
+}
+
+static void mixer_run(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	mixer_reg_writemask(res, MXR_STATUS, ~0, MXR_STATUS_REG_RUN);
+
+	mixer_regs_dump(ctx);
+}
+
+static void vp_video_buffer(struct mixer_context *ctx, int win)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	unsigned long flags;
+	struct hdmi_win_data *win_data;
+	unsigned int full_width, full_height, width, height;
+	unsigned int x_ratio, y_ratio;
+	unsigned int src_x_offset, src_y_offset, dst_x_offset, dst_y_offset;
+	unsigned int mode_width, mode_height;
+	unsigned int buf_num;
+	dma_addr_t luma_addr[2], chroma_addr[2];
+	bool tiled_mode = false;
+	bool crcb_mode = false;
+	u32 val;
+
+	win_data = &ctx->win_data[win];
+
+	switch (win_data->pixel_format) {
+	case DRM_FORMAT_NV12MT:
+		tiled_mode = true;
+	case DRM_FORMAT_NV12M:
+		crcb_mode = false;
+		buf_num = 2;
+		break;
+	/* TODO: single buffer format NV12, NV21 */
+	default:
+		/* ignore pixel format at disable time */
+		if (!win_data->dma_addr)
+			break;
+
+		DRM_ERROR("pixel format for vp is wrong [%d].\n",
+				win_data->pixel_format);
+		return;
+	}
+
+	full_width = win_data->fb_width;
+	full_height = win_data->fb_height;
+	width = win_data->crtc_width;
+	height = win_data->crtc_height;
+	mode_width = win_data->mode_width;
+	mode_height = win_data->mode_height;
+
+	/* scaling feature: (src << 16) / dst */
+	x_ratio = (width << 16) / width;
+	y_ratio = (height << 16) / height;
+
+	src_x_offset = win_data->fb_x;
+	src_y_offset = win_data->fb_y;
+	dst_x_offset = win_data->crtc_x;
+	dst_y_offset = win_data->crtc_y;
+
+	if (buf_num == 2) {
+		luma_addr[0] = win_data->dma_addr;
+		chroma_addr[0] = win_data->chroma_dma_addr;
+	} else {
+		luma_addr[0] = win_data->dma_addr;
+		chroma_addr[0] = win_data->dma_addr
+			+ (full_width * full_height);
+	}
+
+	if (win_data->scan_flags & DRM_MODE_FLAG_INTERLACE) {
+		ctx->interlace = true;
+		if (tiled_mode) {
+			luma_addr[1] = luma_addr[0] + 0x40;
+			chroma_addr[1] = chroma_addr[0] + 0x40;
+		} else {
+			luma_addr[1] = luma_addr[0] + full_width;
+			chroma_addr[1] = chroma_addr[0] + full_width;
+		}
+	} else {
+		ctx->interlace = false;
+		luma_addr[1] = 0;
+		chroma_addr[1] = 0;
+	}
+
+	spin_lock_irqsave(&res->reg_slock, flags);
+	mixer_vsync_set_update(ctx, false);
+
+	/* interlace or progressive scan mode */
+	val = (ctx->interlace ? ~0 : 0);
+	vp_reg_writemask(res, VP_MODE, val, VP_MODE_LINE_SKIP);
+
+	/* setup format */
+	val = (crcb_mode ? VP_MODE_NV21 : VP_MODE_NV12);
+	val |= (tiled_mode ? VP_MODE_MEM_TILED : VP_MODE_MEM_LINEAR);
+	vp_reg_writemask(res, VP_MODE, val, VP_MODE_FMT_MASK);
+
+	/* setting size of input image */
+	vp_reg_write(res, VP_IMG_SIZE_Y, VP_IMG_HSIZE(full_width) |
+		VP_IMG_VSIZE(full_height));
+	/* chroma height has to reduced by 2 to avoid chroma distorions */
+	vp_reg_write(res, VP_IMG_SIZE_C, VP_IMG_HSIZE(full_width) |
+		VP_IMG_VSIZE(full_height / 2));
+
+	vp_reg_write(res, VP_SRC_WIDTH, width);
+	vp_reg_write(res, VP_SRC_HEIGHT, height);
+	vp_reg_write(res, VP_SRC_H_POSITION,
+			VP_SRC_H_POSITION_VAL(src_x_offset));
+	vp_reg_write(res, VP_SRC_V_POSITION, src_y_offset);
+
+	vp_reg_write(res, VP_DST_WIDTH, width);
+	vp_reg_write(res, VP_DST_H_POSITION, dst_x_offset);
+	if (ctx->interlace) {
+		vp_reg_write(res, VP_DST_HEIGHT, height / 2);
+		vp_reg_write(res, VP_DST_V_POSITION, dst_y_offset / 2);
+	} else {
+		vp_reg_write(res, VP_DST_HEIGHT, height);
+		vp_reg_write(res, VP_DST_V_POSITION, dst_y_offset);
+	}
+
+	vp_reg_write(res, VP_H_RATIO, x_ratio);
+	vp_reg_write(res, VP_V_RATIO, y_ratio);
+
+	vp_reg_write(res, VP_ENDIAN_MODE, VP_ENDIAN_MODE_LITTLE);
+
+	/* set buffer address to vp */
+	vp_reg_write(res, VP_TOP_Y_PTR, luma_addr[0]);
+	vp_reg_write(res, VP_BOT_Y_PTR, luma_addr[1]);
+	vp_reg_write(res, VP_TOP_C_PTR, chroma_addr[0]);
+	vp_reg_write(res, VP_BOT_C_PTR, chroma_addr[1]);
+
+	mixer_cfg_scan(ctx, mode_height);
+	mixer_cfg_rgb_fmt(ctx, mode_height);
+	mixer_cfg_layer(ctx, win, true);
+	mixer_run(ctx);
+
+	mixer_vsync_set_update(ctx, true);
+	spin_unlock_irqrestore(&res->reg_slock, flags);
+
+	vp_regs_dump(ctx);
+}
+
+static void mixer_graph_buffer(struct mixer_context *ctx, int win)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	unsigned long flags;
+	struct hdmi_win_data *win_data;
+	unsigned int full_width, width, height;
+	unsigned int x_ratio, y_ratio;
+	unsigned int src_x_offset, src_y_offset, dst_x_offset, dst_y_offset;
+	unsigned int mode_width, mode_height;
+	dma_addr_t dma_addr;
+	unsigned int fmt;
+	u32 val;
+
+	win_data = &ctx->win_data[win];
+
+	#define RGB565 4
+	#define ARGB1555 5
+	#define ARGB4444 6
+	#define ARGB8888 7
+
+	switch (win_data->bpp) {
+	case 16:
+		fmt = ARGB4444;
+		break;
+	case 32:
+		fmt = ARGB8888;
+		break;
+	default:
+		fmt = ARGB8888;
+	}
+
+	dma_addr = win_data->dma_addr;
+	full_width = win_data->fb_width;
+	width = win_data->crtc_width;
+	height = win_data->crtc_height;
+	mode_width = win_data->mode_width;
+	mode_height = win_data->mode_height;
+
+	/* 2x scaling feature */
+	x_ratio = 0;
+	y_ratio = 0;
+
+	src_x_offset = win_data->fb_x;
+	src_y_offset = win_data->fb_y;
+	dst_x_offset = win_data->crtc_x;
+	dst_y_offset = win_data->crtc_y;
+
+	/* converting dma address base and source offset */
+	dma_addr = dma_addr
+		+ (src_x_offset * win_data->bpp >> 3)
+		+ (src_y_offset * full_width * win_data->bpp >> 3);
+	src_x_offset = 0;
+	src_y_offset = 0;
+
+	if (win_data->scan_flags & DRM_MODE_FLAG_INTERLACE)
+		ctx->interlace = true;
+	else
+		ctx->interlace = false;
+
+	spin_lock_irqsave(&res->reg_slock, flags);
+	mixer_vsync_set_update(ctx, false);
+
+	/* setup format */
+	mixer_reg_writemask(res, MXR_GRAPHIC_CFG(win),
+		MXR_GRP_CFG_FORMAT_VAL(fmt), MXR_GRP_CFG_FORMAT_MASK);
+
+	/* setup geometry */
+	mixer_reg_write(res, MXR_GRAPHIC_SPAN(win), full_width);
+
+	val  = MXR_GRP_WH_WIDTH(width);
+	val |= MXR_GRP_WH_HEIGHT(height);
+	val |= MXR_GRP_WH_H_SCALE(x_ratio);
+	val |= MXR_GRP_WH_V_SCALE(y_ratio);
+	mixer_reg_write(res, MXR_GRAPHIC_WH(win), val);
+
+	/* setup offsets in source image */
+	val  = MXR_GRP_SXY_SX(src_x_offset);
+	val |= MXR_GRP_SXY_SY(src_y_offset);
+	mixer_reg_write(res, MXR_GRAPHIC_SXY(win), val);
+
+	/* setup offsets in display image */
+	val  = MXR_GRP_DXY_DX(dst_x_offset);
+	val |= MXR_GRP_DXY_DY(dst_y_offset);
+	mixer_reg_write(res, MXR_GRAPHIC_DXY(win), val);
+
+	/* set buffer address to mixer */
+	mixer_reg_write(res, MXR_GRAPHIC_BASE(win), dma_addr);
+
+	mixer_cfg_scan(ctx, mode_height);
+	mixer_cfg_rgb_fmt(ctx, mode_height);
+	mixer_cfg_layer(ctx, win, true);
+	mixer_run(ctx);
+
+	mixer_vsync_set_update(ctx, true);
+	spin_unlock_irqrestore(&res->reg_slock, flags);
+}
+
+static void vp_win_reset(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	int tries = 100;
+
+	vp_reg_write(res, VP_SRESET, VP_SRESET_PROCESSING);
+	for (tries = 100; tries; --tries) {
+		/* waiting until VP_SRESET_PROCESSING is 0 */
+		if (~vp_reg_read(res, VP_SRESET) & VP_SRESET_PROCESSING)
+			break;
+		mdelay(10);
+	}
+	WARN(tries == 0, "failed to reset Video Processor\n");
+}
+
+static int mixer_enable_vblank(void *ctx, int pipe)
+{
+	struct mixer_context *mixer_ctx = ctx;
+	struct mixer_resources *res = &mixer_ctx->mixer_res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	mixer_ctx->pipe = pipe;
+
+	/* enable vsync interrupt */
+	mixer_reg_writemask(res, MXR_INT_EN, MXR_INT_EN_VSYNC,
+			MXR_INT_EN_VSYNC);
+
+	return 0;
+}
+
+static void mixer_disable_vblank(void *ctx)
+{
+	struct mixer_context *mixer_ctx = ctx;
+	struct mixer_resources *res = &mixer_ctx->mixer_res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	/* disable vsync interrupt */
+	mixer_reg_writemask(res, MXR_INT_EN, 0, MXR_INT_EN_VSYNC);
+}
+
+static void mixer_win_mode_set(void *ctx,
+			      struct exynos_drm_overlay *overlay)
+{
+	struct mixer_context *mixer_ctx = ctx;
+	struct hdmi_win_data *win_data;
+	int win;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	if (!overlay) {
+		DRM_ERROR("overlay is NULL\n");
+		return;
+	}
+
+	DRM_DEBUG_KMS("set [%d]x[%d] at (%d,%d) to [%d]x[%d] at (%d,%d)\n",
+				 overlay->fb_width, overlay->fb_height,
+				 overlay->fb_x, overlay->fb_y,
+				 overlay->crtc_width, overlay->crtc_height,
+				 overlay->crtc_x, overlay->crtc_y);
+
+	win = overlay->zpos;
+	if (win == DEFAULT_ZPOS)
+		win = mixer_ctx->default_win;
+
+	if (win < 0 || win > HDMI_OVERLAY_NUMBER) {
+		DRM_ERROR("overlay plane[%d] is wrong\n", win);
+		return;
+	}
+
+	win_data = &mixer_ctx->win_data[win];
+
+	win_data->dma_addr = overlay->dma_addr[0];
+	win_data->vaddr = overlay->vaddr[0];
+	win_data->chroma_dma_addr = overlay->dma_addr[1];
+	win_data->chroma_vaddr = overlay->vaddr[1];
+	win_data->pixel_format = overlay->pixel_format;
+	win_data->bpp = overlay->bpp;
+
+	win_data->crtc_x = overlay->crtc_x;
+	win_data->crtc_y = overlay->crtc_y;
+	win_data->crtc_width = overlay->crtc_width;
+	win_data->crtc_height = overlay->crtc_height;
+
+	win_data->fb_x = overlay->fb_x;
+	win_data->fb_y = overlay->fb_y;
+	win_data->fb_width = overlay->fb_width;
+	win_data->fb_height = overlay->fb_height;
+
+	win_data->mode_width = overlay->mode_width;
+	win_data->mode_height = overlay->mode_height;
+
+	win_data->scan_flags = overlay->scan_flag;
+}
+
+static void mixer_win_commit(void *ctx, int zpos)
+{
+	struct mixer_context *mixer_ctx = ctx;
+	int win = zpos;
+
+	DRM_DEBUG_KMS("[%d] %s, win: %d\n", __LINE__, __func__, win);
+
+	if (win == DEFAULT_ZPOS)
+		win = mixer_ctx->default_win;
+
+	if (win < 0 || win > HDMI_OVERLAY_NUMBER) {
+		DRM_ERROR("overlay plane[%d] is wrong\n", win);
+		return;
+	}
+
+	if (win > 1)
+		vp_video_buffer(mixer_ctx, win);
+	else
+		mixer_graph_buffer(mixer_ctx, win);
+}
+
+static void mixer_win_disable(void *ctx, int zpos)
+{
+	struct mixer_context *mixer_ctx = ctx;
+	struct mixer_resources *res = &mixer_ctx->mixer_res;
+	unsigned long flags;
+	int win = zpos;
+
+	DRM_DEBUG_KMS("[%d] %s, win: %d\n", __LINE__, __func__, win);
+
+	if (win == DEFAULT_ZPOS)
+		win = mixer_ctx->default_win;
+
+	if (win < 0 || win > HDMI_OVERLAY_NUMBER) {
+		DRM_ERROR("overlay plane[%d] is wrong\n", win);
+		return;
+	}
+
+	spin_lock_irqsave(&res->reg_slock, flags);
+	mixer_vsync_set_update(mixer_ctx, false);
+
+	mixer_cfg_layer(mixer_ctx, win, false);
+
+	mixer_vsync_set_update(mixer_ctx, true);
+	spin_unlock_irqrestore(&res->reg_slock, flags);
+}
+
+static struct exynos_hdmi_overlay_ops overlay_ops = {
+	.enable_vblank		= mixer_enable_vblank,
+	.disable_vblank		= mixer_disable_vblank,
+	.win_mode_set		= mixer_win_mode_set,
+	.win_commit		= mixer_win_commit,
+	.win_disable		= mixer_win_disable,
+};
+
+/* for pageflip event */
+static void mixer_finish_pageflip(struct drm_device *drm_dev, int crtc)
+{
+	struct exynos_drm_private *dev_priv = drm_dev->dev_private;
+	struct drm_pending_vblank_event *e, *t;
+	struct timeval now;
+	unsigned long flags;
+	bool is_checked = false;
+
+	spin_lock_irqsave(&drm_dev->event_lock, flags);
+
+	list_for_each_entry_safe(e, t, &dev_priv->pageflip_event_list,
+			base.link) {
+		/* if event's pipe isn't same as crtc then ignore it. */
+		if (crtc != e->pipe)
+			continue;
+
+		is_checked = true;
+		do_gettimeofday(&now);
+		e->event.sequence = 0;
+		e->event.tv_sec = now.tv_sec;
+		e->event.tv_usec = now.tv_usec;
+
+		list_move_tail(&e->base.link, &e->base.file_priv->event_list);
+		wake_up_interruptible(&e->base.file_priv->event_wait);
+	}
+
+	if (is_checked)
+		drm_vblank_put(drm_dev, crtc);
+
+	spin_unlock_irqrestore(&drm_dev->event_lock, flags);
+}
+
+static irqreturn_t mixer_irq_handler(int irq, void *arg)
+{
+	struct exynos_drm_hdmi_context *drm_hdmi_ctx = arg;
+	struct mixer_context *ctx =
+			(struct mixer_context *)drm_hdmi_ctx->ctx;
+	struct mixer_resources *res = &ctx->mixer_res;
+	u32 val, val_base;
+
+	spin_lock(&res->reg_slock);
+
+	/* read interrupt status for handling and clearing flags for VSYNC */
+	val = mixer_reg_read(res, MXR_INT_STATUS);
+
+	/* handling VSYNC */
+	if (val & MXR_INT_STATUS_VSYNC) {
+		/* interlace scan need to check shadow register */
+		if (ctx->interlace) {
+			val_base = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(0));
+			if (ctx->win_data[0].dma_addr != val_base)
+				goto out;
+
+			val_base = mixer_reg_read(res, MXR_GRAPHIC_BASE_S(1));
+			if (ctx->win_data[1].dma_addr != val_base)
+				goto out;
+		}
+
+		drm_handle_vblank(drm_hdmi_ctx->drm_dev, ctx->pipe);
+		mixer_finish_pageflip(drm_hdmi_ctx->drm_dev, ctx->pipe);
+	}
+
+out:
+	/* clear interrupts */
+	if (~val & MXR_INT_EN_VSYNC) {
+		/* vsync interrupt use different bit for read and clear */
+		val &= ~MXR_INT_EN_VSYNC;
+		val |= MXR_INT_CLEAR_VSYNC;
+	}
+	mixer_reg_write(res, MXR_INT_STATUS, val);
+
+	spin_unlock(&res->reg_slock);
+
+	return IRQ_HANDLED;
+}
+
+static void mixer_win_reset(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	unsigned long flags;
+	u32 val; /* value stored to register */
+
+	spin_lock_irqsave(&res->reg_slock, flags);
+	mixer_vsync_set_update(ctx, false);
+
+	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_DST_HDMI, MXR_CFG_DST_MASK);
+
+	/* set output in RGB888 mode */
+	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_OUT_RGB888, MXR_CFG_OUT_MASK);
+
+	/* 16 beat burst in DMA */
+	mixer_reg_writemask(res, MXR_STATUS, MXR_STATUS_16_BURST,
+		MXR_STATUS_BURST_MASK);
+
+	/* setting default layer priority: layer1 > video > layer0
+	 * because typical usage scenario would be
+	 * layer0 - framebuffer
+	 * video - video overlay
+	 * layer1 - OSD
+	 */
+	val  = MXR_LAYER_CFG_GRP0_VAL(1);
+	val |= MXR_LAYER_CFG_VP_VAL(2);
+	val |= MXR_LAYER_CFG_GRP1_VAL(3);
+	mixer_reg_write(res, MXR_LAYER_CFG, val);
+
+	/* setting background color */
+	mixer_reg_write(res, MXR_BG_COLOR0, 0x008080);
+	mixer_reg_write(res, MXR_BG_COLOR1, 0x008080);
+	mixer_reg_write(res, MXR_BG_COLOR2, 0x008080);
+
+	/* setting graphical layers */
+
+	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
+	val |= MXR_GRP_CFG_WIN_BLEND_EN;
+	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
+
+	/* the same configuration for both layers */
+	mixer_reg_write(res, MXR_GRAPHIC_CFG(0), val);
+
+	val |= MXR_GRP_CFG_BLEND_PRE_MUL;
+	val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
+	mixer_reg_write(res, MXR_GRAPHIC_CFG(1), val);
+
+	/* configuration of Video Processor Registers */
+	vp_win_reset(ctx);
+	vp_default_filter(res);
+
+	/* disable all layers */
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP0_ENABLE);
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP1_ENABLE);
+	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_VP_ENABLE);
+
+	mixer_vsync_set_update(ctx, true);
+	spin_unlock_irqrestore(&res->reg_slock, flags);
+}
+
+static void mixer_resource_poweron(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	clk_enable(res->mixer);
+	clk_enable(res->vp);
+	clk_enable(res->sclk_mixer);
+
+	mixer_win_reset(ctx);
+}
+
+static void mixer_resource_poweroff(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
+
+	clk_disable(res->mixer);
+	clk_disable(res->vp);
+	clk_disable(res->sclk_mixer);
+}
+
+static int mixer_runtime_resume(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *ctx = get_mixer_context(dev);
+
+	DRM_DEBUG_KMS("resume - start\n");
+
+	mixer_resource_poweron((struct mixer_context *)ctx->ctx);
+
+	return 0;
+}
+
+static int mixer_runtime_suspend(struct device *dev)
+{
+	struct exynos_drm_hdmi_context *ctx = get_mixer_context(dev);
+
+	DRM_DEBUG_KMS("suspend - start\n");
+
+	mixer_resource_poweroff((struct mixer_context *)ctx->ctx);
+
+	return 0;
+}
+
+static const struct dev_pm_ops mixer_pm_ops = {
+	.runtime_suspend = mixer_runtime_suspend,
+	.runtime_resume	 = mixer_runtime_resume,
+};
+
+static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
+				 struct platform_device *pdev)
+{
+	struct mixer_context *mixer_ctx =
+			(struct mixer_context *)ctx->ctx;
+	struct device *dev = &pdev->dev;
+	struct mixer_resources *mixer_res = &mixer_ctx->mixer_res;
+	struct resource *res;
+	int ret;
+
+	mixer_res->dev = dev;
+	spin_lock_init(&mixer_res->reg_slock);
+
+	mixer_res->mixer = clk_get(dev, "mixer");
+	if (IS_ERR_OR_NULL(mixer_res->mixer)) {
+		dev_err(dev, "failed to get clock 'mixer'\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+	mixer_res->vp = clk_get(dev, "vp");
+	if (IS_ERR_OR_NULL(mixer_res->vp)) {
+		dev_err(dev, "failed to get clock 'vp'\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+	mixer_res->sclk_mixer = clk_get(dev, "sclk_mixer");
+	if (IS_ERR_OR_NULL(mixer_res->sclk_mixer)) {
+		dev_err(dev, "failed to get clock 'sclk_mixer'\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+	mixer_res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
+	if (IS_ERR_OR_NULL(mixer_res->sclk_hdmi)) {
+		dev_err(dev, "failed to get clock 'sclk_hdmi'\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+	mixer_res->sclk_dac = clk_get(dev, "sclk_dac");
+	if (IS_ERR_OR_NULL(mixer_res->sclk_dac)) {
+		dev_err(dev, "failed to get clock 'sclk_dac'\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "mxr");
+	if (res == NULL) {
+		dev_err(dev, "get memory resource failed.\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	clk_set_parent(mixer_res->sclk_mixer, mixer_res->sclk_hdmi);
+
+	mixer_res->mixer_regs = ioremap(res->start, resource_size(res));
+	if (mixer_res->mixer_regs == NULL) {
+		dev_err(dev, "register mapping failed.\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vp");
+	if (res == NULL) {
+		dev_err(dev, "get memory resource failed.\n");
+		ret = -ENXIO;
+		goto fail_mixer_regs;
+	}
+
+	mixer_res->vp_regs = ioremap(res->start, resource_size(res));
+	if (mixer_res->vp_regs == NULL) {
+		dev_err(dev, "register mapping failed.\n");
+		ret = -ENXIO;
+		goto fail_mixer_regs;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "irq");
+	if (res == NULL) {
+		dev_err(dev, "get interrupt resource failed.\n");
+		ret = -ENXIO;
+		goto fail_vp_regs;
+	}
+
+	ret = request_irq(res->start, mixer_irq_handler, 0, "drm_mixer", ctx);
+	if (ret) {
+		dev_err(dev, "request interrupt failed.\n");
+		goto fail_vp_regs;
+	}
+	mixer_res->irq = res->start;
+
+	return 0;
+
+fail_vp_regs:
+	iounmap(mixer_res->vp_regs);
+
+fail_mixer_regs:
+	iounmap(mixer_res->mixer_regs);
+
+fail:
+	if (!IS_ERR_OR_NULL(mixer_res->sclk_dac))
+		clk_put(mixer_res->sclk_dac);
+	if (!IS_ERR_OR_NULL(mixer_res->sclk_hdmi))
+		clk_put(mixer_res->sclk_hdmi);
+	if (!IS_ERR_OR_NULL(mixer_res->sclk_mixer))
+		clk_put(mixer_res->sclk_mixer);
+	if (!IS_ERR_OR_NULL(mixer_res->vp))
+		clk_put(mixer_res->vp);
+	if (!IS_ERR_OR_NULL(mixer_res->mixer))
+		clk_put(mixer_res->mixer);
+	mixer_res->dev = NULL;
+	return ret;
+}
+
+static void mixer_resources_cleanup(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	disable_irq(res->irq);
+	free_irq(res->irq, ctx);
+
+	iounmap(res->vp_regs);
+	iounmap(res->mixer_regs);
+}
+
+static int __devinit mixer_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct exynos_drm_hdmi_context *drm_hdmi_ctx;
+	struct mixer_context *ctx;
+	int ret;
+
+	dev_info(dev, "probe start\n");
+
+	drm_hdmi_ctx = kzalloc(sizeof(*drm_hdmi_ctx), GFP_KERNEL);
+	if (!drm_hdmi_ctx) {
+		DRM_ERROR("failed to allocate common hdmi context.\n");
+		return -ENOMEM;
+	}
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		DRM_ERROR("failed to alloc mixer context.\n");
+		kfree(drm_hdmi_ctx);
+		return -ENOMEM;
+	}
+
+	drm_hdmi_ctx->ctx = (void *)ctx;
+
+	platform_set_drvdata(pdev, drm_hdmi_ctx);
+
+	/* acquire resources: regs, irqs, clocks */
+	ret = mixer_resources_init(drm_hdmi_ctx, pdev);
+	if (ret)
+		goto fail;
+
+	/* register specific callback point to common hdmi. */
+	exynos_drm_overlay_ops_register(&overlay_ops);
+
+	mixer_resource_poweron(ctx);
+
+	return 0;
+
+
+fail:
+	dev_info(dev, "probe failed\n");
+	return ret;
+}
+
+static int mixer_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct exynos_drm_hdmi_context *drm_hdmi_ctx =
+					platform_get_drvdata(pdev);
+	struct mixer_context *ctx = (struct mixer_context *)drm_hdmi_ctx->ctx;
+
+	dev_info(dev, "remove sucessful\n");
+
+	mixer_resource_poweroff(ctx);
+	mixer_resources_cleanup(ctx);
+
+	return 0;
+}
+
+struct platform_driver mixer_driver = {
+	.driver = {
+		.name = "s5p-mixer",
+		.owner = THIS_MODULE,
+		.pm = &mixer_pm_ops,
+	},
+	.probe = mixer_probe,
+	.remove = __devexit_p(mixer_remove),
+};
+EXPORT_SYMBOL(mixer_driver);
+
+MODULE_AUTHOR("Seung-Woo Kim, <sw0312.kim@samsung.com>");
+MODULE_AUTHOR("Inki Dae <inki.dae@samsung.com>");
+MODULE_AUTHOR("Joonyoung Shim <jy0922.shim@samsung.com>");
+MODULE_DESCRIPTION("Samsung DRM HDMI mixer Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.h b/drivers/gpu/drm/exynos/exynos_mixer.h
new file mode 100644
index 000000000000..cebacfefc077
--- /dev/null
+++ b/drivers/gpu/drm/exynos/exynos_mixer.h
@@ -0,0 +1,92 @@
+/*
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Authors:
+ *	Seung-Woo Kim <sw0312.kim@samsung.com>
+ *	Inki Dae <inki.dae@samsung.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * VA LINUX SYSTEMS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _EXYNOS_MIXER_H_
+#define _EXYNOS_MIXER_H_
+
+#define HDMI_OVERLAY_NUMBER	3
+
+struct hdmi_win_data {
+	dma_addr_t		dma_addr;
+	void __iomem		*vaddr;
+	dma_addr_t		chroma_dma_addr;
+	void __iomem		*chroma_vaddr;
+	uint32_t		pixel_format;
+	unsigned int		bpp;
+	unsigned int		crtc_x;
+	unsigned int		crtc_y;
+	unsigned int		crtc_width;
+	unsigned int		crtc_height;
+	unsigned int		fb_x;
+	unsigned int		fb_y;
+	unsigned int		fb_width;
+	unsigned int		fb_height;
+	unsigned int		mode_width;
+	unsigned int		mode_height;
+	unsigned int		scan_flags;
+};
+
+struct mixer_resources {
+	struct device *dev;
+	/** interrupt index */
+	int irq;
+	/** pointer to Mixer registers */
+	void __iomem *mixer_regs;
+	/** pointer to Video Processor registers */
+	void __iomem *vp_regs;
+	/** spinlock for protection of registers */
+	spinlock_t reg_slock;
+	/** other resources */
+	struct clk *mixer;
+	struct clk *vp;
+	struct clk *sclk_mixer;
+	struct clk *sclk_hdmi;
+	struct clk *sclk_dac;
+};
+
+struct mixer_context {
+	unsigned int			default_win;
+	struct fb_videomode		*default_timing;
+	unsigned int			default_bpp;
+
+	/** mixer interrupt */
+	unsigned int irq;
+	/** current crtc pipe for vblank */
+	int pipe;
+	/** interlace scan mode */
+	bool interlace;
+	/** vp enabled status */
+	bool vp_enabled;
+
+	/** mixer and vp resources */
+	struct mixer_resources mixer_res;
+
+	/** overlay window data */
+	struct hdmi_win_data		win_data[HDMI_OVERLAY_NUMBER];
+};
+
+#endif
diff --git a/drivers/gpu/drm/exynos/regs-hdmi.h b/drivers/gpu/drm/exynos/regs-hdmi.h
new file mode 100644
index 000000000000..72e6b52be740
--- /dev/null
+++ b/drivers/gpu/drm/exynos/regs-hdmi.h
@@ -0,0 +1,147 @@
+/*
+ *
+ *  Cloned from drivers/media/video/s5p-tv/regs-hdmi.h
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * HDMI register header file for Samsung TVOUT driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef SAMSUNG_REGS_HDMI_H
+#define SAMSUNG_REGS_HDMI_H
+
+/*
+ * Register part
+*/
+
+#define HDMI_CTRL_BASE(x)		((x) + 0x00000000)
+#define HDMI_CORE_BASE(x)		((x) + 0x00010000)
+#define HDMI_TG_BASE(x)			((x) + 0x00050000)
+
+/* Control registers */
+#define HDMI_INTC_CON			HDMI_CTRL_BASE(0x0000)
+#define HDMI_INTC_FLAG			HDMI_CTRL_BASE(0x0004)
+#define HDMI_HPD_STATUS			HDMI_CTRL_BASE(0x000C)
+#define HDMI_PHY_RSTOUT			HDMI_CTRL_BASE(0x0014)
+#define HDMI_PHY_VPLL			HDMI_CTRL_BASE(0x0018)
+#define HDMI_PHY_CMU			HDMI_CTRL_BASE(0x001C)
+#define HDMI_CORE_RSTOUT		HDMI_CTRL_BASE(0x0020)
+
+/* Core registers */
+#define HDMI_CON_0			HDMI_CORE_BASE(0x0000)
+#define HDMI_CON_1			HDMI_CORE_BASE(0x0004)
+#define HDMI_CON_2			HDMI_CORE_BASE(0x0008)
+#define HDMI_SYS_STATUS			HDMI_CORE_BASE(0x0010)
+#define HDMI_PHY_STATUS			HDMI_CORE_BASE(0x0014)
+#define HDMI_STATUS_EN			HDMI_CORE_BASE(0x0020)
+#define HDMI_HPD			HDMI_CORE_BASE(0x0030)
+#define HDMI_MODE_SEL			HDMI_CORE_BASE(0x0040)
+#define HDMI_BLUE_SCREEN_0		HDMI_CORE_BASE(0x0050)
+#define HDMI_BLUE_SCREEN_1		HDMI_CORE_BASE(0x0054)
+#define HDMI_BLUE_SCREEN_2		HDMI_CORE_BASE(0x0058)
+#define HDMI_H_BLANK_0			HDMI_CORE_BASE(0x00A0)
+#define HDMI_H_BLANK_1			HDMI_CORE_BASE(0x00A4)
+#define HDMI_V_BLANK_0			HDMI_CORE_BASE(0x00B0)
+#define HDMI_V_BLANK_1			HDMI_CORE_BASE(0x00B4)
+#define HDMI_V_BLANK_2			HDMI_CORE_BASE(0x00B8)
+#define HDMI_H_V_LINE_0			HDMI_CORE_BASE(0x00C0)
+#define HDMI_H_V_LINE_1			HDMI_CORE_BASE(0x00C4)
+#define HDMI_H_V_LINE_2			HDMI_CORE_BASE(0x00C8)
+#define HDMI_VSYNC_POL			HDMI_CORE_BASE(0x00E4)
+#define HDMI_INT_PRO_MODE		HDMI_CORE_BASE(0x00E8)
+#define HDMI_V_BLANK_F_0		HDMI_CORE_BASE(0x0110)
+#define HDMI_V_BLANK_F_1		HDMI_CORE_BASE(0x0114)
+#define HDMI_V_BLANK_F_2		HDMI_CORE_BASE(0x0118)
+#define HDMI_H_SYNC_GEN_0		HDMI_CORE_BASE(0x0120)
+#define HDMI_H_SYNC_GEN_1		HDMI_CORE_BASE(0x0124)
+#define HDMI_H_SYNC_GEN_2		HDMI_CORE_BASE(0x0128)
+#define HDMI_V_SYNC_GEN_1_0		HDMI_CORE_BASE(0x0130)
+#define HDMI_V_SYNC_GEN_1_1		HDMI_CORE_BASE(0x0134)
+#define HDMI_V_SYNC_GEN_1_2		HDMI_CORE_BASE(0x0138)
+#define HDMI_V_SYNC_GEN_2_0		HDMI_CORE_BASE(0x0140)
+#define HDMI_V_SYNC_GEN_2_1		HDMI_CORE_BASE(0x0144)
+#define HDMI_V_SYNC_GEN_2_2		HDMI_CORE_BASE(0x0148)
+#define HDMI_V_SYNC_GEN_3_0		HDMI_CORE_BASE(0x0150)
+#define HDMI_V_SYNC_GEN_3_1		HDMI_CORE_BASE(0x0154)
+#define HDMI_V_SYNC_GEN_3_2		HDMI_CORE_BASE(0x0158)
+#define HDMI_ACR_CON			HDMI_CORE_BASE(0x0180)
+#define HDMI_AVI_CON			HDMI_CORE_BASE(0x0300)
+#define HDMI_AVI_BYTE(n)		HDMI_CORE_BASE(0x0320 + 4 * (n))
+#define HDMI_DC_CONTROL			HDMI_CORE_BASE(0x05C0)
+#define HDMI_VIDEO_PATTERN_GEN		HDMI_CORE_BASE(0x05C4)
+#define HDMI_HPD_GEN			HDMI_CORE_BASE(0x05C8)
+#define HDMI_AUI_CON			HDMI_CORE_BASE(0x0360)
+#define HDMI_SPD_CON			HDMI_CORE_BASE(0x0400)
+
+/* Timing generator registers */
+#define HDMI_TG_CMD			HDMI_TG_BASE(0x0000)
+#define HDMI_TG_H_FSZ_L			HDMI_TG_BASE(0x0018)
+#define HDMI_TG_H_FSZ_H			HDMI_TG_BASE(0x001C)
+#define HDMI_TG_HACT_ST_L		HDMI_TG_BASE(0x0020)
+#define HDMI_TG_HACT_ST_H		HDMI_TG_BASE(0x0024)
+#define HDMI_TG_HACT_SZ_L		HDMI_TG_BASE(0x0028)
+#define HDMI_TG_HACT_SZ_H		HDMI_TG_BASE(0x002C)
+#define HDMI_TG_V_FSZ_L			HDMI_TG_BASE(0x0030)
+#define HDMI_TG_V_FSZ_H			HDMI_TG_BASE(0x0034)
+#define HDMI_TG_VSYNC_L			HDMI_TG_BASE(0x0038)
+#define HDMI_TG_VSYNC_H			HDMI_TG_BASE(0x003C)
+#define HDMI_TG_VSYNC2_L		HDMI_TG_BASE(0x0040)
+#define HDMI_TG_VSYNC2_H		HDMI_TG_BASE(0x0044)
+#define HDMI_TG_VACT_ST_L		HDMI_TG_BASE(0x0048)
+#define HDMI_TG_VACT_ST_H		HDMI_TG_BASE(0x004C)
+#define HDMI_TG_VACT_SZ_L		HDMI_TG_BASE(0x0050)
+#define HDMI_TG_VACT_SZ_H		HDMI_TG_BASE(0x0054)
+#define HDMI_TG_FIELD_CHG_L		HDMI_TG_BASE(0x0058)
+#define HDMI_TG_FIELD_CHG_H		HDMI_TG_BASE(0x005C)
+#define HDMI_TG_VACT_ST2_L		HDMI_TG_BASE(0x0060)
+#define HDMI_TG_VACT_ST2_H		HDMI_TG_BASE(0x0064)
+#define HDMI_TG_VSYNC_TOP_HDMI_L	HDMI_TG_BASE(0x0078)
+#define HDMI_TG_VSYNC_TOP_HDMI_H	HDMI_TG_BASE(0x007C)
+#define HDMI_TG_VSYNC_BOT_HDMI_L	HDMI_TG_BASE(0x0080)
+#define HDMI_TG_VSYNC_BOT_HDMI_H	HDMI_TG_BASE(0x0084)
+#define HDMI_TG_FIELD_TOP_HDMI_L	HDMI_TG_BASE(0x0088)
+#define HDMI_TG_FIELD_TOP_HDMI_H	HDMI_TG_BASE(0x008C)
+#define HDMI_TG_FIELD_BOT_HDMI_L	HDMI_TG_BASE(0x0090)
+#define HDMI_TG_FIELD_BOT_HDMI_H	HDMI_TG_BASE(0x0094)
+
+/*
+ * Bit definition part
+ */
+
+/* HDMI_INTC_CON */
+#define HDMI_INTC_EN_GLOBAL		(1 << 6)
+#define HDMI_INTC_EN_HPD_PLUG		(1 << 3)
+#define HDMI_INTC_EN_HPD_UNPLUG		(1 << 2)
+
+/* HDMI_INTC_FLAG */
+#define HDMI_INTC_FLAG_HPD_PLUG		(1 << 3)
+#define HDMI_INTC_FLAG_HPD_UNPLUG	(1 << 2)
+
+/* HDMI_PHY_RSTOUT */
+#define HDMI_PHY_SW_RSTOUT		(1 << 0)
+
+/* HDMI_CORE_RSTOUT */
+#define HDMI_CORE_SW_RSTOUT		(1 << 0)
+
+/* HDMI_CON_0 */
+#define HDMI_BLUE_SCR_EN		(1 << 5)
+#define HDMI_EN				(1 << 0)
+
+/* HDMI_PHY_STATUS */
+#define HDMI_PHY_STATUS_READY		(1 << 0)
+
+/* HDMI_MODE_SEL */
+#define HDMI_MODE_HDMI_EN		(1 << 1)
+#define HDMI_MODE_DVI_EN		(1 << 0)
+#define HDMI_MODE_MASK			(3 << 0)
+
+/* HDMI_TG_CMD */
+#define HDMI_TG_EN			(1 << 0)
+#define HDMI_FIELD_EN			(1 << 1)
+
+#endif /* SAMSUNG_REGS_HDMI_H */
diff --git a/drivers/gpu/drm/exynos/regs-mixer.h b/drivers/gpu/drm/exynos/regs-mixer.h
new file mode 100644
index 000000000000..fd2f4d14cf6d
--- /dev/null
+++ b/drivers/gpu/drm/exynos/regs-mixer.h
@@ -0,0 +1,141 @@
+/*
+ *
+ *  Cloned from drivers/media/video/s5p-tv/regs-mixer.h
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * Mixer register header file for Samsung Mixer driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+#ifndef SAMSUNG_REGS_MIXER_H
+#define SAMSUNG_REGS_MIXER_H
+
+/*
+ * Register part
+ */
+#define MXR_STATUS			0x0000
+#define MXR_CFG				0x0004
+#define MXR_INT_EN			0x0008
+#define MXR_INT_STATUS			0x000C
+#define MXR_LAYER_CFG			0x0010
+#define MXR_VIDEO_CFG			0x0014
+#define MXR_GRAPHIC0_CFG		0x0020
+#define MXR_GRAPHIC0_BASE		0x0024
+#define MXR_GRAPHIC0_SPAN		0x0028
+#define MXR_GRAPHIC0_SXY		0x002C
+#define MXR_GRAPHIC0_WH			0x0030
+#define MXR_GRAPHIC0_DXY		0x0034
+#define MXR_GRAPHIC0_BLANK		0x0038
+#define MXR_GRAPHIC1_CFG		0x0040
+#define MXR_GRAPHIC1_BASE		0x0044
+#define MXR_GRAPHIC1_SPAN		0x0048
+#define MXR_GRAPHIC1_SXY		0x004C
+#define MXR_GRAPHIC1_WH			0x0050
+#define MXR_GRAPHIC1_DXY		0x0054
+#define MXR_GRAPHIC1_BLANK		0x0058
+#define MXR_BG_CFG			0x0060
+#define MXR_BG_COLOR0			0x0064
+#define MXR_BG_COLOR1			0x0068
+#define MXR_BG_COLOR2			0x006C
+#define MXR_CM_COEFF_Y			0x0080
+#define MXR_CM_COEFF_CB			0x0084
+#define MXR_CM_COEFF_CR			0x0088
+#define MXR_GRAPHIC0_BASE_S		0x2024
+#define MXR_GRAPHIC1_BASE_S		0x2044
+
+/* for parametrized access to layer registers */
+#define MXR_GRAPHIC_CFG(i)		(0x0020 + (i) * 0x20)
+#define MXR_GRAPHIC_BASE(i)		(0x0024 + (i) * 0x20)
+#define MXR_GRAPHIC_SPAN(i)		(0x0028 + (i) * 0x20)
+#define MXR_GRAPHIC_SXY(i)		(0x002C + (i) * 0x20)
+#define MXR_GRAPHIC_WH(i)		(0x0030 + (i) * 0x20)
+#define MXR_GRAPHIC_DXY(i)		(0x0034 + (i) * 0x20)
+#define MXR_GRAPHIC_BLANK(i)		(0x0038 + (i) * 0x20)
+#define MXR_GRAPHIC_BASE_S(i)		(0x2024 + (i) * 0x20)
+
+/*
+ * Bit definition part
+ */
+
+/* generates mask for range of bits */
+#define MXR_MASK(high_bit, low_bit) \
+	(((2 << ((high_bit) - (low_bit))) - 1) << (low_bit))
+
+#define MXR_MASK_VAL(val, high_bit, low_bit) \
+	(((val) << (low_bit)) & MXR_MASK(high_bit, low_bit))
+
+/* bits for MXR_STATUS */
+#define MXR_STATUS_16_BURST		(1 << 7)
+#define MXR_STATUS_BURST_MASK		(1 << 7)
+#define MXR_STATUS_BIG_ENDIAN		(1 << 3)
+#define MXR_STATUS_ENDIAN_MASK		(1 << 3)
+#define MXR_STATUS_SYNC_ENABLE		(1 << 2)
+#define MXR_STATUS_REG_RUN		(1 << 0)
+
+/* bits for MXR_CFG */
+#define MXR_CFG_RGB601_0_255		(0 << 9)
+#define MXR_CFG_RGB601_16_235		(1 << 9)
+#define MXR_CFG_RGB709_0_255		(2 << 9)
+#define MXR_CFG_RGB709_16_235		(3 << 9)
+#define MXR_CFG_RGB_FMT_MASK		0x600
+#define MXR_CFG_OUT_YUV444		(0 << 8)
+#define MXR_CFG_OUT_RGB888		(1 << 8)
+#define MXR_CFG_OUT_MASK		(1 << 8)
+#define MXR_CFG_DST_SDO			(0 << 7)
+#define MXR_CFG_DST_HDMI		(1 << 7)
+#define MXR_CFG_DST_MASK		(1 << 7)
+#define MXR_CFG_SCAN_HD_720		(0 << 6)
+#define MXR_CFG_SCAN_HD_1080		(1 << 6)
+#define MXR_CFG_GRP1_ENABLE		(1 << 5)
+#define MXR_CFG_GRP0_ENABLE		(1 << 4)
+#define MXR_CFG_VP_ENABLE		(1 << 3)
+#define MXR_CFG_SCAN_INTERLACE		(0 << 2)
+#define MXR_CFG_SCAN_PROGRASSIVE	(1 << 2)
+#define MXR_CFG_SCAN_NTSC		(0 << 1)
+#define MXR_CFG_SCAN_PAL		(1 << 1)
+#define MXR_CFG_SCAN_SD			(0 << 0)
+#define MXR_CFG_SCAN_HD			(1 << 0)
+#define MXR_CFG_SCAN_MASK		0x47
+
+/* bits for MXR_GRAPHICn_CFG */
+#define MXR_GRP_CFG_COLOR_KEY_DISABLE	(1 << 21)
+#define MXR_GRP_CFG_BLEND_PRE_MUL	(1 << 20)
+#define MXR_GRP_CFG_WIN_BLEND_EN	(1 << 17)
+#define MXR_GRP_CFG_PIXEL_BLEND_EN	(1 << 16)
+#define MXR_GRP_CFG_FORMAT_VAL(x)	MXR_MASK_VAL(x, 11, 8)
+#define MXR_GRP_CFG_FORMAT_MASK		MXR_GRP_CFG_FORMAT_VAL(~0)
+#define MXR_GRP_CFG_ALPHA_VAL(x)	MXR_MASK_VAL(x, 7, 0)
+
+/* bits for MXR_GRAPHICn_WH */
+#define MXR_GRP_WH_H_SCALE(x)		MXR_MASK_VAL(x, 28, 28)
+#define MXR_GRP_WH_V_SCALE(x)		MXR_MASK_VAL(x, 12, 12)
+#define MXR_GRP_WH_WIDTH(x)		MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_WH_HEIGHT(x)		MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_GRAPHICn_SXY */
+#define MXR_GRP_SXY_SX(x)		MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_SXY_SY(x)		MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_GRAPHICn_DXY */
+#define MXR_GRP_DXY_DX(x)		MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_DXY_DY(x)		MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_INT_EN */
+#define MXR_INT_EN_VSYNC		(1 << 11)
+#define MXR_INT_EN_ALL			(0x0f << 8)
+
+/* bit for MXR_INT_STATUS */
+#define MXR_INT_CLEAR_VSYNC		(1 << 11)
+#define MXR_INT_STATUS_VSYNC		(1 << 0)
+
+/* bit for MXR_LAYER_CFG */
+#define MXR_LAYER_CFG_GRP1_VAL(x)	MXR_MASK_VAL(x, 11, 8)
+#define MXR_LAYER_CFG_GRP0_VAL(x)	MXR_MASK_VAL(x, 7, 4)
+#define MXR_LAYER_CFG_VP_VAL(x)		MXR_MASK_VAL(x, 3, 0)
+
+#endif /* SAMSUNG_REGS_MIXER_H */
+
diff --git a/drivers/gpu/drm/exynos/regs-vp.h b/drivers/gpu/drm/exynos/regs-vp.h
new file mode 100644
index 000000000000..10b737af0a72
--- /dev/null
+++ b/drivers/gpu/drm/exynos/regs-vp.h
@@ -0,0 +1,91 @@
+/*
+ *
+ *  Cloned from drivers/media/video/s5p-tv/regs-vp.h
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * Video processor register header file for Samsung Mixer driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SAMSUNG_REGS_VP_H
+#define SAMSUNG_REGS_VP_H
+
+/*
+ * Register part
+ */
+
+#define VP_ENABLE			0x0000
+#define VP_SRESET			0x0004
+#define VP_SHADOW_UPDATE		0x0008
+#define VP_FIELD_ID			0x000C
+#define VP_MODE				0x0010
+#define VP_IMG_SIZE_Y			0x0014
+#define VP_IMG_SIZE_C			0x0018
+#define VP_PER_RATE_CTRL		0x001C
+#define VP_TOP_Y_PTR			0x0028
+#define VP_BOT_Y_PTR			0x002C
+#define VP_TOP_C_PTR			0x0030
+#define VP_BOT_C_PTR			0x0034
+#define VP_ENDIAN_MODE			0x03CC
+#define VP_SRC_H_POSITION		0x0044
+#define VP_SRC_V_POSITION		0x0048
+#define VP_SRC_WIDTH			0x004C
+#define VP_SRC_HEIGHT			0x0050
+#define VP_DST_H_POSITION		0x0054
+#define VP_DST_V_POSITION		0x0058
+#define VP_DST_WIDTH			0x005C
+#define VP_DST_HEIGHT			0x0060
+#define VP_H_RATIO			0x0064
+#define VP_V_RATIO			0x0068
+#define VP_POLY8_Y0_LL			0x006C
+#define VP_POLY4_Y0_LL			0x00EC
+#define VP_POLY4_C0_LL			0x012C
+
+/*
+ * Bit definition part
+ */
+
+/* generates mask for range of bits */
+
+#define VP_MASK(high_bit, low_bit) \
+	(((2 << ((high_bit) - (low_bit))) - 1) << (low_bit))
+
+#define VP_MASK_VAL(val, high_bit, low_bit) \
+	(((val) << (low_bit)) & VP_MASK(high_bit, low_bit))
+
+ /* VP_ENABLE */
+#define VP_ENABLE_ON			(1 << 0)
+
+/* VP_SRESET */
+#define VP_SRESET_PROCESSING		(1 << 0)
+
+/* VP_SHADOW_UPDATE */
+#define VP_SHADOW_UPDATE_ENABLE		(1 << 0)
+
+/* VP_MODE */
+#define VP_MODE_NV12			(0 << 6)
+#define VP_MODE_NV21			(1 << 6)
+#define VP_MODE_LINE_SKIP		(1 << 5)
+#define VP_MODE_MEM_LINEAR		(0 << 4)
+#define VP_MODE_MEM_TILED		(1 << 4)
+#define VP_MODE_FMT_MASK		(5 << 4)
+#define VP_MODE_FIELD_ID_AUTO_TOGGLING	(1 << 2)
+#define VP_MODE_2D_IPC			(1 << 1)
+
+/* VP_IMG_SIZE_Y */
+/* VP_IMG_SIZE_C */
+#define VP_IMG_HSIZE(x)			VP_MASK_VAL(x, 29, 16)
+#define VP_IMG_VSIZE(x)			VP_MASK_VAL(x, 13, 0)
+
+/* VP_SRC_H_POSITION */
+#define VP_SRC_H_POSITION_VAL(x)	VP_MASK_VAL(x, 14, 4)
+
+/* VP_ENDIAN_MODE */
+#define VP_ENDIAN_MODE_LITTLE		(1 << 0)
+
+#endif /* SAMSUNG_REGS_VP_H */
diff --git a/drivers/gpu/drm/gma500/Kconfig b/drivers/gpu/drm/gma500/Kconfig
new file mode 100644
index 000000000000..754e14bdc801
--- /dev/null
+++ b/drivers/gpu/drm/gma500/Kconfig
@@ -0,0 +1,27 @@
+config DRM_GMA500
+	tristate "Intel GMA5/600 KMS Framebuffer"
+	depends on DRM && PCI && X86 && EXPERIMENTAL
+	select FB_CFB_COPYAREA
+        select FB_CFB_FILLRECT
+        select FB_CFB_IMAGEBLIT
+        select DRM_KMS_HELPER
+        select DRM_TTM
+	help
+	  Say yes for an experimental 2D KMS framebuffer driver for the
+	  Intel GMA500 ('Poulsbo') and other Intel IMG based graphics
+	  devices.
+
+config DRM_GMA600
+	bool "Intel GMA600 support (Experimental)"
+	depends on DRM_GMA500
+	help
+	  Say yes to include support for GMA600 (Intel Moorestown/Oaktrail)
+	  platforms with LVDS ports. HDMI and MIPI are not currently
+	  supported.
+
+config DRM_GMA3600
+	bool "Intel GMA3600/3650 support (Experimental)"
+	depends on DRM_GMA500
+	help
+	  Say yes to include basic support for Intel GMA3600/3650 (Intel
+	  Cedar Trail) platforms.
diff --git a/drivers/gpu/drm/gma500/Makefile b/drivers/gpu/drm/gma500/Makefile
new file mode 100644
index 000000000000..81c103be5e21
--- /dev/null
+++ b/drivers/gpu/drm/gma500/Makefile
@@ -0,0 +1,40 @@
+#
+#	KMS driver for the GMA500
+#
+ccflags-y += -Iinclude/drm
+
+gma500_gfx-y += gem_glue.o \
+	  accel_2d.o \
+	  backlight.o \
+	  framebuffer.o \
+	  gem.o \
+	  gtt.o \
+	  intel_bios.o \
+	  intel_i2c.o \
+	  intel_gmbus.o \
+	  intel_opregion.o \
+	  mmu.o \
+	  power.o \
+	  psb_drv.o \
+	  psb_intel_display.o \
+	  psb_intel_lvds.o \
+	  psb_intel_modes.o \
+	  psb_intel_sdvo.o \
+	  psb_lid.o \
+	  psb_irq.o \
+	  psb_device.o \
+	  mid_bios.o
+
+gma500_gfx-$(CONFIG_DRM_GMA3600) +=  cdv_device.o \
+	  cdv_intel_crt.o \
+	  cdv_intel_display.o \
+	  cdv_intel_hdmi.o \
+	  cdv_intel_lvds.o
+
+gma500_gfx-$(CONFIG_DRM_GMA600) += oaktrail_device.o \
+	  oaktrail_crtc.o \
+	  oaktrail_lvds.o \
+	  oaktrail_hdmi.o \
+	  oaktrail_hdmi_i2c.o
+
+obj-$(CONFIG_DRM_GMA500) += gma500_gfx.o
diff --git a/drivers/gpu/drm/gma500/accel_2d.c b/drivers/gpu/drm/gma500/accel_2d.c
new file mode 100644
index 000000000000..d5ef1a5793c8
--- /dev/null
+++ b/drivers/gpu/drm/gma500/accel_2d.c
@@ -0,0 +1,364 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Intel funded Tungsten Graphics (http://www.tungstengraphics.com) to
+ * develop this driver.
+ *
+ **************************************************************************/
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/tty.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/console.h>
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/drm_crtc.h>
+
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "framebuffer.h"
+
+/**
+ *	psb_spank		-	reset the 2D engine
+ *	@dev_priv: our PSB DRM device
+ *
+ *	Soft reset the graphics engine and then reload the necessary registers.
+ *	We use this at initialisation time but it will become relevant for
+ *	accelerated X later
+ */
+void psb_spank(struct drm_psb_private *dev_priv)
+{
+	PSB_WSGX32(_PSB_CS_RESET_BIF_RESET | _PSB_CS_RESET_DPM_RESET |
+		_PSB_CS_RESET_TA_RESET | _PSB_CS_RESET_USE_RESET |
+		_PSB_CS_RESET_ISP_RESET | _PSB_CS_RESET_TSP_RESET |
+		_PSB_CS_RESET_TWOD_RESET, PSB_CR_SOFT_RESET);
+	PSB_RSGX32(PSB_CR_SOFT_RESET);
+
+	msleep(1);
+
+	PSB_WSGX32(0, PSB_CR_SOFT_RESET);
+	wmb();
+	PSB_WSGX32(PSB_RSGX32(PSB_CR_BIF_CTRL) | _PSB_CB_CTRL_CLEAR_FAULT,
+		   PSB_CR_BIF_CTRL);
+	wmb();
+	(void) PSB_RSGX32(PSB_CR_BIF_CTRL);
+
+	msleep(1);
+	PSB_WSGX32(PSB_RSGX32(PSB_CR_BIF_CTRL) & ~_PSB_CB_CTRL_CLEAR_FAULT,
+		   PSB_CR_BIF_CTRL);
+	(void) PSB_RSGX32(PSB_CR_BIF_CTRL);
+	PSB_WSGX32(dev_priv->gtt.gatt_start, PSB_CR_BIF_TWOD_REQ_BASE);
+}
+
+/**
+ *	psb2_2d_wait_available	-	wait for FIFO room
+ *	@dev_priv: our DRM device
+ *	@size: size (in dwords) of the command we want to issue
+ *
+ *	Wait until there is room to load the FIFO with our data. If the
+ *	device is not responding then reset it
+ */
+static int psb_2d_wait_available(struct drm_psb_private *dev_priv,
+			  unsigned size)
+{
+	uint32_t avail = PSB_RSGX32(PSB_CR_2D_SOCIF);
+	unsigned long t = jiffies + HZ;
+
+	while (avail < size) {
+		avail = PSB_RSGX32(PSB_CR_2D_SOCIF);
+		if (time_after(jiffies, t)) {
+			psb_spank(dev_priv);
+			return -EIO;
+		}
+	}
+	return 0;
+}
+
+/**
+ *	psb_2d_submit		-	submit a 2D command
+ *	@dev_priv: our DRM device
+ *	@cmdbuf: command to issue
+ *	@size: length (in dwords)
+ *
+ *	Issue one or more 2D commands to the accelerator. This needs to be
+ *	serialized later when we add the GEM interfaces for acceleration
+ */
+static int psbfb_2d_submit(struct drm_psb_private *dev_priv, uint32_t *cmdbuf,
+								unsigned size)
+{
+	int ret = 0;
+	int i;
+	unsigned submit_size;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->lock_2d, flags);
+	while (size > 0) {
+		submit_size = (size < 0x60) ? size : 0x60;
+		size -= submit_size;
+		ret = psb_2d_wait_available(dev_priv, submit_size);
+		if (ret)
+			break;
+
+		submit_size <<= 2;
+
+		for (i = 0; i < submit_size; i += 4)
+			PSB_WSGX32(*cmdbuf++, PSB_SGX_2D_SLAVE_PORT + i);
+
+		(void)PSB_RSGX32(PSB_SGX_2D_SLAVE_PORT + i - 4);
+	}
+	spin_unlock_irqrestore(&dev_priv->lock_2d, flags);
+	return ret;
+}
+
+
+/**
+ *	psb_accel_2d_copy_direction	-	compute blit order
+ *	@xdir: X direction of move
+ *	@ydir: Y direction of move
+ *
+ *	Compute the correct order setings to ensure that an overlapping blit
+ *	correctly copies all the pixels.
+ */
+static u32 psb_accel_2d_copy_direction(int xdir, int ydir)
+{
+	if (xdir < 0)
+		return (ydir < 0) ? PSB_2D_COPYORDER_BR2TL :
+						PSB_2D_COPYORDER_TR2BL;
+	else
+		return (ydir < 0) ? PSB_2D_COPYORDER_BL2TR :
+						PSB_2D_COPYORDER_TL2BR;
+}
+
+/**
+ *	psb_accel_2d_copy		-	accelerated 2D copy
+ *	@dev_priv: our DRM device
+ *	@src_offset in bytes
+ *	@src_stride in bytes
+ *	@src_format psb 2D format defines
+ *	@dst_offset in bytes
+ *	@dst_stride in bytes
+ *	@dst_format psb 2D format defines
+ *	@src_x offset in pixels
+ *	@src_y offset in pixels
+ *	@dst_x offset in pixels
+ *	@dst_y offset in pixels
+ *	@size_x of the copied area
+ *	@size_y of the copied area
+ *
+ *	Format and issue a 2D accelerated copy command.
+ */
+static int psb_accel_2d_copy(struct drm_psb_private *dev_priv,
+			     uint32_t src_offset, uint32_t src_stride,
+			     uint32_t src_format, uint32_t dst_offset,
+			     uint32_t dst_stride, uint32_t dst_format,
+			     uint16_t src_x, uint16_t src_y,
+			     uint16_t dst_x, uint16_t dst_y,
+			     uint16_t size_x, uint16_t size_y)
+{
+	uint32_t blit_cmd;
+	uint32_t buffer[10];
+	uint32_t *buf;
+	uint32_t direction;
+
+	buf = buffer;
+
+	direction =
+	    psb_accel_2d_copy_direction(src_x - dst_x, src_y - dst_y);
+
+	if (direction == PSB_2D_COPYORDER_BR2TL ||
+	    direction == PSB_2D_COPYORDER_TR2BL) {
+		src_x += size_x - 1;
+		dst_x += size_x - 1;
+	}
+	if (direction == PSB_2D_COPYORDER_BR2TL ||
+	    direction == PSB_2D_COPYORDER_BL2TR) {
+		src_y += size_y - 1;
+		dst_y += size_y - 1;
+	}
+
+	blit_cmd =
+	    PSB_2D_BLIT_BH |
+	    PSB_2D_ROT_NONE |
+	    PSB_2D_DSTCK_DISABLE |
+	    PSB_2D_SRCCK_DISABLE |
+	    PSB_2D_USE_PAT | PSB_2D_ROP3_SRCCOPY | direction;
+
+	*buf++ = PSB_2D_FENCE_BH;
+	*buf++ =
+	    PSB_2D_DST_SURF_BH | dst_format | (dst_stride <<
+					       PSB_2D_DST_STRIDE_SHIFT);
+	*buf++ = dst_offset;
+	*buf++ =
+	    PSB_2D_SRC_SURF_BH | src_format | (src_stride <<
+					       PSB_2D_SRC_STRIDE_SHIFT);
+	*buf++ = src_offset;
+	*buf++ =
+	    PSB_2D_SRC_OFF_BH | (src_x << PSB_2D_SRCOFF_XSTART_SHIFT) |
+	    (src_y << PSB_2D_SRCOFF_YSTART_SHIFT);
+	*buf++ = blit_cmd;
+	*buf++ =
+	    (dst_x << PSB_2D_DST_XSTART_SHIFT) | (dst_y <<
+						  PSB_2D_DST_YSTART_SHIFT);
+	*buf++ =
+	    (size_x << PSB_2D_DST_XSIZE_SHIFT) | (size_y <<
+						  PSB_2D_DST_YSIZE_SHIFT);
+	*buf++ = PSB_2D_FLUSH_BH;
+
+	return psbfb_2d_submit(dev_priv, buffer, buf - buffer);
+}
+
+/**
+ *	psbfb_copyarea_accel	-	copyarea acceleration for /dev/fb
+ *	@info: our framebuffer
+ *	@a: copyarea parameters from the framebuffer core
+ *
+ *	Perform a 2D copy via the accelerator
+ */
+static void psbfb_copyarea_accel(struct fb_info *info,
+				 const struct fb_copyarea *a)
+{
+	struct psb_fbdev *fbdev = info->par;
+	struct psb_framebuffer *psbfb = &fbdev->pfb;
+	struct drm_device *dev = psbfb->base.dev;
+	struct drm_framebuffer *fb = fbdev->psb_fb_helper.fb;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	uint32_t offset;
+	uint32_t stride;
+	uint32_t src_format;
+	uint32_t dst_format;
+
+	if (!fb)
+		return;
+
+	offset = psbfb->gtt->offset;
+	stride = fb->pitches[0];
+
+	switch (fb->depth) {
+	case 8:
+		src_format = PSB_2D_SRC_332RGB;
+		dst_format = PSB_2D_DST_332RGB;
+		break;
+	case 15:
+		src_format = PSB_2D_SRC_555RGB;
+		dst_format = PSB_2D_DST_555RGB;
+		break;
+	case 16:
+		src_format = PSB_2D_SRC_565RGB;
+		dst_format = PSB_2D_DST_565RGB;
+		break;
+	case 24:
+	case 32:
+		/* this is wrong but since we don't do blending its okay */
+		src_format = PSB_2D_SRC_8888ARGB;
+		dst_format = PSB_2D_DST_8888ARGB;
+		break;
+	default:
+		/* software fallback */
+		cfb_copyarea(info, a);
+		return;
+	}
+
+	if (!gma_power_begin(dev, false)) {
+		cfb_copyarea(info, a);
+		return;
+	}
+	psb_accel_2d_copy(dev_priv,
+			  offset, stride, src_format,
+			  offset, stride, dst_format,
+			  a->sx, a->sy, a->dx, a->dy, a->width, a->height);
+	gma_power_end(dev);
+}
+
+/**
+ *	psbfb_copyarea	-	2D copy interface
+ *	@info: our framebuffer
+ *	@region: region to copy
+ *
+ *	Copy an area of the framebuffer console either by the accelerator
+ *	or directly using the cfb helpers according to the request
+ */
+void psbfb_copyarea(struct fb_info *info,
+			   const struct fb_copyarea *region)
+{
+	if (unlikely(info->state != FBINFO_STATE_RUNNING))
+		return;
+
+	/* Avoid the 8 pixel erratum */
+	if (region->width == 8 || region->height == 8 ||
+		(info->flags & FBINFO_HWACCEL_DISABLED))
+		return cfb_copyarea(info, region);
+
+	psbfb_copyarea_accel(info, region);
+}
+
+/**
+ *	psbfb_sync	-	synchronize 2D
+ *	@info: our framebuffer
+ *
+ *	Wait for the 2D engine to quiesce so that we can do CPU
+ *	access to the framebuffer again
+ */
+int psbfb_sync(struct fb_info *info)
+{
+	struct psb_fbdev *fbdev = info->par;
+	struct psb_framebuffer *psbfb = &fbdev->pfb;
+	struct drm_device *dev = psbfb->base.dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned long _end = jiffies + DRM_HZ;
+	int busy = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->lock_2d, flags);
+	/*
+	 * First idle the 2D engine.
+	 */
+
+	if ((PSB_RSGX32(PSB_CR_2D_SOCIF) == _PSB_C2_SOCIF_EMPTY) &&
+	    ((PSB_RSGX32(PSB_CR_2D_BLIT_STATUS) & _PSB_C2B_STATUS_BUSY) == 0))
+		goto out;
+
+	do {
+		busy = (PSB_RSGX32(PSB_CR_2D_SOCIF) != _PSB_C2_SOCIF_EMPTY);
+		cpu_relax();
+	} while (busy && !time_after_eq(jiffies, _end));
+
+	if (busy)
+		busy = (PSB_RSGX32(PSB_CR_2D_SOCIF) != _PSB_C2_SOCIF_EMPTY);
+	if (busy)
+		goto out;
+
+	do {
+		busy = ((PSB_RSGX32(PSB_CR_2D_BLIT_STATUS) &
+						_PSB_C2B_STATUS_BUSY) != 0);
+		cpu_relax();
+	} while (busy && !time_after_eq(jiffies, _end));
+	if (busy)
+		busy = ((PSB_RSGX32(PSB_CR_2D_BLIT_STATUS) &
+					_PSB_C2B_STATUS_BUSY) != 0);
+
+out:
+	spin_unlock_irqrestore(&dev_priv->lock_2d, flags);
+	return (busy) ? -EBUSY : 0;
+}
diff --git a/drivers/gpu/drm/gma500/backlight.c b/drivers/gpu/drm/gma500/backlight.c
new file mode 100644
index 000000000000..20793951fcac
--- /dev/null
+++ b/drivers/gpu/drm/gma500/backlight.c
@@ -0,0 +1,49 @@
+/*
+ * GMA500 Backlight Interface
+ *
+ * Copyright (c) 2009-2011, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors: Eric Knopp
+ *
+ */
+
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_drv.h"
+#include "intel_bios.h"
+#include "power.h"
+
+int gma_backlight_init(struct drm_device *dev)
+{
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	return dev_priv->ops->backlight_init(dev);
+#else
+	return 0;
+#endif
+}
+
+void gma_backlight_exit(struct drm_device *dev)
+{
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	if (dev_priv->backlight_device) {
+		dev_priv->backlight_device->props.brightness = 0;
+		backlight_update_status(dev_priv->backlight_device);
+		backlight_device_unregister(dev_priv->backlight_device);
+	}
+#endif
+}
diff --git a/drivers/gpu/drm/gma500/cdv_device.c b/drivers/gpu/drm/gma500/cdv_device.c
new file mode 100644
index 000000000000..4a5b099c3bc5
--- /dev/null
+++ b/drivers/gpu/drm/gma500/cdv_device.c
@@ -0,0 +1,351 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+#include <linux/backlight.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "gma_drm.h"
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "intel_bios.h"
+#include "cdv_device.h"
+
+#define VGA_SR_INDEX		0x3c4
+#define VGA_SR_DATA		0x3c5
+
+static void cdv_disable_vga(struct drm_device *dev)
+{
+	u8 sr1;
+	u32 vga_reg;
+
+	vga_reg = VGACNTRL;
+
+	outb(1, VGA_SR_INDEX);
+	sr1 = inb(VGA_SR_DATA);
+	outb(sr1 | 1<<5, VGA_SR_DATA);
+	udelay(300);
+
+	REG_WRITE(vga_reg, VGA_DISP_DISABLE);
+	REG_READ(vga_reg);
+}
+
+static int cdv_output_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	cdv_disable_vga(dev);
+
+	cdv_intel_crt_init(dev, &dev_priv->mode_dev);
+	cdv_intel_lvds_init(dev, &dev_priv->mode_dev);
+
+	/* These bits indicate HDMI not SDVO on CDV, but we don't yet support
+	   the HDMI interface */
+	if (REG_READ(SDVOB) & SDVO_DETECTED)
+		cdv_hdmi_init(dev, &dev_priv->mode_dev, SDVOB);
+	if (REG_READ(SDVOC) & SDVO_DETECTED)
+		cdv_hdmi_init(dev, &dev_priv->mode_dev, SDVOC);
+	return 0;
+}
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+
+/*
+ *	Poulsbo Backlight Interfaces
+ */
+
+#define BLC_PWM_PRECISION_FACTOR 100	/* 10000000 */
+#define BLC_PWM_FREQ_CALC_CONSTANT 32
+#define MHz 1000000
+
+#define PSB_BLC_PWM_PRECISION_FACTOR    10
+#define PSB_BLC_MAX_PWM_REG_FREQ        0xFFFE
+#define PSB_BLC_MIN_PWM_REG_FREQ        0x2
+
+#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
+#define PSB_BACKLIGHT_PWM_CTL_SHIFT	(16)
+
+static int cdv_brightness;
+static struct backlight_device *cdv_backlight_device;
+
+static int cdv_get_brightness(struct backlight_device *bd)
+{
+	/* return locally cached var instead of HW read (due to DPST etc.) */
+	/* FIXME: ideally return actual value in case firmware fiddled with
+	   it */
+	return cdv_brightness;
+}
+
+
+static int cdv_backlight_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned long core_clock;
+	/* u32 bl_max_freq; */
+	/* unsigned long value; */
+	u16 bl_max_freq;
+	uint32_t value;
+	uint32_t blc_pwm_precision_factor;
+
+	/* get bl_max_freq and pol from dev_priv*/
+	if (!dev_priv->lvds_bl) {
+		dev_err(dev->dev, "Has no valid LVDS backlight info\n");
+		return -ENOENT;
+	}
+	bl_max_freq = dev_priv->lvds_bl->freq;
+	blc_pwm_precision_factor = PSB_BLC_PWM_PRECISION_FACTOR;
+
+	core_clock = dev_priv->core_freq;
+
+	value = (core_clock * MHz) / BLC_PWM_FREQ_CALC_CONSTANT;
+	value *= blc_pwm_precision_factor;
+	value /= bl_max_freq;
+	value /= blc_pwm_precision_factor;
+
+	if (value > (unsigned long long)PSB_BLC_MAX_PWM_REG_FREQ ||
+		 value < (unsigned long long)PSB_BLC_MIN_PWM_REG_FREQ)
+				return -ERANGE;
+	else {
+		/* FIXME */
+	}
+	return 0;
+}
+
+static int cdv_set_brightness(struct backlight_device *bd)
+{
+	int level = bd->props.brightness;
+
+	/* Percentage 1-100% being valid */
+	if (level < 1)
+		level = 1;
+
+	/*cdv_intel_lvds_set_brightness(dev, level); FIXME */
+	cdv_brightness = level;
+	return 0;
+}
+
+static const struct backlight_ops cdv_ops = {
+	.get_brightness = cdv_get_brightness,
+	.update_status  = cdv_set_brightness,
+};
+
+static int cdv_backlight_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int ret;
+	struct backlight_properties props;
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.max_brightness = 100;
+	props.type = BACKLIGHT_PLATFORM;
+
+	cdv_backlight_device = backlight_device_register("psb-bl",
+					NULL, (void *)dev, &cdv_ops, &props);
+	if (IS_ERR(cdv_backlight_device))
+		return PTR_ERR(cdv_backlight_device);
+
+	ret = cdv_backlight_setup(dev);
+	if (ret < 0) {
+		backlight_device_unregister(cdv_backlight_device);
+		cdv_backlight_device = NULL;
+		return ret;
+	}
+	cdv_backlight_device->props.brightness = 100;
+	cdv_backlight_device->props.max_brightness = 100;
+	backlight_update_status(cdv_backlight_device);
+	dev_priv->backlight_device = cdv_backlight_device;
+	return 0;
+}
+
+#endif
+
+/*
+ *	Provide the Cedarview specific chip logic and low level methods
+ *	for power management
+ *
+ *	FIXME: we need to implement the apm/ospm base management bits
+ *	for this and the MID devices.
+ */
+
+static inline u32 CDV_MSG_READ32(uint port, uint offset)
+{
+	int mcr = (0x10<<24) | (port << 16) | (offset << 8);
+	uint32_t ret_val = 0;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	pci_write_config_dword(pci_root, 0xD0, mcr);
+	pci_read_config_dword(pci_root, 0xD4, &ret_val);
+	pci_dev_put(pci_root);
+	return ret_val;
+}
+
+static inline void CDV_MSG_WRITE32(uint port, uint offset, u32 value)
+{
+	int mcr = (0x11<<24) | (port << 16) | (offset << 8) | 0xF0;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	pci_write_config_dword(pci_root, 0xD4, value);
+	pci_write_config_dword(pci_root, 0xD0, mcr);
+	pci_dev_put(pci_root);
+}
+
+#define PSB_APM_CMD			0x0
+#define PSB_APM_STS			0x04
+#define PSB_PM_SSC			0x20
+#define PSB_PM_SSS			0x30
+#define PSB_PWRGT_GFX_MASK		0x3
+#define CDV_PWRGT_DISPLAY_CNTR		0x000fc00c
+#define CDV_PWRGT_DISPLAY_STS		0x000fc00c
+
+static void cdv_init_pm(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 pwr_cnt;
+	int i;
+
+	dev_priv->apm_base = CDV_MSG_READ32(PSB_PUNIT_PORT,
+							PSB_APMBA) & 0xFFFF;
+	dev_priv->ospm_base = CDV_MSG_READ32(PSB_PUNIT_PORT,
+							PSB_OSPMBA) & 0xFFFF;
+
+	/* Force power on for now */
+	pwr_cnt = inl(dev_priv->apm_base + PSB_APM_CMD);
+	pwr_cnt &= ~PSB_PWRGT_GFX_MASK;
+
+	outl(pwr_cnt, dev_priv->apm_base + PSB_APM_CMD);
+	for (i = 0; i < 5; i++) {
+		u32 pwr_sts = inl(dev_priv->apm_base + PSB_APM_STS);
+		if ((pwr_sts & PSB_PWRGT_GFX_MASK) == 0)
+			break;
+		udelay(10);
+	}
+	pwr_cnt = inl(dev_priv->ospm_base + PSB_PM_SSC);
+	pwr_cnt &= ~CDV_PWRGT_DISPLAY_CNTR;
+	outl(pwr_cnt, dev_priv->ospm_base + PSB_PM_SSC);
+	for (i = 0; i < 5; i++) {
+		u32 pwr_sts = inl(dev_priv->ospm_base + PSB_PM_SSS);
+		if ((pwr_sts & CDV_PWRGT_DISPLAY_STS) == 0)
+			break;
+		udelay(10);
+	}
+}
+
+/**
+ *	cdv_save_display_registers	-	save registers lost on suspend
+ *	@dev: our DRM device
+ *
+ *	Save the state we need in order to be able to restore the interface
+ *	upon resume from suspend
+ *
+ *	FIXME: review
+ */
+static int cdv_save_display_registers(struct drm_device *dev)
+{
+	return 0;
+}
+
+/**
+ *	cdv_restore_display_registers	-	restore lost register state
+ *	@dev: our DRM device
+ *
+ *	Restore register state that was lost during suspend and resume.
+ *
+ *	FIXME: review
+ */
+static int cdv_restore_display_registers(struct drm_device *dev)
+{
+	return 0;
+}
+
+static int cdv_power_down(struct drm_device *dev)
+{
+	return 0;
+}
+
+static int cdv_power_up(struct drm_device *dev)
+{
+	return 0;
+}
+
+/* FIXME ? - shared with Poulsbo */
+static void cdv_get_core_freq(struct drm_device *dev)
+{
+	uint32_t clock;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	pci_write_config_dword(pci_root, 0xD0, 0xD0050300);
+	pci_read_config_dword(pci_root, 0xD4, &clock);
+	pci_dev_put(pci_root);
+
+	switch (clock & 0x07) {
+	case 0:
+		dev_priv->core_freq = 100;
+		break;
+	case 1:
+		dev_priv->core_freq = 133;
+		break;
+	case 2:
+		dev_priv->core_freq = 150;
+		break;
+	case 3:
+		dev_priv->core_freq = 178;
+		break;
+	case 4:
+		dev_priv->core_freq = 200;
+		break;
+	case 5:
+	case 6:
+	case 7:
+		dev_priv->core_freq = 266;
+	default:
+		dev_priv->core_freq = 0;
+	}
+}
+
+static int cdv_chip_setup(struct drm_device *dev)
+{
+	cdv_get_core_freq(dev);
+	gma_intel_opregion_init(dev);
+	psb_intel_init_bios(dev);
+	return 0;
+}
+
+/* CDV is much like Poulsbo but has MID like SGX offsets and PM */
+
+const struct psb_ops cdv_chip_ops = {
+	.name = "GMA3600/3650",
+	.accel_2d = 0,
+	.pipes = 2,
+	.crtcs = 2,
+	.sgx_offset = MRST_SGX_OFFSET,
+	.chip_setup = cdv_chip_setup,
+
+	.crtc_helper = &cdv_intel_helper_funcs,
+	.crtc_funcs = &cdv_intel_crtc_funcs,
+
+	.output_init = cdv_output_init,
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+	.backlight_init = cdv_backlight_init,
+#endif
+
+	.init_pm = cdv_init_pm,
+	.save_regs = cdv_save_display_registers,
+	.restore_regs = cdv_restore_display_registers,
+	.power_down = cdv_power_down,
+	.power_up = cdv_power_up,
+};
diff --git a/drivers/gpu/drm/gma500/cdv_device.h b/drivers/gpu/drm/gma500/cdv_device.h
new file mode 100644
index 000000000000..2a88b7beb551
--- /dev/null
+++ b/drivers/gpu/drm/gma500/cdv_device.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright © 2011 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc., 
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+extern const struct drm_crtc_helper_funcs cdv_intel_helper_funcs;
+extern const struct drm_crtc_funcs cdv_intel_crtc_funcs;
+extern void cdv_intel_crt_init(struct drm_device *dev,
+			struct psb_intel_mode_device *mode_dev);
+extern void cdv_intel_lvds_init(struct drm_device *dev,
+			struct psb_intel_mode_device *mode_dev);
+extern void cdv_hdmi_init(struct drm_device *dev, struct psb_intel_mode_device *mode_dev,
+			int reg);
+extern struct drm_display_mode *cdv_intel_crtc_mode_get(struct drm_device *dev,
+					     struct drm_crtc *crtc);
+
+extern inline void cdv_intel_wait_for_vblank(struct drm_device *dev)
+{
+	/* Wait for 20ms, i.e. one cycle at 50hz. */
+        /* FIXME: msleep ?? */
+	mdelay(20);
+}
+
+
diff --git a/drivers/gpu/drm/gma500/cdv_intel_crt.c b/drivers/gpu/drm/gma500/cdv_intel_crt.c
new file mode 100644
index 000000000000..6d0f10b7569c
--- /dev/null
+++ b/drivers/gpu/drm/gma500/cdv_intel_crt.c
@@ -0,0 +1,333 @@
+/*
+ * Copyright © 2006-2007 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ */
+
+#include <linux/i2c.h>
+#include <drm/drmP.h>
+
+#include "intel_bios.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include <linux/pm_runtime.h>
+
+
+static void cdv_intel_crt_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	u32 temp, reg;
+	reg = ADPA;
+
+	temp = REG_READ(reg);
+	temp &= ~(ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE);
+	temp &= ~ADPA_DAC_ENABLE;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		temp |= ADPA_DAC_ENABLE;
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+		temp |= ADPA_DAC_ENABLE | ADPA_HSYNC_CNTL_DISABLE;
+		break;
+	case DRM_MODE_DPMS_SUSPEND:
+		temp |= ADPA_DAC_ENABLE | ADPA_VSYNC_CNTL_DISABLE;
+		break;
+	case DRM_MODE_DPMS_OFF:
+		temp |= ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE;
+		break;
+	}
+
+	REG_WRITE(reg, temp);
+}
+
+static int cdv_intel_crt_mode_valid(struct drm_connector *connector,
+				struct drm_display_mode *mode)
+{
+	int max_clock = 0;
+	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+		return MODE_NO_DBLESCAN;
+
+	/* The lowest clock for CDV is 20000KHz */
+	if (mode->clock < 20000)
+		return MODE_CLOCK_LOW;
+
+	/* The max clock for CDV is 355 instead of 400 */
+	max_clock = 355000;
+	if (mode->clock > max_clock)
+		return MODE_CLOCK_HIGH;
+
+	if (mode->hdisplay > 1680 || mode->vdisplay > 1050)
+		return MODE_PANEL;
+
+	return MODE_OK;
+}
+
+static bool cdv_intel_crt_mode_fixup(struct drm_encoder *encoder,
+				 struct drm_display_mode *mode,
+				 struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void cdv_intel_crt_mode_set(struct drm_encoder *encoder,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode)
+{
+
+	struct drm_device *dev = encoder->dev;
+	struct drm_crtc *crtc = encoder->crtc;
+	struct psb_intel_crtc *psb_intel_crtc =
+					to_psb_intel_crtc(crtc);
+	int dpll_md_reg;
+	u32 adpa, dpll_md;
+	u32 adpa_reg;
+
+	if (psb_intel_crtc->pipe == 0)
+		dpll_md_reg = DPLL_A_MD;
+	else
+		dpll_md_reg = DPLL_B_MD;
+
+	adpa_reg = ADPA;
+
+	/*
+	 * Disable separate mode multiplier used when cloning SDVO to CRT
+	 * XXX this needs to be adjusted when we really are cloning
+	 */
+	{
+		dpll_md = REG_READ(dpll_md_reg);
+		REG_WRITE(dpll_md_reg,
+			   dpll_md & ~DPLL_MD_UDI_MULTIPLIER_MASK);
+	}
+
+	adpa = 0;
+	if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
+		adpa |= ADPA_HSYNC_ACTIVE_HIGH;
+	if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
+		adpa |= ADPA_VSYNC_ACTIVE_HIGH;
+
+	if (psb_intel_crtc->pipe == 0)
+		adpa |= ADPA_PIPE_A_SELECT;
+	else
+		adpa |= ADPA_PIPE_B_SELECT;
+
+	REG_WRITE(adpa_reg, adpa);
+}
+
+
+/**
+ * Uses CRT_HOTPLUG_EN and CRT_HOTPLUG_STAT to detect CRT presence.
+ *
+ * \return true if CRT is connected.
+ * \return false if CRT is disconnected.
+ */
+static bool cdv_intel_crt_detect_hotplug(struct drm_connector *connector,
+								bool force)
+{
+	struct drm_device *dev = connector->dev;
+	u32 hotplug_en;
+	int i, tries = 0, ret = false;
+	u32 adpa_orig;
+
+	/* disable the DAC when doing the hotplug detection */
+
+	adpa_orig = REG_READ(ADPA);
+
+	REG_WRITE(ADPA, adpa_orig & ~(ADPA_DAC_ENABLE));
+
+	/*
+	 * On a CDV thep, CRT detect sequence need to be done twice
+	 * to get a reliable result.
+	 */
+	tries = 2;
+
+	hotplug_en = REG_READ(PORT_HOTPLUG_EN);
+	hotplug_en &= ~(CRT_HOTPLUG_DETECT_MASK);
+	hotplug_en |= CRT_HOTPLUG_FORCE_DETECT;
+
+	hotplug_en |= CRT_HOTPLUG_ACTIVATION_PERIOD_64;
+	hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
+
+	for (i = 0; i < tries ; i++) {
+		unsigned long timeout;
+		/* turn on the FORCE_DETECT */
+		REG_WRITE(PORT_HOTPLUG_EN, hotplug_en);
+		timeout = jiffies + msecs_to_jiffies(1000);
+		/* wait for FORCE_DETECT to go off */
+		do {
+			if (!(REG_READ(PORT_HOTPLUG_EN) &
+					CRT_HOTPLUG_FORCE_DETECT))
+				break;
+			msleep(1);
+		} while (time_after(timeout, jiffies));
+	}
+
+	if ((REG_READ(PORT_HOTPLUG_STAT) & CRT_HOTPLUG_MONITOR_MASK) !=
+	    CRT_HOTPLUG_MONITOR_NONE)
+		ret = true;
+
+	/* Restore the saved ADPA */
+	REG_WRITE(ADPA, adpa_orig);
+	return ret;
+}
+
+static enum drm_connector_status cdv_intel_crt_detect(
+				struct drm_connector *connector, bool force)
+{
+	if (cdv_intel_crt_detect_hotplug(connector, force))
+		return connector_status_connected;
+	else
+		return connector_status_disconnected;
+}
+
+static void cdv_intel_crt_destroy(struct drm_connector *connector)
+{
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+
+	psb_intel_i2c_destroy(psb_intel_encoder->ddc_bus);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+static int cdv_intel_crt_get_modes(struct drm_connector *connector)
+{
+	struct psb_intel_encoder *psb_intel_encoder =
+				psb_intel_attached_encoder(connector);
+	return psb_intel_ddc_get_modes(connector, &psb_intel_encoder->ddc_bus->adapter);
+}
+
+static int cdv_intel_crt_set_property(struct drm_connector *connector,
+				  struct drm_property *property,
+				  uint64_t value)
+{
+	return 0;
+}
+
+/*
+ * Routines for controlling stuff on the analog port
+ */
+
+static const struct drm_encoder_helper_funcs cdv_intel_crt_helper_funcs = {
+	.dpms = cdv_intel_crt_dpms,
+	.mode_fixup = cdv_intel_crt_mode_fixup,
+	.prepare = psb_intel_encoder_prepare,
+	.commit = psb_intel_encoder_commit,
+	.mode_set = cdv_intel_crt_mode_set,
+};
+
+static const struct drm_connector_funcs cdv_intel_crt_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = cdv_intel_crt_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = cdv_intel_crt_destroy,
+	.set_property = cdv_intel_crt_set_property,
+};
+
+static const struct drm_connector_helper_funcs
+				cdv_intel_crt_connector_helper_funcs = {
+	.mode_valid = cdv_intel_crt_mode_valid,
+	.get_modes = cdv_intel_crt_get_modes,
+	.best_encoder = psb_intel_best_encoder,
+};
+
+static void cdv_intel_crt_enc_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs cdv_intel_crt_enc_funcs = {
+	.destroy = cdv_intel_crt_enc_destroy,
+};
+
+void cdv_intel_crt_init(struct drm_device *dev,
+			struct psb_intel_mode_device *mode_dev)
+{
+
+	struct psb_intel_connector *psb_intel_connector;
+	struct psb_intel_encoder *psb_intel_encoder;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+
+	u32 i2c_reg;
+
+	psb_intel_encoder = kzalloc(sizeof(struct psb_intel_encoder), GFP_KERNEL);
+	if (!psb_intel_encoder)
+		return;
+
+	psb_intel_connector = kzalloc(sizeof(struct psb_intel_connector), GFP_KERNEL);
+	if (!psb_intel_connector)
+		goto failed_connector;
+
+	connector = &psb_intel_connector->base;
+	drm_connector_init(dev, connector,
+		&cdv_intel_crt_connector_funcs, DRM_MODE_CONNECTOR_VGA);
+
+	encoder = &psb_intel_encoder->base;
+	drm_encoder_init(dev, encoder,
+		&cdv_intel_crt_enc_funcs, DRM_MODE_ENCODER_DAC);
+
+	psb_intel_connector_attach_encoder(psb_intel_connector,
+					   psb_intel_encoder);
+
+	/* Set up the DDC bus. */
+	i2c_reg = GPIOA;
+	/* Remove the following code for CDV */
+	/*
+	if (dev_priv->crt_ddc_bus != 0)
+		i2c_reg = dev_priv->crt_ddc_bus;
+	}*/
+	psb_intel_encoder->ddc_bus = psb_intel_i2c_create(dev,
+							  i2c_reg, "CRTDDC_A");
+	if (!psb_intel_encoder->ddc_bus) {
+		dev_printk(KERN_ERR, &dev->pdev->dev, "DDC bus registration "
+			   "failed.\n");
+		goto failed_ddc;
+	}
+
+	psb_intel_encoder->type = INTEL_OUTPUT_ANALOG;
+	/*
+	psb_intel_output->clone_mask = (1 << INTEL_ANALOG_CLONE_BIT);
+	psb_intel_output->crtc_mask = (1 << 0) | (1 << 1);
+	*/
+	connector->interlace_allowed = 0;
+	connector->doublescan_allowed = 0;
+
+	drm_encoder_helper_add(encoder, &cdv_intel_crt_helper_funcs);
+	drm_connector_helper_add(connector,
+					&cdv_intel_crt_connector_helper_funcs);
+
+	drm_sysfs_connector_add(connector);
+
+	return;
+failed_ddc:
+	drm_encoder_cleanup(&psb_intel_encoder->base);
+	drm_connector_cleanup(&psb_intel_connector->base);
+	kfree(psb_intel_connector);
+failed_connector:
+	kfree(psb_intel_encoder);
+	return;
+}
diff --git a/drivers/gpu/drm/gma500/cdv_intel_display.c b/drivers/gpu/drm/gma500/cdv_intel_display.c
new file mode 100644
index 000000000000..18d11525095e
--- /dev/null
+++ b/drivers/gpu/drm/gma500/cdv_intel_display.c
@@ -0,0 +1,1508 @@
+/*
+ * Copyright © 2006-2011 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ */
+
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+
+#include <drm/drmP.h>
+#include "framebuffer.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_display.h"
+#include "power.h"
+#include "cdv_device.h"
+
+
+struct cdv_intel_range_t {
+	int min, max;
+};
+
+struct cdv_intel_p2_t {
+	int dot_limit;
+	int p2_slow, p2_fast;
+};
+
+struct cdv_intel_clock_t {
+	/* given values */
+	int n;
+	int m1, m2;
+	int p1, p2;
+	/* derived values */
+	int dot;
+	int vco;
+	int m;
+	int p;
+};
+
+#define INTEL_P2_NUM		      2
+
+struct cdv_intel_limit_t {
+	struct cdv_intel_range_t dot, vco, n, m, m1, m2, p, p1;
+	struct cdv_intel_p2_t p2;
+};
+
+#define CDV_LIMIT_SINGLE_LVDS_96	0
+#define CDV_LIMIT_SINGLE_LVDS_100	1
+#define CDV_LIMIT_DAC_HDMI_27		2
+#define CDV_LIMIT_DAC_HDMI_96		3
+
+static const struct cdv_intel_limit_t cdv_intel_limits[] = {
+	{			/* CDV_SIGNLE_LVDS_96MHz */
+	 .dot = {.min = 20000, .max = 115500},
+	 .vco = {.min = 1800000, .max = 3600000},
+	 .n = {.min = 2, .max = 6},
+	 .m = {.min = 60, .max = 160},
+	 .m1 = {.min = 0, .max = 0},
+	 .m2 = {.min = 58, .max = 158},
+	 .p = {.min = 28, .max = 140},
+	 .p1 = {.min = 2, .max = 10},
+	 .p2 = {.dot_limit = 200000,
+		.p2_slow = 14, .p2_fast = 14},
+	 },
+	{			/* CDV_SINGLE_LVDS_100MHz */
+	 .dot = {.min = 20000, .max = 115500},
+	 .vco = {.min = 1800000, .max = 3600000},
+	 .n = {.min = 2, .max = 6},
+	 .m = {.min = 60, .max = 160},
+	 .m1 = {.min = 0, .max = 0},
+	 .m2 = {.min = 58, .max = 158},
+	 .p = {.min = 28, .max = 140},
+	 .p1 = {.min = 2, .max = 10},
+	 /* The single-channel range is 25-112Mhz, and dual-channel
+	  * is 80-224Mhz.  Prefer single channel as much as possible.
+	  */
+	 .p2 = {.dot_limit = 200000, .p2_slow = 14, .p2_fast = 14},
+	 },
+	{			/* CDV_DAC_HDMI_27MHz */
+	 .dot = {.min = 20000, .max = 400000},
+	 .vco = {.min = 1809000, .max = 3564000},
+	 .n = {.min = 1, .max = 1},
+	 .m = {.min = 67, .max = 132},
+	 .m1 = {.min = 0, .max = 0},
+	 .m2 = {.min = 65, .max = 130},
+	 .p = {.min = 5, .max = 90},
+	 .p1 = {.min = 1, .max = 9},
+	 .p2 = {.dot_limit = 225000, .p2_slow = 10, .p2_fast = 5},
+	 },
+	{			/* CDV_DAC_HDMI_96MHz */
+	 .dot = {.min = 20000, .max = 400000},
+	 .vco = {.min = 1800000, .max = 3600000},
+	 .n = {.min = 2, .max = 6},
+	 .m = {.min = 60, .max = 160},
+	 .m1 = {.min = 0, .max = 0},
+	 .m2 = {.min = 58, .max = 158},
+	 .p = {.min = 5, .max = 100},
+	 .p1 = {.min = 1, .max = 10},
+	 .p2 = {.dot_limit = 225000, .p2_slow = 10, .p2_fast = 5},
+	 },
+};
+
+#define _wait_for(COND, MS, W) ({ \
+	unsigned long timeout__ = jiffies + msecs_to_jiffies(MS);	\
+	int ret__ = 0;							\
+	while (!(COND)) {						\
+		if (time_after(jiffies, timeout__)) {			\
+			ret__ = -ETIMEDOUT;				\
+			break;						\
+		}							\
+		if (W && !in_dbg_master())				\
+			msleep(W);					\
+	}								\
+	ret__;								\
+})
+
+#define wait_for(COND, MS) _wait_for(COND, MS, 1)
+
+
+static int cdv_sb_read(struct drm_device *dev, u32 reg, u32 *val)
+{
+	int ret;
+
+	ret = wait_for((REG_READ(SB_PCKT) & SB_BUSY) == 0, 1000);
+	if (ret) {
+		DRM_ERROR("timeout waiting for SB to idle before read\n");
+		return ret;
+	}
+
+	REG_WRITE(SB_ADDR, reg);
+	REG_WRITE(SB_PCKT,
+		   SET_FIELD(SB_OPCODE_READ, SB_OPCODE) |
+		   SET_FIELD(SB_DEST_DPLL, SB_DEST) |
+		   SET_FIELD(0xf, SB_BYTE_ENABLE));
+
+	ret = wait_for((REG_READ(SB_PCKT) & SB_BUSY) == 0, 1000);
+	if (ret) {
+		DRM_ERROR("timeout waiting for SB to idle after read\n");
+		return ret;
+	}
+
+	*val = REG_READ(SB_DATA);
+
+	return 0;
+}
+
+static int cdv_sb_write(struct drm_device *dev, u32 reg, u32 val)
+{
+	int ret;
+	static bool dpio_debug = true;
+	u32 temp;
+
+	if (dpio_debug) {
+		if (cdv_sb_read(dev, reg, &temp) == 0)
+			DRM_DEBUG_KMS("0x%08x: 0x%08x (before)\n", reg, temp);
+		DRM_DEBUG_KMS("0x%08x: 0x%08x\n", reg, val);
+	}
+
+	ret = wait_for((REG_READ(SB_PCKT) & SB_BUSY) == 0, 1000);
+	if (ret) {
+		DRM_ERROR("timeout waiting for SB to idle before write\n");
+		return ret;
+	}
+
+	REG_WRITE(SB_ADDR, reg);
+	REG_WRITE(SB_DATA, val);
+	REG_WRITE(SB_PCKT,
+		   SET_FIELD(SB_OPCODE_WRITE, SB_OPCODE) |
+		   SET_FIELD(SB_DEST_DPLL, SB_DEST) |
+		   SET_FIELD(0xf, SB_BYTE_ENABLE));
+
+	ret = wait_for((REG_READ(SB_PCKT) & SB_BUSY) == 0, 1000);
+	if (ret) {
+		DRM_ERROR("timeout waiting for SB to idle after write\n");
+		return ret;
+	}
+
+	if (dpio_debug) {
+		if (cdv_sb_read(dev, reg, &temp) == 0)
+			DRM_DEBUG_KMS("0x%08x: 0x%08x (after)\n", reg, temp);
+	}
+
+	return 0;
+}
+
+/* Reset the DPIO configuration register.  The BIOS does this at every
+ * mode set.
+ */
+static void cdv_sb_reset(struct drm_device *dev)
+{
+
+	REG_WRITE(DPIO_CFG, 0);
+	REG_READ(DPIO_CFG);
+	REG_WRITE(DPIO_CFG, DPIO_MODE_SELECT_0 | DPIO_CMN_RESET_N);
+}
+
+/* Unlike most Intel display engines, on Cedarview the DPLL registers
+ * are behind this sideband bus.  They must be programmed while the
+ * DPLL reference clock is on in the DPLL control register, but before
+ * the DPLL is enabled in the DPLL control register.
+ */
+static int
+cdv_dpll_set_clock_cdv(struct drm_device *dev, struct drm_crtc *crtc,
+			       struct cdv_intel_clock_t *clock)
+{
+	struct psb_intel_crtc *psb_crtc =
+				to_psb_intel_crtc(crtc);
+	int pipe = psb_crtc->pipe;
+	u32 m, n_vco, p;
+	int ret = 0;
+	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+	u32 ref_value;
+
+	cdv_sb_reset(dev);
+
+	if ((REG_READ(dpll_reg) & DPLL_SYNCLOCK_ENABLE) == 0) {
+		DRM_ERROR("Attempting to set DPLL with refclk disabled\n");
+		return -EBUSY;
+	}
+
+	/* Follow the BIOS and write the REF/SFR Register. Hardcoded value */
+	ref_value = 0x68A701;
+
+	cdv_sb_write(dev, SB_REF_SFR(pipe), ref_value);
+
+	/* We don't know what the other fields of these regs are, so
+	 * leave them in place.
+	 */
+	ret = cdv_sb_read(dev, SB_M(pipe), &m);
+	if (ret)
+		return ret;
+	m &= ~SB_M_DIVIDER_MASK;
+	m |= ((clock->m2) << SB_M_DIVIDER_SHIFT);
+	ret = cdv_sb_write(dev, SB_M(pipe), m);
+	if (ret)
+		return ret;
+
+	ret = cdv_sb_read(dev, SB_N_VCO(pipe), &n_vco);
+	if (ret)
+		return ret;
+
+	/* Follow the BIOS to program the N_DIVIDER REG */
+	n_vco &= 0xFFFF;
+	n_vco |= 0x107;
+	n_vco &= ~(SB_N_VCO_SEL_MASK |
+		   SB_N_DIVIDER_MASK |
+		   SB_N_CB_TUNE_MASK);
+
+	n_vco |= ((clock->n) << SB_N_DIVIDER_SHIFT);
+
+	if (clock->vco < 2250000) {
+		n_vco |= (2 << SB_N_CB_TUNE_SHIFT);
+		n_vco |= (0 << SB_N_VCO_SEL_SHIFT);
+	} else if (clock->vco < 2750000) {
+		n_vco |= (1 << SB_N_CB_TUNE_SHIFT);
+		n_vco |= (1 << SB_N_VCO_SEL_SHIFT);
+	} else if (clock->vco < 3300000) {
+		n_vco |= (0 << SB_N_CB_TUNE_SHIFT);
+		n_vco |= (2 << SB_N_VCO_SEL_SHIFT);
+	} else {
+		n_vco |= (0 << SB_N_CB_TUNE_SHIFT);
+		n_vco |= (3 << SB_N_VCO_SEL_SHIFT);
+	}
+
+	ret = cdv_sb_write(dev, SB_N_VCO(pipe), n_vco);
+	if (ret)
+		return ret;
+
+	ret = cdv_sb_read(dev, SB_P(pipe), &p);
+	if (ret)
+		return ret;
+	p &= ~(SB_P2_DIVIDER_MASK | SB_P1_DIVIDER_MASK);
+	p |= SET_FIELD(clock->p1, SB_P1_DIVIDER);
+	switch (clock->p2) {
+	case 5:
+		p |= SET_FIELD(SB_P2_5, SB_P2_DIVIDER);
+		break;
+	case 10:
+		p |= SET_FIELD(SB_P2_10, SB_P2_DIVIDER);
+		break;
+	case 14:
+		p |= SET_FIELD(SB_P2_14, SB_P2_DIVIDER);
+		break;
+	case 7:
+		p |= SET_FIELD(SB_P2_7, SB_P2_DIVIDER);
+		break;
+	default:
+		DRM_ERROR("Bad P2 clock: %d\n", clock->p2);
+		return -EINVAL;
+	}
+	ret = cdv_sb_write(dev, SB_P(pipe), p);
+	if (ret)
+		return ret;
+
+	/* always Program the Lane Register for the Pipe A*/
+	if (pipe == 0) {
+		/* Program the Lane0/1 for HDMI B */
+		u32 lane_reg, lane_value;
+
+		lane_reg = PSB_LANE0;
+		cdv_sb_read(dev, lane_reg, &lane_value);
+		lane_value &= ~(LANE_PLL_MASK);
+		lane_value |= LANE_PLL_ENABLE;
+		cdv_sb_write(dev, lane_reg, lane_value);
+
+		lane_reg = PSB_LANE1;
+		cdv_sb_read(dev, lane_reg, &lane_value);
+		lane_value &= ~(LANE_PLL_MASK);
+		lane_value |= LANE_PLL_ENABLE;
+		cdv_sb_write(dev, lane_reg, lane_value);
+
+		/* Program the Lane2/3 for HDMI C */
+		lane_reg = PSB_LANE2;
+		cdv_sb_read(dev, lane_reg, &lane_value);
+		lane_value &= ~(LANE_PLL_MASK);
+		lane_value |= LANE_PLL_ENABLE;
+		cdv_sb_write(dev, lane_reg, lane_value);
+
+		lane_reg = PSB_LANE3;
+		cdv_sb_read(dev, lane_reg, &lane_value);
+		lane_value &= ~(LANE_PLL_MASK);
+		lane_value |= LANE_PLL_ENABLE;
+		cdv_sb_write(dev, lane_reg, lane_value);
+	}
+
+	return 0;
+}
+
+/*
+ * Returns whether any encoder on the specified pipe is of the specified type
+ */
+bool cdv_intel_pipe_has_type(struct drm_crtc *crtc, int type)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct drm_connector *l_entry;
+
+	list_for_each_entry(l_entry, &mode_config->connector_list, head) {
+		if (l_entry->encoder && l_entry->encoder->crtc == crtc) {
+			struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(l_entry);
+			if (psb_intel_encoder->type == type)
+				return true;
+		}
+	}
+	return false;
+}
+
+static const struct cdv_intel_limit_t *cdv_intel_limit(struct drm_crtc *crtc,
+							int refclk)
+{
+	const struct cdv_intel_limit_t *limit;
+	if (cdv_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+		/*
+		 * Now only single-channel LVDS is supported on CDV. If it is
+		 * incorrect, please add the dual-channel LVDS.
+		 */
+		if (refclk == 96000)
+			limit = &cdv_intel_limits[CDV_LIMIT_SINGLE_LVDS_96];
+		else
+			limit = &cdv_intel_limits[CDV_LIMIT_SINGLE_LVDS_100];
+	} else {
+		if (refclk == 27000)
+			limit = &cdv_intel_limits[CDV_LIMIT_DAC_HDMI_27];
+		else
+			limit = &cdv_intel_limits[CDV_LIMIT_DAC_HDMI_96];
+	}
+	return limit;
+}
+
+/* m1 is reserved as 0 in CDV, n is a ring counter */
+static void cdv_intel_clock(struct drm_device *dev,
+			int refclk, struct cdv_intel_clock_t *clock)
+{
+	clock->m = clock->m2 + 2;
+	clock->p = clock->p1 * clock->p2;
+	clock->vco = (refclk * clock->m) / clock->n;
+	clock->dot = clock->vco / clock->p;
+}
+
+
+#define INTELPllInvalid(s)   { /* ErrorF (s) */; return false; }
+static bool cdv_intel_PLL_is_valid(struct drm_crtc *crtc,
+				const struct cdv_intel_limit_t *limit,
+			       struct cdv_intel_clock_t *clock)
+{
+	if (clock->p1 < limit->p1.min || limit->p1.max < clock->p1)
+		INTELPllInvalid("p1 out of range\n");
+	if (clock->p < limit->p.min || limit->p.max < clock->p)
+		INTELPllInvalid("p out of range\n");
+	/* unnecessary to check the range of m(m1/M2)/n again */
+	if (clock->vco < limit->vco.min || limit->vco.max < clock->vco)
+		INTELPllInvalid("vco out of range\n");
+	/* XXX: We may need to be checking "Dot clock"
+	 * depending on the multiplier, connector, etc.,
+	 * rather than just a single range.
+	 */
+	if (clock->dot < limit->dot.min || limit->dot.max < clock->dot)
+		INTELPllInvalid("dot out of range\n");
+
+	return true;
+}
+
+static bool cdv_intel_find_best_PLL(struct drm_crtc *crtc, int target,
+				int refclk,
+				struct cdv_intel_clock_t *best_clock)
+{
+	struct drm_device *dev = crtc->dev;
+	struct cdv_intel_clock_t clock;
+	const struct cdv_intel_limit_t *limit = cdv_intel_limit(crtc, refclk);
+	int err = target;
+
+
+	if (cdv_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+	    (REG_READ(LVDS) & LVDS_PORT_EN) != 0) {
+		/*
+		 * For LVDS, if the panel is on, just rely on its current
+		 * settings for dual-channel.  We haven't figured out how to
+		 * reliably set up different single/dual channel state, if we
+		 * even can.
+		 */
+		if ((REG_READ(LVDS) & LVDS_CLKB_POWER_MASK) ==
+		    LVDS_CLKB_POWER_UP)
+			clock.p2 = limit->p2.p2_fast;
+		else
+			clock.p2 = limit->p2.p2_slow;
+	} else {
+		if (target < limit->p2.dot_limit)
+			clock.p2 = limit->p2.p2_slow;
+		else
+			clock.p2 = limit->p2.p2_fast;
+	}
+
+	memset(best_clock, 0, sizeof(*best_clock));
+	clock.m1 = 0;
+	/* m1 is reserved as 0 in CDV, n is a ring counter.
+	   So skip the m1 loop */
+	for (clock.n = limit->n.min; clock.n <= limit->n.max; clock.n++) {
+		for (clock.m2 = limit->m2.min; clock.m2 <= limit->m2.max;
+					     clock.m2++) {
+			for (clock.p1 = limit->p1.min;
+					clock.p1 <= limit->p1.max;
+					clock.p1++) {
+				int this_err;
+
+				cdv_intel_clock(dev, refclk, &clock);
+
+				if (!cdv_intel_PLL_is_valid(crtc,
+								limit, &clock))
+						continue;
+
+				this_err = abs(clock.dot - target);
+				if (this_err < err) {
+					*best_clock = clock;
+					err = this_err;
+				}
+			}
+		}
+	}
+
+	return err != target;
+}
+
+int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
+			    int x, int y, struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
+	int pipe = psb_intel_crtc->pipe;
+	unsigned long start, offset;
+	int dspbase = (pipe == 0 ? DSPABASE : DSPBBASE);
+	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
+	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	u32 dspcntr;
+	int ret = 0;
+
+	if (!gma_power_begin(dev, true))
+		return 0;
+
+	/* no fb bound */
+	if (!crtc->fb) {
+		dev_err(dev->dev, "No FB bound\n");
+		goto psb_intel_pipe_cleaner;
+	}
+
+
+	/* We are displaying this buffer, make sure it is actually loaded
+	   into the GTT */
+	ret = psb_gtt_pin(psbfb->gtt);
+	if (ret < 0)
+		goto psb_intel_pipe_set_base_exit;
+	start = psbfb->gtt->offset;
+	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
+
+	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+
+	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		dspcntr |= DISPPLANE_8BPP;
+		break;
+	case 16:
+		if (crtc->fb->depth == 15)
+			dspcntr |= DISPPLANE_15_16BPP;
+		else
+			dspcntr |= DISPPLANE_16BPP;
+		break;
+	case 24:
+	case 32:
+		dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
+		break;
+	default:
+		dev_err(dev->dev, "Unknown color depth\n");
+		ret = -EINVAL;
+		goto psb_intel_pipe_set_base_exit;
+	}
+	REG_WRITE(dspcntr_reg, dspcntr);
+
+	dev_dbg(dev->dev,
+		"Writing base %08lX %08lX %d %d\n", start, offset, x, y);
+
+	REG_WRITE(dspbase, offset);
+	REG_READ(dspbase);
+	REG_WRITE(dspsurf, start);
+	REG_READ(dspsurf);
+
+psb_intel_pipe_cleaner:
+	/* If there was a previous display we can now unpin it */
+	if (old_fb)
+		psb_gtt_unpin(to_psb_fb(old_fb)->gtt);
+
+psb_intel_pipe_set_base_exit:
+	gma_power_end(dev);
+	return ret;
+}
+
+/**
+ * Sets the power management mode of the pipe and plane.
+ *
+ * This code should probably grow support for turning the cursor off and back
+ * on appropriately at the same time as we're turning the pipe off/on.
+ */
+static void cdv_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	int dspbase_reg = (pipe == 0) ? DSPABASE : DSPBBASE;
+	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	u32 temp;
+	bool enabled;
+
+	/* XXX: When our outputs are all unaware of DPMS modes other than off
+	 * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
+	 */
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+		/* Enable the DPLL */
+		temp = REG_READ(dpll_reg);
+		if ((temp & DPLL_VCO_ENABLE) == 0) {
+			REG_WRITE(dpll_reg, temp);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+		}
+
+		/* Jim Bish - switch plan and pipe per scott */
+		/* Enable the plane */
+		temp = REG_READ(dspcntr_reg);
+		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+			REG_WRITE(dspcntr_reg,
+				  temp | DISPLAY_PLANE_ENABLE);
+			/* Flush the plane changes */
+			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+		}
+
+		udelay(150);
+
+		/* Enable the pipe */
+		temp = REG_READ(pipeconf_reg);
+		if ((temp & PIPEACONF_ENABLE) == 0)
+			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+
+		psb_intel_crtc_load_lut(crtc);
+
+		/* Give the overlay scaler a chance to enable
+		 * if it's on this pipe */
+		/* psb_intel_crtc_dpms_video(crtc, true); TODO */
+		break;
+	case DRM_MODE_DPMS_OFF:
+		/* Give the overlay scaler a chance to disable
+		 * if it's on this pipe */
+		/* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
+
+		/* Disable the VGA plane that we never use */
+		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+		/* Jim Bish - changed pipe/plane here as well. */
+
+		/* Wait for vblank for the disable to take effect */
+		cdv_intel_wait_for_vblank(dev);
+
+		/* Next, disable display pipes */
+		temp = REG_READ(pipeconf_reg);
+		if ((temp & PIPEACONF_ENABLE) != 0) {
+			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
+			REG_READ(pipeconf_reg);
+		}
+
+		/* Wait for vblank for the disable to take effect. */
+		cdv_intel_wait_for_vblank(dev);
+
+		udelay(150);
+
+		/* Disable display plane */
+		temp = REG_READ(dspcntr_reg);
+		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+			REG_WRITE(dspcntr_reg,
+				  temp & ~DISPLAY_PLANE_ENABLE);
+			/* Flush the plane changes */
+			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_READ(dspbase_reg);
+		}
+
+		temp = REG_READ(dpll_reg);
+		if ((temp & DPLL_VCO_ENABLE) != 0) {
+			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+		}
+
+		/* Wait for the clocks to turn off. */
+		udelay(150);
+		break;
+	}
+	enabled = crtc->enabled && mode != DRM_MODE_DPMS_OFF;
+	/*Set FIFO Watermarks*/
+	REG_WRITE(DSPARB, 0x3F3E);
+}
+
+static void cdv_intel_crtc_prepare(struct drm_crtc *crtc)
+{
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
+static void cdv_intel_crtc_commit(struct drm_crtc *crtc)
+{
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+void cdv_intel_encoder_prepare(struct drm_encoder *encoder)
+{
+	struct drm_encoder_helper_funcs *encoder_funcs =
+	    encoder->helper_private;
+	/* lvds has its own version of prepare see cdv_intel_lvds_prepare */
+	encoder_funcs->dpms(encoder, DRM_MODE_DPMS_OFF);
+}
+
+void cdv_intel_encoder_commit(struct drm_encoder *encoder)
+{
+	struct drm_encoder_helper_funcs *encoder_funcs =
+	    encoder->helper_private;
+	/* lvds has its own version of commit see cdv_intel_lvds_commit */
+	encoder_funcs->dpms(encoder, DRM_MODE_DPMS_ON);
+}
+
+static bool cdv_intel_crtc_mode_fixup(struct drm_crtc *crtc,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+
+/**
+ * Return the pipe currently connected to the panel fitter,
+ * or -1 if the panel fitter is not present or not in use
+ */
+static int cdv_intel_panel_fitter_pipe(struct drm_device *dev)
+{
+	u32 pfit_control;
+
+	pfit_control = REG_READ(PFIT_CONTROL);
+
+	/* See if the panel fitter is in use */
+	if ((pfit_control & PFIT_ENABLE) == 0)
+		return -1;
+	return (pfit_control >> 29) & 0x3;
+}
+
+static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode,
+			       int x, int y,
+			       struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+	int dpll_md_reg = (psb_intel_crtc->pipe == 0) ? DPLL_A_MD : DPLL_B_MD;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
+	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
+	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
+	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
+	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
+	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
+	int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
+	int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
+	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	int refclk;
+	struct cdv_intel_clock_t clock;
+	u32 dpll = 0, dspcntr, pipeconf;
+	bool ok, is_sdvo = false, is_dvo = false;
+	bool is_crt = false, is_lvds = false, is_tv = false;
+	bool is_hdmi = false;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct drm_connector *connector;
+
+	list_for_each_entry(connector, &mode_config->connector_list, head) {
+		struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+
+		if (!connector->encoder
+		    || connector->encoder->crtc != crtc)
+			continue;
+
+		switch (psb_intel_encoder->type) {
+		case INTEL_OUTPUT_LVDS:
+			is_lvds = true;
+			break;
+		case INTEL_OUTPUT_SDVO:
+			is_sdvo = true;
+			break;
+		case INTEL_OUTPUT_DVO:
+			is_dvo = true;
+			break;
+		case INTEL_OUTPUT_TVOUT:
+			is_tv = true;
+			break;
+		case INTEL_OUTPUT_ANALOG:
+			is_crt = true;
+			break;
+		case INTEL_OUTPUT_HDMI:
+			is_hdmi = true;
+			break;
+		}
+	}
+
+	refclk = 96000;
+
+	/* Hack selection about ref clk for CRT */
+	/* Select 27MHz as the reference clk for HDMI */
+	if (is_crt || is_hdmi)
+		refclk = 27000;
+
+	drm_mode_debug_printmodeline(adjusted_mode);
+
+	ok = cdv_intel_find_best_PLL(crtc, adjusted_mode->clock, refclk,
+				 &clock);
+	if (!ok) {
+		dev_err(dev->dev, "Couldn't find PLL settings for mode!\n");
+		return 0;
+	}
+
+	dpll = DPLL_VGA_MODE_DIS;
+	if (is_tv) {
+		/* XXX: just matching BIOS for now */
+/*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
+		dpll |= 3;
+	}
+		dpll |= PLL_REF_INPUT_DREFCLK;
+
+	dpll |= DPLL_SYNCLOCK_ENABLE;
+	dpll |= DPLL_VGA_MODE_DIS;
+	if (is_lvds)
+		dpll |= DPLLB_MODE_LVDS;
+	else
+		dpll |= DPLLB_MODE_DAC_SERIAL;
+	/* dpll |= (2 << 11); */
+
+	/* setup pipeconf */
+	pipeconf = REG_READ(pipeconf_reg);
+
+	/* Set up the display plane register */
+	dspcntr = DISPPLANE_GAMMA_ENABLE;
+
+	if (pipe == 0)
+		dspcntr |= DISPPLANE_SEL_PIPE_A;
+	else
+		dspcntr |= DISPPLANE_SEL_PIPE_B;
+
+	dspcntr |= DISPLAY_PLANE_ENABLE;
+	pipeconf |= PIPEACONF_ENABLE;
+
+	REG_WRITE(dpll_reg, dpll | DPLL_VGA_MODE_DIS | DPLL_SYNCLOCK_ENABLE);
+	REG_READ(dpll_reg);
+
+	cdv_dpll_set_clock_cdv(dev, crtc, &clock);
+
+	udelay(150);
+
+
+	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
+	 * This is an exception to the general rule that mode_set doesn't turn
+	 * things on.
+	 */
+	if (is_lvds) {
+		u32 lvds = REG_READ(LVDS);
+
+		lvds |=
+		    LVDS_PORT_EN | LVDS_A0A2_CLKA_POWER_UP |
+		    LVDS_PIPEB_SELECT;
+		/* Set the B0-B3 data pairs corresponding to
+		 * whether we're going to
+		 * set the DPLLs for dual-channel mode or not.
+		 */
+		if (clock.p2 == 7)
+			lvds |= LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP;
+		else
+			lvds &= ~(LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP);
+
+		/* It would be nice to set 24 vs 18-bit mode (LVDS_A3_POWER_UP)
+		 * appropriately here, but we need to look more
+		 * thoroughly into how panels behave in the two modes.
+		 */
+
+		REG_WRITE(LVDS, lvds);
+		REG_READ(LVDS);
+	}
+
+	dpll |= DPLL_VCO_ENABLE;
+
+	/* Disable the panel fitter if it was on our pipe */
+	if (cdv_intel_panel_fitter_pipe(dev) == pipe)
+		REG_WRITE(PFIT_CONTROL, 0);
+
+	DRM_DEBUG_KMS("Mode for pipe %c:\n", pipe == 0 ? 'A' : 'B');
+	drm_mode_debug_printmodeline(mode);
+
+	REG_WRITE(dpll_reg,
+		(REG_READ(dpll_reg) & ~DPLL_LOCK) | DPLL_VCO_ENABLE);
+	REG_READ(dpll_reg);
+	/* Wait for the clocks to stabilize. */
+	udelay(150); /* 42 usec w/o calibration, 110 with.  rounded up. */
+
+	if (!(REG_READ(dpll_reg) & DPLL_LOCK)) {
+		dev_err(dev->dev, "Failed to get DPLL lock\n");
+		return -EBUSY;
+	}
+
+	{
+		int sdvo_pixel_multiply = adjusted_mode->clock / mode->clock;
+		REG_WRITE(dpll_md_reg, (0 << DPLL_MD_UDI_DIVIDER_SHIFT) | ((sdvo_pixel_multiply - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT));
+	}
+
+	REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+		  ((adjusted_mode->crtc_htotal - 1) << 16));
+	REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+		  ((adjusted_mode->crtc_hblank_end - 1) << 16));
+	REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+		  ((adjusted_mode->crtc_hsync_end - 1) << 16));
+	REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+		  ((adjusted_mode->crtc_vtotal - 1) << 16));
+	REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+		  ((adjusted_mode->crtc_vblank_end - 1) << 16));
+	REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+		  ((adjusted_mode->crtc_vsync_end - 1) << 16));
+	/* pipesrc and dspsize control the size that is scaled from,
+	 * which should always be the user's requested size.
+	 */
+	REG_WRITE(dspsize_reg,
+		  ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
+	REG_WRITE(dsppos_reg, 0);
+	REG_WRITE(pipesrc_reg,
+		  ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
+	REG_WRITE(pipeconf_reg, pipeconf);
+	REG_READ(pipeconf_reg);
+
+	cdv_intel_wait_for_vblank(dev);
+
+	REG_WRITE(dspcntr_reg, dspcntr);
+
+	/* Flush the plane changes */
+	{
+		struct drm_crtc_helper_funcs *crtc_funcs =
+		    crtc->helper_private;
+		crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+	}
+
+	cdv_intel_wait_for_vblank(dev);
+
+	return 0;
+}
+
+/** Loads the palette/gamma unit for the CRTC with the prepared values */
+void cdv_intel_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv =
+				(struct drm_psb_private *)dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int palreg = PALETTE_A;
+	int i;
+
+	/* The clocks have to be on to load the palette. */
+	if (!crtc->enabled)
+		return;
+
+	switch (psb_intel_crtc->pipe) {
+	case 0:
+		break;
+	case 1:
+		palreg = PALETTE_B;
+		break;
+	case 2:
+		palreg = PALETTE_C;
+		break;
+	default:
+		dev_err(dev->dev, "Illegal Pipe Number.\n");
+		return;
+	}
+
+	if (gma_power_begin(dev, false)) {
+		for (i = 0; i < 256; i++) {
+			REG_WRITE(palreg + 4 * i,
+				  ((psb_intel_crtc->lut_r[i] +
+				  psb_intel_crtc->lut_adj[i]) << 16) |
+				  ((psb_intel_crtc->lut_g[i] +
+				  psb_intel_crtc->lut_adj[i]) << 8) |
+				  (psb_intel_crtc->lut_b[i] +
+				  psb_intel_crtc->lut_adj[i]));
+		}
+		gma_power_end(dev);
+	} else {
+		for (i = 0; i < 256; i++) {
+			dev_priv->save_palette_a[i] =
+				  ((psb_intel_crtc->lut_r[i] +
+				  psb_intel_crtc->lut_adj[i]) << 16) |
+				  ((psb_intel_crtc->lut_g[i] +
+				  psb_intel_crtc->lut_adj[i]) << 8) |
+				  (psb_intel_crtc->lut_b[i] +
+				  psb_intel_crtc->lut_adj[i]);
+		}
+
+	}
+}
+
+/**
+ * Save HW states of giving crtc
+ */
+static void cdv_intel_crtc_save(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	/* struct drm_psb_private *dev_priv =
+			(struct drm_psb_private *)dev->dev_private; */
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
+	int pipeA = (psb_intel_crtc->pipe == 0);
+	uint32_t paletteReg;
+	int i;
+
+	if (!crtc_state) {
+		dev_dbg(dev->dev, "No CRTC state found\n");
+		return;
+	}
+
+	crtc_state->saveDSPCNTR = REG_READ(pipeA ? DSPACNTR : DSPBCNTR);
+	crtc_state->savePIPECONF = REG_READ(pipeA ? PIPEACONF : PIPEBCONF);
+	crtc_state->savePIPESRC = REG_READ(pipeA ? PIPEASRC : PIPEBSRC);
+	crtc_state->saveFP0 = REG_READ(pipeA ? FPA0 : FPB0);
+	crtc_state->saveFP1 = REG_READ(pipeA ? FPA1 : FPB1);
+	crtc_state->saveDPLL = REG_READ(pipeA ? DPLL_A : DPLL_B);
+	crtc_state->saveHTOTAL = REG_READ(pipeA ? HTOTAL_A : HTOTAL_B);
+	crtc_state->saveHBLANK = REG_READ(pipeA ? HBLANK_A : HBLANK_B);
+	crtc_state->saveHSYNC = REG_READ(pipeA ? HSYNC_A : HSYNC_B);
+	crtc_state->saveVTOTAL = REG_READ(pipeA ? VTOTAL_A : VTOTAL_B);
+	crtc_state->saveVBLANK = REG_READ(pipeA ? VBLANK_A : VBLANK_B);
+	crtc_state->saveVSYNC = REG_READ(pipeA ? VSYNC_A : VSYNC_B);
+	crtc_state->saveDSPSTRIDE = REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE);
+
+	/*NOTE: DSPSIZE DSPPOS only for psb*/
+	crtc_state->saveDSPSIZE = REG_READ(pipeA ? DSPASIZE : DSPBSIZE);
+	crtc_state->saveDSPPOS = REG_READ(pipeA ? DSPAPOS : DSPBPOS);
+
+	crtc_state->saveDSPBASE = REG_READ(pipeA ? DSPABASE : DSPBBASE);
+
+	DRM_DEBUG("(%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
+			crtc_state->saveDSPCNTR,
+			crtc_state->savePIPECONF,
+			crtc_state->savePIPESRC,
+			crtc_state->saveFP0,
+			crtc_state->saveFP1,
+			crtc_state->saveDPLL,
+			crtc_state->saveHTOTAL,
+			crtc_state->saveHBLANK,
+			crtc_state->saveHSYNC,
+			crtc_state->saveVTOTAL,
+			crtc_state->saveVBLANK,
+			crtc_state->saveVSYNC,
+			crtc_state->saveDSPSTRIDE,
+			crtc_state->saveDSPSIZE,
+			crtc_state->saveDSPPOS,
+			crtc_state->saveDSPBASE
+		);
+
+	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	for (i = 0; i < 256; ++i)
+		crtc_state->savePalette[i] = REG_READ(paletteReg + (i << 2));
+}
+
+/**
+ * Restore HW states of giving crtc
+ */
+static void cdv_intel_crtc_restore(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	/* struct drm_psb_private * dev_priv =
+				(struct drm_psb_private *)dev->dev_private; */
+	struct psb_intel_crtc *psb_intel_crtc =  to_psb_intel_crtc(crtc);
+	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
+	/* struct drm_crtc_helper_funcs * crtc_funcs = crtc->helper_private; */
+	int pipeA = (psb_intel_crtc->pipe == 0);
+	uint32_t paletteReg;
+	int i;
+
+	if (!crtc_state) {
+		dev_dbg(dev->dev, "No crtc state\n");
+		return;
+	}
+
+	DRM_DEBUG(
+		"current:(%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
+		REG_READ(pipeA ? DSPACNTR : DSPBCNTR),
+		REG_READ(pipeA ? PIPEACONF : PIPEBCONF),
+		REG_READ(pipeA ? PIPEASRC : PIPEBSRC),
+		REG_READ(pipeA ? FPA0 : FPB0),
+		REG_READ(pipeA ? FPA1 : FPB1),
+		REG_READ(pipeA ? DPLL_A : DPLL_B),
+		REG_READ(pipeA ? HTOTAL_A : HTOTAL_B),
+		REG_READ(pipeA ? HBLANK_A : HBLANK_B),
+		REG_READ(pipeA ? HSYNC_A : HSYNC_B),
+		REG_READ(pipeA ? VTOTAL_A : VTOTAL_B),
+		REG_READ(pipeA ? VBLANK_A : VBLANK_B),
+		REG_READ(pipeA ? VSYNC_A : VSYNC_B),
+		REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE),
+		REG_READ(pipeA ? DSPASIZE : DSPBSIZE),
+		REG_READ(pipeA ? DSPAPOS : DSPBPOS),
+		REG_READ(pipeA ? DSPABASE : DSPBBASE)
+		);
+
+	DRM_DEBUG(
+		"saved: (%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
+		crtc_state->saveDSPCNTR,
+		crtc_state->savePIPECONF,
+		crtc_state->savePIPESRC,
+		crtc_state->saveFP0,
+		crtc_state->saveFP1,
+		crtc_state->saveDPLL,
+		crtc_state->saveHTOTAL,
+		crtc_state->saveHBLANK,
+		crtc_state->saveHSYNC,
+		crtc_state->saveVTOTAL,
+		crtc_state->saveVBLANK,
+		crtc_state->saveVSYNC,
+		crtc_state->saveDSPSTRIDE,
+		crtc_state->saveDSPSIZE,
+		crtc_state->saveDSPPOS,
+		crtc_state->saveDSPBASE
+		);
+
+
+	if (crtc_state->saveDPLL & DPLL_VCO_ENABLE) {
+		REG_WRITE(pipeA ? DPLL_A : DPLL_B,
+			crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
+		REG_READ(pipeA ? DPLL_A : DPLL_B);
+		DRM_DEBUG("write dpll: %x\n",
+				REG_READ(pipeA ? DPLL_A : DPLL_B));
+		udelay(150);
+	}
+
+	REG_WRITE(pipeA ? FPA0 : FPB0, crtc_state->saveFP0);
+	REG_READ(pipeA ? FPA0 : FPB0);
+
+	REG_WRITE(pipeA ? FPA1 : FPB1, crtc_state->saveFP1);
+	REG_READ(pipeA ? FPA1 : FPB1);
+
+	REG_WRITE(pipeA ? DPLL_A : DPLL_B, crtc_state->saveDPLL);
+	REG_READ(pipeA ? DPLL_A : DPLL_B);
+	udelay(150);
+
+	REG_WRITE(pipeA ? HTOTAL_A : HTOTAL_B, crtc_state->saveHTOTAL);
+	REG_WRITE(pipeA ? HBLANK_A : HBLANK_B, crtc_state->saveHBLANK);
+	REG_WRITE(pipeA ? HSYNC_A : HSYNC_B, crtc_state->saveHSYNC);
+	REG_WRITE(pipeA ? VTOTAL_A : VTOTAL_B, crtc_state->saveVTOTAL);
+	REG_WRITE(pipeA ? VBLANK_A : VBLANK_B, crtc_state->saveVBLANK);
+	REG_WRITE(pipeA ? VSYNC_A : VSYNC_B, crtc_state->saveVSYNC);
+	REG_WRITE(pipeA ? DSPASTRIDE : DSPBSTRIDE, crtc_state->saveDSPSTRIDE);
+
+	REG_WRITE(pipeA ? DSPASIZE : DSPBSIZE, crtc_state->saveDSPSIZE);
+	REG_WRITE(pipeA ? DSPAPOS : DSPBPOS, crtc_state->saveDSPPOS);
+
+	REG_WRITE(pipeA ? PIPEASRC : PIPEBSRC, crtc_state->savePIPESRC);
+	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+	REG_WRITE(pipeA ? PIPEACONF : PIPEBCONF, crtc_state->savePIPECONF);
+
+	cdv_intel_wait_for_vblank(dev);
+
+	REG_WRITE(pipeA ? DSPACNTR : DSPBCNTR, crtc_state->saveDSPCNTR);
+	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+
+	cdv_intel_wait_for_vblank(dev);
+
+	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	for (i = 0; i < 256; ++i)
+		REG_WRITE(paletteReg + (i << 2), crtc_state->savePalette[i]);
+}
+
+static int cdv_intel_crtc_cursor_set(struct drm_crtc *crtc,
+				 struct drm_file *file_priv,
+				 uint32_t handle,
+				 uint32_t width, uint32_t height)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	uint32_t control = (pipe == 0) ? CURACNTR : CURBCNTR;
+	uint32_t base = (pipe == 0) ? CURABASE : CURBBASE;
+	uint32_t temp;
+	size_t addr = 0;
+	struct gtt_range *gt;
+	struct drm_gem_object *obj;
+	int ret;
+
+	/* if we want to turn of the cursor ignore width and height */
+	if (!handle) {
+		/* turn off the cursor */
+		temp = CURSOR_MODE_DISABLE;
+
+		if (gma_power_begin(dev, false)) {
+			REG_WRITE(control, temp);
+			REG_WRITE(base, 0);
+			gma_power_end(dev);
+		}
+
+		/* unpin the old GEM object */
+		if (psb_intel_crtc->cursor_obj) {
+			gt = container_of(psb_intel_crtc->cursor_obj,
+							struct gtt_range, gem);
+			psb_gtt_unpin(gt);
+			drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+			psb_intel_crtc->cursor_obj = NULL;
+		}
+
+		return 0;
+	}
+
+	/* Currently we only support 64x64 cursors */
+	if (width != 64 || height != 64) {
+		dev_dbg(dev->dev, "we currently only support 64x64 cursors\n");
+		return -EINVAL;
+	}
+
+	obj = drm_gem_object_lookup(dev, file_priv, handle);
+	if (!obj)
+		return -ENOENT;
+
+	if (obj->size < width * height * 4) {
+		dev_dbg(dev->dev, "buffer is to small\n");
+		return -ENOMEM;
+	}
+
+	gt = container_of(obj, struct gtt_range, gem);
+
+	/* Pin the memory into the GTT */
+	ret = psb_gtt_pin(gt);
+	if (ret) {
+		dev_err(dev->dev, "Can not pin down handle 0x%x\n", handle);
+		return ret;
+	}
+
+	addr = gt->offset;	/* Or resource.start ??? */
+
+	psb_intel_crtc->cursor_addr = addr;
+
+	temp = 0;
+	/* set the pipe for the cursor */
+	temp |= (pipe << 28);
+	temp |= CURSOR_MODE_64_ARGB_AX | MCURSOR_GAMMA_ENABLE;
+
+	if (gma_power_begin(dev, false)) {
+		REG_WRITE(control, temp);
+		REG_WRITE(base, addr);
+		gma_power_end(dev);
+	}
+
+	/* unpin the old GEM object */
+	if (psb_intel_crtc->cursor_obj) {
+		gt = container_of(psb_intel_crtc->cursor_obj,
+							struct gtt_range, gem);
+		psb_gtt_unpin(gt);
+		drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+		psb_intel_crtc->cursor_obj = obj;
+	}
+	return 0;
+}
+
+static int cdv_intel_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	uint32_t temp = 0;
+	uint32_t adder;
+
+
+	if (x < 0) {
+		temp |= (CURSOR_POS_SIGN << CURSOR_X_SHIFT);
+		x = -x;
+	}
+	if (y < 0) {
+		temp |= (CURSOR_POS_SIGN << CURSOR_Y_SHIFT);
+		y = -y;
+	}
+
+	temp |= ((x & CURSOR_POS_MASK) << CURSOR_X_SHIFT);
+	temp |= ((y & CURSOR_POS_MASK) << CURSOR_Y_SHIFT);
+
+	adder = psb_intel_crtc->cursor_addr;
+
+	if (gma_power_begin(dev, false)) {
+		REG_WRITE((pipe == 0) ? CURAPOS : CURBPOS, temp);
+		REG_WRITE((pipe == 0) ? CURABASE : CURBBASE, adder);
+		gma_power_end(dev);
+	}
+	return 0;
+}
+
+static void cdv_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
+			 u16 *green, u16 *blue, uint32_t start, uint32_t size)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int i;
+	int end = (start + size > 256) ? 256 : start + size;
+
+	for (i = start; i < end; i++) {
+		psb_intel_crtc->lut_r[i] = red[i] >> 8;
+		psb_intel_crtc->lut_g[i] = green[i] >> 8;
+		psb_intel_crtc->lut_b[i] = blue[i] >> 8;
+	}
+
+	cdv_intel_crtc_load_lut(crtc);
+}
+
+static int cdv_crtc_set_config(struct drm_mode_set *set)
+{
+	int ret = 0;
+	struct drm_device *dev = set->crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (!dev_priv->rpm_enabled)
+		return drm_crtc_helper_set_config(set);
+
+	pm_runtime_forbid(&dev->pdev->dev);
+
+	ret = drm_crtc_helper_set_config(set);
+
+	pm_runtime_allow(&dev->pdev->dev);
+
+	return ret;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 8xx chips. */
+
+/* FIXME: why are we using this, should it be cdv_ in this tree ? */
+
+static void i8xx_clock(int refclk, struct cdv_intel_clock_t *clock)
+{
+	clock->m = 5 * (clock->m1 + 2) + (clock->m2 + 2);
+	clock->p = clock->p1 * clock->p2;
+	clock->vco = refclk * clock->m / (clock->n + 2);
+	clock->dot = clock->vco / clock->p;
+}
+
+/* Returns the clock of the currently programmed mode of the given pipe. */
+static int cdv_intel_crtc_clock_get(struct drm_device *dev,
+				struct drm_crtc *crtc)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	u32 dpll;
+	u32 fp;
+	struct cdv_intel_clock_t clock;
+	bool is_lvds;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (gma_power_begin(dev, false)) {
+		dpll = REG_READ((pipe == 0) ? DPLL_A : DPLL_B);
+		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
+			fp = REG_READ((pipe == 0) ? FPA0 : FPB0);
+		else
+			fp = REG_READ((pipe == 0) ? FPA1 : FPB1);
+		is_lvds = (pipe == 1) && (REG_READ(LVDS) & LVDS_PORT_EN);
+		gma_power_end(dev);
+	} else {
+		dpll = (pipe == 0) ?
+			dev_priv->saveDPLL_A : dev_priv->saveDPLL_B;
+
+		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
+			fp = (pipe == 0) ?
+				dev_priv->saveFPA0 :
+				dev_priv->saveFPB0;
+		else
+			fp = (pipe == 0) ?
+				dev_priv->saveFPA1 :
+				dev_priv->saveFPB1;
+
+		is_lvds = (pipe == 1) && (dev_priv->saveLVDS & LVDS_PORT_EN);
+	}
+
+	clock.m1 = (fp & FP_M1_DIV_MASK) >> FP_M1_DIV_SHIFT;
+	clock.m2 = (fp & FP_M2_DIV_MASK) >> FP_M2_DIV_SHIFT;
+	clock.n = (fp & FP_N_DIV_MASK) >> FP_N_DIV_SHIFT;
+
+	if (is_lvds) {
+		clock.p1 =
+		    ffs((dpll &
+			 DPLL_FPA01_P1_POST_DIV_MASK_I830_LVDS) >>
+			DPLL_FPA01_P1_POST_DIV_SHIFT);
+		if (clock.p1 == 0) {
+			clock.p1 = 4;
+			dev_err(dev->dev, "PLL %d\n", dpll);
+		}
+		clock.p2 = 14;
+
+		if ((dpll & PLL_REF_INPUT_MASK) ==
+		    PLLB_REF_INPUT_SPREADSPECTRUMIN) {
+			/* XXX: might not be 66MHz */
+			i8xx_clock(66000, &clock);
+		} else
+			i8xx_clock(48000, &clock);
+	} else {
+		if (dpll & PLL_P1_DIVIDE_BY_TWO)
+			clock.p1 = 2;
+		else {
+			clock.p1 =
+			    ((dpll &
+			      DPLL_FPA01_P1_POST_DIV_MASK_I830) >>
+			     DPLL_FPA01_P1_POST_DIV_SHIFT) + 2;
+		}
+		if (dpll & PLL_P2_DIVIDE_BY_4)
+			clock.p2 = 4;
+		else
+			clock.p2 = 2;
+
+		i8xx_clock(48000, &clock);
+	}
+
+	/* XXX: It would be nice to validate the clocks, but we can't reuse
+	 * i830PllIsValid() because it relies on the xf86_config connector
+	 * configuration being accurate, which it isn't necessarily.
+	 */
+
+	return clock.dot;
+}
+
+/** Returns the currently programmed mode of the given pipe. */
+struct drm_display_mode *cdv_intel_crtc_mode_get(struct drm_device *dev,
+					     struct drm_crtc *crtc)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	struct drm_display_mode *mode;
+	int htot;
+	int hsync;
+	int vtot;
+	int vsync;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (gma_power_begin(dev, false)) {
+		htot = REG_READ((pipe == 0) ? HTOTAL_A : HTOTAL_B);
+		hsync = REG_READ((pipe == 0) ? HSYNC_A : HSYNC_B);
+		vtot = REG_READ((pipe == 0) ? VTOTAL_A : VTOTAL_B);
+		vsync = REG_READ((pipe == 0) ? VSYNC_A : VSYNC_B);
+		gma_power_end(dev);
+	} else {
+		htot = (pipe == 0) ?
+			dev_priv->saveHTOTAL_A : dev_priv->saveHTOTAL_B;
+		hsync = (pipe == 0) ?
+			dev_priv->saveHSYNC_A : dev_priv->saveHSYNC_B;
+		vtot = (pipe == 0) ?
+			dev_priv->saveVTOTAL_A : dev_priv->saveVTOTAL_B;
+		vsync = (pipe == 0) ?
+			dev_priv->saveVSYNC_A : dev_priv->saveVSYNC_B;
+	}
+
+	mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+	if (!mode)
+		return NULL;
+
+	mode->clock = cdv_intel_crtc_clock_get(dev, crtc);
+	mode->hdisplay = (htot & 0xffff) + 1;
+	mode->htotal = ((htot & 0xffff0000) >> 16) + 1;
+	mode->hsync_start = (hsync & 0xffff) + 1;
+	mode->hsync_end = ((hsync & 0xffff0000) >> 16) + 1;
+	mode->vdisplay = (vtot & 0xffff) + 1;
+	mode->vtotal = ((vtot & 0xffff0000) >> 16) + 1;
+	mode->vsync_start = (vsync & 0xffff) + 1;
+	mode->vsync_end = ((vsync & 0xffff0000) >> 16) + 1;
+
+	drm_mode_set_name(mode);
+	drm_mode_set_crtcinfo(mode, 0);
+
+	return mode;
+}
+
+static void cdv_intel_crtc_destroy(struct drm_crtc *crtc)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+
+	kfree(psb_intel_crtc->crtc_state);
+	drm_crtc_cleanup(crtc);
+	kfree(psb_intel_crtc);
+}
+
+const struct drm_crtc_helper_funcs cdv_intel_helper_funcs = {
+	.dpms = cdv_intel_crtc_dpms,
+	.mode_fixup = cdv_intel_crtc_mode_fixup,
+	.mode_set = cdv_intel_crtc_mode_set,
+	.mode_set_base = cdv_intel_pipe_set_base,
+	.prepare = cdv_intel_crtc_prepare,
+	.commit = cdv_intel_crtc_commit,
+};
+
+const struct drm_crtc_funcs cdv_intel_crtc_funcs = {
+	.save = cdv_intel_crtc_save,
+	.restore = cdv_intel_crtc_restore,
+	.cursor_set = cdv_intel_crtc_cursor_set,
+	.cursor_move = cdv_intel_crtc_cursor_move,
+	.gamma_set = cdv_intel_crtc_gamma_set,
+	.set_config = cdv_crtc_set_config,
+	.destroy = cdv_intel_crtc_destroy,
+};
+
+/*
+ * Set the default value of cursor control and base register
+ * to zero. This is a workaround for h/w defect on oaktrail
+ */
+void cdv_intel_cursor_init(struct drm_device *dev, int pipe)
+{
+	uint32_t control;
+	uint32_t base;
+
+	switch (pipe) {
+	case 0:
+		control = CURACNTR;
+		base = CURABASE;
+		break;
+	case 1:
+		control = CURBCNTR;
+		base = CURBBASE;
+		break;
+	case 2:
+		control = CURCCNTR;
+		base = CURCBASE;
+		break;
+	default:
+		return;
+	}
+
+	REG_WRITE(control, 0);
+	REG_WRITE(base, 0);
+}
+
diff --git a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
new file mode 100644
index 000000000000..50d7cfb51662
--- /dev/null
+++ b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
@@ -0,0 +1,394 @@
+/*
+ * Copyright © 2006-2011 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	jim liu <jim.liu@intel.com>
+ *
+ * FIXME:
+ *	We should probably make this generic and share it with Medfield
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_edid.h>
+#include "psb_intel_drv.h"
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+#include <linux/pm_runtime.h>
+
+/* hdmi control bits */
+#define HDMI_NULL_PACKETS_DURING_VSYNC	(1 << 9)
+#define HDMI_BORDER_ENABLE		(1 << 7)
+#define HDMI_AUDIO_ENABLE		(1 << 6)
+#define HDMI_VSYNC_ACTIVE_HIGH		(1 << 4)
+#define HDMI_HSYNC_ACTIVE_HIGH		(1 << 3)
+/* hdmi-b control bits */
+#define	HDMIB_PIPE_B_SELECT		(1 << 30)
+
+
+struct mid_intel_hdmi_priv {
+	u32 hdmi_reg;
+	u32 save_HDMIB;
+	bool has_hdmi_sink;
+	bool has_hdmi_audio;
+	/* Should set this when detect hotplug */
+	bool hdmi_device_connected;
+	struct mdfld_hdmi_i2c *i2c_bus;
+	struct i2c_adapter *hdmi_i2c_adapter;	/* for control functions */
+	struct drm_device *dev;
+};
+
+static void cdv_hdmi_mode_set(struct drm_encoder *encoder,
+			struct drm_display_mode *mode,
+			struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct psb_intel_encoder *psb_intel_encoder = to_psb_intel_encoder(encoder);
+	struct mid_intel_hdmi_priv *hdmi_priv = psb_intel_encoder->dev_priv;
+	u32 hdmib;
+	struct drm_crtc *crtc = encoder->crtc;
+	struct psb_intel_crtc *intel_crtc = to_psb_intel_crtc(crtc);
+
+	hdmib = (2 << 10);
+
+	if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
+		hdmib |= HDMI_VSYNC_ACTIVE_HIGH;
+	if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
+		hdmib |= HDMI_HSYNC_ACTIVE_HIGH;
+
+	if (intel_crtc->pipe == 1)
+		hdmib |= HDMIB_PIPE_B_SELECT;
+
+	if (hdmi_priv->has_hdmi_audio) {
+		hdmib |= HDMI_AUDIO_ENABLE;
+		hdmib |= HDMI_NULL_PACKETS_DURING_VSYNC;
+	}
+
+	REG_WRITE(hdmi_priv->hdmi_reg, hdmib);
+	REG_READ(hdmi_priv->hdmi_reg);
+}
+
+static bool cdv_hdmi_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void cdv_hdmi_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct psb_intel_encoder *psb_intel_encoder =
+						to_psb_intel_encoder(encoder);
+	struct mid_intel_hdmi_priv *hdmi_priv = psb_intel_encoder->dev_priv;
+	u32 hdmib;
+
+	hdmib = REG_READ(hdmi_priv->hdmi_reg);
+
+	if (mode != DRM_MODE_DPMS_ON)
+		REG_WRITE(hdmi_priv->hdmi_reg, hdmib & ~HDMIB_PORT_EN);
+	else
+		REG_WRITE(hdmi_priv->hdmi_reg, hdmib | HDMIB_PORT_EN);
+	REG_READ(hdmi_priv->hdmi_reg);
+}
+
+static void cdv_hdmi_save(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct mid_intel_hdmi_priv *hdmi_priv = psb_intel_encoder->dev_priv;
+
+	hdmi_priv->save_HDMIB = REG_READ(hdmi_priv->hdmi_reg);
+}
+
+static void cdv_hdmi_restore(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct mid_intel_hdmi_priv *hdmi_priv = psb_intel_encoder->dev_priv;
+
+	REG_WRITE(hdmi_priv->hdmi_reg, hdmi_priv->save_HDMIB);
+	REG_READ(hdmi_priv->hdmi_reg);
+}
+
+static enum drm_connector_status cdv_hdmi_detect(
+				struct drm_connector *connector, bool force)
+{
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct psb_intel_connector *psb_intel_connector =
+					to_psb_intel_connector(connector);
+	struct mid_intel_hdmi_priv *hdmi_priv = psb_intel_encoder->dev_priv;
+	struct edid *edid = NULL;
+	enum drm_connector_status status = connector_status_disconnected;
+
+	edid = drm_get_edid(connector, &psb_intel_encoder->i2c_bus->adapter);
+
+	hdmi_priv->has_hdmi_sink = false;
+	hdmi_priv->has_hdmi_audio = false;
+	if (edid) {
+		if (edid->input & DRM_EDID_INPUT_DIGITAL) {
+			status = connector_status_connected;
+			hdmi_priv->has_hdmi_sink =
+						drm_detect_hdmi_monitor(edid);
+			hdmi_priv->has_hdmi_audio =
+						drm_detect_monitor_audio(edid);
+		}
+
+		psb_intel_connector->base.display_info.raw_edid = NULL;
+		kfree(edid);
+	}
+	return status;
+}
+
+static int cdv_hdmi_set_property(struct drm_connector *connector,
+				       struct drm_property *property,
+				       uint64_t value)
+{
+	struct drm_encoder *encoder = connector->encoder;
+
+	if (!strcmp(property->name, "scaling mode") && encoder) {
+		struct psb_intel_crtc *crtc = to_psb_intel_crtc(encoder->crtc);
+		bool centre;
+		uint64_t curValue;
+
+		if (!crtc)
+			return -1;
+
+		switch (value) {
+		case DRM_MODE_SCALE_FULLSCREEN:
+			break;
+		case DRM_MODE_SCALE_NO_SCALE:
+			break;
+		case DRM_MODE_SCALE_ASPECT:
+			break;
+		default:
+			return -1;
+		}
+
+		if (drm_connector_property_get_value(connector,
+							property, &curValue))
+			return -1;
+
+		if (curValue == value)
+			return 0;
+
+		if (drm_connector_property_set_value(connector,
+							property, value))
+			return -1;
+
+		centre = (curValue == DRM_MODE_SCALE_NO_SCALE) ||
+			(value == DRM_MODE_SCALE_NO_SCALE);
+
+		if (crtc->saved_mode.hdisplay != 0 &&
+		    crtc->saved_mode.vdisplay != 0) {
+			if (centre) {
+				if (!drm_crtc_helper_set_mode(encoder->crtc, &crtc->saved_mode,
+					    encoder->crtc->x, encoder->crtc->y, encoder->crtc->fb))
+					return -1;
+			} else {
+				struct drm_encoder_helper_funcs *helpers
+						    = encoder->helper_private;
+				helpers->mode_set(encoder, &crtc->saved_mode,
+					     &crtc->saved_adjusted_mode);
+			}
+		}
+	}
+	return 0;
+}
+
+/*
+ * Return the list of HDMI DDC modes if available.
+ */
+static int cdv_hdmi_get_modes(struct drm_connector *connector)
+{
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct edid *edid = NULL;
+	int ret = 0;
+
+	edid = drm_get_edid(connector, &psb_intel_encoder->i2c_bus->adapter);
+	if (edid) {
+		drm_mode_connector_update_edid_property(connector, edid);
+		ret = drm_add_edid_modes(connector, edid);
+		kfree(edid);
+	}
+	return ret;
+}
+
+static int cdv_hdmi_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+
+	if (mode->clock > 165000)
+		return MODE_CLOCK_HIGH;
+	if (mode->clock < 20000)
+		return MODE_CLOCK_HIGH;
+
+	/* just in case */
+	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+		return MODE_NO_DBLESCAN;
+
+	/* just in case */
+	if (mode->flags & DRM_MODE_FLAG_INTERLACE)
+		return MODE_NO_INTERLACE;
+
+	/*
+	 * FIXME: for now we limit the size to 1680x1050 on CDV, otherwise it
+	 * will go beyond the stolen memory size allocated to the framebuffer
+	 */
+	if (mode->hdisplay > 1680)
+		return MODE_PANEL;
+	if (mode->vdisplay > 1050)
+		return MODE_PANEL;
+	return MODE_OK;
+}
+
+static void cdv_hdmi_destroy(struct drm_connector *connector)
+{
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+
+	if (psb_intel_encoder->i2c_bus)
+		psb_intel_i2c_destroy(psb_intel_encoder->i2c_bus);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+static const struct drm_encoder_helper_funcs cdv_hdmi_helper_funcs = {
+	.dpms = cdv_hdmi_dpms,
+	.mode_fixup = cdv_hdmi_mode_fixup,
+	.prepare = psb_intel_encoder_prepare,
+	.mode_set = cdv_hdmi_mode_set,
+	.commit = psb_intel_encoder_commit,
+};
+
+static const struct drm_connector_helper_funcs
+					cdv_hdmi_connector_helper_funcs = {
+	.get_modes = cdv_hdmi_get_modes,
+	.mode_valid = cdv_hdmi_mode_valid,
+	.best_encoder = psb_intel_best_encoder,
+};
+
+static const struct drm_connector_funcs cdv_hdmi_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.save = cdv_hdmi_save,
+	.restore = cdv_hdmi_restore,
+	.detect = cdv_hdmi_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.set_property = cdv_hdmi_set_property,
+	.destroy = cdv_hdmi_destroy,
+};
+
+void cdv_hdmi_init(struct drm_device *dev,
+			struct psb_intel_mode_device *mode_dev, int reg)
+{
+	struct psb_intel_encoder *psb_intel_encoder;
+	struct psb_intel_connector *psb_intel_connector;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+	struct mid_intel_hdmi_priv *hdmi_priv;
+	int ddc_bus;
+
+	psb_intel_encoder = kzalloc(sizeof(struct psb_intel_encoder),
+				    GFP_KERNEL);
+
+	if (!psb_intel_encoder)
+		return;
+
+	psb_intel_connector = kzalloc(sizeof(struct psb_intel_connector),
+				      GFP_KERNEL);
+
+	if (!psb_intel_connector)
+		goto err_connector;
+
+	hdmi_priv = kzalloc(sizeof(struct mid_intel_hdmi_priv), GFP_KERNEL);
+
+	if (!hdmi_priv)
+		goto err_priv;
+
+	connector = &psb_intel_connector->base;
+	encoder = &psb_intel_encoder->base;
+	drm_connector_init(dev, connector,
+			   &cdv_hdmi_connector_funcs,
+			   DRM_MODE_CONNECTOR_DVID);
+
+	drm_encoder_init(dev, encoder, &psb_intel_lvds_enc_funcs,
+			 DRM_MODE_ENCODER_TMDS);
+
+	psb_intel_connector_attach_encoder(psb_intel_connector,
+					   psb_intel_encoder);
+	psb_intel_encoder->type = INTEL_OUTPUT_HDMI;
+	hdmi_priv->hdmi_reg = reg;
+	hdmi_priv->has_hdmi_sink = false;
+	psb_intel_encoder->dev_priv = hdmi_priv;
+
+	drm_encoder_helper_add(encoder, &cdv_hdmi_helper_funcs);
+	drm_connector_helper_add(connector,
+				 &cdv_hdmi_connector_helper_funcs);
+	connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+	connector->interlace_allowed = false;
+	connector->doublescan_allowed = false;
+
+	drm_connector_attach_property(connector,
+				      dev->mode_config.scaling_mode_property,
+				      DRM_MODE_SCALE_FULLSCREEN);
+
+	switch (reg) {
+	case SDVOB:
+		ddc_bus = GPIOE;
+		break;
+	case SDVOC:
+		ddc_bus = GPIOD;
+		break;
+	default:
+		DRM_ERROR("unknown reg 0x%x for HDMI\n", reg);
+		goto failed_ddc;
+		break;
+	}
+
+	psb_intel_encoder->i2c_bus = psb_intel_i2c_create(dev,
+				ddc_bus, (reg == SDVOB) ? "HDMIB" : "HDMIC");
+
+	if (!psb_intel_encoder->i2c_bus) {
+		dev_err(dev->dev, "No ddc adapter available!\n");
+		goto failed_ddc;
+	}
+
+	hdmi_priv->hdmi_i2c_adapter =
+				&(psb_intel_encoder->i2c_bus->adapter);
+	hdmi_priv->dev = dev;
+	drm_sysfs_connector_add(connector);
+	return;
+
+failed_ddc:
+	drm_encoder_cleanup(encoder);
+	drm_connector_cleanup(connector);
+err_priv:
+	kfree(psb_intel_connector);
+err_connector:
+	kfree(psb_intel_encoder);
+}
diff --git a/drivers/gpu/drm/gma500/cdv_intel_lvds.c b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
new file mode 100644
index 000000000000..50e744be9852
--- /dev/null
+++ b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
@@ -0,0 +1,732 @@
+/*
+ * Copyright © 2006-2011 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ *	Dave Airlie <airlied@linux.ie>
+ *	Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <linux/dmi.h>
+#include <drm/drmP.h>
+
+#include "intel_bios.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include <linux/pm_runtime.h>
+#include "cdv_device.h"
+
+/**
+ * LVDS I2C backlight control macros
+ */
+#define BRIGHTNESS_MAX_LEVEL 100
+#define BRIGHTNESS_MASK 0xFF
+#define BLC_I2C_TYPE	0x01
+#define BLC_PWM_TYPT	0x02
+
+#define BLC_POLARITY_NORMAL 0
+#define BLC_POLARITY_INVERSE 1
+
+#define PSB_BLC_MAX_PWM_REG_FREQ       (0xFFFE)
+#define PSB_BLC_MIN_PWM_REG_FREQ	(0x2)
+#define PSB_BLC_PWM_PRECISION_FACTOR	(10)
+#define PSB_BACKLIGHT_PWM_CTL_SHIFT	(16)
+#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
+
+struct cdv_intel_lvds_priv {
+	/**
+	 * Saved LVDO output states
+	 */
+	uint32_t savePP_ON;
+	uint32_t savePP_OFF;
+	uint32_t saveLVDS;
+	uint32_t savePP_CONTROL;
+	uint32_t savePP_CYCLE;
+	uint32_t savePFIT_CONTROL;
+	uint32_t savePFIT_PGM_RATIOS;
+	uint32_t saveBLC_PWM_CTL;
+};
+
+/*
+ * Returns the maximum level of the backlight duty cycle field.
+ */
+static u32 cdv_intel_lvds_get_max_backlight(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 retval;
+
+	if (gma_power_begin(dev, false)) {
+		retval = ((REG_READ(BLC_PWM_CTL) &
+			  BACKLIGHT_MODULATION_FREQ_MASK) >>
+			  BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+		gma_power_end(dev);
+	} else
+		retval = ((dev_priv->saveBLC_PWM_CTL &
+			  BACKLIGHT_MODULATION_FREQ_MASK) >>
+			  BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+	return retval;
+}
+
+/*
+ * Set LVDS backlight level by I2C command
+ */
+static int cdv_lvds_i2c_set_brightness(struct drm_device *dev,
+					unsigned int level)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_i2c_chan *lvds_i2c_bus = dev_priv->lvds_i2c_bus;
+	u8 out_buf[2];
+	unsigned int blc_i2c_brightness;
+
+	struct i2c_msg msgs[] = {
+		{
+			.addr = lvds_i2c_bus->slave_addr,
+			.flags = 0,
+			.len = 2,
+			.buf = out_buf,
+		}
+	};
+
+	blc_i2c_brightness = BRIGHTNESS_MASK & ((unsigned int)level *
+			     BRIGHTNESS_MASK /
+			     BRIGHTNESS_MAX_LEVEL);
+
+	if (dev_priv->lvds_bl->pol == BLC_POLARITY_INVERSE)
+		blc_i2c_brightness = BRIGHTNESS_MASK - blc_i2c_brightness;
+
+	out_buf[0] = dev_priv->lvds_bl->brightnesscmd;
+	out_buf[1] = (u8)blc_i2c_brightness;
+
+	if (i2c_transfer(&lvds_i2c_bus->adapter, msgs, 1) == 1)
+		return 0;
+
+	DRM_ERROR("I2C transfer error\n");
+	return -1;
+}
+
+
+static int cdv_lvds_pwm_set_brightness(struct drm_device *dev, int level)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	u32 max_pwm_blc;
+	u32 blc_pwm_duty_cycle;
+
+	max_pwm_blc = cdv_intel_lvds_get_max_backlight(dev);
+
+	/*BLC_PWM_CTL Should be initiated while backlight device init*/
+	BUG_ON((max_pwm_blc & PSB_BLC_MAX_PWM_REG_FREQ) == 0);
+
+	blc_pwm_duty_cycle = level * max_pwm_blc / BRIGHTNESS_MAX_LEVEL;
+
+	if (dev_priv->lvds_bl->pol == BLC_POLARITY_INVERSE)
+		blc_pwm_duty_cycle = max_pwm_blc - blc_pwm_duty_cycle;
+
+	blc_pwm_duty_cycle &= PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR;
+	REG_WRITE(BLC_PWM_CTL,
+		  (max_pwm_blc << PSB_BACKLIGHT_PWM_CTL_SHIFT) |
+		  (blc_pwm_duty_cycle));
+
+	return 0;
+}
+
+/*
+ * Set LVDS backlight level either by I2C or PWM
+ */
+void cdv_intel_lvds_set_brightness(struct drm_device *dev, int level)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (!dev_priv->lvds_bl) {
+		DRM_ERROR("NO LVDS Backlight Info\n");
+		return;
+	}
+
+	if (dev_priv->lvds_bl->type == BLC_I2C_TYPE)
+		cdv_lvds_i2c_set_brightness(dev, level);
+	else
+		cdv_lvds_pwm_set_brightness(dev, level);
+}
+
+/**
+ * Sets the backlight level.
+ *
+ * level backlight level, from 0 to cdv_intel_lvds_get_max_backlight().
+ */
+static void cdv_intel_lvds_set_backlight(struct drm_device *dev, int level)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 blc_pwm_ctl;
+
+	if (gma_power_begin(dev, false)) {
+		blc_pwm_ctl =
+			REG_READ(BLC_PWM_CTL) & ~BACKLIGHT_DUTY_CYCLE_MASK;
+		REG_WRITE(BLC_PWM_CTL,
+				(blc_pwm_ctl |
+				(level << BACKLIGHT_DUTY_CYCLE_SHIFT)));
+		gma_power_end(dev);
+	} else {
+		blc_pwm_ctl = dev_priv->saveBLC_PWM_CTL &
+				~BACKLIGHT_DUTY_CYCLE_MASK;
+		dev_priv->saveBLC_PWM_CTL = (blc_pwm_ctl |
+					(level << BACKLIGHT_DUTY_CYCLE_SHIFT));
+	}
+}
+
+/**
+ * Sets the power state for the panel.
+ */
+static void cdv_intel_lvds_set_power(struct drm_device *dev,
+				     struct drm_encoder *encoder, bool on)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 pp_status;
+
+	if (!gma_power_begin(dev, true))
+		return;
+
+	if (on) {
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) |
+			  POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while ((pp_status & PP_ON) == 0);
+
+		cdv_intel_lvds_set_backlight(dev,
+				dev_priv->mode_dev.backlight_duty_cycle);
+	} else {
+		cdv_intel_lvds_set_backlight(dev, 0);
+
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) &
+			  ~POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while (pp_status & PP_ON);
+	}
+	gma_power_end(dev);
+}
+
+static void cdv_intel_lvds_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	if (mode == DRM_MODE_DPMS_ON)
+		cdv_intel_lvds_set_power(dev, encoder, true);
+	else
+		cdv_intel_lvds_set_power(dev, encoder, false);
+	/* XXX: We never power down the LVDS pairs. */
+}
+
+static void cdv_intel_lvds_save(struct drm_connector *connector)
+{
+}
+
+static void cdv_intel_lvds_restore(struct drm_connector *connector)
+{
+}
+
+int cdv_intel_lvds_mode_valid(struct drm_connector *connector,
+			      struct drm_display_mode *mode)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct drm_display_mode *fixed_mode =
+					dev_priv->mode_dev.panel_fixed_mode;
+
+	/* just in case */
+	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+		return MODE_NO_DBLESCAN;
+
+	/* just in case */
+	if (mode->flags & DRM_MODE_FLAG_INTERLACE)
+		return MODE_NO_INTERLACE;
+
+	if (fixed_mode) {
+		if (mode->hdisplay > fixed_mode->hdisplay)
+			return MODE_PANEL;
+		if (mode->vdisplay > fixed_mode->vdisplay)
+			return MODE_PANEL;
+	}
+	return MODE_OK;
+}
+
+bool cdv_intel_lvds_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+	struct drm_encoder *tmp_encoder;
+	struct drm_display_mode *panel_fixed_mode = mode_dev->panel_fixed_mode;
+
+	/* Should never happen!! */
+	list_for_each_entry(tmp_encoder, &dev->mode_config.encoder_list,
+			    head) {
+		if (tmp_encoder != encoder
+		    && tmp_encoder->crtc == encoder->crtc) {
+			printk(KERN_ERR "Can't enable LVDS and another "
+			       "encoder on the same pipe\n");
+			return false;
+		}
+	}
+
+	/*
+	 * If we have timings from the BIOS for the panel, put them in
+	 * to the adjusted mode.  The CRTC will be set up for this mode,
+	 * with the panel scaling set up to source from the H/VDisplay
+	 * of the original mode.
+	 */
+	if (panel_fixed_mode != NULL) {
+		adjusted_mode->hdisplay = panel_fixed_mode->hdisplay;
+		adjusted_mode->hsync_start = panel_fixed_mode->hsync_start;
+		adjusted_mode->hsync_end = panel_fixed_mode->hsync_end;
+		adjusted_mode->htotal = panel_fixed_mode->htotal;
+		adjusted_mode->vdisplay = panel_fixed_mode->vdisplay;
+		adjusted_mode->vsync_start = panel_fixed_mode->vsync_start;
+		adjusted_mode->vsync_end = panel_fixed_mode->vsync_end;
+		adjusted_mode->vtotal = panel_fixed_mode->vtotal;
+		adjusted_mode->clock = panel_fixed_mode->clock;
+		drm_mode_set_crtcinfo(adjusted_mode,
+				      CRTC_INTERLACE_HALVE_V);
+	}
+
+	/*
+	 * XXX: It would be nice to support lower refresh rates on the
+	 * panels to reduce power consumption, and perhaps match the
+	 * user's requested refresh rate.
+	 */
+
+	return true;
+}
+
+static void cdv_intel_lvds_prepare(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+
+	if (!gma_power_begin(dev, true))
+		return;
+
+	mode_dev->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+	mode_dev->backlight_duty_cycle = (mode_dev->saveBLC_PWM_CTL &
+					  BACKLIGHT_DUTY_CYCLE_MASK);
+
+	cdv_intel_lvds_set_power(dev, encoder, false);
+
+	gma_power_end(dev);
+}
+
+static void cdv_intel_lvds_commit(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+
+	if (mode_dev->backlight_duty_cycle == 0)
+		mode_dev->backlight_duty_cycle =
+		    cdv_intel_lvds_get_max_backlight(dev);
+
+	cdv_intel_lvds_set_power(dev, encoder, true);
+}
+
+static void cdv_intel_lvds_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 pfit_control;
+
+	/*
+	 * The LVDS pin pair will already have been turned on in the
+	 * cdv_intel_crtc_mode_set since it has a large impact on the DPLL
+	 * settings.
+	 */
+
+	/*
+	 * Enable automatic panel scaling so that non-native modes fill the
+	 * screen.  Should be enabled before the pipe is enabled, according to
+	 * register description and PRM.
+	 */
+	if (mode->hdisplay != adjusted_mode->hdisplay ||
+	    mode->vdisplay != adjusted_mode->vdisplay)
+		pfit_control = (PFIT_ENABLE | VERT_AUTO_SCALE |
+				HORIZ_AUTO_SCALE | VERT_INTERP_BILINEAR |
+				HORIZ_INTERP_BILINEAR);
+	else
+		pfit_control = 0;
+
+	if (dev_priv->lvds_dither)
+		pfit_control |= PANEL_8TO6_DITHER_ENABLE;
+
+	REG_WRITE(PFIT_CONTROL, pfit_control);
+}
+
+/**
+ * Detect the LVDS connection.
+ *
+ * This always returns CONNECTOR_STATUS_CONNECTED.
+ * This connector should only have
+ * been set up if the LVDS was actually connected anyway.
+ */
+static enum drm_connector_status cdv_intel_lvds_detect(
+				struct drm_connector *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+/**
+ * Return the list of DDC modes if available, or the BIOS fixed mode otherwise.
+ */
+static int cdv_intel_lvds_get_modes(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+	int ret;
+
+	ret = psb_intel_ddc_get_modes(connector, &psb_intel_encoder->i2c_bus->adapter);
+
+	if (ret)
+		return ret;
+
+	/* Didn't get an EDID, so
+	 * Set wide sync ranges so we get all modes
+	 * handed to valid_mode for checking
+	 */
+	connector->display_info.min_vfreq = 0;
+	connector->display_info.max_vfreq = 200;
+	connector->display_info.min_hfreq = 0;
+	connector->display_info.max_hfreq = 200;
+	if (mode_dev->panel_fixed_mode != NULL) {
+		struct drm_display_mode *mode =
+		    drm_mode_duplicate(dev, mode_dev->panel_fixed_mode);
+		drm_mode_probed_add(connector, mode);
+		return 1;
+	}
+
+	return 0;
+}
+
+/**
+ * cdv_intel_lvds_destroy - unregister and free LVDS structures
+ * @connector: connector to free
+ *
+ * Unregister the DDC bus for this connector then free the driver private
+ * structure.
+ */
+void cdv_intel_lvds_destroy(struct drm_connector *connector)
+{
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+
+	if (psb_intel_encoder->i2c_bus)
+		psb_intel_i2c_destroy(psb_intel_encoder->i2c_bus);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+int cdv_intel_lvds_set_property(struct drm_connector *connector,
+				       struct drm_property *property,
+				       uint64_t value)
+{
+	struct drm_encoder *encoder = connector->encoder;
+
+	if (!strcmp(property->name, "scaling mode") && encoder) {
+		struct psb_intel_crtc *crtc =
+					to_psb_intel_crtc(encoder->crtc);
+		uint64_t curValue;
+
+		if (!crtc)
+			return -1;
+
+		switch (value) {
+		case DRM_MODE_SCALE_FULLSCREEN:
+			break;
+		case DRM_MODE_SCALE_NO_SCALE:
+			break;
+		case DRM_MODE_SCALE_ASPECT:
+			break;
+		default:
+			return -1;
+		}
+
+		if (drm_connector_property_get_value(connector,
+						     property,
+						     &curValue))
+			return -1;
+
+		if (curValue == value)
+			return 0;
+
+		if (drm_connector_property_set_value(connector,
+							property,
+							value))
+			return -1;
+
+		if (crtc->saved_mode.hdisplay != 0 &&
+		    crtc->saved_mode.vdisplay != 0) {
+			if (!drm_crtc_helper_set_mode(encoder->crtc,
+						      &crtc->saved_mode,
+						      encoder->crtc->x,
+						      encoder->crtc->y,
+						      encoder->crtc->fb))
+				return -1;
+		}
+	} else if (!strcmp(property->name, "backlight") && encoder) {
+		if (drm_connector_property_set_value(connector,
+							property,
+							value))
+			return -1;
+		else {
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+			struct drm_psb_private *dev_priv =
+						encoder->dev->dev_private;
+			struct backlight_device *bd =
+						dev_priv->backlight_device;
+			bd->props.brightness = value;
+			backlight_update_status(bd);
+#endif
+		}
+	} else if (!strcmp(property->name, "DPMS") && encoder) {
+		struct drm_encoder_helper_funcs *helpers =
+					encoder->helper_private;
+		helpers->dpms(encoder, value);
+	}
+	return 0;
+}
+
+static const struct drm_encoder_helper_funcs
+					cdv_intel_lvds_helper_funcs = {
+	.dpms = cdv_intel_lvds_encoder_dpms,
+	.mode_fixup = cdv_intel_lvds_mode_fixup,
+	.prepare = cdv_intel_lvds_prepare,
+	.mode_set = cdv_intel_lvds_mode_set,
+	.commit = cdv_intel_lvds_commit,
+};
+
+static const struct drm_connector_helper_funcs
+				cdv_intel_lvds_connector_helper_funcs = {
+	.get_modes = cdv_intel_lvds_get_modes,
+	.mode_valid = cdv_intel_lvds_mode_valid,
+	.best_encoder = psb_intel_best_encoder,
+};
+
+static const struct drm_connector_funcs cdv_intel_lvds_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.save = cdv_intel_lvds_save,
+	.restore = cdv_intel_lvds_restore,
+	.detect = cdv_intel_lvds_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.set_property = cdv_intel_lvds_set_property,
+	.destroy = cdv_intel_lvds_destroy,
+};
+
+
+static void cdv_intel_lvds_enc_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+}
+
+const struct drm_encoder_funcs cdv_intel_lvds_enc_funcs = {
+	.destroy = cdv_intel_lvds_enc_destroy,
+};
+
+/**
+ * cdv_intel_lvds_init - setup LVDS connectors on this device
+ * @dev: drm device
+ *
+ * Create the connector, register the LVDS DDC bus, and try to figure out what
+ * modes we can display on the LVDS panel (if present).
+ */
+void cdv_intel_lvds_init(struct drm_device *dev,
+		     struct psb_intel_mode_device *mode_dev)
+{
+	struct psb_intel_encoder *psb_intel_encoder;
+	struct psb_intel_connector *psb_intel_connector;
+	struct cdv_intel_lvds_priv *lvds_priv;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+	struct drm_display_mode *scan;
+	struct drm_crtc *crtc;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 lvds;
+	int pipe;
+
+	psb_intel_encoder = kzalloc(sizeof(struct psb_intel_encoder),
+				    GFP_KERNEL);
+	if (!psb_intel_encoder)
+		return;
+
+	psb_intel_connector = kzalloc(sizeof(struct psb_intel_connector),
+				      GFP_KERNEL);
+	if (!psb_intel_connector)
+		goto failed_connector;
+
+	lvds_priv = kzalloc(sizeof(struct cdv_intel_lvds_priv), GFP_KERNEL);
+	if (!lvds_priv)
+		goto failed_lvds_priv;
+
+	psb_intel_encoder->dev_priv = lvds_priv;
+
+	connector = &psb_intel_connector->base;
+	encoder = &psb_intel_encoder->base;
+
+
+	drm_connector_init(dev, connector,
+			   &cdv_intel_lvds_connector_funcs,
+			   DRM_MODE_CONNECTOR_LVDS);
+
+	drm_encoder_init(dev, encoder,
+			 &cdv_intel_lvds_enc_funcs,
+			 DRM_MODE_ENCODER_LVDS);
+
+
+	psb_intel_connector_attach_encoder(psb_intel_connector,
+					   psb_intel_encoder);
+	psb_intel_encoder->type = INTEL_OUTPUT_LVDS;
+
+	drm_encoder_helper_add(encoder, &cdv_intel_lvds_helper_funcs);
+	drm_connector_helper_add(connector,
+				 &cdv_intel_lvds_connector_helper_funcs);
+	connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+	connector->interlace_allowed = false;
+	connector->doublescan_allowed = false;
+
+	/*Attach connector properties*/
+	drm_connector_attach_property(connector,
+				      dev->mode_config.scaling_mode_property,
+				      DRM_MODE_SCALE_FULLSCREEN);
+	drm_connector_attach_property(connector,
+				      dev_priv->backlight_property,
+				      BRIGHTNESS_MAX_LEVEL);
+
+	/**
+	 * Set up I2C bus
+	 * FIXME: distroy i2c_bus when exit
+	 */
+	psb_intel_encoder->i2c_bus = psb_intel_i2c_create(dev,
+							 GPIOB,
+							 "LVDSBLC_B");
+	if (!psb_intel_encoder->i2c_bus) {
+		dev_printk(KERN_ERR,
+			&dev->pdev->dev, "I2C bus registration failed.\n");
+		goto failed_blc_i2c;
+	}
+	psb_intel_encoder->i2c_bus->slave_addr = 0x2C;
+	dev_priv->lvds_i2c_bus = psb_intel_encoder->i2c_bus;
+
+	/*
+	 * LVDS discovery:
+	 * 1) check for EDID on DDC
+	 * 2) check for VBT data
+	 * 3) check to see if LVDS is already on
+	 *    if none of the above, no panel
+	 * 4) make sure lid is open
+	 *    if closed, act like it's not there for now
+	 */
+
+	/* Set up the DDC bus. */
+	psb_intel_encoder->ddc_bus = psb_intel_i2c_create(dev,
+							 GPIOC,
+							 "LVDSDDC_C");
+	if (!psb_intel_encoder->ddc_bus) {
+		dev_printk(KERN_ERR, &dev->pdev->dev,
+			   "DDC bus registration " "failed.\n");
+		goto failed_ddc;
+	}
+
+	/*
+	 * Attempt to get the fixed panel mode from DDC.  Assume that the
+	 * preferred mode is the right one.
+	 */
+	psb_intel_ddc_get_modes(connector,
+				&psb_intel_encoder->ddc_bus->adapter);
+	list_for_each_entry(scan, &connector->probed_modes, head) {
+		if (scan->type & DRM_MODE_TYPE_PREFERRED) {
+			mode_dev->panel_fixed_mode =
+			    drm_mode_duplicate(dev, scan);
+			goto out;	/* FIXME: check for quirks */
+		}
+	}
+
+	/* Failed to get EDID, what about VBT? do we need this?*/
+	if (dev_priv->lfp_lvds_vbt_mode) {
+		mode_dev->panel_fixed_mode =
+			drm_mode_duplicate(dev, dev_priv->lfp_lvds_vbt_mode);
+		if (mode_dev->panel_fixed_mode) {
+			mode_dev->panel_fixed_mode->type |=
+				DRM_MODE_TYPE_PREFERRED;
+			goto out;	/* FIXME: check for quirks */
+		}
+	}
+	/*
+	 * If we didn't get EDID, try checking if the panel is already turned
+	 * on.	If so, assume that whatever is currently programmed is the
+	 * correct mode.
+	 */
+	lvds = REG_READ(LVDS);
+	pipe = (lvds & LVDS_PIPEB_SELECT) ? 1 : 0;
+	crtc = psb_intel_get_crtc_from_pipe(dev, pipe);
+
+	if (crtc && (lvds & LVDS_PORT_EN)) {
+		mode_dev->panel_fixed_mode =
+		    cdv_intel_crtc_mode_get(dev, crtc);
+		if (mode_dev->panel_fixed_mode) {
+			mode_dev->panel_fixed_mode->type |=
+			    DRM_MODE_TYPE_PREFERRED;
+			goto out;	/* FIXME: check for quirks */
+		}
+	}
+
+	/* If we still don't have a mode after all that, give up. */
+	if (!mode_dev->panel_fixed_mode) {
+		DRM_DEBUG
+			("Found no modes on the lvds, ignoring the LVDS\n");
+		goto failed_find;
+	}
+
+out:
+	drm_sysfs_connector_add(connector);
+	return;
+
+failed_find:
+	printk(KERN_ERR "Failed find\n");
+	if (psb_intel_encoder->ddc_bus)
+		psb_intel_i2c_destroy(psb_intel_encoder->ddc_bus);
+failed_ddc:
+	printk(KERN_ERR "Failed DDC\n");
+	if (psb_intel_encoder->i2c_bus)
+		psb_intel_i2c_destroy(psb_intel_encoder->i2c_bus);
+failed_blc_i2c:
+	printk(KERN_ERR "Failed BLC\n");
+	drm_encoder_cleanup(encoder);
+	drm_connector_cleanup(connector);
+	kfree(lvds_priv);
+failed_lvds_priv:
+	kfree(psb_intel_connector);
+failed_connector:
+	kfree(psb_intel_encoder);
+}
diff --git a/drivers/gpu/drm/gma500/framebuffer.c b/drivers/gpu/drm/gma500/framebuffer.c
new file mode 100644
index 000000000000..791c0ef1a65b
--- /dev/null
+++ b/drivers/gpu/drm/gma500/framebuffer.c
@@ -0,0 +1,831 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/tty.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/console.h>
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_fb_helper.h>
+
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_drv.h"
+#include "framebuffer.h"
+#include "gtt.h"
+
+static void psb_user_framebuffer_destroy(struct drm_framebuffer *fb);
+static int psb_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+					      struct drm_file *file_priv,
+					      unsigned int *handle);
+
+static const struct drm_framebuffer_funcs psb_fb_funcs = {
+	.destroy = psb_user_framebuffer_destroy,
+	.create_handle = psb_user_framebuffer_create_handle,
+};
+
+#define CMAP_TOHW(_val, _width) ((((_val) << (_width)) + 0x7FFF - (_val)) >> 16)
+
+static int psbfb_setcolreg(unsigned regno, unsigned red, unsigned green,
+			   unsigned blue, unsigned transp,
+			   struct fb_info *info)
+{
+	struct psb_fbdev *fbdev = info->par;
+	struct drm_framebuffer *fb = fbdev->psb_fb_helper.fb;
+	uint32_t v;
+
+	if (!fb)
+		return -ENOMEM;
+
+	if (regno > 255)
+		return 1;
+
+	red = CMAP_TOHW(red, info->var.red.length);
+	blue = CMAP_TOHW(blue, info->var.blue.length);
+	green = CMAP_TOHW(green, info->var.green.length);
+	transp = CMAP_TOHW(transp, info->var.transp.length);
+
+	v = (red << info->var.red.offset) |
+	    (green << info->var.green.offset) |
+	    (blue << info->var.blue.offset) |
+	    (transp << info->var.transp.offset);
+
+	if (regno < 16) {
+		switch (fb->bits_per_pixel) {
+		case 16:
+			((uint32_t *) info->pseudo_palette)[regno] = v;
+			break;
+		case 24:
+		case 32:
+			((uint32_t *) info->pseudo_palette)[regno] = v;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int psbfb_pan(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct psb_fbdev *fbdev = info->par;
+	struct psb_framebuffer *psbfb = &fbdev->pfb;
+	struct drm_device *dev = psbfb->base.dev;
+
+	/*
+	 *	We have to poke our nose in here. The core fb code assumes
+	 *	panning is part of the hardware that can be invoked before
+	 *	the actual fb is mapped. In our case that isn't quite true.
+	 */
+	if (psbfb->gtt->npage) {
+		/* GTT roll shifts in 4K pages, we need to shift the right
+		   number of pages */
+		int pages = info->fix.line_length >> 12;
+		psb_gtt_roll(dev, psbfb->gtt, var->yoffset * pages);
+	}
+        return 0;
+}
+
+void psbfb_suspend(struct drm_device *dev)
+{
+	struct drm_framebuffer *fb = 0;
+	struct psb_framebuffer *psbfb = to_psb_fb(fb);
+
+	console_lock();
+	mutex_lock(&dev->mode_config.mutex);
+	list_for_each_entry(fb, &dev->mode_config.fb_list, head) {
+		struct fb_info *info = psbfb->fbdev;
+		fb_set_suspend(info, 1);
+		drm_fb_helper_blank(FB_BLANK_POWERDOWN, info);
+	}
+	mutex_unlock(&dev->mode_config.mutex);
+	console_unlock();
+}
+
+void psbfb_resume(struct drm_device *dev)
+{
+	struct drm_framebuffer *fb = 0;
+	struct psb_framebuffer *psbfb = to_psb_fb(fb);
+
+	console_lock();
+	mutex_lock(&dev->mode_config.mutex);
+	list_for_each_entry(fb, &dev->mode_config.fb_list, head) {
+		struct fb_info *info = psbfb->fbdev;
+		fb_set_suspend(info, 0);
+		drm_fb_helper_blank(FB_BLANK_UNBLANK, info);
+	}
+	mutex_unlock(&dev->mode_config.mutex);
+	console_unlock();
+	drm_helper_disable_unused_functions(dev);
+}
+
+static int psbfb_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct psb_framebuffer *psbfb = vma->vm_private_data;
+	struct drm_device *dev = psbfb->base.dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int page_num;
+	int i;
+	unsigned long address;
+	int ret;
+	unsigned long pfn;
+	/* FIXME: assumes fb at stolen base which may not be true */
+	unsigned long phys_addr = (unsigned long)dev_priv->stolen_base;
+
+	page_num = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+	address = (unsigned long)vmf->virtual_address;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	for (i = 0; i < page_num; i++) {
+		pfn = (phys_addr >> PAGE_SHIFT);
+
+		ret = vm_insert_mixed(vma, address, pfn);
+		if (unlikely((ret == -EBUSY) || (ret != 0 && i > 0)))
+			break;
+		else if (unlikely(ret != 0)) {
+			ret = (ret == -ENOMEM) ? VM_FAULT_OOM : VM_FAULT_SIGBUS;
+			return ret;
+		}
+		address += PAGE_SIZE;
+		phys_addr += PAGE_SIZE;
+	}
+	return VM_FAULT_NOPAGE;
+}
+
+static void psbfb_vm_open(struct vm_area_struct *vma)
+{
+}
+
+static void psbfb_vm_close(struct vm_area_struct *vma)
+{
+}
+
+static struct vm_operations_struct psbfb_vm_ops = {
+	.fault	= psbfb_vm_fault,
+	.open	= psbfb_vm_open,
+	.close	= psbfb_vm_close
+};
+
+static int psbfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
+{
+	struct psb_fbdev *fbdev = info->par;
+	struct psb_framebuffer *psbfb = &fbdev->pfb;
+
+	if (vma->vm_pgoff != 0)
+		return -EINVAL;
+	if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
+		return -EINVAL;
+
+	if (!psbfb->addr_space)
+		psbfb->addr_space = vma->vm_file->f_mapping;
+	/*
+	 * If this is a GEM object then info->screen_base is the virtual
+	 * kernel remapping of the object. FIXME: Review if this is
+	 * suitable for our mmap work
+	 */
+	vma->vm_ops = &psbfb_vm_ops;
+	vma->vm_private_data = (void *)psbfb;
+	vma->vm_flags |= VM_RESERVED | VM_IO |
+					VM_MIXEDMAP | VM_DONTEXPAND;
+	return 0;
+}
+
+static int psbfb_ioctl(struct fb_info *info, unsigned int cmd,
+						unsigned long arg)
+{
+	return -ENOTTY;
+}
+
+static struct fb_ops psbfb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcolreg = psbfb_setcolreg,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = psbfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_mmap = psbfb_mmap,
+	.fb_sync = psbfb_sync,
+	.fb_ioctl = psbfb_ioctl,
+};
+
+static struct fb_ops psbfb_roll_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcolreg = psbfb_setcolreg,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_pan_display = psbfb_pan,
+	.fb_mmap = psbfb_mmap,
+	.fb_sync = psbfb_sync,
+	.fb_ioctl = psbfb_ioctl,
+};
+
+static struct fb_ops psbfb_unaccel_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcolreg = psbfb_setcolreg,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_mmap = psbfb_mmap,
+	.fb_ioctl = psbfb_ioctl,
+};
+
+/**
+ *	psb_framebuffer_init	-	initialize a framebuffer
+ *	@dev: our DRM device
+ *	@fb: framebuffer to set up
+ *	@mode_cmd: mode description
+ *	@gt: backing object
+ *
+ *	Configure and fill in the boilerplate for our frame buffer. Return
+ *	0 on success or an error code if we fail.
+ */
+static int psb_framebuffer_init(struct drm_device *dev,
+					struct psb_framebuffer *fb,
+					struct drm_mode_fb_cmd2 *mode_cmd,
+					struct gtt_range *gt)
+{
+	u32 bpp, depth;
+	int ret;
+
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
+
+	if (mode_cmd->pitches[0] & 63)
+		return -EINVAL;
+	switch (bpp) {
+	case 8:
+	case 16:
+	case 24:
+	case 32:
+		break;
+	default:
+		return -EINVAL;
+	}
+	ret = drm_framebuffer_init(dev, &fb->base, &psb_fb_funcs);
+	if (ret) {
+		dev_err(dev->dev, "framebuffer init failed: %d\n", ret);
+		return ret;
+	}
+	drm_helper_mode_fill_fb_struct(&fb->base, mode_cmd);
+	fb->gtt = gt;
+	return 0;
+}
+
+/**
+ *	psb_framebuffer_create	-	create a framebuffer backed by gt
+ *	@dev: our DRM device
+ *	@mode_cmd: the description of the requested mode
+ *	@gt: the backing object
+ *
+ *	Create a framebuffer object backed by the gt, and fill in the
+ *	boilerplate required
+ *
+ *	TODO: review object references
+ */
+
+static struct drm_framebuffer *psb_framebuffer_create
+			(struct drm_device *dev,
+			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 struct gtt_range *gt)
+{
+	struct psb_framebuffer *fb;
+	int ret;
+
+	fb = kzalloc(sizeof(*fb), GFP_KERNEL);
+	if (!fb)
+		return ERR_PTR(-ENOMEM);
+
+	ret = psb_framebuffer_init(dev, fb, mode_cmd, gt);
+	if (ret) {
+		kfree(fb);
+		return ERR_PTR(ret);
+	}
+	return &fb->base;
+}
+
+/**
+ *	psbfb_alloc		-	allocate frame buffer memory
+ *	@dev: the DRM device
+ *	@aligned_size: space needed
+ *	@force: fall back to GEM buffers if need be
+ *
+ *	Allocate the frame buffer. In the usual case we get a GTT range that
+ *	is stolen memory backed and life is simple. If there isn't sufficient
+ *	we fail as we don't have the virtual mapping space to really vmap it
+ *	and the kernel console code can't handle non linear framebuffers.
+ *
+ *	Re-address this as and if the framebuffer layer grows this ability.
+ */
+static struct gtt_range *psbfb_alloc(struct drm_device *dev, int aligned_size)
+{
+	struct gtt_range *backing;
+	/* Begin by trying to use stolen memory backing */
+	backing = psb_gtt_alloc_range(dev, aligned_size, "fb", 1);
+	if (backing) {
+		if (drm_gem_private_object_init(dev,
+					&backing->gem, aligned_size) == 0)
+			return backing;
+		psb_gtt_free_range(dev, backing);
+	}
+	return NULL;
+}
+
+/**
+ *	psbfb_create		-	create a framebuffer
+ *	@fbdev: the framebuffer device
+ *	@sizes: specification of the layout
+ *
+ *	Create a framebuffer to the specifications provided
+ */
+static int psbfb_create(struct psb_fbdev *fbdev,
+				struct drm_fb_helper_surface_size *sizes)
+{
+	struct drm_device *dev = fbdev->psb_fb_helper.dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct fb_info *info;
+	struct drm_framebuffer *fb;
+	struct psb_framebuffer *psbfb = &fbdev->pfb;
+	struct drm_mode_fb_cmd2 mode_cmd;
+	struct device *device = &dev->pdev->dev;
+	int size;
+	int ret;
+	struct gtt_range *backing;
+	u32 bpp, depth;
+	int gtt_roll = 0;
+	int pitch_lines = 0;
+
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+	bpp = sizes->surface_bpp;
+
+	/* No 24bit packed */
+	if (bpp == 24)
+		bpp = 32;
+
+	do {
+		/*
+		 * Acceleration via the GTT requires pitch to be
+		 * power of two aligned. Preferably page but less
+		 * is ok with some fonts
+		 */
+        	mode_cmd.pitches[0] =  ALIGN(mode_cmd.width * ((bpp + 7) / 8), 4096 >> pitch_lines);
+        	depth = sizes->surface_depth;
+
+        	size = mode_cmd.pitches[0] * mode_cmd.height;
+        	size = ALIGN(size, PAGE_SIZE);
+
+		/* Allocate the fb in the GTT with stolen page backing */
+		backing = psbfb_alloc(dev, size);
+
+		if (pitch_lines)
+			pitch_lines *= 2;
+		else
+			pitch_lines = 1;
+		gtt_roll++;
+	} while (backing == NULL && pitch_lines <= 16);
+
+	/* The final pitch we accepted if we succeeded */
+	pitch_lines /= 2;
+
+	if (backing == NULL) {
+		/*
+		 *	We couldn't get the space we wanted, fall back to the
+		 *	display engine requirement instead.  The HW requires
+		 *	the pitch to be 64 byte aligned
+		 */
+
+		gtt_roll = 0;	/* Don't use GTT accelerated scrolling */
+		pitch_lines = 64;
+
+		mode_cmd.pitches[0] =  ALIGN(mode_cmd.width * ((bpp + 7) / 8), 64);
+
+		size = mode_cmd.pitches[0] * mode_cmd.height;
+		size = ALIGN(size, PAGE_SIZE);
+
+		/* Allocate the framebuffer in the GTT with stolen page backing */
+		backing = psbfb_alloc(dev, size);
+		if (backing == NULL)
+			return -ENOMEM;
+	}
+
+	mutex_lock(&dev->struct_mutex);
+
+	info = framebuffer_alloc(0, device);
+	if (!info) {
+		ret = -ENOMEM;
+		goto out_err1;
+	}
+	info->par = fbdev;
+
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(bpp, depth);
+
+	ret = psb_framebuffer_init(dev, psbfb, &mode_cmd, backing);
+	if (ret)
+		goto out_unref;
+
+	fb = &psbfb->base;
+	psbfb->fbdev = info;
+
+	fbdev->psb_fb_helper.fb = fb;
+	fbdev->psb_fb_helper.fbdev = info;
+
+	strcpy(info->fix.id, "psbfb");
+
+	info->flags = FBINFO_DEFAULT;
+	if (dev_priv->ops->accel_2d && pitch_lines > 8)	/* 2D engine */
+		info->fbops = &psbfb_ops;
+	else if (gtt_roll) {	/* GTT rolling seems best */
+		info->fbops = &psbfb_roll_ops;
+		info->flags |= FBINFO_HWACCEL_YPAN;
+	} else	/* Software */
+		info->fbops = &psbfb_unaccel_ops;
+
+	ret = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (ret) {
+		ret = -ENOMEM;
+		goto out_unref;
+	}
+
+	info->fix.smem_start = dev->mode_config.fb_base;
+	info->fix.smem_len = size;
+	info->fix.ywrapstep = gtt_roll;
+	info->fix.ypanstep = 0;
+
+	/* Accessed stolen memory directly */
+	info->screen_base = (char *)dev_priv->vram_addr +
+							backing->offset;
+	info->screen_size = size;
+
+	if (dev_priv->gtt.stolen_size) {
+		info->apertures = alloc_apertures(1);
+		if (!info->apertures) {
+			ret = -ENOMEM;
+			goto out_unref;
+		}
+		info->apertures->ranges[0].base = dev->mode_config.fb_base;
+		info->apertures->ranges[0].size = dev_priv->gtt.stolen_size;
+	}
+
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
+	drm_fb_helper_fill_var(info, &fbdev->psb_fb_helper,
+				sizes->fb_width, sizes->fb_height);
+
+	info->fix.mmio_start = pci_resource_start(dev->pdev, 0);
+	info->fix.mmio_len = pci_resource_len(dev->pdev, 0);
+
+	info->pixmap.size = 64 * 1024;
+	info->pixmap.buf_align = 8;
+	info->pixmap.access_align = 32;
+	info->pixmap.flags = FB_PIXMAP_SYSTEM;
+	info->pixmap.scan_align = 1;
+
+	dev_info(dev->dev, "allocated %dx%d fb\n",
+					psbfb->base.width, psbfb->base.height);
+
+	mutex_unlock(&dev->struct_mutex);
+	return 0;
+out_unref:
+	if (backing->stolen)
+		psb_gtt_free_range(dev, backing);
+	else
+		drm_gem_object_unreference(&backing->gem);
+out_err1:
+	mutex_unlock(&dev->struct_mutex);
+	psb_gtt_free_range(dev, backing);
+	return ret;
+}
+
+/**
+ *	psb_user_framebuffer_create	-	create framebuffer
+ *	@dev: our DRM device
+ *	@filp: client file
+ *	@cmd: mode request
+ *
+ *	Create a new framebuffer backed by a userspace GEM object
+ */
+static struct drm_framebuffer *psb_user_framebuffer_create
+			(struct drm_device *dev, struct drm_file *filp,
+			 struct drm_mode_fb_cmd2 *cmd)
+{
+	struct gtt_range *r;
+	struct drm_gem_object *obj;
+
+	/*
+	 *	Find the GEM object and thus the gtt range object that is
+	 *	to back this space
+	 */
+	obj = drm_gem_object_lookup(dev, filp, cmd->handles[0]);
+	if (obj == NULL)
+		return ERR_PTR(-ENOENT);
+
+	/* Let the core code do all the work */
+	r = container_of(obj, struct gtt_range, gem);
+	return psb_framebuffer_create(dev, cmd, r);
+}
+
+static void psbfb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+							u16 blue, int regno)
+{
+}
+
+static void psbfb_gamma_get(struct drm_crtc *crtc, u16 *red,
+					u16 *green, u16 *blue, int regno)
+{
+}
+
+static int psbfb_probe(struct drm_fb_helper *helper,
+				struct drm_fb_helper_surface_size *sizes)
+{
+	struct psb_fbdev *psb_fbdev = (struct psb_fbdev *)helper;
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = psbfb_create(psb_fbdev, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+struct drm_fb_helper_funcs psb_fb_helper_funcs = {
+	.gamma_set = psbfb_gamma_set,
+	.gamma_get = psbfb_gamma_get,
+	.fb_probe = psbfb_probe,
+};
+
+int psb_fbdev_destroy(struct drm_device *dev, struct psb_fbdev *fbdev)
+{
+	struct fb_info *info;
+	struct psb_framebuffer *psbfb = &fbdev->pfb;
+
+	if (fbdev->psb_fb_helper.fbdev) {
+		info = fbdev->psb_fb_helper.fbdev;
+		unregister_framebuffer(info);
+		if (info->cmap.len)
+			fb_dealloc_cmap(&info->cmap);
+		framebuffer_release(info);
+	}
+	drm_fb_helper_fini(&fbdev->psb_fb_helper);
+	drm_framebuffer_cleanup(&psbfb->base);
+
+	if (psbfb->gtt)
+		drm_gem_object_unreference(&psbfb->gtt->gem);
+	return 0;
+}
+
+int psb_fbdev_init(struct drm_device *dev)
+{
+	struct psb_fbdev *fbdev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	fbdev = kzalloc(sizeof(struct psb_fbdev), GFP_KERNEL);
+	if (!fbdev) {
+		dev_err(dev->dev, "no memory\n");
+		return -ENOMEM;
+	}
+
+	dev_priv->fbdev = fbdev;
+	fbdev->psb_fb_helper.funcs = &psb_fb_helper_funcs;
+
+	drm_fb_helper_init(dev, &fbdev->psb_fb_helper, dev_priv->ops->crtcs,
+							INTELFB_CONN_LIMIT);
+
+	drm_fb_helper_single_add_all_connectors(&fbdev->psb_fb_helper);
+	drm_fb_helper_initial_config(&fbdev->psb_fb_helper, 32);
+	return 0;
+}
+
+void psb_fbdev_fini(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (!dev_priv->fbdev)
+		return;
+
+	psb_fbdev_destroy(dev, dev_priv->fbdev);
+	kfree(dev_priv->fbdev);
+	dev_priv->fbdev = NULL;
+}
+
+static void psbfb_output_poll_changed(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_fbdev *fbdev = (struct psb_fbdev *)dev_priv->fbdev;
+	drm_fb_helper_hotplug_event(&fbdev->psb_fb_helper);
+}
+
+/**
+ *	psb_user_framebuffer_create_handle - add hamdle to a framebuffer
+ *	@fb: framebuffer
+ *	@file_priv: our DRM file
+ *	@handle: returned handle
+ *
+ *	Our framebuffer object is a GTT range which also contains a GEM
+ *	object. We need to turn it into a handle for userspace. GEM will do
+ *	the work for us
+ */
+static int psb_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+					      struct drm_file *file_priv,
+					      unsigned int *handle)
+{
+	struct psb_framebuffer *psbfb = to_psb_fb(fb);
+	struct gtt_range *r = psbfb->gtt;
+	return drm_gem_handle_create(file_priv, &r->gem, handle);
+}
+
+/**
+ *	psb_user_framebuffer_destroy	-	destruct user created fb
+ *	@fb: framebuffer
+ *
+ *	User framebuffers are backed by GEM objects so all we have to do is
+ *	clean up a bit and drop the reference, GEM will handle the fallout
+ */
+static void psb_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct psb_framebuffer *psbfb = to_psb_fb(fb);
+	struct gtt_range *r = psbfb->gtt;
+	struct drm_device *dev = fb->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_fbdev *fbdev = dev_priv->fbdev;
+	struct drm_crtc *crtc;
+	int reset = 0;
+
+	/* Should never get stolen memory for a user fb */
+	WARN_ON(r->stolen);
+
+	/* Check if we are erroneously live */
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+		if (crtc->fb == fb)
+			reset = 1;
+
+	if (reset)
+		/*
+		 * Now force a sane response before we permit the DRM CRTC
+		 * layer to do stupid things like blank the display. Instead
+		 * we reset this framebuffer as if the user had forced a reset.
+		 * We must do this before the cleanup so that the DRM layer
+		 * doesn't get a chance to stick its oar in where it isn't
+		 * wanted.
+		 */
+		drm_fb_helper_restore_fbdev_mode(&fbdev->psb_fb_helper);
+
+	/* Let DRM do its clean up */
+	drm_framebuffer_cleanup(fb);
+	/*  We are no longer using the resource in GEM */
+	drm_gem_object_unreference_unlocked(&r->gem);
+	kfree(fb);
+}
+
+static const struct drm_mode_config_funcs psb_mode_funcs = {
+	.fb_create = psb_user_framebuffer_create,
+	.output_poll_changed = psbfb_output_poll_changed,
+};
+
+static int psb_create_backlight_property(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct drm_property *backlight;
+
+	if (dev_priv->backlight_property)
+		return 0;
+
+	backlight = drm_property_create(dev, DRM_MODE_PROP_RANGE,
+							"backlight", 2);
+	backlight->values[0] = 0;
+	backlight->values[1] = 100;
+
+	dev_priv->backlight_property = backlight;
+
+	return 0;
+}
+
+static void psb_setup_outputs(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct drm_connector *connector;
+
+	drm_mode_create_scaling_mode_property(dev);
+	psb_create_backlight_property(dev);
+
+	dev_priv->ops->output_init(dev);
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list,
+			    head) {
+		struct psb_intel_encoder *psb_intel_encoder =
+			psb_intel_attached_encoder(connector);
+		struct drm_encoder *encoder = &psb_intel_encoder->base;
+		int crtc_mask = 0, clone_mask = 0;
+
+		/* valid crtcs */
+		switch (psb_intel_encoder->type) {
+		case INTEL_OUTPUT_ANALOG:
+			crtc_mask = (1 << 0);
+			clone_mask = (1 << INTEL_OUTPUT_ANALOG);
+			break;
+		case INTEL_OUTPUT_SDVO:
+			crtc_mask = ((1 << 0) | (1 << 1));
+			clone_mask = (1 << INTEL_OUTPUT_SDVO);
+			break;
+		case INTEL_OUTPUT_LVDS:
+			if (IS_MRST(dev))
+				crtc_mask = (1 << 0);
+			else
+				crtc_mask = (1 << 1);
+			clone_mask = (1 << INTEL_OUTPUT_LVDS);
+			break;
+		case INTEL_OUTPUT_MIPI:
+			crtc_mask = (1 << 0);
+			clone_mask = (1 << INTEL_OUTPUT_MIPI);
+			break;
+		case INTEL_OUTPUT_MIPI2:
+			crtc_mask = (1 << 2);
+			clone_mask = (1 << INTEL_OUTPUT_MIPI2);
+			break;
+		case INTEL_OUTPUT_HDMI:
+			if (IS_MFLD(dev))
+				crtc_mask = (1 << 1);
+			else	
+				crtc_mask = (1 << 0);
+			clone_mask = (1 << INTEL_OUTPUT_HDMI);
+			break;
+		}
+		encoder->possible_crtcs = crtc_mask;
+		encoder->possible_clones =
+		    psb_intel_connector_clones(dev, clone_mask);
+	}
+}
+
+void psb_modeset_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+	int i;
+
+	drm_mode_config_init(dev);
+
+	dev->mode_config.min_width = 0;
+	dev->mode_config.min_height = 0;
+
+	dev->mode_config.funcs = (void *) &psb_mode_funcs;
+
+	/* set memory base */
+	/* Oaktrail and Poulsbo should use BAR 2*/
+	pci_read_config_dword(dev->pdev, PSB_BSM, (u32 *)
+					&(dev->mode_config.fb_base));
+
+	/* num pipes is 2 for PSB but 1 for Mrst */
+	for (i = 0; i < dev_priv->num_pipe; i++)
+		psb_intel_crtc_init(dev, i, mode_dev);
+
+	dev->mode_config.max_width = 2048;
+	dev->mode_config.max_height = 2048;
+
+	psb_setup_outputs(dev);
+}
+
+void psb_modeset_cleanup(struct drm_device *dev)
+{
+	mutex_lock(&dev->struct_mutex);
+
+	drm_kms_helper_poll_fini(dev);
+	psb_fbdev_fini(dev);
+	drm_mode_config_cleanup(dev);
+
+	mutex_unlock(&dev->struct_mutex);
+}
diff --git a/drivers/gpu/drm/gma500/framebuffer.h b/drivers/gpu/drm/gma500/framebuffer.h
new file mode 100644
index 000000000000..989558a9e6ee
--- /dev/null
+++ b/drivers/gpu/drm/gma500/framebuffer.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2008-2011, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ *
+ */
+
+#ifndef _FRAMEBUFFER_H_
+#define _FRAMEBUFFER_H_
+
+#include <drm/drmP.h>
+#include <drm/drm_fb_helper.h>
+
+#include "psb_drv.h"
+
+struct psb_framebuffer {
+	struct drm_framebuffer base;
+	struct address_space *addr_space;
+	struct fb_info *fbdev;
+	struct gtt_range *gtt;
+};
+
+struct psb_fbdev {
+	struct drm_fb_helper psb_fb_helper;
+	struct psb_framebuffer pfb;
+};
+
+#define to_psb_fb(x) container_of(x, struct psb_framebuffer, base)
+
+extern int psb_intel_connector_clones(struct drm_device *dev, int type_mask);
+
+#endif
+
diff --git a/drivers/gpu/drm/gma500/gem.c b/drivers/gpu/drm/gma500/gem.c
new file mode 100644
index 000000000000..9fbb86868e2e
--- /dev/null
+++ b/drivers/gpu/drm/gma500/gem.c
@@ -0,0 +1,292 @@
+/*
+ *  psb GEM interface
+ *
+ * Copyright (c) 2011, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors: Alan Cox
+ *
+ * TODO:
+ *	-	we need to work out if the MMU is relevant (eg for
+ *		accelerated operations on a GEM object)
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "gma_drm.h"
+#include "psb_drv.h"
+
+int psb_gem_init_object(struct drm_gem_object *obj)
+{
+	return -EINVAL;
+}
+
+void psb_gem_free_object(struct drm_gem_object *obj)
+{
+	struct gtt_range *gtt = container_of(obj, struct gtt_range, gem);
+	drm_gem_object_release_wrap(obj);
+	/* This must occur last as it frees up the memory of the GEM object */
+	psb_gtt_free_range(obj->dev, gtt);
+}
+
+int psb_gem_get_aperture(struct drm_device *dev, void *data,
+				struct drm_file *file)
+{
+	return -EINVAL;
+}
+
+/**
+ *	psb_gem_dumb_map_gtt	-	buffer mapping for dumb interface
+ *	@file: our drm client file
+ *	@dev: drm device
+ *	@handle: GEM handle to the object (from dumb_create)
+ *
+ *	Do the necessary setup to allow the mapping of the frame buffer
+ *	into user memory. We don't have to do much here at the moment.
+ */
+int psb_gem_dumb_map_gtt(struct drm_file *file, struct drm_device *dev,
+			 uint32_t handle, uint64_t *offset)
+{
+	int ret = 0;
+	struct drm_gem_object *obj;
+
+	if (!(dev->driver->driver_features & DRIVER_GEM))
+		return -ENODEV;
+
+	mutex_lock(&dev->struct_mutex);
+
+	/* GEM does all our handle to object mapping */
+	obj = drm_gem_object_lookup(dev, file, handle);
+	if (obj == NULL) {
+		ret = -ENOENT;
+		goto unlock;
+	}
+	/* What validation is needed here ? */
+
+	/* Make it mmapable */
+	if (!obj->map_list.map) {
+		ret = gem_create_mmap_offset(obj);
+		if (ret)
+			goto out;
+	}
+	/* GEM should really work out the hash offsets for us */
+	*offset = (u64)obj->map_list.hash.key << PAGE_SHIFT;
+out:
+	drm_gem_object_unreference(obj);
+unlock:
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
+}
+
+/**
+ *	psb_gem_create		-	create a mappable object
+ *	@file: the DRM file of the client
+ *	@dev: our device
+ *	@size: the size requested
+ *	@handlep: returned handle (opaque number)
+ *
+ *	Create a GEM object, fill in the boilerplate and attach a handle to
+ *	it so that userspace can speak about it. This does the core work
+ *	for the various methods that do/will create GEM objects for things
+ */
+static int psb_gem_create(struct drm_file *file,
+	struct drm_device *dev, uint64_t size, uint32_t *handlep)
+{
+	struct gtt_range *r;
+	int ret;
+	u32 handle;
+
+	size = roundup(size, PAGE_SIZE);
+
+	/* Allocate our object - for now a direct gtt range which is not
+	   stolen memory backed */
+	r = psb_gtt_alloc_range(dev, size, "gem", 0);
+	if (r == NULL) {
+		dev_err(dev->dev, "no memory for %lld byte GEM object\n", size);
+		return -ENOSPC;
+	}
+	/* Initialize the extra goodies GEM needs to do all the hard work */
+	if (drm_gem_object_init(dev, &r->gem, size) != 0) {
+		psb_gtt_free_range(dev, r);
+		/* GEM doesn't give an error code so use -ENOMEM */
+		dev_err(dev->dev, "GEM init failed for %lld\n", size);
+		return -ENOMEM;
+	}
+	/* Give the object a handle so we can carry it more easily */
+	ret = drm_gem_handle_create(file, &r->gem, &handle);
+	if (ret) {
+		dev_err(dev->dev, "GEM handle failed for %p, %lld\n",
+							&r->gem, size);
+		drm_gem_object_release(&r->gem);
+		psb_gtt_free_range(dev, r);
+		return ret;
+	}
+	/* We have the initial and handle reference but need only one now */
+	drm_gem_object_unreference(&r->gem);
+	*handlep = handle;
+	return 0;
+}
+
+/**
+ *	psb_gem_dumb_create	-	create a dumb buffer
+ *	@drm_file: our client file
+ *	@dev: our device
+ *	@args: the requested arguments copied from userspace
+ *
+ *	Allocate a buffer suitable for use for a frame buffer of the
+ *	form described by user space. Give userspace a handle by which
+ *	to reference it.
+ */
+int psb_gem_dumb_create(struct drm_file *file, struct drm_device *dev,
+			struct drm_mode_create_dumb *args)
+{
+	args->pitch = ALIGN(args->width * ((args->bpp + 7) / 8), 64);
+	args->size = args->pitch * args->height;
+	return psb_gem_create(file, dev, args->size, &args->handle);
+}
+
+/**
+ *	psb_gem_dumb_destroy	-	destroy a dumb buffer
+ *	@file: client file
+ *	@dev: our DRM device
+ *	@handle: the object handle
+ *
+ *	Destroy a handle that was created via psb_gem_dumb_create, at least
+ *	we hope it was created that way. i915 seems to assume the caller
+ *	does the checking but that might be worth review ! FIXME
+ */
+int psb_gem_dumb_destroy(struct drm_file *file, struct drm_device *dev,
+			uint32_t handle)
+{
+	/* No special work needed, drop the reference and see what falls out */
+	return drm_gem_handle_delete(file, handle);
+}
+
+/**
+ *	psb_gem_fault		-	pagefault handler for GEM objects
+ *	@vma: the VMA of the GEM object
+ *	@vmf: fault detail
+ *
+ *	Invoked when a fault occurs on an mmap of a GEM managed area. GEM
+ *	does most of the work for us including the actual map/unmap calls
+ *	but we need to do the actual page work.
+ *
+ *	This code eventually needs to handle faulting objects in and out
+ *	of the GTT and repacking it when we run out of space. We can put
+ *	that off for now and for our simple uses
+ *
+ *	The VMA was set up by GEM. In doing so it also ensured that the
+ *	vma->vm_private_data points to the GEM object that is backing this
+ *	mapping.
+ */
+int psb_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct drm_gem_object *obj;
+	struct gtt_range *r;
+	int ret;
+	unsigned long pfn;
+	pgoff_t page_offset;
+	struct drm_device *dev;
+	struct drm_psb_private *dev_priv;
+
+	obj = vma->vm_private_data;	/* GEM object */
+	dev = obj->dev;
+	dev_priv = dev->dev_private;
+
+	r = container_of(obj, struct gtt_range, gem);	/* Get the gtt range */
+
+	/* Make sure we don't parallel update on a fault, nor move or remove
+	   something from beneath our feet */
+	mutex_lock(&dev->struct_mutex);
+
+	/* For now the mmap pins the object and it stays pinned. As things
+	   stand that will do us no harm */
+	if (r->mmapping == 0) {
+		ret = psb_gtt_pin(r);
+		if (ret < 0) {
+			dev_err(dev->dev, "gma500: pin failed: %d\n", ret);
+			goto fail;
+		}
+		r->mmapping = 1;
+	}
+
+	/* Page relative to the VMA start - we must calculate this ourselves
+	   because vmf->pgoff is the fake GEM offset */
+	page_offset = ((unsigned long) vmf->virtual_address - vma->vm_start)
+				>> PAGE_SHIFT;
+
+	/* CPU view of the page, don't go via the GART for CPU writes */
+	if (r->stolen)
+		pfn = (dev_priv->stolen_base + r->offset) >> PAGE_SHIFT;
+	else
+		pfn = page_to_pfn(r->pages[page_offset]);
+	ret = vm_insert_pfn(vma, (unsigned long)vmf->virtual_address, pfn);
+
+fail:
+	mutex_unlock(&dev->struct_mutex);
+	switch (ret) {
+	case 0:
+	case -ERESTARTSYS:
+	case -EINTR:
+		return VM_FAULT_NOPAGE;
+	case -ENOMEM:
+		return VM_FAULT_OOM;
+	default:
+		return VM_FAULT_SIGBUS;
+	}
+}
+
+static int psb_gem_create_stolen(struct drm_file *file, struct drm_device *dev,
+						int size, u32 *handle)
+{
+	struct gtt_range *gtt = psb_gtt_alloc_range(dev, size, "gem", 1);
+	if (gtt == NULL)
+		return -ENOMEM;
+	if (drm_gem_private_object_init(dev, &gtt->gem, size) != 0)
+		goto free_gtt;
+	if (drm_gem_handle_create(file, &gtt->gem, handle) == 0)
+		return 0;
+free_gtt:
+	psb_gtt_free_range(dev, gtt);
+	return -ENOMEM;
+}
+
+/*
+ *	GEM interfaces for our specific client
+ */
+int psb_gem_create_ioctl(struct drm_device *dev, void *data,
+					struct drm_file *file)
+{
+	struct drm_psb_gem_create *args = data;
+	int ret;
+	if (args->flags & GMA_GEM_CREATE_STOLEN) {
+		ret = psb_gem_create_stolen(file, dev, args->size,
+							&args->handle);
+		if (ret == 0)
+			return 0;
+		/* Fall throguh */
+		args->flags &= ~GMA_GEM_CREATE_STOLEN;
+	}
+	return psb_gem_create(file, dev, args->size, &args->handle);
+}
+
+int psb_gem_mmap_ioctl(struct drm_device *dev, void *data,
+					struct drm_file *file)
+{
+	struct drm_psb_gem_mmap *args = data;
+	return dev->driver->dumb_map_offset(file, dev,
+						args->handle, &args->offset);
+}
+
diff --git a/drivers/gpu/drm/gma500/gem_glue.c b/drivers/gpu/drm/gma500/gem_glue.c
new file mode 100644
index 000000000000..daac12120653
--- /dev/null
+++ b/drivers/gpu/drm/gma500/gem_glue.c
@@ -0,0 +1,89 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+
+void drm_gem_object_release_wrap(struct drm_gem_object *obj)
+{
+	/* Remove the list map if one is present */
+	if (obj->map_list.map) {
+		struct drm_gem_mm *mm = obj->dev->mm_private;
+		struct drm_map_list *list = &obj->map_list;
+		drm_ht_remove_item(&mm->offset_hash, &list->hash);
+		drm_mm_put_block(list->file_offset_node);
+		kfree(list->map);
+		list->map = NULL;
+	}
+	drm_gem_object_release(obj);
+}
+
+/**
+ *	gem_create_mmap_offset		-	invent an mmap offset
+ *	@obj: our object
+ *
+ *	Standard implementation of offset generation for mmap as is
+ *	duplicated in several drivers. This belongs in GEM.
+ */
+int gem_create_mmap_offset(struct drm_gem_object *obj)
+{
+	struct drm_device *dev = obj->dev;
+	struct drm_gem_mm *mm = dev->mm_private;
+	struct drm_map_list *list;
+	struct drm_local_map *map;
+	int ret;
+
+	list = &obj->map_list;
+	list->map = kzalloc(sizeof(struct drm_map_list), GFP_KERNEL);
+	if (list->map == NULL)
+		return -ENOMEM;
+	map = list->map;
+	map->type = _DRM_GEM;
+	map->size = obj->size;
+	map->handle = obj;
+
+	list->file_offset_node = drm_mm_search_free(&mm->offset_manager,
+					obj->size / PAGE_SIZE, 0, 0);
+	if (!list->file_offset_node) {
+		dev_err(dev->dev, "failed to allocate offset for bo %d\n",
+								obj->name);
+		ret = -ENOSPC;
+		goto free_it;
+	}
+	list->file_offset_node = drm_mm_get_block(list->file_offset_node,
+					obj->size / PAGE_SIZE, 0);
+	if (!list->file_offset_node) {
+		ret = -ENOMEM;
+		goto free_it;
+	}
+	list->hash.key = list->file_offset_node->start;
+	ret = drm_ht_insert_item(&mm->offset_hash, &list->hash);
+	if (ret) {
+		dev_err(dev->dev, "failed to add to map hash\n");
+		goto free_mm;
+	}
+	return 0;
+
+free_mm:
+	drm_mm_put_block(list->file_offset_node);
+free_it:
+	kfree(list->map);
+	list->map = NULL;
+	return ret;
+}
diff --git a/drivers/gpu/drm/gma500/gem_glue.h b/drivers/gpu/drm/gma500/gem_glue.h
new file mode 100644
index 000000000000..ce5ce30f74db
--- /dev/null
+++ b/drivers/gpu/drm/gma500/gem_glue.h
@@ -0,0 +1,2 @@
+extern void drm_gem_object_release_wrap(struct drm_gem_object *obj);
+extern int gem_create_mmap_offset(struct drm_gem_object *obj);
diff --git a/drivers/gpu/drm/gma500/gtt.c b/drivers/gpu/drm/gma500/gtt.c
new file mode 100644
index 000000000000..e770bd190a5c
--- /dev/null
+++ b/drivers/gpu/drm/gma500/gtt.c
@@ -0,0 +1,553 @@
+/*
+ * Copyright (c) 2007, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors: Thomas Hellstrom <thomas-at-tungstengraphics.com>
+ *	    Alan Cox <alan@linux.intel.com>
+ */
+
+#include <drm/drmP.h>
+#include "psb_drv.h"
+
+
+/*
+ *	GTT resource allocator - manage page mappings in GTT space
+ */
+
+/**
+ *	psb_gtt_mask_pte	-	generate GTT pte entry
+ *	@pfn: page number to encode
+ *	@type: type of memory in the GTT
+ *
+ *	Set the GTT entry for the appropriate memory type.
+ */
+static inline uint32_t psb_gtt_mask_pte(uint32_t pfn, int type)
+{
+	uint32_t mask = PSB_PTE_VALID;
+
+	if (type & PSB_MMU_CACHED_MEMORY)
+		mask |= PSB_PTE_CACHED;
+	if (type & PSB_MMU_RO_MEMORY)
+		mask |= PSB_PTE_RO;
+	if (type & PSB_MMU_WO_MEMORY)
+		mask |= PSB_PTE_WO;
+
+	return (pfn << PAGE_SHIFT) | mask;
+}
+
+/**
+ *	psb_gtt_entry		-	find the GTT entries for a gtt_range
+ *	@dev: our DRM device
+ *	@r: our GTT range
+ *
+ *	Given a gtt_range object return the GTT offset of the page table
+ *	entries for this gtt_range
+ */
+u32 *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned long offset;
+
+	offset = r->resource.start - dev_priv->gtt_mem->start;
+
+	return dev_priv->gtt_map + (offset >> PAGE_SHIFT);
+}
+
+/**
+ *	psb_gtt_insert	-	put an object into the GTT
+ *	@dev: our DRM device
+ *	@r: our GTT range
+ *
+ *	Take our preallocated GTT range and insert the GEM object into
+ *	the GTT. This is protected via the gtt mutex which the caller
+ *	must hold.
+ */
+static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
+{
+	u32 *gtt_slot, pte;
+	struct page **pages;
+	int i;
+
+	if (r->pages == NULL) {
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	WARN_ON(r->stolen);	/* refcount these maybe ? */
+
+	gtt_slot = psb_gtt_entry(dev, r);
+	pages = r->pages;
+
+	/* Make sure changes are visible to the GPU */
+	set_pages_array_uc(pages, r->npage);
+
+	/* Write our page entries into the GTT itself */
+	for (i = r->roll; i < r->npage; i++) {
+		pte = psb_gtt_mask_pte(page_to_pfn(r->pages[i]), 0);
+		iowrite32(pte, gtt_slot++);
+	}
+	for (i = 0; i < r->roll; i++) {
+		pte = psb_gtt_mask_pte(page_to_pfn(r->pages[i]), 0);
+		iowrite32(pte, gtt_slot++);
+	}
+	/* Make sure all the entries are set before we return */
+	ioread32(gtt_slot - 1);
+
+	return 0;
+}
+
+/**
+ *	psb_gtt_remove	-	remove an object from the GTT
+ *	@dev: our DRM device
+ *	@r: our GTT range
+ *
+ *	Remove a preallocated GTT range from the GTT. Overwrite all the
+ *	page table entries with the dummy page. This is protected via the gtt
+ *	mutex which the caller must hold.
+ */
+static void psb_gtt_remove(struct drm_device *dev, struct gtt_range *r)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 *gtt_slot, pte;
+	int i;
+
+	WARN_ON(r->stolen);
+
+	gtt_slot = psb_gtt_entry(dev, r);
+	pte = psb_gtt_mask_pte(page_to_pfn(dev_priv->scratch_page), 0);
+
+	for (i = 0; i < r->npage; i++)
+		iowrite32(pte, gtt_slot++);
+	ioread32(gtt_slot - 1);
+	set_pages_array_wb(r->pages, r->npage);
+}
+
+/**
+ *	psb_gtt_roll	-	set scrolling position
+ *	@dev: our DRM device
+ *	@r: the gtt mapping we are using
+ *	@roll: roll offset
+ *
+ *	Roll an existing pinned mapping by moving the pages through the GTT.
+ *	This allows us to implement hardware scrolling on the consoles without
+ *	a 2D engine
+ */
+void psb_gtt_roll(struct drm_device *dev, struct gtt_range *r, int roll)
+{
+	u32 *gtt_slot, pte;
+	int i;
+
+	if (roll >= r->npage) {
+		WARN_ON(1);
+		return;
+	}
+
+	r->roll = roll;
+
+	/* Not currently in the GTT - no worry we will write the mapping at
+	   the right position when it gets pinned */
+	if (!r->stolen && !r->in_gart)
+		return;
+
+	gtt_slot = psb_gtt_entry(dev, r);
+
+	for (i = r->roll; i < r->npage; i++) {
+		pte = psb_gtt_mask_pte(page_to_pfn(r->pages[i]), 0);
+		iowrite32(pte, gtt_slot++);
+	}
+	for (i = 0; i < r->roll; i++) {
+		pte = psb_gtt_mask_pte(page_to_pfn(r->pages[i]), 0);
+		iowrite32(pte, gtt_slot++);
+	}
+	ioread32(gtt_slot - 1);
+}
+
+/**
+ *	psb_gtt_attach_pages	-	attach and pin GEM pages
+ *	@gt: the gtt range
+ *
+ *	Pin and build an in kernel list of the pages that back our GEM object.
+ *	While we hold this the pages cannot be swapped out. This is protected
+ *	via the gtt mutex which the caller must hold.
+ */
+static int psb_gtt_attach_pages(struct gtt_range *gt)
+{
+	struct inode *inode;
+	struct address_space *mapping;
+	int i;
+	struct page *p;
+	int pages = gt->gem.size / PAGE_SIZE;
+
+	WARN_ON(gt->pages);
+
+	/* This is the shared memory object that backs the GEM resource */
+	inode = gt->gem.filp->f_path.dentry->d_inode;
+	mapping = inode->i_mapping;
+
+	gt->pages = kmalloc(pages * sizeof(struct page *), GFP_KERNEL);
+	if (gt->pages == NULL)
+		return -ENOMEM;
+	gt->npage = pages;
+
+	for (i = 0; i < pages; i++) {
+		/* FIXME: needs updating as per mail from Hugh Dickins */
+		p = read_cache_page_gfp(mapping, i,
+					__GFP_COLD | GFP_KERNEL);
+		if (IS_ERR(p))
+			goto err;
+		gt->pages[i] = p;
+	}
+	return 0;
+
+err:
+	while (i--)
+		page_cache_release(gt->pages[i]);
+	kfree(gt->pages);
+	gt->pages = NULL;
+	return PTR_ERR(p);
+}
+
+/**
+ *	psb_gtt_detach_pages	-	attach and pin GEM pages
+ *	@gt: the gtt range
+ *
+ *	Undo the effect of psb_gtt_attach_pages. At this point the pages
+ *	must have been removed from the GTT as they could now be paged out
+ *	and move bus address. This is protected via the gtt mutex which the
+ *	caller must hold.
+ */
+static void psb_gtt_detach_pages(struct gtt_range *gt)
+{
+	int i;
+	for (i = 0; i < gt->npage; i++) {
+		/* FIXME: do we need to force dirty */
+		set_page_dirty(gt->pages[i]);
+		page_cache_release(gt->pages[i]);
+	}
+	kfree(gt->pages);
+	gt->pages = NULL;
+}
+
+/**
+ *	psb_gtt_pin		-	pin pages into the GTT
+ *	@gt: range to pin
+ *
+ *	Pin a set of pages into the GTT. The pins are refcounted so that
+ *	multiple pins need multiple unpins to undo.
+ *
+ *	Non GEM backed objects treat this as a no-op as they are always GTT
+ *	backed objects.
+ */
+int psb_gtt_pin(struct gtt_range *gt)
+{
+	int ret = 0;
+	struct drm_device *dev = gt->gem.dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	mutex_lock(&dev_priv->gtt_mutex);
+
+	if (gt->in_gart == 0 && gt->stolen == 0) {
+		ret = psb_gtt_attach_pages(gt);
+		if (ret < 0)
+			goto out;
+		ret = psb_gtt_insert(dev, gt);
+		if (ret < 0) {
+			psb_gtt_detach_pages(gt);
+			goto out;
+		}
+	}
+	gt->in_gart++;
+out:
+	mutex_unlock(&dev_priv->gtt_mutex);
+	return ret;
+}
+
+/**
+ *	psb_gtt_unpin		-	Drop a GTT pin requirement
+ *	@gt: range to pin
+ *
+ *	Undoes the effect of psb_gtt_pin. On the last drop the GEM object
+ *	will be removed from the GTT which will also drop the page references
+ *	and allow the VM to clean up or page stuff.
+ *
+ *	Non GEM backed objects treat this as a no-op as they are always GTT
+ *	backed objects.
+ */
+void psb_gtt_unpin(struct gtt_range *gt)
+{
+	struct drm_device *dev = gt->gem.dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	mutex_lock(&dev_priv->gtt_mutex);
+
+	WARN_ON(!gt->in_gart);
+
+	gt->in_gart--;
+	if (gt->in_gart == 0 && gt->stolen == 0) {
+		psb_gtt_remove(dev, gt);
+		psb_gtt_detach_pages(gt);
+	}
+	mutex_unlock(&dev_priv->gtt_mutex);
+}
+
+/*
+ *	GTT resource allocator - allocate and manage GTT address space
+ */
+
+/**
+ *	psb_gtt_alloc_range	-	allocate GTT address space
+ *	@dev: Our DRM device
+ *	@len: length (bytes) of address space required
+ *	@name: resource name
+ *	@backed: resource should be backed by stolen pages
+ *
+ *	Ask the kernel core to find us a suitable range of addresses
+ *	to use for a GTT mapping.
+ *
+ *	Returns a gtt_range structure describing the object, or NULL on
+ *	error. On successful return the resource is both allocated and marked
+ *	as in use.
+ */
+struct gtt_range *psb_gtt_alloc_range(struct drm_device *dev, int len,
+						const char *name, int backed)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct gtt_range *gt;
+	struct resource *r = dev_priv->gtt_mem;
+	int ret;
+	unsigned long start, end;
+
+	if (backed) {
+		/* The start of the GTT is the stolen pages */
+		start = r->start;
+		end = r->start + dev_priv->gtt.stolen_size - 1;
+	} else {
+		/* The rest we will use for GEM backed objects */
+		start = r->start + dev_priv->gtt.stolen_size;
+		end = r->end;
+	}
+
+	gt = kzalloc(sizeof(struct gtt_range), GFP_KERNEL);
+	if (gt == NULL)
+		return NULL;
+	gt->resource.name = name;
+	gt->stolen = backed;
+	gt->in_gart = backed;
+	gt->roll = 0;
+	/* Ensure this is set for non GEM objects */
+	gt->gem.dev = dev;
+	ret = allocate_resource(dev_priv->gtt_mem, &gt->resource,
+				len, start, end, PAGE_SIZE, NULL, NULL);
+	if (ret == 0) {
+		gt->offset = gt->resource.start - r->start;
+		return gt;
+	}
+	kfree(gt);
+	return NULL;
+}
+
+/**
+ *	psb_gtt_free_range	-	release GTT address space
+ *	@dev: our DRM device
+ *	@gt: a mapping created with psb_gtt_alloc_range
+ *
+ *	Release a resource that was allocated with psb_gtt_alloc_range. If the
+ *	object has been pinned by mmap users we clean this up here currently.
+ */
+void psb_gtt_free_range(struct drm_device *dev, struct gtt_range *gt)
+{
+	/* Undo the mmap pin if we are destroying the object */
+	if (gt->mmapping) {
+		psb_gtt_unpin(gt);
+		gt->mmapping = 0;
+	}
+	WARN_ON(gt->in_gart && !gt->stolen);
+	release_resource(&gt->resource);
+	kfree(gt);
+}
+
+void psb_gtt_alloc(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	init_rwsem(&dev_priv->gtt.sem);
+}
+
+void psb_gtt_takedown(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->gtt_map) {
+		iounmap(dev_priv->gtt_map);
+		dev_priv->gtt_map = NULL;
+	}
+	if (dev_priv->gtt_initialized) {
+		pci_write_config_word(dev->pdev, PSB_GMCH_CTRL,
+				      dev_priv->gmch_ctrl);
+		PSB_WVDC32(dev_priv->pge_ctl, PSB_PGETBL_CTL);
+		(void) PSB_RVDC32(PSB_PGETBL_CTL);
+	}
+	if (dev_priv->vram_addr)
+		iounmap(dev_priv->gtt_map);
+}
+
+int psb_gtt_init(struct drm_device *dev, int resume)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned gtt_pages;
+	unsigned long stolen_size, vram_stolen_size;
+	unsigned i, num_pages;
+	unsigned pfn_base;
+	uint32_t vram_pages;
+	uint32_t dvmt_mode = 0;
+	struct psb_gtt *pg;
+
+	int ret = 0;
+	uint32_t pte;
+
+	mutex_init(&dev_priv->gtt_mutex);
+
+	psb_gtt_alloc(dev);
+	pg = &dev_priv->gtt;
+
+	/* Enable the GTT */
+	pci_read_config_word(dev->pdev, PSB_GMCH_CTRL, &dev_priv->gmch_ctrl);
+	pci_write_config_word(dev->pdev, PSB_GMCH_CTRL,
+			      dev_priv->gmch_ctrl | _PSB_GMCH_ENABLED);
+
+	dev_priv->pge_ctl = PSB_RVDC32(PSB_PGETBL_CTL);
+	PSB_WVDC32(dev_priv->pge_ctl | _PSB_PGETBL_ENABLED, PSB_PGETBL_CTL);
+	(void) PSB_RVDC32(PSB_PGETBL_CTL);
+
+	/* The root resource we allocate address space from */
+	dev_priv->gtt_initialized = 1;
+
+	pg->gtt_phys_start = dev_priv->pge_ctl & PAGE_MASK;
+
+	/*
+	 *	The video mmu has a hw bug when accessing 0x0D0000000.
+	 *	Make gatt start at 0x0e000,0000. This doesn't actually
+	 *	matter for us but may do if the video acceleration ever
+	 *	gets opened up.
+	 */
+	pg->mmu_gatt_start = 0xE0000000;
+
+	pg->gtt_start = pci_resource_start(dev->pdev, PSB_GTT_RESOURCE);
+	gtt_pages = pci_resource_len(dev->pdev, PSB_GTT_RESOURCE)
+								>> PAGE_SHIFT;
+	/* Some CDV firmware doesn't report this currently. In which case the
+	   system has 64 gtt pages */
+	if (pg->gtt_start == 0 || gtt_pages == 0) {
+		dev_err(dev->dev, "GTT PCI BAR not initialized.\n");
+		gtt_pages = 64;
+		pg->gtt_start = dev_priv->pge_ctl;
+	}
+
+	pg->gatt_start = pci_resource_start(dev->pdev, PSB_GATT_RESOURCE);
+	pg->gatt_pages = pci_resource_len(dev->pdev, PSB_GATT_RESOURCE)
+								>> PAGE_SHIFT;
+	dev_priv->gtt_mem = &dev->pdev->resource[PSB_GATT_RESOURCE];
+
+	if (pg->gatt_pages == 0 || pg->gatt_start == 0) {
+		static struct resource fudge;	/* Preferably peppermint */
+		/* This can occur on CDV SDV systems. Fudge it in this case.
+		   We really don't care what imaginary space is being allocated
+		   at this point */
+		dev_err(dev->dev, "GATT PCI BAR not initialized.\n");
+		pg->gatt_start = 0x40000000;
+		pg->gatt_pages = (128 * 1024 * 1024) >> PAGE_SHIFT;
+		/* This is a little confusing but in fact the GTT is providing
+		   a view from the GPU into memory and not vice versa. As such
+		   this is really allocating space that is not the same as the
+		   CPU address space on CDV */
+		fudge.start = 0x40000000;
+		fudge.end = 0x40000000 + 128 * 1024 * 1024 - 1;
+		fudge.name = "fudge";
+		fudge.flags = IORESOURCE_MEM;
+		dev_priv->gtt_mem = &fudge;
+	}
+
+	pci_read_config_dword(dev->pdev, PSB_BSM, &dev_priv->stolen_base);
+	vram_stolen_size = pg->gtt_phys_start - dev_priv->stolen_base
+								- PAGE_SIZE;
+
+	stolen_size = vram_stolen_size;
+
+	printk(KERN_INFO "Stolen memory information\n");
+	printk(KERN_INFO "       base in RAM: 0x%x\n", dev_priv->stolen_base);
+	printk(KERN_INFO "       size: %luK, calculated by (GTT RAM base) - (Stolen base), seems wrong\n",
+		vram_stolen_size/1024);
+	dvmt_mode = (dev_priv->gmch_ctrl >> 4) & 0x7;
+	printk(KERN_INFO "      the correct size should be: %dM(dvmt mode=%d)\n",
+		(dvmt_mode == 1) ? 1 : (2 << (dvmt_mode - 1)), dvmt_mode);
+
+	if (resume && (gtt_pages != pg->gtt_pages) &&
+	    (stolen_size != pg->stolen_size)) {
+		dev_err(dev->dev, "GTT resume error.\n");
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	pg->gtt_pages = gtt_pages;
+	pg->stolen_size = stolen_size;
+	dev_priv->vram_stolen_size = vram_stolen_size;
+
+	/*
+	 *	Map the GTT and the stolen memory area
+	 */
+	dev_priv->gtt_map = ioremap_nocache(pg->gtt_phys_start,
+						gtt_pages << PAGE_SHIFT);
+	if (!dev_priv->gtt_map) {
+		dev_err(dev->dev, "Failure to map gtt.\n");
+		ret = -ENOMEM;
+		goto out_err;
+	}
+
+	dev_priv->vram_addr = ioremap_wc(dev_priv->stolen_base, stolen_size);
+	if (!dev_priv->vram_addr) {
+		dev_err(dev->dev, "Failure to map stolen base.\n");
+		ret = -ENOMEM;
+		goto out_err;
+	}
+
+	/*
+	 * Insert vram stolen pages into the GTT
+	 */
+
+	pfn_base = dev_priv->stolen_base >> PAGE_SHIFT;
+	vram_pages = num_pages = vram_stolen_size >> PAGE_SHIFT;
+	printk(KERN_INFO"Set up %d stolen pages starting at 0x%08x, GTT offset %dK\n",
+		num_pages, pfn_base << PAGE_SHIFT, 0);
+	for (i = 0; i < num_pages; ++i) {
+		pte = psb_gtt_mask_pte(pfn_base + i, 0);
+		iowrite32(pte, dev_priv->gtt_map + i);
+	}
+
+	/*
+	 * Init rest of GTT to the scratch page to avoid accidents or scribbles
+	 */
+
+	pfn_base = page_to_pfn(dev_priv->scratch_page);
+	pte = psb_gtt_mask_pte(pfn_base, 0);
+	for (; i < gtt_pages; ++i)
+		iowrite32(pte, dev_priv->gtt_map + i);
+
+	(void) ioread32(dev_priv->gtt_map + i - 1);
+	return 0;
+
+out_err:
+	psb_gtt_takedown(dev);
+	return ret;
+}
diff --git a/drivers/gpu/drm/gma500/gtt.h b/drivers/gpu/drm/gma500/gtt.h
new file mode 100644
index 000000000000..aa1742387f5a
--- /dev/null
+++ b/drivers/gpu/drm/gma500/gtt.h
@@ -0,0 +1,64 @@
+/**************************************************************************
+ * Copyright (c) 2007-2008, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+#ifndef _PSB_GTT_H_
+#define _PSB_GTT_H_
+
+#include <drm/drmP.h>
+
+/* This wants cleaning up with respect to the psb_dev and un-needed stuff */
+struct psb_gtt {
+	uint32_t gatt_start;
+	uint32_t mmu_gatt_start;
+	uint32_t gtt_start;
+	uint32_t gtt_phys_start;
+	unsigned gtt_pages;
+	unsigned gatt_pages;
+	unsigned long stolen_size;
+	unsigned long vram_stolen_size;
+	struct rw_semaphore sem;
+};
+
+/* Exported functions */
+extern int psb_gtt_init(struct drm_device *dev, int resume);
+extern void psb_gtt_takedown(struct drm_device *dev);
+
+/* Each gtt_range describes an allocation in the GTT area */
+struct gtt_range {
+	struct resource resource;	/* Resource for our allocation */
+	u32 offset;			/* GTT offset of our object */
+	struct drm_gem_object gem;	/* GEM high level stuff */
+	int in_gart;			/* Currently in the GART (ref ct) */
+	bool stolen;			/* Backed from stolen RAM */
+	bool mmapping;			/* Is mmappable */
+	struct page **pages;		/* Backing pages if present */
+	int npage;			/* Number of backing pages */
+	int roll;			/* Roll applied to the GTT entries */
+};
+
+extern struct gtt_range *psb_gtt_alloc_range(struct drm_device *dev, int len,
+						const char *name, int backed);
+extern void psb_gtt_kref_put(struct gtt_range *gt);
+extern void psb_gtt_free_range(struct drm_device *dev, struct gtt_range *gt);
+extern int psb_gtt_pin(struct gtt_range *gt);
+extern void psb_gtt_unpin(struct gtt_range *gt);
+extern void psb_gtt_roll(struct drm_device *dev,
+					struct gtt_range *gt, int roll);
+
+#endif
diff --git a/drivers/gpu/drm/gma500/intel_bios.c b/drivers/gpu/drm/gma500/intel_bios.c
new file mode 100644
index 000000000000..d4d0c5b8bf91
--- /dev/null
+++ b/drivers/gpu/drm/gma500/intel_bios.c
@@ -0,0 +1,303 @@
+/*
+ * Copyright (c) 2006 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *    Eric Anholt <eric@anholt.net>
+ *
+ */
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "gma_drm.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "intel_bios.h"
+
+
+static void *find_section(struct bdb_header *bdb, int section_id)
+{
+	u8 *base = (u8 *)bdb;
+	int index = 0;
+	u16 total, current_size;
+	u8 current_id;
+
+	/* skip to first section */
+	index += bdb->header_size;
+	total = bdb->bdb_size;
+
+	/* walk the sections looking for section_id */
+	while (index < total) {
+		current_id = *(base + index);
+		index++;
+		current_size = *((u16 *)(base + index));
+		index += 2;
+		if (current_id == section_id)
+			return base + index;
+		index += current_size;
+	}
+
+	return NULL;
+}
+
+static void fill_detail_timing_data(struct drm_display_mode *panel_fixed_mode,
+			struct lvds_dvo_timing *dvo_timing)
+{
+	panel_fixed_mode->hdisplay = (dvo_timing->hactive_hi << 8) |
+		dvo_timing->hactive_lo;
+	panel_fixed_mode->hsync_start = panel_fixed_mode->hdisplay +
+		((dvo_timing->hsync_off_hi << 8) | dvo_timing->hsync_off_lo);
+	panel_fixed_mode->hsync_end = panel_fixed_mode->hsync_start +
+		dvo_timing->hsync_pulse_width;
+	panel_fixed_mode->htotal = panel_fixed_mode->hdisplay +
+		((dvo_timing->hblank_hi << 8) | dvo_timing->hblank_lo);
+
+	panel_fixed_mode->vdisplay = (dvo_timing->vactive_hi << 8) |
+		dvo_timing->vactive_lo;
+	panel_fixed_mode->vsync_start = panel_fixed_mode->vdisplay +
+		dvo_timing->vsync_off;
+	panel_fixed_mode->vsync_end = panel_fixed_mode->vsync_start +
+		dvo_timing->vsync_pulse_width;
+	panel_fixed_mode->vtotal = panel_fixed_mode->vdisplay +
+		((dvo_timing->vblank_hi << 8) | dvo_timing->vblank_lo);
+	panel_fixed_mode->clock = dvo_timing->clock * 10;
+	panel_fixed_mode->type = DRM_MODE_TYPE_PREFERRED;
+
+	/* Some VBTs have bogus h/vtotal values */
+	if (panel_fixed_mode->hsync_end > panel_fixed_mode->htotal)
+		panel_fixed_mode->htotal = panel_fixed_mode->hsync_end + 1;
+	if (panel_fixed_mode->vsync_end > panel_fixed_mode->vtotal)
+		panel_fixed_mode->vtotal = panel_fixed_mode->vsync_end + 1;
+
+	drm_mode_set_name(panel_fixed_mode);
+}
+
+static void parse_backlight_data(struct drm_psb_private *dev_priv,
+				struct bdb_header *bdb)
+{
+	struct bdb_lvds_backlight *vbt_lvds_bl = NULL;
+	struct bdb_lvds_backlight *lvds_bl;
+	u8 p_type = 0;
+	void *bl_start = NULL;
+	struct bdb_lvds_options *lvds_opts
+				= find_section(bdb, BDB_LVDS_OPTIONS);
+
+	dev_priv->lvds_bl = NULL;
+
+	if (lvds_opts)
+		p_type = lvds_opts->panel_type;
+	else
+		return;
+
+	bl_start = find_section(bdb, BDB_LVDS_BACKLIGHT);
+	vbt_lvds_bl = (struct bdb_lvds_backlight *)(bl_start + 1) + p_type;
+
+	lvds_bl = kzalloc(sizeof(*vbt_lvds_bl), GFP_KERNEL);
+	if (!lvds_bl) {
+		dev_err(dev_priv->dev->dev, "out of memory for backlight data\n");
+		return;
+	}
+	memcpy(lvds_bl, vbt_lvds_bl, sizeof(*vbt_lvds_bl));
+	dev_priv->lvds_bl = lvds_bl;
+}
+
+/* Try to find integrated panel data */
+static void parse_lfp_panel_data(struct drm_psb_private *dev_priv,
+			    struct bdb_header *bdb)
+{
+	struct bdb_lvds_options *lvds_options;
+	struct bdb_lvds_lfp_data *lvds_lfp_data;
+	struct bdb_lvds_lfp_data_entry *entry;
+	struct lvds_dvo_timing *dvo_timing;
+	struct drm_display_mode *panel_fixed_mode;
+
+	/* Defaults if we can't find VBT info */
+	dev_priv->lvds_dither = 0;
+	dev_priv->lvds_vbt = 0;
+
+	lvds_options = find_section(bdb, BDB_LVDS_OPTIONS);
+	if (!lvds_options)
+		return;
+
+	dev_priv->lvds_dither = lvds_options->pixel_dither;
+	if (lvds_options->panel_type == 0xff)
+		return;
+
+	lvds_lfp_data = find_section(bdb, BDB_LVDS_LFP_DATA);
+	if (!lvds_lfp_data)
+		return;
+
+
+	entry = &lvds_lfp_data->data[lvds_options->panel_type];
+	dvo_timing = &entry->dvo_timing;
+
+	panel_fixed_mode = kzalloc(sizeof(*panel_fixed_mode),
+				      GFP_KERNEL);
+	if (panel_fixed_mode == NULL) {
+		dev_err(dev_priv->dev->dev, "out of memory for fixed panel mode\n");
+		return;
+	}
+
+	dev_priv->lvds_vbt = 1;
+	fill_detail_timing_data(panel_fixed_mode, dvo_timing);
+
+	if (panel_fixed_mode->htotal > 0 && panel_fixed_mode->vtotal > 0) {
+		dev_priv->lfp_lvds_vbt_mode = panel_fixed_mode;
+		drm_mode_debug_printmodeline(panel_fixed_mode);
+	} else {
+		dev_dbg(dev_priv->dev->dev, "ignoring invalid LVDS VBT\n");
+		dev_priv->lvds_vbt = 0;
+		kfree(panel_fixed_mode);
+	}
+	return;
+}
+
+/* Try to find sdvo panel data */
+static void parse_sdvo_panel_data(struct drm_psb_private *dev_priv,
+		      struct bdb_header *bdb)
+{
+	struct bdb_sdvo_lvds_options *sdvo_lvds_options;
+	struct lvds_dvo_timing *dvo_timing;
+	struct drm_display_mode *panel_fixed_mode;
+
+	dev_priv->sdvo_lvds_vbt_mode = NULL;
+
+	sdvo_lvds_options = find_section(bdb, BDB_SDVO_LVDS_OPTIONS);
+	if (!sdvo_lvds_options)
+		return;
+
+	dvo_timing = find_section(bdb, BDB_SDVO_PANEL_DTDS);
+	if (!dvo_timing)
+		return;
+
+	panel_fixed_mode = kzalloc(sizeof(*panel_fixed_mode), GFP_KERNEL);
+
+	if (!panel_fixed_mode)
+		return;
+
+	fill_detail_timing_data(panel_fixed_mode,
+			dvo_timing + sdvo_lvds_options->panel_type);
+
+	dev_priv->sdvo_lvds_vbt_mode = panel_fixed_mode;
+
+	return;
+}
+
+static void parse_general_features(struct drm_psb_private *dev_priv,
+		       struct bdb_header *bdb)
+{
+	struct bdb_general_features *general;
+
+	/* Set sensible defaults in case we can't find the general block */
+	dev_priv->int_tv_support = 1;
+	dev_priv->int_crt_support = 1;
+
+	general = find_section(bdb, BDB_GENERAL_FEATURES);
+	if (general) {
+		dev_priv->int_tv_support = general->int_tv_support;
+		dev_priv->int_crt_support = general->int_crt_support;
+		dev_priv->lvds_use_ssc = general->enable_ssc;
+
+		if (dev_priv->lvds_use_ssc) {
+			dev_priv->lvds_ssc_freq
+				= general->ssc_freq ? 100 : 96;
+		}
+	}
+}
+
+/**
+ * psb_intel_init_bios - initialize VBIOS settings & find VBT
+ * @dev: DRM device
+ *
+ * Loads the Video BIOS and checks that the VBT exists.  Sets scratch registers
+ * to appropriate values.
+ *
+ * VBT existence is a sanity check that is relied on by other i830_bios.c code.
+ * Note that it would be better to use a BIOS call to get the VBT, as BIOSes may
+ * feed an updated VBT back through that, compared to what we'll fetch using
+ * this method of groping around in the BIOS data.
+ *
+ * Returns 0 on success, nonzero on failure.
+ */
+bool psb_intel_init_bios(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct pci_dev *pdev = dev->pdev;
+	struct vbt_header *vbt = NULL;
+	struct bdb_header *bdb;
+	u8 __iomem *bios;
+	size_t size;
+	int i;
+
+	bios = pci_map_rom(pdev, &size);
+	if (!bios)
+		return -1;
+
+	/* Scour memory looking for the VBT signature */
+	for (i = 0; i + 4 < size; i++) {
+		if (!memcmp(bios + i, "$VBT", 4)) {
+			vbt = (struct vbt_header *)(bios + i);
+			break;
+		}
+	}
+
+	if (!vbt) {
+		dev_err(dev->dev, "VBT signature missing\n");
+		pci_unmap_rom(pdev, bios);
+		return -1;
+	}
+
+	bdb = (struct bdb_header *)(bios + i + vbt->bdb_offset);
+
+	/* Grab useful general definitions */
+	parse_general_features(dev_priv, bdb);
+	parse_lfp_panel_data(dev_priv, bdb);
+	parse_sdvo_panel_data(dev_priv, bdb);
+	parse_backlight_data(dev_priv, bdb);
+
+	pci_unmap_rom(pdev, bios);
+
+	return 0;
+}
+
+/**
+ * Destroy and free VBT data
+ */
+void psb_intel_destroy_bios(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct drm_display_mode *sdvo_lvds_vbt_mode =
+				dev_priv->sdvo_lvds_vbt_mode;
+	struct drm_display_mode *lfp_lvds_vbt_mode =
+				dev_priv->lfp_lvds_vbt_mode;
+	struct bdb_lvds_backlight *lvds_bl =
+				dev_priv->lvds_bl;
+
+	/*free sdvo panel mode*/
+	if (sdvo_lvds_vbt_mode) {
+		dev_priv->sdvo_lvds_vbt_mode = NULL;
+		kfree(sdvo_lvds_vbt_mode);
+	}
+
+	if (lfp_lvds_vbt_mode) {
+		dev_priv->lfp_lvds_vbt_mode = NULL;
+		kfree(lfp_lvds_vbt_mode);
+	}
+
+	if (lvds_bl) {
+		dev_priv->lvds_bl = NULL;
+		kfree(lvds_bl);
+	}
+}
diff --git a/drivers/gpu/drm/gma500/intel_bios.h b/drivers/gpu/drm/gma500/intel_bios.h
new file mode 100644
index 000000000000..70f1bf018183
--- /dev/null
+++ b/drivers/gpu/drm/gma500/intel_bios.h
@@ -0,0 +1,430 @@
+/*
+ * Copyright (c) 2006 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *    Eric Anholt <eric@anholt.net>
+ *
+ */
+
+#ifndef _I830_BIOS_H_
+#define _I830_BIOS_H_
+
+#include <drm/drmP.h>
+
+struct vbt_header {
+	u8 signature[20];		/**< Always starts with 'VBT$' */
+	u16 version;			/**< decimal */
+	u16 header_size;		/**< in bytes */
+	u16 vbt_size;			/**< in bytes */
+	u8 vbt_checksum;
+	u8 reserved0;
+	u32 bdb_offset;			/**< from beginning of VBT */
+	u32 aim_offset[4];		/**< from beginning of VBT */
+} __attribute__((packed));
+
+
+struct bdb_header {
+	u8 signature[16];		/**< Always 'BIOS_DATA_BLOCK' */
+	u16 version;			/**< decimal */
+	u16 header_size;		/**< in bytes */
+	u16 bdb_size;			/**< in bytes */
+};
+
+/* strictly speaking, this is a "skip" block, but it has interesting info */
+struct vbios_data {
+	u8 type; /* 0 == desktop, 1 == mobile */
+	u8 relstage;
+	u8 chipset;
+	u8 lvds_present:1;
+	u8 tv_present:1;
+	u8 rsvd2:6; /* finish byte */
+	u8 rsvd3[4];
+	u8 signon[155];
+	u8 copyright[61];
+	u16 code_segment;
+	u8 dos_boot_mode;
+	u8 bandwidth_percent;
+	u8 rsvd4; /* popup memory size */
+	u8 resize_pci_bios;
+	u8 rsvd5; /* is crt already on ddc2 */
+} __attribute__((packed));
+
+/*
+ * There are several types of BIOS data blocks (BDBs), each block has
+ * an ID and size in the first 3 bytes (ID in first, size in next 2).
+ * Known types are listed below.
+ */
+#define BDB_GENERAL_FEATURES	  1
+#define BDB_GENERAL_DEFINITIONS	  2
+#define BDB_OLD_TOGGLE_LIST	  3
+#define BDB_MODE_SUPPORT_LIST	  4
+#define BDB_GENERIC_MODE_TABLE	  5
+#define BDB_EXT_MMIO_REGS	  6
+#define BDB_SWF_IO		  7
+#define BDB_SWF_MMIO		  8
+#define BDB_DOT_CLOCK_TABLE	  9
+#define BDB_MODE_REMOVAL_TABLE	 10
+#define BDB_CHILD_DEVICE_TABLE	 11
+#define BDB_DRIVER_FEATURES	 12
+#define BDB_DRIVER_PERSISTENCE	 13
+#define BDB_EXT_TABLE_PTRS	 14
+#define BDB_DOT_CLOCK_OVERRIDE	 15
+#define BDB_DISPLAY_SELECT	 16
+/* 17 rsvd */
+#define BDB_DRIVER_ROTATION	 18
+#define BDB_DISPLAY_REMOVE	 19
+#define BDB_OEM_CUSTOM		 20
+#define BDB_EFP_LIST		 21 /* workarounds for VGA hsync/vsync */
+#define BDB_SDVO_LVDS_OPTIONS	 22
+#define BDB_SDVO_PANEL_DTDS	 23
+#define BDB_SDVO_LVDS_PNP_IDS	 24
+#define BDB_SDVO_LVDS_POWER_SEQ	 25
+#define BDB_TV_OPTIONS		 26
+#define BDB_LVDS_OPTIONS	 40
+#define BDB_LVDS_LFP_DATA_PTRS	 41
+#define BDB_LVDS_LFP_DATA	 42
+#define BDB_LVDS_BACKLIGHT	 43
+#define BDB_LVDS_POWER		 44
+#define BDB_SKIP		254 /* VBIOS private block, ignore */
+
+struct bdb_general_features {
+	/* bits 1 */
+	u8 panel_fitting:2;
+	u8 flexaim:1;
+	u8 msg_enable:1;
+	u8 clear_screen:3;
+	u8 color_flip:1;
+
+	/* bits 2 */
+	u8 download_ext_vbt:1;
+	u8 enable_ssc:1;
+	u8 ssc_freq:1;
+	u8 enable_lfp_on_override:1;
+	u8 disable_ssc_ddt:1;
+	u8 rsvd8:3; /* finish byte */
+
+	/* bits 3 */
+	u8 disable_smooth_vision:1;
+	u8 single_dvi:1;
+	u8 rsvd9:6; /* finish byte */
+
+	/* bits 4 */
+	u8 legacy_monitor_detect;
+
+	/* bits 5 */
+	u8 int_crt_support:1;
+	u8 int_tv_support:1;
+	u8 rsvd11:6; /* finish byte */
+} __attribute__((packed));
+
+struct bdb_general_definitions {
+	/* DDC GPIO */
+	u8 crt_ddc_gmbus_pin;
+
+	/* DPMS bits */
+	u8 dpms_acpi:1;
+	u8 skip_boot_crt_detect:1;
+	u8 dpms_aim:1;
+	u8 rsvd1:5; /* finish byte */
+
+	/* boot device bits */
+	u8 boot_display[2];
+	u8 child_dev_size;
+
+	/* device info */
+	u8 tv_or_lvds_info[33];
+	u8 dev1[33];
+	u8 dev2[33];
+	u8 dev3[33];
+	u8 dev4[33];
+	/* may be another device block here on some platforms */
+};
+
+struct bdb_lvds_options {
+	u8 panel_type;
+	u8 rsvd1;
+	/* LVDS capabilities, stored in a dword */
+	u8 pfit_mode:2;
+	u8 pfit_text_mode_enhanced:1;
+	u8 pfit_gfx_mode_enhanced:1;
+	u8 pfit_ratio_auto:1;
+	u8 pixel_dither:1;
+	u8 lvds_edid:1;
+	u8 rsvd2:1;
+	u8 rsvd4;
+} __attribute__((packed));
+
+struct bdb_lvds_backlight {
+	u8 type:2;
+	u8 pol:1;
+	u8 gpio:3;
+	u8 gmbus:2;
+	u16 freq;
+	u8 minbrightness;
+	u8 i2caddr;
+	u8 brightnesscmd;
+	/*FIXME: more...*/
+} __attribute__((packed));
+
+/* LFP pointer table contains entries to the struct below */
+struct bdb_lvds_lfp_data_ptr {
+	u16 fp_timing_offset; /* offsets are from start of bdb */
+	u8 fp_table_size;
+	u16 dvo_timing_offset;
+	u8 dvo_table_size;
+	u16 panel_pnp_id_offset;
+	u8 pnp_table_size;
+} __attribute__((packed));
+
+struct bdb_lvds_lfp_data_ptrs {
+	u8 lvds_entries; /* followed by one or more lvds_data_ptr structs */
+	struct bdb_lvds_lfp_data_ptr ptr[16];
+} __attribute__((packed));
+
+/* LFP data has 3 blocks per entry */
+struct lvds_fp_timing {
+	u16 x_res;
+	u16 y_res;
+	u32 lvds_reg;
+	u32 lvds_reg_val;
+	u32 pp_on_reg;
+	u32 pp_on_reg_val;
+	u32 pp_off_reg;
+	u32 pp_off_reg_val;
+	u32 pp_cycle_reg;
+	u32 pp_cycle_reg_val;
+	u32 pfit_reg;
+	u32 pfit_reg_val;
+	u16 terminator;
+} __attribute__((packed));
+
+struct lvds_dvo_timing {
+	u16 clock;		/**< In 10khz */
+	u8 hactive_lo;
+	u8 hblank_lo;
+	u8 hblank_hi:4;
+	u8 hactive_hi:4;
+	u8 vactive_lo;
+	u8 vblank_lo;
+	u8 vblank_hi:4;
+	u8 vactive_hi:4;
+	u8 hsync_off_lo;
+	u8 hsync_pulse_width;
+	u8 vsync_pulse_width:4;
+	u8 vsync_off:4;
+	u8 rsvd0:6;
+	u8 hsync_off_hi:2;
+	u8 h_image;
+	u8 v_image;
+	u8 max_hv;
+	u8 h_border;
+	u8 v_border;
+	u8 rsvd1:3;
+	u8 digital:2;
+	u8 vsync_positive:1;
+	u8 hsync_positive:1;
+	u8 rsvd2:1;
+} __attribute__((packed));
+
+struct lvds_pnp_id {
+	u16 mfg_name;
+	u16 product_code;
+	u32 serial;
+	u8 mfg_week;
+	u8 mfg_year;
+} __attribute__((packed));
+
+struct bdb_lvds_lfp_data_entry {
+	struct lvds_fp_timing fp_timing;
+	struct lvds_dvo_timing dvo_timing;
+	struct lvds_pnp_id pnp_id;
+} __attribute__((packed));
+
+struct bdb_lvds_lfp_data {
+	struct bdb_lvds_lfp_data_entry data[16];
+} __attribute__((packed));
+
+struct aimdb_header {
+	char signature[16];
+	char oem_device[20];
+	u16 aimdb_version;
+	u16 aimdb_header_size;
+	u16 aimdb_size;
+} __attribute__((packed));
+
+struct aimdb_block {
+	u8 aimdb_id;
+	u16 aimdb_size;
+} __attribute__((packed));
+
+struct vch_panel_data {
+	u16 fp_timing_offset;
+	u8 fp_timing_size;
+	u16 dvo_timing_offset;
+	u8 dvo_timing_size;
+	u16 text_fitting_offset;
+	u8 text_fitting_size;
+	u16 graphics_fitting_offset;
+	u8 graphics_fitting_size;
+} __attribute__((packed));
+
+struct vch_bdb_22 {
+	struct aimdb_block aimdb_block;
+	struct vch_panel_data panels[16];
+} __attribute__((packed));
+
+struct bdb_sdvo_lvds_options {
+	u8 panel_backlight;
+	u8 h40_set_panel_type;
+	u8 panel_type;
+	u8 ssc_clk_freq;
+	u16 als_low_trip;
+	u16 als_high_trip;
+	u8 sclalarcoeff_tab_row_num;
+	u8 sclalarcoeff_tab_row_size;
+	u8 coefficient[8];
+	u8 panel_misc_bits_1;
+	u8 panel_misc_bits_2;
+	u8 panel_misc_bits_3;
+	u8 panel_misc_bits_4;
+} __attribute__((packed));
+
+
+extern bool psb_intel_init_bios(struct drm_device *dev);
+extern void psb_intel_destroy_bios(struct drm_device *dev);
+
+/*
+ * Driver<->VBIOS interaction occurs through scratch bits in
+ * GR18 & SWF*.
+ */
+
+/* GR18 bits are set on display switch and hotkey events */
+#define GR18_DRIVER_SWITCH_EN	(1<<7) /* 0: VBIOS control, 1: driver control */
+#define GR18_HOTKEY_MASK	0x78 /* See also SWF4 15:0 */
+#define   GR18_HK_NONE		(0x0<<3)
+#define   GR18_HK_LFP_STRETCH	(0x1<<3)
+#define   GR18_HK_TOGGLE_DISP	(0x2<<3)
+#define   GR18_HK_DISP_SWITCH	(0x4<<3) /* see SWF14 15:0 for what to enable */
+#define   GR18_HK_POPUP_DISABLED (0x6<<3)
+#define   GR18_HK_POPUP_ENABLED	(0x7<<3)
+#define   GR18_HK_PFIT		(0x8<<3)
+#define   GR18_HK_APM_CHANGE	(0xa<<3)
+#define   GR18_HK_MULTIPLE	(0xc<<3)
+#define GR18_USER_INT_EN	(1<<2)
+#define GR18_A0000_FLUSH_EN	(1<<1)
+#define GR18_SMM_EN		(1<<0)
+
+/* Set by driver, cleared by VBIOS */
+#define SWF00_YRES_SHIFT	16
+#define SWF00_XRES_SHIFT	0
+#define SWF00_RES_MASK		0xffff
+
+/* Set by VBIOS at boot time and driver at runtime */
+#define SWF01_TV2_FORMAT_SHIFT	8
+#define SWF01_TV1_FORMAT_SHIFT	0
+#define SWF01_TV_FORMAT_MASK	0xffff
+
+#define SWF10_VBIOS_BLC_I2C_EN	(1<<29)
+#define SWF10_GTT_OVERRIDE_EN	(1<<28)
+#define SWF10_LFP_DPMS_OVR	(1<<27) /* override DPMS on display switch */
+#define SWF10_ACTIVE_TOGGLE_LIST_MASK (7<<24)
+#define   SWF10_OLD_TOGGLE	0x0
+#define   SWF10_TOGGLE_LIST_1	0x1
+#define   SWF10_TOGGLE_LIST_2	0x2
+#define   SWF10_TOGGLE_LIST_3	0x3
+#define   SWF10_TOGGLE_LIST_4	0x4
+#define SWF10_PANNING_EN	(1<<23)
+#define SWF10_DRIVER_LOADED	(1<<22)
+#define SWF10_EXTENDED_DESKTOP	(1<<21)
+#define SWF10_EXCLUSIVE_MODE	(1<<20)
+#define SWF10_OVERLAY_EN	(1<<19)
+#define SWF10_PLANEB_HOLDOFF	(1<<18)
+#define SWF10_PLANEA_HOLDOFF	(1<<17)
+#define SWF10_VGA_HOLDOFF	(1<<16)
+#define SWF10_ACTIVE_DISP_MASK	0xffff
+#define   SWF10_PIPEB_LFP2	(1<<15)
+#define   SWF10_PIPEB_EFP2	(1<<14)
+#define   SWF10_PIPEB_TV2	(1<<13)
+#define   SWF10_PIPEB_CRT2	(1<<12)
+#define   SWF10_PIPEB_LFP	(1<<11)
+#define   SWF10_PIPEB_EFP	(1<<10)
+#define   SWF10_PIPEB_TV	(1<<9)
+#define   SWF10_PIPEB_CRT	(1<<8)
+#define   SWF10_PIPEA_LFP2	(1<<7)
+#define   SWF10_PIPEA_EFP2	(1<<6)
+#define   SWF10_PIPEA_TV2	(1<<5)
+#define   SWF10_PIPEA_CRT2	(1<<4)
+#define   SWF10_PIPEA_LFP	(1<<3)
+#define   SWF10_PIPEA_EFP	(1<<2)
+#define   SWF10_PIPEA_TV	(1<<1)
+#define   SWF10_PIPEA_CRT	(1<<0)
+
+#define SWF11_MEMORY_SIZE_SHIFT	16
+#define SWF11_SV_TEST_EN	(1<<15)
+#define SWF11_IS_AGP		(1<<14)
+#define SWF11_DISPLAY_HOLDOFF	(1<<13)
+#define SWF11_DPMS_REDUCED	(1<<12)
+#define SWF11_IS_VBE_MODE	(1<<11)
+#define SWF11_PIPEB_ACCESS	(1<<10) /* 0 here means pipe a */
+#define SWF11_DPMS_MASK		0x07
+#define   SWF11_DPMS_OFF	(1<<2)
+#define   SWF11_DPMS_SUSPEND	(1<<1)
+#define   SWF11_DPMS_STANDBY	(1<<0)
+#define   SWF11_DPMS_ON		0
+
+#define SWF14_GFX_PFIT_EN	(1<<31)
+#define SWF14_TEXT_PFIT_EN	(1<<30)
+#define SWF14_LID_STATUS_CLOSED	(1<<29) /* 0 here means open */
+#define SWF14_POPUP_EN		(1<<28)
+#define SWF14_DISPLAY_HOLDOFF	(1<<27)
+#define SWF14_DISP_DETECT_EN	(1<<26)
+#define SWF14_DOCKING_STATUS_DOCKED (1<<25) /* 0 here means undocked */
+#define SWF14_DRIVER_STATUS	(1<<24)
+#define SWF14_OS_TYPE_WIN9X	(1<<23)
+#define SWF14_OS_TYPE_WINNT	(1<<22)
+/* 21:19 rsvd */
+#define SWF14_PM_TYPE_MASK	0x00070000
+#define   SWF14_PM_ACPI_VIDEO	(0x4 << 16)
+#define   SWF14_PM_ACPI		(0x3 << 16)
+#define   SWF14_PM_APM_12	(0x2 << 16)
+#define   SWF14_PM_APM_11	(0x1 << 16)
+#define SWF14_HK_REQUEST_MASK	0x0000ffff /* see GR18 6:3 for event type */
+	  /* if GR18 indicates a display switch */
+#define   SWF14_DS_PIPEB_LFP2_EN (1<<15)
+#define   SWF14_DS_PIPEB_EFP2_EN (1<<14)
+#define   SWF14_DS_PIPEB_TV2_EN  (1<<13)
+#define   SWF14_DS_PIPEB_CRT2_EN (1<<12)
+#define   SWF14_DS_PIPEB_LFP_EN  (1<<11)
+#define   SWF14_DS_PIPEB_EFP_EN  (1<<10)
+#define   SWF14_DS_PIPEB_TV_EN	 (1<<9)
+#define   SWF14_DS_PIPEB_CRT_EN  (1<<8)
+#define   SWF14_DS_PIPEA_LFP2_EN (1<<7)
+#define   SWF14_DS_PIPEA_EFP2_EN (1<<6)
+#define   SWF14_DS_PIPEA_TV2_EN  (1<<5)
+#define   SWF14_DS_PIPEA_CRT2_EN (1<<4)
+#define   SWF14_DS_PIPEA_LFP_EN  (1<<3)
+#define   SWF14_DS_PIPEA_EFP_EN  (1<<2)
+#define   SWF14_DS_PIPEA_TV_EN	 (1<<1)
+#define   SWF14_DS_PIPEA_CRT_EN  (1<<0)
+	  /* if GR18 indicates a panel fitting request */
+#define   SWF14_PFIT_EN		(1<<0) /* 0 means disable */
+	  /* if GR18 indicates an APM change request */
+#define   SWF14_APM_HIBERNATE	0x4
+#define   SWF14_APM_SUSPEND	0x3
+#define   SWF14_APM_STANDBY	0x1
+#define   SWF14_APM_RESTORE	0x0
+
+#endif /* _I830_BIOS_H_ */
diff --git a/drivers/gpu/drm/gma500/intel_gmbus.c b/drivers/gpu/drm/gma500/intel_gmbus.c
new file mode 100644
index 000000000000..147584ac8d02
--- /dev/null
+++ b/drivers/gpu/drm/gma500/intel_gmbus.c
@@ -0,0 +1,493 @@
+/*
+ * Copyright (c) 2006 Dave Airlie <airlied@linux.ie>
+ * Copyright © 2006-2008,2010 Intel Corporation
+ *   Jesse Barnes <jesse.barnes@intel.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ *	Chris Wilson <chris@chris-wilson.co.uk>
+ */
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+#include "drmP.h"
+#include "drm.h"
+#include "psb_intel_drv.h"
+#include "gma_drm.h"
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+
+#define _wait_for(COND, MS, W) ({ \
+	unsigned long timeout__ = jiffies + msecs_to_jiffies(MS);	\
+	int ret__ = 0;							\
+	while (! (COND)) {						\
+		if (time_after(jiffies, timeout__)) {			\
+			ret__ = -ETIMEDOUT;				\
+			break;						\
+		}							\
+		if (W && !(in_atomic() || in_dbg_master())) msleep(W);	\
+	}								\
+	ret__;								\
+})
+
+#define wait_for(COND, MS) _wait_for(COND, MS, 1)
+#define wait_for_atomic(COND, MS) _wait_for(COND, MS, 0)
+
+/* Intel GPIO access functions */
+
+#define I2C_RISEFALL_TIME 20
+
+static inline struct intel_gmbus *
+to_intel_gmbus(struct i2c_adapter *i2c)
+{
+	return container_of(i2c, struct intel_gmbus, adapter);
+}
+
+struct intel_gpio {
+	struct i2c_adapter adapter;
+	struct i2c_algo_bit_data algo;
+	struct drm_psb_private *dev_priv;
+	u32 reg;
+};
+
+void
+gma_intel_i2c_reset(struct drm_device *dev)
+{
+	REG_WRITE(GMBUS0, 0);
+}
+
+static void intel_i2c_quirk_set(struct drm_psb_private *dev_priv, bool enable)
+{
+	/* When using bit bashing for I2C, this bit needs to be set to 1 */
+	/* FIXME: We are never Pineview, right?
+
+	u32 val;
+
+	if (!IS_PINEVIEW(dev_priv->dev))
+		return;
+
+	val = REG_READ(DSPCLK_GATE_D);
+	if (enable)
+		val |= DPCUNIT_CLOCK_GATE_DISABLE;
+	else
+		val &= ~DPCUNIT_CLOCK_GATE_DISABLE;
+	REG_WRITE(DSPCLK_GATE_D, val);
+
+	return;
+	*/
+}
+
+static u32 get_reserved(struct intel_gpio *gpio)
+{
+	struct drm_psb_private *dev_priv = gpio->dev_priv;
+	struct drm_device *dev = dev_priv->dev;
+	u32 reserved = 0;
+
+	/* On most chips, these bits must be preserved in software. */
+	reserved = REG_READ(gpio->reg) &
+				     (GPIO_DATA_PULLUP_DISABLE |
+				      GPIO_CLOCK_PULLUP_DISABLE);
+
+	return reserved;
+}
+
+static int get_clock(void *data)
+{
+	struct intel_gpio *gpio = data;
+	struct drm_psb_private *dev_priv = gpio->dev_priv;
+	struct drm_device *dev = dev_priv->dev;
+	u32 reserved = get_reserved(gpio);
+	REG_WRITE(gpio->reg, reserved | GPIO_CLOCK_DIR_MASK);
+	REG_WRITE(gpio->reg, reserved);
+	return (REG_READ(gpio->reg) & GPIO_CLOCK_VAL_IN) != 0;
+}
+
+static int get_data(void *data)
+{
+	struct intel_gpio *gpio = data;
+	struct drm_psb_private *dev_priv = gpio->dev_priv;
+	struct drm_device *dev = dev_priv->dev;
+	u32 reserved = get_reserved(gpio);
+	REG_WRITE(gpio->reg, reserved | GPIO_DATA_DIR_MASK);
+	REG_WRITE(gpio->reg, reserved);
+	return (REG_READ(gpio->reg) & GPIO_DATA_VAL_IN) != 0;
+}
+
+static void set_clock(void *data, int state_high)
+{
+	struct intel_gpio *gpio = data;
+	struct drm_psb_private *dev_priv = gpio->dev_priv;
+	struct drm_device *dev = dev_priv->dev;
+	u32 reserved = get_reserved(gpio);
+	u32 clock_bits;
+
+	if (state_high)
+		clock_bits = GPIO_CLOCK_DIR_IN | GPIO_CLOCK_DIR_MASK;
+	else
+		clock_bits = GPIO_CLOCK_DIR_OUT | GPIO_CLOCK_DIR_MASK |
+			GPIO_CLOCK_VAL_MASK;
+
+	REG_WRITE(gpio->reg, reserved | clock_bits);
+	REG_READ(gpio->reg); /* Posting */
+}
+
+static void set_data(void *data, int state_high)
+{
+	struct intel_gpio *gpio = data;
+	struct drm_psb_private *dev_priv = gpio->dev_priv;
+	struct drm_device *dev = dev_priv->dev;
+	u32 reserved = get_reserved(gpio);
+	u32 data_bits;
+
+	if (state_high)
+		data_bits = GPIO_DATA_DIR_IN | GPIO_DATA_DIR_MASK;
+	else
+		data_bits = GPIO_DATA_DIR_OUT | GPIO_DATA_DIR_MASK |
+			GPIO_DATA_VAL_MASK;
+
+	REG_WRITE(gpio->reg, reserved | data_bits);
+	REG_READ(gpio->reg);
+}
+
+static struct i2c_adapter *
+intel_gpio_create(struct drm_psb_private *dev_priv, u32 pin)
+{
+	static const int map_pin_to_reg[] = {
+		0,
+		GPIOB,
+		GPIOA,
+		GPIOC,
+		GPIOD,
+		GPIOE,
+		0,
+		GPIOF,
+	};
+	struct intel_gpio *gpio;
+
+	if (pin >= ARRAY_SIZE(map_pin_to_reg) || !map_pin_to_reg[pin])
+		return NULL;
+
+	gpio = kzalloc(sizeof(struct intel_gpio), GFP_KERNEL);
+	if (gpio == NULL)
+		return NULL;
+
+	gpio->reg = map_pin_to_reg[pin];
+	gpio->dev_priv = dev_priv;
+
+	snprintf(gpio->adapter.name, sizeof(gpio->adapter.name),
+		 "gma500 GPIO%c", "?BACDE?F"[pin]);
+	gpio->adapter.owner = THIS_MODULE;
+	gpio->adapter.algo_data	= &gpio->algo;
+	gpio->adapter.dev.parent = &dev_priv->dev->pdev->dev;
+	gpio->algo.setsda = set_data;
+	gpio->algo.setscl = set_clock;
+	gpio->algo.getsda = get_data;
+	gpio->algo.getscl = get_clock;
+	gpio->algo.udelay = I2C_RISEFALL_TIME;
+	gpio->algo.timeout = usecs_to_jiffies(2200);
+	gpio->algo.data = gpio;
+
+	if (i2c_bit_add_bus(&gpio->adapter))
+		goto out_free;
+
+	return &gpio->adapter;
+
+out_free:
+	kfree(gpio);
+	return NULL;
+}
+
+static int
+intel_i2c_quirk_xfer(struct drm_psb_private *dev_priv,
+		     struct i2c_adapter *adapter,
+		     struct i2c_msg *msgs,
+		     int num)
+{
+	struct intel_gpio *gpio = container_of(adapter,
+					       struct intel_gpio,
+					       adapter);
+	int ret;
+
+	gma_intel_i2c_reset(dev_priv->dev);
+
+	intel_i2c_quirk_set(dev_priv, true);
+	set_data(gpio, 1);
+	set_clock(gpio, 1);
+	udelay(I2C_RISEFALL_TIME);
+
+	ret = adapter->algo->master_xfer(adapter, msgs, num);
+
+	set_data(gpio, 1);
+	set_clock(gpio, 1);
+	intel_i2c_quirk_set(dev_priv, false);
+
+	return ret;
+}
+
+static int
+gmbus_xfer(struct i2c_adapter *adapter,
+	   struct i2c_msg *msgs,
+	   int num)
+{
+	struct intel_gmbus *bus = container_of(adapter,
+					       struct intel_gmbus,
+					       adapter);
+	struct drm_psb_private *dev_priv = adapter->algo_data;
+	struct drm_device *dev = dev_priv->dev;
+	int i, reg_offset;
+
+	if (bus->force_bit)
+		return intel_i2c_quirk_xfer(dev_priv,
+					    bus->force_bit, msgs, num);
+
+	reg_offset = 0;
+
+	REG_WRITE(GMBUS0 + reg_offset, bus->reg0);
+
+	for (i = 0; i < num; i++) {
+		u16 len = msgs[i].len;
+		u8 *buf = msgs[i].buf;
+
+		if (msgs[i].flags & I2C_M_RD) {
+			REG_WRITE(GMBUS1 + reg_offset,
+				   GMBUS_CYCLE_WAIT | (i + 1 == num ? GMBUS_CYCLE_STOP : 0) |
+				   (len << GMBUS_BYTE_COUNT_SHIFT) |
+				   (msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
+				   GMBUS_SLAVE_READ | GMBUS_SW_RDY);
+			REG_READ(GMBUS2+reg_offset);
+			do {
+				u32 val, loop = 0;
+
+				if (wait_for(REG_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_RDY), 50))
+					goto timeout;
+				if (REG_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
+					goto clear_err;
+
+				val = REG_READ(GMBUS3 + reg_offset);
+				do {
+					*buf++ = val & 0xff;
+					val >>= 8;
+				} while (--len && ++loop < 4);
+			} while (len);
+		} else {
+			u32 val, loop;
+
+			val = loop = 0;
+			do {
+				val |= *buf++ << (8 * loop);
+			} while (--len && ++loop < 4);
+
+			REG_WRITE(GMBUS3 + reg_offset, val);
+			REG_WRITE(GMBUS1 + reg_offset,
+				   (i + 1 == num ? GMBUS_CYCLE_STOP : GMBUS_CYCLE_WAIT) |
+				   (msgs[i].len << GMBUS_BYTE_COUNT_SHIFT) |
+				   (msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
+				   GMBUS_SLAVE_WRITE | GMBUS_SW_RDY);
+			REG_READ(GMBUS2+reg_offset);
+
+			while (len) {
+				if (wait_for(REG_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_RDY), 50))
+					goto timeout;
+				if (REG_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
+					goto clear_err;
+
+				val = loop = 0;
+				do {
+					val |= *buf++ << (8 * loop);
+				} while (--len && ++loop < 4);
+
+				REG_WRITE(GMBUS3 + reg_offset, val);
+				REG_READ(GMBUS2+reg_offset);
+			}
+		}
+
+		if (i + 1 < num && wait_for(REG_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_WAIT_PHASE), 50))
+			goto timeout;
+		if (REG_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
+			goto clear_err;
+	}
+
+	goto done;
+
+clear_err:
+	/* Toggle the Software Clear Interrupt bit. This has the effect
+	 * of resetting the GMBUS controller and so clearing the
+	 * BUS_ERROR raised by the slave's NAK.
+	 */
+	REG_WRITE(GMBUS1 + reg_offset, GMBUS_SW_CLR_INT);
+	REG_WRITE(GMBUS1 + reg_offset, 0);
+
+done:
+	/* Mark the GMBUS interface as disabled. We will re-enable it at the
+	 * start of the next xfer, till then let it sleep.
+	 */
+	REG_WRITE(GMBUS0 + reg_offset, 0);
+	return i;
+
+timeout:
+	DRM_INFO("GMBUS timed out, falling back to bit banging on pin %d [%s]\n",
+		 bus->reg0 & 0xff, bus->adapter.name);
+	REG_WRITE(GMBUS0 + reg_offset, 0);
+
+	/* Hardware may not support GMBUS over these pins? Try GPIO bitbanging instead. */
+	bus->force_bit = intel_gpio_create(dev_priv, bus->reg0 & 0xff);
+	if (!bus->force_bit)
+		return -ENOMEM;
+
+	return intel_i2c_quirk_xfer(dev_priv, bus->force_bit, msgs, num);
+}
+
+static u32 gmbus_func(struct i2c_adapter *adapter)
+{
+	struct intel_gmbus *bus = container_of(adapter,
+					       struct intel_gmbus,
+					       adapter);
+
+	if (bus->force_bit)
+		bus->force_bit->algo->functionality(bus->force_bit);
+
+	return (I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
+		/* I2C_FUNC_10BIT_ADDR | */
+		I2C_FUNC_SMBUS_READ_BLOCK_DATA |
+		I2C_FUNC_SMBUS_BLOCK_PROC_CALL);
+}
+
+static const struct i2c_algorithm gmbus_algorithm = {
+	.master_xfer	= gmbus_xfer,
+	.functionality	= gmbus_func
+};
+
+/**
+ * intel_gmbus_setup - instantiate all Intel i2c GMBuses
+ * @dev: DRM device
+ */
+int gma_intel_setup_gmbus(struct drm_device *dev)
+{
+	static const char *names[GMBUS_NUM_PORTS] = {
+		"disabled",
+		"ssc",
+		"vga",
+		"panel",
+		"dpc",
+		"dpb",
+		"reserved",
+		"dpd",
+	};
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int ret, i;
+
+	dev_priv->gmbus = kcalloc(sizeof(struct intel_gmbus), GMBUS_NUM_PORTS,
+				  GFP_KERNEL);
+	if (dev_priv->gmbus == NULL)
+		return -ENOMEM;
+
+	for (i = 0; i < GMBUS_NUM_PORTS; i++) {
+		struct intel_gmbus *bus = &dev_priv->gmbus[i];
+
+		bus->adapter.owner = THIS_MODULE;
+		bus->adapter.class = I2C_CLASS_DDC;
+		snprintf(bus->adapter.name,
+			 sizeof(bus->adapter.name),
+			 "gma500 gmbus %s",
+			 names[i]);
+
+		bus->adapter.dev.parent = &dev->pdev->dev;
+		bus->adapter.algo_data	= dev_priv;
+
+		bus->adapter.algo = &gmbus_algorithm;
+		ret = i2c_add_adapter(&bus->adapter);
+		if (ret)
+			goto err;
+
+		/* By default use a conservative clock rate */
+		bus->reg0 = i | GMBUS_RATE_100KHZ;
+
+		/* XXX force bit banging until GMBUS is fully debugged */
+		bus->force_bit = intel_gpio_create(dev_priv, i);
+	}
+
+	gma_intel_i2c_reset(dev_priv->dev);
+
+	return 0;
+
+err:
+	while (--i) {
+		struct intel_gmbus *bus = &dev_priv->gmbus[i];
+		i2c_del_adapter(&bus->adapter);
+	}
+	kfree(dev_priv->gmbus);
+	dev_priv->gmbus = NULL;
+	return ret;
+}
+
+void gma_intel_gmbus_set_speed(struct i2c_adapter *adapter, int speed)
+{
+	struct intel_gmbus *bus = to_intel_gmbus(adapter);
+
+	/* speed:
+	 * 0x0 = 100 KHz
+	 * 0x1 = 50 KHz
+	 * 0x2 = 400 KHz
+	 * 0x3 = 1000 Khz
+	 */
+	bus->reg0 = (bus->reg0 & ~(0x3 << 8)) | (speed << 8);
+}
+
+void gma_intel_gmbus_force_bit(struct i2c_adapter *adapter, bool force_bit)
+{
+	struct intel_gmbus *bus = to_intel_gmbus(adapter);
+
+	if (force_bit) {
+		if (bus->force_bit == NULL) {
+			struct drm_psb_private *dev_priv = adapter->algo_data;
+			bus->force_bit = intel_gpio_create(dev_priv,
+							   bus->reg0 & 0xff);
+		}
+	} else {
+		if (bus->force_bit) {
+			i2c_del_adapter(bus->force_bit);
+			kfree(bus->force_bit);
+			bus->force_bit = NULL;
+		}
+	}
+}
+
+void gma_intel_teardown_gmbus(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int i;
+
+	if (dev_priv->gmbus == NULL)
+		return;
+
+	for (i = 0; i < GMBUS_NUM_PORTS; i++) {
+		struct intel_gmbus *bus = &dev_priv->gmbus[i];
+		if (bus->force_bit) {
+			i2c_del_adapter(bus->force_bit);
+			kfree(bus->force_bit);
+		}
+		i2c_del_adapter(&bus->adapter);
+	}
+
+	kfree(dev_priv->gmbus);
+	dev_priv->gmbus = NULL;
+}
diff --git a/drivers/gpu/drm/gma500/intel_i2c.c b/drivers/gpu/drm/gma500/intel_i2c.c
new file mode 100644
index 000000000000..98a28c209555
--- /dev/null
+++ b/drivers/gpu/drm/gma500/intel_i2c.c
@@ -0,0 +1,169 @@
+/*
+ * Copyright © 2006-2007 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ */
+#include <linux/export.h>
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+
+/*
+ * Intel GPIO access functions
+ */
+
+#define I2C_RISEFALL_TIME 20
+
+static int get_clock(void *data)
+{
+	struct psb_intel_i2c_chan *chan = data;
+	struct drm_device *dev = chan->drm_dev;
+	u32 val;
+
+	val = REG_READ(chan->reg);
+	return (val & GPIO_CLOCK_VAL_IN) != 0;
+}
+
+static int get_data(void *data)
+{
+	struct psb_intel_i2c_chan *chan = data;
+	struct drm_device *dev = chan->drm_dev;
+	u32 val;
+
+	val = REG_READ(chan->reg);
+	return (val & GPIO_DATA_VAL_IN) != 0;
+}
+
+static void set_clock(void *data, int state_high)
+{
+	struct psb_intel_i2c_chan *chan = data;
+	struct drm_device *dev = chan->drm_dev;
+	u32 reserved = 0, clock_bits;
+
+	/* On most chips, these bits must be preserved in software. */
+	reserved =
+		    REG_READ(chan->reg) & (GPIO_DATA_PULLUP_DISABLE |
+					   GPIO_CLOCK_PULLUP_DISABLE);
+
+	if (state_high)
+		clock_bits = GPIO_CLOCK_DIR_IN | GPIO_CLOCK_DIR_MASK;
+	else
+		clock_bits = GPIO_CLOCK_DIR_OUT | GPIO_CLOCK_DIR_MASK |
+		    GPIO_CLOCK_VAL_MASK;
+	REG_WRITE(chan->reg, reserved | clock_bits);
+	udelay(I2C_RISEFALL_TIME);	/* wait for the line to change state */
+}
+
+static void set_data(void *data, int state_high)
+{
+	struct psb_intel_i2c_chan *chan = data;
+	struct drm_device *dev = chan->drm_dev;
+	u32 reserved = 0, data_bits;
+
+	/* On most chips, these bits must be preserved in software. */
+	reserved =
+		    REG_READ(chan->reg) & (GPIO_DATA_PULLUP_DISABLE |
+					   GPIO_CLOCK_PULLUP_DISABLE);
+
+	if (state_high)
+		data_bits = GPIO_DATA_DIR_IN | GPIO_DATA_DIR_MASK;
+	else
+		data_bits =
+		    GPIO_DATA_DIR_OUT | GPIO_DATA_DIR_MASK |
+		    GPIO_DATA_VAL_MASK;
+
+	REG_WRITE(chan->reg, reserved | data_bits);
+	udelay(I2C_RISEFALL_TIME);	/* wait for the line to change state */
+}
+
+/**
+ * psb_intel_i2c_create - instantiate an Intel i2c bus using the specified GPIO reg
+ * @dev: DRM device
+ * @output: driver specific output device
+ * @reg: GPIO reg to use
+ * @name: name for this bus
+ *
+ * Creates and registers a new i2c bus with the Linux i2c layer, for use
+ * in output probing and control (e.g. DDC or SDVO control functions).
+ *
+ * Possible values for @reg include:
+ *   %GPIOA
+ *   %GPIOB
+ *   %GPIOC
+ *   %GPIOD
+ *   %GPIOE
+ *   %GPIOF
+ *   %GPIOG
+ *   %GPIOH
+ * see PRM for details on how these different busses are used.
+ */
+struct psb_intel_i2c_chan *psb_intel_i2c_create(struct drm_device *dev,
+					const u32 reg, const char *name)
+{
+	struct psb_intel_i2c_chan *chan;
+
+	chan = kzalloc(sizeof(struct psb_intel_i2c_chan), GFP_KERNEL);
+	if (!chan)
+		goto out_free;
+
+	chan->drm_dev = dev;
+	chan->reg = reg;
+	snprintf(chan->adapter.name, I2C_NAME_SIZE, "intel drm %s", name);
+	chan->adapter.owner = THIS_MODULE;
+	chan->adapter.algo_data = &chan->algo;
+	chan->adapter.dev.parent = &dev->pdev->dev;
+	chan->algo.setsda = set_data;
+	chan->algo.setscl = set_clock;
+	chan->algo.getsda = get_data;
+	chan->algo.getscl = get_clock;
+	chan->algo.udelay = 20;
+	chan->algo.timeout = usecs_to_jiffies(2200);
+	chan->algo.data = chan;
+
+	i2c_set_adapdata(&chan->adapter, chan);
+
+	if (i2c_bit_add_bus(&chan->adapter))
+		goto out_free;
+
+	/* JJJ:  raise SCL and SDA? */
+	set_data(chan, 1);
+	set_clock(chan, 1);
+	udelay(20);
+
+	return chan;
+
+out_free:
+	kfree(chan);
+	return NULL;
+}
+
+/**
+ * psb_intel_i2c_destroy - unregister and free i2c bus resources
+ * @output: channel to free
+ *
+ * Unregister the adapter from the i2c layer, then free the structure.
+ */
+void psb_intel_i2c_destroy(struct psb_intel_i2c_chan *chan)
+{
+	if (!chan)
+		return;
+
+	i2c_del_adapter(&chan->adapter);
+	kfree(chan);
+}
diff --git a/drivers/gpu/drm/gma500/intel_opregion.c b/drivers/gpu/drm/gma500/intel_opregion.c
new file mode 100644
index 000000000000..d946bc1b17bf
--- /dev/null
+++ b/drivers/gpu/drm/gma500/intel_opregion.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright 2010 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * FIXME: resolve with the i915 version
+ */
+
+#include "psb_drv.h"
+
+struct opregion_header {
+	u8 signature[16];
+	u32 size;
+	u32 opregion_ver;
+	u8 bios_ver[32];
+	u8 vbios_ver[16];
+	u8 driver_ver[16];
+	u32 mboxes;
+	u8 reserved[164];
+} __packed;
+
+struct opregion_apci {
+	/*FIXME: add it later*/
+} __packed;
+
+struct opregion_swsci {
+	/*FIXME: add it later*/
+} __packed;
+
+struct opregion_acpi {
+	/*FIXME: add it later*/
+} __packed;
+
+int gma_intel_opregion_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 opregion_phy;
+	void *base;
+	u32 *lid_state;
+
+	dev_priv->lid_state = NULL;
+
+	pci_read_config_dword(dev->pdev, 0xfc, &opregion_phy);
+	if (opregion_phy == 0)
+		return -ENOTSUPP;
+
+	base = ioremap(opregion_phy, 8*1024);
+	if (!base)
+		return -ENOMEM;
+
+	lid_state = base + 0x01ac;
+
+	dev_priv->lid_state = lid_state;
+	dev_priv->lid_last_state = readl(lid_state);
+	return 0;
+}
+
+int gma_intel_opregion_exit(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	if (dev_priv->lid_state)
+		iounmap(dev_priv->lid_state);
+	return 0;
+}
diff --git a/drivers/gpu/drm/gma500/mid_bios.c b/drivers/gpu/drm/gma500/mid_bios.c
new file mode 100644
index 000000000000..5eee9ad80da4
--- /dev/null
+++ b/drivers/gpu/drm/gma500/mid_bios.c
@@ -0,0 +1,263 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+/* TODO
+ * - Split functions by vbt type
+ * - Make them all take drm_device
+ * - Check ioremap failures
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "gma_drm.h"
+#include "psb_drv.h"
+#include "mid_bios.h"
+
+static void mid_get_fuse_settings(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	uint32_t fuse_value = 0;
+	uint32_t fuse_value_tmp = 0;
+
+#define FB_REG06 0xD0810600
+#define FB_MIPI_DISABLE  (1 << 11)
+#define FB_REG09 0xD0810900
+#define FB_REG09 0xD0810900
+#define FB_SKU_MASK  0x7000
+#define FB_SKU_SHIFT 12
+#define FB_SKU_100 0
+#define FB_SKU_100L 1
+#define FB_SKU_83 2
+	if (pci_root == NULL) {
+		WARN_ON(1);
+		return;
+	}
+
+
+	pci_write_config_dword(pci_root, 0xD0, FB_REG06);
+	pci_read_config_dword(pci_root, 0xD4, &fuse_value);
+
+	/* FB_MIPI_DISABLE doesn't mean LVDS on with Medfield */
+	if (IS_MRST(dev))
+		dev_priv->iLVDS_enable = fuse_value & FB_MIPI_DISABLE;
+
+	DRM_INFO("internal display is %s\n",
+		 dev_priv->iLVDS_enable ? "LVDS display" : "MIPI display");
+
+	 /* Prevent runtime suspend at start*/
+	 if (dev_priv->iLVDS_enable) {
+		dev_priv->is_lvds_on = true;
+		dev_priv->is_mipi_on = false;
+	} else {
+		dev_priv->is_mipi_on = true;
+		dev_priv->is_lvds_on = false;
+	}
+
+	dev_priv->video_device_fuse = fuse_value;
+
+	pci_write_config_dword(pci_root, 0xD0, FB_REG09);
+	pci_read_config_dword(pci_root, 0xD4, &fuse_value);
+
+	dev_dbg(dev->dev, "SKU values is 0x%x.\n", fuse_value);
+	fuse_value_tmp = (fuse_value & FB_SKU_MASK) >> FB_SKU_SHIFT;
+
+	dev_priv->fuse_reg_value = fuse_value;
+
+	switch (fuse_value_tmp) {
+	case FB_SKU_100:
+		dev_priv->core_freq = 200;
+		break;
+	case FB_SKU_100L:
+		dev_priv->core_freq = 100;
+		break;
+	case FB_SKU_83:
+		dev_priv->core_freq = 166;
+		break;
+	default:
+		dev_warn(dev->dev, "Invalid SKU values, SKU value = 0x%08x\n",
+								fuse_value_tmp);
+		dev_priv->core_freq = 0;
+	}
+	dev_dbg(dev->dev, "LNC core clk is %dMHz.\n", dev_priv->core_freq);
+	pci_dev_put(pci_root);
+}
+
+/*
+ *	Get the revison ID, B0:D2:F0;0x08
+ */
+static void mid_get_pci_revID(struct drm_psb_private *dev_priv)
+{
+	uint32_t platform_rev_id = 0;
+	struct pci_dev *pci_gfx_root = pci_get_bus_and_slot(0, PCI_DEVFN(2, 0));
+
+	if (pci_gfx_root == NULL) {
+		WARN_ON(1);
+		return;
+	}
+	pci_read_config_dword(pci_gfx_root, 0x08, &platform_rev_id);
+	dev_priv->platform_rev_id = (uint8_t) platform_rev_id;
+	pci_dev_put(pci_gfx_root);
+	dev_dbg(dev_priv->dev->dev, "platform_rev_id is %x\n",
+					dev_priv->platform_rev_id);
+}
+
+static void mid_get_vbt_data(struct drm_psb_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
+	u32 addr;
+	u16 new_size;
+	u8 *vbt_virtual;
+	u8 bpi;
+	u8 number_desc = 0;
+	struct oaktrail_timing_info *dp_ti = &dev_priv->gct_data.DTD;
+	struct gct_r10_timing_info ti;
+	void *pGCT;
+	struct pci_dev *pci_gfx_root = pci_get_bus_and_slot(0, PCI_DEVFN(2, 0));
+
+	/* Get the address of the platform config vbt, B0:D2:F0;0xFC */
+	pci_read_config_dword(pci_gfx_root, 0xFC, &addr);
+	pci_dev_put(pci_gfx_root);
+
+	dev_dbg(dev->dev, "drm platform config address is %x\n", addr);
+
+	/* check for platform config address == 0. */
+	/* this means fw doesn't support vbt */
+
+	if (addr == 0) {
+		vbt->size = 0;
+		return;
+	}
+
+	/* get the virtual address of the vbt */
+	vbt_virtual = ioremap(addr, sizeof(*vbt));
+	if (vbt_virtual == NULL) {
+		vbt->size = 0;
+		return;
+	}
+
+	memcpy(vbt, vbt_virtual, sizeof(*vbt));
+	iounmap(vbt_virtual); /* Free virtual address space */
+
+	/* No matching signature don't process the data */
+	if (memcmp(vbt->signature, "$GCT", 4)) {
+		vbt->size = 0;
+		return;
+	}
+
+	dev_dbg(dev->dev, "GCT revision is %x\n", vbt->revision);
+
+	switch (vbt->revision) {
+	case 0:
+		vbt->oaktrail_gct = ioremap(addr + sizeof(*vbt) - 4,
+					vbt->size - sizeof(*vbt) + 4);
+		pGCT = vbt->oaktrail_gct;
+		bpi = ((struct oaktrail_gct_v1 *)pGCT)->PD.BootPanelIndex;
+		dev_priv->gct_data.bpi = bpi;
+		dev_priv->gct_data.pt =
+			((struct oaktrail_gct_v1 *)pGCT)->PD.PanelType;
+		memcpy(&dev_priv->gct_data.DTD,
+			&((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].DTD,
+				sizeof(struct oaktrail_timing_info));
+		dev_priv->gct_data.Panel_Port_Control =
+		  ((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].Panel_Port_Control;
+		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+			((struct oaktrail_gct_v1 *)pGCT)->panel[bpi].Panel_MIPI_Display_Descriptor;
+		break;
+	case 1:
+		vbt->oaktrail_gct = ioremap(addr + sizeof(*vbt) - 4,
+					vbt->size - sizeof(*vbt) + 4);
+		pGCT = vbt->oaktrail_gct;
+		bpi = ((struct oaktrail_gct_v2 *)pGCT)->PD.BootPanelIndex;
+		dev_priv->gct_data.bpi = bpi;
+		dev_priv->gct_data.pt =
+			((struct oaktrail_gct_v2 *)pGCT)->PD.PanelType;
+		memcpy(&dev_priv->gct_data.DTD,
+			&((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].DTD,
+				sizeof(struct oaktrail_timing_info));
+		dev_priv->gct_data.Panel_Port_Control =
+		  ((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].Panel_Port_Control;
+		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+			((struct oaktrail_gct_v2 *)pGCT)->panel[bpi].Panel_MIPI_Display_Descriptor;
+		break;
+	case 0x10:
+		/*header definition changed from rev 01 (v2) to rev 10h. */
+		/*so, some values have changed location*/
+		new_size = vbt->checksum; /*checksum contains lo size byte*/
+		/*LSB of oaktrail_gct contains hi size byte*/
+		new_size |= ((0xff & (unsigned int)(long)vbt->oaktrail_gct)) << 8;
+
+		vbt->checksum = vbt->size; /*size contains the checksum*/
+		if (new_size > 0xff)
+			vbt->size = 0xff; /*restrict size to 255*/
+		else
+			vbt->size = new_size;
+
+		/* number of descriptors defined in the GCT */
+		number_desc = ((0xff00 & (unsigned int)(long)vbt->oaktrail_gct)) >> 8;
+		bpi = ((0xff0000 & (unsigned int)(long)vbt->oaktrail_gct)) >> 16;
+		vbt->oaktrail_gct = ioremap(addr + GCT_R10_HEADER_SIZE,
+				GCT_R10_DISPLAY_DESC_SIZE * number_desc);
+		pGCT = vbt->oaktrail_gct;
+		pGCT = (u8 *)pGCT + (bpi*GCT_R10_DISPLAY_DESC_SIZE);
+		dev_priv->gct_data.bpi = bpi; /*save boot panel id*/
+
+		/*copy the GCT display timings into a temp structure*/
+		memcpy(&ti, pGCT, sizeof(struct gct_r10_timing_info));
+
+		/*now copy the temp struct into the dev_priv->gct_data*/
+		dp_ti->pixel_clock = ti.pixel_clock;
+		dp_ti->hactive_hi = ti.hactive_hi;
+		dp_ti->hactive_lo = ti.hactive_lo;
+		dp_ti->hblank_hi = ti.hblank_hi;
+		dp_ti->hblank_lo = ti.hblank_lo;
+		dp_ti->hsync_offset_hi = ti.hsync_offset_hi;
+		dp_ti->hsync_offset_lo = ti.hsync_offset_lo;
+		dp_ti->hsync_pulse_width_hi = ti.hsync_pulse_width_hi;
+		dp_ti->hsync_pulse_width_lo = ti.hsync_pulse_width_lo;
+		dp_ti->vactive_hi = ti.vactive_hi;
+		dp_ti->vactive_lo = ti.vactive_lo;
+		dp_ti->vblank_hi = ti.vblank_hi;
+		dp_ti->vblank_lo = ti.vblank_lo;
+		dp_ti->vsync_offset_hi = ti.vsync_offset_hi;
+		dp_ti->vsync_offset_lo = ti.vsync_offset_lo;
+		dp_ti->vsync_pulse_width_hi = ti.vsync_pulse_width_hi;
+		dp_ti->vsync_pulse_width_lo = ti.vsync_pulse_width_lo;
+
+		/* Move the MIPI_Display_Descriptor data from GCT to dev priv */
+		dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+							*((u8 *)pGCT + 0x0d);
+		dev_priv->gct_data.Panel_MIPI_Display_Descriptor |=
+						(*((u8 *)pGCT + 0x0e)) << 8;
+		break;
+	default:
+		dev_err(dev->dev, "Unknown revision of GCT!\n");
+		vbt->size = 0;
+	}
+}
+
+int mid_chip_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	mid_get_fuse_settings(dev);
+	mid_get_vbt_data(dev_priv);
+	mid_get_pci_revID(dev_priv);
+	return 0;
+}
diff --git a/drivers/gpu/drm/gma500/mid_bios.h b/drivers/gpu/drm/gma500/mid_bios.h
new file mode 100644
index 000000000000..00e7d564b7eb
--- /dev/null
+++ b/drivers/gpu/drm/gma500/mid_bios.h
@@ -0,0 +1,21 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+extern int mid_chip_setup(struct drm_device *dev);
+
diff --git a/drivers/gpu/drm/gma500/mmu.c b/drivers/gpu/drm/gma500/mmu.c
new file mode 100644
index 000000000000..c904d73b1de3
--- /dev/null
+++ b/drivers/gpu/drm/gma500/mmu.c
@@ -0,0 +1,858 @@
+/**************************************************************************
+ * Copyright (c) 2007, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+#include <drm/drmP.h>
+#include "psb_drv.h"
+#include "psb_reg.h"
+
+/*
+ * Code for the SGX MMU:
+ */
+
+/*
+ * clflush on one processor only:
+ * clflush should apparently flush the cache line on all processors in an
+ * SMP system.
+ */
+
+/*
+ * kmap atomic:
+ * The usage of the slots must be completely encapsulated within a spinlock, and
+ * no other functions that may be using the locks for other purposed may be
+ * called from within the locked region.
+ * Since the slots are per processor, this will guarantee that we are the only
+ * user.
+ */
+
+/*
+ * TODO: Inserting ptes from an interrupt handler:
+ * This may be desirable for some SGX functionality where the GPU can fault in
+ * needed pages. For that, we need to make an atomic insert_pages function, that
+ * may fail.
+ * If it fails, the caller need to insert the page using a workqueue function,
+ * but on average it should be fast.
+ */
+
+struct psb_mmu_driver {
+	/* protects driver- and pd structures. Always take in read mode
+	 * before taking the page table spinlock.
+	 */
+	struct rw_semaphore sem;
+
+	/* protects page tables, directory tables and pt tables.
+	 * and pt structures.
+	 */
+	spinlock_t lock;
+
+	atomic_t needs_tlbflush;
+
+	uint8_t __iomem *register_map;
+	struct psb_mmu_pd *default_pd;
+	/*uint32_t bif_ctrl;*/
+	int has_clflush;
+	int clflush_add;
+	unsigned long clflush_mask;
+
+	struct drm_psb_private *dev_priv;
+};
+
+struct psb_mmu_pd;
+
+struct psb_mmu_pt {
+	struct psb_mmu_pd *pd;
+	uint32_t index;
+	uint32_t count;
+	struct page *p;
+	uint32_t *v;
+};
+
+struct psb_mmu_pd {
+	struct psb_mmu_driver *driver;
+	int hw_context;
+	struct psb_mmu_pt **tables;
+	struct page *p;
+	struct page *dummy_pt;
+	struct page *dummy_page;
+	uint32_t pd_mask;
+	uint32_t invalid_pde;
+	uint32_t invalid_pte;
+};
+
+static inline uint32_t psb_mmu_pt_index(uint32_t offset)
+{
+	return (offset >> PSB_PTE_SHIFT) & 0x3FF;
+}
+
+static inline uint32_t psb_mmu_pd_index(uint32_t offset)
+{
+	return offset >> PSB_PDE_SHIFT;
+}
+
+static inline void psb_clflush(void *addr)
+{
+	__asm__ __volatile__("clflush (%0)\n" : : "r"(addr) : "memory");
+}
+
+static inline void psb_mmu_clflush(struct psb_mmu_driver *driver,
+				   void *addr)
+{
+	if (!driver->has_clflush)
+		return;
+
+	mb();
+	psb_clflush(addr);
+	mb();
+}
+
+static void psb_page_clflush(struct psb_mmu_driver *driver, struct page* page)
+{
+	uint32_t clflush_add = driver->clflush_add >> PAGE_SHIFT;
+	uint32_t clflush_count = PAGE_SIZE / clflush_add;
+	int i;
+	uint8_t *clf;
+
+	clf = kmap_atomic(page, KM_USER0);
+	mb();
+	for (i = 0; i < clflush_count; ++i) {
+		psb_clflush(clf);
+		clf += clflush_add;
+	}
+	mb();
+	kunmap_atomic(clf, KM_USER0);
+}
+
+static void psb_pages_clflush(struct psb_mmu_driver *driver,
+				struct page *page[], unsigned long num_pages)
+{
+	int i;
+
+	if (!driver->has_clflush)
+		return ;
+
+	for (i = 0; i < num_pages; i++)
+		psb_page_clflush(driver, *page++);
+}
+
+static void psb_mmu_flush_pd_locked(struct psb_mmu_driver *driver,
+				    int force)
+{
+	atomic_set(&driver->needs_tlbflush, 0);
+}
+
+static void psb_mmu_flush_pd(struct psb_mmu_driver *driver, int force)
+{
+	down_write(&driver->sem);
+	psb_mmu_flush_pd_locked(driver, force);
+	up_write(&driver->sem);
+}
+
+void psb_mmu_flush(struct psb_mmu_driver *driver, int rc_prot)
+{
+	if (rc_prot)
+		down_write(&driver->sem);
+	if (rc_prot)
+		up_write(&driver->sem);
+}
+
+void psb_mmu_set_pd_context(struct psb_mmu_pd *pd, int hw_context)
+{
+	/*ttm_tt_cache_flush(&pd->p, 1);*/
+	psb_pages_clflush(pd->driver, &pd->p, 1);
+	down_write(&pd->driver->sem);
+	wmb();
+	psb_mmu_flush_pd_locked(pd->driver, 1);
+	pd->hw_context = hw_context;
+	up_write(&pd->driver->sem);
+
+}
+
+static inline unsigned long psb_pd_addr_end(unsigned long addr,
+					    unsigned long end)
+{
+
+	addr = (addr + PSB_PDE_MASK + 1) & ~PSB_PDE_MASK;
+	return (addr < end) ? addr : end;
+}
+
+static inline uint32_t psb_mmu_mask_pte(uint32_t pfn, int type)
+{
+	uint32_t mask = PSB_PTE_VALID;
+
+	if (type & PSB_MMU_CACHED_MEMORY)
+		mask |= PSB_PTE_CACHED;
+	if (type & PSB_MMU_RO_MEMORY)
+		mask |= PSB_PTE_RO;
+	if (type & PSB_MMU_WO_MEMORY)
+		mask |= PSB_PTE_WO;
+
+	return (pfn << PAGE_SHIFT) | mask;
+}
+
+struct psb_mmu_pd *psb_mmu_alloc_pd(struct psb_mmu_driver *driver,
+				    int trap_pagefaults, int invalid_type)
+{
+	struct psb_mmu_pd *pd = kmalloc(sizeof(*pd), GFP_KERNEL);
+	uint32_t *v;
+	int i;
+
+	if (!pd)
+		return NULL;
+
+	pd->p = alloc_page(GFP_DMA32);
+	if (!pd->p)
+		goto out_err1;
+	pd->dummy_pt = alloc_page(GFP_DMA32);
+	if (!pd->dummy_pt)
+		goto out_err2;
+	pd->dummy_page = alloc_page(GFP_DMA32);
+	if (!pd->dummy_page)
+		goto out_err3;
+
+	if (!trap_pagefaults) {
+		pd->invalid_pde =
+		    psb_mmu_mask_pte(page_to_pfn(pd->dummy_pt),
+				     invalid_type);
+		pd->invalid_pte =
+		    psb_mmu_mask_pte(page_to_pfn(pd->dummy_page),
+				     invalid_type);
+	} else {
+		pd->invalid_pde = 0;
+		pd->invalid_pte = 0;
+	}
+
+	v = kmap(pd->dummy_pt);
+	for (i = 0; i < (PAGE_SIZE / sizeof(uint32_t)); ++i)
+		v[i] = pd->invalid_pte;
+
+	kunmap(pd->dummy_pt);
+
+	v = kmap(pd->p);
+	for (i = 0; i < (PAGE_SIZE / sizeof(uint32_t)); ++i)
+		v[i] = pd->invalid_pde;
+
+	kunmap(pd->p);
+
+	clear_page(kmap(pd->dummy_page));
+	kunmap(pd->dummy_page);
+
+	pd->tables = vmalloc_user(sizeof(struct psb_mmu_pt *) * 1024);
+	if (!pd->tables)
+		goto out_err4;
+
+	pd->hw_context = -1;
+	pd->pd_mask = PSB_PTE_VALID;
+	pd->driver = driver;
+
+	return pd;
+
+out_err4:
+	__free_page(pd->dummy_page);
+out_err3:
+	__free_page(pd->dummy_pt);
+out_err2:
+	__free_page(pd->p);
+out_err1:
+	kfree(pd);
+	return NULL;
+}
+
+void psb_mmu_free_pt(struct psb_mmu_pt *pt)
+{
+	__free_page(pt->p);
+	kfree(pt);
+}
+
+void psb_mmu_free_pagedir(struct psb_mmu_pd *pd)
+{
+	struct psb_mmu_driver *driver = pd->driver;
+	struct psb_mmu_pt *pt;
+	int i;
+
+	down_write(&driver->sem);
+	if (pd->hw_context != -1)
+		psb_mmu_flush_pd_locked(driver, 1);
+
+	/* Should take the spinlock here, but we don't need to do that
+	   since we have the semaphore in write mode. */
+
+	for (i = 0; i < 1024; ++i) {
+		pt = pd->tables[i];
+		if (pt)
+			psb_mmu_free_pt(pt);
+	}
+
+	vfree(pd->tables);
+	__free_page(pd->dummy_page);
+	__free_page(pd->dummy_pt);
+	__free_page(pd->p);
+	kfree(pd);
+	up_write(&driver->sem);
+}
+
+static struct psb_mmu_pt *psb_mmu_alloc_pt(struct psb_mmu_pd *pd)
+{
+	struct psb_mmu_pt *pt = kmalloc(sizeof(*pt), GFP_KERNEL);
+	void *v;
+	uint32_t clflush_add = pd->driver->clflush_add >> PAGE_SHIFT;
+	uint32_t clflush_count = PAGE_SIZE / clflush_add;
+	spinlock_t *lock = &pd->driver->lock;
+	uint8_t *clf;
+	uint32_t *ptes;
+	int i;
+
+	if (!pt)
+		return NULL;
+
+	pt->p = alloc_page(GFP_DMA32);
+	if (!pt->p) {
+		kfree(pt);
+		return NULL;
+	}
+
+	spin_lock(lock);
+
+	v = kmap_atomic(pt->p, KM_USER0);
+	clf = (uint8_t *) v;
+	ptes = (uint32_t *) v;
+	for (i = 0; i < (PAGE_SIZE / sizeof(uint32_t)); ++i)
+		*ptes++ = pd->invalid_pte;
+
+
+	if (pd->driver->has_clflush && pd->hw_context != -1) {
+		mb();
+		for (i = 0; i < clflush_count; ++i) {
+			psb_clflush(clf);
+			clf += clflush_add;
+		}
+		mb();
+	}
+
+	kunmap_atomic(v, KM_USER0);
+	spin_unlock(lock);
+
+	pt->count = 0;
+	pt->pd = pd;
+	pt->index = 0;
+
+	return pt;
+}
+
+struct psb_mmu_pt *psb_mmu_pt_alloc_map_lock(struct psb_mmu_pd *pd,
+					     unsigned long addr)
+{
+	uint32_t index = psb_mmu_pd_index(addr);
+	struct psb_mmu_pt *pt;
+	uint32_t *v;
+	spinlock_t *lock = &pd->driver->lock;
+
+	spin_lock(lock);
+	pt = pd->tables[index];
+	while (!pt) {
+		spin_unlock(lock);
+		pt = psb_mmu_alloc_pt(pd);
+		if (!pt)
+			return NULL;
+		spin_lock(lock);
+
+		if (pd->tables[index]) {
+			spin_unlock(lock);
+			psb_mmu_free_pt(pt);
+			spin_lock(lock);
+			pt = pd->tables[index];
+			continue;
+		}
+
+		v = kmap_atomic(pd->p, KM_USER0);
+		pd->tables[index] = pt;
+		v[index] = (page_to_pfn(pt->p) << 12) | pd->pd_mask;
+		pt->index = index;
+		kunmap_atomic((void *) v, KM_USER0);
+
+		if (pd->hw_context != -1) {
+			psb_mmu_clflush(pd->driver, (void *) &v[index]);
+			atomic_set(&pd->driver->needs_tlbflush, 1);
+		}
+	}
+	pt->v = kmap_atomic(pt->p, KM_USER0);
+	return pt;
+}
+
+static struct psb_mmu_pt *psb_mmu_pt_map_lock(struct psb_mmu_pd *pd,
+					      unsigned long addr)
+{
+	uint32_t index = psb_mmu_pd_index(addr);
+	struct psb_mmu_pt *pt;
+	spinlock_t *lock = &pd->driver->lock;
+
+	spin_lock(lock);
+	pt = pd->tables[index];
+	if (!pt) {
+		spin_unlock(lock);
+		return NULL;
+	}
+	pt->v = kmap_atomic(pt->p, KM_USER0);
+	return pt;
+}
+
+static void psb_mmu_pt_unmap_unlock(struct psb_mmu_pt *pt)
+{
+	struct psb_mmu_pd *pd = pt->pd;
+	uint32_t *v;
+
+	kunmap_atomic(pt->v, KM_USER0);
+	if (pt->count == 0) {
+		v = kmap_atomic(pd->p, KM_USER0);
+		v[pt->index] = pd->invalid_pde;
+		pd->tables[pt->index] = NULL;
+
+		if (pd->hw_context != -1) {
+			psb_mmu_clflush(pd->driver,
+					(void *) &v[pt->index]);
+			atomic_set(&pd->driver->needs_tlbflush, 1);
+		}
+		kunmap_atomic(pt->v, KM_USER0);
+		spin_unlock(&pd->driver->lock);
+		psb_mmu_free_pt(pt);
+		return;
+	}
+	spin_unlock(&pd->driver->lock);
+}
+
+static inline void psb_mmu_set_pte(struct psb_mmu_pt *pt,
+				   unsigned long addr, uint32_t pte)
+{
+	pt->v[psb_mmu_pt_index(addr)] = pte;
+}
+
+static inline void psb_mmu_invalidate_pte(struct psb_mmu_pt *pt,
+					  unsigned long addr)
+{
+	pt->v[psb_mmu_pt_index(addr)] = pt->pd->invalid_pte;
+}
+
+
+void psb_mmu_mirror_gtt(struct psb_mmu_pd *pd,
+			uint32_t mmu_offset, uint32_t gtt_start,
+			uint32_t gtt_pages)
+{
+	uint32_t *v;
+	uint32_t start = psb_mmu_pd_index(mmu_offset);
+	struct psb_mmu_driver *driver = pd->driver;
+	int num_pages = gtt_pages;
+
+	down_read(&driver->sem);
+	spin_lock(&driver->lock);
+
+	v = kmap_atomic(pd->p, KM_USER0);
+	v += start;
+
+	while (gtt_pages--) {
+		*v++ = gtt_start | pd->pd_mask;
+		gtt_start += PAGE_SIZE;
+	}
+
+	/*ttm_tt_cache_flush(&pd->p, num_pages);*/
+	psb_pages_clflush(pd->driver, &pd->p, num_pages);
+	kunmap_atomic(v, KM_USER0);
+	spin_unlock(&driver->lock);
+
+	if (pd->hw_context != -1)
+		atomic_set(&pd->driver->needs_tlbflush, 1);
+
+	up_read(&pd->driver->sem);
+	psb_mmu_flush_pd(pd->driver, 0);
+}
+
+struct psb_mmu_pd *psb_mmu_get_default_pd(struct psb_mmu_driver *driver)
+{
+	struct psb_mmu_pd *pd;
+
+	/* down_read(&driver->sem); */
+	pd = driver->default_pd;
+	/* up_read(&driver->sem); */
+
+	return pd;
+}
+
+/* Returns the physical address of the PD shared by sgx/msvdx */
+uint32_t psb_get_default_pd_addr(struct psb_mmu_driver *driver)
+{
+	struct psb_mmu_pd *pd;
+
+	pd = psb_mmu_get_default_pd(driver);
+	return page_to_pfn(pd->p) << PAGE_SHIFT;
+}
+
+void psb_mmu_driver_takedown(struct psb_mmu_driver *driver)
+{
+	psb_mmu_free_pagedir(driver->default_pd);
+	kfree(driver);
+}
+
+struct psb_mmu_driver *psb_mmu_driver_init(uint8_t __iomem * registers,
+					int trap_pagefaults,
+					int invalid_type,
+					struct drm_psb_private *dev_priv)
+{
+	struct psb_mmu_driver *driver;
+
+	driver = kmalloc(sizeof(*driver), GFP_KERNEL);
+
+	if (!driver)
+		return NULL;
+	driver->dev_priv = dev_priv;
+
+	driver->default_pd = psb_mmu_alloc_pd(driver, trap_pagefaults,
+					      invalid_type);
+	if (!driver->default_pd)
+		goto out_err1;
+
+	spin_lock_init(&driver->lock);
+	init_rwsem(&driver->sem);
+	down_write(&driver->sem);
+	driver->register_map = registers;
+	atomic_set(&driver->needs_tlbflush, 1);
+
+	driver->has_clflush = 0;
+
+	if (boot_cpu_has(X86_FEATURE_CLFLSH)) {
+		uint32_t tfms, misc, cap0, cap4, clflush_size;
+
+		/*
+		 * clflush size is determined at kernel setup for x86_64
+		 *  but not for i386. We have to do it here.
+		 */
+
+		cpuid(0x00000001, &tfms, &misc, &cap0, &cap4);
+		clflush_size = ((misc >> 8) & 0xff) * 8;
+		driver->has_clflush = 1;
+		driver->clflush_add =
+		    PAGE_SIZE * clflush_size / sizeof(uint32_t);
+		driver->clflush_mask = driver->clflush_add - 1;
+		driver->clflush_mask = ~driver->clflush_mask;
+	}
+
+	up_write(&driver->sem);
+	return driver;
+
+out_err1:
+	kfree(driver);
+	return NULL;
+}
+
+static void psb_mmu_flush_ptes(struct psb_mmu_pd *pd,
+			       unsigned long address, uint32_t num_pages,
+			       uint32_t desired_tile_stride,
+			       uint32_t hw_tile_stride)
+{
+	struct psb_mmu_pt *pt;
+	uint32_t rows = 1;
+	uint32_t i;
+	unsigned long addr;
+	unsigned long end;
+	unsigned long next;
+	unsigned long add;
+	unsigned long row_add;
+	unsigned long clflush_add = pd->driver->clflush_add;
+	unsigned long clflush_mask = pd->driver->clflush_mask;
+
+	if (!pd->driver->has_clflush) {
+		/*ttm_tt_cache_flush(&pd->p, num_pages);*/
+		psb_pages_clflush(pd->driver, &pd->p, num_pages);
+		return;
+	}
+
+	if (hw_tile_stride)
+		rows = num_pages / desired_tile_stride;
+	else
+		desired_tile_stride = num_pages;
+
+	add = desired_tile_stride << PAGE_SHIFT;
+	row_add = hw_tile_stride << PAGE_SHIFT;
+	mb();
+	for (i = 0; i < rows; ++i) {
+
+		addr = address;
+		end = addr + add;
+
+		do {
+			next = psb_pd_addr_end(addr, end);
+			pt = psb_mmu_pt_map_lock(pd, addr);
+			if (!pt)
+				continue;
+			do {
+				psb_clflush(&pt->v
+					    [psb_mmu_pt_index(addr)]);
+			} while (addr +=
+				 clflush_add,
+				 (addr & clflush_mask) < next);
+
+			psb_mmu_pt_unmap_unlock(pt);
+		} while (addr = next, next != end);
+		address += row_add;
+	}
+	mb();
+}
+
+void psb_mmu_remove_pfn_sequence(struct psb_mmu_pd *pd,
+				 unsigned long address, uint32_t num_pages)
+{
+	struct psb_mmu_pt *pt;
+	unsigned long addr;
+	unsigned long end;
+	unsigned long next;
+	unsigned long f_address = address;
+
+	down_read(&pd->driver->sem);
+
+	addr = address;
+	end = addr + (num_pages << PAGE_SHIFT);
+
+	do {
+		next = psb_pd_addr_end(addr, end);
+		pt = psb_mmu_pt_alloc_map_lock(pd, addr);
+		if (!pt)
+			goto out;
+		do {
+			psb_mmu_invalidate_pte(pt, addr);
+			--pt->count;
+		} while (addr += PAGE_SIZE, addr < next);
+		psb_mmu_pt_unmap_unlock(pt);
+
+	} while (addr = next, next != end);
+
+out:
+	if (pd->hw_context != -1)
+		psb_mmu_flush_ptes(pd, f_address, num_pages, 1, 1);
+
+	up_read(&pd->driver->sem);
+
+	if (pd->hw_context != -1)
+		psb_mmu_flush(pd->driver, 0);
+
+	return;
+}
+
+void psb_mmu_remove_pages(struct psb_mmu_pd *pd, unsigned long address,
+			  uint32_t num_pages, uint32_t desired_tile_stride,
+			  uint32_t hw_tile_stride)
+{
+	struct psb_mmu_pt *pt;
+	uint32_t rows = 1;
+	uint32_t i;
+	unsigned long addr;
+	unsigned long end;
+	unsigned long next;
+	unsigned long add;
+	unsigned long row_add;
+	unsigned long f_address = address;
+
+	if (hw_tile_stride)
+		rows = num_pages / desired_tile_stride;
+	else
+		desired_tile_stride = num_pages;
+
+	add = desired_tile_stride << PAGE_SHIFT;
+	row_add = hw_tile_stride << PAGE_SHIFT;
+
+	/* down_read(&pd->driver->sem); */
+
+	/* Make sure we only need to flush this processor's cache */
+
+	for (i = 0; i < rows; ++i) {
+
+		addr = address;
+		end = addr + add;
+
+		do {
+			next = psb_pd_addr_end(addr, end);
+			pt = psb_mmu_pt_map_lock(pd, addr);
+			if (!pt)
+				continue;
+			do {
+				psb_mmu_invalidate_pte(pt, addr);
+				--pt->count;
+
+			} while (addr += PAGE_SIZE, addr < next);
+			psb_mmu_pt_unmap_unlock(pt);
+
+		} while (addr = next, next != end);
+		address += row_add;
+	}
+	if (pd->hw_context != -1)
+		psb_mmu_flush_ptes(pd, f_address, num_pages,
+				   desired_tile_stride, hw_tile_stride);
+
+	/* up_read(&pd->driver->sem); */
+
+	if (pd->hw_context != -1)
+		psb_mmu_flush(pd->driver, 0);
+}
+
+int psb_mmu_insert_pfn_sequence(struct psb_mmu_pd *pd, uint32_t start_pfn,
+				unsigned long address, uint32_t num_pages,
+				int type)
+{
+	struct psb_mmu_pt *pt;
+	uint32_t pte;
+	unsigned long addr;
+	unsigned long end;
+	unsigned long next;
+	unsigned long f_address = address;
+	int ret = 0;
+
+	down_read(&pd->driver->sem);
+
+	addr = address;
+	end = addr + (num_pages << PAGE_SHIFT);
+
+	do {
+		next = psb_pd_addr_end(addr, end);
+		pt = psb_mmu_pt_alloc_map_lock(pd, addr);
+		if (!pt) {
+			ret = -ENOMEM;
+			goto out;
+		}
+		do {
+			pte = psb_mmu_mask_pte(start_pfn++, type);
+			psb_mmu_set_pte(pt, addr, pte);
+			pt->count++;
+		} while (addr += PAGE_SIZE, addr < next);
+		psb_mmu_pt_unmap_unlock(pt);
+
+	} while (addr = next, next != end);
+
+out:
+	if (pd->hw_context != -1)
+		psb_mmu_flush_ptes(pd, f_address, num_pages, 1, 1);
+
+	up_read(&pd->driver->sem);
+
+	if (pd->hw_context != -1)
+		psb_mmu_flush(pd->driver, 1);
+
+	return ret;
+}
+
+int psb_mmu_insert_pages(struct psb_mmu_pd *pd, struct page **pages,
+			 unsigned long address, uint32_t num_pages,
+			 uint32_t desired_tile_stride,
+			 uint32_t hw_tile_stride, int type)
+{
+	struct psb_mmu_pt *pt;
+	uint32_t rows = 1;
+	uint32_t i;
+	uint32_t pte;
+	unsigned long addr;
+	unsigned long end;
+	unsigned long next;
+	unsigned long add;
+	unsigned long row_add;
+	unsigned long f_address = address;
+	int ret = 0;
+
+	if (hw_tile_stride) {
+		if (num_pages % desired_tile_stride != 0)
+			return -EINVAL;
+		rows = num_pages / desired_tile_stride;
+	} else {
+		desired_tile_stride = num_pages;
+	}
+
+	add = desired_tile_stride << PAGE_SHIFT;
+	row_add = hw_tile_stride << PAGE_SHIFT;
+
+	down_read(&pd->driver->sem);
+
+	for (i = 0; i < rows; ++i) {
+
+		addr = address;
+		end = addr + add;
+
+		do {
+			next = psb_pd_addr_end(addr, end);
+			pt = psb_mmu_pt_alloc_map_lock(pd, addr);
+			if (!pt) {
+				ret = -ENOMEM;
+				goto out;
+			}
+			do {
+				pte =
+				    psb_mmu_mask_pte(page_to_pfn(*pages++),
+						     type);
+				psb_mmu_set_pte(pt, addr, pte);
+				pt->count++;
+			} while (addr += PAGE_SIZE, addr < next);
+			psb_mmu_pt_unmap_unlock(pt);
+
+		} while (addr = next, next != end);
+
+		address += row_add;
+	}
+out:
+	if (pd->hw_context != -1)
+		psb_mmu_flush_ptes(pd, f_address, num_pages,
+				   desired_tile_stride, hw_tile_stride);
+
+	up_read(&pd->driver->sem);
+
+	if (pd->hw_context != -1)
+		psb_mmu_flush(pd->driver, 1);
+
+	return ret;
+}
+
+int psb_mmu_virtual_to_pfn(struct psb_mmu_pd *pd, uint32_t virtual,
+			   unsigned long *pfn)
+{
+	int ret;
+	struct psb_mmu_pt *pt;
+	uint32_t tmp;
+	spinlock_t *lock = &pd->driver->lock;
+
+	down_read(&pd->driver->sem);
+	pt = psb_mmu_pt_map_lock(pd, virtual);
+	if (!pt) {
+		uint32_t *v;
+
+		spin_lock(lock);
+		v = kmap_atomic(pd->p, KM_USER0);
+		tmp = v[psb_mmu_pd_index(virtual)];
+		kunmap_atomic(v, KM_USER0);
+		spin_unlock(lock);
+
+		if (tmp != pd->invalid_pde || !(tmp & PSB_PTE_VALID) ||
+		    !(pd->invalid_pte & PSB_PTE_VALID)) {
+			ret = -EINVAL;
+			goto out;
+		}
+		ret = 0;
+		*pfn = pd->invalid_pte >> PAGE_SHIFT;
+		goto out;
+	}
+	tmp = pt->v[psb_mmu_pt_index(virtual)];
+	if (!(tmp & PSB_PTE_VALID)) {
+		ret = -EINVAL;
+	} else {
+		ret = 0;
+		*pfn = tmp >> PAGE_SHIFT;
+	}
+	psb_mmu_pt_unmap_unlock(pt);
+out:
+	up_read(&pd->driver->sem);
+	return ret;
+}
diff --git a/drivers/gpu/drm/gma500/oaktrail.h b/drivers/gpu/drm/gma500/oaktrail.h
new file mode 100644
index 000000000000..2da1f368f14e
--- /dev/null
+++ b/drivers/gpu/drm/gma500/oaktrail.h
@@ -0,0 +1,252 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+/* MID device specific descriptors */
+
+struct oaktrail_vbt {
+	s8 signature[4];	/*4 bytes,"$GCT" */
+	u8 revision;
+	u8 size;
+	u8 checksum;
+	void *oaktrail_gct;
+} __packed;
+
+struct oaktrail_timing_info {
+	u16 pixel_clock;
+	u8 hactive_lo;
+	u8 hblank_lo;
+	u8 hblank_hi:4;
+	u8 hactive_hi:4;
+	u8 vactive_lo;
+	u8 vblank_lo;
+	u8 vblank_hi:4;
+	u8 vactive_hi:4;
+	u8 hsync_offset_lo;
+	u8 hsync_pulse_width_lo;
+	u8 vsync_pulse_width_lo:4;
+	u8 vsync_offset_lo:4;
+	u8 vsync_pulse_width_hi:2;
+	u8 vsync_offset_hi:2;
+	u8 hsync_pulse_width_hi:2;
+	u8 hsync_offset_hi:2;
+	u8 width_mm_lo;
+	u8 height_mm_lo;
+	u8 height_mm_hi:4;
+	u8 width_mm_hi:4;
+	u8 hborder;
+	u8 vborder;
+	u8 unknown0:1;
+	u8 hsync_positive:1;
+	u8 vsync_positive:1;
+	u8 separate_sync:2;
+	u8 stereo:1;
+	u8 unknown6:1;
+	u8 interlaced:1;
+} __packed;
+
+struct gct_r10_timing_info {
+	u16 pixel_clock;
+	u32 hactive_lo:8;
+	u32 hactive_hi:4;
+	u32 hblank_lo:8;
+	u32 hblank_hi:4;
+	u32 hsync_offset_lo:8;
+	u16 hsync_offset_hi:2;
+	u16 hsync_pulse_width_lo:8;
+	u16 hsync_pulse_width_hi:2;
+	u16 hsync_positive:1;
+	u16 rsvd_1:3;
+	u8  vactive_lo:8;
+	u16 vactive_hi:4;
+	u16 vblank_lo:8;
+	u16 vblank_hi:4;
+	u16 vsync_offset_lo:4;
+	u16 vsync_offset_hi:2;
+	u16 vsync_pulse_width_lo:4;
+	u16 vsync_pulse_width_hi:2;
+	u16 vsync_positive:1;
+	u16 rsvd_2:3;
+} __packed;
+
+struct oaktrail_panel_descriptor_v1 {
+	u32 Panel_Port_Control; /* 1 dword, Register 0x61180 if LVDS */
+				/* 0x61190 if MIPI */
+	u32 Panel_Power_On_Sequencing;/*1 dword,Register 0x61208,*/
+	u32 Panel_Power_Off_Sequencing;/*1 dword,Register 0x6120C,*/
+	u32 Panel_Power_Cycle_Delay_and_Reference_Divisor;/* 1 dword */
+						/* Register 0x61210 */
+	struct oaktrail_timing_info DTD;/*18 bytes, Standard definition */
+	u16 Panel_Backlight_Inverter_Descriptor;/* 16 bits, as follows */
+				/* Bit 0, Frequency, 15 bits,0 - 32767Hz */
+			/* Bit 15, Polarity, 1 bit, 0: Normal, 1: Inverted */
+	u16 Panel_MIPI_Display_Descriptor;
+			/*16 bits, Defined as follows: */
+			/* if MIPI, 0x0000 if LVDS */
+			/* Bit 0, Type, 2 bits, */
+			/* 0: Type-1, */
+			/* 1: Type-2, */
+			/* 2: Type-3, */
+			/* 3: Type-4 */
+			/* Bit 2, Pixel Format, 4 bits */
+			/* Bit0: 16bpp (not supported in LNC), */
+			/* Bit1: 18bpp loosely packed, */
+			/* Bit2: 18bpp packed, */
+			/* Bit3: 24bpp */
+			/* Bit 6, Reserved, 2 bits, 00b */
+			/* Bit 8, Minimum Supported Frame Rate, 6 bits, 0 - 63Hz */
+			/* Bit 14, Reserved, 2 bits, 00b */
+} __packed;
+
+struct oaktrail_panel_descriptor_v2 {
+	u32 Panel_Port_Control; /* 1 dword, Register 0x61180 if LVDS */
+				/* 0x61190 if MIPI */
+	u32 Panel_Power_On_Sequencing;/*1 dword,Register 0x61208,*/
+	u32 Panel_Power_Off_Sequencing;/*1 dword,Register 0x6120C,*/
+	u8 Panel_Power_Cycle_Delay_and_Reference_Divisor;/* 1 byte */
+						/* Register 0x61210 */
+	struct oaktrail_timing_info DTD;/*18 bytes, Standard definition */
+	u16 Panel_Backlight_Inverter_Descriptor;/*16 bits, as follows*/
+				/*Bit 0, Frequency, 16 bits, 0 - 32767Hz*/
+	u8 Panel_Initial_Brightness;/* [7:0] 0 - 100% */
+			/*Bit 7, Polarity, 1 bit,0: Normal, 1: Inverted*/
+	u16 Panel_MIPI_Display_Descriptor;
+			/*16 bits, Defined as follows: */
+			/* if MIPI, 0x0000 if LVDS */
+			/* Bit 0, Type, 2 bits, */
+			/* 0: Type-1, */
+			/* 1: Type-2, */
+			/* 2: Type-3, */
+			/* 3: Type-4 */
+			/* Bit 2, Pixel Format, 4 bits */
+			/* Bit0: 16bpp (not supported in LNC), */
+			/* Bit1: 18bpp loosely packed, */
+			/* Bit2: 18bpp packed, */
+			/* Bit3: 24bpp */
+			/* Bit 6, Reserved, 2 bits, 00b */
+			/* Bit 8, Minimum Supported Frame Rate, 6 bits, 0 - 63Hz */
+			/* Bit 14, Reserved, 2 bits, 00b */
+} __packed;
+
+union oaktrail_panel_rx {
+	struct {
+		u16 NumberOfLanes:2; /*Num of Lanes, 2 bits,0 = 1 lane,*/
+			/* 1 = 2 lanes, 2 = 3 lanes, 3 = 4 lanes. */
+		u16 MaxLaneFreq:3; /* 0: 100MHz, 1: 200MHz, 2: 300MHz, */
+		/*3: 400MHz, 4: 500MHz, 5: 600MHz, 6: 700MHz, 7: 800MHz.*/
+		u16 SupportedVideoTransferMode:2; /*0: Non-burst only */
+					/* 1: Burst and non-burst */
+					/* 2/3: Reserved */
+		u16 HSClkBehavior:1; /*0: Continuous, 1: Non-continuous*/
+		u16 DuoDisplaySupport:1; /*1 bit,0: No, 1: Yes*/
+		u16 ECC_ChecksumCapabilities:1;/*1 bit,0: No, 1: Yes*/
+		u16 BidirectionalCommunication:1;/*1 bit,0: No, 1: Yes */
+		u16 Rsvd:5;/*5 bits,00000b */
+	} panelrx;
+	u16 panel_receiver;
+} __packed;
+
+struct oaktrail_gct_v1 {
+	union { /*8 bits,Defined as follows: */
+		struct {
+			u8 PanelType:4; /*4 bits, Bit field for panels*/
+					/* 0 - 3: 0 = LVDS, 1 = MIPI*/
+					/*2 bits,Specifies which of the*/
+			u8 BootPanelIndex:2;
+					/* 4 panels to use by default*/
+			u8 BootMIPI_DSI_RxIndex:2;/*Specifies which of*/
+					/* the 4 MIPI DSI receivers to use*/
+		} PD;
+		u8 PanelDescriptor;
+	};
+	struct oaktrail_panel_descriptor_v1 panel[4];/*panel descrs,38 bytes each*/
+	union oaktrail_panel_rx panelrx[4]; /* panel receivers*/
+} __packed;
+
+struct oaktrail_gct_v2 {
+	union { /*8 bits,Defined as follows: */
+		struct {
+			u8 PanelType:4; /*4 bits, Bit field for panels*/
+					/* 0 - 3: 0 = LVDS, 1 = MIPI*/
+					/*2 bits,Specifies which of the*/
+			u8 BootPanelIndex:2;
+					/* 4 panels to use by default*/
+			u8 BootMIPI_DSI_RxIndex:2;/*Specifies which of*/
+					/* the 4 MIPI DSI receivers to use*/
+		} PD;
+		u8 PanelDescriptor;
+	};
+	struct oaktrail_panel_descriptor_v2 panel[4];/*panel descrs,38 bytes each*/
+	union oaktrail_panel_rx panelrx[4]; /* panel receivers*/
+} __packed;
+
+struct oaktrail_gct_data {
+	u8 bpi; /* boot panel index, number of panel used during boot */
+	u8 pt; /* panel type, 4 bit field, 0=lvds, 1=mipi */
+	struct oaktrail_timing_info DTD; /* timing info for the selected panel */
+	u32 Panel_Port_Control;
+	u32 PP_On_Sequencing;/*1 dword,Register 0x61208,*/
+	u32 PP_Off_Sequencing;/*1 dword,Register 0x6120C,*/
+	u32 PP_Cycle_Delay;
+	u16 Panel_Backlight_Inverter_Descriptor;
+	u16 Panel_MIPI_Display_Descriptor;
+} __packed;
+
+#define MODE_SETTING_IN_CRTC		0x1
+#define MODE_SETTING_IN_ENCODER		0x2
+#define MODE_SETTING_ON_GOING		0x3
+#define MODE_SETTING_IN_DSR		0x4
+#define MODE_SETTING_ENCODER_DONE	0x8
+
+#define GCT_R10_HEADER_SIZE		16
+#define GCT_R10_DISPLAY_DESC_SIZE	28
+
+/*
+ *	Moorestown HDMI interfaces
+ */
+
+struct oaktrail_hdmi_dev {
+	struct pci_dev *dev;
+	void __iomem *regs;
+	unsigned int mmio, mmio_len;
+	int dpms_mode;
+	struct hdmi_i2c_dev *i2c_dev;
+
+	/* register state */
+	u32 saveDPLL_CTRL;
+	u32 saveDPLL_DIV_CTRL;
+	u32 saveDPLL_ADJUST;
+	u32 saveDPLL_UPDATE;
+	u32 saveDPLL_CLK_ENABLE;
+	u32 savePCH_HTOTAL_B;
+	u32 savePCH_HBLANK_B;
+	u32 savePCH_HSYNC_B;
+	u32 savePCH_VTOTAL_B;
+	u32 savePCH_VBLANK_B;
+	u32 savePCH_VSYNC_B;
+	u32 savePCH_PIPEBCONF;
+	u32 savePCH_PIPEBSRC;
+};
+
+extern void oaktrail_hdmi_setup(struct drm_device *dev);
+extern void oaktrail_hdmi_teardown(struct drm_device *dev);
+extern int  oaktrail_hdmi_i2c_init(struct pci_dev *dev);
+extern void oaktrail_hdmi_i2c_exit(struct pci_dev *dev);
+extern void oaktrail_hdmi_save(struct drm_device *dev);
+extern void oaktrail_hdmi_restore(struct drm_device *dev);
+extern void oaktrail_hdmi_init(struct drm_device *dev, struct psb_intel_mode_device *mode_dev);
diff --git a/drivers/gpu/drm/gma500/oaktrail_crtc.c b/drivers/gpu/drm/gma500/oaktrail_crtc.c
new file mode 100644
index 000000000000..9d12a3ee1600
--- /dev/null
+++ b/drivers/gpu/drm/gma500/oaktrail_crtc.c
@@ -0,0 +1,604 @@
+/*
+ * Copyright © 2009 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+
+#include <drm/drmP.h>
+#include "framebuffer.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_display.h"
+#include "power.h"
+
+struct psb_intel_range_t {
+	int min, max;
+};
+
+struct oaktrail_limit_t {
+	struct psb_intel_range_t dot, m, p1;
+};
+
+struct oaktrail_clock_t {
+	/* derived values */
+	int dot;
+	int m;
+	int p1;
+};
+
+#define MRST_LIMIT_LVDS_100L	    0
+#define MRST_LIMIT_LVDS_83	    1
+#define MRST_LIMIT_LVDS_100	    2
+
+#define MRST_DOT_MIN		  19750
+#define MRST_DOT_MAX		  120000
+#define MRST_M_MIN_100L		    20
+#define MRST_M_MIN_100		    10
+#define MRST_M_MIN_83		    12
+#define MRST_M_MAX_100L		    34
+#define MRST_M_MAX_100		    17
+#define MRST_M_MAX_83		    20
+#define MRST_P1_MIN		    2
+#define MRST_P1_MAX_0		    7
+#define MRST_P1_MAX_1		    8
+
+static const struct oaktrail_limit_t oaktrail_limits[] = {
+	{			/* MRST_LIMIT_LVDS_100L */
+	 .dot = {.min = MRST_DOT_MIN, .max = MRST_DOT_MAX},
+	 .m = {.min = MRST_M_MIN_100L, .max = MRST_M_MAX_100L},
+	 .p1 = {.min = MRST_P1_MIN, .max = MRST_P1_MAX_1},
+	 },
+	{			/* MRST_LIMIT_LVDS_83L */
+	 .dot = {.min = MRST_DOT_MIN, .max = MRST_DOT_MAX},
+	 .m = {.min = MRST_M_MIN_83, .max = MRST_M_MAX_83},
+	 .p1 = {.min = MRST_P1_MIN, .max = MRST_P1_MAX_0},
+	 },
+	{			/* MRST_LIMIT_LVDS_100 */
+	 .dot = {.min = MRST_DOT_MIN, .max = MRST_DOT_MAX},
+	 .m = {.min = MRST_M_MIN_100, .max = MRST_M_MAX_100},
+	 .p1 = {.min = MRST_P1_MIN, .max = MRST_P1_MAX_1},
+	 },
+};
+
+#define MRST_M_MIN	    10
+static const u32 oaktrail_m_converts[] = {
+	0x2B, 0x15, 0x2A, 0x35, 0x1A, 0x0D, 0x26, 0x33, 0x19, 0x2C,
+	0x36, 0x3B, 0x1D, 0x2E, 0x37, 0x1B, 0x2D, 0x16, 0x0B, 0x25,
+	0x12, 0x09, 0x24, 0x32, 0x39, 0x1c,
+};
+
+static const struct oaktrail_limit_t *oaktrail_limit(struct drm_crtc *crtc)
+{
+	const struct oaktrail_limit_t *limit = NULL;
+	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)
+	    || psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_MIPI)) {
+		switch (dev_priv->core_freq) {
+		case 100:
+			limit = &oaktrail_limits[MRST_LIMIT_LVDS_100L];
+			break;
+		case 166:
+			limit = &oaktrail_limits[MRST_LIMIT_LVDS_83];
+			break;
+		case 200:
+			limit = &oaktrail_limits[MRST_LIMIT_LVDS_100];
+			break;
+		}
+	} else {
+		limit = NULL;
+		dev_err(dev->dev, "oaktrail_limit Wrong display type.\n");
+	}
+
+	return limit;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 8xx chips. */
+static void oaktrail_clock(int refclk, struct oaktrail_clock_t *clock)
+{
+	clock->dot = (refclk * clock->m) / (14 * clock->p1);
+}
+
+void mrstPrintPll(char *prefix, struct oaktrail_clock_t *clock)
+{
+	pr_debug("%s: dotclock = %d,  m = %d, p1 = %d.\n",
+	     prefix, clock->dot, clock->m, clock->p1);
+}
+
+/**
+ * Returns a set of divisors for the desired target clock with the given refclk,
+ * or FALSE.  Divisor values are the actual divisors for
+ */
+static bool
+mrstFindBestPLL(struct drm_crtc *crtc, int target, int refclk,
+		struct oaktrail_clock_t *best_clock)
+{
+	struct oaktrail_clock_t clock;
+	const struct oaktrail_limit_t *limit = oaktrail_limit(crtc);
+	int err = target;
+
+	memset(best_clock, 0, sizeof(*best_clock));
+
+	for (clock.m = limit->m.min; clock.m <= limit->m.max; clock.m++) {
+		for (clock.p1 = limit->p1.min; clock.p1 <= limit->p1.max;
+		     clock.p1++) {
+			int this_err;
+
+			oaktrail_clock(refclk, &clock);
+
+			this_err = abs(clock.dot - target);
+			if (this_err < err) {
+				*best_clock = clock;
+				err = this_err;
+			}
+		}
+	}
+	dev_dbg(crtc->dev->dev, "mrstFindBestPLL err = %d.\n", err);
+	return err != target;
+}
+
+/**
+ * Sets the power management mode of the pipe and plane.
+ *
+ * This code should probably grow support for turning the cursor off and back
+ * on appropriately at the same time as we're turning the pipe off/on.
+ */
+static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	int dspbase_reg = (pipe == 0) ? MRST_DSPABASE : DSPBBASE;
+	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	u32 temp;
+	bool enabled;
+
+	if (!gma_power_begin(dev, true))
+		return;
+
+	/* XXX: When our outputs are all unaware of DPMS modes other than off
+	 * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
+	 */
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+		/* Enable the DPLL */
+		temp = REG_READ(dpll_reg);
+		if ((temp & DPLL_VCO_ENABLE) == 0) {
+			REG_WRITE(dpll_reg, temp);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+		}
+		/* Enable the pipe */
+		temp = REG_READ(pipeconf_reg);
+		if ((temp & PIPEACONF_ENABLE) == 0)
+			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+		/* Enable the plane */
+		temp = REG_READ(dspcntr_reg);
+		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+			REG_WRITE(dspcntr_reg,
+				  temp | DISPLAY_PLANE_ENABLE);
+			/* Flush the plane changes */
+			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+		}
+
+		psb_intel_crtc_load_lut(crtc);
+
+		/* Give the overlay scaler a chance to enable
+		   if it's on this pipe */
+		/* psb_intel_crtc_dpms_video(crtc, true); TODO */
+		break;
+	case DRM_MODE_DPMS_OFF:
+		/* Give the overlay scaler a chance to disable
+		 * if it's on this pipe */
+		/* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
+
+		/* Disable the VGA plane that we never use */
+		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+		/* Disable display plane */
+		temp = REG_READ(dspcntr_reg);
+		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+			REG_WRITE(dspcntr_reg,
+				  temp & ~DISPLAY_PLANE_ENABLE);
+			/* Flush the plane changes */
+			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_READ(dspbase_reg);
+		}
+
+		/* Next, disable display pipes */
+		temp = REG_READ(pipeconf_reg);
+		if ((temp & PIPEACONF_ENABLE) != 0) {
+			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
+			REG_READ(pipeconf_reg);
+		}
+		/* Wait for for the pipe disable to take effect. */
+		psb_intel_wait_for_vblank(dev);
+
+		temp = REG_READ(dpll_reg);
+		if ((temp & DPLL_VCO_ENABLE) != 0) {
+			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+		}
+
+		/* Wait for the clocks to turn off. */
+		udelay(150);
+		break;
+	}
+
+	enabled = crtc->enabled && mode != DRM_MODE_DPMS_OFF;
+
+	/*Set FIFO Watermarks*/
+	REG_WRITE(DSPARB, 0x3FFF);
+	REG_WRITE(DSPFW1, 0x3F88080A);
+	REG_WRITE(DSPFW2, 0x0b060808);
+	REG_WRITE(DSPFW3, 0x0);
+	REG_WRITE(DSPFW4, 0x08030404);
+	REG_WRITE(DSPFW5, 0x04040404);
+	REG_WRITE(DSPFW6, 0x78);
+	REG_WRITE(0x70400, REG_READ(0x70400) | 0x4000);
+	/* Must write Bit 14 of the Chicken Bit Register */
+
+	gma_power_end(dev);
+}
+
+/**
+ * Return the pipe currently connected to the panel fitter,
+ * or -1 if the panel fitter is not present or not in use
+ */
+static int oaktrail_panel_fitter_pipe(struct drm_device *dev)
+{
+	u32 pfit_control;
+
+	pfit_control = REG_READ(PFIT_CONTROL);
+
+	/* See if the panel fitter is in use */
+	if ((pfit_control & PFIT_ENABLE) == 0)
+		return -1;
+	return (pfit_control >> 29) & 3;
+}
+
+static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
+			      struct drm_display_mode *mode,
+			      struct drm_display_mode *adjusted_mode,
+			      int x, int y,
+			      struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int pipe = psb_intel_crtc->pipe;
+	int fp_reg = (pipe == 0) ? MRST_FPA0 : FPB0;
+	int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
+	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
+	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
+	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
+	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
+	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
+	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	int refclk = 0;
+	struct oaktrail_clock_t clock;
+	u32 dpll = 0, fp = 0, dspcntr, pipeconf;
+	bool ok, is_sdvo = false;
+	bool is_crt = false, is_lvds = false, is_tv = false;
+	bool is_mipi = false;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct psb_intel_encoder *psb_intel_encoder = NULL;
+	uint64_t scalingType = DRM_MODE_SCALE_FULLSCREEN;
+	struct drm_connector *connector;
+
+	if (!gma_power_begin(dev, true))
+		return 0;
+
+	memcpy(&psb_intel_crtc->saved_mode,
+		mode,
+		sizeof(struct drm_display_mode));
+	memcpy(&psb_intel_crtc->saved_adjusted_mode,
+		adjusted_mode,
+		sizeof(struct drm_display_mode));
+
+	list_for_each_entry(connector, &mode_config->connector_list, head) {
+		if (!connector->encoder || connector->encoder->crtc != crtc)
+			continue;
+
+		psb_intel_encoder = psb_intel_attached_encoder(connector);
+
+		switch (psb_intel_encoder->type) {
+		case INTEL_OUTPUT_LVDS:
+			is_lvds = true;
+			break;
+		case INTEL_OUTPUT_SDVO:
+			is_sdvo = true;
+			break;
+		case INTEL_OUTPUT_TVOUT:
+			is_tv = true;
+			break;
+		case INTEL_OUTPUT_ANALOG:
+			is_crt = true;
+			break;
+		case INTEL_OUTPUT_MIPI:
+			is_mipi = true;
+			break;
+		}
+	}
+
+	/* Disable the VGA plane that we never use */
+	REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+	/* Disable the panel fitter if it was on our pipe */
+	if (oaktrail_panel_fitter_pipe(dev) == pipe)
+		REG_WRITE(PFIT_CONTROL, 0);
+
+	REG_WRITE(pipesrc_reg,
+		  ((mode->crtc_hdisplay - 1) << 16) |
+		  (mode->crtc_vdisplay - 1));
+
+	if (psb_intel_encoder)
+		drm_connector_property_get_value(connector,
+			dev->mode_config.scaling_mode_property, &scalingType);
+
+	if (scalingType == DRM_MODE_SCALE_NO_SCALE) {
+		/* Moorestown doesn't have register support for centering so
+		 * we need to mess with the h/vblank and h/vsync start and
+		 * ends to get centering */
+		int offsetX = 0, offsetY = 0;
+
+		offsetX = (adjusted_mode->crtc_hdisplay -
+			   mode->crtc_hdisplay) / 2;
+		offsetY = (adjusted_mode->crtc_vdisplay -
+			   mode->crtc_vdisplay) / 2;
+
+		REG_WRITE(htot_reg, (mode->crtc_hdisplay - 1) |
+			((adjusted_mode->crtc_htotal - 1) << 16));
+		REG_WRITE(vtot_reg, (mode->crtc_vdisplay - 1) |
+			((adjusted_mode->crtc_vtotal - 1) << 16));
+		REG_WRITE(hblank_reg,
+			(adjusted_mode->crtc_hblank_start - offsetX - 1) |
+			((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16));
+		REG_WRITE(hsync_reg,
+			(adjusted_mode->crtc_hsync_start - offsetX - 1) |
+			((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16));
+		REG_WRITE(vblank_reg,
+			(adjusted_mode->crtc_vblank_start - offsetY - 1) |
+			((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16));
+		REG_WRITE(vsync_reg,
+			(adjusted_mode->crtc_vsync_start - offsetY - 1) |
+			((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16));
+	} else {
+		REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+			((adjusted_mode->crtc_htotal - 1) << 16));
+		REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+			((adjusted_mode->crtc_vtotal - 1) << 16));
+		REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+			((adjusted_mode->crtc_hblank_end - 1) << 16));
+		REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+			((adjusted_mode->crtc_hsync_end - 1) << 16));
+		REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+			((adjusted_mode->crtc_vblank_end - 1) << 16));
+		REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+			((adjusted_mode->crtc_vsync_end - 1) << 16));
+	}
+
+	/* Flush the plane changes */
+	{
+		struct drm_crtc_helper_funcs *crtc_funcs =
+		    crtc->helper_private;
+		crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+	}
+
+	/* setup pipeconf */
+	pipeconf = REG_READ(pipeconf_reg);
+
+	/* Set up the display plane register */
+	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr |= DISPPLANE_GAMMA_ENABLE;
+
+	if (pipe == 0)
+		dspcntr |= DISPPLANE_SEL_PIPE_A;
+	else
+		dspcntr |= DISPPLANE_SEL_PIPE_B;
+
+	dev_priv->dspcntr = dspcntr |= DISPLAY_PLANE_ENABLE;
+	dev_priv->pipeconf = pipeconf |= PIPEACONF_ENABLE;
+
+	if (is_mipi)
+		goto oaktrail_crtc_mode_set_exit;
+
+	refclk = dev_priv->core_freq * 1000;
+
+	dpll = 0;		/*BIT16 = 0 for 100MHz reference */
+
+	ok = mrstFindBestPLL(crtc, adjusted_mode->clock, refclk, &clock);
+
+	if (!ok) {
+		dev_dbg(dev->dev, "mrstFindBestPLL fail in oaktrail_crtc_mode_set.\n");
+	} else {
+		dev_dbg(dev->dev, "oaktrail_crtc_mode_set pixel clock = %d,"
+			 "m = %x, p1 = %x.\n", clock.dot, clock.m,
+			 clock.p1);
+	}
+
+	fp = oaktrail_m_converts[(clock.m - MRST_M_MIN)] << 8;
+
+	dpll |= DPLL_VGA_MODE_DIS;
+
+
+	dpll |= DPLL_VCO_ENABLE;
+
+	if (is_lvds)
+		dpll |= DPLLA_MODE_LVDS;
+	else
+		dpll |= DPLLB_MODE_DAC_SERIAL;
+
+	if (is_sdvo) {
+		int sdvo_pixel_multiply =
+		    adjusted_mode->clock / mode->clock;
+
+		dpll |= DPLL_DVO_HIGH_SPEED;
+		dpll |=
+		    (sdvo_pixel_multiply -
+		     1) << SDVO_MULTIPLIER_SHIFT_HIRES;
+	}
+
+
+	/* compute bitmask from p1 value */
+	dpll |= (1 << (clock.p1 - 2)) << 17;
+
+	dpll |= DPLL_VCO_ENABLE;
+
+	mrstPrintPll("chosen", &clock);
+
+	if (dpll & DPLL_VCO_ENABLE) {
+		REG_WRITE(fp_reg, fp);
+		REG_WRITE(dpll_reg, dpll & ~DPLL_VCO_ENABLE);
+		REG_READ(dpll_reg);
+		/* Check the DPLLA lock bit PIPEACONF[29] */
+		udelay(150);
+	}
+
+	REG_WRITE(fp_reg, fp);
+	REG_WRITE(dpll_reg, dpll);
+	REG_READ(dpll_reg);
+	/* Wait for the clocks to stabilize. */
+	udelay(150);
+
+	/* write it again -- the BIOS does, after all */
+	REG_WRITE(dpll_reg, dpll);
+	REG_READ(dpll_reg);
+	/* Wait for the clocks to stabilize. */
+	udelay(150);
+
+	REG_WRITE(pipeconf_reg, pipeconf);
+	REG_READ(pipeconf_reg);
+	psb_intel_wait_for_vblank(dev);
+
+	REG_WRITE(dspcntr_reg, dspcntr);
+	psb_intel_wait_for_vblank(dev);
+
+oaktrail_crtc_mode_set_exit:
+	gma_power_end(dev);
+	return 0;
+}
+
+static bool oaktrail_crtc_mode_fixup(struct drm_crtc *crtc,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+int oaktrail_pipe_set_base(struct drm_crtc *crtc,
+			    int x, int y, struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
+	int pipe = psb_intel_crtc->pipe;
+	unsigned long start, offset;
+
+	int dspbase = (pipe == 0 ? DSPALINOFF : DSPBBASE);
+	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
+	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	u32 dspcntr;
+	int ret = 0;
+
+	/* no fb bound */
+	if (!crtc->fb) {
+		dev_dbg(dev->dev, "No FB bound\n");
+		return 0;
+	}
+
+	if (!gma_power_begin(dev, true))
+		return 0;
+
+	start = psbfb->gtt->offset;
+	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
+
+	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+
+	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		dspcntr |= DISPPLANE_8BPP;
+		break;
+	case 16:
+		if (crtc->fb->depth == 15)
+			dspcntr |= DISPPLANE_15_16BPP;
+		else
+			dspcntr |= DISPPLANE_16BPP;
+		break;
+	case 24:
+	case 32:
+		dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
+		break;
+	default:
+		dev_err(dev->dev, "Unknown color depth\n");
+		ret = -EINVAL;
+		goto pipe_set_base_exit;
+	}
+	REG_WRITE(dspcntr_reg, dspcntr);
+
+	REG_WRITE(dspbase, offset);
+	REG_READ(dspbase);
+	REG_WRITE(dspsurf, start);
+	REG_READ(dspsurf);
+
+pipe_set_base_exit:
+	gma_power_end(dev);
+	return ret;
+}
+
+static void oaktrail_crtc_prepare(struct drm_crtc *crtc)
+{
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
+static void oaktrail_crtc_commit(struct drm_crtc *crtc)
+{
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+const struct drm_crtc_helper_funcs oaktrail_helper_funcs = {
+	.dpms = oaktrail_crtc_dpms,
+	.mode_fixup = oaktrail_crtc_mode_fixup,
+	.mode_set = oaktrail_crtc_mode_set,
+	.mode_set_base = oaktrail_pipe_set_base,
+	.prepare = oaktrail_crtc_prepare,
+	.commit = oaktrail_crtc_commit,
+};
+
diff --git a/drivers/gpu/drm/gma500/oaktrail_device.c b/drivers/gpu/drm/gma500/oaktrail_device.c
new file mode 100644
index 000000000000..63aea2f010d9
--- /dev/null
+++ b/drivers/gpu/drm/gma500/oaktrail_device.c
@@ -0,0 +1,512 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+#include <linux/backlight.h>
+#include <linux/module.h>
+#include <linux/dmi.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "gma_drm.h"
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include <asm/mrst.h>
+#include <asm/intel_scu_ipc.h>
+#include "mid_bios.h"
+#include "intel_bios.h"
+
+static int oaktrail_output_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	if (dev_priv->iLVDS_enable)
+		oaktrail_lvds_init(dev, &dev_priv->mode_dev);
+	else
+		dev_err(dev->dev, "DSI is not supported\n");
+	if (dev_priv->hdmi_priv)
+		oaktrail_hdmi_init(dev, &dev_priv->mode_dev);
+	return 0;
+}
+
+/*
+ *	Provide the low level interfaces for the Moorestown backlight
+ */
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+
+#define MRST_BLC_MAX_PWM_REG_FREQ	    0xFFFF
+#define BLC_PWM_PRECISION_FACTOR 100	/* 10000000 */
+#define BLC_PWM_FREQ_CALC_CONSTANT 32
+#define MHz 1000000
+#define BLC_ADJUSTMENT_MAX 100
+
+static struct backlight_device *oaktrail_backlight_device;
+static int oaktrail_brightness;
+
+static int oaktrail_set_brightness(struct backlight_device *bd)
+{
+	struct drm_device *dev = bl_get_data(oaktrail_backlight_device);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int level = bd->props.brightness;
+	u32 blc_pwm_ctl;
+	u32 max_pwm_blc;
+
+	/* Percentage 1-100% being valid */
+	if (level < 1)
+		level = 1;
+
+	if (gma_power_begin(dev, 0)) {
+		/* Calculate and set the brightness value */
+		max_pwm_blc = REG_READ(BLC_PWM_CTL) >> 16;
+		blc_pwm_ctl = level * max_pwm_blc / 100;
+
+		/* Adjust the backlight level with the percent in
+		 * dev_priv->blc_adj1;
+		 */
+		blc_pwm_ctl = blc_pwm_ctl * dev_priv->blc_adj1;
+		blc_pwm_ctl = blc_pwm_ctl / 100;
+
+		/* Adjust the backlight level with the percent in
+		 * dev_priv->blc_adj2;
+		 */
+		blc_pwm_ctl = blc_pwm_ctl * dev_priv->blc_adj2;
+		blc_pwm_ctl = blc_pwm_ctl / 100;
+
+		/* force PWM bit on */
+		REG_WRITE(BLC_PWM_CTL2, (0x80000000 | REG_READ(BLC_PWM_CTL2)));
+		REG_WRITE(BLC_PWM_CTL, (max_pwm_blc << 16) | blc_pwm_ctl);
+		gma_power_end(dev);
+	}
+	oaktrail_brightness = level;
+	return 0;
+}
+
+static int oaktrail_get_brightness(struct backlight_device *bd)
+{
+	/* return locally cached var instead of HW read (due to DPST etc.) */
+	/* FIXME: ideally return actual value in case firmware fiddled with
+	   it */
+	return oaktrail_brightness;
+}
+
+static int device_backlight_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned long core_clock;
+	u16 bl_max_freq;
+	uint32_t value;
+	uint32_t blc_pwm_precision_factor;
+
+	dev_priv->blc_adj1 = BLC_ADJUSTMENT_MAX;
+	dev_priv->blc_adj2 = BLC_ADJUSTMENT_MAX;
+	bl_max_freq = 256;
+	/* this needs to be set elsewhere */
+	blc_pwm_precision_factor = BLC_PWM_PRECISION_FACTOR;
+
+	core_clock = dev_priv->core_freq;
+
+	value = (core_clock * MHz) / BLC_PWM_FREQ_CALC_CONSTANT;
+	value *= blc_pwm_precision_factor;
+	value /= bl_max_freq;
+	value /= blc_pwm_precision_factor;
+
+	if (value > (unsigned long long)MRST_BLC_MAX_PWM_REG_FREQ)
+			return -ERANGE;
+
+	if (gma_power_begin(dev, false)) {
+		REG_WRITE(BLC_PWM_CTL2, (0x80000000 | REG_READ(BLC_PWM_CTL2)));
+		REG_WRITE(BLC_PWM_CTL, value | (value << 16));
+		gma_power_end(dev);
+	}
+	return 0;
+}
+
+static const struct backlight_ops oaktrail_ops = {
+	.get_brightness = oaktrail_get_brightness,
+	.update_status  = oaktrail_set_brightness,
+};
+
+int oaktrail_backlight_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int ret;
+	struct backlight_properties props;
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.max_brightness = 100;
+	props.type = BACKLIGHT_PLATFORM;
+
+	oaktrail_backlight_device = backlight_device_register("oaktrail-bl",
+				NULL, (void *)dev, &oaktrail_ops, &props);
+
+	if (IS_ERR(oaktrail_backlight_device))
+		return PTR_ERR(oaktrail_backlight_device);
+
+	ret = device_backlight_init(dev);
+	if (ret < 0) {
+		backlight_device_unregister(oaktrail_backlight_device);
+		return ret;
+	}
+	oaktrail_backlight_device->props.brightness = 100;
+	oaktrail_backlight_device->props.max_brightness = 100;
+	backlight_update_status(oaktrail_backlight_device);
+	dev_priv->backlight_device = oaktrail_backlight_device;
+	return 0;
+}
+
+#endif
+
+/*
+ *	Provide the Moorestown specific chip logic and low level methods
+ *	for power management
+ */
+
+static void oaktrail_init_pm(struct drm_device *dev)
+{
+}
+
+/**
+ *	oaktrail_save_display_registers	-	save registers lost on suspend
+ *	@dev: our DRM device
+ *
+ *	Save the state we need in order to be able to restore the interface
+ *	upon resume from suspend
+ */
+static int oaktrail_save_display_registers(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int i;
+	u32 pp_stat;
+
+	/* Display arbitration control + watermarks */
+	dev_priv->saveDSPARB = PSB_RVDC32(DSPARB);
+	dev_priv->saveDSPFW1 = PSB_RVDC32(DSPFW1);
+	dev_priv->saveDSPFW2 = PSB_RVDC32(DSPFW2);
+	dev_priv->saveDSPFW3 = PSB_RVDC32(DSPFW3);
+	dev_priv->saveDSPFW4 = PSB_RVDC32(DSPFW4);
+	dev_priv->saveDSPFW5 = PSB_RVDC32(DSPFW5);
+	dev_priv->saveDSPFW6 = PSB_RVDC32(DSPFW6);
+	dev_priv->saveCHICKENBIT = PSB_RVDC32(DSPCHICKENBIT);
+
+	/* Pipe & plane A info */
+	dev_priv->savePIPEACONF = PSB_RVDC32(PIPEACONF);
+	dev_priv->savePIPEASRC = PSB_RVDC32(PIPEASRC);
+	dev_priv->saveFPA0 = PSB_RVDC32(MRST_FPA0);
+	dev_priv->saveFPA1 = PSB_RVDC32(MRST_FPA1);
+	dev_priv->saveDPLL_A = PSB_RVDC32(MRST_DPLL_A);
+	dev_priv->saveHTOTAL_A = PSB_RVDC32(HTOTAL_A);
+	dev_priv->saveHBLANK_A = PSB_RVDC32(HBLANK_A);
+	dev_priv->saveHSYNC_A = PSB_RVDC32(HSYNC_A);
+	dev_priv->saveVTOTAL_A = PSB_RVDC32(VTOTAL_A);
+	dev_priv->saveVBLANK_A = PSB_RVDC32(VBLANK_A);
+	dev_priv->saveVSYNC_A = PSB_RVDC32(VSYNC_A);
+	dev_priv->saveBCLRPAT_A = PSB_RVDC32(BCLRPAT_A);
+	dev_priv->saveDSPACNTR = PSB_RVDC32(DSPACNTR);
+	dev_priv->saveDSPASTRIDE = PSB_RVDC32(DSPASTRIDE);
+	dev_priv->saveDSPAADDR = PSB_RVDC32(DSPABASE);
+	dev_priv->saveDSPASURF = PSB_RVDC32(DSPASURF);
+	dev_priv->saveDSPALINOFF = PSB_RVDC32(DSPALINOFF);
+	dev_priv->saveDSPATILEOFF = PSB_RVDC32(DSPATILEOFF);
+
+	/* Save cursor regs */
+	dev_priv->saveDSPACURSOR_CTRL = PSB_RVDC32(CURACNTR);
+	dev_priv->saveDSPACURSOR_BASE = PSB_RVDC32(CURABASE);
+	dev_priv->saveDSPACURSOR_POS = PSB_RVDC32(CURAPOS);
+
+	/* Save palette (gamma) */
+	for (i = 0; i < 256; i++)
+		dev_priv->save_palette_a[i] = PSB_RVDC32(PALETTE_A + (i << 2));
+
+	if (dev_priv->hdmi_priv)
+		oaktrail_hdmi_save(dev);
+
+	/* Save performance state */
+	dev_priv->savePERF_MODE = PSB_RVDC32(MRST_PERF_MODE);
+
+	/* LVDS state */
+	dev_priv->savePP_CONTROL = PSB_RVDC32(PP_CONTROL);
+	dev_priv->savePFIT_PGM_RATIOS = PSB_RVDC32(PFIT_PGM_RATIOS);
+	dev_priv->savePFIT_AUTO_RATIOS = PSB_RVDC32(PFIT_AUTO_RATIOS);
+	dev_priv->saveBLC_PWM_CTL = PSB_RVDC32(BLC_PWM_CTL);
+	dev_priv->saveBLC_PWM_CTL2 = PSB_RVDC32(BLC_PWM_CTL2);
+	dev_priv->saveLVDS = PSB_RVDC32(LVDS);
+	dev_priv->savePFIT_CONTROL = PSB_RVDC32(PFIT_CONTROL);
+	dev_priv->savePP_ON_DELAYS = PSB_RVDC32(LVDSPP_ON);
+	dev_priv->savePP_OFF_DELAYS = PSB_RVDC32(LVDSPP_OFF);
+	dev_priv->savePP_DIVISOR = PSB_RVDC32(PP_CYCLE);
+
+	/* HW overlay */
+	dev_priv->saveOV_OVADD = PSB_RVDC32(OV_OVADD);
+	dev_priv->saveOV_OGAMC0 = PSB_RVDC32(OV_OGAMC0);
+	dev_priv->saveOV_OGAMC1 = PSB_RVDC32(OV_OGAMC1);
+	dev_priv->saveOV_OGAMC2 = PSB_RVDC32(OV_OGAMC2);
+	dev_priv->saveOV_OGAMC3 = PSB_RVDC32(OV_OGAMC3);
+	dev_priv->saveOV_OGAMC4 = PSB_RVDC32(OV_OGAMC4);
+	dev_priv->saveOV_OGAMC5 = PSB_RVDC32(OV_OGAMC5);
+
+	/* DPST registers */
+	dev_priv->saveHISTOGRAM_INT_CONTROL_REG =
+					PSB_RVDC32(HISTOGRAM_INT_CONTROL);
+	dev_priv->saveHISTOGRAM_LOGIC_CONTROL_REG =
+					PSB_RVDC32(HISTOGRAM_LOGIC_CONTROL);
+	dev_priv->savePWM_CONTROL_LOGIC = PSB_RVDC32(PWM_CONTROL_LOGIC);
+
+	if (dev_priv->iLVDS_enable) {
+		/* Shut down the panel */
+		PSB_WVDC32(0, PP_CONTROL);
+
+		do {
+			pp_stat = PSB_RVDC32(PP_STATUS);
+		} while (pp_stat & 0x80000000);
+
+		/* Turn off the plane */
+		PSB_WVDC32(0x58000000, DSPACNTR);
+		/* Trigger the plane disable */
+		PSB_WVDC32(0, DSPASURF);
+
+		/* Wait ~4 ticks */
+		msleep(4);
+
+		/* Turn off pipe */
+		PSB_WVDC32(0x0, PIPEACONF);
+		/* Wait ~8 ticks */
+		msleep(8);
+
+		/* Turn off PLLs */
+		PSB_WVDC32(0, MRST_DPLL_A);
+	}
+	return 0;
+}
+
+/**
+ *	oaktrail_restore_display_registers	-	restore lost register state
+ *	@dev: our DRM device
+ *
+ *	Restore register state that was lost during suspend and resume.
+ */
+static int oaktrail_restore_display_registers(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 pp_stat;
+	int i;
+
+	/* Display arbitration + watermarks */
+	PSB_WVDC32(dev_priv->saveDSPARB, DSPARB);
+	PSB_WVDC32(dev_priv->saveDSPFW1, DSPFW1);
+	PSB_WVDC32(dev_priv->saveDSPFW2, DSPFW2);
+	PSB_WVDC32(dev_priv->saveDSPFW3, DSPFW3);
+	PSB_WVDC32(dev_priv->saveDSPFW4, DSPFW4);
+	PSB_WVDC32(dev_priv->saveDSPFW5, DSPFW5);
+	PSB_WVDC32(dev_priv->saveDSPFW6, DSPFW6);
+	PSB_WVDC32(dev_priv->saveCHICKENBIT, DSPCHICKENBIT);
+
+	/* Make sure VGA plane is off. it initializes to on after reset!*/
+	PSB_WVDC32(0x80000000, VGACNTRL);
+
+	/* set the plls */
+	PSB_WVDC32(dev_priv->saveFPA0, MRST_FPA0);
+	PSB_WVDC32(dev_priv->saveFPA1, MRST_FPA1);
+
+	/* Actually enable it */
+	PSB_WVDC32(dev_priv->saveDPLL_A, MRST_DPLL_A);
+	DRM_UDELAY(150);
+
+	/* Restore mode */
+	PSB_WVDC32(dev_priv->saveHTOTAL_A, HTOTAL_A);
+	PSB_WVDC32(dev_priv->saveHBLANK_A, HBLANK_A);
+	PSB_WVDC32(dev_priv->saveHSYNC_A, HSYNC_A);
+	PSB_WVDC32(dev_priv->saveVTOTAL_A, VTOTAL_A);
+	PSB_WVDC32(dev_priv->saveVBLANK_A, VBLANK_A);
+	PSB_WVDC32(dev_priv->saveVSYNC_A, VSYNC_A);
+	PSB_WVDC32(dev_priv->savePIPEASRC, PIPEASRC);
+	PSB_WVDC32(dev_priv->saveBCLRPAT_A, BCLRPAT_A);
+
+	/* Restore performance mode*/
+	PSB_WVDC32(dev_priv->savePERF_MODE, MRST_PERF_MODE);
+
+	/* Enable the pipe*/
+	if (dev_priv->iLVDS_enable)
+		PSB_WVDC32(dev_priv->savePIPEACONF, PIPEACONF);
+
+	/* Set up the plane*/
+	PSB_WVDC32(dev_priv->saveDSPALINOFF, DSPALINOFF);
+	PSB_WVDC32(dev_priv->saveDSPASTRIDE, DSPASTRIDE);
+	PSB_WVDC32(dev_priv->saveDSPATILEOFF, DSPATILEOFF);
+
+	/* Enable the plane */
+	PSB_WVDC32(dev_priv->saveDSPACNTR, DSPACNTR);
+	PSB_WVDC32(dev_priv->saveDSPASURF, DSPASURF);
+
+	/* Enable Cursor A */
+	PSB_WVDC32(dev_priv->saveDSPACURSOR_CTRL, CURACNTR);
+	PSB_WVDC32(dev_priv->saveDSPACURSOR_POS, CURAPOS);
+	PSB_WVDC32(dev_priv->saveDSPACURSOR_BASE, CURABASE);
+
+	/* Restore palette (gamma) */
+	for (i = 0; i < 256; i++)
+		PSB_WVDC32(dev_priv->save_palette_a[i], PALETTE_A + (i << 2));
+
+	if (dev_priv->hdmi_priv)
+		oaktrail_hdmi_restore(dev);
+
+	if (dev_priv->iLVDS_enable) {
+		PSB_WVDC32(dev_priv->saveBLC_PWM_CTL2, BLC_PWM_CTL2);
+		PSB_WVDC32(dev_priv->saveLVDS, LVDS); /*port 61180h*/
+		PSB_WVDC32(dev_priv->savePFIT_CONTROL, PFIT_CONTROL);
+		PSB_WVDC32(dev_priv->savePFIT_PGM_RATIOS, PFIT_PGM_RATIOS);
+		PSB_WVDC32(dev_priv->savePFIT_AUTO_RATIOS, PFIT_AUTO_RATIOS);
+		PSB_WVDC32(dev_priv->saveBLC_PWM_CTL, BLC_PWM_CTL);
+		PSB_WVDC32(dev_priv->savePP_ON_DELAYS, LVDSPP_ON);
+		PSB_WVDC32(dev_priv->savePP_OFF_DELAYS, LVDSPP_OFF);
+		PSB_WVDC32(dev_priv->savePP_DIVISOR, PP_CYCLE);
+		PSB_WVDC32(dev_priv->savePP_CONTROL, PP_CONTROL);
+	}
+
+	/* Wait for cycle delay */
+	do {
+		pp_stat = PSB_RVDC32(PP_STATUS);
+	} while (pp_stat & 0x08000000);
+
+	/* Wait for panel power up */
+	do {
+		pp_stat = PSB_RVDC32(PP_STATUS);
+	} while (pp_stat & 0x10000000);
+
+	/* Restore HW overlay */
+	PSB_WVDC32(dev_priv->saveOV_OVADD, OV_OVADD);
+	PSB_WVDC32(dev_priv->saveOV_OGAMC0, OV_OGAMC0);
+	PSB_WVDC32(dev_priv->saveOV_OGAMC1, OV_OGAMC1);
+	PSB_WVDC32(dev_priv->saveOV_OGAMC2, OV_OGAMC2);
+	PSB_WVDC32(dev_priv->saveOV_OGAMC3, OV_OGAMC3);
+	PSB_WVDC32(dev_priv->saveOV_OGAMC4, OV_OGAMC4);
+	PSB_WVDC32(dev_priv->saveOV_OGAMC5, OV_OGAMC5);
+
+	/* DPST registers */
+	PSB_WVDC32(dev_priv->saveHISTOGRAM_INT_CONTROL_REG,
+						HISTOGRAM_INT_CONTROL);
+	PSB_WVDC32(dev_priv->saveHISTOGRAM_LOGIC_CONTROL_REG,
+						HISTOGRAM_LOGIC_CONTROL);
+	PSB_WVDC32(dev_priv->savePWM_CONTROL_LOGIC, PWM_CONTROL_LOGIC);
+
+	return 0;
+}
+
+/**
+ *	oaktrail_power_down	-	power down the display island
+ *	@dev: our DRM device
+ *
+ *	Power down the display interface of our device
+ */
+static int oaktrail_power_down(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 pwr_mask ;
+	u32 pwr_sts;
+
+	pwr_mask = PSB_PWRGT_DISPLAY_MASK;
+	outl(pwr_mask, dev_priv->ospm_base + PSB_PM_SSC);
+
+	while (true) {
+		pwr_sts = inl(dev_priv->ospm_base + PSB_PM_SSS);
+		if ((pwr_sts & pwr_mask) == pwr_mask)
+			break;
+		else
+			udelay(10);
+	}
+	return 0;
+}
+
+/*
+ * oaktrail_power_up
+ *
+ * Restore power to the specified island(s) (powergating)
+ */
+static int oaktrail_power_up(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 pwr_mask = PSB_PWRGT_DISPLAY_MASK;
+	u32 pwr_sts, pwr_cnt;
+
+	pwr_cnt = inl(dev_priv->ospm_base + PSB_PM_SSC);
+	pwr_cnt &= ~pwr_mask;
+	outl(pwr_cnt, (dev_priv->ospm_base + PSB_PM_SSC));
+
+	while (true) {
+		pwr_sts = inl(dev_priv->ospm_base + PSB_PM_SSS);
+		if ((pwr_sts & pwr_mask) == 0)
+			break;
+		else
+			udelay(10);
+	}
+	return 0;
+}
+
+
+static int oaktrail_chip_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
+	int ret;
+	
+	ret = mid_chip_setup(dev);
+	if (ret < 0)
+		return ret;
+	if (vbt->size == 0) {
+		/* Now pull the BIOS data */
+		gma_intel_opregion_init(dev);
+		psb_intel_init_bios(dev);
+	}
+	return 0;
+}
+
+static void oaktrail_teardown(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_vbt *vbt = &dev_priv->vbt_data;
+
+	oaktrail_hdmi_teardown(dev);
+	if (vbt->size == 0)
+		psb_intel_destroy_bios(dev);
+}
+
+const struct psb_ops oaktrail_chip_ops = {
+	.name = "Oaktrail",
+	.accel_2d = 1,
+	.pipes = 2,
+	.crtcs = 2,
+	.sgx_offset = MRST_SGX_OFFSET,
+
+	.chip_setup = oaktrail_chip_setup,
+	.chip_teardown = oaktrail_teardown,
+	.crtc_helper = &oaktrail_helper_funcs,
+	.crtc_funcs = &psb_intel_crtc_funcs,
+
+	.output_init = oaktrail_output_init,
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+	.backlight_init = oaktrail_backlight_init,
+#endif
+
+	.init_pm = oaktrail_init_pm,
+	.save_regs = oaktrail_save_display_registers,
+	.restore_regs = oaktrail_restore_display_registers,
+	.power_down = oaktrail_power_down,
+	.power_up = oaktrail_power_up,
+
+	.i2c_bus = 1,
+};
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi.c b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
new file mode 100644
index 000000000000..36878a60080d
--- /dev/null
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
@@ -0,0 +1,859 @@
+/*
+ * Copyright © 2010 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Li Peng <peng.li@intel.com>
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_drv.h"
+
+#define HDMI_READ(reg)		readl(hdmi_dev->regs + (reg))
+#define HDMI_WRITE(reg, val)	writel(val, hdmi_dev->regs + (reg))
+
+#define HDMI_HCR	0x1000
+#define HCR_ENABLE_HDCP		(1 << 5)
+#define HCR_ENABLE_AUDIO	(1 << 2)
+#define HCR_ENABLE_PIXEL	(1 << 1)
+#define HCR_ENABLE_TMDS		(1 << 0)
+
+#define HDMI_HICR	0x1004
+#define HDMI_HSR	0x1008
+#define HDMI_HISR	0x100C
+#define HDMI_DETECT_HDP		(1 << 0)
+
+#define HDMI_VIDEO_REG	0x3000
+#define HDMI_UNIT_EN		(1 << 7)
+#define HDMI_MODE_OUTPUT	(1 << 0)
+#define HDMI_HBLANK_A	0x3100
+
+#define HDMI_AUDIO_CTRL	0x4000
+#define HDMI_ENABLE_AUDIO	(1 << 0)
+
+#define PCH_HTOTAL_B	0x3100
+#define PCH_HBLANK_B	0x3104
+#define PCH_HSYNC_B	0x3108
+#define PCH_VTOTAL_B	0x310C
+#define PCH_VBLANK_B	0x3110
+#define PCH_VSYNC_B	0x3114
+#define PCH_PIPEBSRC	0x311C
+
+#define PCH_PIPEB_DSL	0x3800
+#define PCH_PIPEB_SLC	0x3804
+#define PCH_PIPEBCONF	0x3808
+#define PCH_PIPEBSTAT	0x3824
+
+#define CDVO_DFT	0x5000
+#define CDVO_SLEWRATE	0x5004
+#define CDVO_STRENGTH	0x5008
+#define CDVO_RCOMP	0x500C
+
+#define DPLL_CTRL       0x6000
+#define DPLL_PDIV_SHIFT		16
+#define DPLL_PDIV_MASK		(0xf << 16)
+#define DPLL_PWRDN		(1 << 4)
+#define DPLL_RESET		(1 << 3)
+#define DPLL_FASTEN		(1 << 2)
+#define DPLL_ENSTAT		(1 << 1)
+#define DPLL_DITHEN		(1 << 0)
+
+#define DPLL_DIV_CTRL   0x6004
+#define DPLL_CLKF_MASK		0xffffffc0
+#define DPLL_CLKR_MASK		(0x3f)
+
+#define DPLL_CLK_ENABLE 0x6008
+#define DPLL_EN_DISP		(1 << 31)
+#define DPLL_SEL_HDMI		(1 << 8)
+#define DPLL_EN_HDMI		(1 << 1)
+#define DPLL_EN_VGA		(1 << 0)
+
+#define DPLL_ADJUST     0x600C
+#define DPLL_STATUS     0x6010
+#define DPLL_UPDATE     0x6014
+#define DPLL_DFT        0x6020
+
+struct intel_range {
+	int	min, max;
+};
+
+struct oaktrail_hdmi_limit {
+	struct intel_range vco, np, nr, nf;
+};
+
+struct oaktrail_hdmi_clock {
+	int np;
+	int nr;
+	int nf;
+	int dot;
+};
+
+#define VCO_MIN		320000
+#define VCO_MAX		1650000
+#define	NP_MIN		1
+#define	NP_MAX		15
+#define	NR_MIN		1
+#define	NR_MAX		64
+#define NF_MIN		2
+#define NF_MAX		4095
+
+static const struct oaktrail_hdmi_limit oaktrail_hdmi_limit = {
+	.vco = { .min = VCO_MIN,		.max = VCO_MAX },
+	.np  = { .min = NP_MIN,			.max = NP_MAX  },
+	.nr  = { .min = NR_MIN,			.max = NR_MAX  },
+	.nf  = { .min = NF_MIN,			.max = NF_MAX  },
+};
+
+static void wait_for_vblank(struct drm_device *dev)
+{
+	/* FIXME: Can we do this as a sleep ? */
+	/* Wait for 20ms, i.e. one cycle at 50hz. */
+	mdelay(20);
+}
+
+static void scu_busy_loop(void *scu_base)
+{
+	u32 status = 0;
+	u32 loop_count = 0;
+
+	status = readl(scu_base + 0x04);
+	while (status & 1) {
+		udelay(1); /* scu processing time is in few u secods */
+		status = readl(scu_base + 0x04);
+		loop_count++;
+		/* break if scu doesn't reset busy bit after huge retry */
+		if (loop_count > 1000) {
+			DRM_DEBUG_KMS("SCU IPC timed out");
+			return;
+		}
+	}
+}
+
+static void oaktrail_hdmi_reset(struct drm_device *dev)
+{
+	void *base;
+	/* FIXME: at least make these defines */
+	unsigned int scu_ipc_mmio = 0xff11c000;
+	int scu_len = 1024;
+
+	base = ioremap((resource_size_t)scu_ipc_mmio, scu_len);
+	if (base == NULL) {
+		DRM_ERROR("failed to map SCU mmio\n");
+		return;
+	}
+
+	/* scu ipc: assert hdmi controller reset */
+	writel(0xff11d118, base + 0x0c);
+	writel(0x7fffffdf, base + 0x80);
+	writel(0x42005, base + 0x0);
+	scu_busy_loop(base);
+
+	/* scu ipc: de-assert hdmi controller reset */
+	writel(0xff11d118, base + 0x0c);
+	writel(0x7fffffff, base + 0x80);
+	writel(0x42005, base + 0x0);
+	scu_busy_loop(base);
+
+	iounmap(base);
+}
+
+static void oaktrail_hdmi_audio_enable(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+
+	HDMI_WRITE(HDMI_HCR, 0x67);
+	HDMI_READ(HDMI_HCR);
+
+	HDMI_WRITE(0x51a8, 0x10);
+	HDMI_READ(0x51a8);
+
+	HDMI_WRITE(HDMI_AUDIO_CTRL, 0x1);
+	HDMI_READ(HDMI_AUDIO_CTRL);
+}
+
+static void oaktrail_hdmi_audio_disable(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+
+	HDMI_WRITE(0x51a8, 0x0);
+	HDMI_READ(0x51a8);
+
+	HDMI_WRITE(HDMI_AUDIO_CTRL, 0x0);
+	HDMI_READ(HDMI_AUDIO_CTRL);
+
+	HDMI_WRITE(HDMI_HCR, 0x47);
+	HDMI_READ(HDMI_HCR);
+}
+
+void oaktrail_crtc_hdmi_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct drm_device *dev = crtc->dev;
+	u32 temp;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_OFF:
+		/* Disable VGACNTRL */
+		REG_WRITE(VGACNTRL, 0x80000000);
+
+		/* Disable plane */
+		temp = REG_READ(DSPBCNTR);
+		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+			REG_WRITE(DSPBCNTR, temp & ~DISPLAY_PLANE_ENABLE);
+			REG_READ(DSPBCNTR);
+			/* Flush the plane changes */
+			REG_WRITE(DSPBSURF, REG_READ(DSPBSURF));
+			REG_READ(DSPBSURF);
+		}
+
+		/* Disable pipe B */
+		temp = REG_READ(PIPEBCONF);
+		if ((temp & PIPEACONF_ENABLE) != 0) {
+			REG_WRITE(PIPEBCONF, temp & ~PIPEACONF_ENABLE);
+			REG_READ(PIPEBCONF);
+		}
+
+		/* Disable LNW Pipes, etc */
+		temp = REG_READ(PCH_PIPEBCONF);
+		if ((temp & PIPEACONF_ENABLE) != 0) {
+			REG_WRITE(PCH_PIPEBCONF, temp & ~PIPEACONF_ENABLE);
+			REG_READ(PCH_PIPEBCONF);
+		}
+		/* wait for pipe off */
+		udelay(150);
+		/* Disable dpll */
+		temp = REG_READ(DPLL_CTRL);
+		if ((temp & DPLL_PWRDN) == 0) {
+			REG_WRITE(DPLL_CTRL, temp | (DPLL_PWRDN | DPLL_RESET));
+			REG_WRITE(DPLL_STATUS, 0x1);
+		}
+		/* wait for dpll off */
+		udelay(150);
+		break;
+	case DRM_MODE_DPMS_ON:
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+		/* Enable dpll */
+		temp = REG_READ(DPLL_CTRL);
+		if ((temp & DPLL_PWRDN) != 0) {
+			REG_WRITE(DPLL_CTRL, temp & ~(DPLL_PWRDN | DPLL_RESET));
+			temp = REG_READ(DPLL_CLK_ENABLE);
+			REG_WRITE(DPLL_CLK_ENABLE, temp | DPLL_EN_DISP | DPLL_SEL_HDMI | DPLL_EN_HDMI);
+			REG_READ(DPLL_CLK_ENABLE);
+		}
+		/* wait for dpll warm up */
+		udelay(150);
+
+		/* Enable pipe B */
+		temp = REG_READ(PIPEBCONF);
+		if ((temp & PIPEACONF_ENABLE) == 0) {
+			REG_WRITE(PIPEBCONF, temp | PIPEACONF_ENABLE);
+			REG_READ(PIPEBCONF);
+		}
+
+		/* Enable LNW Pipe B */
+		temp = REG_READ(PCH_PIPEBCONF);
+		if ((temp & PIPEACONF_ENABLE) == 0) {
+			REG_WRITE(PCH_PIPEBCONF, temp | PIPEACONF_ENABLE);
+			REG_READ(PCH_PIPEBCONF);
+		}
+		wait_for_vblank(dev);
+
+		/* Enable plane */
+		temp = REG_READ(DSPBCNTR);
+		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+			REG_WRITE(DSPBCNTR, temp | DISPLAY_PLANE_ENABLE);
+			/* Flush the plane changes */
+			REG_WRITE(DSPBSURF, REG_READ(DSPBSURF));
+			REG_READ(DSPBSURF);
+		}
+		psb_intel_crtc_load_lut(crtc);
+	}
+	/* DSPARB */
+	REG_WRITE(DSPARB, 0x00003fbf);
+	/* FW1 */
+	REG_WRITE(0x70034, 0x3f880a0a);
+	/* FW2 */
+	REG_WRITE(0x70038, 0x0b060808);
+	/* FW4 */
+	REG_WRITE(0x70050, 0x08030404);
+	/* FW5 */
+	REG_WRITE(0x70054, 0x04040404);
+	/* LNC Chicken Bits */
+	REG_WRITE(0x70400, 0x4000);
+}
+
+
+static void oaktrail_hdmi_dpms(struct drm_encoder *encoder, int mode)
+{
+	static int dpms_mode = -1;
+
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+	u32 temp;
+
+	if (dpms_mode == mode)
+		return;
+
+	if (mode != DRM_MODE_DPMS_ON)
+		temp = 0x0;
+	else
+		temp = 0x99;
+
+	dpms_mode = mode;
+	HDMI_WRITE(HDMI_VIDEO_REG, temp);
+}
+
+static unsigned int htotal_calculate(struct drm_display_mode *mode)
+{
+	u32 htotal, new_crtc_htotal;
+
+	htotal = (mode->crtc_hdisplay - 1) | ((mode->crtc_htotal - 1) << 16);
+
+	/*
+	 * 1024 x 768  new_crtc_htotal = 0x1024;
+	 * 1280 x 1024 new_crtc_htotal = 0x0c34;
+	 */
+	new_crtc_htotal = (mode->crtc_htotal - 1) * 200 * 1000 / mode->clock;
+
+	return (mode->crtc_hdisplay - 1) | (new_crtc_htotal << 16);
+}
+
+static void oaktrail_hdmi_find_dpll(struct drm_crtc *crtc, int target,
+				int refclk, struct oaktrail_hdmi_clock *best_clock)
+{
+	int np_min, np_max, nr_min, nr_max;
+	int np, nr, nf;
+
+	np_min = DIV_ROUND_UP(oaktrail_hdmi_limit.vco.min, target * 10);
+	np_max = oaktrail_hdmi_limit.vco.max / (target * 10);
+	if (np_min < oaktrail_hdmi_limit.np.min)
+		np_min = oaktrail_hdmi_limit.np.min;
+	if (np_max > oaktrail_hdmi_limit.np.max)
+		np_max = oaktrail_hdmi_limit.np.max;
+
+	nr_min = DIV_ROUND_UP((refclk * 1000), (target * 10 * np_max));
+	nr_max = DIV_ROUND_UP((refclk * 1000), (target * 10 * np_min));
+	if (nr_min < oaktrail_hdmi_limit.nr.min)
+		nr_min = oaktrail_hdmi_limit.nr.min;
+	if (nr_max > oaktrail_hdmi_limit.nr.max)
+		nr_max = oaktrail_hdmi_limit.nr.max;
+
+	np = DIV_ROUND_UP((refclk * 1000), (target * 10 * nr_max));
+	nr = DIV_ROUND_UP((refclk * 1000), (target * 10 * np));
+	nf = DIV_ROUND_CLOSEST((target * 10 * np * nr), refclk);
+	DRM_DEBUG_KMS("np, nr, nf %d %d %d\n", np, nr, nf);
+
+	/*
+	 * 1024 x 768  np = 1; nr = 0x26; nf = 0x0fd8000;
+	 * 1280 x 1024 np = 1; nr = 0x17; nf = 0x1034000;
+	 */
+	best_clock->np = np;
+	best_clock->nr = nr - 1;
+	best_clock->nf = (nf << 14);
+}
+
+int oaktrail_crtc_hdmi_mode_set(struct drm_crtc *crtc,
+			    struct drm_display_mode *mode,
+			    struct drm_display_mode *adjusted_mode,
+			    int x, int y,
+			    struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+	int pipe = 1;
+	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
+	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
+	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
+	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
+	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
+	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
+	int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
+	int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
+	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	int refclk;
+	struct oaktrail_hdmi_clock clock;
+	u32 dspcntr, pipeconf, dpll, temp;
+	int dspcntr_reg = DSPBCNTR;
+
+	/* Disable the VGA plane that we never use */
+	REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+	/* XXX: Disable the panel fitter if it was on our pipe */
+
+	/* Disable dpll if necessary */
+	dpll = REG_READ(DPLL_CTRL);
+	if ((dpll & DPLL_PWRDN) == 0) {
+		REG_WRITE(DPLL_CTRL, dpll | (DPLL_PWRDN | DPLL_RESET));
+		REG_WRITE(DPLL_DIV_CTRL, 0x00000000);
+		REG_WRITE(DPLL_STATUS, 0x1);
+	}
+	udelay(150);
+
+	/* reset controller: FIXME - can we sort out the ioremap mess ? */
+	iounmap(hdmi_dev->regs);
+	oaktrail_hdmi_reset(dev);
+
+	/* program and enable dpll */
+	refclk = 25000;
+	oaktrail_hdmi_find_dpll(crtc, adjusted_mode->clock, refclk, &clock);
+
+	/* Setting DPLL */
+	dpll = REG_READ(DPLL_CTRL);
+	dpll &= ~DPLL_PDIV_MASK;
+	dpll &= ~(DPLL_PWRDN | DPLL_RESET);
+	REG_WRITE(DPLL_CTRL, 0x00000008);
+	REG_WRITE(DPLL_DIV_CTRL, ((clock.nf << 6) | clock.nr));
+	REG_WRITE(DPLL_ADJUST, ((clock.nf >> 14) - 1));
+	REG_WRITE(DPLL_CTRL, (dpll | (clock.np << DPLL_PDIV_SHIFT) | DPLL_ENSTAT | DPLL_DITHEN));
+	REG_WRITE(DPLL_UPDATE, 0x80000000);
+	REG_WRITE(DPLL_CLK_ENABLE, 0x80050102);
+	udelay(150);
+
+	hdmi_dev->regs = ioremap(hdmi_dev->mmio, hdmi_dev->mmio_len);
+	if (hdmi_dev->regs == NULL) {
+		DRM_ERROR("failed to do hdmi mmio mapping\n");
+		return -ENOMEM;
+	}
+
+	/* configure HDMI */
+	HDMI_WRITE(0x1004, 0x1fd);
+	HDMI_WRITE(0x2000, 0x1);
+	HDMI_WRITE(0x2008, 0x0);
+	HDMI_WRITE(0x3130, 0x8);
+	HDMI_WRITE(0x101c, 0x1800810);
+
+	temp = htotal_calculate(adjusted_mode);
+	REG_WRITE(htot_reg, temp);
+	REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) | ((adjusted_mode->crtc_hblank_end - 1) << 16));
+	REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) | ((adjusted_mode->crtc_hsync_end - 1) << 16));
+	REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) | ((adjusted_mode->crtc_vtotal - 1) << 16));
+	REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) | ((adjusted_mode->crtc_vblank_end - 1) << 16));
+	REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) | ((adjusted_mode->crtc_vsync_end - 1) << 16));
+	REG_WRITE(pipesrc_reg,
+		((mode->crtc_hdisplay - 1) << 16) |  (mode->crtc_vdisplay - 1));
+
+	REG_WRITE(PCH_HTOTAL_B, (adjusted_mode->crtc_hdisplay - 1) | ((adjusted_mode->crtc_htotal - 1) << 16));
+	REG_WRITE(PCH_HBLANK_B, (adjusted_mode->crtc_hblank_start - 1) | ((adjusted_mode->crtc_hblank_end - 1) << 16));
+	REG_WRITE(PCH_HSYNC_B, (adjusted_mode->crtc_hsync_start - 1) | ((adjusted_mode->crtc_hsync_end - 1) << 16));
+	REG_WRITE(PCH_VTOTAL_B, (adjusted_mode->crtc_vdisplay - 1) | ((adjusted_mode->crtc_vtotal - 1) << 16));
+	REG_WRITE(PCH_VBLANK_B, (adjusted_mode->crtc_vblank_start - 1) | ((adjusted_mode->crtc_vblank_end - 1) << 16));
+	REG_WRITE(PCH_VSYNC_B, (adjusted_mode->crtc_vsync_start - 1) | ((adjusted_mode->crtc_vsync_end - 1) << 16));
+	REG_WRITE(PCH_PIPEBSRC,
+		((mode->crtc_hdisplay - 1) << 16) |  (mode->crtc_vdisplay - 1));
+
+	temp = adjusted_mode->crtc_hblank_end - adjusted_mode->crtc_hblank_start;
+	HDMI_WRITE(HDMI_HBLANK_A, ((adjusted_mode->crtc_hdisplay - 1) << 16) |  temp);
+
+	REG_WRITE(dspsize_reg,
+			((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
+	REG_WRITE(dsppos_reg, 0);
+
+	/* Flush the plane changes */
+	{
+		struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+		crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+	}
+
+	/* Set up the display plane register */
+	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr |= DISPPLANE_GAMMA_ENABLE;
+	dspcntr |= DISPPLANE_SEL_PIPE_B;
+	dspcntr |= DISPLAY_PLANE_ENABLE;
+
+	/* setup pipeconf */
+	pipeconf = REG_READ(pipeconf_reg);
+	pipeconf |= PIPEACONF_ENABLE;
+
+	REG_WRITE(pipeconf_reg, pipeconf);
+	REG_READ(pipeconf_reg);
+
+	REG_WRITE(PCH_PIPEBCONF, pipeconf);
+	REG_READ(PCH_PIPEBCONF);
+	wait_for_vblank(dev);
+
+	REG_WRITE(dspcntr_reg, dspcntr);
+	wait_for_vblank(dev);
+
+	return 0;
+}
+
+static int oaktrail_hdmi_mode_valid(struct drm_connector *connector,
+				struct drm_display_mode *mode)
+{
+	if (mode->clock > 165000)
+		return MODE_CLOCK_HIGH;
+	if (mode->clock < 20000)
+		return MODE_CLOCK_LOW;
+
+	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+		return MODE_NO_DBLESCAN;
+
+	return MODE_OK;
+}
+
+static bool oaktrail_hdmi_mode_fixup(struct drm_encoder *encoder,
+				 struct drm_display_mode *mode,
+				 struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static enum drm_connector_status
+oaktrail_hdmi_detect(struct drm_connector *connector, bool force)
+{
+	enum drm_connector_status status;
+	struct drm_device *dev = connector->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+	u32 temp;
+
+	temp = HDMI_READ(HDMI_HSR);
+	DRM_DEBUG_KMS("HDMI_HSR %x\n", temp);
+
+	if ((temp & HDMI_DETECT_HDP) != 0)
+		status = connector_status_connected;
+	else
+		status = connector_status_disconnected;
+
+	return status;
+}
+
+static const unsigned char raw_edid[] = {
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x10, 0xac, 0x2f, 0xa0,
+	0x53, 0x55, 0x33, 0x30, 0x16, 0x13, 0x01, 0x03, 0x0e, 0x3a, 0x24, 0x78,
+	0xea, 0xe9, 0xf5, 0xac, 0x51, 0x30, 0xb4, 0x25, 0x11, 0x50, 0x54, 0xa5,
+	0x4b, 0x00, 0x81, 0x80, 0xa9, 0x40, 0x71, 0x4f, 0xb3, 0x00, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x28, 0x3c, 0x80, 0xa0, 0x70, 0xb0,
+	0x23, 0x40, 0x30, 0x20, 0x36, 0x00, 0x46, 0x6c, 0x21, 0x00, 0x00, 0x1a,
+	0x00, 0x00, 0x00, 0xff, 0x00, 0x47, 0x4e, 0x37, 0x32, 0x31, 0x39, 0x35,
+	0x52, 0x30, 0x33, 0x55, 0x53, 0x0a, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x44,
+	0x45, 0x4c, 0x4c, 0x20, 0x32, 0x37, 0x30, 0x39, 0x57, 0x0a, 0x20, 0x20,
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x38, 0x4c, 0x1e, 0x53, 0x11, 0x00, 0x0a,
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x00, 0x8d
+};
+
+static int oaktrail_hdmi_get_modes(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct i2c_adapter *i2c_adap;
+	struct edid *edid;
+	struct drm_display_mode *mode, *t;
+	int i = 0, ret = 0;
+
+	i2c_adap = i2c_get_adapter(3);
+	if (i2c_adap == NULL) {
+		DRM_ERROR("No ddc adapter available!\n");
+		edid = (struct edid *)raw_edid;
+	} else {
+		edid = (struct edid *)raw_edid;
+		/* FIXME ? edid = drm_get_edid(connector, i2c_adap); */
+	}
+
+	if (edid) {
+		drm_mode_connector_update_edid_property(connector, edid);
+		ret = drm_add_edid_modes(connector, edid);
+		connector->display_info.raw_edid = NULL;
+	}
+
+	/*
+	 * prune modes that require frame buffer bigger than stolen mem
+	 */
+	list_for_each_entry_safe(mode, t, &connector->probed_modes, head) {
+		if ((mode->hdisplay * mode->vdisplay * 4) >= dev_priv->vram_stolen_size) {
+			i++;
+			drm_mode_remove(connector, mode);
+		}
+	}
+	return ret - i;
+}
+
+static void oaktrail_hdmi_mode_set(struct drm_encoder *encoder,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+
+	oaktrail_hdmi_audio_enable(dev);
+	return;
+}
+
+static void oaktrail_hdmi_destroy(struct drm_connector *connector)
+{
+	return;
+}
+
+static const struct drm_encoder_helper_funcs oaktrail_hdmi_helper_funcs = {
+	.dpms = oaktrail_hdmi_dpms,
+	.mode_fixup = oaktrail_hdmi_mode_fixup,
+	.prepare = psb_intel_encoder_prepare,
+	.mode_set = oaktrail_hdmi_mode_set,
+	.commit = psb_intel_encoder_commit,
+};
+
+static const struct drm_connector_helper_funcs
+					oaktrail_hdmi_connector_helper_funcs = {
+	.get_modes = oaktrail_hdmi_get_modes,
+	.mode_valid = oaktrail_hdmi_mode_valid,
+	.best_encoder = psb_intel_best_encoder,
+};
+
+static const struct drm_connector_funcs oaktrail_hdmi_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = oaktrail_hdmi_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = oaktrail_hdmi_destroy,
+};
+
+static void oaktrail_hdmi_enc_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs oaktrail_hdmi_enc_funcs = {
+	.destroy = oaktrail_hdmi_enc_destroy,
+};
+
+void oaktrail_hdmi_init(struct drm_device *dev,
+					struct psb_intel_mode_device *mode_dev)
+{
+	struct psb_intel_encoder *psb_intel_encoder;
+	struct psb_intel_connector *psb_intel_connector;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+
+	psb_intel_encoder = kzalloc(sizeof(struct psb_intel_encoder), GFP_KERNEL);
+	if (!psb_intel_encoder)
+		return;
+
+	psb_intel_connector = kzalloc(sizeof(struct psb_intel_connector), GFP_KERNEL);
+	if (!psb_intel_connector)
+		goto failed_connector;
+
+	connector = &psb_intel_connector->base;
+	encoder = &psb_intel_encoder->base;
+	drm_connector_init(dev, connector,
+			   &oaktrail_hdmi_connector_funcs,
+			   DRM_MODE_CONNECTOR_DVID);
+
+	drm_encoder_init(dev, encoder,
+			 &oaktrail_hdmi_enc_funcs,
+			 DRM_MODE_ENCODER_TMDS);
+
+	psb_intel_connector_attach_encoder(psb_intel_connector,
+					   psb_intel_encoder);
+
+	psb_intel_encoder->type = INTEL_OUTPUT_HDMI;
+	drm_encoder_helper_add(encoder, &oaktrail_hdmi_helper_funcs);
+	drm_connector_helper_add(connector, &oaktrail_hdmi_connector_helper_funcs);
+
+	connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+	connector->interlace_allowed = false;
+	connector->doublescan_allowed = false;
+	drm_sysfs_connector_add(connector);
+
+	return;
+
+failed_connector:
+	kfree(psb_intel_encoder);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(hdmi_ids) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x080d) },
+	{}
+};
+
+void oaktrail_hdmi_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct pci_dev *pdev;
+	struct oaktrail_hdmi_dev *hdmi_dev;
+	int ret;
+
+	pdev = pci_get_device(PCI_VENDOR_ID_INTEL, 0x080d, NULL);
+	if (!pdev)
+		return;
+
+	hdmi_dev = kzalloc(sizeof(struct oaktrail_hdmi_dev), GFP_KERNEL);
+	if (!hdmi_dev) {
+		dev_err(dev->dev, "failed to allocate memory\n");
+		goto out;
+	}
+
+
+	ret = pci_enable_device(pdev);
+	if (ret) {
+		dev_err(dev->dev, "failed to enable hdmi controller\n");
+		goto free;
+	}
+
+	hdmi_dev->mmio = pci_resource_start(pdev, 0);
+	hdmi_dev->mmio_len = pci_resource_len(pdev, 0);
+	hdmi_dev->regs = ioremap(hdmi_dev->mmio, hdmi_dev->mmio_len);
+	if (!hdmi_dev->regs) {
+		dev_err(dev->dev, "failed to map hdmi mmio\n");
+		goto free;
+	}
+
+	hdmi_dev->dev = pdev;
+	pci_set_drvdata(pdev, hdmi_dev);
+
+	/* Initialize i2c controller */
+	ret = oaktrail_hdmi_i2c_init(hdmi_dev->dev);
+	if (ret)
+		dev_err(dev->dev, "HDMI I2C initialization failed\n");
+
+	dev_priv->hdmi_priv = hdmi_dev;
+	oaktrail_hdmi_audio_disable(dev);
+	return;
+
+free:
+	kfree(hdmi_dev);
+out:
+	return;
+}
+
+void oaktrail_hdmi_teardown(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+	struct pci_dev *pdev;
+
+	if (hdmi_dev) {
+		pdev = hdmi_dev->dev;
+		pci_set_drvdata(pdev, NULL);
+		oaktrail_hdmi_i2c_exit(pdev);
+		iounmap(hdmi_dev->regs);
+		kfree(hdmi_dev);
+		pci_dev_put(pdev);
+	}
+}
+
+/* save HDMI register state */
+void oaktrail_hdmi_save(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+	int i;
+
+	/* dpll */
+	hdmi_dev->saveDPLL_CTRL = PSB_RVDC32(DPLL_CTRL);
+	hdmi_dev->saveDPLL_DIV_CTRL = PSB_RVDC32(DPLL_DIV_CTRL);
+	hdmi_dev->saveDPLL_ADJUST = PSB_RVDC32(DPLL_ADJUST);
+	hdmi_dev->saveDPLL_UPDATE = PSB_RVDC32(DPLL_UPDATE);
+	hdmi_dev->saveDPLL_CLK_ENABLE = PSB_RVDC32(DPLL_CLK_ENABLE);
+
+	/* pipe B */
+	dev_priv->savePIPEBCONF = PSB_RVDC32(PIPEBCONF);
+	dev_priv->savePIPEBSRC  = PSB_RVDC32(PIPEBSRC);
+	dev_priv->saveHTOTAL_B  = PSB_RVDC32(HTOTAL_B);
+	dev_priv->saveHBLANK_B  = PSB_RVDC32(HBLANK_B);
+	dev_priv->saveHSYNC_B   = PSB_RVDC32(HSYNC_B);
+	dev_priv->saveVTOTAL_B  = PSB_RVDC32(VTOTAL_B);
+	dev_priv->saveVBLANK_B  = PSB_RVDC32(VBLANK_B);
+	dev_priv->saveVSYNC_B   = PSB_RVDC32(VSYNC_B);
+
+	hdmi_dev->savePCH_PIPEBCONF = PSB_RVDC32(PCH_PIPEBCONF);
+	hdmi_dev->savePCH_PIPEBSRC = PSB_RVDC32(PCH_PIPEBSRC);
+	hdmi_dev->savePCH_HTOTAL_B = PSB_RVDC32(PCH_HTOTAL_B);
+	hdmi_dev->savePCH_HBLANK_B = PSB_RVDC32(PCH_HBLANK_B);
+	hdmi_dev->savePCH_HSYNC_B  = PSB_RVDC32(PCH_HSYNC_B);
+	hdmi_dev->savePCH_VTOTAL_B = PSB_RVDC32(PCH_VTOTAL_B);
+	hdmi_dev->savePCH_VBLANK_B = PSB_RVDC32(PCH_VBLANK_B);
+	hdmi_dev->savePCH_VSYNC_B  = PSB_RVDC32(PCH_VSYNC_B);
+
+	/* plane */
+	dev_priv->saveDSPBCNTR = PSB_RVDC32(DSPBCNTR);
+	dev_priv->saveDSPBSTRIDE = PSB_RVDC32(DSPBSTRIDE);
+	dev_priv->saveDSPBADDR = PSB_RVDC32(DSPBBASE);
+	dev_priv->saveDSPBSURF = PSB_RVDC32(DSPBSURF);
+	dev_priv->saveDSPBLINOFF = PSB_RVDC32(DSPBLINOFF);
+	dev_priv->saveDSPBTILEOFF = PSB_RVDC32(DSPBTILEOFF);
+
+	/* cursor B */
+	dev_priv->saveDSPBCURSOR_CTRL = PSB_RVDC32(CURBCNTR);
+	dev_priv->saveDSPBCURSOR_BASE = PSB_RVDC32(CURBBASE);
+	dev_priv->saveDSPBCURSOR_POS = PSB_RVDC32(CURBPOS);
+
+	/* save palette */
+	for (i = 0; i < 256; i++)
+		dev_priv->save_palette_b[i] = PSB_RVDC32(PALETTE_B + (i << 2));
+}
+
+/* restore HDMI register state */
+void oaktrail_hdmi_restore(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+	int i;
+
+	/* dpll */
+	PSB_WVDC32(hdmi_dev->saveDPLL_CTRL, DPLL_CTRL);
+	PSB_WVDC32(hdmi_dev->saveDPLL_DIV_CTRL, DPLL_DIV_CTRL);
+	PSB_WVDC32(hdmi_dev->saveDPLL_ADJUST, DPLL_ADJUST);
+	PSB_WVDC32(hdmi_dev->saveDPLL_UPDATE, DPLL_UPDATE);
+	PSB_WVDC32(hdmi_dev->saveDPLL_CLK_ENABLE, DPLL_CLK_ENABLE);
+	DRM_UDELAY(150);
+
+	/* pipe */
+	PSB_WVDC32(dev_priv->savePIPEBSRC, PIPEBSRC);
+	PSB_WVDC32(dev_priv->saveHTOTAL_B, HTOTAL_B);
+	PSB_WVDC32(dev_priv->saveHBLANK_B, HBLANK_B);
+	PSB_WVDC32(dev_priv->saveHSYNC_B,  HSYNC_B);
+	PSB_WVDC32(dev_priv->saveVTOTAL_B, VTOTAL_B);
+	PSB_WVDC32(dev_priv->saveVBLANK_B, VBLANK_B);
+	PSB_WVDC32(dev_priv->saveVSYNC_B,  VSYNC_B);
+
+	PSB_WVDC32(hdmi_dev->savePCH_PIPEBSRC, PCH_PIPEBSRC);
+	PSB_WVDC32(hdmi_dev->savePCH_HTOTAL_B, PCH_HTOTAL_B);
+	PSB_WVDC32(hdmi_dev->savePCH_HBLANK_B, PCH_HBLANK_B);
+	PSB_WVDC32(hdmi_dev->savePCH_HSYNC_B,  PCH_HSYNC_B);
+	PSB_WVDC32(hdmi_dev->savePCH_VTOTAL_B, PCH_VTOTAL_B);
+	PSB_WVDC32(hdmi_dev->savePCH_VBLANK_B, PCH_VBLANK_B);
+	PSB_WVDC32(hdmi_dev->savePCH_VSYNC_B,  PCH_VSYNC_B);
+
+	PSB_WVDC32(dev_priv->savePIPEBCONF, PIPEBCONF);
+	PSB_WVDC32(hdmi_dev->savePCH_PIPEBCONF, PCH_PIPEBCONF);
+
+	/* plane */
+	PSB_WVDC32(dev_priv->saveDSPBLINOFF, DSPBLINOFF);
+	PSB_WVDC32(dev_priv->saveDSPBSTRIDE, DSPBSTRIDE);
+	PSB_WVDC32(dev_priv->saveDSPBTILEOFF, DSPBTILEOFF);
+	PSB_WVDC32(dev_priv->saveDSPBCNTR, DSPBCNTR);
+	PSB_WVDC32(dev_priv->saveDSPBSURF, DSPBSURF);
+
+	/* cursor B */
+	PSB_WVDC32(dev_priv->saveDSPBCURSOR_CTRL, CURBCNTR);
+	PSB_WVDC32(dev_priv->saveDSPBCURSOR_POS, CURBPOS);
+	PSB_WVDC32(dev_priv->saveDSPBCURSOR_BASE, CURBBASE);
+
+	/* restore palette */
+	for (i = 0; i < 256; i++)
+		PSB_WVDC32(dev_priv->save_palette_b[i], PALETTE_B + (i << 2));
+}
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c b/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
new file mode 100644
index 000000000000..705440874ac0
--- /dev/null
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi_i2c.c
@@ -0,0 +1,328 @@
+/*
+ * Copyright © 2010 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Li Peng <peng.li@intel.com>
+ */
+
+#include <linux/export.h>
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include "psb_drv.h"
+
+#define HDMI_READ(reg)		readl(hdmi_dev->regs + (reg))
+#define HDMI_WRITE(reg, val)	writel(val, hdmi_dev->regs + (reg))
+
+#define HDMI_HCR	0x1000
+#define HCR_DETECT_HDP		(1 << 6)
+#define HCR_ENABLE_HDCP		(1 << 5)
+#define HCR_ENABLE_AUDIO	(1 << 2)
+#define HCR_ENABLE_PIXEL	(1 << 1)
+#define HCR_ENABLE_TMDS		(1 << 0)
+#define HDMI_HICR	0x1004
+#define HDMI_INTR_I2C_ERROR	(1 << 4)
+#define HDMI_INTR_I2C_FULL	(1 << 3)
+#define HDMI_INTR_I2C_DONE	(1 << 2)
+#define HDMI_INTR_HPD		(1 << 0)
+#define HDMI_HSR	0x1008
+#define HDMI_HISR	0x100C
+#define HDMI_HI2CRDB0	0x1200
+#define HDMI_HI2CHCR	0x1240
+#define HI2C_HDCP_WRITE		(0 << 2)
+#define HI2C_HDCP_RI_READ	(1 << 2)
+#define HI2C_HDCP_READ		(2 << 2)
+#define HI2C_EDID_READ		(3 << 2)
+#define HI2C_READ_CONTINUE	(1 << 1)
+#define HI2C_ENABLE_TRANSACTION	(1 << 0)
+
+#define HDMI_ICRH	0x1100
+#define HDMI_HI2CTDR0	0x1244
+#define HDMI_HI2CTDR1	0x1248
+
+#define I2C_STAT_INIT		0
+#define I2C_READ_DONE		1
+#define I2C_TRANSACTION_DONE	2
+
+struct hdmi_i2c_dev {
+	struct i2c_adapter *adap;
+	struct mutex i2c_lock;
+	struct completion complete;
+	int status;
+	struct i2c_msg *msg;
+	int buf_offset;
+};
+
+static void hdmi_i2c_irq_enable(struct oaktrail_hdmi_dev *hdmi_dev)
+{
+	u32 temp;
+
+	temp = HDMI_READ(HDMI_HICR);
+	temp |= (HDMI_INTR_I2C_ERROR | HDMI_INTR_I2C_FULL | HDMI_INTR_I2C_DONE);
+	HDMI_WRITE(HDMI_HICR, temp);
+	HDMI_READ(HDMI_HICR);
+}
+
+static void hdmi_i2c_irq_disable(struct oaktrail_hdmi_dev *hdmi_dev)
+{
+	HDMI_WRITE(HDMI_HICR, 0x0);
+	HDMI_READ(HDMI_HICR);
+}
+
+static int xfer_read(struct i2c_adapter *adap, struct i2c_msg *pmsg)
+{
+	struct oaktrail_hdmi_dev *hdmi_dev = i2c_get_adapdata(adap);
+	struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+	u32 temp;
+
+	i2c_dev->status = I2C_STAT_INIT;
+	i2c_dev->msg = pmsg;
+	i2c_dev->buf_offset = 0;
+	INIT_COMPLETION(i2c_dev->complete);
+
+	/* Enable I2C transaction */
+	temp = ((pmsg->len) << 20) | HI2C_EDID_READ | HI2C_ENABLE_TRANSACTION;
+	HDMI_WRITE(HDMI_HI2CHCR, temp);
+	HDMI_READ(HDMI_HI2CHCR);
+
+	while (i2c_dev->status != I2C_TRANSACTION_DONE)
+		wait_for_completion_interruptible_timeout(&i2c_dev->complete,
+								10 * HZ);
+
+	return 0;
+}
+
+static int xfer_write(struct i2c_adapter *adap, struct i2c_msg *pmsg)
+{
+	/*
+	 * XXX: i2c write seems isn't useful for EDID probe, don't do anything
+	 */
+	return 0;
+}
+
+static int oaktrail_hdmi_i2c_access(struct i2c_adapter *adap,
+				struct i2c_msg *pmsg,
+				int num)
+{
+	struct oaktrail_hdmi_dev *hdmi_dev = i2c_get_adapdata(adap);
+	struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+	int i, err = 0;
+
+	mutex_lock(&i2c_dev->i2c_lock);
+
+	/* Enable i2c unit */
+	HDMI_WRITE(HDMI_ICRH, 0x00008760);
+
+	/* Enable irq */
+	hdmi_i2c_irq_enable(hdmi_dev);
+	for (i = 0; i < num; i++) {
+		if (pmsg->len && pmsg->buf) {
+			if (pmsg->flags & I2C_M_RD)
+				err = xfer_read(adap, pmsg);
+			else
+				err = xfer_write(adap, pmsg);
+		}
+		pmsg++;         /* next message */
+	}
+
+	/* Disable irq */
+	hdmi_i2c_irq_disable(hdmi_dev);
+
+	mutex_unlock(&i2c_dev->i2c_lock);
+
+	return i;
+}
+
+static u32 oaktrail_hdmi_i2c_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_10BIT_ADDR;
+}
+
+static const struct i2c_algorithm oaktrail_hdmi_i2c_algorithm = {
+	.master_xfer	= oaktrail_hdmi_i2c_access,
+	.functionality  = oaktrail_hdmi_i2c_func,
+};
+
+static struct i2c_adapter oaktrail_hdmi_i2c_adapter = {
+	.name		= "oaktrail_hdmi_i2c",
+	.nr		= 3,
+	.owner		= THIS_MODULE,
+	.class		= I2C_CLASS_DDC,
+	.algo		= &oaktrail_hdmi_i2c_algorithm,
+};
+
+static void hdmi_i2c_read(struct oaktrail_hdmi_dev *hdmi_dev)
+{
+	struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+	struct i2c_msg *msg = i2c_dev->msg;
+	u8 *buf = msg->buf;
+	u32 temp;
+	int i, offset;
+
+	offset = i2c_dev->buf_offset;
+	for (i = 0; i < 0x10; i++) {
+		temp = HDMI_READ(HDMI_HI2CRDB0 + (i * 4));
+		memcpy(buf + (offset + i * 4), &temp, 4);
+	}
+	i2c_dev->buf_offset += (0x10 * 4);
+
+	/* clearing read buffer full intr */
+	temp = HDMI_READ(HDMI_HISR);
+	HDMI_WRITE(HDMI_HISR, temp | HDMI_INTR_I2C_FULL);
+	HDMI_READ(HDMI_HISR);
+
+	/* continue read transaction */
+	temp = HDMI_READ(HDMI_HI2CHCR);
+	HDMI_WRITE(HDMI_HI2CHCR, temp | HI2C_READ_CONTINUE);
+	HDMI_READ(HDMI_HI2CHCR);
+
+	i2c_dev->status = I2C_READ_DONE;
+	return;
+}
+
+static void hdmi_i2c_transaction_done(struct oaktrail_hdmi_dev *hdmi_dev)
+{
+	struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+	u32 temp;
+
+	/* clear transaction done intr */
+	temp = HDMI_READ(HDMI_HISR);
+	HDMI_WRITE(HDMI_HISR, temp | HDMI_INTR_I2C_DONE);
+	HDMI_READ(HDMI_HISR);
+
+
+	temp = HDMI_READ(HDMI_HI2CHCR);
+	HDMI_WRITE(HDMI_HI2CHCR, temp & ~HI2C_ENABLE_TRANSACTION);
+	HDMI_READ(HDMI_HI2CHCR);
+
+	i2c_dev->status = I2C_TRANSACTION_DONE;
+	return;
+}
+
+static irqreturn_t oaktrail_hdmi_i2c_handler(int this_irq, void *dev)
+{
+	struct oaktrail_hdmi_dev *hdmi_dev = dev;
+	struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+	u32 stat;
+
+	stat = HDMI_READ(HDMI_HISR);
+
+	if (stat & HDMI_INTR_HPD) {
+		HDMI_WRITE(HDMI_HISR, stat | HDMI_INTR_HPD);
+		HDMI_READ(HDMI_HISR);
+	}
+
+	if (stat & HDMI_INTR_I2C_FULL)
+		hdmi_i2c_read(hdmi_dev);
+
+	if (stat & HDMI_INTR_I2C_DONE)
+		hdmi_i2c_transaction_done(hdmi_dev);
+
+	complete(&i2c_dev->complete);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * choose alternate function 2 of GPIO pin 52, 53,
+ * which is used by HDMI I2C logic
+ */
+static void oaktrail_hdmi_i2c_gpio_fix(void)
+{
+	void *base;
+	unsigned int gpio_base = 0xff12c000;
+	int gpio_len = 0x1000;
+	u32 temp;
+
+	base = ioremap((resource_size_t)gpio_base, gpio_len);
+	if (base == NULL) {
+		DRM_ERROR("gpio ioremap fail\n");
+		return;
+	}
+
+	temp = readl(base + 0x44);
+	DRM_DEBUG_DRIVER("old gpio val %x\n", temp);
+	writel((temp | 0x00000a00), (base +  0x44));
+	temp = readl(base + 0x44);
+	DRM_DEBUG_DRIVER("new gpio val %x\n", temp);
+
+	iounmap(base);
+}
+
+int oaktrail_hdmi_i2c_init(struct pci_dev *dev)
+{
+	struct oaktrail_hdmi_dev *hdmi_dev;
+	struct hdmi_i2c_dev *i2c_dev;
+	int ret;
+
+	hdmi_dev = pci_get_drvdata(dev);
+
+	i2c_dev = kzalloc(sizeof(struct hdmi_i2c_dev), GFP_KERNEL);
+	if (i2c_dev == NULL) {
+		DRM_ERROR("Can't allocate interface\n");
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	i2c_dev->adap = &oaktrail_hdmi_i2c_adapter;
+	i2c_dev->status = I2C_STAT_INIT;
+	init_completion(&i2c_dev->complete);
+	mutex_init(&i2c_dev->i2c_lock);
+	i2c_set_adapdata(&oaktrail_hdmi_i2c_adapter, hdmi_dev);
+	hdmi_dev->i2c_dev = i2c_dev;
+
+	/* Enable HDMI I2C function on gpio */
+	oaktrail_hdmi_i2c_gpio_fix();
+
+	/* request irq */
+	ret = request_irq(dev->irq, oaktrail_hdmi_i2c_handler, IRQF_SHARED,
+			  oaktrail_hdmi_i2c_adapter.name, hdmi_dev);
+	if (ret) {
+		DRM_ERROR("Failed to request IRQ for I2C controller\n");
+		goto err;
+	}
+
+	/* Adapter registration */
+	ret = i2c_add_numbered_adapter(&oaktrail_hdmi_i2c_adapter);
+	return ret;
+
+err:
+	kfree(i2c_dev);
+exit:
+	return ret;
+}
+
+void oaktrail_hdmi_i2c_exit(struct pci_dev *dev)
+{
+	struct oaktrail_hdmi_dev *hdmi_dev;
+	struct hdmi_i2c_dev *i2c_dev;
+
+	hdmi_dev = pci_get_drvdata(dev);
+	if (i2c_del_adapter(&oaktrail_hdmi_i2c_adapter))
+		DRM_DEBUG_DRIVER("Failed to delete hdmi-i2c adapter\n");
+
+	i2c_dev = hdmi_dev->i2c_dev;
+	kfree(i2c_dev);
+	free_irq(dev->irq, hdmi_dev);
+}
diff --git a/drivers/gpu/drm/gma500/oaktrail_lvds.c b/drivers/gpu/drm/gma500/oaktrail_lvds.c
new file mode 100644
index 000000000000..238bbe105304
--- /dev/null
+++ b/drivers/gpu/drm/gma500/oaktrail_lvds.c
@@ -0,0 +1,449 @@
+/*
+ * Copyright © 2006-2009 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ *	Dave Airlie <airlied@linux.ie>
+ *	Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <drm/drmP.h>
+#include <asm/mrst.h>
+
+#include "intel_bios.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include <linux/pm_runtime.h>
+
+/* The max/min PWM frequency in BPCR[31:17] - */
+/* The smallest number is 1 (not 0) that can fit in the
+ * 15-bit field of the and then*/
+/* shifts to the left by one bit to get the actual 16-bit
+ * value that the 15-bits correspond to.*/
+#define MRST_BLC_MAX_PWM_REG_FREQ	    0xFFFF
+#define BRIGHTNESS_MAX_LEVEL 100
+
+/**
+ * Sets the power state for the panel.
+ */
+static void oaktrail_lvds_set_power(struct drm_device *dev,
+				struct psb_intel_encoder *psb_intel_encoder,
+				bool on)
+{
+	u32 pp_status;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (!gma_power_begin(dev, true))
+		return;
+
+	if (on) {
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) |
+			  POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while ((pp_status & (PP_ON | PP_READY)) == PP_READY);
+		dev_priv->is_lvds_on = true;
+		if (dev_priv->ops->lvds_bl_power)
+			dev_priv->ops->lvds_bl_power(dev, true);
+	} else {
+		if (dev_priv->ops->lvds_bl_power)
+			dev_priv->ops->lvds_bl_power(dev, false);
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) &
+			  ~POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while (pp_status & PP_ON);
+		dev_priv->is_lvds_on = false;
+		pm_request_idle(&dev->pdev->dev);
+	}
+	gma_power_end(dev);
+}
+
+static void oaktrail_lvds_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct psb_intel_encoder *psb_intel_encoder =
+						to_psb_intel_encoder(encoder);
+
+	if (mode == DRM_MODE_DPMS_ON)
+		oaktrail_lvds_set_power(dev, psb_intel_encoder, true);
+	else
+		oaktrail_lvds_set_power(dev, psb_intel_encoder, false);
+
+	/* XXX: We never power down the LVDS pairs. */
+}
+
+static void oaktrail_lvds_mode_set(struct drm_encoder *encoder,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct drm_connector *connector = NULL;
+	struct drm_crtc *crtc = encoder->crtc;
+	u32 lvds_port;
+	uint64_t v = DRM_MODE_SCALE_FULLSCREEN;
+
+	if (!gma_power_begin(dev, true))
+		return;
+
+	/*
+	 * The LVDS pin pair will already have been turned on in the
+	 * psb_intel_crtc_mode_set since it has a large impact on the DPLL
+	 * settings.
+	 */
+	lvds_port = (REG_READ(LVDS) &
+		    (~LVDS_PIPEB_SELECT)) |
+		    LVDS_PORT_EN |
+		    LVDS_BORDER_EN;
+
+	/* If the firmware says dither on Moorestown, or the BIOS does
+	   on Oaktrail then enable dithering */
+	if (mode_dev->panel_wants_dither || dev_priv->lvds_dither)
+		lvds_port |= MRST_PANEL_8TO6_DITHER_ENABLE;
+
+	REG_WRITE(LVDS, lvds_port);
+
+	/* Find the connector we're trying to set up */
+	list_for_each_entry(connector, &mode_config->connector_list, head) {
+		if (!connector->encoder || connector->encoder->crtc != crtc)
+			continue;
+	}
+
+	if (!connector) {
+		DRM_ERROR("Couldn't find connector when setting mode");
+		return;
+	}
+
+	drm_connector_property_get_value(
+		connector,
+		dev->mode_config.scaling_mode_property,
+		&v);
+
+	if (v == DRM_MODE_SCALE_NO_SCALE)
+		REG_WRITE(PFIT_CONTROL, 0);
+	else if (v == DRM_MODE_SCALE_ASPECT) {
+		if ((mode->vdisplay != adjusted_mode->crtc_vdisplay) ||
+		    (mode->hdisplay != adjusted_mode->crtc_hdisplay)) {
+			if ((adjusted_mode->crtc_hdisplay * mode->vdisplay) ==
+			    (mode->hdisplay * adjusted_mode->crtc_vdisplay))
+				REG_WRITE(PFIT_CONTROL, PFIT_ENABLE);
+			else if ((adjusted_mode->crtc_hdisplay *
+				mode->vdisplay) > (mode->hdisplay *
+				adjusted_mode->crtc_vdisplay))
+				REG_WRITE(PFIT_CONTROL, PFIT_ENABLE |
+					  PFIT_SCALING_MODE_PILLARBOX);
+			else
+				REG_WRITE(PFIT_CONTROL, PFIT_ENABLE |
+					  PFIT_SCALING_MODE_LETTERBOX);
+		} else
+			REG_WRITE(PFIT_CONTROL, PFIT_ENABLE);
+	} else /*(v == DRM_MODE_SCALE_FULLSCREEN)*/
+		REG_WRITE(PFIT_CONTROL, PFIT_ENABLE);
+
+	gma_power_end(dev);
+}
+
+static void oaktrail_lvds_prepare(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_encoder *psb_intel_encoder =
+						to_psb_intel_encoder(encoder);
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+
+	if (!gma_power_begin(dev, true))
+		return;
+
+	mode_dev->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+	mode_dev->backlight_duty_cycle = (mode_dev->saveBLC_PWM_CTL &
+					  BACKLIGHT_DUTY_CYCLE_MASK);
+	oaktrail_lvds_set_power(dev, psb_intel_encoder, false);
+	gma_power_end(dev);
+}
+
+static u32 oaktrail_lvds_get_max_backlight(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 ret;
+
+	if (gma_power_begin(dev, false)) {
+		ret = ((REG_READ(BLC_PWM_CTL) &
+			  BACKLIGHT_MODULATION_FREQ_MASK) >>
+			  BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+		gma_power_end(dev);
+	} else
+		ret = ((dev_priv->saveBLC_PWM_CTL &
+			  BACKLIGHT_MODULATION_FREQ_MASK) >>
+			  BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+	return ret;
+}
+
+static void oaktrail_lvds_commit(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_encoder *psb_intel_encoder =
+						to_psb_intel_encoder(encoder);
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+
+	if (mode_dev->backlight_duty_cycle == 0)
+		mode_dev->backlight_duty_cycle =
+					oaktrail_lvds_get_max_backlight(dev);
+	oaktrail_lvds_set_power(dev, psb_intel_encoder, true);
+}
+
+static const struct drm_encoder_helper_funcs oaktrail_lvds_helper_funcs = {
+	.dpms = oaktrail_lvds_dpms,
+	.mode_fixup = psb_intel_lvds_mode_fixup,
+	.prepare = oaktrail_lvds_prepare,
+	.mode_set = oaktrail_lvds_mode_set,
+	.commit = oaktrail_lvds_commit,
+};
+
+static struct drm_display_mode lvds_configuration_modes[] = {
+	/* hard coded fixed mode for TPO LTPS LPJ040K001A */
+	{ DRM_MODE("800x480",  DRM_MODE_TYPE_DRIVER, 33264, 800, 836,
+		   846, 1056, 0, 480, 489, 491, 525, 0, 0) },
+	/* hard coded fixed mode for LVDS 800x480 */
+	{ DRM_MODE("800x480",  DRM_MODE_TYPE_DRIVER, 30994, 800, 801,
+		   802, 1024, 0, 480, 481, 482, 525, 0, 0) },
+	/* hard coded fixed mode for Samsung 480wsvga LVDS 1024x600@75 */
+	{ DRM_MODE("1024x600", DRM_MODE_TYPE_DRIVER, 53990, 1024, 1072,
+		   1104, 1184, 0, 600, 603, 604, 608, 0, 0) },
+	/* hard coded fixed mode for Samsung 480wsvga LVDS 1024x600@75 */
+	{ DRM_MODE("1024x600", DRM_MODE_TYPE_DRIVER, 53990, 1024, 1104,
+		   1136, 1184, 0, 600, 603, 604, 608, 0, 0) },
+	/* hard coded fixed mode for Sharp wsvga LVDS 1024x600 */
+	{ DRM_MODE("1024x600", DRM_MODE_TYPE_DRIVER, 48885, 1024, 1124,
+		   1204, 1312, 0, 600, 607, 610, 621, 0, 0) },
+	/* hard coded fixed mode for LVDS 1024x768 */
+	{ DRM_MODE("1024x768", DRM_MODE_TYPE_DRIVER, 65000, 1024, 1048,
+		   1184, 1344, 0, 768, 771, 777, 806, 0, 0) },
+	/* hard coded fixed mode for LVDS 1366x768 */
+	{ DRM_MODE("1366x768", DRM_MODE_TYPE_DRIVER, 77500, 1366, 1430,
+		   1558, 1664, 0, 768, 769, 770, 776, 0, 0) },
+};
+
+/* Returns the panel fixed mode from configuration. */
+
+static void oaktrail_lvds_get_configuration_mode(struct drm_device *dev,
+					struct psb_intel_mode_device *mode_dev)
+{
+	struct drm_display_mode *mode = NULL;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct oaktrail_timing_info *ti = &dev_priv->gct_data.DTD;
+
+	mode_dev->panel_fixed_mode = NULL;
+
+	/* Use the firmware provided data on Moorestown */
+	if (dev_priv->vbt_data.size != 0x00) { /*if non-zero, then use vbt*/
+		mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+		if (!mode)
+			return;
+
+		mode->hdisplay = (ti->hactive_hi << 8) | ti->hactive_lo;
+		mode->vdisplay = (ti->vactive_hi << 8) | ti->vactive_lo;
+		mode->hsync_start = mode->hdisplay + \
+				((ti->hsync_offset_hi << 8) | \
+				ti->hsync_offset_lo);
+		mode->hsync_end = mode->hsync_start + \
+				((ti->hsync_pulse_width_hi << 8) | \
+				ti->hsync_pulse_width_lo);
+		mode->htotal = mode->hdisplay + ((ti->hblank_hi << 8) | \
+							ti->hblank_lo);
+		mode->vsync_start = \
+			mode->vdisplay + ((ti->vsync_offset_hi << 4) | \
+						ti->vsync_offset_lo);
+		mode->vsync_end = \
+			mode->vsync_start + ((ti->vsync_pulse_width_hi << 4) | \
+						ti->vsync_pulse_width_lo);
+		mode->vtotal = mode->vdisplay + \
+				((ti->vblank_hi << 8) | ti->vblank_lo);
+		mode->clock = ti->pixel_clock * 10;
+#if 0
+		printk(KERN_INFO "hdisplay is %d\n", mode->hdisplay);
+		printk(KERN_INFO "vdisplay is %d\n", mode->vdisplay);
+		printk(KERN_INFO "HSS is %d\n", mode->hsync_start);
+		printk(KERN_INFO "HSE is %d\n", mode->hsync_end);
+		printk(KERN_INFO "htotal is %d\n", mode->htotal);
+		printk(KERN_INFO "VSS is %d\n", mode->vsync_start);
+		printk(KERN_INFO "VSE is %d\n", mode->vsync_end);
+		printk(KERN_INFO "vtotal is %d\n", mode->vtotal);
+		printk(KERN_INFO "clock is %d\n", mode->clock);
+#endif
+		mode_dev->panel_fixed_mode = mode;
+	}
+
+	/* Use the BIOS VBT mode if available */
+	if (mode_dev->panel_fixed_mode == NULL && mode_dev->vbt_mode)
+		mode_dev->panel_fixed_mode = drm_mode_duplicate(dev,
+						mode_dev->vbt_mode);
+
+	/* Then try the LVDS VBT mode */
+	if (mode_dev->panel_fixed_mode == NULL)
+		if (dev_priv->lfp_lvds_vbt_mode)
+			mode_dev->panel_fixed_mode =
+				drm_mode_duplicate(dev,
+					dev_priv->lfp_lvds_vbt_mode);
+	/* Then guess */
+	if (mode_dev->panel_fixed_mode == NULL)
+		mode_dev->panel_fixed_mode
+			= drm_mode_duplicate(dev, &lvds_configuration_modes[2]);
+
+	drm_mode_set_name(mode_dev->panel_fixed_mode);
+	drm_mode_set_crtcinfo(mode_dev->panel_fixed_mode, 0);
+}
+
+/**
+ * oaktrail_lvds_init - setup LVDS connectors on this device
+ * @dev: drm device
+ *
+ * Create the connector, register the LVDS DDC bus, and try to figure out what
+ * modes we can display on the LVDS panel (if present).
+ */
+void oaktrail_lvds_init(struct drm_device *dev,
+		    struct psb_intel_mode_device *mode_dev)
+{
+	struct psb_intel_encoder *psb_intel_encoder;
+	struct psb_intel_connector *psb_intel_connector;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct edid *edid;
+	int ret = 0;
+	struct i2c_adapter *i2c_adap;
+	struct drm_display_mode *scan;	/* *modes, *bios_mode; */
+
+	psb_intel_encoder = kzalloc(sizeof(struct psb_intel_encoder), GFP_KERNEL);
+	if (!psb_intel_encoder)
+		return;
+
+	psb_intel_connector = kzalloc(sizeof(struct psb_intel_connector), GFP_KERNEL);
+	if (!psb_intel_connector)
+		goto failed_connector;
+
+	connector = &psb_intel_connector->base;
+	encoder = &psb_intel_encoder->base;
+	dev_priv->is_lvds_on = true;
+	drm_connector_init(dev, connector,
+			   &psb_intel_lvds_connector_funcs,
+			   DRM_MODE_CONNECTOR_LVDS);
+
+	drm_encoder_init(dev, encoder, &psb_intel_lvds_enc_funcs,
+			 DRM_MODE_ENCODER_LVDS);
+
+	psb_intel_connector_attach_encoder(psb_intel_connector,
+					   psb_intel_encoder);
+	psb_intel_encoder->type = INTEL_OUTPUT_LVDS;
+
+	drm_encoder_helper_add(encoder, &oaktrail_lvds_helper_funcs);
+	drm_connector_helper_add(connector,
+				 &psb_intel_lvds_connector_helper_funcs);
+	connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+	connector->interlace_allowed = false;
+	connector->doublescan_allowed = false;
+
+	drm_connector_attach_property(connector,
+					dev->mode_config.scaling_mode_property,
+					DRM_MODE_SCALE_FULLSCREEN);
+	drm_connector_attach_property(connector,
+					dev_priv->backlight_property,
+					BRIGHTNESS_MAX_LEVEL);
+
+	mode_dev->panel_wants_dither = false;
+	if (dev_priv->vbt_data.size != 0x00)
+		mode_dev->panel_wants_dither = (dev_priv->gct_data.
+			Panel_Port_Control & MRST_PANEL_8TO6_DITHER_ENABLE);
+        if (dev_priv->lvds_dither)
+                mode_dev->panel_wants_dither = 1;
+
+	/*
+	 * LVDS discovery:
+	 * 1) check for EDID on DDC
+	 * 2) check for VBT data
+	 * 3) check to see if LVDS is already on
+	 *    if none of the above, no panel
+	 * 4) make sure lid is open
+	 *    if closed, act like it's not there for now
+	 */
+
+	i2c_adap = i2c_get_adapter(dev_priv->ops->i2c_bus);
+	if (i2c_adap == NULL)
+		dev_err(dev->dev, "No ddc adapter available!\n");
+	/*
+	 * Attempt to get the fixed panel mode from DDC.  Assume that the
+	 * preferred mode is the right one.
+	 */
+	if (i2c_adap) {
+		edid = drm_get_edid(connector, i2c_adap);
+		if (edid) {
+			drm_mode_connector_update_edid_property(connector,
+									edid);
+			ret = drm_add_edid_modes(connector, edid);
+			kfree(edid);
+		}
+
+		list_for_each_entry(scan, &connector->probed_modes, head) {
+			if (scan->type & DRM_MODE_TYPE_PREFERRED) {
+				mode_dev->panel_fixed_mode =
+				    drm_mode_duplicate(dev, scan);
+				goto out;	/* FIXME: check for quirks */
+			}
+		}
+	}
+	/*
+	 * If we didn't get EDID, try geting panel timing
+	 * from configuration data
+	 */
+	oaktrail_lvds_get_configuration_mode(dev, mode_dev);
+
+	if (mode_dev->panel_fixed_mode) {
+		mode_dev->panel_fixed_mode->type |= DRM_MODE_TYPE_PREFERRED;
+		goto out;	/* FIXME: check for quirks */
+	}
+
+	/* If we still don't have a mode after all that, give up. */
+	if (!mode_dev->panel_fixed_mode) {
+		dev_err(dev->dev, "Found no modes on the lvds, ignoring the LVDS\n");
+		goto failed_find;
+	}
+
+out:
+	drm_sysfs_connector_add(connector);
+	return;
+
+failed_find:
+	dev_dbg(dev->dev, "No LVDS modes found, disabling.\n");
+	if (psb_intel_encoder->ddc_bus)
+		psb_intel_i2c_destroy(psb_intel_encoder->ddc_bus);
+
+/* failed_ddc: */
+
+	drm_encoder_cleanup(encoder);
+	drm_connector_cleanup(connector);
+	kfree(psb_intel_connector);
+failed_connector:
+	kfree(psb_intel_encoder);
+}
+
diff --git a/drivers/gpu/drm/gma500/power.c b/drivers/gpu/drm/gma500/power.c
new file mode 100644
index 000000000000..94025693bae1
--- /dev/null
+++ b/drivers/gpu/drm/gma500/power.c
@@ -0,0 +1,316 @@
+/**************************************************************************
+ * Copyright (c) 2009-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Benjamin Defnet <benjamin.r.defnet@intel.com>
+ *    Rajesh Poornachandran <rajesh.poornachandran@intel.com>
+ * Massively reworked
+ *    Alan Cox <alan@linux.intel.com>
+ */
+
+#include "power.h"
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+
+static struct mutex power_mutex;	/* Serialize power ops */
+static spinlock_t power_ctrl_lock;	/* Serialize power claim */
+
+/**
+ *	gma_power_init		-	initialise power manager
+ *	@dev: our device
+ *
+ *	Set up for power management tracking of our hardware.
+ */
+void gma_power_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	/* FIXME: Move APM/OSPM base into relevant device code */
+	dev_priv->apm_base = dev_priv->apm_reg & 0xffff;
+	dev_priv->ospm_base &= 0xffff;
+
+	dev_priv->display_power = true;	/* We start active */
+	dev_priv->display_count = 0;	/* Currently no users */
+	dev_priv->suspended = false;	/* And not suspended */
+	spin_lock_init(&power_ctrl_lock);
+	mutex_init(&power_mutex);
+
+	dev_priv->ops->init_pm(dev);
+}
+
+/**
+ *	gma_power_uninit	-	end power manager
+ *	@dev: device to end for
+ *
+ *	Undo the effects of gma_power_init
+ */
+void gma_power_uninit(struct drm_device *dev)
+{
+	pm_runtime_disable(&dev->pdev->dev);
+	pm_runtime_set_suspended(&dev->pdev->dev);
+}
+
+/**
+ *	gma_suspend_display	-	suspend the display logic
+ *	@dev: our DRM device
+ *
+ *	Suspend the display logic of the graphics interface
+ */
+static void gma_suspend_display(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->suspended)
+		return;
+	dev_priv->ops->save_regs(dev);
+	dev_priv->ops->power_down(dev);
+	dev_priv->display_power = false;
+}
+
+/**
+ *	gma_resume_display	-	resume display side logic
+ *
+ *	Resume the display hardware restoring state and enabling
+ *	as necessary.
+ */
+static void gma_resume_display(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->suspended == false)
+		return;
+
+	/* turn on the display power island */
+	dev_priv->ops->power_up(dev);
+	dev_priv->suspended = false;
+	dev_priv->display_power = true;
+
+	PSB_WVDC32(dev_priv->pge_ctl | _PSB_PGETBL_ENABLED, PSB_PGETBL_CTL);
+	pci_write_config_word(pdev, PSB_GMCH_CTRL,
+			dev_priv->gmch_ctrl | _PSB_GMCH_ENABLED);
+	dev_priv->ops->restore_regs(dev);
+}
+
+/**
+ *	gma_suspend_pci		-	suspend PCI side
+ *	@pdev: PCI device
+ *
+ *	Perform the suspend processing on our PCI device state
+ */
+static void gma_suspend_pci(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int bsm, vbt;
+
+	if (dev_priv->suspended)
+		return;
+
+	pci_save_state(pdev);
+	pci_read_config_dword(pdev, 0x5C, &bsm);
+	dev_priv->saveBSM = bsm;
+	pci_read_config_dword(pdev, 0xFC, &vbt);
+	dev_priv->saveVBT = vbt;
+	pci_read_config_dword(pdev, PSB_PCIx_MSI_ADDR_LOC, &dev_priv->msi_addr);
+	pci_read_config_dword(pdev, PSB_PCIx_MSI_DATA_LOC, &dev_priv->msi_data);
+
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, PCI_D3hot);
+
+	dev_priv->suspended = true;
+}
+
+/**
+ *	gma_resume_pci		-	resume helper
+ *	@dev: our PCI device
+ *
+ *	Perform the resume processing on our PCI device state - rewrite
+ *	register state and re-enable the PCI device
+ */
+static bool gma_resume_pci(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int ret;
+
+	if (!dev_priv->suspended)
+		return true;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+	pci_write_config_dword(pdev, 0x5c, dev_priv->saveBSM);
+	pci_write_config_dword(pdev, 0xFC, dev_priv->saveVBT);
+	/* restoring MSI address and data in PCIx space */
+	pci_write_config_dword(pdev, PSB_PCIx_MSI_ADDR_LOC, dev_priv->msi_addr);
+	pci_write_config_dword(pdev, PSB_PCIx_MSI_DATA_LOC, dev_priv->msi_data);
+	ret = pci_enable_device(pdev);
+
+	if (ret != 0)
+		dev_err(&pdev->dev, "pci_enable failed: %d\n", ret);
+	else
+		dev_priv->suspended = false;
+	return !dev_priv->suspended;
+}
+
+/**
+ *	gma_power_suspend		-	bus callback for suspend
+ *	@pdev: our PCI device
+ *	@state: suspend type
+ *
+ *	Called back by the PCI layer during a suspend of the system. We
+ *	perform the necessary shut down steps and save enough state that
+ *	we can undo this when resume is called.
+ */
+int gma_power_suspend(struct device *_dev)
+{
+	struct pci_dev *pdev = container_of(_dev, struct pci_dev, dev);
+	struct drm_device *dev = pci_get_drvdata(pdev);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	mutex_lock(&power_mutex);
+	if (!dev_priv->suspended) {
+		if (dev_priv->display_count) {
+			mutex_unlock(&power_mutex);
+			return -EBUSY;
+		}
+		psb_irq_uninstall(dev);
+		gma_suspend_display(dev);
+		gma_suspend_pci(pdev);
+	}
+	mutex_unlock(&power_mutex);
+	return 0;
+}
+
+/**
+ *	gma_power_resume		-	resume power
+ *	@pdev: PCI device
+ *
+ *	Resume the PCI side of the graphics and then the displays
+ */
+int gma_power_resume(struct device *_dev)
+{
+	struct pci_dev *pdev = container_of(_dev, struct pci_dev, dev);
+	struct drm_device *dev = pci_get_drvdata(pdev);
+
+	mutex_lock(&power_mutex);
+	gma_resume_pci(pdev);
+	gma_resume_display(pdev);
+	psb_irq_preinstall(dev);
+	psb_irq_postinstall(dev);
+	mutex_unlock(&power_mutex);
+	return 0;
+}
+
+/**
+ *	gma_power_is_on		-	returne true if power is on
+ *	@dev: our DRM device
+ *
+ *	Returns true if the display island power is on at this moment
+ */
+bool gma_power_is_on(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	return dev_priv->display_power;
+}
+
+/**
+ *	gma_power_begin		-	begin requiring power
+ *	@dev: our DRM device
+ *	@force_on: true to force power on
+ *
+ *	Begin an action that requires the display power island is enabled.
+ *	We refcount the islands.
+ */
+bool gma_power_begin(struct drm_device *dev, bool force_on)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&power_ctrl_lock, flags);
+	/* Power already on ? */
+	if (dev_priv->display_power) {
+		dev_priv->display_count++;
+		pm_runtime_get(&dev->pdev->dev);
+		spin_unlock_irqrestore(&power_ctrl_lock, flags);
+		return true;
+	}
+	if (force_on == false)
+		goto out_false;
+
+	/* Ok power up needed */
+	ret = gma_resume_pci(dev->pdev);
+	if (ret == 0) {
+		psb_irq_preinstall(dev);
+		psb_irq_postinstall(dev);
+		pm_runtime_get(&dev->pdev->dev);
+		dev_priv->display_count++;
+		spin_unlock_irqrestore(&power_ctrl_lock, flags);
+		return true;
+	}
+out_false:
+	spin_unlock_irqrestore(&power_ctrl_lock, flags);
+	return false;
+}
+
+/**
+ *	gma_power_end		-	end use of power
+ *	@dev: Our DRM device
+ *
+ *	Indicate that one of our gma_power_begin() requested periods when
+ *	the diplay island power is needed has completed.
+ */
+void gma_power_end(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned long flags;
+	spin_lock_irqsave(&power_ctrl_lock, flags);
+	dev_priv->display_count--;
+	WARN_ON(dev_priv->display_count < 0);
+	spin_unlock_irqrestore(&power_ctrl_lock, flags);
+	pm_runtime_put(&dev->pdev->dev);
+}
+
+int psb_runtime_suspend(struct device *dev)
+{
+	return gma_power_suspend(dev);
+}
+
+int psb_runtime_resume(struct device *dev)
+{
+	return gma_power_resume(dev);;
+}
+
+int psb_runtime_idle(struct device *dev)
+{
+	struct drm_device *drmdev = pci_get_drvdata(to_pci_dev(dev));
+	struct drm_psb_private *dev_priv = drmdev->dev_private;
+	if (dev_priv->display_count)
+		return 0;
+	else
+		return 1;
+}
diff --git a/drivers/gpu/drm/gma500/power.h b/drivers/gpu/drm/gma500/power.h
new file mode 100644
index 000000000000..1969d2ecb328
--- /dev/null
+++ b/drivers/gpu/drm/gma500/power.h
@@ -0,0 +1,67 @@
+/**************************************************************************
+ * Copyright (c) 2009-2011, Intel Corporation.
+ * All Rights Reserved.
+
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *    Benjamin Defnet <benjamin.r.defnet@intel.com>
+ *    Rajesh Poornachandran <rajesh.poornachandran@intel.com>
+ * Massively reworked
+ *    Alan Cox <alan@linux.intel.com>
+ */
+#ifndef _PSB_POWERMGMT_H_
+#define _PSB_POWERMGMT_H_
+
+#include <linux/pci.h>
+#include <drm/drmP.h>
+
+void gma_power_init(struct drm_device *dev);
+void gma_power_uninit(struct drm_device *dev);
+
+/*
+ * The kernel bus power management  will call these functions
+ */
+int gma_power_suspend(struct device *dev);
+int gma_power_resume(struct device *dev);
+
+/*
+ * These are the functions the driver should use to wrap all hw access
+ * (i.e. register reads and writes)
+ */
+bool gma_power_begin(struct drm_device *dev, bool force);
+void gma_power_end(struct drm_device *dev);
+
+/*
+ * Use this function to do an instantaneous check for if the hw is on.
+ * Only use this in cases where you know the mutex is already held such
+ * as in irq install/uninstall and you need to
+ * prevent a deadlock situation.  Otherwise use gma_power_begin().
+ */
+bool gma_power_is_on(struct drm_device *dev);
+
+/*
+ * GFX-Runtime PM callbacks
+ */
+int psb_runtime_suspend(struct device *dev);
+int psb_runtime_resume(struct device *dev);
+int psb_runtime_idle(struct device *dev);
+
+#endif /*_PSB_POWERMGMT_H_*/
diff --git a/drivers/gpu/drm/gma500/psb_device.c b/drivers/gpu/drm/gma500/psb_device.c
new file mode 100644
index 000000000000..e5f5906172b0
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_device.c
@@ -0,0 +1,328 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+#include <linux/backlight.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "gma_drm.h"
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "intel_bios.h"
+
+
+static int psb_output_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	psb_intel_lvds_init(dev, &dev_priv->mode_dev);
+	psb_intel_sdvo_init(dev, SDVOB);
+	return 0;
+}
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+
+/*
+ *	Poulsbo Backlight Interfaces
+ */
+
+#define BLC_PWM_PRECISION_FACTOR 100	/* 10000000 */
+#define BLC_PWM_FREQ_CALC_CONSTANT 32
+#define MHz 1000000
+
+#define PSB_BLC_PWM_PRECISION_FACTOR    10
+#define PSB_BLC_MAX_PWM_REG_FREQ        0xFFFE
+#define PSB_BLC_MIN_PWM_REG_FREQ        0x2
+
+#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
+#define PSB_BACKLIGHT_PWM_CTL_SHIFT	(16)
+
+static int psb_brightness;
+static struct backlight_device *psb_backlight_device;
+
+static int psb_get_brightness(struct backlight_device *bd)
+{
+	/* return locally cached var instead of HW read (due to DPST etc.) */
+	/* FIXME: ideally return actual value in case firmware fiddled with
+	   it */
+	return psb_brightness;
+}
+
+
+static int psb_backlight_setup(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned long core_clock;
+	/* u32 bl_max_freq; */
+	/* unsigned long value; */
+	u16 bl_max_freq;
+	uint32_t value;
+	uint32_t blc_pwm_precision_factor;
+
+	/* get bl_max_freq and pol from dev_priv*/
+	if (!dev_priv->lvds_bl) {
+		dev_err(dev->dev, "Has no valid LVDS backlight info\n");
+		return -ENOENT;
+	}
+	bl_max_freq = dev_priv->lvds_bl->freq;
+	blc_pwm_precision_factor = PSB_BLC_PWM_PRECISION_FACTOR;
+
+	core_clock = dev_priv->core_freq;
+
+	value = (core_clock * MHz) / BLC_PWM_FREQ_CALC_CONSTANT;
+	value *= blc_pwm_precision_factor;
+	value /= bl_max_freq;
+	value /= blc_pwm_precision_factor;
+
+	if (value > (unsigned long long)PSB_BLC_MAX_PWM_REG_FREQ ||
+		 value < (unsigned long long)PSB_BLC_MIN_PWM_REG_FREQ)
+				return -ERANGE;
+	else {
+		value &= PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR;
+		REG_WRITE(BLC_PWM_CTL,
+			(value << PSB_BACKLIGHT_PWM_CTL_SHIFT) | (value));
+	}
+	return 0;
+}
+
+static int psb_set_brightness(struct backlight_device *bd)
+{
+	struct drm_device *dev = bl_get_data(psb_backlight_device);
+	int level = bd->props.brightness;
+
+	/* Percentage 1-100% being valid */
+	if (level < 1)
+		level = 1;
+
+	psb_intel_lvds_set_brightness(dev, level);
+	psb_brightness = level;
+	return 0;
+}
+
+static const struct backlight_ops psb_ops = {
+	.get_brightness = psb_get_brightness,
+	.update_status  = psb_set_brightness,
+};
+
+static int psb_backlight_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	int ret;
+	struct backlight_properties props;
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.max_brightness = 100;
+	props.type = BACKLIGHT_PLATFORM;
+
+	psb_backlight_device = backlight_device_register("psb-bl",
+					NULL, (void *)dev, &psb_ops, &props);
+	if (IS_ERR(psb_backlight_device))
+		return PTR_ERR(psb_backlight_device);
+
+	ret = psb_backlight_setup(dev);
+	if (ret < 0) {
+		backlight_device_unregister(psb_backlight_device);
+		psb_backlight_device = NULL;
+		return ret;
+	}
+	psb_backlight_device->props.brightness = 100;
+	psb_backlight_device->props.max_brightness = 100;
+	backlight_update_status(psb_backlight_device);
+	dev_priv->backlight_device = psb_backlight_device;
+	return 0;
+}
+
+#endif
+
+/*
+ *	Provide the Poulsbo specific chip logic and low level methods
+ *	for power management
+ */
+
+static void psb_init_pm(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	u32 gating = PSB_RSGX32(PSB_CR_CLKGATECTL);
+	gating &= ~3;	/* Disable 2D clock gating */
+	gating |= 1;
+	PSB_WSGX32(gating, PSB_CR_CLKGATECTL);
+	PSB_RSGX32(PSB_CR_CLKGATECTL);
+}
+
+/**
+ *	psb_save_display_registers	-	save registers lost on suspend
+ *	@dev: our DRM device
+ *
+ *	Save the state we need in order to be able to restore the interface
+ *	upon resume from suspend
+ */
+static int psb_save_display_registers(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	struct drm_connector *connector;
+
+	/* Display arbitration control + watermarks */
+	dev_priv->saveDSPARB = PSB_RVDC32(DSPARB);
+	dev_priv->saveDSPFW1 = PSB_RVDC32(DSPFW1);
+	dev_priv->saveDSPFW2 = PSB_RVDC32(DSPFW2);
+	dev_priv->saveDSPFW3 = PSB_RVDC32(DSPFW3);
+	dev_priv->saveDSPFW4 = PSB_RVDC32(DSPFW4);
+	dev_priv->saveDSPFW5 = PSB_RVDC32(DSPFW5);
+	dev_priv->saveDSPFW6 = PSB_RVDC32(DSPFW6);
+	dev_priv->saveCHICKENBIT = PSB_RVDC32(DSPCHICKENBIT);
+
+	/* Save crtc and output state */
+	mutex_lock(&dev->mode_config.mutex);
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		if (drm_helper_crtc_in_use(crtc))
+			crtc->funcs->save(crtc);
+	}
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
+		connector->funcs->save(connector);
+
+	mutex_unlock(&dev->mode_config.mutex);
+	return 0;
+}
+
+/**
+ *	psb_restore_display_registers	-	restore lost register state
+ *	@dev: our DRM device
+ *
+ *	Restore register state that was lost during suspend and resume.
+ */
+static int psb_restore_display_registers(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	struct drm_connector *connector;
+
+	/* Display arbitration + watermarks */
+	PSB_WVDC32(dev_priv->saveDSPARB, DSPARB);
+	PSB_WVDC32(dev_priv->saveDSPFW1, DSPFW1);
+	PSB_WVDC32(dev_priv->saveDSPFW2, DSPFW2);
+	PSB_WVDC32(dev_priv->saveDSPFW3, DSPFW3);
+	PSB_WVDC32(dev_priv->saveDSPFW4, DSPFW4);
+	PSB_WVDC32(dev_priv->saveDSPFW5, DSPFW5);
+	PSB_WVDC32(dev_priv->saveDSPFW6, DSPFW6);
+	PSB_WVDC32(dev_priv->saveCHICKENBIT, DSPCHICKENBIT);
+
+	/*make sure VGA plane is off. it initializes to on after reset!*/
+	PSB_WVDC32(0x80000000, VGACNTRL);
+
+	mutex_lock(&dev->mode_config.mutex);
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+		if (drm_helper_crtc_in_use(crtc))
+			crtc->funcs->restore(crtc);
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
+		connector->funcs->restore(connector);
+
+	mutex_unlock(&dev->mode_config.mutex);
+	return 0;
+}
+
+static int psb_power_down(struct drm_device *dev)
+{
+	return 0;
+}
+
+static int psb_power_up(struct drm_device *dev)
+{
+	return 0;
+}
+
+static void psb_get_core_freq(struct drm_device *dev)
+{
+	uint32_t clock;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	/*pci_write_config_dword(pci_root, 0xD4, 0x00C32004);*/
+	/*pci_write_config_dword(pci_root, 0xD0, 0xE0033000);*/
+
+	pci_write_config_dword(pci_root, 0xD0, 0xD0050300);
+	pci_read_config_dword(pci_root, 0xD4, &clock);
+	pci_dev_put(pci_root);
+
+	switch (clock & 0x07) {
+	case 0:
+		dev_priv->core_freq = 100;
+		break;
+	case 1:
+		dev_priv->core_freq = 133;
+		break;
+	case 2:
+		dev_priv->core_freq = 150;
+		break;
+	case 3:
+		dev_priv->core_freq = 178;
+		break;
+	case 4:
+		dev_priv->core_freq = 200;
+		break;
+	case 5:
+	case 6:
+	case 7:
+		dev_priv->core_freq = 266;
+	default:
+		dev_priv->core_freq = 0;
+	}
+}
+
+static int psb_chip_setup(struct drm_device *dev)
+{
+	psb_get_core_freq(dev);
+	gma_intel_setup_gmbus(dev);
+	gma_intel_opregion_init(dev);
+	psb_intel_init_bios(dev);
+	return 0;
+}
+
+static void psb_chip_teardown(struct drm_device *dev)
+{
+	gma_intel_teardown_gmbus(dev);
+}
+
+const struct psb_ops psb_chip_ops = {
+	.name = "Poulsbo",
+	.accel_2d = 1,
+	.pipes = 2,
+	.crtcs = 2,
+	.sgx_offset = PSB_SGX_OFFSET,
+	.chip_setup = psb_chip_setup,
+	.chip_teardown = psb_chip_teardown,
+
+	.crtc_helper = &psb_intel_helper_funcs,
+	.crtc_funcs = &psb_intel_crtc_funcs,
+
+	.output_init = psb_output_init,
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+	.backlight_init = psb_backlight_init,
+#endif
+
+	.init_pm = psb_init_pm,
+	.save_regs = psb_save_display_registers,
+	.restore_regs = psb_restore_display_registers,
+	.power_down = psb_power_down,
+	.power_up = psb_power_up,
+};
+
diff --git a/drivers/gpu/drm/gma500/psb_drv.c b/drivers/gpu/drm/gma500/psb_drv.c
new file mode 100644
index 000000000000..f14768f2b364
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_drv.c
@@ -0,0 +1,703 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ * Copyright (c) 2008, Tungsten Graphics, Inc. Cedar Park, TX., USA.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "gma_drm.h"
+#include "psb_drv.h"
+#include "framebuffer.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "intel_bios.h"
+#include "mid_bios.h"
+#include <drm/drm_pciids.h>
+#include "power.h"
+#include <linux/cpu.h>
+#include <linux/notifier.h>
+#include <linux/spinlock.h>
+#include <linux/pm_runtime.h>
+#include <acpi/video.h>
+#include <linux/module.h>
+
+static int drm_psb_trap_pagefaults;
+
+static int psb_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
+
+MODULE_PARM_DESC(trap_pagefaults, "Error and reset on MMU pagefaults");
+module_param_named(trap_pagefaults, drm_psb_trap_pagefaults, int, 0600);
+
+
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+	{ 0x8086, 0x8108, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &psb_chip_ops },
+	{ 0x8086, 0x8109, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &psb_chip_ops },
+#if defined(CONFIG_DRM_GMA600)
+	{ 0x8086, 0x4100, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+	{ 0x8086, 0x4101, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+	{ 0x8086, 0x4102, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+	{ 0x8086, 0x4103, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+	{ 0x8086, 0x4104, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+	{ 0x8086, 0x4105, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+	{ 0x8086, 0x4106, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+	{ 0x8086, 0x4107, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+	/* Atom E620 */
+	{ 0x8086, 0x4108, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &oaktrail_chip_ops},
+#endif
+#if defined(CONFIG_DRM_GMA3600)
+	{ 0x8086, 0x0be0, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be1, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be2, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be3, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be4, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be5, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be6, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+	{ 0x8086, 0x0be7, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+#endif
+	{ 0, 0, 0}
+};
+MODULE_DEVICE_TABLE(pci, pciidlist);
+
+/*
+ * Standard IOCTLs.
+ */
+
+#define DRM_IOCTL_PSB_ADB	\
+		DRM_IOWR(DRM_GMA_ADB + DRM_COMMAND_BASE, uint32_t)
+#define DRM_IOCTL_PSB_MODE_OPERATION	\
+		DRM_IOWR(DRM_GMA_MODE_OPERATION + DRM_COMMAND_BASE, \
+			 struct drm_psb_mode_operation_arg)
+#define DRM_IOCTL_PSB_STOLEN_MEMORY	\
+		DRM_IOWR(DRM_GMA_STOLEN_MEMORY + DRM_COMMAND_BASE, \
+			 struct drm_psb_stolen_memory_arg)
+#define DRM_IOCTL_PSB_GAMMA	\
+		DRM_IOWR(DRM_GMA_GAMMA + DRM_COMMAND_BASE, \
+			 struct drm_psb_dpst_lut_arg)
+#define DRM_IOCTL_PSB_DPST_BL	\
+		DRM_IOWR(DRM_GMA_DPST_BL + DRM_COMMAND_BASE, \
+			 uint32_t)
+#define DRM_IOCTL_PSB_GET_PIPE_FROM_CRTC_ID	\
+		DRM_IOWR(DRM_GMA_GET_PIPE_FROM_CRTC_ID + DRM_COMMAND_BASE, \
+			 struct drm_psb_get_pipe_from_crtc_id_arg)
+#define DRM_IOCTL_PSB_GEM_CREATE	\
+		DRM_IOWR(DRM_GMA_GEM_CREATE + DRM_COMMAND_BASE, \
+			 struct drm_psb_gem_create)
+#define DRM_IOCTL_PSB_GEM_MMAP	\
+		DRM_IOWR(DRM_GMA_GEM_MMAP + DRM_COMMAND_BASE, \
+			 struct drm_psb_gem_mmap)
+
+static int psb_adb_ioctl(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv);
+static int psb_mode_operation_ioctl(struct drm_device *dev, void *data,
+				    struct drm_file *file_priv);
+static int psb_stolen_memory_ioctl(struct drm_device *dev, void *data,
+				   struct drm_file *file_priv);
+static int psb_gamma_ioctl(struct drm_device *dev, void *data,
+			   struct drm_file *file_priv);
+static int psb_dpst_bl_ioctl(struct drm_device *dev, void *data,
+			     struct drm_file *file_priv);
+
+#define PSB_IOCTL_DEF(ioctl, func, flags) \
+	[DRM_IOCTL_NR(ioctl) - DRM_COMMAND_BASE] = {ioctl, flags, func}
+
+static struct drm_ioctl_desc psb_ioctls[] = {
+	PSB_IOCTL_DEF(DRM_IOCTL_PSB_ADB, psb_adb_ioctl, DRM_AUTH),
+	PSB_IOCTL_DEF(DRM_IOCTL_PSB_MODE_OPERATION, psb_mode_operation_ioctl,
+		      DRM_AUTH),
+	PSB_IOCTL_DEF(DRM_IOCTL_PSB_STOLEN_MEMORY, psb_stolen_memory_ioctl,
+		      DRM_AUTH),
+	PSB_IOCTL_DEF(DRM_IOCTL_PSB_GAMMA, psb_gamma_ioctl, DRM_AUTH),
+	PSB_IOCTL_DEF(DRM_IOCTL_PSB_DPST_BL, psb_dpst_bl_ioctl, DRM_AUTH),
+	PSB_IOCTL_DEF(DRM_IOCTL_PSB_GET_PIPE_FROM_CRTC_ID,
+					psb_intel_get_pipe_from_crtc_id, 0),
+	PSB_IOCTL_DEF(DRM_IOCTL_PSB_GEM_CREATE, psb_gem_create_ioctl,
+						DRM_UNLOCKED | DRM_AUTH),
+	PSB_IOCTL_DEF(DRM_IOCTL_PSB_GEM_MMAP, psb_gem_mmap_ioctl,
+						DRM_UNLOCKED | DRM_AUTH),
+};
+
+static void psb_lastclose(struct drm_device *dev)
+{
+	return;
+}
+
+static void psb_do_takedown(struct drm_device *dev)
+{
+}
+
+static int psb_do_init(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_gtt *pg = &dev_priv->gtt;
+
+	uint32_t stolen_gtt;
+
+	int ret = -ENOMEM;
+
+	if (pg->mmu_gatt_start & 0x0FFFFFFF) {
+		dev_err(dev->dev, "Gatt must be 256M aligned. This is a bug.\n");
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+
+	stolen_gtt = (pg->stolen_size >> PAGE_SHIFT) * 4;
+	stolen_gtt = (stolen_gtt + PAGE_SIZE - 1) >> PAGE_SHIFT;
+	stolen_gtt =
+	    (stolen_gtt < pg->gtt_pages) ? stolen_gtt : pg->gtt_pages;
+
+	dev_priv->gatt_free_offset = pg->mmu_gatt_start +
+	    (stolen_gtt << PAGE_SHIFT) * 1024;
+
+	if (1 || drm_debug) {
+		uint32_t core_id = PSB_RSGX32(PSB_CR_CORE_ID);
+		uint32_t core_rev = PSB_RSGX32(PSB_CR_CORE_REVISION);
+		DRM_INFO("SGX core id = 0x%08x\n", core_id);
+		DRM_INFO("SGX core rev major = 0x%02x, minor = 0x%02x\n",
+			 (core_rev & _PSB_CC_REVISION_MAJOR_MASK) >>
+			 _PSB_CC_REVISION_MAJOR_SHIFT,
+			 (core_rev & _PSB_CC_REVISION_MINOR_MASK) >>
+			 _PSB_CC_REVISION_MINOR_SHIFT);
+		DRM_INFO
+		    ("SGX core rev maintenance = 0x%02x, designer = 0x%02x\n",
+		     (core_rev & _PSB_CC_REVISION_MAINTENANCE_MASK) >>
+		     _PSB_CC_REVISION_MAINTENANCE_SHIFT,
+		     (core_rev & _PSB_CC_REVISION_DESIGNER_MASK) >>
+		     _PSB_CC_REVISION_DESIGNER_SHIFT);
+	}
+
+
+	spin_lock_init(&dev_priv->irqmask_lock);
+	spin_lock_init(&dev_priv->lock_2d);
+
+	PSB_WSGX32(0x00000000, PSB_CR_BIF_BANK0);
+	PSB_WSGX32(0x00000000, PSB_CR_BIF_BANK1);
+	PSB_RSGX32(PSB_CR_BIF_BANK1);
+	PSB_WSGX32(PSB_RSGX32(PSB_CR_BIF_CTRL) | _PSB_MMU_ER_MASK,
+							PSB_CR_BIF_CTRL);
+	psb_spank(dev_priv);
+
+	/* mmu_gatt ?? */
+	PSB_WSGX32(pg->gatt_start, PSB_CR_BIF_TWOD_REQ_BASE);
+	return 0;
+out_err:
+	psb_do_takedown(dev);
+	return ret;
+}
+
+static int psb_driver_unload(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	/* Kill vblank etc here */
+
+	gma_backlight_exit(dev);
+
+	psb_modeset_cleanup(dev);
+
+	if (dev_priv) {
+		psb_lid_timer_takedown(dev_priv);
+		gma_intel_opregion_exit(dev);
+
+		if (dev_priv->ops->chip_teardown)
+			dev_priv->ops->chip_teardown(dev);
+		psb_do_takedown(dev);
+
+
+		if (dev_priv->pf_pd) {
+			psb_mmu_free_pagedir(dev_priv->pf_pd);
+			dev_priv->pf_pd = NULL;
+		}
+		if (dev_priv->mmu) {
+			struct psb_gtt *pg = &dev_priv->gtt;
+
+			down_read(&pg->sem);
+			psb_mmu_remove_pfn_sequence(
+				psb_mmu_get_default_pd
+				(dev_priv->mmu),
+				pg->mmu_gatt_start,
+				dev_priv->vram_stolen_size >> PAGE_SHIFT);
+			up_read(&pg->sem);
+			psb_mmu_driver_takedown(dev_priv->mmu);
+			dev_priv->mmu = NULL;
+		}
+		psb_gtt_takedown(dev);
+		if (dev_priv->scratch_page) {
+			__free_page(dev_priv->scratch_page);
+			dev_priv->scratch_page = NULL;
+		}
+		if (dev_priv->vdc_reg) {
+			iounmap(dev_priv->vdc_reg);
+			dev_priv->vdc_reg = NULL;
+		}
+		if (dev_priv->sgx_reg) {
+			iounmap(dev_priv->sgx_reg);
+			dev_priv->sgx_reg = NULL;
+		}
+
+		kfree(dev_priv);
+		dev->dev_private = NULL;
+
+		/*destroy VBT data*/
+		psb_intel_destroy_bios(dev);
+	}
+
+	gma_power_uninit(dev);
+
+	return 0;
+}
+
+
+static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
+{
+	struct drm_psb_private *dev_priv;
+	unsigned long resource_start;
+	struct psb_gtt *pg;
+	unsigned long irqflags;
+	int ret = -ENOMEM;
+	uint32_t tt_pages;
+	struct drm_connector *connector;
+	struct psb_intel_encoder *psb_intel_encoder;
+
+	dev_priv = kzalloc(sizeof(*dev_priv), GFP_KERNEL);
+	if (dev_priv == NULL)
+		return -ENOMEM;
+
+	dev_priv->ops = (struct psb_ops *)chipset;
+	dev_priv->dev = dev;
+	dev->dev_private = (void *) dev_priv;
+
+	if (!IS_PSB(dev)) {
+		if (pci_enable_msi(dev->pdev))
+			dev_warn(dev->dev, "Enabling MSI failed!\n");
+	}
+
+	dev_priv->num_pipe = dev_priv->ops->pipes;
+
+	resource_start = pci_resource_start(dev->pdev, PSB_MMIO_RESOURCE);
+
+	dev_priv->vdc_reg =
+	    ioremap(resource_start + PSB_VDC_OFFSET, PSB_VDC_SIZE);
+	if (!dev_priv->vdc_reg)
+		goto out_err;
+
+	dev_priv->sgx_reg = ioremap(resource_start + dev_priv->ops->sgx_offset,
+							PSB_SGX_SIZE);
+	if (!dev_priv->sgx_reg)
+		goto out_err;
+
+	ret = dev_priv->ops->chip_setup(dev);
+	if (ret)
+		goto out_err;
+
+	/* Init OSPM support */
+	gma_power_init(dev);
+
+	ret = -ENOMEM;
+
+	dev_priv->scratch_page = alloc_page(GFP_DMA32 | __GFP_ZERO);
+	if (!dev_priv->scratch_page)
+		goto out_err;
+
+	set_pages_uc(dev_priv->scratch_page, 1);
+
+	ret = psb_gtt_init(dev, 0);
+	if (ret)
+		goto out_err;
+
+	dev_priv->mmu = psb_mmu_driver_init((void *)0,
+					drm_psb_trap_pagefaults, 0,
+					dev_priv);
+	if (!dev_priv->mmu)
+		goto out_err;
+
+	pg = &dev_priv->gtt;
+
+	tt_pages = (pg->gatt_pages < PSB_TT_PRIV0_PLIMIT) ?
+		(pg->gatt_pages) : PSB_TT_PRIV0_PLIMIT;
+
+
+	dev_priv->pf_pd = psb_mmu_alloc_pd(dev_priv->mmu, 1, 0);
+	if (!dev_priv->pf_pd)
+		goto out_err;
+
+	psb_mmu_set_pd_context(psb_mmu_get_default_pd(dev_priv->mmu), 0);
+	psb_mmu_set_pd_context(dev_priv->pf_pd, 1);
+
+	ret = psb_do_init(dev);
+	if (ret)
+		return ret;
+
+	PSB_WSGX32(0x20000000, PSB_CR_PDS_EXEC_BASE);
+	PSB_WSGX32(0x30000000, PSB_CR_BIF_3D_REQ_BASE);
+
+/*	igd_opregion_init(&dev_priv->opregion_dev); */
+	acpi_video_register();
+	if (dev_priv->lid_state)
+		psb_lid_timer_init(dev_priv);
+
+	ret = drm_vblank_init(dev, dev_priv->num_pipe);
+	if (ret)
+		goto out_err;
+
+	/*
+	 * Install interrupt handlers prior to powering off SGX or else we will
+	 * crash.
+	 */
+	dev_priv->vdc_irq_mask = 0;
+	dev_priv->pipestat[0] = 0;
+	dev_priv->pipestat[1] = 0;
+	dev_priv->pipestat[2] = 0;
+	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+	PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
+	PSB_WVDC32(0x00000000, PSB_INT_ENABLE_R);
+	PSB_WVDC32(0xFFFFFFFF, PSB_INT_MASK_R);
+	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+	if (IS_PSB(dev) && drm_core_check_feature(dev, DRIVER_MODESET))
+		drm_irq_install(dev);
+
+	dev->vblank_disable_allowed = 1;
+
+	dev->max_vblank_count = 0xffffff; /* only 24 bits of frame count */
+
+	dev->driver->get_vblank_counter = psb_get_vblank_counter;
+
+	psb_modeset_init(dev);
+	psb_fbdev_init(dev);
+	drm_kms_helper_poll_init(dev);
+
+	/* Only add backlight support if we have LVDS output */
+	list_for_each_entry(connector, &dev->mode_config.connector_list,
+			    head) {
+		psb_intel_encoder = psb_intel_attached_encoder(connector);
+
+		switch (psb_intel_encoder->type) {
+		case INTEL_OUTPUT_LVDS:
+		case INTEL_OUTPUT_MIPI:
+			ret = gma_backlight_init(dev);
+			break;
+		}
+	}
+
+	if (ret)
+		return ret;
+#if 0
+	/*enable runtime pm at last*/
+	pm_runtime_enable(&dev->pdev->dev);
+	pm_runtime_set_active(&dev->pdev->dev);
+#endif
+	/*Intel drm driver load is done, continue doing pvr load*/
+	return 0;
+out_err:
+	psb_driver_unload(dev);
+	return ret;
+}
+
+int psb_driver_device_is_agp(struct drm_device *dev)
+{
+	return 0;
+}
+
+static inline void get_brightness(struct backlight_device *bd)
+{
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+	if (bd) {
+		bd->props.brightness = bd->ops->get_brightness(bd);
+		backlight_update_status(bd);
+	}
+#endif
+}
+
+static int psb_dpst_bl_ioctl(struct drm_device *dev, void *data,
+		       struct drm_file *file_priv)
+{
+	struct drm_psb_private *dev_priv = psb_priv(dev);
+	uint32_t *arg = data;
+
+	dev_priv->blc_adj2 = *arg;
+	get_brightness(dev_priv->backlight_device);
+	return 0;
+}
+
+static int psb_adb_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file_priv)
+{
+	struct drm_psb_private *dev_priv = psb_priv(dev);
+	uint32_t *arg = data;
+
+	dev_priv->blc_adj1 = *arg;
+	get_brightness(dev_priv->backlight_device);
+	return 0;
+}
+
+static int psb_gamma_ioctl(struct drm_device *dev, void *data,
+			   struct drm_file *file_priv)
+{
+	struct drm_psb_dpst_lut_arg *lut_arg = data;
+	struct drm_mode_object *obj;
+	struct drm_crtc *crtc;
+	struct drm_connector *connector;
+	struct psb_intel_crtc *psb_intel_crtc;
+	int i = 0;
+	int32_t obj_id;
+
+	obj_id = lut_arg->output_id;
+	obj = drm_mode_object_find(dev, obj_id, DRM_MODE_OBJECT_CONNECTOR);
+	if (!obj) {
+		dev_dbg(dev->dev, "Invalid Connector object.\n");
+		return -EINVAL;
+	}
+
+	connector = obj_to_connector(obj);
+	crtc = connector->encoder->crtc;
+	psb_intel_crtc = to_psb_intel_crtc(crtc);
+
+	for (i = 0; i < 256; i++)
+		psb_intel_crtc->lut_adj[i] = lut_arg->lut[i];
+
+	psb_intel_crtc_load_lut(crtc);
+
+	return 0;
+}
+
+static int psb_mode_operation_ioctl(struct drm_device *dev, void *data,
+				struct drm_file *file_priv)
+{
+	uint32_t obj_id;
+	uint16_t op;
+	struct drm_mode_modeinfo *umode;
+	struct drm_display_mode *mode = NULL;
+	struct drm_psb_mode_operation_arg *arg;
+	struct drm_mode_object *obj;
+	struct drm_connector *connector;
+	struct drm_connector_helper_funcs *connector_funcs;
+	int ret = 0;
+	int resp = MODE_OK;
+
+	arg = (struct drm_psb_mode_operation_arg *)data;
+	obj_id = arg->obj_id;
+	op = arg->operation;
+
+	switch (op) {
+	case PSB_MODE_OPERATION_MODE_VALID:
+		umode = &arg->mode;
+
+		mutex_lock(&dev->mode_config.mutex);
+
+		obj = drm_mode_object_find(dev, obj_id,
+					DRM_MODE_OBJECT_CONNECTOR);
+		if (!obj) {
+			ret = -EINVAL;
+			goto mode_op_out;
+		}
+
+		connector = obj_to_connector(obj);
+
+		mode = drm_mode_create(dev);
+		if (!mode) {
+			ret = -ENOMEM;
+			goto mode_op_out;
+		}
+
+		/* drm_crtc_convert_umode(mode, umode); */
+		{
+			mode->clock = umode->clock;
+			mode->hdisplay = umode->hdisplay;
+			mode->hsync_start = umode->hsync_start;
+			mode->hsync_end = umode->hsync_end;
+			mode->htotal = umode->htotal;
+			mode->hskew = umode->hskew;
+			mode->vdisplay = umode->vdisplay;
+			mode->vsync_start = umode->vsync_start;
+			mode->vsync_end = umode->vsync_end;
+			mode->vtotal = umode->vtotal;
+			mode->vscan = umode->vscan;
+			mode->vrefresh = umode->vrefresh;
+			mode->flags = umode->flags;
+			mode->type = umode->type;
+			strncpy(mode->name, umode->name, DRM_DISPLAY_MODE_LEN);
+			mode->name[DRM_DISPLAY_MODE_LEN-1] = 0;
+		}
+
+		connector_funcs = (struct drm_connector_helper_funcs *)
+				   connector->helper_private;
+
+		if (connector_funcs->mode_valid) {
+			resp = connector_funcs->mode_valid(connector, mode);
+			arg->data = resp;
+		}
+
+		/*do some clean up work*/
+		if (mode)
+			drm_mode_destroy(dev, mode);
+mode_op_out:
+		mutex_unlock(&dev->mode_config.mutex);
+		return ret;
+
+	default:
+		dev_dbg(dev->dev, "Unsupported psb mode operation\n");
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static int psb_stolen_memory_ioctl(struct drm_device *dev, void *data,
+				   struct drm_file *file_priv)
+{
+	struct drm_psb_private *dev_priv = psb_priv(dev);
+	struct drm_psb_stolen_memory_arg *arg = data;
+
+	arg->base = dev_priv->stolen_base;
+	arg->size = dev_priv->vram_stolen_size;
+
+	return 0;
+}
+
+static int psb_driver_open(struct drm_device *dev, struct drm_file *priv)
+{
+	return 0;
+}
+
+static void psb_driver_close(struct drm_device *dev, struct drm_file *priv)
+{
+}
+
+static long psb_unlocked_ioctl(struct file *filp, unsigned int cmd,
+			       unsigned long arg)
+{
+	struct drm_file *file_priv = filp->private_data;
+	struct drm_device *dev = file_priv->minor->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	static unsigned int runtime_allowed;
+
+	if (runtime_allowed == 1 && dev_priv->is_lvds_on) {
+		runtime_allowed++;
+		pm_runtime_allow(&dev->pdev->dev);
+		dev_priv->rpm_enabled = 1;
+	}
+	return drm_ioctl(filp, cmd, arg);
+	/* FIXME: do we need to wrap the other side of this */
+}
+
+
+/* When a client dies:
+ *    - Check for and clean up flipped page state
+ */
+void psb_driver_preclose(struct drm_device *dev, struct drm_file *priv)
+{
+}
+
+static void psb_remove(struct pci_dev *pdev)
+{
+	struct drm_device *dev = pci_get_drvdata(pdev);
+	drm_put_dev(dev);
+}
+
+static const struct dev_pm_ops psb_pm_ops = {
+	.resume = gma_power_resume,
+	.suspend = gma_power_suspend,
+	.runtime_suspend = psb_runtime_suspend,
+	.runtime_resume = psb_runtime_resume,
+	.runtime_idle = psb_runtime_idle,
+};
+
+static struct vm_operations_struct psb_gem_vm_ops = {
+	.fault = psb_gem_fault,
+	.open = drm_gem_vm_open,
+	.close = drm_gem_vm_close,
+};
+
+static const struct file_operations psb_gem_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = psb_unlocked_ioctl,
+	.mmap = drm_gem_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+};
+
+static struct drm_driver driver = {
+	.driver_features = DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | \
+			   DRIVER_IRQ_VBL | DRIVER_MODESET | DRIVER_GEM ,
+	.load = psb_driver_load,
+	.unload = psb_driver_unload,
+
+	.ioctls = psb_ioctls,
+	.num_ioctls = DRM_ARRAY_SIZE(psb_ioctls),
+	.device_is_agp = psb_driver_device_is_agp,
+	.irq_preinstall = psb_irq_preinstall,
+	.irq_postinstall = psb_irq_postinstall,
+	.irq_uninstall = psb_irq_uninstall,
+	.irq_handler = psb_irq_handler,
+	.enable_vblank = psb_enable_vblank,
+	.disable_vblank = psb_disable_vblank,
+	.get_vblank_counter = psb_get_vblank_counter,
+	.lastclose = psb_lastclose,
+	.open = psb_driver_open,
+	.preclose = psb_driver_preclose,
+	.postclose = psb_driver_close,
+	.reclaim_buffers = drm_core_reclaim_buffers,
+
+	.gem_init_object = psb_gem_init_object,
+	.gem_free_object = psb_gem_free_object,
+	.gem_vm_ops = &psb_gem_vm_ops,
+	.dumb_create = psb_gem_dumb_create,
+	.dumb_map_offset = psb_gem_dumb_map_gtt,
+	.dumb_destroy = psb_gem_dumb_destroy,
+	.fops = &psb_gem_fops,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = PSB_DRM_DRIVER_DATE,
+	.major = PSB_DRM_DRIVER_MAJOR,
+	.minor = PSB_DRM_DRIVER_MINOR,
+	.patchlevel = PSB_DRM_DRIVER_PATCHLEVEL
+};
+
+static struct pci_driver psb_pci_driver = {
+	.name = DRIVER_NAME,
+	.id_table = pciidlist,
+	.probe = psb_probe,
+	.remove = psb_remove,
+	.driver.pm = &psb_pm_ops,
+};
+
+static int psb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	return drm_get_pci_dev(pdev, ent, &driver);
+}
+
+static int __init psb_init(void)
+{
+	return drm_pci_init(&driver, &psb_pci_driver);
+}
+
+static void __exit psb_exit(void)
+{
+	drm_pci_exit(&driver, &psb_pci_driver);
+}
+
+late_initcall(psb_init);
+module_exit(psb_exit);
+
+MODULE_AUTHOR("Alan Cox <alan@linux.intel.com> and others");
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/gma500/psb_drv.h b/drivers/gpu/drm/gma500/psb_drv.h
new file mode 100644
index 000000000000..eb1568a0da95
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_drv.h
@@ -0,0 +1,956 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ **************************************************************************/
+
+#ifndef _PSB_DRV_H_
+#define _PSB_DRV_H_
+
+#include <linux/kref.h>
+
+#include <drm/drmP.h>
+#include "drm_global.h"
+#include "gem_glue.h"
+#include "gma_drm.h"
+#include "psb_reg.h"
+#include "psb_intel_drv.h"
+#include "gtt.h"
+#include "power.h"
+#include "oaktrail.h"
+
+/* Append new drm mode definition here, align with libdrm definition */
+#define DRM_MODE_SCALE_NO_SCALE   	2
+
+enum {
+	CHIP_PSB_8108 = 0,		/* Poulsbo */
+	CHIP_PSB_8109 = 1,		/* Poulsbo */
+	CHIP_MRST_4100 = 2,		/* Moorestown/Oaktrail */
+	CHIP_MFLD_0130 = 3,		/* Medfield */
+};
+
+#define IS_PSB(dev) (((dev)->pci_device & 0xfffe) == 0x8108)
+#define IS_MRST(dev) (((dev)->pci_device & 0xfffc) == 0x4100)
+#define IS_MFLD(dev) (((dev)->pci_device & 0xfff8) == 0x0130)
+
+/*
+ * Driver definitions
+ */
+
+#define DRIVER_NAME "gma500"
+#define DRIVER_DESC "DRM driver for the Intel GMA500"
+
+#define PSB_DRM_DRIVER_DATE "2011-06-06"
+#define PSB_DRM_DRIVER_MAJOR 1
+#define PSB_DRM_DRIVER_MINOR 0
+#define PSB_DRM_DRIVER_PATCHLEVEL 0
+
+/*
+ *	Hardware offsets
+ */
+#define PSB_VDC_OFFSET		 0x00000000
+#define PSB_VDC_SIZE		 0x000080000
+#define MRST_MMIO_SIZE		 0x0000C0000
+#define MDFLD_MMIO_SIZE          0x000100000
+#define PSB_SGX_SIZE		 0x8000
+#define PSB_SGX_OFFSET		 0x00040000
+#define MRST_SGX_OFFSET		 0x00080000
+/*
+ *	PCI resource identifiers
+ */
+#define PSB_MMIO_RESOURCE	 0
+#define PSB_GATT_RESOURCE	 2
+#define PSB_GTT_RESOURCE	 3
+/*
+ *	PCI configuration
+ */
+#define PSB_GMCH_CTRL		 0x52
+#define PSB_BSM			 0x5C
+#define _PSB_GMCH_ENABLED	 0x4
+#define PSB_PGETBL_CTL		 0x2020
+#define _PSB_PGETBL_ENABLED	 0x00000001
+#define PSB_SGX_2D_SLAVE_PORT	 0x4000
+
+/* To get rid of */
+#define PSB_TT_PRIV0_LIMIT	 (256*1024*1024)
+#define PSB_TT_PRIV0_PLIMIT	 (PSB_TT_PRIV0_LIMIT >> PAGE_SHIFT)
+
+/*
+ *	SGX side MMU definitions (these can probably go)
+ */
+
+/*
+ *	Flags for external memory type field.
+ */
+#define PSB_MMU_CACHED_MEMORY	  0x0001	/* Bind to MMU only */
+#define PSB_MMU_RO_MEMORY	  0x0002	/* MMU RO memory */
+#define PSB_MMU_WO_MEMORY	  0x0004	/* MMU WO memory */
+/*
+ *	PTE's and PDE's
+ */
+#define PSB_PDE_MASK		  0x003FFFFF
+#define PSB_PDE_SHIFT		  22
+#define PSB_PTE_SHIFT		  12
+/*
+ *	Cache control
+ */
+#define PSB_PTE_VALID		  0x0001	/* PTE / PDE valid */
+#define PSB_PTE_WO		  0x0002	/* Write only */
+#define PSB_PTE_RO		  0x0004	/* Read only */
+#define PSB_PTE_CACHED		  0x0008	/* CPU cache coherent */
+
+/*
+ *	VDC registers and bits
+ */
+#define PSB_MSVDX_CLOCKGATING	  0x2064
+#define PSB_TOPAZ_CLOCKGATING	  0x2068
+#define PSB_HWSTAM		  0x2098
+#define PSB_INSTPM		  0x20C0
+#define PSB_INT_IDENTITY_R        0x20A4
+#define _MDFLD_PIPEC_EVENT_FLAG   (1<<2)
+#define _MDFLD_PIPEC_VBLANK_FLAG  (1<<3)
+#define _PSB_DPST_PIPEB_FLAG      (1<<4)
+#define _MDFLD_PIPEB_EVENT_FLAG   (1<<4)
+#define _PSB_VSYNC_PIPEB_FLAG	  (1<<5)
+#define _PSB_DPST_PIPEA_FLAG      (1<<6)
+#define _PSB_PIPEA_EVENT_FLAG     (1<<6)
+#define _PSB_VSYNC_PIPEA_FLAG	  (1<<7)
+#define _MDFLD_MIPIA_FLAG	  (1<<16)
+#define _MDFLD_MIPIC_FLAG	  (1<<17)
+#define _PSB_IRQ_SGX_FLAG	  (1<<18)
+#define _PSB_IRQ_MSVDX_FLAG	  (1<<19)
+#define _LNC_IRQ_TOPAZ_FLAG	  (1<<20)
+
+#define _PSB_PIPE_EVENT_FLAG	(_PSB_VSYNC_PIPEA_FLAG | \
+				 _PSB_VSYNC_PIPEB_FLAG)
+
+/* This flag includes all the display IRQ bits excepts the vblank irqs. */
+#define _MDFLD_DISP_ALL_IRQ_FLAG (_MDFLD_PIPEC_EVENT_FLAG | \
+				  _MDFLD_PIPEB_EVENT_FLAG | \
+				  _PSB_PIPEA_EVENT_FLAG | \
+				  _PSB_VSYNC_PIPEA_FLAG | \
+				  _MDFLD_MIPIA_FLAG | \
+				  _MDFLD_MIPIC_FLAG)
+#define PSB_INT_IDENTITY_R	  0x20A4
+#define PSB_INT_MASK_R		  0x20A8
+#define PSB_INT_ENABLE_R	  0x20A0
+
+#define _PSB_MMU_ER_MASK      0x0001FF00
+#define _PSB_MMU_ER_HOST      (1 << 16)
+#define GPIOA			0x5010
+#define GPIOB			0x5014
+#define GPIOC			0x5018
+#define GPIOD			0x501c
+#define GPIOE			0x5020
+#define GPIOF			0x5024
+#define GPIOG			0x5028
+#define GPIOH			0x502c
+#define GPIO_CLOCK_DIR_MASK		(1 << 0)
+#define GPIO_CLOCK_DIR_IN		(0 << 1)
+#define GPIO_CLOCK_DIR_OUT		(1 << 1)
+#define GPIO_CLOCK_VAL_MASK		(1 << 2)
+#define GPIO_CLOCK_VAL_OUT		(1 << 3)
+#define GPIO_CLOCK_VAL_IN		(1 << 4)
+#define GPIO_CLOCK_PULLUP_DISABLE	(1 << 5)
+#define GPIO_DATA_DIR_MASK		(1 << 8)
+#define GPIO_DATA_DIR_IN		(0 << 9)
+#define GPIO_DATA_DIR_OUT		(1 << 9)
+#define GPIO_DATA_VAL_MASK		(1 << 10)
+#define GPIO_DATA_VAL_OUT		(1 << 11)
+#define GPIO_DATA_VAL_IN		(1 << 12)
+#define GPIO_DATA_PULLUP_DISABLE	(1 << 13)
+
+#define VCLK_DIVISOR_VGA0   0x6000
+#define VCLK_DIVISOR_VGA1   0x6004
+#define VCLK_POST_DIV	    0x6010
+
+#define PSB_COMM_2D (PSB_ENGINE_2D << 4)
+#define PSB_COMM_3D (PSB_ENGINE_3D << 4)
+#define PSB_COMM_TA (PSB_ENGINE_TA << 4)
+#define PSB_COMM_HP (PSB_ENGINE_HP << 4)
+#define PSB_COMM_USER_IRQ (1024 >> 2)
+#define PSB_COMM_USER_IRQ_LOST (PSB_COMM_USER_IRQ + 1)
+#define PSB_COMM_FW (2048 >> 2)
+
+#define PSB_UIRQ_VISTEST	       1
+#define PSB_UIRQ_OOM_REPLY	       2
+#define PSB_UIRQ_FIRE_TA_REPLY	       3
+#define PSB_UIRQ_FIRE_RASTER_REPLY     4
+
+#define PSB_2D_SIZE (256*1024*1024)
+#define PSB_MAX_RELOC_PAGES 1024
+
+#define PSB_LOW_REG_OFFS 0x0204
+#define PSB_HIGH_REG_OFFS 0x0600
+
+#define PSB_NUM_VBLANKS 2
+
+
+#define PSB_2D_SIZE (256*1024*1024)
+#define PSB_MAX_RELOC_PAGES 1024
+
+#define PSB_LOW_REG_OFFS 0x0204
+#define PSB_HIGH_REG_OFFS 0x0600
+
+#define PSB_NUM_VBLANKS 2
+#define PSB_WATCHDOG_DELAY (DRM_HZ * 2)
+#define PSB_LID_DELAY (DRM_HZ / 10)
+
+#define MDFLD_PNW_B0 0x04
+#define MDFLD_PNW_C0 0x08
+
+#define MDFLD_DSR_2D_3D_0 	(1 << 0)
+#define MDFLD_DSR_2D_3D_2 	(1 << 1)
+#define MDFLD_DSR_CURSOR_0 	(1 << 2)
+#define MDFLD_DSR_CURSOR_2	(1 << 3)
+#define MDFLD_DSR_OVERLAY_0 	(1 << 4)
+#define MDFLD_DSR_OVERLAY_2 	(1 << 5)
+#define MDFLD_DSR_MIPI_CONTROL	(1 << 6)
+#define MDFLD_DSR_DAMAGE_MASK_0	((1 << 0) | (1 << 2) | (1 << 4))
+#define MDFLD_DSR_DAMAGE_MASK_2	((1 << 1) | (1 << 3) | (1 << 5))
+#define MDFLD_DSR_2D_3D 	(MDFLD_DSR_2D_3D_0 | MDFLD_DSR_2D_3D_2)
+
+#define MDFLD_DSR_RR		45
+#define MDFLD_DPU_ENABLE 	(1 << 31)
+#define MDFLD_DSR_FULLSCREEN 	(1 << 30)
+#define MDFLD_DSR_DELAY		(DRM_HZ / MDFLD_DSR_RR)
+
+#define PSB_PWR_STATE_ON		1
+#define PSB_PWR_STATE_OFF		2
+
+#define PSB_PMPOLICY_NOPM		0
+#define PSB_PMPOLICY_CLOCKGATING	1
+#define PSB_PMPOLICY_POWERDOWN		2
+
+#define PSB_PMSTATE_POWERUP		0
+#define PSB_PMSTATE_CLOCKGATED		1
+#define PSB_PMSTATE_POWERDOWN		2
+#define PSB_PCIx_MSI_ADDR_LOC		0x94
+#define PSB_PCIx_MSI_DATA_LOC		0x98
+
+/* Medfield crystal settings */
+#define KSEL_CRYSTAL_19 1
+#define KSEL_BYPASS_19 5
+#define KSEL_BYPASS_25 6
+#define KSEL_BYPASS_83_100 7
+
+struct opregion_header;
+struct opregion_acpi;
+struct opregion_swsci;
+struct opregion_asle;
+
+struct psb_intel_opregion {
+	struct opregion_header *header;
+	struct opregion_acpi *acpi;
+	struct opregion_swsci *swsci;
+	struct opregion_asle *asle;
+	int enabled;
+};
+
+struct sdvo_device_mapping {
+	u8 initialized;
+	u8 dvo_port;
+	u8 slave_addr;
+	u8 dvo_wiring;
+	u8 i2c_pin;
+	u8 i2c_speed;
+	u8 ddc_pin;
+};
+
+struct intel_gmbus {
+	struct i2c_adapter adapter;
+	struct i2c_adapter *force_bit;
+	u32 reg0;
+};
+
+struct psb_ops;
+
+#define PSB_NUM_PIPE		3
+
+struct drm_psb_private {
+	struct drm_device *dev;
+	const struct psb_ops *ops;
+
+	struct psb_gtt gtt;
+
+	/* GTT Memory manager */
+	struct psb_gtt_mm *gtt_mm;
+	struct page *scratch_page;
+	u32 *gtt_map;
+	uint32_t stolen_base;
+	void *vram_addr;
+	unsigned long vram_stolen_size;
+	int gtt_initialized;
+	u16 gmch_ctrl;		/* Saved GTT setup */
+	u32 pge_ctl;
+
+	struct mutex gtt_mutex;
+	struct resource *gtt_mem;	/* Our PCI resource */
+
+	struct psb_mmu_driver *mmu;
+	struct psb_mmu_pd *pf_pd;
+
+	/*
+	 * Register base
+	 */
+
+	uint8_t *sgx_reg;
+	uint8_t *vdc_reg;
+	uint32_t gatt_free_offset;
+
+	/*
+	 * Fencing / irq.
+	 */
+
+	uint32_t vdc_irq_mask;
+	uint32_t pipestat[PSB_NUM_PIPE];
+
+	spinlock_t irqmask_lock;
+
+	/*
+	 * Power
+	 */
+
+	bool suspended;
+	bool display_power;
+	int display_count;
+
+	/*
+	 * Modesetting
+	 */
+	struct psb_intel_mode_device mode_dev;
+
+	struct drm_crtc *plane_to_crtc_mapping[PSB_NUM_PIPE];
+	struct drm_crtc *pipe_to_crtc_mapping[PSB_NUM_PIPE];
+	uint32_t num_pipe;
+
+	/*
+	 * OSPM info (Power management base) (can go ?)
+	 */
+	uint32_t ospm_base;
+
+	/*
+	 * Sizes info
+	 */
+
+	u32 fuse_reg_value;
+	u32 video_device_fuse;
+
+	/* PCI revision ID for B0:D2:F0 */
+	uint8_t platform_rev_id;
+
+	/* gmbus */
+	struct intel_gmbus *gmbus;
+
+	/* Used by SDVO */
+	int crt_ddc_pin;
+	/* FIXME: The mappings should be parsed from bios but for now we can
+		  pretend there are no mappings available */
+	struct sdvo_device_mapping sdvo_mappings[2];
+	u32 hotplug_supported_mask;
+	struct drm_property *broadcast_rgb_property;
+	struct drm_property *force_audio_property;
+
+	/*
+	 * LVDS info
+	 */
+	int backlight_duty_cycle;	/* restore backlight to this value */
+	bool panel_wants_dither;
+	struct drm_display_mode *panel_fixed_mode;
+	struct drm_display_mode *lfp_lvds_vbt_mode;
+	struct drm_display_mode *sdvo_lvds_vbt_mode;
+
+	struct bdb_lvds_backlight *lvds_bl; /* LVDS backlight info from VBT */
+	struct psb_intel_i2c_chan *lvds_i2c_bus; /* FIXME: Remove this? */
+
+	/* Feature bits from the VBIOS */
+	unsigned int int_tv_support:1;
+	unsigned int lvds_dither:1;
+	unsigned int lvds_vbt:1;
+	unsigned int int_crt_support:1;
+	unsigned int lvds_use_ssc:1;
+	int lvds_ssc_freq;
+	bool is_lvds_on;
+	bool is_mipi_on;
+	u32 mipi_ctrl_display;
+
+	unsigned int core_freq;
+	uint32_t iLVDS_enable;
+
+	/* Runtime PM state */
+	int rpm_enabled;
+
+	/* MID specific */
+	struct oaktrail_vbt vbt_data;
+	struct oaktrail_gct_data gct_data;
+
+	/* MIPI Panel type etc */
+	int panel_id;
+	bool dual_mipi;		/* dual display - DPI & DBI */
+	bool dpi_panel_on;	/* The DPI panel power is on */
+	bool dpi_panel_on2;	/* The DPI panel power is on */
+	bool dbi_panel_on;	/* The DBI panel power is on */
+	bool dbi_panel_on2;	/* The DBI panel power is on */
+	u32 dsr_fb_update;	/* DSR FB update counter */
+
+	/* Moorestown HDMI state */
+	struct oaktrail_hdmi_dev *hdmi_priv;
+
+	/* Moorestown pipe config register value cache */
+	uint32_t pipeconf;
+	uint32_t pipeconf1;
+	uint32_t pipeconf2;
+
+	/* Moorestown plane control register value cache */
+	uint32_t dspcntr;
+	uint32_t dspcntr1;
+	uint32_t dspcntr2;
+
+	/* Moorestown MM backlight cache */
+	uint8_t saveBKLTCNT;
+	uint8_t saveBKLTREQ;
+	uint8_t saveBKLTBRTL;
+
+	/*
+	 * Register state
+	 */
+	uint32_t saveDSPACNTR;
+	uint32_t saveDSPBCNTR;
+	uint32_t savePIPEACONF;
+	uint32_t savePIPEBCONF;
+	uint32_t savePIPEASRC;
+	uint32_t savePIPEBSRC;
+	uint32_t saveFPA0;
+	uint32_t saveFPA1;
+	uint32_t saveDPLL_A;
+	uint32_t saveDPLL_A_MD;
+	uint32_t saveHTOTAL_A;
+	uint32_t saveHBLANK_A;
+	uint32_t saveHSYNC_A;
+	uint32_t saveVTOTAL_A;
+	uint32_t saveVBLANK_A;
+	uint32_t saveVSYNC_A;
+	uint32_t saveDSPASTRIDE;
+	uint32_t saveDSPASIZE;
+	uint32_t saveDSPAPOS;
+	uint32_t saveDSPABASE;
+	uint32_t saveDSPASURF;
+	uint32_t saveDSPASTATUS;
+	uint32_t saveFPB0;
+	uint32_t saveFPB1;
+	uint32_t saveDPLL_B;
+	uint32_t saveDPLL_B_MD;
+	uint32_t saveHTOTAL_B;
+	uint32_t saveHBLANK_B;
+	uint32_t saveHSYNC_B;
+	uint32_t saveVTOTAL_B;
+	uint32_t saveVBLANK_B;
+	uint32_t saveVSYNC_B;
+	uint32_t saveDSPBSTRIDE;
+	uint32_t saveDSPBSIZE;
+	uint32_t saveDSPBPOS;
+	uint32_t saveDSPBBASE;
+	uint32_t saveDSPBSURF;
+	uint32_t saveDSPBSTATUS;
+	uint32_t saveVCLK_DIVISOR_VGA0;
+	uint32_t saveVCLK_DIVISOR_VGA1;
+	uint32_t saveVCLK_POST_DIV;
+	uint32_t saveVGACNTRL;
+	uint32_t saveADPA;
+	uint32_t saveLVDS;
+	uint32_t saveDVOA;
+	uint32_t saveDVOB;
+	uint32_t saveDVOC;
+	uint32_t savePP_ON;
+	uint32_t savePP_OFF;
+	uint32_t savePP_CONTROL;
+	uint32_t savePP_CYCLE;
+	uint32_t savePFIT_CONTROL;
+	uint32_t savePaletteA[256];
+	uint32_t savePaletteB[256];
+	uint32_t saveBLC_PWM_CTL2;
+	uint32_t saveBLC_PWM_CTL;
+	uint32_t saveCLOCKGATING;
+	uint32_t saveDSPARB;
+	uint32_t saveDSPATILEOFF;
+	uint32_t saveDSPBTILEOFF;
+	uint32_t saveDSPAADDR;
+	uint32_t saveDSPBADDR;
+	uint32_t savePFIT_AUTO_RATIOS;
+	uint32_t savePFIT_PGM_RATIOS;
+	uint32_t savePP_ON_DELAYS;
+	uint32_t savePP_OFF_DELAYS;
+	uint32_t savePP_DIVISOR;
+	uint32_t saveBSM;
+	uint32_t saveVBT;
+	uint32_t saveBCLRPAT_A;
+	uint32_t saveBCLRPAT_B;
+	uint32_t saveDSPALINOFF;
+	uint32_t saveDSPBLINOFF;
+	uint32_t savePERF_MODE;
+	uint32_t saveDSPFW1;
+	uint32_t saveDSPFW2;
+	uint32_t saveDSPFW3;
+	uint32_t saveDSPFW4;
+	uint32_t saveDSPFW5;
+	uint32_t saveDSPFW6;
+	uint32_t saveCHICKENBIT;
+	uint32_t saveDSPACURSOR_CTRL;
+	uint32_t saveDSPBCURSOR_CTRL;
+	uint32_t saveDSPACURSOR_BASE;
+	uint32_t saveDSPBCURSOR_BASE;
+	uint32_t saveDSPACURSOR_POS;
+	uint32_t saveDSPBCURSOR_POS;
+	uint32_t save_palette_a[256];
+	uint32_t save_palette_b[256];
+	uint32_t saveOV_OVADD;
+	uint32_t saveOV_OGAMC0;
+	uint32_t saveOV_OGAMC1;
+	uint32_t saveOV_OGAMC2;
+	uint32_t saveOV_OGAMC3;
+	uint32_t saveOV_OGAMC4;
+	uint32_t saveOV_OGAMC5;
+	uint32_t saveOVC_OVADD;
+	uint32_t saveOVC_OGAMC0;
+	uint32_t saveOVC_OGAMC1;
+	uint32_t saveOVC_OGAMC2;
+	uint32_t saveOVC_OGAMC3;
+	uint32_t saveOVC_OGAMC4;
+	uint32_t saveOVC_OGAMC5;
+
+	/* MSI reg save */
+	uint32_t msi_addr;
+	uint32_t msi_data;
+
+	/* Medfield specific register save state */
+	uint32_t saveHDMIPHYMISCCTL;
+	uint32_t saveHDMIB_CONTROL;
+	uint32_t saveDSPCCNTR;
+	uint32_t savePIPECCONF;
+	uint32_t savePIPECSRC;
+	uint32_t saveHTOTAL_C;
+	uint32_t saveHBLANK_C;
+	uint32_t saveHSYNC_C;
+	uint32_t saveVTOTAL_C;
+	uint32_t saveVBLANK_C;
+	uint32_t saveVSYNC_C;
+	uint32_t saveDSPCSTRIDE;
+	uint32_t saveDSPCSIZE;
+	uint32_t saveDSPCPOS;
+	uint32_t saveDSPCSURF;
+	uint32_t saveDSPCSTATUS;
+	uint32_t saveDSPCLINOFF;
+	uint32_t saveDSPCTILEOFF;
+	uint32_t saveDSPCCURSOR_CTRL;
+	uint32_t saveDSPCCURSOR_BASE;
+	uint32_t saveDSPCCURSOR_POS;
+	uint32_t save_palette_c[256];
+	uint32_t saveOV_OVADD_C;
+	uint32_t saveOV_OGAMC0_C;
+	uint32_t saveOV_OGAMC1_C;
+	uint32_t saveOV_OGAMC2_C;
+	uint32_t saveOV_OGAMC3_C;
+	uint32_t saveOV_OGAMC4_C;
+	uint32_t saveOV_OGAMC5_C;
+
+	/* DSI register save */
+	uint32_t saveDEVICE_READY_REG;
+	uint32_t saveINTR_EN_REG;
+	uint32_t saveDSI_FUNC_PRG_REG;
+	uint32_t saveHS_TX_TIMEOUT_REG;
+	uint32_t saveLP_RX_TIMEOUT_REG;
+	uint32_t saveTURN_AROUND_TIMEOUT_REG;
+	uint32_t saveDEVICE_RESET_REG;
+	uint32_t saveDPI_RESOLUTION_REG;
+	uint32_t saveHORIZ_SYNC_PAD_COUNT_REG;
+	uint32_t saveHORIZ_BACK_PORCH_COUNT_REG;
+	uint32_t saveHORIZ_FRONT_PORCH_COUNT_REG;
+	uint32_t saveHORIZ_ACTIVE_AREA_COUNT_REG;
+	uint32_t saveVERT_SYNC_PAD_COUNT_REG;
+	uint32_t saveVERT_BACK_PORCH_COUNT_REG;
+	uint32_t saveVERT_FRONT_PORCH_COUNT_REG;
+	uint32_t saveHIGH_LOW_SWITCH_COUNT_REG;
+	uint32_t saveINIT_COUNT_REG;
+	uint32_t saveMAX_RET_PAK_REG;
+	uint32_t saveVIDEO_FMT_REG;
+	uint32_t saveEOT_DISABLE_REG;
+	uint32_t saveLP_BYTECLK_REG;
+	uint32_t saveHS_LS_DBI_ENABLE_REG;
+	uint32_t saveTXCLKESC_REG;
+	uint32_t saveDPHY_PARAM_REG;
+	uint32_t saveMIPI_CONTROL_REG;
+	uint32_t saveMIPI;
+	uint32_t saveMIPI_C;
+
+	/* DPST register save */
+	uint32_t saveHISTOGRAM_INT_CONTROL_REG;
+	uint32_t saveHISTOGRAM_LOGIC_CONTROL_REG;
+	uint32_t savePWM_CONTROL_LOGIC;
+
+	/*
+	 * DSI info. 
+	 */
+	void * dbi_dsr_info;	
+	void * dbi_dpu_info;
+	void * dsi_configs[2];
+	/*
+	 * LID-Switch
+	 */
+	spinlock_t lid_lock;
+	struct timer_list lid_timer;
+	struct psb_intel_opregion opregion;
+	u32 *lid_state;
+	u32 lid_last_state;
+
+	/*
+	 * Watchdog
+	 */
+
+	uint32_t apm_reg;
+	uint16_t apm_base;
+
+	/*
+	 * Used for modifying backlight from
+	 * xrandr -- consider removing and using HAL instead
+	 */
+	struct backlight_device *backlight_device;
+	struct drm_property *backlight_property;
+	uint32_t blc_adj1;
+	uint32_t blc_adj2;
+
+	void *fbdev;
+
+	/* 2D acceleration */
+	spinlock_t lock_2d;
+};
+
+
+/*
+ *	Operations for each board type
+ */
+ 
+struct psb_ops {
+	const char *name;
+	unsigned int accel_2d:1;
+	int pipes;		/* Number of output pipes */
+	int crtcs;		/* Number of CRTCs */
+	int sgx_offset;		/* Base offset of SGX device */
+
+	/* Sub functions */
+	struct drm_crtc_helper_funcs const *crtc_helper;
+	struct drm_crtc_funcs const *crtc_funcs;
+
+	/* Setup hooks */
+	int (*chip_setup)(struct drm_device *dev);
+	void (*chip_teardown)(struct drm_device *dev);
+
+	/* Display management hooks */
+	int (*output_init)(struct drm_device *dev);
+	/* Power management hooks */
+	void (*init_pm)(struct drm_device *dev);
+	int (*save_regs)(struct drm_device *dev);
+	int (*restore_regs)(struct drm_device *dev);
+	int (*power_up)(struct drm_device *dev);
+	int (*power_down)(struct drm_device *dev);
+
+	void (*lvds_bl_power)(struct drm_device *dev, bool on);
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+	/* Backlight */
+	int (*backlight_init)(struct drm_device *dev);
+#endif
+	int i2c_bus;		/* I2C bus identifier for Moorestown */
+};
+
+
+
+struct psb_mmu_driver;
+
+extern int drm_crtc_probe_output_modes(struct drm_device *dev, int, int);
+extern int drm_pick_crtcs(struct drm_device *dev);
+
+static inline struct drm_psb_private *psb_priv(struct drm_device *dev)
+{
+	return (struct drm_psb_private *) dev->dev_private;
+}
+
+/*
+ * MMU stuff.
+ */
+
+extern struct psb_mmu_driver *psb_mmu_driver_init(uint8_t __iomem * registers,
+					int trap_pagefaults,
+					int invalid_type,
+					struct drm_psb_private *dev_priv);
+extern void psb_mmu_driver_takedown(struct psb_mmu_driver *driver);
+extern struct psb_mmu_pd *psb_mmu_get_default_pd(struct psb_mmu_driver
+						 *driver);
+extern void psb_mmu_mirror_gtt(struct psb_mmu_pd *pd, uint32_t mmu_offset,
+			       uint32_t gtt_start, uint32_t gtt_pages);
+extern struct psb_mmu_pd *psb_mmu_alloc_pd(struct psb_mmu_driver *driver,
+					   int trap_pagefaults,
+					   int invalid_type);
+extern void psb_mmu_free_pagedir(struct psb_mmu_pd *pd);
+extern void psb_mmu_flush(struct psb_mmu_driver *driver, int rc_prot);
+extern void psb_mmu_remove_pfn_sequence(struct psb_mmu_pd *pd,
+					unsigned long address,
+					uint32_t num_pages);
+extern int psb_mmu_insert_pfn_sequence(struct psb_mmu_pd *pd,
+				       uint32_t start_pfn,
+				       unsigned long address,
+				       uint32_t num_pages, int type);
+extern int psb_mmu_virtual_to_pfn(struct psb_mmu_pd *pd, uint32_t virtual,
+				  unsigned long *pfn);
+
+/*
+ * Enable / disable MMU for different requestors.
+ */
+
+
+extern void psb_mmu_set_pd_context(struct psb_mmu_pd *pd, int hw_context);
+extern int psb_mmu_insert_pages(struct psb_mmu_pd *pd, struct page **pages,
+				unsigned long address, uint32_t num_pages,
+				uint32_t desired_tile_stride,
+				uint32_t hw_tile_stride, int type);
+extern void psb_mmu_remove_pages(struct psb_mmu_pd *pd,
+				 unsigned long address, uint32_t num_pages,
+				 uint32_t desired_tile_stride,
+				 uint32_t hw_tile_stride);
+/*
+ *psb_irq.c
+ */
+
+extern irqreturn_t psb_irq_handler(DRM_IRQ_ARGS);
+extern int psb_irq_enable_dpst(struct drm_device *dev);
+extern int psb_irq_disable_dpst(struct drm_device *dev);
+extern void psb_irq_preinstall(struct drm_device *dev);
+extern int psb_irq_postinstall(struct drm_device *dev);
+extern void psb_irq_uninstall(struct drm_device *dev);
+extern void psb_irq_turn_on_dpst(struct drm_device *dev);
+extern void psb_irq_turn_off_dpst(struct drm_device *dev);
+
+extern void psb_irq_uninstall_islands(struct drm_device *dev, int hw_islands);
+extern int psb_vblank_wait2(struct drm_device *dev, unsigned int *sequence);
+extern int psb_vblank_wait(struct drm_device *dev, unsigned int *sequence);
+extern int psb_enable_vblank(struct drm_device *dev, int crtc);
+extern void psb_disable_vblank(struct drm_device *dev, int crtc);
+void
+psb_enable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask);
+
+void
+psb_disable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask);
+
+extern u32 psb_get_vblank_counter(struct drm_device *dev, int crtc);
+
+/*
+ * intel_opregion.c
+ */
+extern int gma_intel_opregion_init(struct drm_device *dev);
+extern int gma_intel_opregion_exit(struct drm_device *dev);
+
+/*
+ * framebuffer.c
+ */
+extern int psbfb_probed(struct drm_device *dev);
+extern int psbfb_remove(struct drm_device *dev,
+			struct drm_framebuffer *fb);
+/*
+ * accel_2d.c
+ */
+extern void psbfb_copyarea(struct fb_info *info,
+					const struct fb_copyarea *region);
+extern int psbfb_sync(struct fb_info *info);
+extern void psb_spank(struct drm_psb_private *dev_priv);
+
+/*
+ * psb_reset.c
+ */
+
+extern void psb_lid_timer_init(struct drm_psb_private *dev_priv);
+extern void psb_lid_timer_takedown(struct drm_psb_private *dev_priv);
+extern void psb_print_pagefault(struct drm_psb_private *dev_priv);
+
+/* modesetting */
+extern void psb_modeset_init(struct drm_device *dev);
+extern void psb_modeset_cleanup(struct drm_device *dev);
+extern int psb_fbdev_init(struct drm_device *dev);
+
+/* backlight.c */
+int gma_backlight_init(struct drm_device *dev);
+void gma_backlight_exit(struct drm_device *dev);
+
+/* oaktrail_crtc.c */
+extern const struct drm_crtc_helper_funcs oaktrail_helper_funcs;
+
+/* oaktrail_lvds.c */
+extern void oaktrail_lvds_init(struct drm_device *dev,
+		    struct psb_intel_mode_device *mode_dev);
+
+/* psb_intel_display.c */
+extern const struct drm_crtc_helper_funcs psb_intel_helper_funcs;
+extern const struct drm_crtc_funcs psb_intel_crtc_funcs;
+
+/* psb_intel_lvds.c */
+extern const struct drm_connector_helper_funcs
+					psb_intel_lvds_connector_helper_funcs;
+extern const struct drm_connector_funcs psb_intel_lvds_connector_funcs;
+
+/* gem.c */
+extern int psb_gem_init_object(struct drm_gem_object *obj);
+extern void psb_gem_free_object(struct drm_gem_object *obj);
+extern int psb_gem_get_aperture(struct drm_device *dev, void *data,
+			struct drm_file *file);
+extern int psb_gem_dumb_create(struct drm_file *file, struct drm_device *dev,
+			struct drm_mode_create_dumb *args);
+extern int psb_gem_dumb_destroy(struct drm_file *file, struct drm_device *dev,
+			uint32_t handle);
+extern int psb_gem_dumb_map_gtt(struct drm_file *file, struct drm_device *dev,
+			uint32_t handle, uint64_t *offset);
+extern int psb_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
+extern int psb_gem_create_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file);
+extern int psb_gem_mmap_ioctl(struct drm_device *dev, void *data,
+					struct drm_file *file);
+
+/* psb_device.c */
+extern const struct psb_ops psb_chip_ops;
+
+/* oaktrail_device.c */
+extern const struct psb_ops oaktrail_chip_ops;
+
+/* cdv_device.c */
+extern const struct psb_ops cdv_chip_ops;
+
+/*
+ * Debug print bits setting
+ */
+#define PSB_D_GENERAL (1 << 0)
+#define PSB_D_INIT    (1 << 1)
+#define PSB_D_IRQ     (1 << 2)
+#define PSB_D_ENTRY   (1 << 3)
+/* debug the get H/V BP/FP count */
+#define PSB_D_HV      (1 << 4)
+#define PSB_D_DBI_BF  (1 << 5)
+#define PSB_D_PM      (1 << 6)
+#define PSB_D_RENDER  (1 << 7)
+#define PSB_D_REG     (1 << 8)
+#define PSB_D_MSVDX   (1 << 9)
+#define PSB_D_TOPAZ   (1 << 10)
+
+extern int drm_psb_no_fb;
+extern int drm_idle_check_interval;
+
+/*
+ *	Utilities
+ */
+
+static inline u32 MRST_MSG_READ32(uint port, uint offset)
+{
+	int mcr = (0xD0<<24) | (port << 16) | (offset << 8);
+	uint32_t ret_val = 0;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	pci_write_config_dword(pci_root, 0xD0, mcr);
+	pci_read_config_dword(pci_root, 0xD4, &ret_val);
+	pci_dev_put(pci_root);
+	return ret_val;
+}
+static inline void MRST_MSG_WRITE32(uint port, uint offset, u32 value)
+{
+	int mcr = (0xE0<<24) | (port << 16) | (offset << 8) | 0xF0;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	pci_write_config_dword(pci_root, 0xD4, value);
+	pci_write_config_dword(pci_root, 0xD0, mcr);
+	pci_dev_put(pci_root);
+}
+static inline u32 MDFLD_MSG_READ32(uint port, uint offset)
+{
+	int mcr = (0x10<<24) | (port << 16) | (offset << 8);
+	uint32_t ret_val = 0;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	pci_write_config_dword(pci_root, 0xD0, mcr);
+	pci_read_config_dword(pci_root, 0xD4, &ret_val);
+	pci_dev_put(pci_root);
+	return ret_val;
+}
+static inline void MDFLD_MSG_WRITE32(uint port, uint offset, u32 value)
+{
+	int mcr = (0x11<<24) | (port << 16) | (offset << 8) | 0xF0;
+	struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+	pci_write_config_dword(pci_root, 0xD4, value);
+	pci_write_config_dword(pci_root, 0xD0, mcr);
+	pci_dev_put(pci_root);
+}
+
+static inline uint32_t REGISTER_READ(struct drm_device *dev, uint32_t reg)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	return ioread32(dev_priv->vdc_reg + reg);
+}
+
+#define REG_READ(reg)	       REGISTER_READ(dev, (reg))
+
+static inline void REGISTER_WRITE(struct drm_device *dev, uint32_t reg,
+				      uint32_t val)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	iowrite32((val), dev_priv->vdc_reg + (reg));
+}
+
+#define REG_WRITE(reg, val)	REGISTER_WRITE(dev, (reg), (val))
+
+static inline void REGISTER_WRITE16(struct drm_device *dev,
+					uint32_t reg, uint32_t val)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	iowrite16((val), dev_priv->vdc_reg + (reg));
+}
+
+#define REG_WRITE16(reg, val)	  REGISTER_WRITE16(dev, (reg), (val))
+
+static inline void REGISTER_WRITE8(struct drm_device *dev,
+				       uint32_t reg, uint32_t val)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	iowrite8((val), dev_priv->vdc_reg + (reg));
+}
+
+#define REG_WRITE8(reg, val)		REGISTER_WRITE8(dev, (reg), (val))
+
+#define PSB_WVDC32(_val, _offs)		iowrite32(_val, dev_priv->vdc_reg + (_offs))
+#define PSB_RVDC32(_offs)		ioread32(dev_priv->vdc_reg + (_offs))
+
+/* #define TRAP_SGX_PM_FAULT 1 */
+#ifdef TRAP_SGX_PM_FAULT
+#define PSB_RSGX32(_offs)						\
+({									\
+	if (inl(dev_priv->apm_base + PSB_APM_STS) & 0x3) {		\
+		printk(KERN_ERR						\
+			"access sgx when it's off!! (READ) %s, %d\n",	\
+	       __FILE__, __LINE__);					\
+		melay(1000);						\
+	}								\
+	ioread32(dev_priv->sgx_reg + (_offs));				\
+})
+#else
+#define PSB_RSGX32(_offs)		ioread32(dev_priv->sgx_reg + (_offs))
+#endif
+#define PSB_WSGX32(_val, _offs)		iowrite32(_val, dev_priv->sgx_reg + (_offs))
+
+#define MSVDX_REG_DUMP 0
+
+#define PSB_WMSVDX32(_val, _offs)	iowrite32(_val, dev_priv->msvdx_reg + (_offs))
+#define PSB_RMSVDX32(_offs)		ioread32(dev_priv->msvdx_reg + (_offs))
+
+#endif
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.c b/drivers/gpu/drm/gma500/psb_intel_display.c
new file mode 100644
index 000000000000..49e983508d5c
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_intel_display.c
@@ -0,0 +1,1446 @@
+/*
+ * Copyright © 2006-2011 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ */
+
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+
+#include <drm/drmP.h>
+#include "framebuffer.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_display.h"
+#include "power.h"
+
+struct psb_intel_clock_t {
+	/* given values */
+	int n;
+	int m1, m2;
+	int p1, p2;
+	/* derived values */
+	int dot;
+	int vco;
+	int m;
+	int p;
+};
+
+struct psb_intel_range_t {
+	int min, max;
+};
+
+struct psb_intel_p2_t {
+	int dot_limit;
+	int p2_slow, p2_fast;
+};
+
+#define INTEL_P2_NUM		      2
+
+struct psb_intel_limit_t {
+	struct psb_intel_range_t dot, vco, n, m, m1, m2, p, p1;
+	struct psb_intel_p2_t p2;
+};
+
+#define I8XX_DOT_MIN		  25000
+#define I8XX_DOT_MAX		 350000
+#define I8XX_VCO_MIN		 930000
+#define I8XX_VCO_MAX		1400000
+#define I8XX_N_MIN		      3
+#define I8XX_N_MAX		     16
+#define I8XX_M_MIN		     96
+#define I8XX_M_MAX		    140
+#define I8XX_M1_MIN		     18
+#define I8XX_M1_MAX		     26
+#define I8XX_M2_MIN		      6
+#define I8XX_M2_MAX		     16
+#define I8XX_P_MIN		      4
+#define I8XX_P_MAX		    128
+#define I8XX_P1_MIN		      2
+#define I8XX_P1_MAX		     33
+#define I8XX_P1_LVDS_MIN	      1
+#define I8XX_P1_LVDS_MAX	      6
+#define I8XX_P2_SLOW		      4
+#define I8XX_P2_FAST		      2
+#define I8XX_P2_LVDS_SLOW	      14
+#define I8XX_P2_LVDS_FAST	      14	/* No fast option */
+#define I8XX_P2_SLOW_LIMIT	 165000
+
+#define I9XX_DOT_MIN		  20000
+#define I9XX_DOT_MAX		 400000
+#define I9XX_VCO_MIN		1400000
+#define I9XX_VCO_MAX		2800000
+#define I9XX_N_MIN		      3
+#define I9XX_N_MAX		      8
+#define I9XX_M_MIN		     70
+#define I9XX_M_MAX		    120
+#define I9XX_M1_MIN		     10
+#define I9XX_M1_MAX		     20
+#define I9XX_M2_MIN		      5
+#define I9XX_M2_MAX		      9
+#define I9XX_P_SDVO_DAC_MIN	      5
+#define I9XX_P_SDVO_DAC_MAX	     80
+#define I9XX_P_LVDS_MIN		      7
+#define I9XX_P_LVDS_MAX		     98
+#define I9XX_P1_MIN		      1
+#define I9XX_P1_MAX		      8
+#define I9XX_P2_SDVO_DAC_SLOW		     10
+#define I9XX_P2_SDVO_DAC_FAST		      5
+#define I9XX_P2_SDVO_DAC_SLOW_LIMIT	 200000
+#define I9XX_P2_LVDS_SLOW		     14
+#define I9XX_P2_LVDS_FAST		      7
+#define I9XX_P2_LVDS_SLOW_LIMIT		 112000
+
+#define INTEL_LIMIT_I8XX_DVO_DAC    0
+#define INTEL_LIMIT_I8XX_LVDS	    1
+#define INTEL_LIMIT_I9XX_SDVO_DAC   2
+#define INTEL_LIMIT_I9XX_LVDS	    3
+
+static const struct psb_intel_limit_t psb_intel_limits[] = {
+	{			/* INTEL_LIMIT_I8XX_DVO_DAC */
+	 .dot = {.min = I8XX_DOT_MIN, .max = I8XX_DOT_MAX},
+	 .vco = {.min = I8XX_VCO_MIN, .max = I8XX_VCO_MAX},
+	 .n = {.min = I8XX_N_MIN, .max = I8XX_N_MAX},
+	 .m = {.min = I8XX_M_MIN, .max = I8XX_M_MAX},
+	 .m1 = {.min = I8XX_M1_MIN, .max = I8XX_M1_MAX},
+	 .m2 = {.min = I8XX_M2_MIN, .max = I8XX_M2_MAX},
+	 .p = {.min = I8XX_P_MIN, .max = I8XX_P_MAX},
+	 .p1 = {.min = I8XX_P1_MIN, .max = I8XX_P1_MAX},
+	 .p2 = {.dot_limit = I8XX_P2_SLOW_LIMIT,
+		.p2_slow = I8XX_P2_SLOW, .p2_fast = I8XX_P2_FAST},
+	 },
+	{			/* INTEL_LIMIT_I8XX_LVDS */
+	 .dot = {.min = I8XX_DOT_MIN, .max = I8XX_DOT_MAX},
+	 .vco = {.min = I8XX_VCO_MIN, .max = I8XX_VCO_MAX},
+	 .n = {.min = I8XX_N_MIN, .max = I8XX_N_MAX},
+	 .m = {.min = I8XX_M_MIN, .max = I8XX_M_MAX},
+	 .m1 = {.min = I8XX_M1_MIN, .max = I8XX_M1_MAX},
+	 .m2 = {.min = I8XX_M2_MIN, .max = I8XX_M2_MAX},
+	 .p = {.min = I8XX_P_MIN, .max = I8XX_P_MAX},
+	 .p1 = {.min = I8XX_P1_LVDS_MIN, .max = I8XX_P1_LVDS_MAX},
+	 .p2 = {.dot_limit = I8XX_P2_SLOW_LIMIT,
+		.p2_slow = I8XX_P2_LVDS_SLOW, .p2_fast = I8XX_P2_LVDS_FAST},
+	 },
+	{			/* INTEL_LIMIT_I9XX_SDVO_DAC */
+	 .dot = {.min = I9XX_DOT_MIN, .max = I9XX_DOT_MAX},
+	 .vco = {.min = I9XX_VCO_MIN, .max = I9XX_VCO_MAX},
+	 .n = {.min = I9XX_N_MIN, .max = I9XX_N_MAX},
+	 .m = {.min = I9XX_M_MIN, .max = I9XX_M_MAX},
+	 .m1 = {.min = I9XX_M1_MIN, .max = I9XX_M1_MAX},
+	 .m2 = {.min = I9XX_M2_MIN, .max = I9XX_M2_MAX},
+	 .p = {.min = I9XX_P_SDVO_DAC_MIN, .max = I9XX_P_SDVO_DAC_MAX},
+	 .p1 = {.min = I9XX_P1_MIN, .max = I9XX_P1_MAX},
+	 .p2 = {.dot_limit = I9XX_P2_SDVO_DAC_SLOW_LIMIT,
+		.p2_slow = I9XX_P2_SDVO_DAC_SLOW, .p2_fast =
+		I9XX_P2_SDVO_DAC_FAST},
+	 },
+	{			/* INTEL_LIMIT_I9XX_LVDS */
+	 .dot = {.min = I9XX_DOT_MIN, .max = I9XX_DOT_MAX},
+	 .vco = {.min = I9XX_VCO_MIN, .max = I9XX_VCO_MAX},
+	 .n = {.min = I9XX_N_MIN, .max = I9XX_N_MAX},
+	 .m = {.min = I9XX_M_MIN, .max = I9XX_M_MAX},
+	 .m1 = {.min = I9XX_M1_MIN, .max = I9XX_M1_MAX},
+	 .m2 = {.min = I9XX_M2_MIN, .max = I9XX_M2_MAX},
+	 .p = {.min = I9XX_P_LVDS_MIN, .max = I9XX_P_LVDS_MAX},
+	 .p1 = {.min = I9XX_P1_MIN, .max = I9XX_P1_MAX},
+	 /* The single-channel range is 25-112Mhz, and dual-channel
+	  * is 80-224Mhz.  Prefer single channel as much as possible.
+	  */
+	 .p2 = {.dot_limit = I9XX_P2_LVDS_SLOW_LIMIT,
+		.p2_slow = I9XX_P2_LVDS_SLOW, .p2_fast = I9XX_P2_LVDS_FAST},
+	 },
+};
+
+static const struct psb_intel_limit_t *psb_intel_limit(struct drm_crtc *crtc)
+{
+	const struct psb_intel_limit_t *limit;
+
+	if (psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+		limit = &psb_intel_limits[INTEL_LIMIT_I9XX_LVDS];
+	else
+		limit = &psb_intel_limits[INTEL_LIMIT_I9XX_SDVO_DAC];
+	return limit;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 8xx chips. */
+
+static void i8xx_clock(int refclk, struct psb_intel_clock_t *clock)
+{
+	clock->m = 5 * (clock->m1 + 2) + (clock->m2 + 2);
+	clock->p = clock->p1 * clock->p2;
+	clock->vco = refclk * clock->m / (clock->n + 2);
+	clock->dot = clock->vco / clock->p;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 9xx chips. */
+
+static void i9xx_clock(int refclk, struct psb_intel_clock_t *clock)
+{
+	clock->m = 5 * (clock->m1 + 2) + (clock->m2 + 2);
+	clock->p = clock->p1 * clock->p2;
+	clock->vco = refclk * clock->m / (clock->n + 2);
+	clock->dot = clock->vco / clock->p;
+}
+
+static void psb_intel_clock(struct drm_device *dev, int refclk,
+			struct psb_intel_clock_t *clock)
+{
+	return i9xx_clock(refclk, clock);
+}
+
+/**
+ * Returns whether any output on the specified pipe is of the specified type
+ */
+bool psb_intel_pipe_has_type(struct drm_crtc *crtc, int type)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct drm_connector *l_entry;
+
+	list_for_each_entry(l_entry, &mode_config->connector_list, head) {
+		if (l_entry->encoder && l_entry->encoder->crtc == crtc) {
+			struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(l_entry);
+			if (psb_intel_encoder->type == type)
+				return true;
+		}
+	}
+	return false;
+}
+
+#define INTELPllInvalid(s)   { /* ErrorF (s) */; return false; }
+/**
+ * Returns whether the given set of divisors are valid for a given refclk with
+ * the given connectors.
+ */
+
+static bool psb_intel_PLL_is_valid(struct drm_crtc *crtc,
+			       struct psb_intel_clock_t *clock)
+{
+	const struct psb_intel_limit_t *limit = psb_intel_limit(crtc);
+
+	if (clock->p1 < limit->p1.min || limit->p1.max < clock->p1)
+		INTELPllInvalid("p1 out of range\n");
+	if (clock->p < limit->p.min || limit->p.max < clock->p)
+		INTELPllInvalid("p out of range\n");
+	if (clock->m2 < limit->m2.min || limit->m2.max < clock->m2)
+		INTELPllInvalid("m2 out of range\n");
+	if (clock->m1 < limit->m1.min || limit->m1.max < clock->m1)
+		INTELPllInvalid("m1 out of range\n");
+	if (clock->m1 <= clock->m2)
+		INTELPllInvalid("m1 <= m2\n");
+	if (clock->m < limit->m.min || limit->m.max < clock->m)
+		INTELPllInvalid("m out of range\n");
+	if (clock->n < limit->n.min || limit->n.max < clock->n)
+		INTELPllInvalid("n out of range\n");
+	if (clock->vco < limit->vco.min || limit->vco.max < clock->vco)
+		INTELPllInvalid("vco out of range\n");
+	/* XXX: We may need to be checking "Dot clock"
+	 * depending on the multiplier, connector, etc.,
+	 * rather than just a single range.
+	 */
+	if (clock->dot < limit->dot.min || limit->dot.max < clock->dot)
+		INTELPllInvalid("dot out of range\n");
+
+	return true;
+}
+
+/**
+ * Returns a set of divisors for the desired target clock with the given
+ * refclk, or FALSE.  The returned values represent the clock equation:
+ * reflck * (5 * (m1 + 2) + (m2 + 2)) / (n + 2) / p1 / p2.
+ */
+static bool psb_intel_find_best_PLL(struct drm_crtc *crtc, int target,
+				int refclk,
+				struct psb_intel_clock_t *best_clock)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_clock_t clock;
+	const struct psb_intel_limit_t *limit = psb_intel_limit(crtc);
+	int err = target;
+
+	if (psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+	    (REG_READ(LVDS) & LVDS_PORT_EN) != 0) {
+		/*
+		 * For LVDS, if the panel is on, just rely on its current
+		 * settings for dual-channel.  We haven't figured out how to
+		 * reliably set up different single/dual channel state, if we
+		 * even can.
+		 */
+		if ((REG_READ(LVDS) & LVDS_CLKB_POWER_MASK) ==
+		    LVDS_CLKB_POWER_UP)
+			clock.p2 = limit->p2.p2_fast;
+		else
+			clock.p2 = limit->p2.p2_slow;
+	} else {
+		if (target < limit->p2.dot_limit)
+			clock.p2 = limit->p2.p2_slow;
+		else
+			clock.p2 = limit->p2.p2_fast;
+	}
+
+	memset(best_clock, 0, sizeof(*best_clock));
+
+	for (clock.m1 = limit->m1.min; clock.m1 <= limit->m1.max;
+	     clock.m1++) {
+		for (clock.m2 = limit->m2.min;
+		     clock.m2 < clock.m1 && clock.m2 <= limit->m2.max;
+		     clock.m2++) {
+			for (clock.n = limit->n.min;
+			     clock.n <= limit->n.max; clock.n++) {
+				for (clock.p1 = limit->p1.min;
+				     clock.p1 <= limit->p1.max;
+				     clock.p1++) {
+					int this_err;
+
+					psb_intel_clock(dev, refclk, &clock);
+
+					if (!psb_intel_PLL_is_valid
+					    (crtc, &clock))
+						continue;
+
+					this_err = abs(clock.dot - target);
+					if (this_err < err) {
+						*best_clock = clock;
+						err = this_err;
+					}
+				}
+			}
+		}
+	}
+
+	return err != target;
+}
+
+void psb_intel_wait_for_vblank(struct drm_device *dev)
+{
+	/* Wait for 20ms, i.e. one cycle at 50hz. */
+	mdelay(20);
+}
+
+int psb_intel_pipe_set_base(struct drm_crtc *crtc,
+			    int x, int y, struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	/* struct drm_i915_master_private *master_priv; */
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
+	int pipe = psb_intel_crtc->pipe;
+	unsigned long start, offset;
+	int dspbase = (pipe == 0 ? DSPABASE : DSPBBASE);
+	int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
+	int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	u32 dspcntr;
+	int ret = 0;
+
+	if (!gma_power_begin(dev, true))
+		return 0;
+
+	/* no fb bound */
+	if (!crtc->fb) {
+		dev_dbg(dev->dev, "No FB bound\n");
+		goto psb_intel_pipe_cleaner;
+	}
+
+	/* We are displaying this buffer, make sure it is actually loaded
+	   into the GTT */
+	ret = psb_gtt_pin(psbfb->gtt);
+	if (ret < 0)
+		goto psb_intel_pipe_set_base_exit;
+	start = psbfb->gtt->offset;
+
+	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
+
+	REG_WRITE(dspstride, crtc->fb->pitches[0]);
+
+	dspcntr = REG_READ(dspcntr_reg);
+	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
+
+	switch (crtc->fb->bits_per_pixel) {
+	case 8:
+		dspcntr |= DISPPLANE_8BPP;
+		break;
+	case 16:
+		if (crtc->fb->depth == 15)
+			dspcntr |= DISPPLANE_15_16BPP;
+		else
+			dspcntr |= DISPPLANE_16BPP;
+		break;
+	case 24:
+	case 32:
+		dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
+		break;
+	default:
+		dev_err(dev->dev, "Unknown color depth\n");
+		ret = -EINVAL;
+		psb_gtt_unpin(psbfb->gtt);
+		goto psb_intel_pipe_set_base_exit;
+	}
+	REG_WRITE(dspcntr_reg, dspcntr);
+
+
+	if (0 /* FIXMEAC - check what PSB needs */) {
+		REG_WRITE(dspbase, offset);
+		REG_READ(dspbase);
+		REG_WRITE(dspsurf, start);
+		REG_READ(dspsurf);
+	} else {
+		REG_WRITE(dspbase, start + offset);
+		REG_READ(dspbase);
+	}
+
+psb_intel_pipe_cleaner:
+	/* If there was a previous display we can now unpin it */
+	if (old_fb)
+		psb_gtt_unpin(to_psb_fb(old_fb)->gtt);
+
+psb_intel_pipe_set_base_exit:
+	gma_power_end(dev);
+	return ret;
+}
+
+/**
+ * Sets the power management mode of the pipe and plane.
+ *
+ * This code should probably grow support for turning the cursor off and back
+ * on appropriately at the same time as we're turning the pipe off/on.
+ */
+static void psb_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct drm_device *dev = crtc->dev;
+	/* struct drm_i915_master_private *master_priv; */
+	/* struct drm_i915_private *dev_priv = dev->dev_private; */
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	int dspbase_reg = (pipe == 0) ? DSPABASE : DSPBBASE;
+	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	u32 temp;
+	bool enabled;
+
+	/* XXX: When our outputs are all unaware of DPMS modes other than off
+	 * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
+	 */
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+	case DRM_MODE_DPMS_STANDBY:
+	case DRM_MODE_DPMS_SUSPEND:
+		/* Enable the DPLL */
+		temp = REG_READ(dpll_reg);
+		if ((temp & DPLL_VCO_ENABLE) == 0) {
+			REG_WRITE(dpll_reg, temp);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+			REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+			/* Wait for the clocks to stabilize. */
+			udelay(150);
+		}
+
+		/* Enable the pipe */
+		temp = REG_READ(pipeconf_reg);
+		if ((temp & PIPEACONF_ENABLE) == 0)
+			REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+
+		/* Enable the plane */
+		temp = REG_READ(dspcntr_reg);
+		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+			REG_WRITE(dspcntr_reg,
+				  temp | DISPLAY_PLANE_ENABLE);
+			/* Flush the plane changes */
+			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+		}
+
+		psb_intel_crtc_load_lut(crtc);
+
+		/* Give the overlay scaler a chance to enable
+		 * if it's on this pipe */
+		/* psb_intel_crtc_dpms_video(crtc, true); TODO */
+		break;
+	case DRM_MODE_DPMS_OFF:
+		/* Give the overlay scaler a chance to disable
+		 * if it's on this pipe */
+		/* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
+
+		/* Disable the VGA plane that we never use */
+		REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+		/* Disable display plane */
+		temp = REG_READ(dspcntr_reg);
+		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+			REG_WRITE(dspcntr_reg,
+				  temp & ~DISPLAY_PLANE_ENABLE);
+			/* Flush the plane changes */
+			REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+			REG_READ(dspbase_reg);
+		}
+
+		/* Next, disable display pipes */
+		temp = REG_READ(pipeconf_reg);
+		if ((temp & PIPEACONF_ENABLE) != 0) {
+			REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
+			REG_READ(pipeconf_reg);
+		}
+
+		/* Wait for vblank for the disable to take effect. */
+		psb_intel_wait_for_vblank(dev);
+
+		temp = REG_READ(dpll_reg);
+		if ((temp & DPLL_VCO_ENABLE) != 0) {
+			REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
+			REG_READ(dpll_reg);
+		}
+
+		/* Wait for the clocks to turn off. */
+		udelay(150);
+		break;
+	}
+
+	enabled = crtc->enabled && mode != DRM_MODE_DPMS_OFF;
+
+	/*Set FIFO Watermarks*/
+	REG_WRITE(DSPARB, 0x3F3E);
+}
+
+static void psb_intel_crtc_prepare(struct drm_crtc *crtc)
+{
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
+static void psb_intel_crtc_commit(struct drm_crtc *crtc)
+{
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+void psb_intel_encoder_prepare(struct drm_encoder *encoder)
+{
+	struct drm_encoder_helper_funcs *encoder_funcs =
+	    encoder->helper_private;
+	/* lvds has its own version of prepare see psb_intel_lvds_prepare */
+	encoder_funcs->dpms(encoder, DRM_MODE_DPMS_OFF);
+}
+
+void psb_intel_encoder_commit(struct drm_encoder *encoder)
+{
+	struct drm_encoder_helper_funcs *encoder_funcs =
+	    encoder->helper_private;
+	/* lvds has its own version of commit see psb_intel_lvds_commit */
+	encoder_funcs->dpms(encoder, DRM_MODE_DPMS_ON);
+}
+
+void psb_intel_encoder_destroy(struct drm_encoder *encoder)
+{
+	struct psb_intel_encoder *intel_encoder = to_psb_intel_encoder(encoder);
+
+	drm_encoder_cleanup(encoder);
+	kfree(intel_encoder);
+}
+
+static bool psb_intel_crtc_mode_fixup(struct drm_crtc *crtc,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+
+/**
+ * Return the pipe currently connected to the panel fitter,
+ * or -1 if the panel fitter is not present or not in use
+ */
+static int psb_intel_panel_fitter_pipe(struct drm_device *dev)
+{
+	u32 pfit_control;
+
+	pfit_control = REG_READ(PFIT_CONTROL);
+
+	/* See if the panel fitter is in use */
+	if ((pfit_control & PFIT_ENABLE) == 0)
+		return -1;
+	/* Must be on PIPE 1 for PSB */
+	return 1;
+}
+
+static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode,
+			       int x, int y,
+			       struct drm_framebuffer *old_fb)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+	int pipe = psb_intel_crtc->pipe;
+	int fp_reg = (pipe == 0) ? FPA0 : FPB0;
+	int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+	int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+	int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+	int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
+	int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
+	int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
+	int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
+	int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
+	int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
+	int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
+	int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
+	int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+	int refclk;
+	struct psb_intel_clock_t clock;
+	u32 dpll = 0, fp = 0, dspcntr, pipeconf;
+	bool ok, is_sdvo = false, is_dvo = false;
+	bool is_crt = false, is_lvds = false, is_tv = false;
+	struct drm_mode_config *mode_config = &dev->mode_config;
+	struct drm_connector *connector;
+
+	/* No scan out no play */
+	if (crtc->fb == NULL) {
+		crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+		return 0;
+	}
+
+	list_for_each_entry(connector, &mode_config->connector_list, head) {
+		struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+
+		if (!connector->encoder
+		    || connector->encoder->crtc != crtc)
+			continue;
+
+		switch (psb_intel_encoder->type) {
+		case INTEL_OUTPUT_LVDS:
+			is_lvds = true;
+			break;
+		case INTEL_OUTPUT_SDVO:
+			is_sdvo = true;
+			break;
+		case INTEL_OUTPUT_DVO:
+			is_dvo = true;
+			break;
+		case INTEL_OUTPUT_TVOUT:
+			is_tv = true;
+			break;
+		case INTEL_OUTPUT_ANALOG:
+			is_crt = true;
+			break;
+		}
+	}
+
+	refclk = 96000;
+
+	ok = psb_intel_find_best_PLL(crtc, adjusted_mode->clock, refclk,
+				 &clock);
+	if (!ok) {
+		dev_err(dev->dev, "Couldn't find PLL settings for mode!\n");
+		return 0;
+	}
+
+	fp = clock.n << 16 | clock.m1 << 8 | clock.m2;
+
+	dpll = DPLL_VGA_MODE_DIS;
+	if (is_lvds) {
+		dpll |= DPLLB_MODE_LVDS;
+		dpll |= DPLL_DVO_HIGH_SPEED;
+	} else
+		dpll |= DPLLB_MODE_DAC_SERIAL;
+	if (is_sdvo) {
+		int sdvo_pixel_multiply =
+			    adjusted_mode->clock / mode->clock;
+		dpll |= DPLL_DVO_HIGH_SPEED;
+		dpll |=
+		    (sdvo_pixel_multiply - 1) << SDVO_MULTIPLIER_SHIFT_HIRES;
+	}
+
+	/* compute bitmask from p1 value */
+	dpll |= (1 << (clock.p1 - 1)) << 16;
+	switch (clock.p2) {
+	case 5:
+		dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_5;
+		break;
+	case 7:
+		dpll |= DPLLB_LVDS_P2_CLOCK_DIV_7;
+		break;
+	case 10:
+		dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_10;
+		break;
+	case 14:
+		dpll |= DPLLB_LVDS_P2_CLOCK_DIV_14;
+		break;
+	}
+
+	if (is_tv) {
+		/* XXX: just matching BIOS for now */
+/*	dpll |= PLL_REF_INPUT_TVCLKINBC; */
+		dpll |= 3;
+	}
+	dpll |= PLL_REF_INPUT_DREFCLK;
+
+	/* setup pipeconf */
+	pipeconf = REG_READ(pipeconf_reg);
+
+	/* Set up the display plane register */
+	dspcntr = DISPPLANE_GAMMA_ENABLE;
+
+	if (pipe == 0)
+		dspcntr |= DISPPLANE_SEL_PIPE_A;
+	else
+		dspcntr |= DISPPLANE_SEL_PIPE_B;
+
+	dspcntr |= DISPLAY_PLANE_ENABLE;
+	pipeconf |= PIPEACONF_ENABLE;
+	dpll |= DPLL_VCO_ENABLE;
+
+
+	/* Disable the panel fitter if it was on our pipe */
+	if (psb_intel_panel_fitter_pipe(dev) == pipe)
+		REG_WRITE(PFIT_CONTROL, 0);
+
+	drm_mode_debug_printmodeline(mode);
+
+	if (dpll & DPLL_VCO_ENABLE) {
+		REG_WRITE(fp_reg, fp);
+		REG_WRITE(dpll_reg, dpll & ~DPLL_VCO_ENABLE);
+		REG_READ(dpll_reg);
+		udelay(150);
+	}
+
+	/* The LVDS pin pair needs to be on before the DPLLs are enabled.
+	 * This is an exception to the general rule that mode_set doesn't turn
+	 * things on.
+	 */
+	if (is_lvds) {
+		u32 lvds = REG_READ(LVDS);
+
+		lvds &= ~LVDS_PIPEB_SELECT;
+		if (pipe == 1)
+			lvds |= LVDS_PIPEB_SELECT;
+
+		lvds |= LVDS_PORT_EN | LVDS_A0A2_CLKA_POWER_UP;
+		/* Set the B0-B3 data pairs corresponding to
+		 * whether we're going to
+		 * set the DPLLs for dual-channel mode or not.
+		 */
+		lvds &= ~(LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP);
+		if (clock.p2 == 7)
+			lvds |= LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP;
+
+		/* It would be nice to set 24 vs 18-bit mode (LVDS_A3_POWER_UP)
+		 * appropriately here, but we need to look more
+		 * thoroughly into how panels behave in the two modes.
+		 */
+
+		REG_WRITE(LVDS, lvds);
+		REG_READ(LVDS);
+	}
+
+	REG_WRITE(fp_reg, fp);
+	REG_WRITE(dpll_reg, dpll);
+	REG_READ(dpll_reg);
+	/* Wait for the clocks to stabilize. */
+	udelay(150);
+
+	/* write it again -- the BIOS does, after all */
+	REG_WRITE(dpll_reg, dpll);
+
+	REG_READ(dpll_reg);
+	/* Wait for the clocks to stabilize. */
+	udelay(150);
+
+	REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+		  ((adjusted_mode->crtc_htotal - 1) << 16));
+	REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+		  ((adjusted_mode->crtc_hblank_end - 1) << 16));
+	REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+		  ((adjusted_mode->crtc_hsync_end - 1) << 16));
+	REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+		  ((adjusted_mode->crtc_vtotal - 1) << 16));
+	REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+		  ((adjusted_mode->crtc_vblank_end - 1) << 16));
+	REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+		  ((adjusted_mode->crtc_vsync_end - 1) << 16));
+	/* pipesrc and dspsize control the size that is scaled from,
+	 * which should always be the user's requested size.
+	 */
+	REG_WRITE(dspsize_reg,
+		  ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
+	REG_WRITE(dsppos_reg, 0);
+	REG_WRITE(pipesrc_reg,
+		  ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
+	REG_WRITE(pipeconf_reg, pipeconf);
+	REG_READ(pipeconf_reg);
+
+	psb_intel_wait_for_vblank(dev);
+
+	REG_WRITE(dspcntr_reg, dspcntr);
+
+	/* Flush the plane changes */
+	crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+
+	psb_intel_wait_for_vblank(dev);
+
+	return 0;
+}
+
+/** Loads the palette/gamma unit for the CRTC with the prepared values */
+void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_psb_private *dev_priv =
+				(struct drm_psb_private *)dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int palreg = PALETTE_A;
+	int i;
+
+	/* The clocks have to be on to load the palette. */
+	if (!crtc->enabled)
+		return;
+
+	switch (psb_intel_crtc->pipe) {
+	case 0:
+		break;
+	case 1:
+		palreg = PALETTE_B;
+		break;
+	case 2:
+		palreg = PALETTE_C;
+		break;
+	default:
+		dev_err(dev->dev, "Illegal Pipe Number.\n");
+		return;
+	}
+
+	if (gma_power_begin(dev, false)) {
+		for (i = 0; i < 256; i++) {
+			REG_WRITE(palreg + 4 * i,
+				  ((psb_intel_crtc->lut_r[i] +
+				  psb_intel_crtc->lut_adj[i]) << 16) |
+				  ((psb_intel_crtc->lut_g[i] +
+				  psb_intel_crtc->lut_adj[i]) << 8) |
+				  (psb_intel_crtc->lut_b[i] +
+				  psb_intel_crtc->lut_adj[i]));
+		}
+		gma_power_end(dev);
+	} else {
+		for (i = 0; i < 256; i++) {
+			dev_priv->save_palette_a[i] =
+				  ((psb_intel_crtc->lut_r[i] +
+				  psb_intel_crtc->lut_adj[i]) << 16) |
+				  ((psb_intel_crtc->lut_g[i] +
+				  psb_intel_crtc->lut_adj[i]) << 8) |
+				  (psb_intel_crtc->lut_b[i] +
+				  psb_intel_crtc->lut_adj[i]);
+		}
+
+	}
+}
+
+/**
+ * Save HW states of giving crtc
+ */
+static void psb_intel_crtc_save(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	/* struct drm_psb_private *dev_priv =
+			(struct drm_psb_private *)dev->dev_private; */
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
+	int pipeA = (psb_intel_crtc->pipe == 0);
+	uint32_t paletteReg;
+	int i;
+
+	if (!crtc_state) {
+		dev_err(dev->dev, "No CRTC state found\n");
+		return;
+	}
+
+	crtc_state->saveDSPCNTR = REG_READ(pipeA ? DSPACNTR : DSPBCNTR);
+	crtc_state->savePIPECONF = REG_READ(pipeA ? PIPEACONF : PIPEBCONF);
+	crtc_state->savePIPESRC = REG_READ(pipeA ? PIPEASRC : PIPEBSRC);
+	crtc_state->saveFP0 = REG_READ(pipeA ? FPA0 : FPB0);
+	crtc_state->saveFP1 = REG_READ(pipeA ? FPA1 : FPB1);
+	crtc_state->saveDPLL = REG_READ(pipeA ? DPLL_A : DPLL_B);
+	crtc_state->saveHTOTAL = REG_READ(pipeA ? HTOTAL_A : HTOTAL_B);
+	crtc_state->saveHBLANK = REG_READ(pipeA ? HBLANK_A : HBLANK_B);
+	crtc_state->saveHSYNC = REG_READ(pipeA ? HSYNC_A : HSYNC_B);
+	crtc_state->saveVTOTAL = REG_READ(pipeA ? VTOTAL_A : VTOTAL_B);
+	crtc_state->saveVBLANK = REG_READ(pipeA ? VBLANK_A : VBLANK_B);
+	crtc_state->saveVSYNC = REG_READ(pipeA ? VSYNC_A : VSYNC_B);
+	crtc_state->saveDSPSTRIDE = REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE);
+
+	/*NOTE: DSPSIZE DSPPOS only for psb*/
+	crtc_state->saveDSPSIZE = REG_READ(pipeA ? DSPASIZE : DSPBSIZE);
+	crtc_state->saveDSPPOS = REG_READ(pipeA ? DSPAPOS : DSPBPOS);
+
+	crtc_state->saveDSPBASE = REG_READ(pipeA ? DSPABASE : DSPBBASE);
+
+	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	for (i = 0; i < 256; ++i)
+		crtc_state->savePalette[i] = REG_READ(paletteReg + (i << 2));
+}
+
+/**
+ * Restore HW states of giving crtc
+ */
+static void psb_intel_crtc_restore(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	/* struct drm_psb_private * dev_priv =
+				(struct drm_psb_private *)dev->dev_private; */
+	struct psb_intel_crtc *psb_intel_crtc =  to_psb_intel_crtc(crtc);
+	struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
+	/* struct drm_crtc_helper_funcs * crtc_funcs = crtc->helper_private; */
+	int pipeA = (psb_intel_crtc->pipe == 0);
+	uint32_t paletteReg;
+	int i;
+
+	if (!crtc_state) {
+		dev_err(dev->dev, "No crtc state\n");
+		return;
+	}
+
+	if (crtc_state->saveDPLL & DPLL_VCO_ENABLE) {
+		REG_WRITE(pipeA ? DPLL_A : DPLL_B,
+			crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
+		REG_READ(pipeA ? DPLL_A : DPLL_B);
+		udelay(150);
+	}
+
+	REG_WRITE(pipeA ? FPA0 : FPB0, crtc_state->saveFP0);
+	REG_READ(pipeA ? FPA0 : FPB0);
+
+	REG_WRITE(pipeA ? FPA1 : FPB1, crtc_state->saveFP1);
+	REG_READ(pipeA ? FPA1 : FPB1);
+
+	REG_WRITE(pipeA ? DPLL_A : DPLL_B, crtc_state->saveDPLL);
+	REG_READ(pipeA ? DPLL_A : DPLL_B);
+	udelay(150);
+
+	REG_WRITE(pipeA ? HTOTAL_A : HTOTAL_B, crtc_state->saveHTOTAL);
+	REG_WRITE(pipeA ? HBLANK_A : HBLANK_B, crtc_state->saveHBLANK);
+	REG_WRITE(pipeA ? HSYNC_A : HSYNC_B, crtc_state->saveHSYNC);
+	REG_WRITE(pipeA ? VTOTAL_A : VTOTAL_B, crtc_state->saveVTOTAL);
+	REG_WRITE(pipeA ? VBLANK_A : VBLANK_B, crtc_state->saveVBLANK);
+	REG_WRITE(pipeA ? VSYNC_A : VSYNC_B, crtc_state->saveVSYNC);
+	REG_WRITE(pipeA ? DSPASTRIDE : DSPBSTRIDE, crtc_state->saveDSPSTRIDE);
+
+	REG_WRITE(pipeA ? DSPASIZE : DSPBSIZE, crtc_state->saveDSPSIZE);
+	REG_WRITE(pipeA ? DSPAPOS : DSPBPOS, crtc_state->saveDSPPOS);
+
+	REG_WRITE(pipeA ? PIPEASRC : PIPEBSRC, crtc_state->savePIPESRC);
+	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+	REG_WRITE(pipeA ? PIPEACONF : PIPEBCONF, crtc_state->savePIPECONF);
+
+	psb_intel_wait_for_vblank(dev);
+
+	REG_WRITE(pipeA ? DSPACNTR : DSPBCNTR, crtc_state->saveDSPCNTR);
+	REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+
+	psb_intel_wait_for_vblank(dev);
+
+	paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+	for (i = 0; i < 256; ++i)
+		REG_WRITE(paletteReg + (i << 2), crtc_state->savePalette[i]);
+}
+
+static int psb_intel_crtc_cursor_set(struct drm_crtc *crtc,
+				 struct drm_file *file_priv,
+				 uint32_t handle,
+				 uint32_t width, uint32_t height)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	uint32_t control = (pipe == 0) ? CURACNTR : CURBCNTR;
+	uint32_t base = (pipe == 0) ? CURABASE : CURBBASE;
+	uint32_t temp;
+	size_t addr = 0;
+	struct gtt_range *gt;
+	struct drm_gem_object *obj;
+	int ret;
+
+	/* if we want to turn of the cursor ignore width and height */
+	if (!handle) {
+		/* turn off the cursor */
+		temp = CURSOR_MODE_DISABLE;
+
+		if (gma_power_begin(dev, false)) {
+			REG_WRITE(control, temp);
+			REG_WRITE(base, 0);
+			gma_power_end(dev);
+		}
+
+		/* Unpin the old GEM object */
+		if (psb_intel_crtc->cursor_obj) {
+			gt = container_of(psb_intel_crtc->cursor_obj,
+							struct gtt_range, gem);
+			psb_gtt_unpin(gt);
+			drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+			psb_intel_crtc->cursor_obj = NULL;
+		}
+
+		return 0;
+	}
+
+	/* Currently we only support 64x64 cursors */
+	if (width != 64 || height != 64) {
+		dev_dbg(dev->dev, "we currently only support 64x64 cursors\n");
+		return -EINVAL;
+	}
+
+	obj = drm_gem_object_lookup(dev, file_priv, handle);
+	if (!obj)
+		return -ENOENT;
+
+	if (obj->size < width * height * 4) {
+		dev_dbg(dev->dev, "buffer is to small\n");
+		return -ENOMEM;
+	}
+
+	gt = container_of(obj, struct gtt_range, gem);
+
+	/* Pin the memory into the GTT */
+	ret = psb_gtt_pin(gt);
+	if (ret) {
+		dev_err(dev->dev, "Can not pin down handle 0x%x\n", handle);
+		return ret;
+	}
+
+
+	addr = gt->offset;	/* Or resource.start ??? */
+
+	psb_intel_crtc->cursor_addr = addr;
+
+	temp = 0;
+	/* set the pipe for the cursor */
+	temp |= (pipe << 28);
+	temp |= CURSOR_MODE_64_ARGB_AX | MCURSOR_GAMMA_ENABLE;
+
+	if (gma_power_begin(dev, false)) {
+		REG_WRITE(control, temp);
+		REG_WRITE(base, addr);
+		gma_power_end(dev);
+	}
+
+	/* unpin the old bo */
+	if (psb_intel_crtc->cursor_obj) {
+		gt = container_of(psb_intel_crtc->cursor_obj,
+							struct gtt_range, gem);
+		psb_gtt_unpin(gt);
+		drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+		psb_intel_crtc->cursor_obj = obj;
+	}
+	return 0;
+}
+
+static int psb_intel_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
+{
+	struct drm_device *dev = crtc->dev;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	uint32_t temp = 0;
+	uint32_t addr;
+
+
+	if (x < 0) {
+		temp |= (CURSOR_POS_SIGN << CURSOR_X_SHIFT);
+		x = -x;
+	}
+	if (y < 0) {
+		temp |= (CURSOR_POS_SIGN << CURSOR_Y_SHIFT);
+		y = -y;
+	}
+
+	temp |= ((x & CURSOR_POS_MASK) << CURSOR_X_SHIFT);
+	temp |= ((y & CURSOR_POS_MASK) << CURSOR_Y_SHIFT);
+
+	addr = psb_intel_crtc->cursor_addr;
+
+	if (gma_power_begin(dev, false)) {
+		REG_WRITE((pipe == 0) ? CURAPOS : CURBPOS, temp);
+		REG_WRITE((pipe == 0) ? CURABASE : CURBBASE, addr);
+		gma_power_end(dev);
+	}
+	return 0;
+}
+
+void psb_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
+			 u16 *green, u16 *blue, uint32_t type, uint32_t size)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int i;
+
+	if (size != 256)
+		return;
+
+	for (i = 0; i < 256; i++) {
+		psb_intel_crtc->lut_r[i] = red[i] >> 8;
+		psb_intel_crtc->lut_g[i] = green[i] >> 8;
+		psb_intel_crtc->lut_b[i] = blue[i] >> 8;
+	}
+
+	psb_intel_crtc_load_lut(crtc);
+}
+
+static int psb_crtc_set_config(struct drm_mode_set *set)
+{
+	int ret;
+	struct drm_device *dev = set->crtc->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (!dev_priv->rpm_enabled)
+		return drm_crtc_helper_set_config(set);
+
+	pm_runtime_forbid(&dev->pdev->dev);
+	ret = drm_crtc_helper_set_config(set);
+	pm_runtime_allow(&dev->pdev->dev);
+	return ret;
+}
+
+/* Returns the clock of the currently programmed mode of the given pipe. */
+static int psb_intel_crtc_clock_get(struct drm_device *dev,
+				struct drm_crtc *crtc)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	u32 dpll;
+	u32 fp;
+	struct psb_intel_clock_t clock;
+	bool is_lvds;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (gma_power_begin(dev, false)) {
+		dpll = REG_READ((pipe == 0) ? DPLL_A : DPLL_B);
+		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
+			fp = REG_READ((pipe == 0) ? FPA0 : FPB0);
+		else
+			fp = REG_READ((pipe == 0) ? FPA1 : FPB1);
+		is_lvds = (pipe == 1) && (REG_READ(LVDS) & LVDS_PORT_EN);
+		gma_power_end(dev);
+	} else {
+		dpll = (pipe == 0) ?
+			dev_priv->saveDPLL_A : dev_priv->saveDPLL_B;
+
+		if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
+			fp = (pipe == 0) ?
+				dev_priv->saveFPA0 :
+				dev_priv->saveFPB0;
+		else
+			fp = (pipe == 0) ?
+				dev_priv->saveFPA1 :
+				dev_priv->saveFPB1;
+
+		is_lvds = (pipe == 1) && (dev_priv->saveLVDS & LVDS_PORT_EN);
+	}
+
+	clock.m1 = (fp & FP_M1_DIV_MASK) >> FP_M1_DIV_SHIFT;
+	clock.m2 = (fp & FP_M2_DIV_MASK) >> FP_M2_DIV_SHIFT;
+	clock.n = (fp & FP_N_DIV_MASK) >> FP_N_DIV_SHIFT;
+
+	if (is_lvds) {
+		clock.p1 =
+		    ffs((dpll &
+			 DPLL_FPA01_P1_POST_DIV_MASK_I830_LVDS) >>
+			DPLL_FPA01_P1_POST_DIV_SHIFT);
+		clock.p2 = 14;
+
+		if ((dpll & PLL_REF_INPUT_MASK) ==
+		    PLLB_REF_INPUT_SPREADSPECTRUMIN) {
+			/* XXX: might not be 66MHz */
+			i8xx_clock(66000, &clock);
+		} else
+			i8xx_clock(48000, &clock);
+	} else {
+		if (dpll & PLL_P1_DIVIDE_BY_TWO)
+			clock.p1 = 2;
+		else {
+			clock.p1 =
+			    ((dpll &
+			      DPLL_FPA01_P1_POST_DIV_MASK_I830) >>
+			     DPLL_FPA01_P1_POST_DIV_SHIFT) + 2;
+		}
+		if (dpll & PLL_P2_DIVIDE_BY_4)
+			clock.p2 = 4;
+		else
+			clock.p2 = 2;
+
+		i8xx_clock(48000, &clock);
+	}
+
+	/* XXX: It would be nice to validate the clocks, but we can't reuse
+	 * i830PllIsValid() because it relies on the xf86_config connector
+	 * configuration being accurate, which it isn't necessarily.
+	 */
+
+	return clock.dot;
+}
+
+/** Returns the currently programmed mode of the given pipe. */
+struct drm_display_mode *psb_intel_crtc_mode_get(struct drm_device *dev,
+					     struct drm_crtc *crtc)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	int pipe = psb_intel_crtc->pipe;
+	struct drm_display_mode *mode;
+	int htot;
+	int hsync;
+	int vtot;
+	int vsync;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	if (gma_power_begin(dev, false)) {
+		htot = REG_READ((pipe == 0) ? HTOTAL_A : HTOTAL_B);
+		hsync = REG_READ((pipe == 0) ? HSYNC_A : HSYNC_B);
+		vtot = REG_READ((pipe == 0) ? VTOTAL_A : VTOTAL_B);
+		vsync = REG_READ((pipe == 0) ? VSYNC_A : VSYNC_B);
+		gma_power_end(dev);
+	} else {
+		htot = (pipe == 0) ?
+			dev_priv->saveHTOTAL_A : dev_priv->saveHTOTAL_B;
+		hsync = (pipe == 0) ?
+			dev_priv->saveHSYNC_A : dev_priv->saveHSYNC_B;
+		vtot = (pipe == 0) ?
+			dev_priv->saveVTOTAL_A : dev_priv->saveVTOTAL_B;
+		vsync = (pipe == 0) ?
+			dev_priv->saveVSYNC_A : dev_priv->saveVSYNC_B;
+	}
+
+	mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+	if (!mode)
+		return NULL;
+
+	mode->clock = psb_intel_crtc_clock_get(dev, crtc);
+	mode->hdisplay = (htot & 0xffff) + 1;
+	mode->htotal = ((htot & 0xffff0000) >> 16) + 1;
+	mode->hsync_start = (hsync & 0xffff) + 1;
+	mode->hsync_end = ((hsync & 0xffff0000) >> 16) + 1;
+	mode->vdisplay = (vtot & 0xffff) + 1;
+	mode->vtotal = ((vtot & 0xffff0000) >> 16) + 1;
+	mode->vsync_start = (vsync & 0xffff) + 1;
+	mode->vsync_end = ((vsync & 0xffff0000) >> 16) + 1;
+
+	drm_mode_set_name(mode);
+	drm_mode_set_crtcinfo(mode, 0);
+
+	return mode;
+}
+
+void psb_intel_crtc_destroy(struct drm_crtc *crtc)
+{
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct gtt_range *gt;
+
+	/* Unpin the old GEM object */
+	if (psb_intel_crtc->cursor_obj) {
+		gt = container_of(psb_intel_crtc->cursor_obj,
+						struct gtt_range, gem);
+		psb_gtt_unpin(gt);
+		drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+		psb_intel_crtc->cursor_obj = NULL;
+	}
+	kfree(psb_intel_crtc->crtc_state);
+	drm_crtc_cleanup(crtc);
+	kfree(psb_intel_crtc);
+}
+
+const struct drm_crtc_helper_funcs psb_intel_helper_funcs = {
+	.dpms = psb_intel_crtc_dpms,
+	.mode_fixup = psb_intel_crtc_mode_fixup,
+	.mode_set = psb_intel_crtc_mode_set,
+	.mode_set_base = psb_intel_pipe_set_base,
+	.prepare = psb_intel_crtc_prepare,
+	.commit = psb_intel_crtc_commit,
+};
+
+const struct drm_crtc_funcs psb_intel_crtc_funcs = {
+	.save = psb_intel_crtc_save,
+	.restore = psb_intel_crtc_restore,
+	.cursor_set = psb_intel_crtc_cursor_set,
+	.cursor_move = psb_intel_crtc_cursor_move,
+	.gamma_set = psb_intel_crtc_gamma_set,
+	.set_config = psb_crtc_set_config,
+	.destroy = psb_intel_crtc_destroy,
+};
+
+/*
+ * Set the default value of cursor control and base register
+ * to zero. This is a workaround for h/w defect on Oaktrail
+ */
+static void psb_intel_cursor_init(struct drm_device *dev, int pipe)
+{
+	u32 control[3] = { CURACNTR, CURBCNTR, CURCCNTR };
+	u32 base[3] = { CURABASE, CURBBASE, CURCBASE };
+
+	REG_WRITE(control[pipe], 0);
+	REG_WRITE(base[pipe], 0);
+}
+
+void psb_intel_crtc_init(struct drm_device *dev, int pipe,
+		     struct psb_intel_mode_device *mode_dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_crtc *psb_intel_crtc;
+	int i;
+	uint16_t *r_base, *g_base, *b_base;
+
+	/* We allocate a extra array of drm_connector pointers
+	 * for fbdev after the crtc */
+	psb_intel_crtc =
+	    kzalloc(sizeof(struct psb_intel_crtc) +
+		    (INTELFB_CONN_LIMIT * sizeof(struct drm_connector *)),
+		    GFP_KERNEL);
+	if (psb_intel_crtc == NULL)
+		return;
+
+	psb_intel_crtc->crtc_state =
+		kzalloc(sizeof(struct psb_intel_crtc_state), GFP_KERNEL);
+	if (!psb_intel_crtc->crtc_state) {
+		dev_err(dev->dev, "Crtc state error: No memory\n");
+		kfree(psb_intel_crtc);
+		return;
+	}
+
+	/* Set the CRTC operations from the chip specific data */
+	drm_crtc_init(dev, &psb_intel_crtc->base, dev_priv->ops->crtc_funcs);
+
+	drm_mode_crtc_set_gamma_size(&psb_intel_crtc->base, 256);
+	psb_intel_crtc->pipe = pipe;
+	psb_intel_crtc->plane = pipe;
+
+	r_base = psb_intel_crtc->base.gamma_store;
+	g_base = r_base + 256;
+	b_base = g_base + 256;
+	for (i = 0; i < 256; i++) {
+		psb_intel_crtc->lut_r[i] = i;
+		psb_intel_crtc->lut_g[i] = i;
+		psb_intel_crtc->lut_b[i] = i;
+		r_base[i] = i << 8;
+		g_base[i] = i << 8;
+		b_base[i] = i << 8;
+
+		psb_intel_crtc->lut_adj[i] = 0;
+	}
+
+	psb_intel_crtc->mode_dev = mode_dev;
+	psb_intel_crtc->cursor_addr = 0;
+
+	drm_crtc_helper_add(&psb_intel_crtc->base,
+						dev_priv->ops->crtc_helper);
+
+	/* Setup the array of drm_connector pointer array */
+	psb_intel_crtc->mode_set.crtc = &psb_intel_crtc->base;
+	BUG_ON(pipe >= ARRAY_SIZE(dev_priv->plane_to_crtc_mapping) ||
+	       dev_priv->plane_to_crtc_mapping[psb_intel_crtc->plane] != NULL);
+	dev_priv->plane_to_crtc_mapping[psb_intel_crtc->plane] =
+							&psb_intel_crtc->base;
+	dev_priv->pipe_to_crtc_mapping[psb_intel_crtc->pipe] =
+							&psb_intel_crtc->base;
+	psb_intel_crtc->mode_set.connectors =
+	    (struct drm_connector **) (psb_intel_crtc + 1);
+	psb_intel_crtc->mode_set.num_connectors = 0;
+	psb_intel_cursor_init(dev, pipe);
+}
+
+int psb_intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
+				struct drm_file *file_priv)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct drm_psb_get_pipe_from_crtc_id_arg *pipe_from_crtc_id = data;
+	struct drm_mode_object *drmmode_obj;
+	struct psb_intel_crtc *crtc;
+
+	if (!dev_priv) {
+		dev_err(dev->dev, "called with no initialization\n");
+		return -EINVAL;
+	}
+
+	drmmode_obj = drm_mode_object_find(dev, pipe_from_crtc_id->crtc_id,
+			DRM_MODE_OBJECT_CRTC);
+
+	if (!drmmode_obj) {
+		dev_err(dev->dev, "no such CRTC id\n");
+		return -EINVAL;
+	}
+
+	crtc = to_psb_intel_crtc(obj_to_crtc(drmmode_obj));
+	pipe_from_crtc_id->pipe = crtc->pipe;
+
+	return 0;
+}
+
+struct drm_crtc *psb_intel_get_crtc_from_pipe(struct drm_device *dev, int pipe)
+{
+	struct drm_crtc *crtc = NULL;
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+		if (psb_intel_crtc->pipe == pipe)
+			break;
+	}
+	return crtc;
+}
+
+int psb_intel_connector_clones(struct drm_device *dev, int type_mask)
+{
+	int index_mask = 0;
+	struct drm_connector *connector;
+	int entry = 0;
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list,
+			    head) {
+		struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+		if (type_mask & (1 << psb_intel_encoder->type))
+			index_mask |= (1 << entry);
+		entry++;
+	}
+	return index_mask;
+}
+
+
+void psb_intel_modeset_cleanup(struct drm_device *dev)
+{
+	drm_mode_config_cleanup(dev);
+}
+
+
+/* current intel driver doesn't take advantage of encoders
+   always give back the encoder for the connector
+*/
+struct drm_encoder *psb_intel_best_encoder(struct drm_connector *connector)
+{
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+
+	return &psb_intel_encoder->base;
+}
+
+void psb_intel_connector_attach_encoder(struct psb_intel_connector *connector,
+					struct psb_intel_encoder *encoder)
+{
+	connector->encoder = encoder;
+	drm_mode_connector_attach_encoder(&connector->base,
+					  &encoder->base);
+}
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.h b/drivers/gpu/drm/gma500/psb_intel_display.h
new file mode 100644
index 000000000000..535b49a5e409
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_intel_display.h
@@ -0,0 +1,28 @@
+/* copyright (c) 2008, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ * Eric Anholt <eric@anholt.net>
+ */
+
+#ifndef _INTEL_DISPLAY_H_
+#define _INTEL_DISPLAY_H_
+
+bool psb_intel_pipe_has_type(struct drm_crtc *crtc, int type);
+void psb_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
+			 u16 *green, u16 *blue, uint32_t type, uint32_t size);
+void psb_intel_crtc_destroy(struct drm_crtc *crtc);
+
+#endif
diff --git a/drivers/gpu/drm/gma500/psb_intel_drv.h b/drivers/gpu/drm/gma500/psb_intel_drv.h
new file mode 100644
index 000000000000..f40535e56689
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_intel_drv.h
@@ -0,0 +1,289 @@
+/*
+ * Copyright (c) 2009-2011, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#ifndef __INTEL_DRV_H__
+#define __INTEL_DRV_H__
+
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_crtc_helper.h>
+#include <linux/gpio.h>
+
+/*
+ * Display related stuff
+ */
+
+/* store information about an Ixxx DVO */
+/* The i830->i865 use multiple DVOs with multiple i2cs */
+/* the i915, i945 have a single sDVO i2c bus - which is different */
+#define MAX_OUTPUTS 6
+/* maximum connectors per crtcs in the mode set */
+#define INTELFB_CONN_LIMIT 4
+
+#define INTEL_I2C_BUS_DVO 1
+#define INTEL_I2C_BUS_SDVO 2
+
+/* Intel Pipe Clone Bit */
+#define INTEL_HDMIB_CLONE_BIT 1
+#define INTEL_HDMIC_CLONE_BIT 2
+#define INTEL_HDMID_CLONE_BIT 3
+#define INTEL_HDMIE_CLONE_BIT 4
+#define INTEL_HDMIF_CLONE_BIT 5
+#define INTEL_SDVO_NON_TV_CLONE_BIT 6
+#define INTEL_SDVO_TV_CLONE_BIT 7
+#define INTEL_SDVO_LVDS_CLONE_BIT 8
+#define INTEL_ANALOG_CLONE_BIT 9
+#define INTEL_TV_CLONE_BIT 10
+#define INTEL_DP_B_CLONE_BIT 11
+#define INTEL_DP_C_CLONE_BIT 12
+#define INTEL_DP_D_CLONE_BIT 13
+#define INTEL_LVDS_CLONE_BIT 14
+#define INTEL_DVO_TMDS_CLONE_BIT 15
+#define INTEL_DVO_LVDS_CLONE_BIT 16
+#define INTEL_EDP_CLONE_BIT 17
+
+/* these are outputs from the chip - integrated only
+ * external chips are via DVO or SDVO output */
+#define INTEL_OUTPUT_UNUSED 0
+#define INTEL_OUTPUT_ANALOG 1
+#define INTEL_OUTPUT_DVO 2
+#define INTEL_OUTPUT_SDVO 3
+#define INTEL_OUTPUT_LVDS 4
+#define INTEL_OUTPUT_TVOUT 5
+#define INTEL_OUTPUT_HDMI 6
+#define INTEL_OUTPUT_MIPI 7
+#define INTEL_OUTPUT_MIPI2 8
+
+#define INTEL_DVO_CHIP_NONE 0
+#define INTEL_DVO_CHIP_LVDS 1
+#define INTEL_DVO_CHIP_TMDS 2
+#define INTEL_DVO_CHIP_TVOUT 4
+
+#define INTEL_MODE_PIXEL_MULTIPLIER_SHIFT (0x0)
+#define INTEL_MODE_PIXEL_MULTIPLIER_MASK (0xf << INTEL_MODE_PIXEL_MULTIPLIER_SHIFT)
+
+static inline void
+psb_intel_mode_set_pixel_multiplier(struct drm_display_mode *mode,
+				int multiplier)
+{
+	mode->clock *= multiplier;
+	mode->private_flags |= multiplier;
+}
+
+static inline int
+psb_intel_mode_get_pixel_multiplier(const struct drm_display_mode *mode)
+{
+	return (mode->private_flags & INTEL_MODE_PIXEL_MULTIPLIER_MASK)
+	       >> INTEL_MODE_PIXEL_MULTIPLIER_SHIFT;
+}
+
+
+/*
+ * Hold information useally put on the device driver privates here,
+ * since it needs to be shared across multiple of devices drivers privates.
+ */
+struct psb_intel_mode_device {
+
+	/*
+	 * Abstracted memory manager operations
+	 */
+	 size_t(*bo_offset) (struct drm_device *dev, void *bo);
+
+	/*
+	 * Cursor (Can go ?)
+	 */
+	int cursor_needs_physical;
+
+	/*
+	 * LVDS info
+	 */
+	int backlight_duty_cycle;	/* restore backlight to this value */
+	bool panel_wants_dither;
+	struct drm_display_mode *panel_fixed_mode;
+	struct drm_display_mode *panel_fixed_mode2;
+	struct drm_display_mode *vbt_mode;	/* if any */
+
+	uint32_t saveBLC_PWM_CTL;
+};
+
+struct psb_intel_i2c_chan {
+	/* for getting at dev. private (mmio etc.) */
+	struct drm_device *drm_dev;
+	u32 reg;		/* GPIO reg */
+	struct i2c_adapter adapter;
+	struct i2c_algo_bit_data algo;
+	u8 slave_addr;
+};
+
+struct psb_intel_encoder {
+	struct drm_encoder base;
+	int type;
+	bool needs_tv_clock;
+	void (*hot_plug)(struct psb_intel_encoder *);
+	int crtc_mask;
+	int clone_mask;
+	void *dev_priv; /* For sdvo_priv, lvds_priv, etc... */
+
+	/* FIXME: Either make SDVO and LVDS store it's i2c here or give CDV it's
+	   own set of output privates */
+	struct psb_intel_i2c_chan *i2c_bus;
+	struct psb_intel_i2c_chan *ddc_bus;
+};
+
+struct psb_intel_connector {
+	struct drm_connector base;
+	struct psb_intel_encoder *encoder;
+};
+
+struct psb_intel_crtc_state {
+	uint32_t saveDSPCNTR;
+	uint32_t savePIPECONF;
+	uint32_t savePIPESRC;
+	uint32_t saveDPLL;
+	uint32_t saveFP0;
+	uint32_t saveFP1;
+	uint32_t saveHTOTAL;
+	uint32_t saveHBLANK;
+	uint32_t saveHSYNC;
+	uint32_t saveVTOTAL;
+	uint32_t saveVBLANK;
+	uint32_t saveVSYNC;
+	uint32_t saveDSPSTRIDE;
+	uint32_t saveDSPSIZE;
+	uint32_t saveDSPPOS;
+	uint32_t saveDSPBASE;
+	uint32_t savePalette[256];
+};
+
+struct psb_intel_crtc {
+	struct drm_crtc base;
+	int pipe;
+	int plane;
+	uint32_t cursor_addr;
+	u8 lut_r[256], lut_g[256], lut_b[256];
+	u8 lut_adj[256];
+	struct psb_intel_framebuffer *fbdev_fb;
+	/* a mode_set for fbdev users on this crtc */
+	struct drm_mode_set mode_set;
+
+	/* GEM object that holds our cursor */
+	struct drm_gem_object *cursor_obj;
+
+	struct drm_display_mode saved_mode;
+	struct drm_display_mode saved_adjusted_mode;
+
+	struct psb_intel_mode_device *mode_dev;
+
+	/*crtc mode setting flags*/
+	u32 mode_flags;
+
+	/* Saved Crtc HW states */
+	struct psb_intel_crtc_state *crtc_state;
+};
+
+#define to_psb_intel_crtc(x)	\
+		container_of(x, struct psb_intel_crtc, base)
+#define to_psb_intel_connector(x) \
+		container_of(x, struct psb_intel_connector, base)
+#define to_psb_intel_encoder(x)	\
+		container_of(x, struct psb_intel_encoder, base)
+#define to_psb_intel_framebuffer(x)	\
+		container_of(x, struct psb_intel_framebuffer, base)
+
+struct psb_intel_i2c_chan *psb_intel_i2c_create(struct drm_device *dev,
+					const u32 reg, const char *name);
+void psb_intel_i2c_destroy(struct psb_intel_i2c_chan *chan);
+int psb_intel_ddc_get_modes(struct drm_connector *connector,
+			    struct i2c_adapter *adapter);
+extern bool psb_intel_ddc_probe(struct i2c_adapter *adapter);
+
+extern void psb_intel_crtc_init(struct drm_device *dev, int pipe,
+			    struct psb_intel_mode_device *mode_dev);
+extern void psb_intel_crt_init(struct drm_device *dev);
+extern bool psb_intel_sdvo_init(struct drm_device *dev, int output_device);
+extern void psb_intel_dvo_init(struct drm_device *dev);
+extern void psb_intel_tv_init(struct drm_device *dev);
+extern void psb_intel_lvds_init(struct drm_device *dev,
+			    struct psb_intel_mode_device *mode_dev);
+extern void psb_intel_lvds_set_brightness(struct drm_device *dev, int level);
+extern void oaktrail_lvds_init(struct drm_device *dev,
+			   struct psb_intel_mode_device *mode_dev);
+extern void oaktrail_wait_for_INTR_PKT_SENT(struct drm_device *dev);
+extern void oaktrail_dsi_init(struct drm_device *dev,
+			   struct psb_intel_mode_device *mode_dev);
+extern void mid_dsi_init(struct drm_device *dev,
+		    struct psb_intel_mode_device *mode_dev, int dsi_num);
+
+extern void psb_intel_crtc_load_lut(struct drm_crtc *crtc);
+extern void psb_intel_encoder_prepare(struct drm_encoder *encoder);
+extern void psb_intel_encoder_commit(struct drm_encoder *encoder);
+extern void psb_intel_encoder_destroy(struct drm_encoder *encoder);
+
+static inline struct psb_intel_encoder *psb_intel_attached_encoder(
+						struct drm_connector *connector)
+{
+	return to_psb_intel_connector(connector)->encoder;
+}
+
+extern void psb_intel_connector_attach_encoder(
+					struct psb_intel_connector *connector,
+					struct psb_intel_encoder *encoder);
+
+extern struct drm_encoder *psb_intel_best_encoder(struct drm_connector
+					      *connector);
+
+extern struct drm_display_mode *psb_intel_crtc_mode_get(struct drm_device *dev,
+						    struct drm_crtc *crtc);
+extern void psb_intel_wait_for_vblank(struct drm_device *dev);
+extern int psb_intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
+				struct drm_file *file_priv);
+extern struct drm_crtc *psb_intel_get_crtc_from_pipe(struct drm_device *dev,
+						 int pipe);
+extern struct drm_connector *psb_intel_sdvo_find(struct drm_device *dev,
+					     int sdvoB);
+extern int psb_intel_sdvo_supports_hotplug(struct drm_connector *connector);
+extern void psb_intel_sdvo_set_hotplug(struct drm_connector *connector,
+				   int enable);
+extern int intelfb_probe(struct drm_device *dev);
+extern int intelfb_remove(struct drm_device *dev,
+			  struct drm_framebuffer *fb);
+extern struct drm_framebuffer *psb_intel_framebuffer_create(struct drm_device
+							*dev, struct
+							drm_mode_fb_cmd
+							*mode_cmd,
+							void *mm_private);
+extern bool psb_intel_lvds_mode_fixup(struct drm_encoder *encoder,
+				      struct drm_display_mode *mode,
+				      struct drm_display_mode *adjusted_mode);
+extern int psb_intel_lvds_mode_valid(struct drm_connector *connector,
+				     struct drm_display_mode *mode);
+extern int psb_intel_lvds_set_property(struct drm_connector *connector,
+					struct drm_property *property,
+					uint64_t value);
+extern void psb_intel_lvds_destroy(struct drm_connector *connector);
+extern const struct drm_encoder_funcs psb_intel_lvds_enc_funcs;
+
+/* intel_gmbus.c */
+extern void gma_intel_i2c_reset(struct drm_device *dev);
+extern int gma_intel_setup_gmbus(struct drm_device *dev);
+extern void gma_intel_gmbus_set_speed(struct i2c_adapter *adapter, int speed);
+extern void gma_intel_gmbus_force_bit(struct i2c_adapter *adapter, bool force_bit);
+extern void gma_intel_teardown_gmbus(struct drm_device *dev);
+
+#endif				/* __INTEL_DRV_H__ */
diff --git a/drivers/gpu/drm/gma500/psb_intel_lvds.c b/drivers/gpu/drm/gma500/psb_intel_lvds.c
new file mode 100644
index 000000000000..a25e4ca5e91c
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_intel_lvds.c
@@ -0,0 +1,868 @@
+/*
+ * Copyright © 2006-2007 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ *	Dave Airlie <airlied@linux.ie>
+ *	Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <drm/drmP.h>
+
+#include "intel_bios.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include <linux/pm_runtime.h>
+
+/*
+ * LVDS I2C backlight control macros
+ */
+#define BRIGHTNESS_MAX_LEVEL 100
+#define BRIGHTNESS_MASK 0xFF
+#define BLC_I2C_TYPE	0x01
+#define BLC_PWM_TYPT	0x02
+
+#define BLC_POLARITY_NORMAL 0
+#define BLC_POLARITY_INVERSE 1
+
+#define PSB_BLC_MAX_PWM_REG_FREQ       (0xFFFE)
+#define PSB_BLC_MIN_PWM_REG_FREQ	(0x2)
+#define PSB_BLC_PWM_PRECISION_FACTOR	(10)
+#define PSB_BACKLIGHT_PWM_CTL_SHIFT	(16)
+#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
+
+struct psb_intel_lvds_priv {
+	/*
+	 * Saved LVDO output states
+	 */
+	uint32_t savePP_ON;
+	uint32_t savePP_OFF;
+	uint32_t saveLVDS;
+	uint32_t savePP_CONTROL;
+	uint32_t savePP_CYCLE;
+	uint32_t savePFIT_CONTROL;
+	uint32_t savePFIT_PGM_RATIOS;
+	uint32_t saveBLC_PWM_CTL;
+
+	struct psb_intel_i2c_chan *i2c_bus;
+	struct psb_intel_i2c_chan *ddc_bus;
+};
+
+
+/*
+ * Returns the maximum level of the backlight duty cycle field.
+ */
+static u32 psb_intel_lvds_get_max_backlight(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 ret;
+
+	if (gma_power_begin(dev, false)) {
+		ret = REG_READ(BLC_PWM_CTL);
+		gma_power_end(dev);
+	} else /* Powered off, use the saved value */
+		ret = dev_priv->saveBLC_PWM_CTL;
+
+	/* Top 15bits hold the frequency mask */
+	ret = (ret &  BACKLIGHT_MODULATION_FREQ_MASK) >>
+					BACKLIGHT_MODULATION_FREQ_SHIFT;
+
+        ret *= 2;	/* Return a 16bit range as needed for setting */
+        if (ret == 0)
+                dev_err(dev->dev, "BL bug: Reg %08x save %08X\n",
+                        REG_READ(BLC_PWM_CTL), dev_priv->saveBLC_PWM_CTL);
+	return ret;
+}
+
+/*
+ * Set LVDS backlight level by I2C command
+ *
+ * FIXME: at some point we need to both track this for PM and also
+ * disable runtime pm on MRST if the brightness is nil (ie blanked)
+ */
+static int psb_lvds_i2c_set_brightness(struct drm_device *dev,
+					unsigned int level)
+{
+	struct drm_psb_private *dev_priv =
+		(struct drm_psb_private *)dev->dev_private;
+
+	struct psb_intel_i2c_chan *lvds_i2c_bus = dev_priv->lvds_i2c_bus;
+	u8 out_buf[2];
+	unsigned int blc_i2c_brightness;
+
+	struct i2c_msg msgs[] = {
+		{
+			.addr = lvds_i2c_bus->slave_addr,
+			.flags = 0,
+			.len = 2,
+			.buf = out_buf,
+		}
+	};
+
+	blc_i2c_brightness = BRIGHTNESS_MASK & ((unsigned int)level *
+			     BRIGHTNESS_MASK /
+			     BRIGHTNESS_MAX_LEVEL);
+
+	if (dev_priv->lvds_bl->pol == BLC_POLARITY_INVERSE)
+		blc_i2c_brightness = BRIGHTNESS_MASK - blc_i2c_brightness;
+
+	out_buf[0] = dev_priv->lvds_bl->brightnesscmd;
+	out_buf[1] = (u8)blc_i2c_brightness;
+
+	if (i2c_transfer(&lvds_i2c_bus->adapter, msgs, 1) == 1) {
+		dev_dbg(dev->dev, "I2C set brightness.(command, value) (%d, %d)\n",
+			dev_priv->lvds_bl->brightnesscmd,
+			blc_i2c_brightness);
+		return 0;
+	}
+
+	dev_err(dev->dev, "I2C transfer error\n");
+	return -1;
+}
+
+
+static int psb_lvds_pwm_set_brightness(struct drm_device *dev, int level)
+{
+	struct drm_psb_private *dev_priv =
+			(struct drm_psb_private *)dev->dev_private;
+
+	u32 max_pwm_blc;
+	u32 blc_pwm_duty_cycle;
+
+	max_pwm_blc = psb_intel_lvds_get_max_backlight(dev);
+
+	/*BLC_PWM_CTL Should be initiated while backlight device init*/
+	BUG_ON(max_pwm_blc == 0);
+
+	blc_pwm_duty_cycle = level * max_pwm_blc / BRIGHTNESS_MAX_LEVEL;
+
+	if (dev_priv->lvds_bl->pol == BLC_POLARITY_INVERSE)
+		blc_pwm_duty_cycle = max_pwm_blc - blc_pwm_duty_cycle;
+
+	blc_pwm_duty_cycle &= PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR;
+	REG_WRITE(BLC_PWM_CTL,
+		  (max_pwm_blc << PSB_BACKLIGHT_PWM_CTL_SHIFT) |
+		  (blc_pwm_duty_cycle));
+
+        dev_info(dev->dev, "Backlight lvds set brightness %08x\n",
+		  (max_pwm_blc << PSB_BACKLIGHT_PWM_CTL_SHIFT) |
+		  (blc_pwm_duty_cycle));
+
+	return 0;
+}
+
+/*
+ * Set LVDS backlight level either by I2C or PWM
+ */
+void psb_intel_lvds_set_brightness(struct drm_device *dev, int level)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+
+	dev_dbg(dev->dev, "backlight level is %d\n", level);
+
+	if (!dev_priv->lvds_bl) {
+		dev_err(dev->dev, "NO LVDS backlight info\n");
+		return;
+	}
+
+	if (dev_priv->lvds_bl->type == BLC_I2C_TYPE)
+		psb_lvds_i2c_set_brightness(dev, level);
+	else
+		psb_lvds_pwm_set_brightness(dev, level);
+}
+
+/*
+ * Sets the backlight level.
+ *
+ * level: backlight level, from 0 to psb_intel_lvds_get_max_backlight().
+ */
+static void psb_intel_lvds_set_backlight(struct drm_device *dev, int level)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 blc_pwm_ctl;
+
+	if (gma_power_begin(dev, false)) {
+		blc_pwm_ctl = REG_READ(BLC_PWM_CTL);
+		blc_pwm_ctl &= ~BACKLIGHT_DUTY_CYCLE_MASK;
+		REG_WRITE(BLC_PWM_CTL,
+				(blc_pwm_ctl |
+				(level << BACKLIGHT_DUTY_CYCLE_SHIFT)));
+		dev_priv->saveBLC_PWM_CTL = (blc_pwm_ctl |
+					(level << BACKLIGHT_DUTY_CYCLE_SHIFT));
+		gma_power_end(dev);
+	} else {
+		blc_pwm_ctl = dev_priv->saveBLC_PWM_CTL &
+				~BACKLIGHT_DUTY_CYCLE_MASK;
+		dev_priv->saveBLC_PWM_CTL = (blc_pwm_ctl |
+					(level << BACKLIGHT_DUTY_CYCLE_SHIFT));
+	}
+}
+
+/*
+ * Sets the power state for the panel.
+ */
+static void psb_intel_lvds_set_power(struct drm_device *dev, bool on)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+	u32 pp_status;
+
+	if (!gma_power_begin(dev, true)) {
+	        dev_err(dev->dev, "set power, chip off!\n");
+		return;
+        }
+        
+	if (on) {
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) |
+			  POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while ((pp_status & PP_ON) == 0);
+
+		psb_intel_lvds_set_backlight(dev,
+					     mode_dev->backlight_duty_cycle);
+	} else {
+		psb_intel_lvds_set_backlight(dev, 0);
+
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) &
+			  ~POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while (pp_status & PP_ON);
+	}
+
+	gma_power_end(dev);
+}
+
+static void psb_intel_lvds_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+
+	if (mode == DRM_MODE_DPMS_ON)
+		psb_intel_lvds_set_power(dev, true);
+	else
+		psb_intel_lvds_set_power(dev, false);
+
+	/* XXX: We never power down the LVDS pairs. */
+}
+
+static void psb_intel_lvds_save(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_psb_private *dev_priv =
+		(struct drm_psb_private *)dev->dev_private;
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct psb_intel_lvds_priv *lvds_priv =
+		(struct psb_intel_lvds_priv *)psb_intel_encoder->dev_priv;
+
+	lvds_priv->savePP_ON = REG_READ(LVDSPP_ON);
+	lvds_priv->savePP_OFF = REG_READ(LVDSPP_OFF);
+	lvds_priv->saveLVDS = REG_READ(LVDS);
+	lvds_priv->savePP_CONTROL = REG_READ(PP_CONTROL);
+	lvds_priv->savePP_CYCLE = REG_READ(PP_CYCLE);
+	/*lvds_priv->savePP_DIVISOR = REG_READ(PP_DIVISOR);*/
+	lvds_priv->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+	lvds_priv->savePFIT_CONTROL = REG_READ(PFIT_CONTROL);
+	lvds_priv->savePFIT_PGM_RATIOS = REG_READ(PFIT_PGM_RATIOS);
+
+	/*TODO: move backlight_duty_cycle to psb_intel_lvds_priv*/
+	dev_priv->backlight_duty_cycle = (dev_priv->saveBLC_PWM_CTL &
+						BACKLIGHT_DUTY_CYCLE_MASK);
+
+	/*
+	 * If the light is off at server startup,
+	 * just make it full brightness
+	 */
+	if (dev_priv->backlight_duty_cycle == 0)
+		dev_priv->backlight_duty_cycle =
+		psb_intel_lvds_get_max_backlight(dev);
+
+	dev_dbg(dev->dev, "(0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x)\n",
+			lvds_priv->savePP_ON,
+			lvds_priv->savePP_OFF,
+			lvds_priv->saveLVDS,
+			lvds_priv->savePP_CONTROL,
+			lvds_priv->savePP_CYCLE,
+			lvds_priv->saveBLC_PWM_CTL);
+}
+
+static void psb_intel_lvds_restore(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	u32 pp_status;
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct psb_intel_lvds_priv *lvds_priv =
+		(struct psb_intel_lvds_priv *)psb_intel_encoder->dev_priv;
+
+	dev_dbg(dev->dev, "(0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x)\n",
+			lvds_priv->savePP_ON,
+			lvds_priv->savePP_OFF,
+			lvds_priv->saveLVDS,
+			lvds_priv->savePP_CONTROL,
+			lvds_priv->savePP_CYCLE,
+			lvds_priv->saveBLC_PWM_CTL);
+
+	REG_WRITE(BLC_PWM_CTL, lvds_priv->saveBLC_PWM_CTL);
+	REG_WRITE(PFIT_CONTROL, lvds_priv->savePFIT_CONTROL);
+	REG_WRITE(PFIT_PGM_RATIOS, lvds_priv->savePFIT_PGM_RATIOS);
+	REG_WRITE(LVDSPP_ON, lvds_priv->savePP_ON);
+	REG_WRITE(LVDSPP_OFF, lvds_priv->savePP_OFF);
+	/*REG_WRITE(PP_DIVISOR, lvds_priv->savePP_DIVISOR);*/
+	REG_WRITE(PP_CYCLE, lvds_priv->savePP_CYCLE);
+	REG_WRITE(PP_CONTROL, lvds_priv->savePP_CONTROL);
+	REG_WRITE(LVDS, lvds_priv->saveLVDS);
+
+	if (lvds_priv->savePP_CONTROL & POWER_TARGET_ON) {
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) |
+			POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while ((pp_status & PP_ON) == 0);
+	} else {
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) &
+			~POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while (pp_status & PP_ON);
+	}
+}
+
+int psb_intel_lvds_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	struct drm_psb_private *dev_priv = connector->dev->dev_private;
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct drm_display_mode *fixed_mode =
+					dev_priv->mode_dev.panel_fixed_mode;
+
+	if (psb_intel_encoder->type == INTEL_OUTPUT_MIPI2)
+		fixed_mode = dev_priv->mode_dev.panel_fixed_mode2;
+
+	/* just in case */
+	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+		return MODE_NO_DBLESCAN;
+
+	/* just in case */
+	if (mode->flags & DRM_MODE_FLAG_INTERLACE)
+		return MODE_NO_INTERLACE;
+
+	if (fixed_mode) {
+		if (mode->hdisplay > fixed_mode->hdisplay)
+			return MODE_PANEL;
+		if (mode->vdisplay > fixed_mode->vdisplay)
+			return MODE_PANEL;
+	}
+	return MODE_OK;
+}
+
+bool psb_intel_lvds_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+	struct psb_intel_crtc *psb_intel_crtc =
+				to_psb_intel_crtc(encoder->crtc);
+	struct drm_encoder *tmp_encoder;
+	struct drm_display_mode *panel_fixed_mode = mode_dev->panel_fixed_mode;
+	struct psb_intel_encoder *psb_intel_encoder =
+						to_psb_intel_encoder(encoder);
+
+	if (psb_intel_encoder->type == INTEL_OUTPUT_MIPI2)
+		panel_fixed_mode = mode_dev->panel_fixed_mode2;
+
+	/* PSB requires the LVDS is on pipe B, MRST has only one pipe anyway */
+	if (!IS_MRST(dev) && psb_intel_crtc->pipe == 0) {
+		printk(KERN_ERR "Can't support LVDS on pipe A\n");
+		return false;
+	}
+	if (IS_MRST(dev) && psb_intel_crtc->pipe != 0) {
+		printk(KERN_ERR "Must use PIPE A\n");
+		return false;
+	}
+	/* Should never happen!! */
+	list_for_each_entry(tmp_encoder, &dev->mode_config.encoder_list,
+			    head) {
+		if (tmp_encoder != encoder
+		    && tmp_encoder->crtc == encoder->crtc) {
+			printk(KERN_ERR "Can't enable LVDS and another "
+			       "encoder on the same pipe\n");
+			return false;
+		}
+	}
+
+	/*
+	 * If we have timings from the BIOS for the panel, put them in
+	 * to the adjusted mode.  The CRTC will be set up for this mode,
+	 * with the panel scaling set up to source from the H/VDisplay
+	 * of the original mode.
+	 */
+	if (panel_fixed_mode != NULL) {
+		adjusted_mode->hdisplay = panel_fixed_mode->hdisplay;
+		adjusted_mode->hsync_start = panel_fixed_mode->hsync_start;
+		adjusted_mode->hsync_end = panel_fixed_mode->hsync_end;
+		adjusted_mode->htotal = panel_fixed_mode->htotal;
+		adjusted_mode->vdisplay = panel_fixed_mode->vdisplay;
+		adjusted_mode->vsync_start = panel_fixed_mode->vsync_start;
+		adjusted_mode->vsync_end = panel_fixed_mode->vsync_end;
+		adjusted_mode->vtotal = panel_fixed_mode->vtotal;
+		adjusted_mode->clock = panel_fixed_mode->clock;
+		drm_mode_set_crtcinfo(adjusted_mode,
+				      CRTC_INTERLACE_HALVE_V);
+	}
+
+	/*
+	 * XXX: It would be nice to support lower refresh rates on the
+	 * panels to reduce power consumption, and perhaps match the
+	 * user's requested refresh rate.
+	 */
+
+	return true;
+}
+
+static void psb_intel_lvds_prepare(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+
+	if (!gma_power_begin(dev, true))
+		return;
+
+	mode_dev->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+	mode_dev->backlight_duty_cycle = (mode_dev->saveBLC_PWM_CTL &
+					  BACKLIGHT_DUTY_CYCLE_MASK);
+
+	psb_intel_lvds_set_power(dev, false);
+
+	gma_power_end(dev);
+}
+
+static void psb_intel_lvds_commit(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+
+	if (mode_dev->backlight_duty_cycle == 0)
+		mode_dev->backlight_duty_cycle =
+		    psb_intel_lvds_get_max_backlight(dev);
+
+	psb_intel_lvds_set_power(dev, true);
+}
+
+static void psb_intel_lvds_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 pfit_control;
+
+	/*
+	 * The LVDS pin pair will already have been turned on in the
+	 * psb_intel_crtc_mode_set since it has a large impact on the DPLL
+	 * settings.
+	 */
+
+	/*
+	 * Enable automatic panel scaling so that non-native modes fill the
+	 * screen.  Should be enabled before the pipe is enabled, according to
+	 * register description and PRM.
+	 */
+	if (mode->hdisplay != adjusted_mode->hdisplay ||
+	    mode->vdisplay != adjusted_mode->vdisplay)
+		pfit_control = (PFIT_ENABLE | VERT_AUTO_SCALE |
+				HORIZ_AUTO_SCALE | VERT_INTERP_BILINEAR |
+				HORIZ_INTERP_BILINEAR);
+	else
+		pfit_control = 0;
+
+	if (dev_priv->lvds_dither)
+		pfit_control |= PANEL_8TO6_DITHER_ENABLE;
+
+	REG_WRITE(PFIT_CONTROL, pfit_control);
+}
+
+/*
+ * Detect the LVDS connection.
+ *
+ * This always returns CONNECTOR_STATUS_CONNECTED.
+ * This connector should only have
+ * been set up if the LVDS was actually connected anyway.
+ */
+static enum drm_connector_status psb_intel_lvds_detect(struct drm_connector
+						   *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+/*
+ * Return the list of DDC modes if available, or the BIOS fixed mode otherwise.
+ */
+static int psb_intel_lvds_get_modes(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct psb_intel_lvds_priv *lvds_priv = psb_intel_encoder->dev_priv;
+	int ret = 0;
+
+	if (!IS_MRST(dev))
+		ret = psb_intel_ddc_get_modes(connector, &lvds_priv->i2c_bus->adapter);
+
+	if (ret)
+		return ret;
+
+	/* Didn't get an EDID, so
+	 * Set wide sync ranges so we get all modes
+	 * handed to valid_mode for checking
+	 */
+	connector->display_info.min_vfreq = 0;
+	connector->display_info.max_vfreq = 200;
+	connector->display_info.min_hfreq = 0;
+	connector->display_info.max_hfreq = 200;
+
+	if (mode_dev->panel_fixed_mode != NULL) {
+		struct drm_display_mode *mode =
+		    drm_mode_duplicate(dev, mode_dev->panel_fixed_mode);
+		drm_mode_probed_add(connector, mode);
+		return 1;
+	}
+
+	return 0;
+}
+
+/**
+ * psb_intel_lvds_destroy - unregister and free LVDS structures
+ * @connector: connector to free
+ *
+ * Unregister the DDC bus for this connector then free the driver private
+ * structure.
+ */
+void psb_intel_lvds_destroy(struct drm_connector *connector)
+{
+	struct psb_intel_encoder *psb_intel_encoder =
+					psb_intel_attached_encoder(connector);
+	struct psb_intel_lvds_priv *lvds_priv = psb_intel_encoder->dev_priv;
+
+	if (lvds_priv->ddc_bus)
+		psb_intel_i2c_destroy(lvds_priv->ddc_bus);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+int psb_intel_lvds_set_property(struct drm_connector *connector,
+				       struct drm_property *property,
+				       uint64_t value)
+{
+	struct drm_encoder *encoder = connector->encoder;
+
+	if (!encoder)
+		return -1;
+
+	if (!strcmp(property->name, "scaling mode")) {
+		struct psb_intel_crtc *crtc =
+					to_psb_intel_crtc(encoder->crtc);
+		uint64_t curval;
+
+		if (!crtc)
+			goto set_prop_error;
+
+		switch (value) {
+		case DRM_MODE_SCALE_FULLSCREEN:
+			break;
+		case DRM_MODE_SCALE_NO_SCALE:
+			break;
+		case DRM_MODE_SCALE_ASPECT:
+			break;
+		default:
+			goto set_prop_error;
+		}
+
+		if (drm_connector_property_get_value(connector,
+						     property,
+						     &curval))
+			goto set_prop_error;
+
+		if (curval == value)
+			goto set_prop_done;
+
+		if (drm_connector_property_set_value(connector,
+							property,
+							value))
+			goto set_prop_error;
+
+		if (crtc->saved_mode.hdisplay != 0 &&
+		    crtc->saved_mode.vdisplay != 0) {
+			if (!drm_crtc_helper_set_mode(encoder->crtc,
+						      &crtc->saved_mode,
+						      encoder->crtc->x,
+						      encoder->crtc->y,
+						      encoder->crtc->fb))
+				goto set_prop_error;
+		}
+	} else if (!strcmp(property->name, "backlight")) {
+		if (drm_connector_property_set_value(connector,
+							property,
+							value))
+			goto set_prop_error;
+		else {
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+			struct drm_psb_private *devp =
+						encoder->dev->dev_private;
+			struct backlight_device *bd = devp->backlight_device;
+			if (bd) {
+				bd->props.brightness = value;
+				backlight_update_status(bd);
+			}
+#endif
+		}
+	} else if (!strcmp(property->name, "DPMS")) {
+		struct drm_encoder_helper_funcs *hfuncs
+						= encoder->helper_private;
+		hfuncs->dpms(encoder, value);
+	}
+
+set_prop_done:
+	return 0;
+set_prop_error:
+	return -1;
+}
+
+static const struct drm_encoder_helper_funcs psb_intel_lvds_helper_funcs = {
+	.dpms = psb_intel_lvds_encoder_dpms,
+	.mode_fixup = psb_intel_lvds_mode_fixup,
+	.prepare = psb_intel_lvds_prepare,
+	.mode_set = psb_intel_lvds_mode_set,
+	.commit = psb_intel_lvds_commit,
+};
+
+const struct drm_connector_helper_funcs
+				psb_intel_lvds_connector_helper_funcs = {
+	.get_modes = psb_intel_lvds_get_modes,
+	.mode_valid = psb_intel_lvds_mode_valid,
+	.best_encoder = psb_intel_best_encoder,
+};
+
+const struct drm_connector_funcs psb_intel_lvds_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.save = psb_intel_lvds_save,
+	.restore = psb_intel_lvds_restore,
+	.detect = psb_intel_lvds_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.set_property = psb_intel_lvds_set_property,
+	.destroy = psb_intel_lvds_destroy,
+};
+
+
+static void psb_intel_lvds_enc_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+}
+
+const struct drm_encoder_funcs psb_intel_lvds_enc_funcs = {
+	.destroy = psb_intel_lvds_enc_destroy,
+};
+
+
+
+/**
+ * psb_intel_lvds_init - setup LVDS connectors on this device
+ * @dev: drm device
+ *
+ * Create the connector, register the LVDS DDC bus, and try to figure out what
+ * modes we can display on the LVDS panel (if present).
+ */
+void psb_intel_lvds_init(struct drm_device *dev,
+			 struct psb_intel_mode_device *mode_dev)
+{
+	struct psb_intel_encoder *psb_intel_encoder;
+	struct psb_intel_connector *psb_intel_connector;
+	struct psb_intel_lvds_priv *lvds_priv;
+	struct drm_connector *connector;
+	struct drm_encoder *encoder;
+	struct drm_display_mode *scan;	/* *modes, *bios_mode; */
+	struct drm_crtc *crtc;
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	u32 lvds;
+	int pipe;
+
+	psb_intel_encoder =
+			kzalloc(sizeof(struct psb_intel_encoder), GFP_KERNEL);
+
+	if (!psb_intel_encoder) {
+		dev_err(dev->dev, "psb_intel_encoder allocation error\n");
+		return;
+	}
+
+	psb_intel_connector =
+		kzalloc(sizeof(struct psb_intel_connector), GFP_KERNEL);
+
+	if (!psb_intel_connector) {
+		kfree(psb_intel_encoder);
+		dev_err(dev->dev, "psb_intel_connector allocation error\n");
+	}
+
+	lvds_priv = kzalloc(sizeof(struct psb_intel_lvds_priv), GFP_KERNEL);
+	if (!lvds_priv) {
+		dev_err(dev->dev, "LVDS private allocation error\n");
+		goto failed_connector;
+	}
+
+	psb_intel_encoder->dev_priv = lvds_priv;
+
+	connector = &psb_intel_connector->base;
+	encoder = &psb_intel_encoder->base;
+	drm_connector_init(dev, connector,
+			   &psb_intel_lvds_connector_funcs,
+			   DRM_MODE_CONNECTOR_LVDS);
+
+	drm_encoder_init(dev, encoder,
+			 &psb_intel_lvds_enc_funcs,
+			 DRM_MODE_ENCODER_LVDS);
+
+	psb_intel_connector_attach_encoder(psb_intel_connector,
+					   psb_intel_encoder);
+	psb_intel_encoder->type = INTEL_OUTPUT_LVDS;
+
+	drm_encoder_helper_add(encoder, &psb_intel_lvds_helper_funcs);
+	drm_connector_helper_add(connector,
+				 &psb_intel_lvds_connector_helper_funcs);
+	connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+	connector->interlace_allowed = false;
+	connector->doublescan_allowed = false;
+
+	/*Attach connector properties*/
+	drm_connector_attach_property(connector,
+				      dev->mode_config.scaling_mode_property,
+				      DRM_MODE_SCALE_FULLSCREEN);
+	drm_connector_attach_property(connector,
+				      dev_priv->backlight_property,
+				      BRIGHTNESS_MAX_LEVEL);
+
+	/*
+	 * Set up I2C bus
+	 * FIXME: distroy i2c_bus when exit
+	 */
+	lvds_priv->i2c_bus = psb_intel_i2c_create(dev, GPIOB, "LVDSBLC_B");
+	if (!lvds_priv->i2c_bus) {
+		dev_printk(KERN_ERR,
+			&dev->pdev->dev, "I2C bus registration failed.\n");
+		goto failed_blc_i2c;
+	}
+	lvds_priv->i2c_bus->slave_addr = 0x2C;
+	dev_priv->lvds_i2c_bus =  lvds_priv->i2c_bus;
+
+	/*
+	 * LVDS discovery:
+	 * 1) check for EDID on DDC
+	 * 2) check for VBT data
+	 * 3) check to see if LVDS is already on
+	 *    if none of the above, no panel
+	 * 4) make sure lid is open
+	 *    if closed, act like it's not there for now
+	 */
+
+	/* Set up the DDC bus. */
+	lvds_priv->ddc_bus = psb_intel_i2c_create(dev, GPIOC, "LVDSDDC_C");
+	if (!lvds_priv->ddc_bus) {
+		dev_printk(KERN_ERR, &dev->pdev->dev,
+			   "DDC bus registration " "failed.\n");
+		goto failed_ddc;
+	}
+
+	/*
+	 * Attempt to get the fixed panel mode from DDC.  Assume that the
+	 * preferred mode is the right one.
+	 */
+	psb_intel_ddc_get_modes(connector, &lvds_priv->ddc_bus->adapter);
+	list_for_each_entry(scan, &connector->probed_modes, head) {
+		if (scan->type & DRM_MODE_TYPE_PREFERRED) {
+			mode_dev->panel_fixed_mode =
+			    drm_mode_duplicate(dev, scan);
+			goto out;	/* FIXME: check for quirks */
+		}
+	}
+
+	/* Failed to get EDID, what about VBT? do we need this? */
+	if (mode_dev->vbt_mode)
+		mode_dev->panel_fixed_mode =
+		    drm_mode_duplicate(dev, mode_dev->vbt_mode);
+
+	if (!mode_dev->panel_fixed_mode)
+		if (dev_priv->lfp_lvds_vbt_mode)
+			mode_dev->panel_fixed_mode =
+				drm_mode_duplicate(dev,
+					dev_priv->lfp_lvds_vbt_mode);
+
+	/*
+	 * If we didn't get EDID, try checking if the panel is already turned
+	 * on.	If so, assume that whatever is currently programmed is the
+	 * correct mode.
+	 */
+	lvds = REG_READ(LVDS);
+	pipe = (lvds & LVDS_PIPEB_SELECT) ? 1 : 0;
+	crtc = psb_intel_get_crtc_from_pipe(dev, pipe);
+
+	if (crtc && (lvds & LVDS_PORT_EN)) {
+		mode_dev->panel_fixed_mode =
+		    psb_intel_crtc_mode_get(dev, crtc);
+		if (mode_dev->panel_fixed_mode) {
+			mode_dev->panel_fixed_mode->type |=
+			    DRM_MODE_TYPE_PREFERRED;
+			goto out;	/* FIXME: check for quirks */
+		}
+	}
+
+	/* If we still don't have a mode after all that, give up. */
+	if (!mode_dev->panel_fixed_mode) {
+		dev_err(dev->dev, "Found no modes on the lvds, ignoring the LVDS\n");
+		goto failed_find;
+	}
+
+	/*
+	 * Blacklist machines with BIOSes that list an LVDS panel without
+	 * actually having one.
+	 */
+out:
+	drm_sysfs_connector_add(connector);
+	return;
+
+failed_find:
+	if (lvds_priv->ddc_bus)
+		psb_intel_i2c_destroy(lvds_priv->ddc_bus);
+failed_ddc:
+	if (lvds_priv->i2c_bus)
+		psb_intel_i2c_destroy(lvds_priv->i2c_bus);
+failed_blc_i2c:
+	drm_encoder_cleanup(encoder);
+	drm_connector_cleanup(connector);
+failed_connector:
+	if (psb_intel_connector)
+		kfree(psb_intel_connector);
+}
+
diff --git a/drivers/gpu/drm/gma500/psb_intel_modes.c b/drivers/gpu/drm/gma500/psb_intel_modes.c
new file mode 100644
index 000000000000..4fca0d6feebe
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_intel_modes.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2007 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authers: Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <linux/fb.h>
+#include <drm/drmP.h>
+#include "psb_intel_drv.h"
+
+/**
+ * psb_intel_ddc_probe
+ *
+ */
+bool psb_intel_ddc_probe(struct i2c_adapter *adapter)
+{
+	u8 out_buf[] = { 0x0, 0x0 };
+	u8 buf[2];
+	int ret;
+	struct i2c_msg msgs[] = {
+		{
+		 .addr = 0x50,
+		 .flags = 0,
+		 .len = 1,
+		 .buf = out_buf,
+		 },
+		{
+		 .addr = 0x50,
+		 .flags = I2C_M_RD,
+		 .len = 1,
+		 .buf = buf,
+		 }
+	};
+
+	ret = i2c_transfer(adapter, msgs, 2);
+	if (ret == 2)
+		return true;
+
+	return false;
+}
+
+/**
+ * psb_intel_ddc_get_modes - get modelist from monitor
+ * @connector: DRM connector device to use
+ *
+ * Fetch the EDID information from @connector using the DDC bus.
+ */
+int psb_intel_ddc_get_modes(struct drm_connector *connector,
+			    struct i2c_adapter *adapter)
+{
+	struct edid *edid;
+	int ret = 0;
+
+	edid = drm_get_edid(connector, adapter);
+	if (edid) {
+		drm_mode_connector_update_edid_property(connector, edid);
+		ret = drm_add_edid_modes(connector, edid);
+		kfree(edid);
+	}
+	return ret;
+}
diff --git a/drivers/gpu/drm/gma500/psb_intel_reg.h b/drivers/gpu/drm/gma500/psb_intel_reg.h
new file mode 100644
index 000000000000..fcc0af03d685
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_intel_reg.h
@@ -0,0 +1,1309 @@
+/*
+ * Copyright (c) 2009, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#ifndef __PSB_INTEL_REG_H__
+#define __PSB_INTEL_REG_H__
+
+/*
+ * GPIO regs
+ */
+#define GPIOA			0x5010
+#define GPIOB			0x5014
+#define GPIOC			0x5018
+#define GPIOD			0x501c
+#define GPIOE			0x5020
+#define GPIOF			0x5024
+#define GPIOG			0x5028
+#define GPIOH			0x502c
+# define GPIO_CLOCK_DIR_MASK		(1 << 0)
+# define GPIO_CLOCK_DIR_IN		(0 << 1)
+# define GPIO_CLOCK_DIR_OUT		(1 << 1)
+# define GPIO_CLOCK_VAL_MASK		(1 << 2)
+# define GPIO_CLOCK_VAL_OUT		(1 << 3)
+# define GPIO_CLOCK_VAL_IN		(1 << 4)
+# define GPIO_CLOCK_PULLUP_DISABLE	(1 << 5)
+# define GPIO_DATA_DIR_MASK		(1 << 8)
+# define GPIO_DATA_DIR_IN		(0 << 9)
+# define GPIO_DATA_DIR_OUT		(1 << 9)
+# define GPIO_DATA_VAL_MASK		(1 << 10)
+# define GPIO_DATA_VAL_OUT		(1 << 11)
+# define GPIO_DATA_VAL_IN		(1 << 12)
+# define GPIO_DATA_PULLUP_DISABLE	(1 << 13)
+
+#define GMBUS0			0x5100 /* clock/port select */
+#define   GMBUS_RATE_100KHZ	(0<<8)
+#define   GMBUS_RATE_50KHZ	(1<<8)
+#define   GMBUS_RATE_400KHZ	(2<<8) /* reserved on Pineview */
+#define   GMBUS_RATE_1MHZ	(3<<8) /* reserved on Pineview */
+#define   GMBUS_HOLD_EXT	(1<<7) /* 300ns hold time, rsvd on Pineview */
+#define   GMBUS_PORT_DISABLED	0
+#define   GMBUS_PORT_SSC	1
+#define   GMBUS_PORT_VGADDC	2
+#define   GMBUS_PORT_PANEL	3
+#define   GMBUS_PORT_DPC	4 /* HDMIC */
+#define   GMBUS_PORT_DPB	5 /* SDVO, HDMIB */
+				  /* 6 reserved */
+#define   GMBUS_PORT_DPD	7 /* HDMID */
+#define   GMBUS_NUM_PORTS       8
+#define GMBUS1			0x5104 /* command/status */
+#define   GMBUS_SW_CLR_INT	(1<<31)
+#define   GMBUS_SW_RDY		(1<<30)
+#define   GMBUS_ENT		(1<<29) /* enable timeout */
+#define   GMBUS_CYCLE_NONE	(0<<25)
+#define   GMBUS_CYCLE_WAIT	(1<<25)
+#define   GMBUS_CYCLE_INDEX	(2<<25)
+#define   GMBUS_CYCLE_STOP	(4<<25)
+#define   GMBUS_BYTE_COUNT_SHIFT 16
+#define   GMBUS_SLAVE_INDEX_SHIFT 8
+#define   GMBUS_SLAVE_ADDR_SHIFT 1
+#define   GMBUS_SLAVE_READ	(1<<0)
+#define   GMBUS_SLAVE_WRITE	(0<<0)
+#define GMBUS2			0x5108 /* status */
+#define   GMBUS_INUSE		(1<<15)
+#define   GMBUS_HW_WAIT_PHASE	(1<<14)
+#define   GMBUS_STALL_TIMEOUT	(1<<13)
+#define   GMBUS_INT		(1<<12)
+#define   GMBUS_HW_RDY		(1<<11)
+#define   GMBUS_SATOER		(1<<10)
+#define   GMBUS_ACTIVE		(1<<9)
+#define GMBUS3			0x510c /* data buffer bytes 3-0 */
+#define GMBUS4			0x5110 /* interrupt mask (Pineview+) */
+#define   GMBUS_SLAVE_TIMEOUT_EN (1<<4)
+#define   GMBUS_NAK_EN		(1<<3)
+#define   GMBUS_IDLE_EN		(1<<2)
+#define   GMBUS_HW_WAIT_EN	(1<<1)
+#define   GMBUS_HW_RDY_EN	(1<<0)
+#define GMBUS5			0x5120 /* byte index */
+#define   GMBUS_2BYTE_INDEX_EN	(1<<31)
+
+#define BLC_PWM_CTL		0x61254
+#define BLC_PWM_CTL2		0x61250
+#define BLC_PWM_CTL_C		0x62254
+#define BLC_PWM_CTL2_C		0x62250
+#define BACKLIGHT_MODULATION_FREQ_SHIFT		(17)
+/*
+ * This is the most significant 15 bits of the number of backlight cycles in a
+ * complete cycle of the modulated backlight control.
+ *
+ * The actual value is this field multiplied by two.
+ */
+#define BACKLIGHT_MODULATION_FREQ_MASK	(0x7fff << 17)
+#define BLM_LEGACY_MODE			(1 << 16)
+/*
+ * This is the number of cycles out of the backlight modulation cycle for which
+ * the backlight is on.
+ *
+ * This field must be no greater than the number of cycles in the complete
+ * backlight modulation cycle.
+ */
+#define BACKLIGHT_DUTY_CYCLE_SHIFT	(0)
+#define BACKLIGHT_DUTY_CYCLE_MASK	(0xffff)
+
+#define I915_GCFGC			0xf0
+#define I915_LOW_FREQUENCY_ENABLE	(1 << 7)
+#define I915_DISPLAY_CLOCK_190_200_MHZ	(0 << 4)
+#define I915_DISPLAY_CLOCK_333_MHZ	(4 << 4)
+#define I915_DISPLAY_CLOCK_MASK		(7 << 4)
+
+#define I855_HPLLCC			0xc0
+#define I855_CLOCK_CONTROL_MASK		(3 << 0)
+#define I855_CLOCK_133_200		(0 << 0)
+#define I855_CLOCK_100_200		(1 << 0)
+#define I855_CLOCK_100_133		(2 << 0)
+#define I855_CLOCK_166_250		(3 << 0)
+
+/* I830 CRTC registers */
+#define HTOTAL_A		0x60000
+#define HBLANK_A		0x60004
+#define HSYNC_A			0x60008
+#define VTOTAL_A		0x6000c
+#define VBLANK_A		0x60010
+#define VSYNC_A			0x60014
+#define PIPEASRC		0x6001c
+#define BCLRPAT_A		0x60020
+#define VSYNCSHIFT_A		0x60028
+
+#define HTOTAL_B		0x61000
+#define HBLANK_B		0x61004
+#define HSYNC_B			0x61008
+#define VTOTAL_B		0x6100c
+#define VBLANK_B		0x61010
+#define VSYNC_B			0x61014
+#define PIPEBSRC		0x6101c
+#define BCLRPAT_B		0x61020
+#define VSYNCSHIFT_B		0x61028
+
+#define HTOTAL_C		0x62000
+#define HBLANK_C		0x62004
+#define HSYNC_C			0x62008
+#define VTOTAL_C		0x6200c
+#define VBLANK_C		0x62010
+#define VSYNC_C			0x62014
+#define PIPECSRC		0x6201c
+#define BCLRPAT_C		0x62020
+#define VSYNCSHIFT_C		0x62028
+
+#define PP_STATUS		0x61200
+# define PP_ON				(1 << 31)
+/*
+ * Indicates that all dependencies of the panel are on:
+ *
+ * - PLL enabled
+ * - pipe enabled
+ * - LVDS/DVOB/DVOC on
+ */
+#define PP_READY			(1 << 30)
+#define PP_SEQUENCE_NONE		(0 << 28)
+#define PP_SEQUENCE_ON			(1 << 28)
+#define PP_SEQUENCE_OFF			(2 << 28)
+#define PP_SEQUENCE_MASK		0x30000000
+#define PP_CONTROL		0x61204
+#define POWER_TARGET_ON			(1 << 0)
+
+#define LVDSPP_ON		0x61208
+#define LVDSPP_OFF		0x6120c
+#define PP_CYCLE		0x61210
+
+#define PFIT_CONTROL		0x61230
+#define PFIT_ENABLE			(1 << 31)
+#define PFIT_PIPE_MASK			(3 << 29)
+#define PFIT_PIPE_SHIFT			29
+#define PFIT_SCALING_MODE_PILLARBOX	(1 << 27)
+#define PFIT_SCALING_MODE_LETTERBOX	(3 << 26)
+#define VERT_INTERP_DISABLE		(0 << 10)
+#define VERT_INTERP_BILINEAR		(1 << 10)
+#define VERT_INTERP_MASK		(3 << 10)
+#define VERT_AUTO_SCALE			(1 << 9)
+#define HORIZ_INTERP_DISABLE		(0 << 6)
+#define HORIZ_INTERP_BILINEAR		(1 << 6)
+#define HORIZ_INTERP_MASK		(3 << 6)
+#define HORIZ_AUTO_SCALE		(1 << 5)
+#define PANEL_8TO6_DITHER_ENABLE	(1 << 3)
+
+#define PFIT_PGM_RATIOS		0x61234
+#define PFIT_VERT_SCALE_MASK			0xfff00000
+#define PFIT_HORIZ_SCALE_MASK			0x0000fff0
+
+#define PFIT_AUTO_RATIOS	0x61238
+
+#define DPLL_A			0x06014
+#define DPLL_B			0x06018
+#define DPLL_VCO_ENABLE			(1 << 31)
+#define DPLL_DVO_HIGH_SPEED		(1 << 30)
+#define DPLL_SYNCLOCK_ENABLE		(1 << 29)
+#define DPLL_VGA_MODE_DIS		(1 << 28)
+#define DPLLB_MODE_DAC_SERIAL		(1 << 26)	/* i915 */
+#define DPLLB_MODE_LVDS			(2 << 26)	/* i915 */
+#define DPLL_MODE_MASK			(3 << 26)
+#define DPLL_DAC_SERIAL_P2_CLOCK_DIV_10	(0 << 24)	/* i915 */
+#define DPLL_DAC_SERIAL_P2_CLOCK_DIV_5	(1 << 24)	/* i915 */
+#define DPLLB_LVDS_P2_CLOCK_DIV_14	(0 << 24)	/* i915 */
+#define DPLLB_LVDS_P2_CLOCK_DIV_7	(1 << 24)	/* i915 */
+#define DPLL_P2_CLOCK_DIV_MASK		0x03000000	/* i915 */
+#define DPLL_FPA01_P1_POST_DIV_MASK	0x00ff0000	/* i915 */
+#define DPLL_LOCK			(1 << 15)	/* CDV */
+
+/*
+ *  The i830 generation, in DAC/serial mode, defines p1 as two plus this
+ * bitfield, or just 2 if PLL_P1_DIVIDE_BY_TWO is set.
+ */
+# define DPLL_FPA01_P1_POST_DIV_MASK_I830	0x001f0000
+/*
+ * The i830 generation, in LVDS mode, defines P1 as the bit number set within
+ * this field (only one bit may be set).
+ */
+#define DPLL_FPA01_P1_POST_DIV_MASK_I830_LVDS	0x003f0000
+#define DPLL_FPA01_P1_POST_DIV_SHIFT	16
+#define PLL_P2_DIVIDE_BY_4		(1 << 23)	/* i830, required
+							 * in DVO non-gang */
+# define PLL_P1_DIVIDE_BY_TWO		(1 << 21)	/* i830 */
+#define PLL_REF_INPUT_DREFCLK		(0 << 13)
+#define PLL_REF_INPUT_TVCLKINA		(1 << 13)	/* i830 */
+#define PLL_REF_INPUT_TVCLKINBC		(2 << 13)	/* SDVO
+								 * TVCLKIN */
+#define PLLB_REF_INPUT_SPREADSPECTRUMIN	(3 << 13)
+#define PLL_REF_INPUT_MASK		(3 << 13)
+#define PLL_LOAD_PULSE_PHASE_SHIFT	9
+/*
+ * Parallel to Serial Load Pulse phase selection.
+ * Selects the phase for the 10X DPLL clock for the PCIe
+ * digital display port. The range is 4 to 13; 10 or more
+ * is just a flip delay. The default is 6
+ */
+#define PLL_LOAD_PULSE_PHASE_MASK	(0xf << PLL_LOAD_PULSE_PHASE_SHIFT)
+#define DISPLAY_RATE_SELECT_FPA1	(1 << 8)
+
+/*
+ * SDVO multiplier for 945G/GM. Not used on 965.
+ *
+ * DPLL_MD_UDI_MULTIPLIER_MASK
+ */
+#define SDVO_MULTIPLIER_MASK		0x000000ff
+#define SDVO_MULTIPLIER_SHIFT_HIRES	4
+#define SDVO_MULTIPLIER_SHIFT_VGA	0
+
+/*
+ * PLL_MD
+ */
+/* Pipe A SDVO/UDI clock multiplier/divider register for G965. */
+#define DPLL_A_MD		0x0601c
+/* Pipe B SDVO/UDI clock multiplier/divider register for G965. */
+#define DPLL_B_MD		0x06020
+/*
+ * UDI pixel divider, controlling how many pixels are stuffed into a packet.
+ *
+ * Value is pixels minus 1.  Must be set to 1 pixel for SDVO.
+ */
+#define DPLL_MD_UDI_DIVIDER_MASK	0x3f000000
+#define DPLL_MD_UDI_DIVIDER_SHIFT	24
+/* UDI pixel divider for VGA, same as DPLL_MD_UDI_DIVIDER_MASK. */
+#define DPLL_MD_VGA_UDI_DIVIDER_MASK	0x003f0000
+#define DPLL_MD_VGA_UDI_DIVIDER_SHIFT	16
+/*
+ * SDVO/UDI pixel multiplier.
+ *
+ * SDVO requires that the bus clock rate be between 1 and 2 Ghz, and the bus
+ * clock rate is 10 times the DPLL clock.  At low resolution/refresh rate
+ * modes, the bus rate would be below the limits, so SDVO allows for stuffing
+ * dummy bytes in the datastream at an increased clock rate, with both sides of
+ * the link knowing how many bytes are fill.
+ *
+ * So, for a mode with a dotclock of 65Mhz, we would want to double the clock
+ * rate to 130Mhz to get a bus rate of 1.30Ghz.  The DPLL clock rate would be
+ * set to 130Mhz, and the SDVO multiplier set to 2x in this register and
+ * through an SDVO command.
+ *
+ * This register field has values of multiplication factor minus 1, with
+ * a maximum multiplier of 5 for SDVO.
+ */
+#define DPLL_MD_UDI_MULTIPLIER_MASK	0x00003f00
+#define DPLL_MD_UDI_MULTIPLIER_SHIFT	8
+/*
+ * SDVO/UDI pixel multiplier for VGA, same as DPLL_MD_UDI_MULTIPLIER_MASK.
+ * This best be set to the default value (3) or the CRT won't work. No,
+ * I don't entirely understand what this does...
+ */
+#define DPLL_MD_VGA_UDI_MULTIPLIER_MASK	0x0000003f
+#define DPLL_MD_VGA_UDI_MULTIPLIER_SHIFT 0
+
+#define DPLL_TEST		0x606c
+#define DPLLB_TEST_SDVO_DIV_1		(0 << 22)
+#define DPLLB_TEST_SDVO_DIV_2		(1 << 22)
+#define DPLLB_TEST_SDVO_DIV_4		(2 << 22)
+#define DPLLB_TEST_SDVO_DIV_MASK	(3 << 22)
+#define DPLLB_TEST_N_BYPASS		(1 << 19)
+#define DPLLB_TEST_M_BYPASS		(1 << 18)
+#define DPLLB_INPUT_BUFFER_ENABLE	(1 << 16)
+#define DPLLA_TEST_N_BYPASS		(1 << 3)
+#define DPLLA_TEST_M_BYPASS		(1 << 2)
+#define DPLLA_INPUT_BUFFER_ENABLE	(1 << 0)
+
+#define ADPA			0x61100
+#define ADPA_DAC_ENABLE			(1 << 31)
+#define ADPA_DAC_DISABLE		0
+#define ADPA_PIPE_SELECT_MASK		(1 << 30)
+#define ADPA_PIPE_A_SELECT		0
+#define ADPA_PIPE_B_SELECT		(1 << 30)
+#define ADPA_USE_VGA_HVPOLARITY		(1 << 15)
+#define ADPA_SETS_HVPOLARITY		0
+#define ADPA_VSYNC_CNTL_DISABLE		(1 << 11)
+#define ADPA_VSYNC_CNTL_ENABLE		0
+#define ADPA_HSYNC_CNTL_DISABLE		(1 << 10)
+#define ADPA_HSYNC_CNTL_ENABLE		0
+#define ADPA_VSYNC_ACTIVE_HIGH		(1 << 4)
+#define ADPA_VSYNC_ACTIVE_LOW		0
+#define ADPA_HSYNC_ACTIVE_HIGH		(1 << 3)
+#define ADPA_HSYNC_ACTIVE_LOW		0
+
+#define FPA0			0x06040
+#define FPA1			0x06044
+#define FPB0			0x06048
+#define FPB1			0x0604c
+#define FP_N_DIV_MASK			0x003f0000
+#define FP_N_DIV_SHIFT			16
+#define FP_M1_DIV_MASK			0x00003f00
+#define FP_M1_DIV_SHIFT			8
+#define FP_M2_DIV_MASK			0x0000003f
+#define FP_M2_DIV_SHIFT			0
+
+#define PORT_HOTPLUG_EN		0x61110
+#define SDVOB_HOTPLUG_INT_EN		(1 << 26)
+#define SDVOC_HOTPLUG_INT_EN		(1 << 25)
+#define TV_HOTPLUG_INT_EN		(1 << 18)
+#define CRT_HOTPLUG_INT_EN		(1 << 9)
+#define CRT_HOTPLUG_FORCE_DETECT	(1 << 3)
+/* CDV.. */
+#define CRT_HOTPLUG_ACTIVATION_PERIOD_64	(1 << 8)
+#define CRT_HOTPLUG_DAC_ON_TIME_2M		(0 << 7)
+#define CRT_HOTPLUG_DAC_ON_TIME_4M		(1 << 7)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_40		(0 << 5)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_50		(1 << 5)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_60		(2 << 5)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_70		(3 << 5)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_MASK	(3 << 5)
+#define CRT_HOTPLUG_DETECT_DELAY_1G		(0 << 4)
+#define CRT_HOTPLUG_DETECT_DELAY_2G		(1 << 4)
+#define CRT_HOTPLUG_DETECT_VOLTAGE_325MV	(0 << 2)
+#define CRT_HOTPLUG_DETECT_VOLTAGE_475MV	(1 << 2)
+#define CRT_HOTPLUG_DETECT_MASK			0x000000F8
+
+#define PORT_HOTPLUG_STAT	0x61114
+#define CRT_HOTPLUG_INT_STATUS		(1 << 11)
+#define TV_HOTPLUG_INT_STATUS		(1 << 10)
+#define CRT_HOTPLUG_MONITOR_MASK	(3 << 8)
+#define CRT_HOTPLUG_MONITOR_COLOR	(3 << 8)
+#define CRT_HOTPLUG_MONITOR_MONO	(2 << 8)
+#define CRT_HOTPLUG_MONITOR_NONE	(0 << 8)
+#define SDVOC_HOTPLUG_INT_STATUS	(1 << 7)
+#define SDVOB_HOTPLUG_INT_STATUS	(1 << 6)
+
+#define SDVOB			0x61140
+#define SDVOC			0x61160
+#define SDVO_ENABLE			(1 << 31)
+#define SDVO_PIPE_B_SELECT		(1 << 30)
+#define SDVO_STALL_SELECT		(1 << 29)
+#define SDVO_INTERRUPT_ENABLE		(1 << 26)
+#define SDVO_COLOR_RANGE_16_235		(1 << 8)
+#define SDVO_AUDIO_ENABLE		(1 << 6)
+
+/**
+ * 915G/GM SDVO pixel multiplier.
+ *
+ * Programmed value is multiplier - 1, up to 5x.
+ *
+ * DPLL_MD_UDI_MULTIPLIER_MASK
+ */
+#define SDVO_PORT_MULTIPLY_MASK		(7 << 23)
+#define SDVO_PORT_MULTIPLY_SHIFT	23
+#define SDVO_PHASE_SELECT_MASK		(15 << 19)
+#define SDVO_PHASE_SELECT_DEFAULT	(6 << 19)
+#define SDVO_CLOCK_OUTPUT_INVERT	(1 << 18)
+#define SDVOC_GANG_MODE			(1 << 16)
+#define SDVO_BORDER_ENABLE		(1 << 7)
+#define SDVOB_PCIE_CONCURRENCY		(1 << 3)
+#define SDVO_DETECTED			(1 << 2)
+/* Bits to be preserved when writing */
+#define SDVOB_PRESERVE_MASK		((1 << 17) | (1 << 16) | (1 << 14))
+#define SDVOC_PRESERVE_MASK		(1 << 17)
+
+/*
+ * This register controls the LVDS output enable, pipe selection, and data
+ * format selection.
+ *
+ * All of the clock/data pairs are force powered down by power sequencing.
+ */
+#define LVDS			0x61180
+/*
+ * Enables the LVDS port.  This bit must be set before DPLLs are enabled, as
+ * the DPLL semantics change when the LVDS is assigned to that pipe.
+ */
+#define LVDS_PORT_EN			(1 << 31)
+/* Selects pipe B for LVDS data.  Must be set on pre-965. */
+#define LVDS_PIPEB_SELECT		(1 << 30)
+
+/* Turns on border drawing to allow centered display. */
+#define LVDS_BORDER_EN			(1 << 15)
+
+/*
+ * Enables the A0-A2 data pairs and CLKA, containing 18 bits of color data per
+ * pixel.
+ */
+#define LVDS_A0A2_CLKA_POWER_MASK	(3 << 8)
+#define LVDS_A0A2_CLKA_POWER_DOWN	(0 << 8)
+#define LVDS_A0A2_CLKA_POWER_UP		(3 << 8)
+/*
+ * Controls the A3 data pair, which contains the additional LSBs for 24 bit
+ * mode.  Only enabled if LVDS_A0A2_CLKA_POWER_UP also indicates it should be
+ * on.
+ */
+#define LVDS_A3_POWER_MASK		(3 << 6)
+#define LVDS_A3_POWER_DOWN		(0 << 6)
+#define LVDS_A3_POWER_UP		(3 << 6)
+/*
+ * Controls the CLKB pair.  This should only be set when LVDS_B0B3_POWER_UP
+ * is set.
+ */
+#define LVDS_CLKB_POWER_MASK		(3 << 4)
+#define LVDS_CLKB_POWER_DOWN		(0 << 4)
+#define LVDS_CLKB_POWER_UP		(3 << 4)
+/*
+ * Controls the B0-B3 data pairs.  This must be set to match the DPLL p2
+ * setting for whether we are in dual-channel mode.  The B3 pair will
+ * additionally only be powered up when LVDS_A3_POWER_UP is set.
+ */
+#define LVDS_B0B3_POWER_MASK		(3 << 2)
+#define LVDS_B0B3_POWER_DOWN		(0 << 2)
+#define LVDS_B0B3_POWER_UP		(3 << 2)
+
+#define PIPEACONF		0x70008
+#define PIPEACONF_ENABLE		(1 << 31)
+#define PIPEACONF_DISABLE		0
+#define PIPEACONF_DOUBLE_WIDE		(1 << 30)
+#define PIPECONF_ACTIVE			(1 << 30)
+#define I965_PIPECONF_ACTIVE		(1 << 30)
+#define PIPECONF_DSIPLL_LOCK		(1 << 29)
+#define PIPEACONF_SINGLE_WIDE		0
+#define PIPEACONF_PIPE_UNLOCKED		0
+#define PIPEACONF_DSR			(1 << 26)
+#define PIPEACONF_PIPE_LOCKED		(1 << 25)
+#define PIPEACONF_PALETTE		0
+#define PIPECONF_FORCE_BORDER		(1 << 25)
+#define PIPEACONF_GAMMA			(1 << 24)
+#define PIPECONF_PROGRESSIVE		(0 << 21)
+#define PIPECONF_INTERLACE_W_FIELD_INDICATION	(6 << 21)
+#define PIPECONF_INTERLACE_FIELD_0_ONLY		(7 << 21)
+#define PIPECONF_PLANE_OFF		(1 << 19)
+#define PIPECONF_CURSOR_OFF		(1 << 18)
+
+#define PIPEBCONF		0x71008
+#define PIPEBCONF_ENABLE		(1 << 31)
+#define PIPEBCONF_DISABLE		0
+#define PIPEBCONF_DOUBLE_WIDE		(1 << 30)
+#define PIPEBCONF_DISABLE		0
+#define PIPEBCONF_GAMMA			(1 << 24)
+#define PIPEBCONF_PALETTE		0
+
+#define PIPECCONF		0x72008
+
+#define PIPEBGCMAXRED		0x71010
+#define PIPEBGCMAXGREEN		0x71014
+#define PIPEBGCMAXBLUE		0x71018
+
+#define PIPEASTAT		0x70024
+#define PIPEBSTAT		0x71024
+#define PIPECSTAT		0x72024
+#define PIPE_VBLANK_INTERRUPT_STATUS		(1UL << 1)
+#define PIPE_START_VBLANK_INTERRUPT_STATUS	(1UL << 2)
+#define PIPE_VBLANK_CLEAR			(1 << 1)
+#define PIPE_VBLANK_STATUS			(1 << 1)
+#define PIPE_TE_STATUS				(1UL << 6)
+#define PIPE_DPST_EVENT_STATUS			(1UL << 7)
+#define PIPE_VSYNC_CLEAR			(1UL << 9)
+#define PIPE_VSYNC_STATUS			(1UL << 9)
+#define PIPE_HDMI_AUDIO_UNDERRUN_STATUS		(1UL << 10)
+#define PIPE_HDMI_AUDIO_BUFFER_DONE_STATUS	(1UL << 11)
+#define PIPE_VBLANK_INTERRUPT_ENABLE		(1UL << 17)
+#define PIPE_START_VBLANK_INTERRUPT_ENABLE	(1UL << 18)
+#define PIPE_TE_ENABLE				(1UL << 22)
+#define PIPE_DPST_EVENT_ENABLE			(1UL << 23)
+#define PIPE_VSYNC_ENABL			(1UL << 25)
+#define PIPE_HDMI_AUDIO_UNDERRUN		(1UL << 26)
+#define PIPE_HDMI_AUDIO_BUFFER_DONE		(1UL << 27)
+#define PIPE_HDMI_AUDIO_INT_MASK		(PIPE_HDMI_AUDIO_UNDERRUN | \
+						PIPE_HDMI_AUDIO_BUFFER_DONE)
+#define PIPE_EVENT_MASK ((1 << 29)|(1 << 28)|(1 << 27)|(1 << 26)|(1 << 24)|(1 << 23)|(1 << 22)|(1 << 21)|(1 << 20)|(1 << 16))
+#define PIPE_VBLANK_MASK ((1 << 25)|(1 << 24)|(1 << 18)|(1 << 17))
+#define HISTOGRAM_INT_CONTROL		0x61268
+#define HISTOGRAM_BIN_DATA		0X61264
+#define HISTOGRAM_LOGIC_CONTROL		0x61260
+#define PWM_CONTROL_LOGIC		0x61250
+#define PIPE_HOTPLUG_INTERRUPT_STATUS		(1UL << 10)
+#define HISTOGRAM_INTERRUPT_ENABLE		(1UL << 31)
+#define HISTOGRAM_LOGIC_ENABLE			(1UL << 31)
+#define PWM_LOGIC_ENABLE			(1UL << 31)
+#define PWM_PHASEIN_ENABLE			(1UL << 25)
+#define PWM_PHASEIN_INT_ENABLE			(1UL << 24)
+#define PWM_PHASEIN_VB_COUNT			0x00001f00
+#define PWM_PHASEIN_INC				0x0000001f
+#define HISTOGRAM_INT_CTRL_CLEAR		(1UL << 30)
+#define DPST_YUV_LUMA_MODE			0
+
+struct dpst_ie_histogram_control {
+	union {
+		uint32_t data;
+		struct {
+			uint32_t bin_reg_index:7;
+			uint32_t reserved:4;
+			uint32_t bin_reg_func_select:1;
+			uint32_t sync_to_phase_in:1;
+			uint32_t alt_enhancement_mode:2;
+			uint32_t reserved1:1;
+			uint32_t sync_to_phase_in_count:8;
+			uint32_t histogram_mode_select:1;
+			uint32_t reserved2:4;
+			uint32_t ie_pipe_assignment:1;
+			uint32_t ie_mode_table_enabled:1;
+			uint32_t ie_histogram_enable:1;
+		};
+	};
+};
+
+struct dpst_guardband {
+	union {
+		uint32_t data;
+		struct {
+			uint32_t guardband:22;
+			uint32_t guardband_interrupt_delay:8;
+			uint32_t interrupt_status:1;
+			uint32_t interrupt_enable:1;
+		};
+	};
+};
+
+#define PIPEAFRAMEHIGH		0x70040
+#define PIPEAFRAMEPIXEL		0x70044
+#define PIPEBFRAMEHIGH		0x71040
+#define PIPEBFRAMEPIXEL		0x71044
+#define PIPECFRAMEHIGH		0x72040
+#define PIPECFRAMEPIXEL		0x72044
+#define PIPE_FRAME_HIGH_MASK	0x0000ffff
+#define PIPE_FRAME_HIGH_SHIFT	0
+#define PIPE_FRAME_LOW_MASK	0xff000000
+#define PIPE_FRAME_LOW_SHIFT	24
+#define PIPE_PIXEL_MASK		0x00ffffff
+#define PIPE_PIXEL_SHIFT	0
+
+#define DSPARB			0x70030
+#define DSPFW1			0x70034
+#define DSPFW2			0x70038
+#define DSPFW3			0x7003c
+#define DSPFW4			0x70050
+#define DSPFW5			0x70054
+#define DSPFW6			0x70058
+#define DSPCHICKENBIT		0x70400
+#define DSPACNTR		0x70180
+#define DSPBCNTR		0x71180
+#define DSPCCNTR		0x72180
+#define DISPLAY_PLANE_ENABLE			(1 << 31)
+#define DISPLAY_PLANE_DISABLE			0
+#define DISPPLANE_GAMMA_ENABLE			(1 << 30)
+#define DISPPLANE_GAMMA_DISABLE			0
+#define DISPPLANE_PIXFORMAT_MASK		(0xf << 26)
+#define DISPPLANE_8BPP				(0x2 << 26)
+#define DISPPLANE_15_16BPP			(0x4 << 26)
+#define DISPPLANE_16BPP				(0x5 << 26)
+#define DISPPLANE_32BPP_NO_ALPHA		(0x6 << 26)
+#define DISPPLANE_32BPP				(0x7 << 26)
+#define DISPPLANE_STEREO_ENABLE			(1 << 25)
+#define DISPPLANE_STEREO_DISABLE		0
+#define DISPPLANE_SEL_PIPE_MASK			(1 << 24)
+#define DISPPLANE_SEL_PIPE_POS			24
+#define DISPPLANE_SEL_PIPE_A			0
+#define DISPPLANE_SEL_PIPE_B			(1 << 24)
+#define DISPPLANE_SRC_KEY_ENABLE		(1 << 22)
+#define DISPPLANE_SRC_KEY_DISABLE		0
+#define DISPPLANE_LINE_DOUBLE			(1 << 20)
+#define DISPPLANE_NO_LINE_DOUBLE		0
+#define DISPPLANE_STEREO_POLARITY_FIRST		0
+#define DISPPLANE_STEREO_POLARITY_SECOND	(1 << 18)
+/* plane B only */
+#define DISPPLANE_ALPHA_TRANS_ENABLE		(1 << 15)
+#define DISPPLANE_ALPHA_TRANS_DISABLE		0
+#define DISPPLANE_SPRITE_ABOVE_DISPLAYA		0
+#define DISPPLANE_SPRITE_ABOVE_OVERLAY		(1)
+#define DISPPLANE_BOTTOM			(4)
+
+#define DSPABASE		0x70184
+#define DSPALINOFF		0x70184
+#define DSPASTRIDE		0x70188
+
+#define DSPBBASE		0x71184
+#define DSPBLINOFF		0X71184
+#define DSPBADDR		DSPBBASE
+#define DSPBSTRIDE		0x71188
+
+#define DSPCBASE		0x72184
+#define DSPCLINOFF		0x72184
+#define DSPCSTRIDE		0x72188
+
+#define DSPAKEYVAL		0x70194
+#define DSPAKEYMASK		0x70198
+
+#define DSPAPOS			0x7018C	/* reserved */
+#define DSPASIZE		0x70190
+#define DSPBPOS			0x7118C
+#define DSPBSIZE		0x71190
+#define DSPCPOS			0x7218C
+#define DSPCSIZE		0x72190
+
+#define DSPASURF		0x7019C
+#define DSPATILEOFF		0x701A4
+
+#define DSPBSURF		0x7119C
+#define DSPBTILEOFF		0x711A4
+
+#define DSPCSURF		0x7219C
+#define DSPCTILEOFF		0x721A4
+#define DSPCKEYMAXVAL		0x721A0
+#define DSPCKEYMINVAL		0x72194
+#define DSPCKEYMSK		0x72198
+
+#define VGACNTRL		0x71400
+#define VGA_DISP_DISABLE		(1 << 31)
+#define VGA_2X_MODE			(1 << 30)
+#define VGA_PIPE_B_SELECT		(1 << 29)
+
+/*
+ * Overlay registers
+ */
+#define OV_C_OFFSET		0x08000
+#define OV_OVADD		0x30000
+#define OV_DOVASTA		0x30008
+# define OV_PIPE_SELECT			((1 << 6)|(1 << 7))
+# define OV_PIPE_SELECT_POS		6
+# define OV_PIPE_A			0
+# define OV_PIPE_C			1
+#define OV_OGAMC5		0x30010
+#define OV_OGAMC4		0x30014
+#define OV_OGAMC3		0x30018
+#define OV_OGAMC2		0x3001C
+#define OV_OGAMC1		0x30020
+#define OV_OGAMC0		0x30024
+#define OVC_OVADD		0x38000
+#define OVC_DOVCSTA		0x38008
+#define OVC_OGAMC5		0x38010
+#define OVC_OGAMC4		0x38014
+#define OVC_OGAMC3		0x38018
+#define OVC_OGAMC2		0x3801C
+#define OVC_OGAMC1		0x38020
+#define OVC_OGAMC0		0x38024
+
+/*
+ * Some BIOS scratch area registers.  The 845 (and 830?) store the amount
+ * of video memory available to the BIOS in SWF1.
+ */
+#define SWF0			0x71410
+#define SWF1			0x71414
+#define SWF2			0x71418
+#define SWF3			0x7141c
+#define SWF4			0x71420
+#define SWF5			0x71424
+#define SWF6			0x71428
+
+/*
+ * 855 scratch registers.
+ */
+#define SWF00			0x70410
+#define SWF01			0x70414
+#define SWF02			0x70418
+#define SWF03			0x7041c
+#define SWF04			0x70420
+#define SWF05			0x70424
+#define SWF06			0x70428
+
+#define SWF10			SWF0
+#define SWF11			SWF1
+#define SWF12			SWF2
+#define SWF13			SWF3
+#define SWF14			SWF4
+#define SWF15			SWF5
+#define SWF16			SWF6
+
+#define SWF30			0x72414
+#define SWF31			0x72418
+#define SWF32			0x7241c
+
+
+/*
+ * Palette registers
+ */
+#define PALETTE_A		0x0a000
+#define PALETTE_B		0x0a800
+#define PALETTE_C		0x0ac00
+
+/* Cursor A & B regs */
+#define CURACNTR		0x70080
+#define CURSOR_MODE_DISABLE		0x00
+#define CURSOR_MODE_64_32B_AX		0x07
+#define CURSOR_MODE_64_ARGB_AX		((1 << 5) | CURSOR_MODE_64_32B_AX)
+#define MCURSOR_GAMMA_ENABLE		(1 << 26)
+#define CURABASE		0x70084
+#define CURAPOS			0x70088
+#define CURSOR_POS_MASK			0x007FF
+#define CURSOR_POS_SIGN			0x8000
+#define CURSOR_X_SHIFT			0
+#define CURSOR_Y_SHIFT			16
+#define CURBCNTR		0x700c0
+#define CURBBASE		0x700c4
+#define CURBPOS			0x700c8
+#define CURCCNTR		0x700e0
+#define CURCBASE		0x700e4
+#define CURCPOS			0x700e8
+
+/*
+ * Interrupt Registers
+ */
+#define IER			0x020a0
+#define IIR			0x020a4
+#define IMR			0x020a8
+#define ISR			0x020ac
+
+/*
+ * MOORESTOWN delta registers
+ */
+#define MRST_DPLL_A		0x0f014
+#define MDFLD_DPLL_B		0x0f018
+#define MDFLD_INPUT_REF_SEL		(1 << 14)
+#define MDFLD_VCO_SEL			(1 << 16)
+#define DPLLA_MODE_LVDS			(2 << 26)	/* mrst */
+#define MDFLD_PLL_LATCHEN		(1 << 28)
+#define MDFLD_PWR_GATE_EN		(1 << 30)
+#define MDFLD_P1_MASK			(0x1FF << 17)
+#define MRST_FPA0		0x0f040
+#define MRST_FPA1		0x0f044
+#define MDFLD_DPLL_DIV0		0x0f048
+#define MDFLD_DPLL_DIV1		0x0f04c
+#define MRST_PERF_MODE		0x020f4
+
+/*
+ * MEDFIELD HDMI registers
+ */
+#define HDMIPHYMISCCTL		0x61134
+#define HDMI_PHY_POWER_DOWN		0x7f
+#define HDMIB_CONTROL		0x61140
+#define HDMIB_PORT_EN			(1 << 31)
+#define HDMIB_PIPE_B_SELECT		(1 << 30)
+#define HDMIB_NULL_PACKET		(1 << 9)
+#define HDMIB_HDCP_PORT			(1 << 5)
+
+/* #define LVDS			0x61180 */
+#define MRST_PANEL_8TO6_DITHER_ENABLE	(1 << 25)
+#define MRST_PANEL_24_DOT_1_FORMAT	(1 << 24)
+#define LVDS_A3_POWER_UP_0_OUTPUT	(1 << 6)
+
+#define MIPI			0x61190
+#define MIPI_C			0x62190
+#define MIPI_PORT_EN			(1 << 31)
+/* Turns on border drawing to allow centered display. */
+#define SEL_FLOPPED_HSTX		(1 << 23)
+#define PASS_FROM_SPHY_TO_AFE		(1 << 16)
+#define MIPI_BORDER_EN			(1 << 15)
+#define MIPIA_3LANE_MIPIC_1LANE		0x1
+#define MIPIA_2LANE_MIPIC_2LANE		0x2
+#define TE_TRIGGER_DSI_PROTOCOL		(1 << 2)
+#define TE_TRIGGER_GPIO_PIN		(1 << 3)
+#define MIPI_TE_COUNT		0x61194
+
+/* #define PP_CONTROL	0x61204 */
+#define POWER_DOWN_ON_RESET		(1 << 1)
+
+/* #define PFIT_CONTROL	0x61230 */
+#define PFIT_PIPE_SELECT		(3 << 29)
+#define PFIT_PIPE_SELECT_SHIFT		(29)
+
+/* #define BLC_PWM_CTL		0x61254 */
+#define MRST_BACKLIGHT_MODULATION_FREQ_SHIFT	(16)
+#define MRST_BACKLIGHT_MODULATION_FREQ_MASK	(0xffff << 16)
+
+/* #define PIPEACONF 0x70008 */
+#define PIPEACONF_PIPE_STATE		(1 << 30)
+/* #define DSPACNTR		0x70180 */
+
+#define MRST_DSPABASE		0x7019c
+#define MRST_DSPBBASE		0x7119c
+#define MDFLD_DSPCBASE		0x7219c
+
+/*
+ * Moorestown registers.
+ */
+
+/*
+ *	MIPI IP registers
+ */
+#define MIPIC_REG_OFFSET		0x800
+
+#define DEVICE_READY_REG		0xb000
+#define LP_OUTPUT_HOLD				(1 << 16)
+#define EXIT_ULPS_DEV_READY			0x3
+#define LP_OUTPUT_HOLD_RELEASE			0x810000
+# define ENTERING_ULPS				(2 << 1)
+# define EXITING_ULPS				(1 << 1)
+# define ULPS_MASK				(3 << 1)
+# define BUS_POSSESSION				(1 << 3)
+#define INTR_STAT_REG			0xb004
+#define RX_SOT_ERROR				(1 << 0)
+#define RX_SOT_SYNC_ERROR			(1 << 1)
+#define RX_ESCAPE_MODE_ENTRY_ERROR		(1 << 3)
+#define RX_LP_TX_SYNC_ERROR			(1 << 4)
+#define RX_HS_RECEIVE_TIMEOUT_ERROR		(1 << 5)
+#define RX_FALSE_CONTROL_ERROR			(1 << 6)
+#define RX_ECC_SINGLE_BIT_ERROR			(1 << 7)
+#define RX_ECC_MULTI_BIT_ERROR			(1 << 8)
+#define RX_CHECKSUM_ERROR			(1 << 9)
+#define RX_DSI_DATA_TYPE_NOT_RECOGNIZED		(1 << 10)
+#define RX_DSI_VC_ID_INVALID			(1 << 11)
+#define TX_FALSE_CONTROL_ERROR			(1 << 12)
+#define TX_ECC_SINGLE_BIT_ERROR			(1 << 13)
+#define TX_ECC_MULTI_BIT_ERROR			(1 << 14)
+#define TX_CHECKSUM_ERROR			(1 << 15)
+#define TX_DSI_DATA_TYPE_NOT_RECOGNIZED		(1 << 16)
+#define TX_DSI_VC_ID_INVALID			(1 << 17)
+#define HIGH_CONTENTION				(1 << 18)
+#define LOW_CONTENTION				(1 << 19)
+#define DPI_FIFO_UNDER_RUN			(1 << 20)
+#define HS_TX_TIMEOUT				(1 << 21)
+#define LP_RX_TIMEOUT				(1 << 22)
+#define TURN_AROUND_ACK_TIMEOUT			(1 << 23)
+#define ACK_WITH_NO_ERROR			(1 << 24)
+#define HS_GENERIC_WR_FIFO_FULL			(1 << 27)
+#define LP_GENERIC_WR_FIFO_FULL			(1 << 28)
+#define SPL_PKT_SENT				(1 << 30)
+#define INTR_EN_REG			0xb008
+#define DSI_FUNC_PRG_REG		0xb00c
+#define DPI_CHANNEL_NUMBER_POS			0x03
+#define DBI_CHANNEL_NUMBER_POS			0x05
+#define FMT_DPI_POS				0x07
+#define FMT_DBI_POS				0x0A
+#define DBI_DATA_WIDTH_POS			0x0D
+
+/* DPI PIXEL FORMATS */
+#define RGB_565_FMT				0x01	/* RGB 565 FORMAT */
+#define RGB_666_FMT				0x02	/* RGB 666 FORMAT */
+#define LRGB_666_FMT				0x03	/* RGB LOOSELY PACKED
+							 * 666 FORMAT
+							 */
+#define RGB_888_FMT				0x04	/* RGB 888 FORMAT */
+#define VIRTUAL_CHANNEL_NUMBER_0		0x00	/* Virtual channel 0 */
+#define VIRTUAL_CHANNEL_NUMBER_1		0x01	/* Virtual channel 1 */
+#define VIRTUAL_CHANNEL_NUMBER_2		0x02	/* Virtual channel 2 */
+#define VIRTUAL_CHANNEL_NUMBER_3		0x03	/* Virtual channel 3 */
+
+#define DBI_NOT_SUPPORTED			0x00	/* command mode
+							 * is not supported
+							 */
+#define DBI_DATA_WIDTH_16BIT			0x01	/* 16 bit data */
+#define DBI_DATA_WIDTH_9BIT			0x02	/* 9 bit data */
+#define DBI_DATA_WIDTH_8BIT			0x03	/* 8 bit data */
+#define DBI_DATA_WIDTH_OPT1			0x04	/* option 1 */
+#define DBI_DATA_WIDTH_OPT2			0x05	/* option 2 */
+
+#define HS_TX_TIMEOUT_REG		0xb010
+#define LP_RX_TIMEOUT_REG		0xb014
+#define TURN_AROUND_TIMEOUT_REG		0xb018
+#define DEVICE_RESET_REG		0xb01C
+#define DPI_RESOLUTION_REG		0xb020
+#define RES_V_POS				0x10
+#define DBI_RESOLUTION_REG		0xb024 /* Reserved for MDFLD */
+#define HORIZ_SYNC_PAD_COUNT_REG	0xb028
+#define HORIZ_BACK_PORCH_COUNT_REG	0xb02C
+#define HORIZ_FRONT_PORCH_COUNT_REG	0xb030
+#define HORIZ_ACTIVE_AREA_COUNT_REG	0xb034
+#define VERT_SYNC_PAD_COUNT_REG		0xb038
+#define VERT_BACK_PORCH_COUNT_REG	0xb03c
+#define VERT_FRONT_PORCH_COUNT_REG	0xb040
+#define HIGH_LOW_SWITCH_COUNT_REG	0xb044
+#define DPI_CONTROL_REG			0xb048
+#define DPI_SHUT_DOWN				(1 << 0)
+#define DPI_TURN_ON				(1 << 1)
+#define DPI_COLOR_MODE_ON			(1 << 2)
+#define DPI_COLOR_MODE_OFF			(1 << 3)
+#define DPI_BACK_LIGHT_ON			(1 << 4)
+#define DPI_BACK_LIGHT_OFF			(1 << 5)
+#define DPI_LP					(1 << 6)
+#define DPI_DATA_REG			0xb04c
+#define DPI_BACK_LIGHT_ON_DATA			0x07
+#define DPI_BACK_LIGHT_OFF_DATA			0x17
+#define INIT_COUNT_REG			0xb050
+#define MAX_RET_PAK_REG			0xb054
+#define VIDEO_FMT_REG			0xb058
+#define COMPLETE_LAST_PCKT			(1 << 2)
+#define EOT_DISABLE_REG			0xb05c
+#define ENABLE_CLOCK_STOPPING			(1 << 1)
+#define LP_BYTECLK_REG			0xb060
+#define LP_GEN_DATA_REG			0xb064
+#define HS_GEN_DATA_REG			0xb068
+#define LP_GEN_CTRL_REG			0xb06C
+#define HS_GEN_CTRL_REG			0xb070
+#define DCS_CHANNEL_NUMBER_POS		0x6
+#define MCS_COMMANDS_POS		0x8
+#define WORD_COUNTS_POS			0x8
+#define MCS_PARAMETER_POS			0x10
+#define GEN_FIFO_STAT_REG		0xb074
+#define HS_DATA_FIFO_FULL			(1 << 0)
+#define HS_DATA_FIFO_HALF_EMPTY			(1 << 1)
+#define HS_DATA_FIFO_EMPTY			(1 << 2)
+#define LP_DATA_FIFO_FULL			(1 << 8)
+#define LP_DATA_FIFO_HALF_EMPTY			(1 << 9)
+#define LP_DATA_FIFO_EMPTY			(1 << 10)
+#define HS_CTRL_FIFO_FULL			(1 << 16)
+#define HS_CTRL_FIFO_HALF_EMPTY			(1 << 17)
+#define HS_CTRL_FIFO_EMPTY			(1 << 18)
+#define LP_CTRL_FIFO_FULL			(1 << 24)
+#define LP_CTRL_FIFO_HALF_EMPTY			(1 << 25)
+#define LP_CTRL_FIFO_EMPTY			(1 << 26)
+#define DBI_FIFO_EMPTY				(1 << 27)
+#define DPI_FIFO_EMPTY				(1 << 28)
+#define HS_LS_DBI_ENABLE_REG		0xb078
+#define TXCLKESC_REG			0xb07c
+#define DPHY_PARAM_REG			0xb080
+#define DBI_BW_CTRL_REG			0xb084
+#define CLK_LANE_SWT_REG		0xb088
+
+/*
+ * MIPI Adapter registers
+ */
+#define MIPI_CONTROL_REG		0xb104
+#define MIPI_2X_CLOCK_BITS			((1 << 0) | (1 << 1))
+#define MIPI_DATA_ADDRESS_REG		0xb108
+#define MIPI_DATA_LENGTH_REG		0xb10C
+#define MIPI_COMMAND_ADDRESS_REG	0xb110
+#define MIPI_COMMAND_LENGTH_REG		0xb114
+#define MIPI_READ_DATA_RETURN_REG0	0xb118
+#define MIPI_READ_DATA_RETURN_REG1	0xb11C
+#define MIPI_READ_DATA_RETURN_REG2	0xb120
+#define MIPI_READ_DATA_RETURN_REG3	0xb124
+#define MIPI_READ_DATA_RETURN_REG4	0xb128
+#define MIPI_READ_DATA_RETURN_REG5	0xb12C
+#define MIPI_READ_DATA_RETURN_REG6	0xb130
+#define MIPI_READ_DATA_RETURN_REG7	0xb134
+#define MIPI_READ_DATA_VALID_REG	0xb138
+
+/* DBI COMMANDS */
+#define soft_reset			0x01
+/*
+ *	The display module performs a software reset.
+ *	Registers are written with their SW Reset default values.
+ */
+#define get_power_mode			0x0a
+/*
+ *	The display module returns the current power mode
+ */
+#define get_address_mode		0x0b
+/*
+ *	The display module returns the current status.
+ */
+#define get_pixel_format		0x0c
+/*
+ *	This command gets the pixel format for the RGB image data
+ *	used by the interface.
+ */
+#define get_display_mode		0x0d
+/*
+ *	The display module returns the Display Image Mode status.
+ */
+#define get_signal_mode			0x0e
+/*
+ *	The display module returns the Display Signal Mode.
+ */
+#define get_diagnostic_result		0x0f
+/*
+ *	The display module returns the self-diagnostic results following
+ *	a Sleep Out command.
+ */
+#define enter_sleep_mode		0x10
+/*
+ *	This command causes the display module to enter the Sleep mode.
+ *	In this mode, all unnecessary blocks inside the display module are
+ *	disabled except interface communication. This is the lowest power
+ *	mode the display module supports.
+ */
+#define exit_sleep_mode			0x11
+/*
+ *	This command causes the display module to exit Sleep mode.
+ *	All blocks inside the display module are enabled.
+ */
+#define enter_partial_mode		0x12
+/*
+ *	This command causes the display module to enter the Partial Display
+ *	Mode. The Partial Display Mode window is described by the
+ *	set_partial_area command.
+ */
+#define enter_normal_mode		0x13
+/*
+ *	This command causes the display module to enter the Normal mode.
+ *	Normal Mode is defined as Partial Display mode and Scroll mode are off
+ */
+#define exit_invert_mode		0x20
+/*
+ *	This command causes the display module to stop inverting the image
+ *	data on the display device. The frame memory contents remain unchanged.
+ *	No status bits are changed.
+ */
+#define enter_invert_mode		0x21
+/*
+ *	This command causes the display module to invert the image data only on
+ *	the display device. The frame memory contents remain unchanged.
+ *	No status bits are changed.
+ */
+#define set_gamma_curve			0x26
+/*
+ *	This command selects the desired gamma curve for the display device.
+ *	Four fixed gamma curves are defined in section DCS spec.
+ */
+#define set_display_off			0x28
+/* ************************************************************************* *\
+This command causes the display module to stop displaying the image data
+on the display device. The frame memory contents remain unchanged.
+No status bits are changed.
+\* ************************************************************************* */
+#define set_display_on			0x29
+/* ************************************************************************* *\
+This command causes the display module to start displaying the image data
+on the display device. The frame memory contents remain unchanged.
+No status bits are changed.
+\* ************************************************************************* */
+#define set_column_address		0x2a
+/*
+ *	This command defines the column extent of the frame memory accessed by
+ *	the hostprocessor with the read_memory_continue and
+ *	write_memory_continue commands.
+ *	No status bits are changed.
+ */
+#define set_page_addr			0x2b
+/*
+ *	This command defines the page extent of the frame memory accessed by
+ *	the host processor with the write_memory_continue and
+ *	read_memory_continue command.
+ *	No status bits are changed.
+ */
+#define write_mem_start			0x2c
+/*
+ *	This command transfers image data from the host processor to the
+ *	display modules frame memory starting at the pixel location specified
+ *	by preceding set_column_address and set_page_address commands.
+ */
+#define set_partial_area		0x30
+/*
+ *	This command defines the Partial Display mode s display area.
+ *	There are two parameters associated with this command, the first
+ *	defines the Start Row (SR) and the second the End Row (ER). SR and ER
+ *	refer to the Frame Memory Line Pointer.
+ */
+#define set_scroll_area			0x33
+/*
+ *	This command defines the display modules Vertical Scrolling Area.
+ */
+#define set_tear_off			0x34
+/*
+ *	This command turns off the display modules Tearing Effect output
+ *	signal on the TE signal line.
+ */
+#define set_tear_on			0x35
+/*
+ *	This command turns on the display modules Tearing Effect output signal
+ *	on the TE signal line.
+ */
+#define set_address_mode		0x36
+/*
+ *	This command sets the data order for transfers from the host processor
+ *	to display modules frame memory,bits B[7:5] and B3, and from the
+ *	display modules frame memory to the display device, bits B[2:0] and B4.
+ */
+#define set_scroll_start		0x37
+/*
+ *	This command sets the start of the vertical scrolling area in the frame
+ *	memory. The vertical scrolling area is fully defined when this command
+ *	is used with the set_scroll_area command The set_scroll_start command
+ *	has one parameter, the Vertical Scroll Pointer. The VSP defines the
+ *	line in the frame memory that is written to the display device as the
+ *	first line of the vertical scroll area.
+ */
+#define exit_idle_mode			0x38
+/*
+ *	This command causes the display module to exit Idle mode.
+ */
+#define enter_idle_mode			0x39
+/*
+ *	This command causes the display module to enter Idle Mode.
+ *	In Idle Mode, color expression is reduced. Colors are shown on the
+ *	display device using the MSB of each of the R, G and B color
+ *	components in the frame memory
+ */
+#define set_pixel_format		0x3a
+/*
+ *	This command sets the pixel format for the RGB image data used by the
+ *	interface.
+ *	Bits D[6:4]  DPI Pixel Format Definition
+ *	Bits D[2:0]  DBI Pixel Format Definition
+ *	Bits D7 and D3 are not used.
+ */
+#define DCS_PIXEL_FORMAT_3bpp		0x1
+#define DCS_PIXEL_FORMAT_8bpp		0x2
+#define DCS_PIXEL_FORMAT_12bpp		0x3
+#define DCS_PIXEL_FORMAT_16bpp		0x5
+#define DCS_PIXEL_FORMAT_18bpp		0x6
+#define DCS_PIXEL_FORMAT_24bpp		0x7
+
+#define write_mem_cont			0x3c
+
+/*
+ *	This command transfers image data from the host processor to the
+ *	display module's frame memory continuing from the pixel location
+ *	following the previous write_memory_continue or write_memory_start
+ *	command.
+ */
+#define set_tear_scanline		0x44
+/*
+ *	This command turns on the display modules Tearing Effect output signal
+ *	on the TE signal line when the display module reaches line N.
+ */
+#define get_scanline			0x45
+/*
+ *	The display module returns the current scanline, N, used to update the
+ *	 display device. The total number of scanlines on a display device is
+ *	defined as VSYNC + VBP + VACT + VFP.The first scanline is defined as
+ *	the first line of V Sync and is denoted as Line 0.
+ *	When in Sleep Mode, the value returned by get_scanline is undefined.
+ */
+
+/* MCS or Generic COMMANDS */
+/* MCS/generic data type */
+#define GEN_SHORT_WRITE_0	0x03  /* generic short write, no parameters */
+#define GEN_SHORT_WRITE_1	0x13  /* generic short write, 1 parameters */
+#define GEN_SHORT_WRITE_2	0x23  /* generic short write, 2 parameters */
+#define GEN_READ_0		0x04  /* generic read, no parameters */
+#define GEN_READ_1		0x14  /* generic read, 1 parameters */
+#define GEN_READ_2		0x24  /* generic read, 2 parameters */
+#define GEN_LONG_WRITE		0x29  /* generic long write */
+#define MCS_SHORT_WRITE_0	0x05  /* MCS short write, no parameters */
+#define MCS_SHORT_WRITE_1	0x15  /* MCS short write, 1 parameters */
+#define MCS_READ		0x06  /* MCS read, no parameters */
+#define MCS_LONG_WRITE		0x39  /* MCS long write */
+/* MCS/generic commands */
+/* TPO MCS */
+#define write_display_profile		0x50
+#define write_display_brightness	0x51
+#define write_ctrl_display		0x53
+#define write_ctrl_cabc			0x55
+  #define UI_IMAGE		0x01
+  #define STILL_IMAGE		0x02
+  #define MOVING_IMAGE		0x03
+#define write_hysteresis		0x57
+#define write_gamma_setting		0x58
+#define write_cabc_min_bright		0x5e
+#define write_kbbc_profile		0x60
+/* TMD MCS */
+#define tmd_write_display_brightness 0x8c
+
+/*
+ *	This command is used to control ambient light, panel backlight
+ *	brightness and gamma settings.
+ */
+#define BRIGHT_CNTL_BLOCK_ON	(1 << 5)
+#define AMBIENT_LIGHT_SENSE_ON	(1 << 4)
+#define DISPLAY_DIMMING_ON	(1 << 3)
+#define BACKLIGHT_ON		(1 << 2)
+#define DISPLAY_BRIGHTNESS_AUTO	(1 << 1)
+#define GAMMA_AUTO		(1 << 0)
+
+/* DCS Interface Pixel Formats */
+#define DCS_PIXEL_FORMAT_3BPP	0x1
+#define DCS_PIXEL_FORMAT_8BPP	0x2
+#define DCS_PIXEL_FORMAT_12BPP	0x3
+#define DCS_PIXEL_FORMAT_16BPP	0x5
+#define DCS_PIXEL_FORMAT_18BPP	0x6
+#define DCS_PIXEL_FORMAT_24BPP	0x7
+/* ONE PARAMETER READ DATA */
+#define addr_mode_data		0xfc
+#define diag_res_data		0x00
+#define disp_mode_data		0x23
+#define pxl_fmt_data		0x77
+#define pwr_mode_data		0x74
+#define sig_mode_data		0x00
+/* TWO PARAMETERS READ DATA */
+#define scanline_data1		0xff
+#define scanline_data2		0xff
+#define NON_BURST_MODE_SYNC_PULSE	0x01	/* Non Burst Mode
+						 * with Sync Pulse
+						 */
+#define NON_BURST_MODE_SYNC_EVENTS	0x02	/* Non Burst Mode
+						 * with Sync events
+						 */
+#define BURST_MODE			0x03	/* Burst Mode */
+#define DBI_COMMAND_BUFFER_SIZE		0x240   /* 0x32 */    /* 0x120 */
+						/* Allocate at least
+						 * 0x100 Byte with 32
+						 * byte alignment
+						 */
+#define DBI_DATA_BUFFER_SIZE		0x120	/* Allocate at least
+						 * 0x100 Byte with 32
+						 * byte alignment
+						 */
+#define DBI_CB_TIME_OUT			0xFFFF
+
+#define GEN_FB_TIME_OUT			2000
+
+#define SKU_83				0x01
+#define SKU_100				0x02
+#define SKU_100L			0x04
+#define SKU_BYPASS			0x08
+
+/* Some handy macros for playing with bitfields. */
+#define PSB_MASK(high, low) (((1<<((high)-(low)+1))-1)<<(low))
+#define SET_FIELD(value, field) (((value) << field ## _SHIFT) & field ## _MASK)
+#define GET_FIELD(word, field) (((word)  & field ## _MASK) >> field ## _SHIFT)
+
+#define _PIPE(pipe, a, b) ((a) + (pipe)*((b)-(a)))
+
+/* PCI config space */
+
+#define SB_PCKT         0x02100 /* cedarview */
+# define SB_OPCODE_MASK                         PSB_MASK(31, 16)
+# define SB_OPCODE_SHIFT                        16
+# define SB_OPCODE_READ                         0
+# define SB_OPCODE_WRITE                        1
+# define SB_DEST_MASK                           PSB_MASK(15, 8)
+# define SB_DEST_SHIFT                          8
+# define SB_DEST_DPLL                           0x88
+# define SB_BYTE_ENABLE_MASK                    PSB_MASK(7, 4)
+# define SB_BYTE_ENABLE_SHIFT                   4
+# define SB_BUSY                                (1 << 0)
+
+
+/* 32-bit value read/written from the DPIO reg. */
+#define SB_DATA		0x02104 /* cedarview */
+/* 32-bit address of the DPIO reg to be read/written. */
+#define SB_ADDR		0x02108 /* cedarview */
+#define DPIO_CFG	0x02110 /* cedarview */
+# define DPIO_MODE_SELECT_1			(1 << 3)
+# define DPIO_MODE_SELECT_0			(1 << 2)
+# define DPIO_SFR_BYPASS			(1 << 1)
+/* reset is active low */
+# define DPIO_CMN_RESET_N			(1 << 0)
+
+/* Cedarview sideband registers */
+#define _SB_M_A			0x8008
+#define _SB_M_B			0x8028
+#define SB_M(pipe) _PIPE(pipe, _SB_M_A, _SB_M_B)
+# define SB_M_DIVIDER_MASK			(0xFF << 24)
+# define SB_M_DIVIDER_SHIFT			24
+
+#define _SB_N_VCO_A		0x8014
+#define _SB_N_VCO_B		0x8034
+#define SB_N_VCO(pipe) _PIPE(pipe, _SB_N_VCO_A, _SB_N_VCO_B)
+#define SB_N_VCO_SEL_MASK			PSB_MASK(31, 30)
+#define SB_N_VCO_SEL_SHIFT			30
+#define SB_N_DIVIDER_MASK			PSB_MASK(29, 26)
+#define SB_N_DIVIDER_SHIFT			26
+#define SB_N_CB_TUNE_MASK			PSB_MASK(25, 24)
+#define SB_N_CB_TUNE_SHIFT			24
+
+#define _SB_REF_A		0x8018
+#define _SB_REF_B		0x8038
+#define SB_REF_SFR(pipe)	_PIPE(pipe, _SB_REF_A, _SB_REF_B)
+
+#define _SB_P_A			0x801c
+#define _SB_P_B			0x803c
+#define SB_P(pipe) _PIPE(pipe, _SB_P_A, _SB_P_B)
+#define SB_P2_DIVIDER_MASK			PSB_MASK(31, 30)
+#define SB_P2_DIVIDER_SHIFT			30
+#define SB_P2_10				0 /* HDMI, DP, DAC */
+#define SB_P2_5				1 /* DAC */
+#define SB_P2_14				2 /* LVDS single */
+#define SB_P2_7				3 /* LVDS double */
+#define SB_P1_DIVIDER_MASK			PSB_MASK(15, 12)
+#define SB_P1_DIVIDER_SHIFT			12
+
+#define PSB_LANE0		0x120
+#define PSB_LANE1		0x220
+#define PSB_LANE2		0x2320
+#define PSB_LANE3		0x2420
+
+#define LANE_PLL_MASK		(0x7 << 20)
+#define LANE_PLL_ENABLE		(0x3 << 20)
+
+
+#endif
diff --git a/drivers/gpu/drm/gma500/psb_intel_sdvo.c b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
new file mode 100644
index 000000000000..4882b29119e0
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
@@ -0,0 +1,2617 @@
+/*
+ * Copyright 2006 Dave Airlie <airlied@linux.ie>
+ * Copyright © 2006-2007 Intel Corporation
+ *   Jesse Barnes <jesse.barnes@intel.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ */
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc.h"
+#include "drm_edid.h"
+#include "psb_intel_drv.h"
+#include "gma_drm.h"
+#include "psb_drv.h"
+#include "psb_intel_sdvo_regs.h"
+#include "psb_intel_reg.h"
+
+#define SDVO_TMDS_MASK (SDVO_OUTPUT_TMDS0 | SDVO_OUTPUT_TMDS1)
+#define SDVO_RGB_MASK  (SDVO_OUTPUT_RGB0 | SDVO_OUTPUT_RGB1)
+#define SDVO_LVDS_MASK (SDVO_OUTPUT_LVDS0 | SDVO_OUTPUT_LVDS1)
+#define SDVO_TV_MASK   (SDVO_OUTPUT_CVBS0 | SDVO_OUTPUT_SVID0)
+
+#define SDVO_OUTPUT_MASK (SDVO_TMDS_MASK | SDVO_RGB_MASK | SDVO_LVDS_MASK |\
+                         SDVO_TV_MASK)
+
+#define IS_TV(c)	(c->output_flag & SDVO_TV_MASK)
+#define IS_TMDS(c)	(c->output_flag & SDVO_TMDS_MASK)
+#define IS_LVDS(c)	(c->output_flag & SDVO_LVDS_MASK)
+#define IS_TV_OR_LVDS(c) (c->output_flag & (SDVO_TV_MASK | SDVO_LVDS_MASK))
+
+
+static const char *tv_format_names[] = {
+	"NTSC_M"   , "NTSC_J"  , "NTSC_443",
+	"PAL_B"    , "PAL_D"   , "PAL_G"   ,
+	"PAL_H"    , "PAL_I"   , "PAL_M"   ,
+	"PAL_N"    , "PAL_NC"  , "PAL_60"  ,
+	"SECAM_B"  , "SECAM_D" , "SECAM_G" ,
+	"SECAM_K"  , "SECAM_K1", "SECAM_L" ,
+	"SECAM_60"
+};
+
+#define TV_FORMAT_NUM  (sizeof(tv_format_names) / sizeof(*tv_format_names))
+
+struct psb_intel_sdvo {
+	struct psb_intel_encoder base;
+
+	struct i2c_adapter *i2c;
+	u8 slave_addr;
+
+	struct i2c_adapter ddc;
+
+	/* Register for the SDVO device: SDVOB or SDVOC */
+	int sdvo_reg;
+
+	/* Active outputs controlled by this SDVO output */
+	uint16_t controlled_output;
+
+	/*
+	 * Capabilities of the SDVO device returned by
+	 * i830_sdvo_get_capabilities()
+	 */
+	struct psb_intel_sdvo_caps caps;
+
+	/* Pixel clock limitations reported by the SDVO device, in kHz */
+	int pixel_clock_min, pixel_clock_max;
+
+	/*
+	* For multiple function SDVO device,
+	* this is for current attached outputs.
+	*/
+	uint16_t attached_output;
+
+	/**
+	 * This is used to select the color range of RBG outputs in HDMI mode.
+	 * It is only valid when using TMDS encoding and 8 bit per color mode.
+	 */
+	uint32_t color_range;
+
+	/**
+	 * This is set if we're going to treat the device as TV-out.
+	 *
+	 * While we have these nice friendly flags for output types that ought
+	 * to decide this for us, the S-Video output on our HDMI+S-Video card
+	 * shows up as RGB1 (VGA).
+	 */
+	bool is_tv;
+
+	/* This is for current tv format name */
+	int tv_format_index;
+
+	/**
+	 * This is set if we treat the device as HDMI, instead of DVI.
+	 */
+	bool is_hdmi;
+	bool has_hdmi_monitor;
+	bool has_hdmi_audio;
+
+	/**
+	 * This is set if we detect output of sdvo device as LVDS and
+	 * have a valid fixed mode to use with the panel.
+	 */
+	bool is_lvds;
+
+	/**
+	 * This is sdvo fixed pannel mode pointer
+	 */
+	struct drm_display_mode *sdvo_lvds_fixed_mode;
+
+	/* DDC bus used by this SDVO encoder */
+	uint8_t ddc_bus;
+
+	/* Input timings for adjusted_mode */
+	struct psb_intel_sdvo_dtd input_dtd;
+};
+
+struct psb_intel_sdvo_connector {
+	struct psb_intel_connector base;
+
+	/* Mark the type of connector */
+	uint16_t output_flag;
+
+	int force_audio;
+
+	/* This contains all current supported TV format */
+	u8 tv_format_supported[TV_FORMAT_NUM];
+	int   format_supported_num;
+	struct drm_property *tv_format;
+
+	/* add the property for the SDVO-TV */
+	struct drm_property *left;
+	struct drm_property *right;
+	struct drm_property *top;
+	struct drm_property *bottom;
+	struct drm_property *hpos;
+	struct drm_property *vpos;
+	struct drm_property *contrast;
+	struct drm_property *saturation;
+	struct drm_property *hue;
+	struct drm_property *sharpness;
+	struct drm_property *flicker_filter;
+	struct drm_property *flicker_filter_adaptive;
+	struct drm_property *flicker_filter_2d;
+	struct drm_property *tv_chroma_filter;
+	struct drm_property *tv_luma_filter;
+	struct drm_property *dot_crawl;
+
+	/* add the property for the SDVO-TV/LVDS */
+	struct drm_property *brightness;
+
+	/* Add variable to record current setting for the above property */
+	u32	left_margin, right_margin, top_margin, bottom_margin;
+
+	/* this is to get the range of margin.*/
+	u32	max_hscan,  max_vscan;
+	u32	max_hpos, cur_hpos;
+	u32	max_vpos, cur_vpos;
+	u32	cur_brightness, max_brightness;
+	u32	cur_contrast,	max_contrast;
+	u32	cur_saturation, max_saturation;
+	u32	cur_hue,	max_hue;
+	u32	cur_sharpness,	max_sharpness;
+	u32	cur_flicker_filter,		max_flicker_filter;
+	u32	cur_flicker_filter_adaptive,	max_flicker_filter_adaptive;
+	u32	cur_flicker_filter_2d,		max_flicker_filter_2d;
+	u32	cur_tv_chroma_filter,	max_tv_chroma_filter;
+	u32	cur_tv_luma_filter,	max_tv_luma_filter;
+	u32	cur_dot_crawl,	max_dot_crawl;
+};
+
+static struct psb_intel_sdvo *to_psb_intel_sdvo(struct drm_encoder *encoder)
+{
+	return container_of(encoder, struct psb_intel_sdvo, base.base);
+}
+
+static struct psb_intel_sdvo *intel_attached_sdvo(struct drm_connector *connector)
+{
+	return container_of(psb_intel_attached_encoder(connector),
+			    struct psb_intel_sdvo, base);
+}
+
+static struct psb_intel_sdvo_connector *to_psb_intel_sdvo_connector(struct drm_connector *connector)
+{
+	return container_of(to_psb_intel_connector(connector), struct psb_intel_sdvo_connector, base);
+}
+
+static bool
+psb_intel_sdvo_output_setup(struct psb_intel_sdvo *psb_intel_sdvo, uint16_t flags);
+static bool
+psb_intel_sdvo_tv_create_property(struct psb_intel_sdvo *psb_intel_sdvo,
+			      struct psb_intel_sdvo_connector *psb_intel_sdvo_connector,
+			      int type);
+static bool
+psb_intel_sdvo_create_enhance_property(struct psb_intel_sdvo *psb_intel_sdvo,
+				   struct psb_intel_sdvo_connector *psb_intel_sdvo_connector);
+
+/**
+ * Writes the SDVOB or SDVOC with the given value, but always writes both
+ * SDVOB and SDVOC to work around apparent hardware issues (according to
+ * comments in the BIOS).
+ */
+static void psb_intel_sdvo_write_sdvox(struct psb_intel_sdvo *psb_intel_sdvo, u32 val)
+{
+	struct drm_device *dev = psb_intel_sdvo->base.base.dev;
+	u32 bval = val, cval = val;
+	int i;
+
+	if (psb_intel_sdvo->sdvo_reg == SDVOB) {
+		cval = REG_READ(SDVOC);
+	} else {
+		bval = REG_READ(SDVOB);
+	}
+	/*
+	 * Write the registers twice for luck. Sometimes,
+	 * writing them only once doesn't appear to 'stick'.
+	 * The BIOS does this too. Yay, magic
+	 */
+	for (i = 0; i < 2; i++)
+	{
+		REG_WRITE(SDVOB, bval);
+		REG_READ(SDVOB);
+		REG_WRITE(SDVOC, cval);
+		REG_READ(SDVOC);
+	}
+}
+
+static bool psb_intel_sdvo_read_byte(struct psb_intel_sdvo *psb_intel_sdvo, u8 addr, u8 *ch)
+{
+	struct i2c_msg msgs[] = {
+		{
+			.addr = psb_intel_sdvo->slave_addr,
+			.flags = 0,
+			.len = 1,
+			.buf = &addr,
+		},
+		{
+			.addr = psb_intel_sdvo->slave_addr,
+			.flags = I2C_M_RD,
+			.len = 1,
+			.buf = ch,
+		}
+	};
+	int ret;
+
+	if ((ret = i2c_transfer(psb_intel_sdvo->i2c, msgs, 2)) == 2)
+		return true;
+
+	DRM_DEBUG_KMS("i2c transfer returned %d\n", ret);
+	return false;
+}
+
+#define SDVO_CMD_NAME_ENTRY(cmd) {cmd, #cmd}
+/** Mapping of command numbers to names, for debug output */
+static const struct _sdvo_cmd_name {
+	u8 cmd;
+	const char *name;
+} sdvo_cmd_names[] = {
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_RESET),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_DEVICE_CAPS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_FIRMWARE_REV),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_TRAINED_INPUTS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_ACTIVE_OUTPUTS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_ACTIVE_OUTPUTS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_IN_OUT_MAP),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_IN_OUT_MAP),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_ATTACHED_DISPLAYS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HOT_PLUG_SUPPORT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_ACTIVE_HOT_PLUG),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_ACTIVE_HOT_PLUG),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_INTERRUPT_EVENT_SOURCE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_TARGET_INPUT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_TARGET_OUTPUT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_INPUT_TIMINGS_PART1),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_INPUT_TIMINGS_PART2),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_INPUT_TIMINGS_PART1),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_INPUT_TIMINGS_PART2),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_INPUT_TIMINGS_PART1),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_OUTPUT_TIMINGS_PART1),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_OUTPUT_TIMINGS_PART2),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_OUTPUT_TIMINGS_PART1),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_OUTPUT_TIMINGS_PART2),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_CREATE_PREFERRED_INPUT_TIMING),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART1),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART2),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_INPUT_PIXEL_CLOCK_RANGE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_OUTPUT_PIXEL_CLOCK_RANGE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SUPPORTED_CLOCK_RATE_MULTS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_CLOCK_RATE_MULT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_CLOCK_RATE_MULT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SUPPORTED_TV_FORMATS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_TV_FORMAT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_TV_FORMAT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SUPPORTED_POWER_STATES),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_POWER_STATE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_ENCODER_POWER_STATE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_DISPLAY_POWER_STATE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_CONTROL_BUS_SWITCH),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SDTV_RESOLUTION_SUPPORT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SCALED_HDTV_RESOLUTION_SUPPORT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SUPPORTED_ENHANCEMENTS),
+
+    /* Add the op code for SDVO enhancements */
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_HPOS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HPOS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_HPOS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_VPOS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_VPOS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_VPOS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_SATURATION),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SATURATION),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_SATURATION),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_HUE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HUE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_HUE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_CONTRAST),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_CONTRAST),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_CONTRAST),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_BRIGHTNESS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_BRIGHTNESS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_BRIGHTNESS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_OVERSCAN_H),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_OVERSCAN_H),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_OVERSCAN_H),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_OVERSCAN_V),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_OVERSCAN_V),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_OVERSCAN_V),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_FLICKER_FILTER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_FLICKER_FILTER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_FLICKER_FILTER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_FLICKER_FILTER_ADAPTIVE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_FLICKER_FILTER_ADAPTIVE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_FLICKER_FILTER_ADAPTIVE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_FLICKER_FILTER_2D),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_FLICKER_FILTER_2D),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_FLICKER_FILTER_2D),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_SHARPNESS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SHARPNESS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_SHARPNESS),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_DOT_CRAWL),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_DOT_CRAWL),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_TV_CHROMA_FILTER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_TV_CHROMA_FILTER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_TV_CHROMA_FILTER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_MAX_TV_LUMA_FILTER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_TV_LUMA_FILTER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_TV_LUMA_FILTER),
+
+    /* HDMI op code */
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SUPP_ENCODE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_ENCODE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_ENCODE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_PIXEL_REPLI),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_PIXEL_REPLI),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_COLORIMETRY_CAP),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_COLORIMETRY),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_COLORIMETRY),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_AUDIO_ENCRYPT_PREFER),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_AUDIO_STAT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_AUDIO_STAT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HBUF_INDEX),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_HBUF_INDEX),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HBUF_INFO),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HBUF_AV_SPLIT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_HBUF_AV_SPLIT),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HBUF_TXRATE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_HBUF_TXRATE),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_HBUF_DATA),
+    SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HBUF_DATA),
+};
+
+#define IS_SDVOB(reg)	(reg == SDVOB)
+#define SDVO_NAME(svdo) (IS_SDVOB((svdo)->sdvo_reg) ? "SDVOB" : "SDVOC")
+
+static void psb_intel_sdvo_debug_write(struct psb_intel_sdvo *psb_intel_sdvo, u8 cmd,
+				   const void *args, int args_len)
+{
+	int i;
+
+	DRM_DEBUG_KMS("%s: W: %02X ",
+				SDVO_NAME(psb_intel_sdvo), cmd);
+	for (i = 0; i < args_len; i++)
+		DRM_LOG_KMS("%02X ", ((u8 *)args)[i]);
+	for (; i < 8; i++)
+		DRM_LOG_KMS("   ");
+	for (i = 0; i < ARRAY_SIZE(sdvo_cmd_names); i++) {
+		if (cmd == sdvo_cmd_names[i].cmd) {
+			DRM_LOG_KMS("(%s)", sdvo_cmd_names[i].name);
+			break;
+		}
+	}
+	if (i == ARRAY_SIZE(sdvo_cmd_names))
+		DRM_LOG_KMS("(%02X)", cmd);
+	DRM_LOG_KMS("\n");
+}
+
+static const char *cmd_status_names[] = {
+	"Power on",
+	"Success",
+	"Not supported",
+	"Invalid arg",
+	"Pending",
+	"Target not specified",
+	"Scaling not supported"
+};
+
+static bool psb_intel_sdvo_write_cmd(struct psb_intel_sdvo *psb_intel_sdvo, u8 cmd,
+				 const void *args, int args_len)
+{
+	u8 buf[args_len*2 + 2], status;
+	struct i2c_msg msgs[args_len + 3];
+	int i, ret;
+
+	psb_intel_sdvo_debug_write(psb_intel_sdvo, cmd, args, args_len);
+
+	for (i = 0; i < args_len; i++) {
+		msgs[i].addr = psb_intel_sdvo->slave_addr;
+		msgs[i].flags = 0;
+		msgs[i].len = 2;
+		msgs[i].buf = buf + 2 *i;
+		buf[2*i + 0] = SDVO_I2C_ARG_0 - i;
+		buf[2*i + 1] = ((u8*)args)[i];
+	}
+	msgs[i].addr = psb_intel_sdvo->slave_addr;
+	msgs[i].flags = 0;
+	msgs[i].len = 2;
+	msgs[i].buf = buf + 2*i;
+	buf[2*i + 0] = SDVO_I2C_OPCODE;
+	buf[2*i + 1] = cmd;
+
+	/* the following two are to read the response */
+	status = SDVO_I2C_CMD_STATUS;
+	msgs[i+1].addr = psb_intel_sdvo->slave_addr;
+	msgs[i+1].flags = 0;
+	msgs[i+1].len = 1;
+	msgs[i+1].buf = &status;
+
+	msgs[i+2].addr = psb_intel_sdvo->slave_addr;
+	msgs[i+2].flags = I2C_M_RD;
+	msgs[i+2].len = 1;
+	msgs[i+2].buf = &status;
+
+	ret = i2c_transfer(psb_intel_sdvo->i2c, msgs, i+3);
+	if (ret < 0) {
+		DRM_DEBUG_KMS("I2c transfer returned %d\n", ret);
+		return false;
+	}
+	if (ret != i+3) {
+		/* failure in I2C transfer */
+		DRM_DEBUG_KMS("I2c transfer returned %d/%d\n", ret, i+3);
+		return false;
+	}
+
+	return true;
+}
+
+static bool psb_intel_sdvo_read_response(struct psb_intel_sdvo *psb_intel_sdvo,
+				     void *response, int response_len)
+{
+	u8 retry = 5;
+	u8 status;
+	int i;
+
+	DRM_DEBUG_KMS("%s: R: ", SDVO_NAME(psb_intel_sdvo));
+
+	/*
+	 * The documentation states that all commands will be
+	 * processed within 15µs, and that we need only poll
+	 * the status byte a maximum of 3 times in order for the
+	 * command to be complete.
+	 *
+	 * Check 5 times in case the hardware failed to read the docs.
+	 */
+	if (!psb_intel_sdvo_read_byte(psb_intel_sdvo,
+				  SDVO_I2C_CMD_STATUS,
+				  &status))
+		goto log_fail;
+
+	while (status == SDVO_CMD_STATUS_PENDING && retry--) {
+		udelay(15);
+		if (!psb_intel_sdvo_read_byte(psb_intel_sdvo,
+					  SDVO_I2C_CMD_STATUS,
+					  &status))
+			goto log_fail;
+	}
+
+	if (status <= SDVO_CMD_STATUS_SCALING_NOT_SUPP)
+		DRM_LOG_KMS("(%s)", cmd_status_names[status]);
+	else
+		DRM_LOG_KMS("(??? %d)", status);
+
+	if (status != SDVO_CMD_STATUS_SUCCESS)
+		goto log_fail;
+
+	/* Read the command response */
+	for (i = 0; i < response_len; i++) {
+		if (!psb_intel_sdvo_read_byte(psb_intel_sdvo,
+					  SDVO_I2C_RETURN_0 + i,
+					  &((u8 *)response)[i]))
+			goto log_fail;
+		DRM_LOG_KMS(" %02X", ((u8 *)response)[i]);
+	}
+	DRM_LOG_KMS("\n");
+	return true;
+
+log_fail:
+	DRM_LOG_KMS("... failed\n");
+	return false;
+}
+
+static int psb_intel_sdvo_get_pixel_multiplier(struct drm_display_mode *mode)
+{
+	if (mode->clock >= 100000)
+		return 1;
+	else if (mode->clock >= 50000)
+		return 2;
+	else
+		return 4;
+}
+
+static bool psb_intel_sdvo_set_control_bus_switch(struct psb_intel_sdvo *psb_intel_sdvo,
+					      u8 ddc_bus)
+{
+	/* This must be the immediately preceding write before the i2c xfer */
+	return psb_intel_sdvo_write_cmd(psb_intel_sdvo,
+				    SDVO_CMD_SET_CONTROL_BUS_SWITCH,
+				    &ddc_bus, 1);
+}
+
+static bool psb_intel_sdvo_set_value(struct psb_intel_sdvo *psb_intel_sdvo, u8 cmd, const void *data, int len)
+{
+	if (!psb_intel_sdvo_write_cmd(psb_intel_sdvo, cmd, data, len))
+		return false;
+
+	return psb_intel_sdvo_read_response(psb_intel_sdvo, NULL, 0);
+}
+
+static bool
+psb_intel_sdvo_get_value(struct psb_intel_sdvo *psb_intel_sdvo, u8 cmd, void *value, int len)
+{
+	if (!psb_intel_sdvo_write_cmd(psb_intel_sdvo, cmd, NULL, 0))
+		return false;
+
+	return psb_intel_sdvo_read_response(psb_intel_sdvo, value, len);
+}
+
+static bool psb_intel_sdvo_set_target_input(struct psb_intel_sdvo *psb_intel_sdvo)
+{
+	struct psb_intel_sdvo_set_target_input_args targets = {0};
+	return psb_intel_sdvo_set_value(psb_intel_sdvo,
+				    SDVO_CMD_SET_TARGET_INPUT,
+				    &targets, sizeof(targets));
+}
+
+/**
+ * Return whether each input is trained.
+ *
+ * This function is making an assumption about the layout of the response,
+ * which should be checked against the docs.
+ */
+static bool psb_intel_sdvo_get_trained_inputs(struct psb_intel_sdvo *psb_intel_sdvo, bool *input_1, bool *input_2)
+{
+	struct psb_intel_sdvo_get_trained_inputs_response response;
+
+	BUILD_BUG_ON(sizeof(response) != 1);
+	if (!psb_intel_sdvo_get_value(psb_intel_sdvo, SDVO_CMD_GET_TRAINED_INPUTS,
+				  &response, sizeof(response)))
+		return false;
+
+	*input_1 = response.input0_trained;
+	*input_2 = response.input1_trained;
+	return true;
+}
+
+static bool psb_intel_sdvo_set_active_outputs(struct psb_intel_sdvo *psb_intel_sdvo,
+					  u16 outputs)
+{
+	return psb_intel_sdvo_set_value(psb_intel_sdvo,
+				    SDVO_CMD_SET_ACTIVE_OUTPUTS,
+				    &outputs, sizeof(outputs));
+}
+
+static bool psb_intel_sdvo_set_encoder_power_state(struct psb_intel_sdvo *psb_intel_sdvo,
+					       int mode)
+{
+	u8 state = SDVO_ENCODER_STATE_ON;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		state = SDVO_ENCODER_STATE_ON;
+		break;
+	case DRM_MODE_DPMS_STANDBY:
+		state = SDVO_ENCODER_STATE_STANDBY;
+		break;
+	case DRM_MODE_DPMS_SUSPEND:
+		state = SDVO_ENCODER_STATE_SUSPEND;
+		break;
+	case DRM_MODE_DPMS_OFF:
+		state = SDVO_ENCODER_STATE_OFF;
+		break;
+	}
+
+	return psb_intel_sdvo_set_value(psb_intel_sdvo,
+				    SDVO_CMD_SET_ENCODER_POWER_STATE, &state, sizeof(state));
+}
+
+static bool psb_intel_sdvo_get_input_pixel_clock_range(struct psb_intel_sdvo *psb_intel_sdvo,
+						   int *clock_min,
+						   int *clock_max)
+{
+	struct psb_intel_sdvo_pixel_clock_range clocks;
+
+	BUILD_BUG_ON(sizeof(clocks) != 4);
+	if (!psb_intel_sdvo_get_value(psb_intel_sdvo,
+				  SDVO_CMD_GET_INPUT_PIXEL_CLOCK_RANGE,
+				  &clocks, sizeof(clocks)))
+		return false;
+
+	/* Convert the values from units of 10 kHz to kHz. */
+	*clock_min = clocks.min * 10;
+	*clock_max = clocks.max * 10;
+	return true;
+}
+
+static bool psb_intel_sdvo_set_target_output(struct psb_intel_sdvo *psb_intel_sdvo,
+					 u16 outputs)
+{
+	return psb_intel_sdvo_set_value(psb_intel_sdvo,
+				    SDVO_CMD_SET_TARGET_OUTPUT,
+				    &outputs, sizeof(outputs));
+}
+
+static bool psb_intel_sdvo_set_timing(struct psb_intel_sdvo *psb_intel_sdvo, u8 cmd,
+				  struct psb_intel_sdvo_dtd *dtd)
+{
+	return psb_intel_sdvo_set_value(psb_intel_sdvo, cmd, &dtd->part1, sizeof(dtd->part1)) &&
+		psb_intel_sdvo_set_value(psb_intel_sdvo, cmd + 1, &dtd->part2, sizeof(dtd->part2));
+}
+
+static bool psb_intel_sdvo_set_input_timing(struct psb_intel_sdvo *psb_intel_sdvo,
+					 struct psb_intel_sdvo_dtd *dtd)
+{
+	return psb_intel_sdvo_set_timing(psb_intel_sdvo,
+				     SDVO_CMD_SET_INPUT_TIMINGS_PART1, dtd);
+}
+
+static bool psb_intel_sdvo_set_output_timing(struct psb_intel_sdvo *psb_intel_sdvo,
+					 struct psb_intel_sdvo_dtd *dtd)
+{
+	return psb_intel_sdvo_set_timing(psb_intel_sdvo,
+				     SDVO_CMD_SET_OUTPUT_TIMINGS_PART1, dtd);
+}
+
+static bool
+psb_intel_sdvo_create_preferred_input_timing(struct psb_intel_sdvo *psb_intel_sdvo,
+					 uint16_t clock,
+					 uint16_t width,
+					 uint16_t height)
+{
+	struct psb_intel_sdvo_preferred_input_timing_args args;
+
+	memset(&args, 0, sizeof(args));
+	args.clock = clock;
+	args.width = width;
+	args.height = height;
+	args.interlace = 0;
+
+	if (psb_intel_sdvo->is_lvds &&
+	   (psb_intel_sdvo->sdvo_lvds_fixed_mode->hdisplay != width ||
+	    psb_intel_sdvo->sdvo_lvds_fixed_mode->vdisplay != height))
+		args.scaled = 1;
+
+	return psb_intel_sdvo_set_value(psb_intel_sdvo,
+				    SDVO_CMD_CREATE_PREFERRED_INPUT_TIMING,
+				    &args, sizeof(args));
+}
+
+static bool psb_intel_sdvo_get_preferred_input_timing(struct psb_intel_sdvo *psb_intel_sdvo,
+						  struct psb_intel_sdvo_dtd *dtd)
+{
+	BUILD_BUG_ON(sizeof(dtd->part1) != 8);
+	BUILD_BUG_ON(sizeof(dtd->part2) != 8);
+	return psb_intel_sdvo_get_value(psb_intel_sdvo, SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART1,
+				    &dtd->part1, sizeof(dtd->part1)) &&
+		psb_intel_sdvo_get_value(psb_intel_sdvo, SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART2,
+				     &dtd->part2, sizeof(dtd->part2));
+}
+
+static bool psb_intel_sdvo_set_clock_rate_mult(struct psb_intel_sdvo *psb_intel_sdvo, u8 val)
+{
+	return psb_intel_sdvo_set_value(psb_intel_sdvo, SDVO_CMD_SET_CLOCK_RATE_MULT, &val, 1);
+}
+
+static void psb_intel_sdvo_get_dtd_from_mode(struct psb_intel_sdvo_dtd *dtd,
+					 const struct drm_display_mode *mode)
+{
+	uint16_t width, height;
+	uint16_t h_blank_len, h_sync_len, v_blank_len, v_sync_len;
+	uint16_t h_sync_offset, v_sync_offset;
+
+	width = mode->crtc_hdisplay;
+	height = mode->crtc_vdisplay;
+
+	/* do some mode translations */
+	h_blank_len = mode->crtc_hblank_end - mode->crtc_hblank_start;
+	h_sync_len = mode->crtc_hsync_end - mode->crtc_hsync_start;
+
+	v_blank_len = mode->crtc_vblank_end - mode->crtc_vblank_start;
+	v_sync_len = mode->crtc_vsync_end - mode->crtc_vsync_start;
+
+	h_sync_offset = mode->crtc_hsync_start - mode->crtc_hblank_start;
+	v_sync_offset = mode->crtc_vsync_start - mode->crtc_vblank_start;
+
+	dtd->part1.clock = mode->clock / 10;
+	dtd->part1.h_active = width & 0xff;
+	dtd->part1.h_blank = h_blank_len & 0xff;
+	dtd->part1.h_high = (((width >> 8) & 0xf) << 4) |
+		((h_blank_len >> 8) & 0xf);
+	dtd->part1.v_active = height & 0xff;
+	dtd->part1.v_blank = v_blank_len & 0xff;
+	dtd->part1.v_high = (((height >> 8) & 0xf) << 4) |
+		((v_blank_len >> 8) & 0xf);
+
+	dtd->part2.h_sync_off = h_sync_offset & 0xff;
+	dtd->part2.h_sync_width = h_sync_len & 0xff;
+	dtd->part2.v_sync_off_width = (v_sync_offset & 0xf) << 4 |
+		(v_sync_len & 0xf);
+	dtd->part2.sync_off_width_high = ((h_sync_offset & 0x300) >> 2) |
+		((h_sync_len & 0x300) >> 4) | ((v_sync_offset & 0x30) >> 2) |
+		((v_sync_len & 0x30) >> 4);
+
+	dtd->part2.dtd_flags = 0x18;
+	if (mode->flags & DRM_MODE_FLAG_PHSYNC)
+		dtd->part2.dtd_flags |= 0x2;
+	if (mode->flags & DRM_MODE_FLAG_PVSYNC)
+		dtd->part2.dtd_flags |= 0x4;
+
+	dtd->part2.sdvo_flags = 0;
+	dtd->part2.v_sync_off_high = v_sync_offset & 0xc0;
+	dtd->part2.reserved = 0;
+}
+
+static void psb_intel_sdvo_get_mode_from_dtd(struct drm_display_mode * mode,
+					 const struct psb_intel_sdvo_dtd *dtd)
+{
+	mode->hdisplay = dtd->part1.h_active;
+	mode->hdisplay += ((dtd->part1.h_high >> 4) & 0x0f) << 8;
+	mode->hsync_start = mode->hdisplay + dtd->part2.h_sync_off;
+	mode->hsync_start += (dtd->part2.sync_off_width_high & 0xc0) << 2;
+	mode->hsync_end = mode->hsync_start + dtd->part2.h_sync_width;
+	mode->hsync_end += (dtd->part2.sync_off_width_high & 0x30) << 4;
+	mode->htotal = mode->hdisplay + dtd->part1.h_blank;
+	mode->htotal += (dtd->part1.h_high & 0xf) << 8;
+
+	mode->vdisplay = dtd->part1.v_active;
+	mode->vdisplay += ((dtd->part1.v_high >> 4) & 0x0f) << 8;
+	mode->vsync_start = mode->vdisplay;
+	mode->vsync_start += (dtd->part2.v_sync_off_width >> 4) & 0xf;
+	mode->vsync_start += (dtd->part2.sync_off_width_high & 0x0c) << 2;
+	mode->vsync_start += dtd->part2.v_sync_off_high & 0xc0;
+	mode->vsync_end = mode->vsync_start +
+		(dtd->part2.v_sync_off_width & 0xf);
+	mode->vsync_end += (dtd->part2.sync_off_width_high & 0x3) << 4;
+	mode->vtotal = mode->vdisplay + dtd->part1.v_blank;
+	mode->vtotal += (dtd->part1.v_high & 0xf) << 8;
+
+	mode->clock = dtd->part1.clock * 10;
+
+	mode->flags &= ~(DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC);
+	if (dtd->part2.dtd_flags & 0x2)
+		mode->flags |= DRM_MODE_FLAG_PHSYNC;
+	if (dtd->part2.dtd_flags & 0x4)
+		mode->flags |= DRM_MODE_FLAG_PVSYNC;
+}
+
+static bool psb_intel_sdvo_check_supp_encode(struct psb_intel_sdvo *psb_intel_sdvo)
+{
+	struct psb_intel_sdvo_encode encode;
+
+	BUILD_BUG_ON(sizeof(encode) != 2);
+	return psb_intel_sdvo_get_value(psb_intel_sdvo,
+				  SDVO_CMD_GET_SUPP_ENCODE,
+				  &encode, sizeof(encode));
+}
+
+static bool psb_intel_sdvo_set_encode(struct psb_intel_sdvo *psb_intel_sdvo,
+				  uint8_t mode)
+{
+	return psb_intel_sdvo_set_value(psb_intel_sdvo, SDVO_CMD_SET_ENCODE, &mode, 1);
+}
+
+static bool psb_intel_sdvo_set_colorimetry(struct psb_intel_sdvo *psb_intel_sdvo,
+				       uint8_t mode)
+{
+	return psb_intel_sdvo_set_value(psb_intel_sdvo, SDVO_CMD_SET_COLORIMETRY, &mode, 1);
+}
+
+#if 0
+static void psb_intel_sdvo_dump_hdmi_buf(struct psb_intel_sdvo *psb_intel_sdvo)
+{
+	int i, j;
+	uint8_t set_buf_index[2];
+	uint8_t av_split;
+	uint8_t buf_size;
+	uint8_t buf[48];
+	uint8_t *pos;
+
+	psb_intel_sdvo_get_value(encoder, SDVO_CMD_GET_HBUF_AV_SPLIT, &av_split, 1);
+
+	for (i = 0; i <= av_split; i++) {
+		set_buf_index[0] = i; set_buf_index[1] = 0;
+		psb_intel_sdvo_write_cmd(encoder, SDVO_CMD_SET_HBUF_INDEX,
+				     set_buf_index, 2);
+		psb_intel_sdvo_write_cmd(encoder, SDVO_CMD_GET_HBUF_INFO, NULL, 0);
+		psb_intel_sdvo_read_response(encoder, &buf_size, 1);
+
+		pos = buf;
+		for (j = 0; j <= buf_size; j += 8) {
+			psb_intel_sdvo_write_cmd(encoder, SDVO_CMD_GET_HBUF_DATA,
+					     NULL, 0);
+			psb_intel_sdvo_read_response(encoder, pos, 8);
+			pos += 8;
+		}
+	}
+}
+#endif
+
+static bool psb_intel_sdvo_set_avi_infoframe(struct psb_intel_sdvo *psb_intel_sdvo)
+{
+	DRM_INFO("HDMI is not supported yet");
+
+	return false;
+#if 0
+	struct dip_infoframe avi_if = {
+		.type = DIP_TYPE_AVI,
+		.ver = DIP_VERSION_AVI,
+		.len = DIP_LEN_AVI,
+	};
+	uint8_t tx_rate = SDVO_HBUF_TX_VSYNC;
+	uint8_t set_buf_index[2] = { 1, 0 };
+	uint64_t *data = (uint64_t *)&avi_if;
+	unsigned i;
+
+	intel_dip_infoframe_csum(&avi_if);
+
+	if (!psb_intel_sdvo_set_value(psb_intel_sdvo,
+				  SDVO_CMD_SET_HBUF_INDEX,
+				  set_buf_index, 2))
+		return false;
+
+	for (i = 0; i < sizeof(avi_if); i += 8) {
+		if (!psb_intel_sdvo_set_value(psb_intel_sdvo,
+					  SDVO_CMD_SET_HBUF_DATA,
+					  data, 8))
+			return false;
+		data++;
+	}
+
+	return psb_intel_sdvo_set_value(psb_intel_sdvo,
+				    SDVO_CMD_SET_HBUF_TXRATE,
+				    &tx_rate, 1);
+#endif
+}
+
+static bool psb_intel_sdvo_set_tv_format(struct psb_intel_sdvo *psb_intel_sdvo)
+{
+	struct psb_intel_sdvo_tv_format format;
+	uint32_t format_map;
+
+	format_map = 1 << psb_intel_sdvo->tv_format_index;
+	memset(&format, 0, sizeof(format));
+	memcpy(&format, &format_map, min(sizeof(format), sizeof(format_map)));
+
+	BUILD_BUG_ON(sizeof(format) != 6);
+	return psb_intel_sdvo_set_value(psb_intel_sdvo,
+				    SDVO_CMD_SET_TV_FORMAT,
+				    &format, sizeof(format));
+}
+
+static bool
+psb_intel_sdvo_set_output_timings_from_mode(struct psb_intel_sdvo *psb_intel_sdvo,
+					struct drm_display_mode *mode)
+{
+	struct psb_intel_sdvo_dtd output_dtd;
+
+	if (!psb_intel_sdvo_set_target_output(psb_intel_sdvo,
+					  psb_intel_sdvo->attached_output))
+		return false;
+
+	psb_intel_sdvo_get_dtd_from_mode(&output_dtd, mode);
+	if (!psb_intel_sdvo_set_output_timing(psb_intel_sdvo, &output_dtd))
+		return false;
+
+	return true;
+}
+
+static bool
+psb_intel_sdvo_set_input_timings_for_mode(struct psb_intel_sdvo *psb_intel_sdvo,
+					struct drm_display_mode *mode,
+					struct drm_display_mode *adjusted_mode)
+{
+	/* Reset the input timing to the screen. Assume always input 0. */
+	if (!psb_intel_sdvo_set_target_input(psb_intel_sdvo))
+		return false;
+
+	if (!psb_intel_sdvo_create_preferred_input_timing(psb_intel_sdvo,
+						      mode->clock / 10,
+						      mode->hdisplay,
+						      mode->vdisplay))
+		return false;
+
+	if (!psb_intel_sdvo_get_preferred_input_timing(psb_intel_sdvo,
+						   &psb_intel_sdvo->input_dtd))
+		return false;
+
+	psb_intel_sdvo_get_mode_from_dtd(adjusted_mode, &psb_intel_sdvo->input_dtd);
+
+	drm_mode_set_crtcinfo(adjusted_mode, 0);
+	return true;
+}
+
+static bool psb_intel_sdvo_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	struct psb_intel_sdvo *psb_intel_sdvo = to_psb_intel_sdvo(encoder);
+	int multiplier;
+
+	/* We need to construct preferred input timings based on our
+	 * output timings.  To do that, we have to set the output
+	 * timings, even though this isn't really the right place in
+	 * the sequence to do it. Oh well.
+	 */
+	if (psb_intel_sdvo->is_tv) {
+		if (!psb_intel_sdvo_set_output_timings_from_mode(psb_intel_sdvo, mode))
+			return false;
+
+		(void) psb_intel_sdvo_set_input_timings_for_mode(psb_intel_sdvo,
+							     mode,
+							     adjusted_mode);
+	} else if (psb_intel_sdvo->is_lvds) {
+		if (!psb_intel_sdvo_set_output_timings_from_mode(psb_intel_sdvo,
+							     psb_intel_sdvo->sdvo_lvds_fixed_mode))
+			return false;
+
+		(void) psb_intel_sdvo_set_input_timings_for_mode(psb_intel_sdvo,
+							     mode,
+							     adjusted_mode);
+	}
+
+	/* Make the CRTC code factor in the SDVO pixel multiplier.  The
+	 * SDVO device will factor out the multiplier during mode_set.
+	 */
+	multiplier = psb_intel_sdvo_get_pixel_multiplier(adjusted_mode);
+	psb_intel_mode_set_pixel_multiplier(adjusted_mode, multiplier);
+
+	return true;
+}
+
+static void psb_intel_sdvo_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_crtc *crtc = encoder->crtc;
+	struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+	struct psb_intel_sdvo *psb_intel_sdvo = to_psb_intel_sdvo(encoder);
+	u32 sdvox;
+	struct psb_intel_sdvo_in_out_map in_out;
+	struct psb_intel_sdvo_dtd input_dtd;
+	int pixel_multiplier = psb_intel_mode_get_pixel_multiplier(adjusted_mode);
+	int rate;
+
+	if (!mode)
+		return;
+
+	/* First, set the input mapping for the first input to our controlled
+	 * output. This is only correct if we're a single-input device, in
+	 * which case the first input is the output from the appropriate SDVO
+	 * channel on the motherboard.  In a two-input device, the first input
+	 * will be SDVOB and the second SDVOC.
+	 */
+	in_out.in0 = psb_intel_sdvo->attached_output;
+	in_out.in1 = 0;
+
+	psb_intel_sdvo_set_value(psb_intel_sdvo,
+			     SDVO_CMD_SET_IN_OUT_MAP,
+			     &in_out, sizeof(in_out));
+
+	/* Set the output timings to the screen */
+	if (!psb_intel_sdvo_set_target_output(psb_intel_sdvo,
+					  psb_intel_sdvo->attached_output))
+		return;
+
+	/* We have tried to get input timing in mode_fixup, and filled into
+	 * adjusted_mode.
+	 */
+	if (psb_intel_sdvo->is_tv || psb_intel_sdvo->is_lvds) {
+		input_dtd = psb_intel_sdvo->input_dtd;
+	} else {
+		/* Set the output timing to the screen */
+		if (!psb_intel_sdvo_set_target_output(psb_intel_sdvo,
+						  psb_intel_sdvo->attached_output))
+			return;
+
+		psb_intel_sdvo_get_dtd_from_mode(&input_dtd, adjusted_mode);
+		(void) psb_intel_sdvo_set_output_timing(psb_intel_sdvo, &input_dtd);
+	}
+
+	/* Set the input timing to the screen. Assume always input 0. */
+	if (!psb_intel_sdvo_set_target_input(psb_intel_sdvo))
+		return;
+
+	if (psb_intel_sdvo->has_hdmi_monitor) {
+		psb_intel_sdvo_set_encode(psb_intel_sdvo, SDVO_ENCODE_HDMI);
+		psb_intel_sdvo_set_colorimetry(psb_intel_sdvo,
+					   SDVO_COLORIMETRY_RGB256);
+		psb_intel_sdvo_set_avi_infoframe(psb_intel_sdvo);
+	} else
+		psb_intel_sdvo_set_encode(psb_intel_sdvo, SDVO_ENCODE_DVI);
+
+	if (psb_intel_sdvo->is_tv &&
+	    !psb_intel_sdvo_set_tv_format(psb_intel_sdvo))
+		return;
+
+	(void) psb_intel_sdvo_set_input_timing(psb_intel_sdvo, &input_dtd);
+
+	switch (pixel_multiplier) {
+	default:
+	case 1: rate = SDVO_CLOCK_RATE_MULT_1X; break;
+	case 2: rate = SDVO_CLOCK_RATE_MULT_2X; break;
+	case 4: rate = SDVO_CLOCK_RATE_MULT_4X; break;
+	}
+	if (!psb_intel_sdvo_set_clock_rate_mult(psb_intel_sdvo, rate))
+		return;
+
+	/* Set the SDVO control regs. */
+	sdvox = REG_READ(psb_intel_sdvo->sdvo_reg);
+	switch (psb_intel_sdvo->sdvo_reg) {
+	case SDVOB:
+		sdvox &= SDVOB_PRESERVE_MASK;
+		break;
+	case SDVOC:
+		sdvox &= SDVOC_PRESERVE_MASK;
+		break;
+	}
+	sdvox |= (9 << 19) | SDVO_BORDER_ENABLE;
+
+	if (psb_intel_crtc->pipe == 1)
+		sdvox |= SDVO_PIPE_B_SELECT;
+	if (psb_intel_sdvo->has_hdmi_audio)
+		sdvox |= SDVO_AUDIO_ENABLE;
+
+	/* FIXME: Check if this is needed for PSB
+	sdvox |= (pixel_multiplier - 1) << SDVO_PORT_MULTIPLY_SHIFT;
+	*/
+
+	if (input_dtd.part2.sdvo_flags & SDVO_NEED_TO_STALL)
+		sdvox |= SDVO_STALL_SELECT;
+	psb_intel_sdvo_write_sdvox(psb_intel_sdvo, sdvox);
+}
+
+static void psb_intel_sdvo_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct psb_intel_sdvo *psb_intel_sdvo = to_psb_intel_sdvo(encoder);
+	u32 temp;
+
+	switch (mode) {
+	case DRM_MODE_DPMS_ON:
+		DRM_DEBUG("DPMS_ON");
+		break;
+	case DRM_MODE_DPMS_OFF:
+		DRM_DEBUG("DPMS_OFF");
+		break;
+	default:
+		DRM_DEBUG("DPMS: %d", mode);
+	}
+
+	if (mode != DRM_MODE_DPMS_ON) {
+		psb_intel_sdvo_set_active_outputs(psb_intel_sdvo, 0);
+		if (0)
+			psb_intel_sdvo_set_encoder_power_state(psb_intel_sdvo, mode);
+
+		if (mode == DRM_MODE_DPMS_OFF) {
+			temp = REG_READ(psb_intel_sdvo->sdvo_reg);
+			if ((temp & SDVO_ENABLE) != 0) {
+				psb_intel_sdvo_write_sdvox(psb_intel_sdvo, temp & ~SDVO_ENABLE);
+			}
+		}
+	} else {
+		bool input1, input2;
+		int i;
+		u8 status;
+
+		temp = REG_READ(psb_intel_sdvo->sdvo_reg);
+		if ((temp & SDVO_ENABLE) == 0)
+			psb_intel_sdvo_write_sdvox(psb_intel_sdvo, temp | SDVO_ENABLE);
+		for (i = 0; i < 2; i++)
+			psb_intel_wait_for_vblank(dev);
+
+		status = psb_intel_sdvo_get_trained_inputs(psb_intel_sdvo, &input1, &input2);
+		/* Warn if the device reported failure to sync.
+		 * A lot of SDVO devices fail to notify of sync, but it's
+		 * a given it the status is a success, we succeeded.
+		 */
+		if (status == SDVO_CMD_STATUS_SUCCESS && !input1) {
+			DRM_DEBUG_KMS("First %s output reported failure to "
+					"sync\n", SDVO_NAME(psb_intel_sdvo));
+		}
+
+		if (0)
+			psb_intel_sdvo_set_encoder_power_state(psb_intel_sdvo, mode);
+		psb_intel_sdvo_set_active_outputs(psb_intel_sdvo, psb_intel_sdvo->attached_output);
+	}
+	return;
+}
+
+static int psb_intel_sdvo_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
+
+	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+		return MODE_NO_DBLESCAN;
+
+	if (psb_intel_sdvo->pixel_clock_min > mode->clock)
+		return MODE_CLOCK_LOW;
+
+	if (psb_intel_sdvo->pixel_clock_max < mode->clock)
+		return MODE_CLOCK_HIGH;
+
+	if (psb_intel_sdvo->is_lvds) {
+		if (mode->hdisplay > psb_intel_sdvo->sdvo_lvds_fixed_mode->hdisplay)
+			return MODE_PANEL;
+
+		if (mode->vdisplay > psb_intel_sdvo->sdvo_lvds_fixed_mode->vdisplay)
+			return MODE_PANEL;
+	}
+
+	return MODE_OK;
+}
+
+static bool psb_intel_sdvo_get_capabilities(struct psb_intel_sdvo *psb_intel_sdvo, struct psb_intel_sdvo_caps *caps)
+{
+	BUILD_BUG_ON(sizeof(*caps) != 8);
+	if (!psb_intel_sdvo_get_value(psb_intel_sdvo,
+				  SDVO_CMD_GET_DEVICE_CAPS,
+				  caps, sizeof(*caps)))
+		return false;
+
+	DRM_DEBUG_KMS("SDVO capabilities:\n"
+		      "  vendor_id: %d\n"
+		      "  device_id: %d\n"
+		      "  device_rev_id: %d\n"
+		      "  sdvo_version_major: %d\n"
+		      "  sdvo_version_minor: %d\n"
+		      "  sdvo_inputs_mask: %d\n"
+		      "  smooth_scaling: %d\n"
+		      "  sharp_scaling: %d\n"
+		      "  up_scaling: %d\n"
+		      "  down_scaling: %d\n"
+		      "  stall_support: %d\n"
+		      "  output_flags: %d\n",
+		      caps->vendor_id,
+		      caps->device_id,
+		      caps->device_rev_id,
+		      caps->sdvo_version_major,
+		      caps->sdvo_version_minor,
+		      caps->sdvo_inputs_mask,
+		      caps->smooth_scaling,
+		      caps->sharp_scaling,
+		      caps->up_scaling,
+		      caps->down_scaling,
+		      caps->stall_support,
+		      caps->output_flags);
+
+	return true;
+}
+
+/* No use! */
+#if 0
+struct drm_connector* psb_intel_sdvo_find(struct drm_device *dev, int sdvoB)
+{
+	struct drm_connector *connector = NULL;
+	struct psb_intel_sdvo *iout = NULL;
+	struct psb_intel_sdvo *sdvo;
+
+	/* find the sdvo connector */
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		iout = to_psb_intel_sdvo(connector);
+
+		if (iout->type != INTEL_OUTPUT_SDVO)
+			continue;
+
+		sdvo = iout->dev_priv;
+
+		if (sdvo->sdvo_reg == SDVOB && sdvoB)
+			return connector;
+
+		if (sdvo->sdvo_reg == SDVOC && !sdvoB)
+			return connector;
+
+	}
+
+	return NULL;
+}
+
+int psb_intel_sdvo_supports_hotplug(struct drm_connector *connector)
+{
+	u8 response[2];
+	u8 status;
+	struct psb_intel_sdvo *psb_intel_sdvo;
+	DRM_DEBUG_KMS("\n");
+
+	if (!connector)
+		return 0;
+
+	psb_intel_sdvo = to_psb_intel_sdvo(connector);
+
+	return psb_intel_sdvo_get_value(psb_intel_sdvo, SDVO_CMD_GET_HOT_PLUG_SUPPORT,
+				    &response, 2) && response[0];
+}
+
+void psb_intel_sdvo_set_hotplug(struct drm_connector *connector, int on)
+{
+	u8 response[2];
+	u8 status;
+	struct psb_intel_sdvo *psb_intel_sdvo = to_psb_intel_sdvo(connector);
+
+	psb_intel_sdvo_write_cmd(psb_intel_sdvo, SDVO_CMD_GET_ACTIVE_HOT_PLUG, NULL, 0);
+	psb_intel_sdvo_read_response(psb_intel_sdvo, &response, 2);
+
+	if (on) {
+		psb_intel_sdvo_write_cmd(psb_intel_sdvo, SDVO_CMD_GET_HOT_PLUG_SUPPORT, NULL, 0);
+		status = psb_intel_sdvo_read_response(psb_intel_sdvo, &response, 2);
+
+		psb_intel_sdvo_write_cmd(psb_intel_sdvo, SDVO_CMD_SET_ACTIVE_HOT_PLUG, &response, 2);
+	} else {
+		response[0] = 0;
+		response[1] = 0;
+		psb_intel_sdvo_write_cmd(psb_intel_sdvo, SDVO_CMD_SET_ACTIVE_HOT_PLUG, &response, 2);
+	}
+
+	psb_intel_sdvo_write_cmd(psb_intel_sdvo, SDVO_CMD_GET_ACTIVE_HOT_PLUG, NULL, 0);
+	psb_intel_sdvo_read_response(psb_intel_sdvo, &response, 2);
+}
+#endif
+
+static bool
+psb_intel_sdvo_multifunc_encoder(struct psb_intel_sdvo *psb_intel_sdvo)
+{
+	/* Is there more than one type of output? */
+	int caps = psb_intel_sdvo->caps.output_flags & 0xf;
+	return caps & -caps;
+}
+
+static struct edid *
+psb_intel_sdvo_get_edid(struct drm_connector *connector)
+{
+	struct psb_intel_sdvo *sdvo = intel_attached_sdvo(connector);
+	return drm_get_edid(connector, &sdvo->ddc);
+}
+
+/* Mac mini hack -- use the same DDC as the analog connector */
+static struct edid *
+psb_intel_sdvo_get_analog_edid(struct drm_connector *connector)
+{
+	struct drm_psb_private *dev_priv = connector->dev->dev_private;
+
+	return drm_get_edid(connector,
+			    &dev_priv->gmbus[dev_priv->crt_ddc_pin].adapter);
+	return NULL;
+}
+
+enum drm_connector_status
+psb_intel_sdvo_hdmi_sink_detect(struct drm_connector *connector)
+{
+	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
+	enum drm_connector_status status;
+	struct edid *edid;
+
+	edid = psb_intel_sdvo_get_edid(connector);
+
+	if (edid == NULL && psb_intel_sdvo_multifunc_encoder(psb_intel_sdvo)) {
+		u8 ddc, saved_ddc = psb_intel_sdvo->ddc_bus;
+
+		/*
+		 * Don't use the 1 as the argument of DDC bus switch to get
+		 * the EDID. It is used for SDVO SPD ROM.
+		 */
+		for (ddc = psb_intel_sdvo->ddc_bus >> 1; ddc > 1; ddc >>= 1) {
+			psb_intel_sdvo->ddc_bus = ddc;
+			edid = psb_intel_sdvo_get_edid(connector);
+			if (edid)
+				break;
+		}
+		/*
+		 * If we found the EDID on the other bus,
+		 * assume that is the correct DDC bus.
+		 */
+		if (edid == NULL)
+			psb_intel_sdvo->ddc_bus = saved_ddc;
+	}
+
+	/*
+	 * When there is no edid and no monitor is connected with VGA
+	 * port, try to use the CRT ddc to read the EDID for DVI-connector.
+	 */
+	if (edid == NULL)
+		edid = psb_intel_sdvo_get_analog_edid(connector);
+
+	status = connector_status_unknown;
+	if (edid != NULL) {
+		/* DDC bus is shared, match EDID to connector type */
+		if (edid->input & DRM_EDID_INPUT_DIGITAL) {
+			status = connector_status_connected;
+			if (psb_intel_sdvo->is_hdmi) {
+				psb_intel_sdvo->has_hdmi_monitor = drm_detect_hdmi_monitor(edid);
+				psb_intel_sdvo->has_hdmi_audio = drm_detect_monitor_audio(edid);
+			}
+		} else
+			status = connector_status_disconnected;
+		connector->display_info.raw_edid = NULL;
+		kfree(edid);
+	}
+
+	if (status == connector_status_connected) {
+		struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
+		if (psb_intel_sdvo_connector->force_audio)
+			psb_intel_sdvo->has_hdmi_audio = psb_intel_sdvo_connector->force_audio > 0;
+	}
+
+	return status;
+}
+
+static enum drm_connector_status
+psb_intel_sdvo_detect(struct drm_connector *connector, bool force)
+{
+	uint16_t response;
+	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
+	enum drm_connector_status ret;
+
+	if (!psb_intel_sdvo_write_cmd(psb_intel_sdvo,
+				  SDVO_CMD_GET_ATTACHED_DISPLAYS, NULL, 0))
+		return connector_status_unknown;
+
+	/* add 30ms delay when the output type might be TV */
+	if (psb_intel_sdvo->caps.output_flags &
+	    (SDVO_OUTPUT_SVID0 | SDVO_OUTPUT_CVBS0))
+		mdelay(30);
+
+	if (!psb_intel_sdvo_read_response(psb_intel_sdvo, &response, 2))
+		return connector_status_unknown;
+
+	DRM_DEBUG_KMS("SDVO response %d %d [%x]\n",
+		      response & 0xff, response >> 8,
+		      psb_intel_sdvo_connector->output_flag);
+
+	if (response == 0)
+		return connector_status_disconnected;
+
+	psb_intel_sdvo->attached_output = response;
+
+	psb_intel_sdvo->has_hdmi_monitor = false;
+	psb_intel_sdvo->has_hdmi_audio = false;
+
+	if ((psb_intel_sdvo_connector->output_flag & response) == 0)
+		ret = connector_status_disconnected;
+	else if (IS_TMDS(psb_intel_sdvo_connector))
+		ret = psb_intel_sdvo_hdmi_sink_detect(connector);
+	else {
+		struct edid *edid;
+
+		/* if we have an edid check it matches the connection */
+		edid = psb_intel_sdvo_get_edid(connector);
+		if (edid == NULL)
+			edid = psb_intel_sdvo_get_analog_edid(connector);
+		if (edid != NULL) {
+			if (edid->input & DRM_EDID_INPUT_DIGITAL)
+				ret = connector_status_disconnected;
+			else
+				ret = connector_status_connected;
+			connector->display_info.raw_edid = NULL;
+			kfree(edid);
+		} else
+			ret = connector_status_connected;
+	}
+
+	/* May update encoder flag for like clock for SDVO TV, etc.*/
+	if (ret == connector_status_connected) {
+		psb_intel_sdvo->is_tv = false;
+		psb_intel_sdvo->is_lvds = false;
+		psb_intel_sdvo->base.needs_tv_clock = false;
+
+		if (response & SDVO_TV_MASK) {
+			psb_intel_sdvo->is_tv = true;
+			psb_intel_sdvo->base.needs_tv_clock = true;
+		}
+		if (response & SDVO_LVDS_MASK)
+			psb_intel_sdvo->is_lvds = psb_intel_sdvo->sdvo_lvds_fixed_mode != NULL;
+	}
+
+	return ret;
+}
+
+static void psb_intel_sdvo_get_ddc_modes(struct drm_connector *connector)
+{
+	struct edid *edid;
+
+	/* set the bus switch and get the modes */
+	edid = psb_intel_sdvo_get_edid(connector);
+
+	/*
+	 * Mac mini hack.  On this device, the DVI-I connector shares one DDC
+	 * link between analog and digital outputs. So, if the regular SDVO
+	 * DDC fails, check to see if the analog output is disconnected, in
+	 * which case we'll look there for the digital DDC data.
+	 */
+	if (edid == NULL)
+		edid = psb_intel_sdvo_get_analog_edid(connector);
+
+	if (edid != NULL) {
+		struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
+		bool monitor_is_digital = !!(edid->input & DRM_EDID_INPUT_DIGITAL);
+		bool connector_is_digital = !!IS_TMDS(psb_intel_sdvo_connector);
+
+		if (connector_is_digital == monitor_is_digital) {
+			drm_mode_connector_update_edid_property(connector, edid);
+			drm_add_edid_modes(connector, edid);
+		}
+
+		connector->display_info.raw_edid = NULL;
+		kfree(edid);
+	}
+}
+
+/*
+ * Set of SDVO TV modes.
+ * Note!  This is in reply order (see loop in get_tv_modes).
+ * XXX: all 60Hz refresh?
+ */
+static const struct drm_display_mode sdvo_tv_modes[] = {
+	{ DRM_MODE("320x200", DRM_MODE_TYPE_DRIVER, 5815, 320, 321, 384,
+		   416, 0, 200, 201, 232, 233, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("320x240", DRM_MODE_TYPE_DRIVER, 6814, 320, 321, 384,
+		   416, 0, 240, 241, 272, 273, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("400x300", DRM_MODE_TYPE_DRIVER, 9910, 400, 401, 464,
+		   496, 0, 300, 301, 332, 333, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("640x350", DRM_MODE_TYPE_DRIVER, 16913, 640, 641, 704,
+		   736, 0, 350, 351, 382, 383, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("640x400", DRM_MODE_TYPE_DRIVER, 19121, 640, 641, 704,
+		   736, 0, 400, 401, 432, 433, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("640x480", DRM_MODE_TYPE_DRIVER, 22654, 640, 641, 704,
+		   736, 0, 480, 481, 512, 513, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("704x480", DRM_MODE_TYPE_DRIVER, 24624, 704, 705, 768,
+		   800, 0, 480, 481, 512, 513, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("704x576", DRM_MODE_TYPE_DRIVER, 29232, 704, 705, 768,
+		   800, 0, 576, 577, 608, 609, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("720x350", DRM_MODE_TYPE_DRIVER, 18751, 720, 721, 784,
+		   816, 0, 350, 351, 382, 383, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("720x400", DRM_MODE_TYPE_DRIVER, 21199, 720, 721, 784,
+		   816, 0, 400, 401, 432, 433, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("720x480", DRM_MODE_TYPE_DRIVER, 25116, 720, 721, 784,
+		   816, 0, 480, 481, 512, 513, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("720x540", DRM_MODE_TYPE_DRIVER, 28054, 720, 721, 784,
+		   816, 0, 540, 541, 572, 573, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("720x576", DRM_MODE_TYPE_DRIVER, 29816, 720, 721, 784,
+		   816, 0, 576, 577, 608, 609, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("768x576", DRM_MODE_TYPE_DRIVER, 31570, 768, 769, 832,
+		   864, 0, 576, 577, 608, 609, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("800x600", DRM_MODE_TYPE_DRIVER, 34030, 800, 801, 864,
+		   896, 0, 600, 601, 632, 633, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("832x624", DRM_MODE_TYPE_DRIVER, 36581, 832, 833, 896,
+		   928, 0, 624, 625, 656, 657, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("920x766", DRM_MODE_TYPE_DRIVER, 48707, 920, 921, 984,
+		   1016, 0, 766, 767, 798, 799, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("1024x768", DRM_MODE_TYPE_DRIVER, 53827, 1024, 1025, 1088,
+		   1120, 0, 768, 769, 800, 801, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+	{ DRM_MODE("1280x1024", DRM_MODE_TYPE_DRIVER, 87265, 1280, 1281, 1344,
+		   1376, 0, 1024, 1025, 1056, 1057, 0,
+		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+};
+
+static void psb_intel_sdvo_get_tv_modes(struct drm_connector *connector)
+{
+	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
+	struct psb_intel_sdvo_sdtv_resolution_request tv_res;
+	uint32_t reply = 0, format_map = 0;
+	int i;
+
+	/* Read the list of supported input resolutions for the selected TV
+	 * format.
+	 */
+	format_map = 1 << psb_intel_sdvo->tv_format_index;
+	memcpy(&tv_res, &format_map,
+	       min(sizeof(format_map), sizeof(struct psb_intel_sdvo_sdtv_resolution_request)));
+
+	if (!psb_intel_sdvo_set_target_output(psb_intel_sdvo, psb_intel_sdvo->attached_output))
+		return;
+
+	BUILD_BUG_ON(sizeof(tv_res) != 3);
+	if (!psb_intel_sdvo_write_cmd(psb_intel_sdvo,
+				  SDVO_CMD_GET_SDTV_RESOLUTION_SUPPORT,
+				  &tv_res, sizeof(tv_res)))
+		return;
+	if (!psb_intel_sdvo_read_response(psb_intel_sdvo, &reply, 3))
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(sdvo_tv_modes); i++)
+		if (reply & (1 << i)) {
+			struct drm_display_mode *nmode;
+			nmode = drm_mode_duplicate(connector->dev,
+						   &sdvo_tv_modes[i]);
+			if (nmode)
+				drm_mode_probed_add(connector, nmode);
+		}
+}
+
+static void psb_intel_sdvo_get_lvds_modes(struct drm_connector *connector)
+{
+	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
+	struct drm_psb_private *dev_priv = connector->dev->dev_private;
+	struct drm_display_mode *newmode;
+
+	/*
+	 * Attempt to get the mode list from DDC.
+	 * Assume that the preferred modes are
+	 * arranged in priority order.
+	 */
+	psb_intel_ddc_get_modes(connector, psb_intel_sdvo->i2c);
+	if (list_empty(&connector->probed_modes) == false)
+		goto end;
+
+	/* Fetch modes from VBT */
+	if (dev_priv->sdvo_lvds_vbt_mode != NULL) {
+		newmode = drm_mode_duplicate(connector->dev,
+					     dev_priv->sdvo_lvds_vbt_mode);
+		if (newmode != NULL) {
+			/* Guarantee the mode is preferred */
+			newmode->type = (DRM_MODE_TYPE_PREFERRED |
+					 DRM_MODE_TYPE_DRIVER);
+			drm_mode_probed_add(connector, newmode);
+		}
+	}
+
+end:
+	list_for_each_entry(newmode, &connector->probed_modes, head) {
+		if (newmode->type & DRM_MODE_TYPE_PREFERRED) {
+			psb_intel_sdvo->sdvo_lvds_fixed_mode =
+				drm_mode_duplicate(connector->dev, newmode);
+
+			drm_mode_set_crtcinfo(psb_intel_sdvo->sdvo_lvds_fixed_mode,
+					      0);
+
+			psb_intel_sdvo->is_lvds = true;
+			break;
+		}
+	}
+
+}
+
+static int psb_intel_sdvo_get_modes(struct drm_connector *connector)
+{
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
+
+	if (IS_TV(psb_intel_sdvo_connector))
+		psb_intel_sdvo_get_tv_modes(connector);
+	else if (IS_LVDS(psb_intel_sdvo_connector))
+		psb_intel_sdvo_get_lvds_modes(connector);
+	else
+		psb_intel_sdvo_get_ddc_modes(connector);
+
+	return !list_empty(&connector->probed_modes);
+}
+
+static void
+psb_intel_sdvo_destroy_enhance_property(struct drm_connector *connector)
+{
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
+	struct drm_device *dev = connector->dev;
+
+	if (psb_intel_sdvo_connector->left)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->left);
+	if (psb_intel_sdvo_connector->right)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->right);
+	if (psb_intel_sdvo_connector->top)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->top);
+	if (psb_intel_sdvo_connector->bottom)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->bottom);
+	if (psb_intel_sdvo_connector->hpos)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->hpos);
+	if (psb_intel_sdvo_connector->vpos)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->vpos);
+	if (psb_intel_sdvo_connector->saturation)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->saturation);
+	if (psb_intel_sdvo_connector->contrast)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->contrast);
+	if (psb_intel_sdvo_connector->hue)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->hue);
+	if (psb_intel_sdvo_connector->sharpness)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->sharpness);
+	if (psb_intel_sdvo_connector->flicker_filter)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->flicker_filter);
+	if (psb_intel_sdvo_connector->flicker_filter_2d)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->flicker_filter_2d);
+	if (psb_intel_sdvo_connector->flicker_filter_adaptive)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->flicker_filter_adaptive);
+	if (psb_intel_sdvo_connector->tv_luma_filter)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->tv_luma_filter);
+	if (psb_intel_sdvo_connector->tv_chroma_filter)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->tv_chroma_filter);
+	if (psb_intel_sdvo_connector->dot_crawl)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->dot_crawl);
+	if (psb_intel_sdvo_connector->brightness)
+		drm_property_destroy(dev, psb_intel_sdvo_connector->brightness);
+}
+
+static void psb_intel_sdvo_destroy(struct drm_connector *connector)
+{
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
+
+	if (psb_intel_sdvo_connector->tv_format)
+		drm_property_destroy(connector->dev,
+				     psb_intel_sdvo_connector->tv_format);
+
+	psb_intel_sdvo_destroy_enhance_property(connector);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(connector);
+}
+
+static bool psb_intel_sdvo_detect_hdmi_audio(struct drm_connector *connector)
+{
+	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
+	struct edid *edid;
+	bool has_audio = false;
+
+	if (!psb_intel_sdvo->is_hdmi)
+		return false;
+
+	edid = psb_intel_sdvo_get_edid(connector);
+	if (edid != NULL && edid->input & DRM_EDID_INPUT_DIGITAL)
+		has_audio = drm_detect_monitor_audio(edid);
+
+	return has_audio;
+}
+
+static int
+psb_intel_sdvo_set_property(struct drm_connector *connector,
+			struct drm_property *property,
+			uint64_t val)
+{
+	struct psb_intel_sdvo *psb_intel_sdvo = intel_attached_sdvo(connector);
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector = to_psb_intel_sdvo_connector(connector);
+	struct drm_psb_private *dev_priv = connector->dev->dev_private;
+	uint16_t temp_value;
+	uint8_t cmd;
+	int ret;
+
+	ret = drm_connector_property_set_value(connector, property, val);
+	if (ret)
+		return ret;
+
+	if (property == dev_priv->force_audio_property) {
+		int i = val;
+		bool has_audio;
+
+		if (i == psb_intel_sdvo_connector->force_audio)
+			return 0;
+
+		psb_intel_sdvo_connector->force_audio = i;
+
+		if (i == 0)
+			has_audio = psb_intel_sdvo_detect_hdmi_audio(connector);
+		else
+			has_audio = i > 0;
+
+		if (has_audio == psb_intel_sdvo->has_hdmi_audio)
+			return 0;
+
+		psb_intel_sdvo->has_hdmi_audio = has_audio;
+		goto done;
+	}
+
+	if (property == dev_priv->broadcast_rgb_property) {
+		if (val == !!psb_intel_sdvo->color_range)
+			return 0;
+
+		psb_intel_sdvo->color_range = val ? SDVO_COLOR_RANGE_16_235 : 0;
+		goto done;
+	}
+
+#define CHECK_PROPERTY(name, NAME) \
+	if (psb_intel_sdvo_connector->name == property) { \
+		if (psb_intel_sdvo_connector->cur_##name == temp_value) return 0; \
+		if (psb_intel_sdvo_connector->max_##name < temp_value) return -EINVAL; \
+		cmd = SDVO_CMD_SET_##NAME; \
+		psb_intel_sdvo_connector->cur_##name = temp_value; \
+		goto set_value; \
+	}
+
+	if (property == psb_intel_sdvo_connector->tv_format) {
+		if (val >= TV_FORMAT_NUM)
+			return -EINVAL;
+
+		if (psb_intel_sdvo->tv_format_index ==
+		    psb_intel_sdvo_connector->tv_format_supported[val])
+			return 0;
+
+		psb_intel_sdvo->tv_format_index = psb_intel_sdvo_connector->tv_format_supported[val];
+		goto done;
+	} else if (IS_TV_OR_LVDS(psb_intel_sdvo_connector)) {
+		temp_value = val;
+		if (psb_intel_sdvo_connector->left == property) {
+			drm_connector_property_set_value(connector,
+							 psb_intel_sdvo_connector->right, val);
+			if (psb_intel_sdvo_connector->left_margin == temp_value)
+				return 0;
+
+			psb_intel_sdvo_connector->left_margin = temp_value;
+			psb_intel_sdvo_connector->right_margin = temp_value;
+			temp_value = psb_intel_sdvo_connector->max_hscan -
+				psb_intel_sdvo_connector->left_margin;
+			cmd = SDVO_CMD_SET_OVERSCAN_H;
+			goto set_value;
+		} else if (psb_intel_sdvo_connector->right == property) {
+			drm_connector_property_set_value(connector,
+							 psb_intel_sdvo_connector->left, val);
+			if (psb_intel_sdvo_connector->right_margin == temp_value)
+				return 0;
+
+			psb_intel_sdvo_connector->left_margin = temp_value;
+			psb_intel_sdvo_connector->right_margin = temp_value;
+			temp_value = psb_intel_sdvo_connector->max_hscan -
+				psb_intel_sdvo_connector->left_margin;
+			cmd = SDVO_CMD_SET_OVERSCAN_H;
+			goto set_value;
+		} else if (psb_intel_sdvo_connector->top == property) {
+			drm_connector_property_set_value(connector,
+							 psb_intel_sdvo_connector->bottom, val);
+			if (psb_intel_sdvo_connector->top_margin == temp_value)
+				return 0;
+
+			psb_intel_sdvo_connector->top_margin = temp_value;
+			psb_intel_sdvo_connector->bottom_margin = temp_value;
+			temp_value = psb_intel_sdvo_connector->max_vscan -
+				psb_intel_sdvo_connector->top_margin;
+			cmd = SDVO_CMD_SET_OVERSCAN_V;
+			goto set_value;
+		} else if (psb_intel_sdvo_connector->bottom == property) {
+			drm_connector_property_set_value(connector,
+							 psb_intel_sdvo_connector->top, val);
+			if (psb_intel_sdvo_connector->bottom_margin == temp_value)
+				return 0;
+
+			psb_intel_sdvo_connector->top_margin = temp_value;
+			psb_intel_sdvo_connector->bottom_margin = temp_value;
+			temp_value = psb_intel_sdvo_connector->max_vscan -
+				psb_intel_sdvo_connector->top_margin;
+			cmd = SDVO_CMD_SET_OVERSCAN_V;
+			goto set_value;
+		}
+		CHECK_PROPERTY(hpos, HPOS)
+		CHECK_PROPERTY(vpos, VPOS)
+		CHECK_PROPERTY(saturation, SATURATION)
+		CHECK_PROPERTY(contrast, CONTRAST)
+		CHECK_PROPERTY(hue, HUE)
+		CHECK_PROPERTY(brightness, BRIGHTNESS)
+		CHECK_PROPERTY(sharpness, SHARPNESS)
+		CHECK_PROPERTY(flicker_filter, FLICKER_FILTER)
+		CHECK_PROPERTY(flicker_filter_2d, FLICKER_FILTER_2D)
+		CHECK_PROPERTY(flicker_filter_adaptive, FLICKER_FILTER_ADAPTIVE)
+		CHECK_PROPERTY(tv_chroma_filter, TV_CHROMA_FILTER)
+		CHECK_PROPERTY(tv_luma_filter, TV_LUMA_FILTER)
+		CHECK_PROPERTY(dot_crawl, DOT_CRAWL)
+	}
+
+	return -EINVAL; /* unknown property */
+
+set_value:
+	if (!psb_intel_sdvo_set_value(psb_intel_sdvo, cmd, &temp_value, 2))
+		return -EIO;
+
+
+done:
+	if (psb_intel_sdvo->base.base.crtc) {
+		struct drm_crtc *crtc = psb_intel_sdvo->base.base.crtc;
+		drm_crtc_helper_set_mode(crtc, &crtc->mode, crtc->x,
+					 crtc->y, crtc->fb);
+	}
+
+	return 0;
+#undef CHECK_PROPERTY
+}
+
+static const struct drm_encoder_helper_funcs psb_intel_sdvo_helper_funcs = {
+	.dpms = psb_intel_sdvo_dpms,
+	.mode_fixup = psb_intel_sdvo_mode_fixup,
+	.prepare = psb_intel_encoder_prepare,
+	.mode_set = psb_intel_sdvo_mode_set,
+	.commit = psb_intel_encoder_commit,
+};
+
+static const struct drm_connector_funcs psb_intel_sdvo_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = psb_intel_sdvo_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.set_property = psb_intel_sdvo_set_property,
+	.destroy = psb_intel_sdvo_destroy,
+};
+
+static const struct drm_connector_helper_funcs psb_intel_sdvo_connector_helper_funcs = {
+	.get_modes = psb_intel_sdvo_get_modes,
+	.mode_valid = psb_intel_sdvo_mode_valid,
+	.best_encoder = psb_intel_best_encoder,
+};
+
+static void psb_intel_sdvo_enc_destroy(struct drm_encoder *encoder)
+{
+	struct psb_intel_sdvo *psb_intel_sdvo = to_psb_intel_sdvo(encoder);
+
+	if (psb_intel_sdvo->sdvo_lvds_fixed_mode != NULL)
+		drm_mode_destroy(encoder->dev,
+				 psb_intel_sdvo->sdvo_lvds_fixed_mode);
+
+	i2c_del_adapter(&psb_intel_sdvo->ddc);
+	psb_intel_encoder_destroy(encoder);
+}
+
+static const struct drm_encoder_funcs psb_intel_sdvo_enc_funcs = {
+	.destroy = psb_intel_sdvo_enc_destroy,
+};
+
+static void
+psb_intel_sdvo_guess_ddc_bus(struct psb_intel_sdvo *sdvo)
+{
+	/* FIXME: At the moment, ddc_bus = 2 is the only thing that works.
+	 * We need to figure out if this is true for all available poulsbo
+	 * hardware, or if we need to fiddle with the guessing code above.
+	 * The problem might go away if we can parse sdvo mappings from bios */
+	sdvo->ddc_bus = 2;
+
+#if 0
+	uint16_t mask = 0;
+	unsigned int num_bits;
+
+	/* Make a mask of outputs less than or equal to our own priority in the
+	 * list.
+	 */
+	switch (sdvo->controlled_output) {
+	case SDVO_OUTPUT_LVDS1:
+		mask |= SDVO_OUTPUT_LVDS1;
+	case SDVO_OUTPUT_LVDS0:
+		mask |= SDVO_OUTPUT_LVDS0;
+	case SDVO_OUTPUT_TMDS1:
+		mask |= SDVO_OUTPUT_TMDS1;
+	case SDVO_OUTPUT_TMDS0:
+		mask |= SDVO_OUTPUT_TMDS0;
+	case SDVO_OUTPUT_RGB1:
+		mask |= SDVO_OUTPUT_RGB1;
+	case SDVO_OUTPUT_RGB0:
+		mask |= SDVO_OUTPUT_RGB0;
+		break;
+	}
+
+	/* Count bits to find what number we are in the priority list. */
+	mask &= sdvo->caps.output_flags;
+	num_bits = hweight16(mask);
+	/* If more than 3 outputs, default to DDC bus 3 for now. */
+	if (num_bits > 3)
+		num_bits = 3;
+
+	/* Corresponds to SDVO_CONTROL_BUS_DDCx */
+	sdvo->ddc_bus = 1 << num_bits;
+#endif
+}
+
+/**
+ * Choose the appropriate DDC bus for control bus switch command for this
+ * SDVO output based on the controlled output.
+ *
+ * DDC bus number assignment is in a priority order of RGB outputs, then TMDS
+ * outputs, then LVDS outputs.
+ */
+static void
+psb_intel_sdvo_select_ddc_bus(struct drm_psb_private *dev_priv,
+			  struct psb_intel_sdvo *sdvo, u32 reg)
+{
+	struct sdvo_device_mapping *mapping;
+
+	if (IS_SDVOB(reg))
+		mapping = &(dev_priv->sdvo_mappings[0]);
+	else
+		mapping = &(dev_priv->sdvo_mappings[1]);
+
+	if (mapping->initialized)
+		sdvo->ddc_bus = 1 << ((mapping->ddc_pin & 0xf0) >> 4);
+	else
+		psb_intel_sdvo_guess_ddc_bus(sdvo);
+}
+
+static void
+psb_intel_sdvo_select_i2c_bus(struct drm_psb_private *dev_priv,
+			  struct psb_intel_sdvo *sdvo, u32 reg)
+{
+	struct sdvo_device_mapping *mapping;
+	u8 pin, speed;
+
+	if (IS_SDVOB(reg))
+		mapping = &dev_priv->sdvo_mappings[0];
+	else
+		mapping = &dev_priv->sdvo_mappings[1];
+
+	pin = GMBUS_PORT_DPB;
+	speed = GMBUS_RATE_1MHZ >> 8;
+	if (mapping->initialized) {
+		pin = mapping->i2c_pin;
+		speed = mapping->i2c_speed;
+	}
+
+	if (pin < GMBUS_NUM_PORTS) {
+		sdvo->i2c = &dev_priv->gmbus[pin].adapter;
+		gma_intel_gmbus_set_speed(sdvo->i2c, speed);
+		gma_intel_gmbus_force_bit(sdvo->i2c, true);
+	} else
+		sdvo->i2c = &dev_priv->gmbus[GMBUS_PORT_DPB].adapter;
+}
+
+static bool
+psb_intel_sdvo_is_hdmi_connector(struct psb_intel_sdvo *psb_intel_sdvo, int device)
+{
+	return psb_intel_sdvo_check_supp_encode(psb_intel_sdvo);
+}
+
+static u8
+psb_intel_sdvo_get_slave_addr(struct drm_device *dev, int sdvo_reg)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct sdvo_device_mapping *my_mapping, *other_mapping;
+
+	if (IS_SDVOB(sdvo_reg)) {
+		my_mapping = &dev_priv->sdvo_mappings[0];
+		other_mapping = &dev_priv->sdvo_mappings[1];
+	} else {
+		my_mapping = &dev_priv->sdvo_mappings[1];
+		other_mapping = &dev_priv->sdvo_mappings[0];
+	}
+
+	/* If the BIOS described our SDVO device, take advantage of it. */
+	if (my_mapping->slave_addr)
+		return my_mapping->slave_addr;
+
+	/* If the BIOS only described a different SDVO device, use the
+	 * address that it isn't using.
+	 */
+	if (other_mapping->slave_addr) {
+		if (other_mapping->slave_addr == 0x70)
+			return 0x72;
+		else
+			return 0x70;
+	}
+
+	/* No SDVO device info is found for another DVO port,
+	 * so use mapping assumption we had before BIOS parsing.
+	 */
+	if (IS_SDVOB(sdvo_reg))
+		return 0x70;
+	else
+		return 0x72;
+}
+
+static void
+psb_intel_sdvo_connector_init(struct psb_intel_sdvo_connector *connector,
+			  struct psb_intel_sdvo *encoder)
+{
+	drm_connector_init(encoder->base.base.dev,
+			   &connector->base.base,
+			   &psb_intel_sdvo_connector_funcs,
+			   connector->base.base.connector_type);
+
+	drm_connector_helper_add(&connector->base.base,
+				 &psb_intel_sdvo_connector_helper_funcs);
+
+	connector->base.base.interlace_allowed = 0;
+	connector->base.base.doublescan_allowed = 0;
+	connector->base.base.display_info.subpixel_order = SubPixelHorizontalRGB;
+
+	psb_intel_connector_attach_encoder(&connector->base, &encoder->base);
+	drm_sysfs_connector_add(&connector->base.base);
+}
+
+static void
+psb_intel_sdvo_add_hdmi_properties(struct psb_intel_sdvo_connector *connector)
+{
+	/* FIXME: We don't support HDMI at the moment
+	struct drm_device *dev = connector->base.base.dev;
+
+	intel_attach_force_audio_property(&connector->base.base);
+	if (INTEL_INFO(dev)->gen >= 4 && IS_MOBILE(dev))
+		intel_attach_broadcast_rgb_property(&connector->base.base);
+	*/
+}
+
+static bool
+psb_intel_sdvo_dvi_init(struct psb_intel_sdvo *psb_intel_sdvo, int device)
+{
+	struct drm_encoder *encoder = &psb_intel_sdvo->base.base;
+	struct drm_connector *connector;
+	struct psb_intel_connector *intel_connector;
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector;
+
+	psb_intel_sdvo_connector = kzalloc(sizeof(struct psb_intel_sdvo_connector), GFP_KERNEL);
+	if (!psb_intel_sdvo_connector)
+		return false;
+
+	if (device == 0) {
+		psb_intel_sdvo->controlled_output |= SDVO_OUTPUT_TMDS0;
+		psb_intel_sdvo_connector->output_flag = SDVO_OUTPUT_TMDS0;
+	} else if (device == 1) {
+		psb_intel_sdvo->controlled_output |= SDVO_OUTPUT_TMDS1;
+		psb_intel_sdvo_connector->output_flag = SDVO_OUTPUT_TMDS1;
+	}
+
+	intel_connector = &psb_intel_sdvo_connector->base;
+	connector = &intel_connector->base;
+	// connector->polled = DRM_CONNECTOR_POLL_CONNECT | DRM_CONNECTOR_POLL_DISCONNECT;
+	encoder->encoder_type = DRM_MODE_ENCODER_TMDS;
+	connector->connector_type = DRM_MODE_CONNECTOR_DVID;
+
+	if (psb_intel_sdvo_is_hdmi_connector(psb_intel_sdvo, device)) {
+		connector->connector_type = DRM_MODE_CONNECTOR_HDMIA;
+		psb_intel_sdvo->is_hdmi = true;
+	}
+	psb_intel_sdvo->base.clone_mask = ((1 << INTEL_SDVO_NON_TV_CLONE_BIT) |
+				       (1 << INTEL_ANALOG_CLONE_BIT));
+
+	psb_intel_sdvo_connector_init(psb_intel_sdvo_connector, psb_intel_sdvo);
+	if (psb_intel_sdvo->is_hdmi)
+		psb_intel_sdvo_add_hdmi_properties(psb_intel_sdvo_connector);
+
+	return true;
+}
+
+static bool
+psb_intel_sdvo_tv_init(struct psb_intel_sdvo *psb_intel_sdvo, int type)
+{
+	struct drm_encoder *encoder = &psb_intel_sdvo->base.base;
+	struct drm_connector *connector;
+	struct psb_intel_connector *intel_connector;
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector;
+
+	psb_intel_sdvo_connector = kzalloc(sizeof(struct psb_intel_sdvo_connector), GFP_KERNEL);
+	if (!psb_intel_sdvo_connector)
+		return false;
+
+	intel_connector = &psb_intel_sdvo_connector->base;
+	connector = &intel_connector->base;
+	encoder->encoder_type = DRM_MODE_ENCODER_TVDAC;
+	connector->connector_type = DRM_MODE_CONNECTOR_SVIDEO;
+
+	psb_intel_sdvo->controlled_output |= type;
+	psb_intel_sdvo_connector->output_flag = type;
+
+	psb_intel_sdvo->is_tv = true;
+	psb_intel_sdvo->base.needs_tv_clock = true;
+	psb_intel_sdvo->base.clone_mask = 1 << INTEL_SDVO_TV_CLONE_BIT;
+
+	psb_intel_sdvo_connector_init(psb_intel_sdvo_connector, psb_intel_sdvo);
+
+	if (!psb_intel_sdvo_tv_create_property(psb_intel_sdvo, psb_intel_sdvo_connector, type))
+		goto err;
+
+	if (!psb_intel_sdvo_create_enhance_property(psb_intel_sdvo, psb_intel_sdvo_connector))
+		goto err;
+
+	return true;
+
+err:
+	psb_intel_sdvo_destroy(connector);
+	return false;
+}
+
+static bool
+psb_intel_sdvo_analog_init(struct psb_intel_sdvo *psb_intel_sdvo, int device)
+{
+	struct drm_encoder *encoder = &psb_intel_sdvo->base.base;
+	struct drm_connector *connector;
+	struct psb_intel_connector *intel_connector;
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector;
+
+	psb_intel_sdvo_connector = kzalloc(sizeof(struct psb_intel_sdvo_connector), GFP_KERNEL);
+	if (!psb_intel_sdvo_connector)
+		return false;
+
+	intel_connector = &psb_intel_sdvo_connector->base;
+	connector = &intel_connector->base;
+	connector->polled = DRM_CONNECTOR_POLL_CONNECT;
+	encoder->encoder_type = DRM_MODE_ENCODER_DAC;
+	connector->connector_type = DRM_MODE_CONNECTOR_VGA;
+
+	if (device == 0) {
+		psb_intel_sdvo->controlled_output |= SDVO_OUTPUT_RGB0;
+		psb_intel_sdvo_connector->output_flag = SDVO_OUTPUT_RGB0;
+	} else if (device == 1) {
+		psb_intel_sdvo->controlled_output |= SDVO_OUTPUT_RGB1;
+		psb_intel_sdvo_connector->output_flag = SDVO_OUTPUT_RGB1;
+	}
+
+	psb_intel_sdvo->base.clone_mask = ((1 << INTEL_SDVO_NON_TV_CLONE_BIT) |
+				       (1 << INTEL_ANALOG_CLONE_BIT));
+
+	psb_intel_sdvo_connector_init(psb_intel_sdvo_connector,
+				  psb_intel_sdvo);
+	return true;
+}
+
+static bool
+psb_intel_sdvo_lvds_init(struct psb_intel_sdvo *psb_intel_sdvo, int device)
+{
+	struct drm_encoder *encoder = &psb_intel_sdvo->base.base;
+	struct drm_connector *connector;
+	struct psb_intel_connector *intel_connector;
+	struct psb_intel_sdvo_connector *psb_intel_sdvo_connector;
+
+	psb_intel_sdvo_connector = kzalloc(sizeof(struct psb_intel_sdvo_connector), GFP_KERNEL);
+	if (!psb_intel_sdvo_connector)
+		return false;
+
+	intel_connector = &psb_intel_sdvo_connector->base;
+	connector = &intel_connector->base;
+	encoder->encoder_type = DRM_MODE_ENCODER_LVDS;
+	connector->connector_type = DRM_MODE_CONNECTOR_LVDS;
+
+	if (device == 0) {
+		psb_intel_sdvo->controlled_output |= SDVO_OUTPUT_LVDS0;
+		psb_intel_sdvo_connector->output_flag = SDVO_OUTPUT_LVDS0;
+	} else if (device == 1) {
+		psb_intel_sdvo->controlled_output |= SDVO_OUTPUT_LVDS1;
+		psb_intel_sdvo_connector->output_flag = SDVO_OUTPUT_LVDS1;
+	}
+
+	psb_intel_sdvo->base.clone_mask = ((1 << INTEL_ANALOG_CLONE_BIT) |
+				       (1 << INTEL_SDVO_LVDS_CLONE_BIT));
+
+	psb_intel_sdvo_connector_init(psb_intel_sdvo_connector, psb_intel_sdvo);
+	if (!psb_intel_sdvo_create_enhance_property(psb_intel_sdvo, psb_intel_sdvo_connector))
+		goto err;
+
+	return true;
+
+err:
+	psb_intel_sdvo_destroy(connector);
+	return false;
+}
+
+static bool
+psb_intel_sdvo_output_setup(struct psb_intel_sdvo *psb_intel_sdvo, uint16_t flags)
+{
+	psb_intel_sdvo->is_tv = false;
+	psb_intel_sdvo->base.needs_tv_clock = false;
+	psb_intel_sdvo->is_lvds = false;
+
+	/* SDVO requires XXX1 function may not exist unless it has XXX0 function.*/
+
+	if (flags & SDVO_OUTPUT_TMDS0)
+		if (!psb_intel_sdvo_dvi_init(psb_intel_sdvo, 0))
+			return false;
+
+	if ((flags & SDVO_TMDS_MASK) == SDVO_TMDS_MASK)
+		if (!psb_intel_sdvo_dvi_init(psb_intel_sdvo, 1))
+			return false;
+
+	/* TV has no XXX1 function block */
+	if (flags & SDVO_OUTPUT_SVID0)
+		if (!psb_intel_sdvo_tv_init(psb_intel_sdvo, SDVO_OUTPUT_SVID0))
+			return false;
+
+	if (flags & SDVO_OUTPUT_CVBS0)
+		if (!psb_intel_sdvo_tv_init(psb_intel_sdvo, SDVO_OUTPUT_CVBS0))
+			return false;
+
+	if (flags & SDVO_OUTPUT_RGB0)
+		if (!psb_intel_sdvo_analog_init(psb_intel_sdvo, 0))
+			return false;
+
+	if ((flags & SDVO_RGB_MASK) == SDVO_RGB_MASK)
+		if (!psb_intel_sdvo_analog_init(psb_intel_sdvo, 1))
+			return false;
+
+	if (flags & SDVO_OUTPUT_LVDS0)
+		if (!psb_intel_sdvo_lvds_init(psb_intel_sdvo, 0))
+			return false;
+
+	if ((flags & SDVO_LVDS_MASK) == SDVO_LVDS_MASK)
+		if (!psb_intel_sdvo_lvds_init(psb_intel_sdvo, 1))
+			return false;
+
+	if ((flags & SDVO_OUTPUT_MASK) == 0) {
+		unsigned char bytes[2];
+
+		psb_intel_sdvo->controlled_output = 0;
+		memcpy(bytes, &psb_intel_sdvo->caps.output_flags, 2);
+		DRM_DEBUG_KMS("%s: Unknown SDVO output type (0x%02x%02x)\n",
+			      SDVO_NAME(psb_intel_sdvo),
+			      bytes[0], bytes[1]);
+		return false;
+	}
+	psb_intel_sdvo->base.crtc_mask = (1 << 0) | (1 << 1);
+
+	return true;
+}
+
+static bool psb_intel_sdvo_tv_create_property(struct psb_intel_sdvo *psb_intel_sdvo,
+					  struct psb_intel_sdvo_connector *psb_intel_sdvo_connector,
+					  int type)
+{
+	struct drm_device *dev = psb_intel_sdvo->base.base.dev;
+	struct psb_intel_sdvo_tv_format format;
+	uint32_t format_map, i;
+
+	if (!psb_intel_sdvo_set_target_output(psb_intel_sdvo, type))
+		return false;
+
+	BUILD_BUG_ON(sizeof(format) != 6);
+	if (!psb_intel_sdvo_get_value(psb_intel_sdvo,
+				  SDVO_CMD_GET_SUPPORTED_TV_FORMATS,
+				  &format, sizeof(format)))
+		return false;
+
+	memcpy(&format_map, &format, min(sizeof(format_map), sizeof(format)));
+
+	if (format_map == 0)
+		return false;
+
+	psb_intel_sdvo_connector->format_supported_num = 0;
+	for (i = 0 ; i < TV_FORMAT_NUM; i++)
+		if (format_map & (1 << i))
+			psb_intel_sdvo_connector->tv_format_supported[psb_intel_sdvo_connector->format_supported_num++] = i;
+
+
+	psb_intel_sdvo_connector->tv_format =
+			drm_property_create(dev, DRM_MODE_PROP_ENUM,
+					    "mode", psb_intel_sdvo_connector->format_supported_num);
+	if (!psb_intel_sdvo_connector->tv_format)
+		return false;
+
+	for (i = 0; i < psb_intel_sdvo_connector->format_supported_num; i++)
+		drm_property_add_enum(
+				psb_intel_sdvo_connector->tv_format, i,
+				i, tv_format_names[psb_intel_sdvo_connector->tv_format_supported[i]]);
+
+	psb_intel_sdvo->tv_format_index = psb_intel_sdvo_connector->tv_format_supported[0];
+	drm_connector_attach_property(&psb_intel_sdvo_connector->base.base,
+				      psb_intel_sdvo_connector->tv_format, 0);
+	return true;
+
+}
+
+#define ENHANCEMENT(name, NAME) do { \
+	if (enhancements.name) { \
+		if (!psb_intel_sdvo_get_value(psb_intel_sdvo, SDVO_CMD_GET_MAX_##NAME, &data_value, 4) || \
+		    !psb_intel_sdvo_get_value(psb_intel_sdvo, SDVO_CMD_GET_##NAME, &response, 2)) \
+			return false; \
+		psb_intel_sdvo_connector->max_##name = data_value[0]; \
+		psb_intel_sdvo_connector->cur_##name = response; \
+		psb_intel_sdvo_connector->name = \
+			drm_property_create(dev, DRM_MODE_PROP_RANGE, #name, 2); \
+		if (!psb_intel_sdvo_connector->name) return false; \
+		psb_intel_sdvo_connector->name->values[0] = 0; \
+		psb_intel_sdvo_connector->name->values[1] = data_value[0]; \
+		drm_connector_attach_property(connector, \
+					      psb_intel_sdvo_connector->name, \
+					      psb_intel_sdvo_connector->cur_##name); \
+		DRM_DEBUG_KMS(#name ": max %d, default %d, current %d\n", \
+			      data_value[0], data_value[1], response); \
+	} \
+} while(0)
+
+static bool
+psb_intel_sdvo_create_enhance_property_tv(struct psb_intel_sdvo *psb_intel_sdvo,
+				      struct psb_intel_sdvo_connector *psb_intel_sdvo_connector,
+				      struct psb_intel_sdvo_enhancements_reply enhancements)
+{
+	struct drm_device *dev = psb_intel_sdvo->base.base.dev;
+	struct drm_connector *connector = &psb_intel_sdvo_connector->base.base;
+	uint16_t response, data_value[2];
+
+	/* when horizontal overscan is supported, Add the left/right  property */
+	if (enhancements.overscan_h) {
+		if (!psb_intel_sdvo_get_value(psb_intel_sdvo,
+					  SDVO_CMD_GET_MAX_OVERSCAN_H,
+					  &data_value, 4))
+			return false;
+
+		if (!psb_intel_sdvo_get_value(psb_intel_sdvo,
+					  SDVO_CMD_GET_OVERSCAN_H,
+					  &response, 2))
+			return false;
+
+		psb_intel_sdvo_connector->max_hscan = data_value[0];
+		psb_intel_sdvo_connector->left_margin = data_value[0] - response;
+		psb_intel_sdvo_connector->right_margin = psb_intel_sdvo_connector->left_margin;
+		psb_intel_sdvo_connector->left =
+			drm_property_create(dev, DRM_MODE_PROP_RANGE,
+					    "left_margin", 2);
+		if (!psb_intel_sdvo_connector->left)
+			return false;
+
+		psb_intel_sdvo_connector->left->values[0] = 0;
+		psb_intel_sdvo_connector->left->values[1] = data_value[0];
+		drm_connector_attach_property(connector,
+					      psb_intel_sdvo_connector->left,
+					      psb_intel_sdvo_connector->left_margin);
+
+		psb_intel_sdvo_connector->right =
+			drm_property_create(dev, DRM_MODE_PROP_RANGE,
+					    "right_margin", 2);
+		if (!psb_intel_sdvo_connector->right)
+			return false;
+
+		psb_intel_sdvo_connector->right->values[0] = 0;
+		psb_intel_sdvo_connector->right->values[1] = data_value[0];
+		drm_connector_attach_property(connector,
+					      psb_intel_sdvo_connector->right,
+					      psb_intel_sdvo_connector->right_margin);
+		DRM_DEBUG_KMS("h_overscan: max %d, "
+			      "default %d, current %d\n",
+			      data_value[0], data_value[1], response);
+	}
+
+	if (enhancements.overscan_v) {
+		if (!psb_intel_sdvo_get_value(psb_intel_sdvo,
+					  SDVO_CMD_GET_MAX_OVERSCAN_V,
+					  &data_value, 4))
+			return false;
+
+		if (!psb_intel_sdvo_get_value(psb_intel_sdvo,
+					  SDVO_CMD_GET_OVERSCAN_V,
+					  &response, 2))
+			return false;
+
+		psb_intel_sdvo_connector->max_vscan = data_value[0];
+		psb_intel_sdvo_connector->top_margin = data_value[0] - response;
+		psb_intel_sdvo_connector->bottom_margin = psb_intel_sdvo_connector->top_margin;
+		psb_intel_sdvo_connector->top =
+			drm_property_create(dev, DRM_MODE_PROP_RANGE,
+					    "top_margin", 2);
+		if (!psb_intel_sdvo_connector->top)
+			return false;
+
+		psb_intel_sdvo_connector->top->values[0] = 0;
+		psb_intel_sdvo_connector->top->values[1] = data_value[0];
+		drm_connector_attach_property(connector,
+					      psb_intel_sdvo_connector->top,
+					      psb_intel_sdvo_connector->top_margin);
+
+		psb_intel_sdvo_connector->bottom =
+			drm_property_create(dev, DRM_MODE_PROP_RANGE,
+					    "bottom_margin", 2);
+		if (!psb_intel_sdvo_connector->bottom)
+			return false;
+
+		psb_intel_sdvo_connector->bottom->values[0] = 0;
+		psb_intel_sdvo_connector->bottom->values[1] = data_value[0];
+		drm_connector_attach_property(connector,
+					      psb_intel_sdvo_connector->bottom,
+					      psb_intel_sdvo_connector->bottom_margin);
+		DRM_DEBUG_KMS("v_overscan: max %d, "
+			      "default %d, current %d\n",
+			      data_value[0], data_value[1], response);
+	}
+
+	ENHANCEMENT(hpos, HPOS);
+	ENHANCEMENT(vpos, VPOS);
+	ENHANCEMENT(saturation, SATURATION);
+	ENHANCEMENT(contrast, CONTRAST);
+	ENHANCEMENT(hue, HUE);
+	ENHANCEMENT(sharpness, SHARPNESS);
+	ENHANCEMENT(brightness, BRIGHTNESS);
+	ENHANCEMENT(flicker_filter, FLICKER_FILTER);
+	ENHANCEMENT(flicker_filter_adaptive, FLICKER_FILTER_ADAPTIVE);
+	ENHANCEMENT(flicker_filter_2d, FLICKER_FILTER_2D);
+	ENHANCEMENT(tv_chroma_filter, TV_CHROMA_FILTER);
+	ENHANCEMENT(tv_luma_filter, TV_LUMA_FILTER);
+
+	if (enhancements.dot_crawl) {
+		if (!psb_intel_sdvo_get_value(psb_intel_sdvo, SDVO_CMD_GET_DOT_CRAWL, &response, 2))
+			return false;
+
+		psb_intel_sdvo_connector->max_dot_crawl = 1;
+		psb_intel_sdvo_connector->cur_dot_crawl = response & 0x1;
+		psb_intel_sdvo_connector->dot_crawl =
+			drm_property_create(dev, DRM_MODE_PROP_RANGE, "dot_crawl", 2);
+		if (!psb_intel_sdvo_connector->dot_crawl)
+			return false;
+
+		psb_intel_sdvo_connector->dot_crawl->values[0] = 0;
+		psb_intel_sdvo_connector->dot_crawl->values[1] = 1;
+		drm_connector_attach_property(connector,
+					      psb_intel_sdvo_connector->dot_crawl,
+					      psb_intel_sdvo_connector->cur_dot_crawl);
+		DRM_DEBUG_KMS("dot crawl: current %d\n", response);
+	}
+
+	return true;
+}
+
+static bool
+psb_intel_sdvo_create_enhance_property_lvds(struct psb_intel_sdvo *psb_intel_sdvo,
+					struct psb_intel_sdvo_connector *psb_intel_sdvo_connector,
+					struct psb_intel_sdvo_enhancements_reply enhancements)
+{
+	struct drm_device *dev = psb_intel_sdvo->base.base.dev;
+	struct drm_connector *connector = &psb_intel_sdvo_connector->base.base;
+	uint16_t response, data_value[2];
+
+	ENHANCEMENT(brightness, BRIGHTNESS);
+
+	return true;
+}
+#undef ENHANCEMENT
+
+static bool psb_intel_sdvo_create_enhance_property(struct psb_intel_sdvo *psb_intel_sdvo,
+					       struct psb_intel_sdvo_connector *psb_intel_sdvo_connector)
+{
+	union {
+		struct psb_intel_sdvo_enhancements_reply reply;
+		uint16_t response;
+	} enhancements;
+
+	BUILD_BUG_ON(sizeof(enhancements) != 2);
+
+	enhancements.response = 0;
+	psb_intel_sdvo_get_value(psb_intel_sdvo,
+			     SDVO_CMD_GET_SUPPORTED_ENHANCEMENTS,
+			     &enhancements, sizeof(enhancements));
+	if (enhancements.response == 0) {
+		DRM_DEBUG_KMS("No enhancement is supported\n");
+		return true;
+	}
+
+	if (IS_TV(psb_intel_sdvo_connector))
+		return psb_intel_sdvo_create_enhance_property_tv(psb_intel_sdvo, psb_intel_sdvo_connector, enhancements.reply);
+	else if(IS_LVDS(psb_intel_sdvo_connector))
+		return psb_intel_sdvo_create_enhance_property_lvds(psb_intel_sdvo, psb_intel_sdvo_connector, enhancements.reply);
+	else
+		return true;
+}
+
+static int psb_intel_sdvo_ddc_proxy_xfer(struct i2c_adapter *adapter,
+				     struct i2c_msg *msgs,
+				     int num)
+{
+	struct psb_intel_sdvo *sdvo = adapter->algo_data;
+
+	if (!psb_intel_sdvo_set_control_bus_switch(sdvo, sdvo->ddc_bus))
+		return -EIO;
+
+	return sdvo->i2c->algo->master_xfer(sdvo->i2c, msgs, num);
+}
+
+static u32 psb_intel_sdvo_ddc_proxy_func(struct i2c_adapter *adapter)
+{
+	struct psb_intel_sdvo *sdvo = adapter->algo_data;
+	return sdvo->i2c->algo->functionality(sdvo->i2c);
+}
+
+static const struct i2c_algorithm psb_intel_sdvo_ddc_proxy = {
+	.master_xfer	= psb_intel_sdvo_ddc_proxy_xfer,
+	.functionality	= psb_intel_sdvo_ddc_proxy_func
+};
+
+static bool
+psb_intel_sdvo_init_ddc_proxy(struct psb_intel_sdvo *sdvo,
+			  struct drm_device *dev)
+{
+	sdvo->ddc.owner = THIS_MODULE;
+	sdvo->ddc.class = I2C_CLASS_DDC;
+	snprintf(sdvo->ddc.name, I2C_NAME_SIZE, "SDVO DDC proxy");
+	sdvo->ddc.dev.parent = &dev->pdev->dev;
+	sdvo->ddc.algo_data = sdvo;
+	sdvo->ddc.algo = &psb_intel_sdvo_ddc_proxy;
+
+	return i2c_add_adapter(&sdvo->ddc) == 0;
+}
+
+bool psb_intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	struct psb_intel_encoder *psb_intel_encoder;
+	struct psb_intel_sdvo *psb_intel_sdvo;
+	int i;
+
+	psb_intel_sdvo = kzalloc(sizeof(struct psb_intel_sdvo), GFP_KERNEL);
+	if (!psb_intel_sdvo)
+		return false;
+
+	psb_intel_sdvo->sdvo_reg = sdvo_reg;
+	psb_intel_sdvo->slave_addr = psb_intel_sdvo_get_slave_addr(dev, sdvo_reg) >> 1;
+	psb_intel_sdvo_select_i2c_bus(dev_priv, psb_intel_sdvo, sdvo_reg);
+	if (!psb_intel_sdvo_init_ddc_proxy(psb_intel_sdvo, dev)) {
+		kfree(psb_intel_sdvo);
+		return false;
+	}
+
+	/* encoder type will be decided later */
+	psb_intel_encoder = &psb_intel_sdvo->base;
+	psb_intel_encoder->type = INTEL_OUTPUT_SDVO;
+	drm_encoder_init(dev, &psb_intel_encoder->base, &psb_intel_sdvo_enc_funcs, 0);
+
+	/* Read the regs to test if we can talk to the device */
+	for (i = 0; i < 0x40; i++) {
+		u8 byte;
+
+		if (!psb_intel_sdvo_read_byte(psb_intel_sdvo, i, &byte)) {
+			DRM_DEBUG_KMS("No SDVO device found on SDVO%c\n",
+				      IS_SDVOB(sdvo_reg) ? 'B' : 'C');
+			goto err;
+		}
+	}
+
+	if (IS_SDVOB(sdvo_reg))
+		dev_priv->hotplug_supported_mask |= SDVOB_HOTPLUG_INT_STATUS;
+	else
+		dev_priv->hotplug_supported_mask |= SDVOC_HOTPLUG_INT_STATUS;
+
+	drm_encoder_helper_add(&psb_intel_encoder->base, &psb_intel_sdvo_helper_funcs);
+
+	/* In default case sdvo lvds is false */
+	if (!psb_intel_sdvo_get_capabilities(psb_intel_sdvo, &psb_intel_sdvo->caps))
+		goto err;
+
+	if (psb_intel_sdvo_output_setup(psb_intel_sdvo,
+				    psb_intel_sdvo->caps.output_flags) != true) {
+		DRM_DEBUG_KMS("SDVO output failed to setup on SDVO%c\n",
+			      IS_SDVOB(sdvo_reg) ? 'B' : 'C');
+		goto err;
+	}
+
+	psb_intel_sdvo_select_ddc_bus(dev_priv, psb_intel_sdvo, sdvo_reg);
+
+	/* Set the input timing to the screen. Assume always input 0. */
+	if (!psb_intel_sdvo_set_target_input(psb_intel_sdvo))
+		goto err;
+
+	if (!psb_intel_sdvo_get_input_pixel_clock_range(psb_intel_sdvo,
+						    &psb_intel_sdvo->pixel_clock_min,
+						    &psb_intel_sdvo->pixel_clock_max))
+		goto err;
+
+	DRM_DEBUG_KMS("%s device VID/DID: %02X:%02X.%02X, "
+			"clock range %dMHz - %dMHz, "
+			"input 1: %c, input 2: %c, "
+			"output 1: %c, output 2: %c\n",
+			SDVO_NAME(psb_intel_sdvo),
+			psb_intel_sdvo->caps.vendor_id, psb_intel_sdvo->caps.device_id,
+			psb_intel_sdvo->caps.device_rev_id,
+			psb_intel_sdvo->pixel_clock_min / 1000,
+			psb_intel_sdvo->pixel_clock_max / 1000,
+			(psb_intel_sdvo->caps.sdvo_inputs_mask & 0x1) ? 'Y' : 'N',
+			(psb_intel_sdvo->caps.sdvo_inputs_mask & 0x2) ? 'Y' : 'N',
+			/* check currently supported outputs */
+			psb_intel_sdvo->caps.output_flags &
+			(SDVO_OUTPUT_TMDS0 | SDVO_OUTPUT_RGB0) ? 'Y' : 'N',
+			psb_intel_sdvo->caps.output_flags &
+			(SDVO_OUTPUT_TMDS1 | SDVO_OUTPUT_RGB1) ? 'Y' : 'N');
+	return true;
+
+err:
+	drm_encoder_cleanup(&psb_intel_encoder->base);
+	i2c_del_adapter(&psb_intel_sdvo->ddc);
+	kfree(psb_intel_sdvo);
+
+	return false;
+}
diff --git a/drivers/gpu/drm/gma500/psb_intel_sdvo_regs.h b/drivers/gpu/drm/gma500/psb_intel_sdvo_regs.h
new file mode 100644
index 000000000000..600e79744d68
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_intel_sdvo_regs.h
@@ -0,0 +1,723 @@
+/*
+ * Copyright ? 2006-2007 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ */
+
+/**
+ * @file SDVO command definitions and structures.
+ */
+
+#define SDVO_OUTPUT_FIRST   (0)
+#define SDVO_OUTPUT_TMDS0   (1 << 0)
+#define SDVO_OUTPUT_RGB0    (1 << 1)
+#define SDVO_OUTPUT_CVBS0   (1 << 2)
+#define SDVO_OUTPUT_SVID0   (1 << 3)
+#define SDVO_OUTPUT_YPRPB0  (1 << 4)
+#define SDVO_OUTPUT_SCART0  (1 << 5)
+#define SDVO_OUTPUT_LVDS0   (1 << 6)
+#define SDVO_OUTPUT_TMDS1   (1 << 8)
+#define SDVO_OUTPUT_RGB1    (1 << 9)
+#define SDVO_OUTPUT_CVBS1   (1 << 10)
+#define SDVO_OUTPUT_SVID1   (1 << 11)
+#define SDVO_OUTPUT_YPRPB1  (1 << 12)
+#define SDVO_OUTPUT_SCART1  (1 << 13)
+#define SDVO_OUTPUT_LVDS1   (1 << 14)
+#define SDVO_OUTPUT_LAST    (14)
+
+struct psb_intel_sdvo_caps {
+    u8 vendor_id;
+    u8 device_id;
+    u8 device_rev_id;
+    u8 sdvo_version_major;
+    u8 sdvo_version_minor;
+    unsigned int sdvo_inputs_mask:2;
+    unsigned int smooth_scaling:1;
+    unsigned int sharp_scaling:1;
+    unsigned int up_scaling:1;
+    unsigned int down_scaling:1;
+    unsigned int stall_support:1;
+    unsigned int pad:1;
+    u16 output_flags;
+} __attribute__((packed));
+
+/** This matches the EDID DTD structure, more or less */
+struct psb_intel_sdvo_dtd {
+    struct {
+	u16 clock;		/**< pixel clock, in 10kHz units */
+	u8 h_active;		/**< lower 8 bits (pixels) */
+	u8 h_blank;		/**< lower 8 bits (pixels) */
+	u8 h_high;		/**< upper 4 bits each h_active, h_blank */
+	u8 v_active;		/**< lower 8 bits (lines) */
+	u8 v_blank;		/**< lower 8 bits (lines) */
+	u8 v_high;		/**< upper 4 bits each v_active, v_blank */
+    } part1;
+
+    struct {
+	u8 h_sync_off;	/**< lower 8 bits, from hblank start */
+	u8 h_sync_width;	/**< lower 8 bits (pixels) */
+	/** lower 4 bits each vsync offset, vsync width */
+	u8 v_sync_off_width;
+	/**
+	 * 2 high bits of hsync offset, 2 high bits of hsync width,
+	 * bits 4-5 of vsync offset, and 2 high bits of vsync width.
+	 */
+	u8 sync_off_width_high;
+	u8 dtd_flags;
+	u8 sdvo_flags;
+	/** bits 6-7 of vsync offset at bits 6-7 */
+	u8 v_sync_off_high;
+	u8 reserved;
+    } part2;
+} __attribute__((packed));
+
+struct psb_intel_sdvo_pixel_clock_range {
+    u16 min;			/**< pixel clock, in 10kHz units */
+    u16 max;			/**< pixel clock, in 10kHz units */
+} __attribute__((packed));
+
+struct psb_intel_sdvo_preferred_input_timing_args {
+    u16 clock;
+    u16 width;
+    u16 height;
+    u8	interlace:1;
+    u8	scaled:1;
+    u8	pad:6;
+} __attribute__((packed));
+
+/* I2C registers for SDVO */
+#define SDVO_I2C_ARG_0				0x07
+#define SDVO_I2C_ARG_1				0x06
+#define SDVO_I2C_ARG_2				0x05
+#define SDVO_I2C_ARG_3				0x04
+#define SDVO_I2C_ARG_4				0x03
+#define SDVO_I2C_ARG_5				0x02
+#define SDVO_I2C_ARG_6				0x01
+#define SDVO_I2C_ARG_7				0x00
+#define SDVO_I2C_OPCODE				0x08
+#define SDVO_I2C_CMD_STATUS			0x09
+#define SDVO_I2C_RETURN_0			0x0a
+#define SDVO_I2C_RETURN_1			0x0b
+#define SDVO_I2C_RETURN_2			0x0c
+#define SDVO_I2C_RETURN_3			0x0d
+#define SDVO_I2C_RETURN_4			0x0e
+#define SDVO_I2C_RETURN_5			0x0f
+#define SDVO_I2C_RETURN_6			0x10
+#define SDVO_I2C_RETURN_7			0x11
+#define SDVO_I2C_VENDOR_BEGIN			0x20
+
+/* Status results */
+#define SDVO_CMD_STATUS_POWER_ON		0x0
+#define SDVO_CMD_STATUS_SUCCESS			0x1
+#define SDVO_CMD_STATUS_NOTSUPP			0x2
+#define SDVO_CMD_STATUS_INVALID_ARG		0x3
+#define SDVO_CMD_STATUS_PENDING			0x4
+#define SDVO_CMD_STATUS_TARGET_NOT_SPECIFIED	0x5
+#define SDVO_CMD_STATUS_SCALING_NOT_SUPP	0x6
+
+/* SDVO commands, argument/result registers */
+
+#define SDVO_CMD_RESET					0x01
+
+/** Returns a struct intel_sdvo_caps */
+#define SDVO_CMD_GET_DEVICE_CAPS			0x02
+
+#define SDVO_CMD_GET_FIRMWARE_REV			0x86
+# define SDVO_DEVICE_FIRMWARE_MINOR			SDVO_I2C_RETURN_0
+# define SDVO_DEVICE_FIRMWARE_MAJOR			SDVO_I2C_RETURN_1
+# define SDVO_DEVICE_FIRMWARE_PATCH			SDVO_I2C_RETURN_2
+
+/**
+ * Reports which inputs are trained (managed to sync).
+ *
+ * Devices must have trained within 2 vsyncs of a mode change.
+ */
+#define SDVO_CMD_GET_TRAINED_INPUTS			0x03
+struct psb_intel_sdvo_get_trained_inputs_response {
+    unsigned int input0_trained:1;
+    unsigned int input1_trained:1;
+    unsigned int pad:6;
+} __attribute__((packed));
+
+/** Returns a struct intel_sdvo_output_flags of active outputs. */
+#define SDVO_CMD_GET_ACTIVE_OUTPUTS			0x04
+
+/**
+ * Sets the current set of active outputs.
+ *
+ * Takes a struct intel_sdvo_output_flags.  Must be preceded by a SET_IN_OUT_MAP
+ * on multi-output devices.
+ */
+#define SDVO_CMD_SET_ACTIVE_OUTPUTS			0x05
+
+/**
+ * Returns the current mapping of SDVO inputs to outputs on the device.
+ *
+ * Returns two struct intel_sdvo_output_flags structures.
+ */
+#define SDVO_CMD_GET_IN_OUT_MAP				0x06
+struct psb_intel_sdvo_in_out_map {
+    u16 in0, in1;
+};
+
+/**
+ * Sets the current mapping of SDVO inputs to outputs on the device.
+ *
+ * Takes two struct i380_sdvo_output_flags structures.
+ */
+#define SDVO_CMD_SET_IN_OUT_MAP				0x07
+
+/**
+ * Returns a struct intel_sdvo_output_flags of attached displays.
+ */
+#define SDVO_CMD_GET_ATTACHED_DISPLAYS			0x0b
+
+/**
+ * Returns a struct intel_sdvo_ouptut_flags of displays supporting hot plugging.
+ */
+#define SDVO_CMD_GET_HOT_PLUG_SUPPORT			0x0c
+
+/**
+ * Takes a struct intel_sdvo_output_flags.
+ */
+#define SDVO_CMD_SET_ACTIVE_HOT_PLUG			0x0d
+
+/**
+ * Returns a struct intel_sdvo_output_flags of displays with hot plug
+ * interrupts enabled.
+ */
+#define SDVO_CMD_GET_ACTIVE_HOT_PLUG			0x0e
+
+#define SDVO_CMD_GET_INTERRUPT_EVENT_SOURCE		0x0f
+struct intel_sdvo_get_interrupt_event_source_response {
+    u16 interrupt_status;
+    unsigned int ambient_light_interrupt:1;
+    unsigned int hdmi_audio_encrypt_change:1;
+    unsigned int pad:6;
+} __attribute__((packed));
+
+/**
+ * Selects which input is affected by future input commands.
+ *
+ * Commands affected include SET_INPUT_TIMINGS_PART[12],
+ * GET_INPUT_TIMINGS_PART[12], GET_PREFERRED_INPUT_TIMINGS_PART[12],
+ * GET_INPUT_PIXEL_CLOCK_RANGE, and CREATE_PREFERRED_INPUT_TIMINGS.
+ */
+#define SDVO_CMD_SET_TARGET_INPUT			0x10
+struct psb_intel_sdvo_set_target_input_args {
+    unsigned int target_1:1;
+    unsigned int pad:7;
+} __attribute__((packed));
+
+/**
+ * Takes a struct intel_sdvo_output_flags of which outputs are targeted by
+ * future output commands.
+ *
+ * Affected commands inclue SET_OUTPUT_TIMINGS_PART[12],
+ * GET_OUTPUT_TIMINGS_PART[12], and GET_OUTPUT_PIXEL_CLOCK_RANGE.
+ */
+#define SDVO_CMD_SET_TARGET_OUTPUT			0x11
+
+#define SDVO_CMD_GET_INPUT_TIMINGS_PART1		0x12
+#define SDVO_CMD_GET_INPUT_TIMINGS_PART2		0x13
+#define SDVO_CMD_SET_INPUT_TIMINGS_PART1		0x14
+#define SDVO_CMD_SET_INPUT_TIMINGS_PART2		0x15
+#define SDVO_CMD_SET_OUTPUT_TIMINGS_PART1		0x16
+#define SDVO_CMD_SET_OUTPUT_TIMINGS_PART2		0x17
+#define SDVO_CMD_GET_OUTPUT_TIMINGS_PART1		0x18
+#define SDVO_CMD_GET_OUTPUT_TIMINGS_PART2		0x19
+/* Part 1 */
+# define SDVO_DTD_CLOCK_LOW				SDVO_I2C_ARG_0
+# define SDVO_DTD_CLOCK_HIGH				SDVO_I2C_ARG_1
+# define SDVO_DTD_H_ACTIVE				SDVO_I2C_ARG_2
+# define SDVO_DTD_H_BLANK				SDVO_I2C_ARG_3
+# define SDVO_DTD_H_HIGH				SDVO_I2C_ARG_4
+# define SDVO_DTD_V_ACTIVE				SDVO_I2C_ARG_5
+# define SDVO_DTD_V_BLANK				SDVO_I2C_ARG_6
+# define SDVO_DTD_V_HIGH				SDVO_I2C_ARG_7
+/* Part 2 */
+# define SDVO_DTD_HSYNC_OFF				SDVO_I2C_ARG_0
+# define SDVO_DTD_HSYNC_WIDTH				SDVO_I2C_ARG_1
+# define SDVO_DTD_VSYNC_OFF_WIDTH			SDVO_I2C_ARG_2
+# define SDVO_DTD_SYNC_OFF_WIDTH_HIGH			SDVO_I2C_ARG_3
+# define SDVO_DTD_DTD_FLAGS				SDVO_I2C_ARG_4
+# define SDVO_DTD_DTD_FLAG_INTERLACED				(1 << 7)
+# define SDVO_DTD_DTD_FLAG_STEREO_MASK				(3 << 5)
+# define SDVO_DTD_DTD_FLAG_INPUT_MASK				(3 << 3)
+# define SDVO_DTD_DTD_FLAG_SYNC_MASK				(3 << 1)
+# define SDVO_DTD_SDVO_FLAS				SDVO_I2C_ARG_5
+# define SDVO_DTD_SDVO_FLAG_STALL				(1 << 7)
+# define SDVO_DTD_SDVO_FLAG_CENTERED				(0 << 6)
+# define SDVO_DTD_SDVO_FLAG_UPPER_LEFT				(1 << 6)
+# define SDVO_DTD_SDVO_FLAG_SCALING_MASK			(3 << 4)
+# define SDVO_DTD_SDVO_FLAG_SCALING_NONE			(0 << 4)
+# define SDVO_DTD_SDVO_FLAG_SCALING_SHARP			(1 << 4)
+# define SDVO_DTD_SDVO_FLAG_SCALING_SMOOTH			(2 << 4)
+# define SDVO_DTD_VSYNC_OFF_HIGH			SDVO_I2C_ARG_6
+
+/**
+ * Generates a DTD based on the given width, height, and flags.
+ *
+ * This will be supported by any device supporting scaling or interlaced
+ * modes.
+ */
+#define SDVO_CMD_CREATE_PREFERRED_INPUT_TIMING		0x1a
+# define SDVO_PREFERRED_INPUT_TIMING_CLOCK_LOW		SDVO_I2C_ARG_0
+# define SDVO_PREFERRED_INPUT_TIMING_CLOCK_HIGH		SDVO_I2C_ARG_1
+# define SDVO_PREFERRED_INPUT_TIMING_WIDTH_LOW		SDVO_I2C_ARG_2
+# define SDVO_PREFERRED_INPUT_TIMING_WIDTH_HIGH		SDVO_I2C_ARG_3
+# define SDVO_PREFERRED_INPUT_TIMING_HEIGHT_LOW		SDVO_I2C_ARG_4
+# define SDVO_PREFERRED_INPUT_TIMING_HEIGHT_HIGH	SDVO_I2C_ARG_5
+# define SDVO_PREFERRED_INPUT_TIMING_FLAGS		SDVO_I2C_ARG_6
+# define SDVO_PREFERRED_INPUT_TIMING_FLAGS_INTERLACED		(1 << 0)
+# define SDVO_PREFERRED_INPUT_TIMING_FLAGS_SCALED		(1 << 1)
+
+#define SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART1	0x1b
+#define SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART2	0x1c
+
+/** Returns a struct intel_sdvo_pixel_clock_range */
+#define SDVO_CMD_GET_INPUT_PIXEL_CLOCK_RANGE		0x1d
+/** Returns a struct intel_sdvo_pixel_clock_range */
+#define SDVO_CMD_GET_OUTPUT_PIXEL_CLOCK_RANGE		0x1e
+
+/** Returns a byte bitfield containing SDVO_CLOCK_RATE_MULT_* flags */
+#define SDVO_CMD_GET_SUPPORTED_CLOCK_RATE_MULTS		0x1f
+
+/** Returns a byte containing a SDVO_CLOCK_RATE_MULT_* flag */
+#define SDVO_CMD_GET_CLOCK_RATE_MULT			0x20
+/** Takes a byte containing a SDVO_CLOCK_RATE_MULT_* flag */
+#define SDVO_CMD_SET_CLOCK_RATE_MULT			0x21
+# define SDVO_CLOCK_RATE_MULT_1X				(1 << 0)
+# define SDVO_CLOCK_RATE_MULT_2X				(1 << 1)
+# define SDVO_CLOCK_RATE_MULT_4X				(1 << 3)
+
+#define SDVO_CMD_GET_SUPPORTED_TV_FORMATS		0x27
+/** 6 bytes of bit flags for TV formats shared by all TV format functions */
+struct psb_intel_sdvo_tv_format {
+    unsigned int ntsc_m:1;
+    unsigned int ntsc_j:1;
+    unsigned int ntsc_443:1;
+    unsigned int pal_b:1;
+    unsigned int pal_d:1;
+    unsigned int pal_g:1;
+    unsigned int pal_h:1;
+    unsigned int pal_i:1;
+
+    unsigned int pal_m:1;
+    unsigned int pal_n:1;
+    unsigned int pal_nc:1;
+    unsigned int pal_60:1;
+    unsigned int secam_b:1;
+    unsigned int secam_d:1;
+    unsigned int secam_g:1;
+    unsigned int secam_k:1;
+
+    unsigned int secam_k1:1;
+    unsigned int secam_l:1;
+    unsigned int secam_60:1;
+    unsigned int hdtv_std_smpte_240m_1080i_59:1;
+    unsigned int hdtv_std_smpte_240m_1080i_60:1;
+    unsigned int hdtv_std_smpte_260m_1080i_59:1;
+    unsigned int hdtv_std_smpte_260m_1080i_60:1;
+    unsigned int hdtv_std_smpte_274m_1080i_50:1;
+
+    unsigned int hdtv_std_smpte_274m_1080i_59:1;
+    unsigned int hdtv_std_smpte_274m_1080i_60:1;
+    unsigned int hdtv_std_smpte_274m_1080p_23:1;
+    unsigned int hdtv_std_smpte_274m_1080p_24:1;
+    unsigned int hdtv_std_smpte_274m_1080p_25:1;
+    unsigned int hdtv_std_smpte_274m_1080p_29:1;
+    unsigned int hdtv_std_smpte_274m_1080p_30:1;
+    unsigned int hdtv_std_smpte_274m_1080p_50:1;
+
+    unsigned int hdtv_std_smpte_274m_1080p_59:1;
+    unsigned int hdtv_std_smpte_274m_1080p_60:1;
+    unsigned int hdtv_std_smpte_295m_1080i_50:1;
+    unsigned int hdtv_std_smpte_295m_1080p_50:1;
+    unsigned int hdtv_std_smpte_296m_720p_59:1;
+    unsigned int hdtv_std_smpte_296m_720p_60:1;
+    unsigned int hdtv_std_smpte_296m_720p_50:1;
+    unsigned int hdtv_std_smpte_293m_480p_59:1;
+
+    unsigned int hdtv_std_smpte_170m_480i_59:1;
+    unsigned int hdtv_std_iturbt601_576i_50:1;
+    unsigned int hdtv_std_iturbt601_576p_50:1;
+    unsigned int hdtv_std_eia_7702a_480i_60:1;
+    unsigned int hdtv_std_eia_7702a_480p_60:1;
+    unsigned int pad:3;
+} __attribute__((packed));
+
+#define SDVO_CMD_GET_TV_FORMAT				0x28
+
+#define SDVO_CMD_SET_TV_FORMAT				0x29
+
+/** Returns the resolutiosn that can be used with the given TV format */
+#define SDVO_CMD_GET_SDTV_RESOLUTION_SUPPORT		0x83
+struct psb_intel_sdvo_sdtv_resolution_request {
+    unsigned int ntsc_m:1;
+    unsigned int ntsc_j:1;
+    unsigned int ntsc_443:1;
+    unsigned int pal_b:1;
+    unsigned int pal_d:1;
+    unsigned int pal_g:1;
+    unsigned int pal_h:1;
+    unsigned int pal_i:1;
+
+    unsigned int pal_m:1;
+    unsigned int pal_n:1;
+    unsigned int pal_nc:1;
+    unsigned int pal_60:1;
+    unsigned int secam_b:1;
+    unsigned int secam_d:1;
+    unsigned int secam_g:1;
+    unsigned int secam_k:1;
+
+    unsigned int secam_k1:1;
+    unsigned int secam_l:1;
+    unsigned int secam_60:1;
+    unsigned int pad:5;
+} __attribute__((packed));
+
+struct psb_intel_sdvo_sdtv_resolution_reply {
+    unsigned int res_320x200:1;
+    unsigned int res_320x240:1;
+    unsigned int res_400x300:1;
+    unsigned int res_640x350:1;
+    unsigned int res_640x400:1;
+    unsigned int res_640x480:1;
+    unsigned int res_704x480:1;
+    unsigned int res_704x576:1;
+
+    unsigned int res_720x350:1;
+    unsigned int res_720x400:1;
+    unsigned int res_720x480:1;
+    unsigned int res_720x540:1;
+    unsigned int res_720x576:1;
+    unsigned int res_768x576:1;
+    unsigned int res_800x600:1;
+    unsigned int res_832x624:1;
+
+    unsigned int res_920x766:1;
+    unsigned int res_1024x768:1;
+    unsigned int res_1280x1024:1;
+    unsigned int pad:5;
+} __attribute__((packed));
+
+/* Get supported resolution with squire pixel aspect ratio that can be
+   scaled for the requested HDTV format */
+#define SDVO_CMD_GET_SCALED_HDTV_RESOLUTION_SUPPORT		0x85
+
+struct psb_intel_sdvo_hdtv_resolution_request {
+    unsigned int hdtv_std_smpte_240m_1080i_59:1;
+    unsigned int hdtv_std_smpte_240m_1080i_60:1;
+    unsigned int hdtv_std_smpte_260m_1080i_59:1;
+    unsigned int hdtv_std_smpte_260m_1080i_60:1;
+    unsigned int hdtv_std_smpte_274m_1080i_50:1;
+    unsigned int hdtv_std_smpte_274m_1080i_59:1;
+    unsigned int hdtv_std_smpte_274m_1080i_60:1;
+    unsigned int hdtv_std_smpte_274m_1080p_23:1;
+
+    unsigned int hdtv_std_smpte_274m_1080p_24:1;
+    unsigned int hdtv_std_smpte_274m_1080p_25:1;
+    unsigned int hdtv_std_smpte_274m_1080p_29:1;
+    unsigned int hdtv_std_smpte_274m_1080p_30:1;
+    unsigned int hdtv_std_smpte_274m_1080p_50:1;
+    unsigned int hdtv_std_smpte_274m_1080p_59:1;
+    unsigned int hdtv_std_smpte_274m_1080p_60:1;
+    unsigned int hdtv_std_smpte_295m_1080i_50:1;
+
+    unsigned int hdtv_std_smpte_295m_1080p_50:1;
+    unsigned int hdtv_std_smpte_296m_720p_59:1;
+    unsigned int hdtv_std_smpte_296m_720p_60:1;
+    unsigned int hdtv_std_smpte_296m_720p_50:1;
+    unsigned int hdtv_std_smpte_293m_480p_59:1;
+    unsigned int hdtv_std_smpte_170m_480i_59:1;
+    unsigned int hdtv_std_iturbt601_576i_50:1;
+    unsigned int hdtv_std_iturbt601_576p_50:1;
+
+    unsigned int hdtv_std_eia_7702a_480i_60:1;
+    unsigned int hdtv_std_eia_7702a_480p_60:1;
+    unsigned int pad:6;
+} __attribute__((packed));
+
+struct psb_intel_sdvo_hdtv_resolution_reply {
+    unsigned int res_640x480:1;
+    unsigned int res_800x600:1;
+    unsigned int res_1024x768:1;
+    unsigned int res_1280x960:1;
+    unsigned int res_1400x1050:1;
+    unsigned int res_1600x1200:1;
+    unsigned int res_1920x1440:1;
+    unsigned int res_2048x1536:1;
+
+    unsigned int res_2560x1920:1;
+    unsigned int res_3200x2400:1;
+    unsigned int res_3840x2880:1;
+    unsigned int pad1:5;
+
+    unsigned int res_848x480:1;
+    unsigned int res_1064x600:1;
+    unsigned int res_1280x720:1;
+    unsigned int res_1360x768:1;
+    unsigned int res_1704x960:1;
+    unsigned int res_1864x1050:1;
+    unsigned int res_1920x1080:1;
+    unsigned int res_2128x1200:1;
+
+    unsigned int res_2560x1400:1;
+    unsigned int res_2728x1536:1;
+    unsigned int res_3408x1920:1;
+    unsigned int res_4264x2400:1;
+    unsigned int res_5120x2880:1;
+    unsigned int pad2:3;
+
+    unsigned int res_768x480:1;
+    unsigned int res_960x600:1;
+    unsigned int res_1152x720:1;
+    unsigned int res_1124x768:1;
+    unsigned int res_1536x960:1;
+    unsigned int res_1680x1050:1;
+    unsigned int res_1728x1080:1;
+    unsigned int res_1920x1200:1;
+
+    unsigned int res_2304x1440:1;
+    unsigned int res_2456x1536:1;
+    unsigned int res_3072x1920:1;
+    unsigned int res_3840x2400:1;
+    unsigned int res_4608x2880:1;
+    unsigned int pad3:3;
+
+    unsigned int res_1280x1024:1;
+    unsigned int pad4:7;
+
+    unsigned int res_1280x768:1;
+    unsigned int pad5:7;
+} __attribute__((packed));
+
+/* Get supported power state returns info for encoder and monitor, rely on
+   last SetTargetInput and SetTargetOutput calls */
+#define SDVO_CMD_GET_SUPPORTED_POWER_STATES		0x2a
+/* Get power state returns info for encoder and monitor, rely on last
+   SetTargetInput and SetTargetOutput calls */
+#define SDVO_CMD_GET_POWER_STATE			0x2b
+#define SDVO_CMD_GET_ENCODER_POWER_STATE		0x2b
+#define SDVO_CMD_SET_ENCODER_POWER_STATE		0x2c
+# define SDVO_ENCODER_STATE_ON					(1 << 0)
+# define SDVO_ENCODER_STATE_STANDBY				(1 << 1)
+# define SDVO_ENCODER_STATE_SUSPEND				(1 << 2)
+# define SDVO_ENCODER_STATE_OFF					(1 << 3)
+# define SDVO_MONITOR_STATE_ON					(1 << 4)
+# define SDVO_MONITOR_STATE_STANDBY				(1 << 5)
+# define SDVO_MONITOR_STATE_SUSPEND				(1 << 6)
+# define SDVO_MONITOR_STATE_OFF					(1 << 7)
+
+#define SDVO_CMD_GET_MAX_PANEL_POWER_SEQUENCING		0x2d
+#define SDVO_CMD_GET_PANEL_POWER_SEQUENCING		0x2e
+#define SDVO_CMD_SET_PANEL_POWER_SEQUENCING		0x2f
+/**
+ * The panel power sequencing parameters are in units of milliseconds.
+ * The high fields are bits 8:9 of the 10-bit values.
+ */
+struct psb_sdvo_panel_power_sequencing {
+    u8 t0;
+    u8 t1;
+    u8 t2;
+    u8 t3;
+    u8 t4;
+
+    unsigned int t0_high:2;
+    unsigned int t1_high:2;
+    unsigned int t2_high:2;
+    unsigned int t3_high:2;
+
+    unsigned int t4_high:2;
+    unsigned int pad:6;
+} __attribute__((packed));
+
+#define SDVO_CMD_GET_MAX_BACKLIGHT_LEVEL		0x30
+struct sdvo_max_backlight_reply {
+    u8 max_value;
+    u8 default_value;
+} __attribute__((packed));
+
+#define SDVO_CMD_GET_BACKLIGHT_LEVEL			0x31
+#define SDVO_CMD_SET_BACKLIGHT_LEVEL			0x32
+
+#define SDVO_CMD_GET_AMBIENT_LIGHT			0x33
+struct sdvo_get_ambient_light_reply {
+    u16 trip_low;
+    u16 trip_high;
+    u16 value;
+} __attribute__((packed));
+#define SDVO_CMD_SET_AMBIENT_LIGHT			0x34
+struct sdvo_set_ambient_light_reply {
+    u16 trip_low;
+    u16 trip_high;
+    unsigned int enable:1;
+    unsigned int pad:7;
+} __attribute__((packed));
+
+/* Set display power state */
+#define SDVO_CMD_SET_DISPLAY_POWER_STATE		0x7d
+# define SDVO_DISPLAY_STATE_ON				(1 << 0)
+# define SDVO_DISPLAY_STATE_STANDBY			(1 << 1)
+# define SDVO_DISPLAY_STATE_SUSPEND			(1 << 2)
+# define SDVO_DISPLAY_STATE_OFF				(1 << 3)
+
+#define SDVO_CMD_GET_SUPPORTED_ENHANCEMENTS		0x84
+struct psb_intel_sdvo_enhancements_reply {
+    unsigned int flicker_filter:1;
+    unsigned int flicker_filter_adaptive:1;
+    unsigned int flicker_filter_2d:1;
+    unsigned int saturation:1;
+    unsigned int hue:1;
+    unsigned int brightness:1;
+    unsigned int contrast:1;
+    unsigned int overscan_h:1;
+
+    unsigned int overscan_v:1;
+    unsigned int hpos:1;
+    unsigned int vpos:1;
+    unsigned int sharpness:1;
+    unsigned int dot_crawl:1;
+    unsigned int dither:1;
+    unsigned int tv_chroma_filter:1;
+    unsigned int tv_luma_filter:1;
+} __attribute__((packed));
+
+/* Picture enhancement limits below are dependent on the current TV format,
+ * and thus need to be queried and set after it.
+ */
+#define SDVO_CMD_GET_MAX_FLICKER_FILTER			0x4d
+#define SDVO_CMD_GET_MAX_FLICKER_FILTER_ADAPTIVE	0x7b
+#define SDVO_CMD_GET_MAX_FLICKER_FILTER_2D		0x52
+#define SDVO_CMD_GET_MAX_SATURATION			0x55
+#define SDVO_CMD_GET_MAX_HUE				0x58
+#define SDVO_CMD_GET_MAX_BRIGHTNESS			0x5b
+#define SDVO_CMD_GET_MAX_CONTRAST			0x5e
+#define SDVO_CMD_GET_MAX_OVERSCAN_H			0x61
+#define SDVO_CMD_GET_MAX_OVERSCAN_V			0x64
+#define SDVO_CMD_GET_MAX_HPOS				0x67
+#define SDVO_CMD_GET_MAX_VPOS				0x6a
+#define SDVO_CMD_GET_MAX_SHARPNESS			0x6d
+#define SDVO_CMD_GET_MAX_TV_CHROMA_FILTER		0x74
+#define SDVO_CMD_GET_MAX_TV_LUMA_FILTER			0x77
+struct psb_intel_sdvo_enhancement_limits_reply {
+    u16 max_value;
+    u16 default_value;
+} __attribute__((packed));
+
+#define SDVO_CMD_GET_LVDS_PANEL_INFORMATION		0x7f
+#define SDVO_CMD_SET_LVDS_PANEL_INFORMATION		0x80
+# define SDVO_LVDS_COLOR_DEPTH_18			(0 << 0)
+# define SDVO_LVDS_COLOR_DEPTH_24			(1 << 0)
+# define SDVO_LVDS_CONNECTOR_SPWG			(0 << 2)
+# define SDVO_LVDS_CONNECTOR_OPENLDI			(1 << 2)
+# define SDVO_LVDS_SINGLE_CHANNEL			(0 << 4)
+# define SDVO_LVDS_DUAL_CHANNEL				(1 << 4)
+
+#define SDVO_CMD_GET_FLICKER_FILTER			0x4e
+#define SDVO_CMD_SET_FLICKER_FILTER			0x4f
+#define SDVO_CMD_GET_FLICKER_FILTER_ADAPTIVE		0x50
+#define SDVO_CMD_SET_FLICKER_FILTER_ADAPTIVE		0x51
+#define SDVO_CMD_GET_FLICKER_FILTER_2D			0x53
+#define SDVO_CMD_SET_FLICKER_FILTER_2D			0x54
+#define SDVO_CMD_GET_SATURATION				0x56
+#define SDVO_CMD_SET_SATURATION				0x57
+#define SDVO_CMD_GET_HUE				0x59
+#define SDVO_CMD_SET_HUE				0x5a
+#define SDVO_CMD_GET_BRIGHTNESS				0x5c
+#define SDVO_CMD_SET_BRIGHTNESS				0x5d
+#define SDVO_CMD_GET_CONTRAST				0x5f
+#define SDVO_CMD_SET_CONTRAST				0x60
+#define SDVO_CMD_GET_OVERSCAN_H				0x62
+#define SDVO_CMD_SET_OVERSCAN_H				0x63
+#define SDVO_CMD_GET_OVERSCAN_V				0x65
+#define SDVO_CMD_SET_OVERSCAN_V				0x66
+#define SDVO_CMD_GET_HPOS				0x68
+#define SDVO_CMD_SET_HPOS				0x69
+#define SDVO_CMD_GET_VPOS				0x6b
+#define SDVO_CMD_SET_VPOS				0x6c
+#define SDVO_CMD_GET_SHARPNESS				0x6e
+#define SDVO_CMD_SET_SHARPNESS				0x6f
+#define SDVO_CMD_GET_TV_CHROMA_FILTER			0x75
+#define SDVO_CMD_SET_TV_CHROMA_FILTER			0x76
+#define SDVO_CMD_GET_TV_LUMA_FILTER			0x78
+#define SDVO_CMD_SET_TV_LUMA_FILTER			0x79
+struct psb_intel_sdvo_enhancements_arg {
+    u16 value;
+}__attribute__((packed));
+
+#define SDVO_CMD_GET_DOT_CRAWL				0x70
+#define SDVO_CMD_SET_DOT_CRAWL				0x71
+# define SDVO_DOT_CRAWL_ON					(1 << 0)
+# define SDVO_DOT_CRAWL_DEFAULT_ON				(1 << 1)
+
+#define SDVO_CMD_GET_DITHER				0x72
+#define SDVO_CMD_SET_DITHER				0x73
+# define SDVO_DITHER_ON						(1 << 0)
+# define SDVO_DITHER_DEFAULT_ON					(1 << 1)
+
+#define SDVO_CMD_SET_CONTROL_BUS_SWITCH			0x7a
+# define SDVO_CONTROL_BUS_PROM				(1 << 0)
+# define SDVO_CONTROL_BUS_DDC1				(1 << 1)
+# define SDVO_CONTROL_BUS_DDC2				(1 << 2)
+# define SDVO_CONTROL_BUS_DDC3				(1 << 3)
+
+/* HDMI op codes */
+#define SDVO_CMD_GET_SUPP_ENCODE	0x9d
+#define SDVO_CMD_GET_ENCODE		0x9e
+#define SDVO_CMD_SET_ENCODE		0x9f
+  #define SDVO_ENCODE_DVI	0x0
+  #define SDVO_ENCODE_HDMI	0x1
+#define SDVO_CMD_SET_PIXEL_REPLI	0x8b
+#define SDVO_CMD_GET_PIXEL_REPLI	0x8c
+#define SDVO_CMD_GET_COLORIMETRY_CAP	0x8d
+#define SDVO_CMD_SET_COLORIMETRY	0x8e
+  #define SDVO_COLORIMETRY_RGB256   0x0
+  #define SDVO_COLORIMETRY_RGB220   0x1
+  #define SDVO_COLORIMETRY_YCrCb422 0x3
+  #define SDVO_COLORIMETRY_YCrCb444 0x4
+#define SDVO_CMD_GET_COLORIMETRY	0x8f
+#define SDVO_CMD_GET_AUDIO_ENCRYPT_PREFER 0x90
+#define SDVO_CMD_SET_AUDIO_STAT		0x91
+#define SDVO_CMD_GET_AUDIO_STAT		0x92
+#define SDVO_CMD_SET_HBUF_INDEX		0x93
+#define SDVO_CMD_GET_HBUF_INDEX		0x94
+#define SDVO_CMD_GET_HBUF_INFO		0x95
+#define SDVO_CMD_SET_HBUF_AV_SPLIT	0x96
+#define SDVO_CMD_GET_HBUF_AV_SPLIT	0x97
+#define SDVO_CMD_SET_HBUF_DATA		0x98
+#define SDVO_CMD_GET_HBUF_DATA		0x99
+#define SDVO_CMD_SET_HBUF_TXRATE	0x9a
+#define SDVO_CMD_GET_HBUF_TXRATE	0x9b
+  #define SDVO_HBUF_TX_DISABLED	(0 << 6)
+  #define SDVO_HBUF_TX_ONCE	(2 << 6)
+  #define SDVO_HBUF_TX_VSYNC	(3 << 6)
+#define SDVO_CMD_GET_AUDIO_TX_INFO	0x9c
+#define SDVO_NEED_TO_STALL  (1 << 7)
+
+struct psb_intel_sdvo_encode {
+    u8 dvi_rev;
+    u8 hdmi_rev;
+} __attribute__ ((packed));
diff --git a/drivers/gpu/drm/gma500/psb_irq.c b/drivers/gpu/drm/gma500/psb_irq.c
new file mode 100644
index 000000000000..7be802baceb5
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_irq.c
@@ -0,0 +1,564 @@
+/**************************************************************************
+ * Copyright (c) 2007, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Intel funded Tungsten Graphics (http://www.tungstengraphics.com) to
+ * develop this driver.
+ *
+ **************************************************************************/
+/*
+ */
+
+#include <drm/drmP.h>
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+
+/*
+ * inline functions
+ */
+
+static inline u32
+psb_pipestat(int pipe)
+{
+	if (pipe == 0)
+		return PIPEASTAT;
+	if (pipe == 1)
+		return PIPEBSTAT;
+	if (pipe == 2)
+		return PIPECSTAT;
+	BUG();
+}
+
+static inline u32
+mid_pipe_event(int pipe)
+{
+	if (pipe == 0)
+		return _PSB_PIPEA_EVENT_FLAG;
+	if (pipe == 1)
+		return _MDFLD_PIPEB_EVENT_FLAG;
+	if (pipe == 2)
+		return _MDFLD_PIPEC_EVENT_FLAG;
+	BUG();
+}
+
+static inline u32
+mid_pipe_vsync(int pipe)
+{
+	if (pipe == 0)
+		return _PSB_VSYNC_PIPEA_FLAG;
+	if (pipe == 1)
+		return _PSB_VSYNC_PIPEB_FLAG;
+	if (pipe == 2)
+		return _MDFLD_PIPEC_VBLANK_FLAG;
+	BUG();
+}
+
+static inline u32
+mid_pipeconf(int pipe)
+{
+	if (pipe == 0)
+		return PIPEACONF;
+	if (pipe == 1)
+		return PIPEBCONF;
+	if (pipe == 2)
+		return PIPECCONF;
+	BUG();
+}
+
+void
+psb_enable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask)
+{
+	if ((dev_priv->pipestat[pipe] & mask) != mask) {
+		u32 reg = psb_pipestat(pipe);
+		dev_priv->pipestat[pipe] |= mask;
+		/* Enable the interrupt, clear any pending status */
+		if (gma_power_begin(dev_priv->dev, false)) {
+			u32 writeVal = PSB_RVDC32(reg);
+			writeVal |= (mask | (mask >> 16));
+			PSB_WVDC32(writeVal, reg);
+			(void) PSB_RVDC32(reg);
+			gma_power_end(dev_priv->dev);
+		}
+	}
+}
+
+void
+psb_disable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask)
+{
+	if ((dev_priv->pipestat[pipe] & mask) != 0) {
+		u32 reg = psb_pipestat(pipe);
+		dev_priv->pipestat[pipe] &= ~mask;
+		if (gma_power_begin(dev_priv->dev, false)) {
+			u32 writeVal = PSB_RVDC32(reg);
+			writeVal &= ~mask;
+			PSB_WVDC32(writeVal, reg);
+			(void) PSB_RVDC32(reg);
+			gma_power_end(dev_priv->dev);
+		}
+	}
+}
+
+void mid_enable_pipe_event(struct drm_psb_private *dev_priv, int pipe)
+{
+	if (gma_power_begin(dev_priv->dev, false)) {
+		u32 pipe_event = mid_pipe_event(pipe);
+		dev_priv->vdc_irq_mask |= pipe_event;
+		PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+		PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+		gma_power_end(dev_priv->dev);
+	}
+}
+
+void mid_disable_pipe_event(struct drm_psb_private *dev_priv, int pipe)
+{
+	if (dev_priv->pipestat[pipe] == 0) {
+		if (gma_power_begin(dev_priv->dev, false)) {
+			u32 pipe_event = mid_pipe_event(pipe);
+			dev_priv->vdc_irq_mask &= ~pipe_event;
+			PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+			PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+			gma_power_end(dev_priv->dev);
+		}
+	}
+}
+
+/**
+ * Display controller interrupt handler for pipe event.
+ *
+ */
+static void mid_pipe_event_handler(struct drm_device *dev, int pipe)
+{
+	struct drm_psb_private *dev_priv =
+	    (struct drm_psb_private *) dev->dev_private;
+
+	uint32_t pipe_stat_val = 0;
+	uint32_t pipe_stat_reg = psb_pipestat(pipe);
+	uint32_t pipe_enable = dev_priv->pipestat[pipe];
+	uint32_t pipe_status = dev_priv->pipestat[pipe] >> 16;
+	uint32_t pipe_clear;
+	uint32_t i = 0;
+
+	spin_lock(&dev_priv->irqmask_lock);
+
+	pipe_stat_val = PSB_RVDC32(pipe_stat_reg);
+	pipe_stat_val &= pipe_enable | pipe_status;
+	pipe_stat_val &= pipe_stat_val >> 16;
+
+	spin_unlock(&dev_priv->irqmask_lock);
+
+	/* Clear the 2nd level interrupt status bits
+	 * Sometimes the bits are very sticky so we repeat until they unstick */
+	for (i = 0; i < 0xffff; i++) {
+		PSB_WVDC32(PSB_RVDC32(pipe_stat_reg), pipe_stat_reg);
+		pipe_clear = PSB_RVDC32(pipe_stat_reg) & pipe_status;
+
+		if (pipe_clear == 0)
+			break;
+	}
+
+	if (pipe_clear)
+		dev_err(dev->dev,
+		"%s, can't clear status bits for pipe %d, its value = 0x%x.\n",
+		__func__, pipe, PSB_RVDC32(pipe_stat_reg));
+
+	if (pipe_stat_val & PIPE_VBLANK_STATUS)
+		drm_handle_vblank(dev, pipe);
+
+	if (pipe_stat_val & PIPE_TE_STATUS)
+		drm_handle_vblank(dev, pipe);
+}
+
+/*
+ * Display controller interrupt handler.
+ */
+static void psb_vdc_interrupt(struct drm_device *dev, uint32_t vdc_stat)
+{
+	if (vdc_stat & _PSB_VSYNC_PIPEA_FLAG)
+		mid_pipe_event_handler(dev, 0);
+
+	if (vdc_stat & _PSB_VSYNC_PIPEB_FLAG)
+		mid_pipe_event_handler(dev, 1);
+}
+
+irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
+{
+	struct drm_device *dev = (struct drm_device *) arg;
+	struct drm_psb_private *dev_priv =
+	    (struct drm_psb_private *) dev->dev_private;
+
+	uint32_t vdc_stat, dsp_int = 0, sgx_int = 0;
+	int handled = 0;
+
+	spin_lock(&dev_priv->irqmask_lock);
+
+	vdc_stat = PSB_RVDC32(PSB_INT_IDENTITY_R);
+
+	if (vdc_stat & _PSB_PIPE_EVENT_FLAG)
+		dsp_int = 1;
+
+	/* FIXME: Handle Medfield
+	if (vdc_stat & _MDFLD_DISP_ALL_IRQ_FLAG)
+		dsp_int = 1;
+	*/
+
+	if (vdc_stat & _PSB_IRQ_SGX_FLAG)
+		sgx_int = 1;
+
+	vdc_stat &= dev_priv->vdc_irq_mask;
+	spin_unlock(&dev_priv->irqmask_lock);
+
+	if (dsp_int && gma_power_is_on(dev)) {
+		psb_vdc_interrupt(dev, vdc_stat);
+		handled = 1;
+	}
+
+	if (sgx_int) {
+		/* Not expected - we have it masked, shut it up */
+		u32 s, s2;
+		s = PSB_RSGX32(PSB_CR_EVENT_STATUS);
+		s2 = PSB_RSGX32(PSB_CR_EVENT_STATUS2);
+		PSB_WSGX32(s, PSB_CR_EVENT_HOST_CLEAR);
+		PSB_WSGX32(s2, PSB_CR_EVENT_HOST_CLEAR2);
+		/* if s & _PSB_CE_TWOD_COMPLETE we have 2D done but
+		   we may as well poll even if we add that ! */
+		handled = 1;
+	}
+
+	PSB_WVDC32(vdc_stat, PSB_INT_IDENTITY_R);
+	(void) PSB_RVDC32(PSB_INT_IDENTITY_R);
+	DRM_READMEMORYBARRIER();
+
+	if (!handled)
+		return IRQ_NONE;
+
+	return IRQ_HANDLED;
+}
+
+void psb_irq_preinstall(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv =
+	    (struct drm_psb_private *) dev->dev_private;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+	if (gma_power_is_on(dev))
+		PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
+	if (dev->vblank_enabled[0])
+		dev_priv->vdc_irq_mask |= _PSB_VSYNC_PIPEA_FLAG;
+	if (dev->vblank_enabled[1])
+		dev_priv->vdc_irq_mask |= _PSB_VSYNC_PIPEB_FLAG;
+
+	/* FIXME: Handle Medfield irq mask
+	if (dev->vblank_enabled[1])
+		dev_priv->vdc_irq_mask |= _MDFLD_PIPEB_EVENT_FLAG;
+	if (dev->vblank_enabled[2])
+		dev_priv->vdc_irq_mask |= _MDFLD_PIPEC_EVENT_FLAG;
+	*/
+
+	/* This register is safe even if display island is off */
+	PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+}
+
+int psb_irq_postinstall(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv =
+	    (struct drm_psb_private *) dev->dev_private;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+	/* This register is safe even if display island is off */
+	PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+	PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
+
+	if (dev->vblank_enabled[0])
+		psb_enable_pipestat(dev_priv, 0, PIPE_VBLANK_INTERRUPT_ENABLE);
+	else
+		psb_disable_pipestat(dev_priv, 0, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+	if (dev->vblank_enabled[1])
+		psb_enable_pipestat(dev_priv, 1, PIPE_VBLANK_INTERRUPT_ENABLE);
+	else
+		psb_disable_pipestat(dev_priv, 1, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+	if (dev->vblank_enabled[2])
+		psb_enable_pipestat(dev_priv, 2, PIPE_VBLANK_INTERRUPT_ENABLE);
+	else
+		psb_disable_pipestat(dev_priv, 2, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+	return 0;
+}
+
+void psb_irq_uninstall(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv =
+	    (struct drm_psb_private *) dev->dev_private;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+	PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
+
+	if (dev->vblank_enabled[0])
+		psb_disable_pipestat(dev_priv, 0, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+	if (dev->vblank_enabled[1])
+		psb_disable_pipestat(dev_priv, 1, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+	if (dev->vblank_enabled[2])
+		psb_disable_pipestat(dev_priv, 2, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+	dev_priv->vdc_irq_mask &= _PSB_IRQ_SGX_FLAG |
+				  _PSB_IRQ_MSVDX_FLAG |
+				  _LNC_IRQ_TOPAZ_FLAG;
+
+	/* These two registers are safe even if display island is off */
+	PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+	PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+
+	wmb();
+
+	/* This register is safe even if display island is off */
+	PSB_WVDC32(PSB_RVDC32(PSB_INT_IDENTITY_R), PSB_INT_IDENTITY_R);
+	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+}
+
+void psb_irq_turn_on_dpst(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv =
+		(struct drm_psb_private *) dev->dev_private;
+	u32 hist_reg;
+	u32 pwm_reg;
+
+	if (gma_power_begin(dev, false)) {
+		PSB_WVDC32(1 << 31, HISTOGRAM_LOGIC_CONTROL);
+		hist_reg = PSB_RVDC32(HISTOGRAM_LOGIC_CONTROL);
+		PSB_WVDC32(1 << 31, HISTOGRAM_INT_CONTROL);
+		hist_reg = PSB_RVDC32(HISTOGRAM_INT_CONTROL);
+
+		PSB_WVDC32(0x80010100, PWM_CONTROL_LOGIC);
+		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+		PSB_WVDC32(pwm_reg | PWM_PHASEIN_ENABLE
+						| PWM_PHASEIN_INT_ENABLE,
+							   PWM_CONTROL_LOGIC);
+		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+
+		psb_enable_pipestat(dev_priv, 0, PIPE_DPST_EVENT_ENABLE);
+
+		hist_reg = PSB_RVDC32(HISTOGRAM_INT_CONTROL);
+		PSB_WVDC32(hist_reg | HISTOGRAM_INT_CTRL_CLEAR,
+							HISTOGRAM_INT_CONTROL);
+		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+		PSB_WVDC32(pwm_reg | 0x80010100 | PWM_PHASEIN_ENABLE,
+							PWM_CONTROL_LOGIC);
+
+		gma_power_end(dev);
+	}
+}
+
+int psb_irq_enable_dpst(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv =
+		(struct drm_psb_private *) dev->dev_private;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+	/* enable DPST */
+	mid_enable_pipe_event(dev_priv, 0);
+	psb_irq_turn_on_dpst(dev);
+
+	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+	return 0;
+}
+
+void psb_irq_turn_off_dpst(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv =
+	    (struct drm_psb_private *) dev->dev_private;
+	u32 hist_reg;
+	u32 pwm_reg;
+
+	if (gma_power_begin(dev, false)) {
+		PSB_WVDC32(0x00000000, HISTOGRAM_INT_CONTROL);
+		hist_reg = PSB_RVDC32(HISTOGRAM_INT_CONTROL);
+
+		psb_disable_pipestat(dev_priv, 0, PIPE_DPST_EVENT_ENABLE);
+
+		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+		PSB_WVDC32(pwm_reg & !(PWM_PHASEIN_INT_ENABLE),
+							PWM_CONTROL_LOGIC);
+		pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+
+		gma_power_end(dev);
+	}
+}
+
+int psb_irq_disable_dpst(struct drm_device *dev)
+{
+	struct drm_psb_private *dev_priv =
+	    (struct drm_psb_private *) dev->dev_private;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+	mid_disable_pipe_event(dev_priv, 0);
+	psb_irq_turn_off_dpst(dev);
+
+	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+
+	return 0;
+}
+
+#ifdef PSB_FIXME
+static int psb_vblank_do_wait(struct drm_device *dev,
+			      unsigned int *sequence, atomic_t *counter)
+{
+	unsigned int cur_vblank;
+	int ret = 0;
+	DRM_WAIT_ON(ret, dev->vbl_queue, 3 * DRM_HZ,
+		    (((cur_vblank = atomic_read(counter))
+		      - *sequence) <= (1 << 23)));
+	*sequence = cur_vblank;
+
+	return ret;
+}
+#endif
+
+/*
+ * It is used to enable VBLANK interrupt
+ */
+int psb_enable_vblank(struct drm_device *dev, int pipe)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned long irqflags;
+	uint32_t reg_val = 0;
+	uint32_t pipeconf_reg = mid_pipeconf(pipe);
+
+	if (gma_power_begin(dev, false)) {
+		reg_val = REG_READ(pipeconf_reg);
+		gma_power_end(dev);
+	}
+
+	if (!(reg_val & PIPEACONF_ENABLE))
+		return -EINVAL;
+
+	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+	if (pipe == 0)
+		dev_priv->vdc_irq_mask |= _PSB_VSYNC_PIPEA_FLAG;
+	else if (pipe == 1)
+		dev_priv->vdc_irq_mask |= _PSB_VSYNC_PIPEB_FLAG;
+
+	PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+	PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+	psb_enable_pipestat(dev_priv, pipe, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+
+	return 0;
+}
+
+/*
+ * It is used to disable VBLANK interrupt
+ */
+void psb_disable_vblank(struct drm_device *dev, int pipe)
+{
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+	if (pipe == 0)
+		dev_priv->vdc_irq_mask &= ~_PSB_VSYNC_PIPEA_FLAG;
+	else if (pipe == 1)
+		dev_priv->vdc_irq_mask &= ~_PSB_VSYNC_PIPEB_FLAG;
+
+	PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+	PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+	psb_disable_pipestat(dev_priv, pipe, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+	spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+}
+
+/* Called from drm generic code, passed a 'crtc', which
+ * we use as a pipe index
+ */
+u32 psb_get_vblank_counter(struct drm_device *dev, int pipe)
+{
+	uint32_t high_frame = PIPEAFRAMEHIGH;
+	uint32_t low_frame = PIPEAFRAMEPIXEL;
+	uint32_t pipeconf_reg = PIPEACONF;
+	uint32_t reg_val = 0;
+	uint32_t high1 = 0, high2 = 0, low = 0, count = 0;
+
+	switch (pipe) {
+	case 0:
+		break;
+	case 1:
+		high_frame = PIPEBFRAMEHIGH;
+		low_frame = PIPEBFRAMEPIXEL;
+		pipeconf_reg = PIPEBCONF;
+		break;
+	case 2:
+		high_frame = PIPECFRAMEHIGH;
+		low_frame = PIPECFRAMEPIXEL;
+		pipeconf_reg = PIPECCONF;
+		break;
+	default:
+		dev_err(dev->dev, "%s, invalid pipe.\n", __func__);
+		return 0;
+	}
+
+	if (!gma_power_begin(dev, false))
+		return 0;
+
+	reg_val = REG_READ(pipeconf_reg);
+
+	if (!(reg_val & PIPEACONF_ENABLE)) {
+		dev_err(dev->dev, "trying to get vblank count for disabled pipe %d\n",
+								pipe);
+		goto psb_get_vblank_counter_exit;
+	}
+
+	/*
+	 * High & low register fields aren't synchronized, so make sure
+	 * we get a low value that's stable across two reads of the high
+	 * register.
+	 */
+	do {
+		high1 = ((REG_READ(high_frame) & PIPE_FRAME_HIGH_MASK) >>
+			 PIPE_FRAME_HIGH_SHIFT);
+		low =  ((REG_READ(low_frame) & PIPE_FRAME_LOW_MASK) >>
+			PIPE_FRAME_LOW_SHIFT);
+		high2 = ((REG_READ(high_frame) & PIPE_FRAME_HIGH_MASK) >>
+			 PIPE_FRAME_HIGH_SHIFT);
+	} while (high1 != high2);
+
+	count = (high1 << 8) | low;
+
+psb_get_vblank_counter_exit:
+
+	gma_power_end(dev);
+
+	return count;
+}
+
diff --git a/drivers/gpu/drm/gma500/psb_irq.h b/drivers/gpu/drm/gma500/psb_irq.h
new file mode 100644
index 000000000000..216fda38b57d
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_irq.h
@@ -0,0 +1,45 @@
+/**************************************************************************
+ * Copyright (c) 2009-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors:
+ *    Benjamin Defnet <benjamin.r.defnet@intel.com>
+ *    Rajesh Poornachandran <rajesh.poornachandran@intel.com>
+ *
+ **************************************************************************/
+
+#ifndef _SYSIRQ_H_
+#define _SYSIRQ_H_
+
+#include <drm/drmP.h>
+
+bool sysirq_init(struct drm_device *dev);
+void sysirq_uninit(struct drm_device *dev);
+
+void psb_irq_preinstall(struct drm_device *dev);
+int  psb_irq_postinstall(struct drm_device *dev);
+void psb_irq_uninstall(struct drm_device *dev);
+irqreturn_t psb_irq_handler(DRM_IRQ_ARGS);
+
+int psb_irq_enable_dpst(struct drm_device *dev);
+int psb_irq_disable_dpst(struct drm_device *dev);
+void psb_irq_turn_on_dpst(struct drm_device *dev);
+void psb_irq_turn_off_dpst(struct drm_device *dev);
+int  psb_enable_vblank(struct drm_device *dev, int pipe);
+void psb_disable_vblank(struct drm_device *dev, int pipe);
+u32  psb_get_vblank_counter(struct drm_device *dev, int pipe);
+
+#endif /* _SYSIRQ_H_ */
diff --git a/drivers/gpu/drm/gma500/psb_lid.c b/drivers/gpu/drm/gma500/psb_lid.c
new file mode 100644
index 000000000000..b867aabe6bf3
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_lid.c
@@ -0,0 +1,88 @@
+/**************************************************************************
+ * Copyright (c) 2007, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ * Authors: Thomas Hellstrom <thomas-at-tungstengraphics-dot-com>
+ **************************************************************************/
+
+#include <drm/drmP.h>
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include <linux/spinlock.h>
+
+static void psb_lid_timer_func(unsigned long data)
+{
+	struct drm_psb_private * dev_priv = (struct drm_psb_private *)data;
+	struct drm_device *dev = (struct drm_device *)dev_priv->dev;
+	struct timer_list *lid_timer = &dev_priv->lid_timer;
+	unsigned long irq_flags;
+	u32 *lid_state = dev_priv->lid_state;
+	u32 pp_status;
+
+	if (readl(lid_state) == dev_priv->lid_last_state)
+		goto lid_timer_schedule;
+
+	if ((readl(lid_state)) & 0x01) {
+		/*lid state is open*/
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) | POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while ((pp_status & PP_ON) == 0);
+
+		/*FIXME: should be backlight level before*/
+		psb_intel_lvds_set_brightness(dev, 100);
+	} else {
+		psb_intel_lvds_set_brightness(dev, 0);
+
+		REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) & ~POWER_TARGET_ON);
+		do {
+			pp_status = REG_READ(PP_STATUS);
+		} while ((pp_status & PP_ON) == 0);
+	}
+	dev_priv->lid_last_state =  readl(lid_state);
+
+lid_timer_schedule:
+	spin_lock_irqsave(&dev_priv->lid_lock, irq_flags);
+	if (!timer_pending(lid_timer)) {
+		lid_timer->expires = jiffies + PSB_LID_DELAY;
+		add_timer(lid_timer);
+	}
+	spin_unlock_irqrestore(&dev_priv->lid_lock, irq_flags);
+}
+
+void psb_lid_timer_init(struct drm_psb_private *dev_priv)
+{
+	struct timer_list *lid_timer = &dev_priv->lid_timer;
+	unsigned long irq_flags;
+
+	spin_lock_init(&dev_priv->lid_lock);
+	spin_lock_irqsave(&dev_priv->lid_lock, irq_flags);
+
+	init_timer(lid_timer);
+
+	lid_timer->data = (unsigned long)dev_priv;
+	lid_timer->function = psb_lid_timer_func;
+	lid_timer->expires = jiffies + PSB_LID_DELAY;
+
+	add_timer(lid_timer);
+	spin_unlock_irqrestore(&dev_priv->lid_lock, irq_flags);
+}
+
+void psb_lid_timer_takedown(struct drm_psb_private *dev_priv)
+{
+	del_timer_sync(&dev_priv->lid_timer);
+}
+
diff --git a/drivers/gpu/drm/gma500/psb_reg.h b/drivers/gpu/drm/gma500/psb_reg.h
new file mode 100644
index 000000000000..b81c7c1e9c2d
--- /dev/null
+++ b/drivers/gpu/drm/gma500/psb_reg.h
@@ -0,0 +1,582 @@
+/**************************************************************************
+ *
+ * Copyright (c) (2005-2007) Imagination Technologies Limited.
+ * Copyright (c) 2007, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA..
+ *
+ **************************************************************************/
+
+#ifndef _PSB_REG_H_
+#define _PSB_REG_H_
+
+#define PSB_CR_CLKGATECTL		0x0000
+#define _PSB_C_CLKGATECTL_AUTO_MAN_REG		(1 << 24)
+#define _PSB_C_CLKGATECTL_USE_CLKG_SHIFT	(20)
+#define _PSB_C_CLKGATECTL_USE_CLKG_MASK		(0x3 << 20)
+#define _PSB_C_CLKGATECTL_DPM_CLKG_SHIFT	(16)
+#define _PSB_C_CLKGATECTL_DPM_CLKG_MASK		(0x3 << 16)
+#define _PSB_C_CLKGATECTL_TA_CLKG_SHIFT		(12)
+#define _PSB_C_CLKGATECTL_TA_CLKG_MASK		(0x3 << 12)
+#define _PSB_C_CLKGATECTL_TSP_CLKG_SHIFT	(8)
+#define _PSB_C_CLKGATECTL_TSP_CLKG_MASK		(0x3 << 8)
+#define _PSB_C_CLKGATECTL_ISP_CLKG_SHIFT	(4)
+#define _PSB_C_CLKGATECTL_ISP_CLKG_MASK		(0x3 << 4)
+#define _PSB_C_CLKGATECTL_2D_CLKG_SHIFT		(0)
+#define _PSB_C_CLKGATECTL_2D_CLKG_MASK		(0x3 << 0)
+#define _PSB_C_CLKGATECTL_CLKG_ENABLED		(0)
+#define _PSB_C_CLKGATECTL_CLKG_DISABLED		(1)
+#define _PSB_C_CLKGATECTL_CLKG_AUTO		(2)
+
+#define PSB_CR_CORE_ID			0x0010
+#define _PSB_CC_ID_ID_SHIFT			(16)
+#define _PSB_CC_ID_ID_MASK			(0xFFFF << 16)
+#define _PSB_CC_ID_CONFIG_SHIFT			(0)
+#define _PSB_CC_ID_CONFIG_MASK			(0xFFFF << 0)
+
+#define PSB_CR_CORE_REVISION		0x0014
+#define _PSB_CC_REVISION_DESIGNER_SHIFT		(24)
+#define _PSB_CC_REVISION_DESIGNER_MASK		(0xFF << 24)
+#define _PSB_CC_REVISION_MAJOR_SHIFT		(16)
+#define _PSB_CC_REVISION_MAJOR_MASK		(0xFF << 16)
+#define _PSB_CC_REVISION_MINOR_SHIFT		(8)
+#define _PSB_CC_REVISION_MINOR_MASK		(0xFF << 8)
+#define _PSB_CC_REVISION_MAINTENANCE_SHIFT	(0)
+#define _PSB_CC_REVISION_MAINTENANCE_MASK	(0xFF << 0)
+
+#define PSB_CR_DESIGNER_REV_FIELD1	0x0018
+
+#define PSB_CR_SOFT_RESET		0x0080
+#define _PSB_CS_RESET_TSP_RESET		(1 << 6)
+#define _PSB_CS_RESET_ISP_RESET		(1 << 5)
+#define _PSB_CS_RESET_USE_RESET		(1 << 4)
+#define _PSB_CS_RESET_TA_RESET		(1 << 3)
+#define _PSB_CS_RESET_DPM_RESET		(1 << 2)
+#define _PSB_CS_RESET_TWOD_RESET	(1 << 1)
+#define _PSB_CS_RESET_BIF_RESET			(1 << 0)
+
+#define PSB_CR_DESIGNER_REV_FIELD2	0x001C
+
+#define PSB_CR_EVENT_HOST_ENABLE2	0x0110
+
+#define PSB_CR_EVENT_STATUS2		0x0118
+
+#define PSB_CR_EVENT_HOST_CLEAR2	0x0114
+#define _PSB_CE2_BIF_REQUESTER_FAULT		(1 << 4)
+
+#define PSB_CR_EVENT_STATUS		0x012C
+
+#define PSB_CR_EVENT_HOST_ENABLE	0x0130
+
+#define PSB_CR_EVENT_HOST_CLEAR		0x0134
+#define _PSB_CE_MASTER_INTERRUPT		(1 << 31)
+#define _PSB_CE_TA_DPM_FAULT			(1 << 28)
+#define _PSB_CE_TWOD_COMPLETE			(1 << 27)
+#define _PSB_CE_DPM_OUT_OF_MEMORY_ZLS		(1 << 25)
+#define _PSB_CE_DPM_TA_MEM_FREE			(1 << 24)
+#define _PSB_CE_PIXELBE_END_RENDER		(1 << 18)
+#define _PSB_CE_SW_EVENT			(1 << 14)
+#define _PSB_CE_TA_FINISHED			(1 << 13)
+#define _PSB_CE_TA_TERMINATE			(1 << 12)
+#define _PSB_CE_DPM_REACHED_MEM_THRESH		(1 << 3)
+#define _PSB_CE_DPM_OUT_OF_MEMORY_GBL		(1 << 2)
+#define _PSB_CE_DPM_OUT_OF_MEMORY_MT		(1 << 1)
+#define _PSB_CE_DPM_3D_MEM_FREE			(1 << 0)
+
+
+#define PSB_USE_OFFSET_MASK		0x0007FFFF
+#define PSB_USE_OFFSET_SIZE		(PSB_USE_OFFSET_MASK + 1)
+#define PSB_CR_USE_CODE_BASE0		0x0A0C
+#define PSB_CR_USE_CODE_BASE1		0x0A10
+#define PSB_CR_USE_CODE_BASE2		0x0A14
+#define PSB_CR_USE_CODE_BASE3		0x0A18
+#define PSB_CR_USE_CODE_BASE4		0x0A1C
+#define PSB_CR_USE_CODE_BASE5		0x0A20
+#define PSB_CR_USE_CODE_BASE6		0x0A24
+#define PSB_CR_USE_CODE_BASE7		0x0A28
+#define PSB_CR_USE_CODE_BASE8		0x0A2C
+#define PSB_CR_USE_CODE_BASE9		0x0A30
+#define PSB_CR_USE_CODE_BASE10		0x0A34
+#define PSB_CR_USE_CODE_BASE11		0x0A38
+#define PSB_CR_USE_CODE_BASE12		0x0A3C
+#define PSB_CR_USE_CODE_BASE13		0x0A40
+#define PSB_CR_USE_CODE_BASE14		0x0A44
+#define PSB_CR_USE_CODE_BASE15		0x0A48
+#define PSB_CR_USE_CODE_BASE(_i)	(0x0A0C + ((_i) << 2))
+#define _PSB_CUC_BASE_DM_SHIFT			(25)
+#define _PSB_CUC_BASE_DM_MASK			(0x3 << 25)
+#define _PSB_CUC_BASE_ADDR_SHIFT		(0)	/* 1024-bit aligned address? */
+#define _PSB_CUC_BASE_ADDR_ALIGNSHIFT		(7)
+#define _PSB_CUC_BASE_ADDR_MASK			(0x1FFFFFF << 0)
+#define _PSB_CUC_DM_VERTEX			(0)
+#define _PSB_CUC_DM_PIXEL			(1)
+#define _PSB_CUC_DM_RESERVED			(2)
+#define _PSB_CUC_DM_EDM				(3)
+
+#define PSB_CR_PDS_EXEC_BASE		0x0AB8
+#define _PSB_CR_PDS_EXEC_BASE_ADDR_SHIFT	(20)	/* 1MB aligned address */
+#define _PSB_CR_PDS_EXEC_BASE_ADDR_ALIGNSHIFT	(20)
+
+#define PSB_CR_EVENT_KICKER		0x0AC4
+#define _PSB_CE_KICKER_ADDRESS_SHIFT		(4)	/* 128-bit aligned address */
+
+#define PSB_CR_EVENT_KICK		0x0AC8
+#define _PSB_CE_KICK_NOW			(1 << 0)
+
+#define PSB_CR_BIF_DIR_LIST_BASE1	0x0C38
+
+#define PSB_CR_BIF_CTRL			0x0C00
+#define _PSB_CB_CTRL_CLEAR_FAULT		(1 << 4)
+#define _PSB_CB_CTRL_INVALDC			(1 << 3)
+#define _PSB_CB_CTRL_FLUSH			(1 << 2)
+
+#define PSB_CR_BIF_INT_STAT		0x0C04
+
+#define PSB_CR_BIF_FAULT		0x0C08
+#define _PSB_CBI_STAT_PF_N_RW			(1 << 14)
+#define _PSB_CBI_STAT_FAULT_SHIFT		(0)
+#define _PSB_CBI_STAT_FAULT_MASK		(0x3FFF << 0)
+#define _PSB_CBI_STAT_FAULT_CACHE		(1 << 1)
+#define _PSB_CBI_STAT_FAULT_TA			(1 << 2)
+#define _PSB_CBI_STAT_FAULT_VDM			(1 << 3)
+#define _PSB_CBI_STAT_FAULT_2D			(1 << 4)
+#define _PSB_CBI_STAT_FAULT_PBE			(1 << 5)
+#define _PSB_CBI_STAT_FAULT_TSP			(1 << 6)
+#define _PSB_CBI_STAT_FAULT_ISP			(1 << 7)
+#define _PSB_CBI_STAT_FAULT_USSEPDS		(1 << 8)
+#define _PSB_CBI_STAT_FAULT_HOST		(1 << 9)
+
+#define PSB_CR_BIF_BANK0		0x0C78
+#define PSB_CR_BIF_BANK1		0x0C7C
+#define PSB_CR_BIF_DIR_LIST_BASE0	0x0C84
+#define PSB_CR_BIF_TWOD_REQ_BASE	0x0C88
+#define PSB_CR_BIF_3D_REQ_BASE		0x0CAC
+
+#define PSB_CR_2D_SOCIF			0x0E18
+#define _PSB_C2_SOCIF_FREESPACE_SHIFT		(0)
+#define _PSB_C2_SOCIF_FREESPACE_MASK		(0xFF << 0)
+#define _PSB_C2_SOCIF_EMPTY			(0x80 << 0)
+
+#define PSB_CR_2D_BLIT_STATUS		0x0E04
+#define _PSB_C2B_STATUS_BUSY			(1 << 24)
+#define _PSB_C2B_STATUS_COMPLETE_SHIFT		(0)
+#define _PSB_C2B_STATUS_COMPLETE_MASK		(0xFFFFFF << 0)
+
+/*
+ * 2D defs.
+ */
+
+/*
+ * 2D Slave Port Data : Block Header's Object Type
+ */
+
+#define	PSB_2D_CLIP_BH			(0x00000000)
+#define	PSB_2D_PAT_BH			(0x10000000)
+#define	PSB_2D_CTRL_BH			(0x20000000)
+#define	PSB_2D_SRC_OFF_BH		(0x30000000)
+#define	PSB_2D_MASK_OFF_BH		(0x40000000)
+#define	PSB_2D_RESERVED1_BH		(0x50000000)
+#define	PSB_2D_RESERVED2_BH		(0x60000000)
+#define	PSB_2D_FENCE_BH			(0x70000000)
+#define	PSB_2D_BLIT_BH			(0x80000000)
+#define	PSB_2D_SRC_SURF_BH		(0x90000000)
+#define	PSB_2D_DST_SURF_BH		(0xA0000000)
+#define	PSB_2D_PAT_SURF_BH		(0xB0000000)
+#define	PSB_2D_SRC_PAL_BH		(0xC0000000)
+#define	PSB_2D_PAT_PAL_BH		(0xD0000000)
+#define	PSB_2D_MASK_SURF_BH		(0xE0000000)
+#define	PSB_2D_FLUSH_BH			(0xF0000000)
+
+/*
+ * Clip Definition block (PSB_2D_CLIP_BH)
+ */
+#define PSB_2D_CLIPCOUNT_MAX		(1)
+#define PSB_2D_CLIPCOUNT_MASK		(0x00000000)
+#define PSB_2D_CLIPCOUNT_CLRMASK	(0xFFFFFFFF)
+#define PSB_2D_CLIPCOUNT_SHIFT		(0)
+/* clip rectangle min & max */
+#define PSB_2D_CLIP_XMAX_MASK		(0x00FFF000)
+#define PSB_2D_CLIP_XMAX_CLRMASK	(0xFF000FFF)
+#define PSB_2D_CLIP_XMAX_SHIFT		(12)
+#define PSB_2D_CLIP_XMIN_MASK		(0x00000FFF)
+#define PSB_2D_CLIP_XMIN_CLRMASK	(0x00FFF000)
+#define PSB_2D_CLIP_XMIN_SHIFT		(0)
+/* clip rectangle offset */
+#define PSB_2D_CLIP_YMAX_MASK		(0x00FFF000)
+#define PSB_2D_CLIP_YMAX_CLRMASK	(0xFF000FFF)
+#define PSB_2D_CLIP_YMAX_SHIFT		(12)
+#define PSB_2D_CLIP_YMIN_MASK		(0x00000FFF)
+#define PSB_2D_CLIP_YMIN_CLRMASK	(0x00FFF000)
+#define PSB_2D_CLIP_YMIN_SHIFT		(0)
+
+/*
+ * Pattern Control (PSB_2D_PAT_BH)
+ */
+#define PSB_2D_PAT_HEIGHT_MASK		(0x0000001F)
+#define PSB_2D_PAT_HEIGHT_SHIFT		(0)
+#define PSB_2D_PAT_WIDTH_MASK		(0x000003E0)
+#define PSB_2D_PAT_WIDTH_SHIFT		(5)
+#define PSB_2D_PAT_YSTART_MASK		(0x00007C00)
+#define PSB_2D_PAT_YSTART_SHIFT		(10)
+#define PSB_2D_PAT_XSTART_MASK		(0x000F8000)
+#define PSB_2D_PAT_XSTART_SHIFT		(15)
+
+/*
+ * 2D Control block (PSB_2D_CTRL_BH)
+ */
+/* Present Flags */
+#define PSB_2D_SRCCK_CTRL		(0x00000001)
+#define PSB_2D_DSTCK_CTRL		(0x00000002)
+#define PSB_2D_ALPHA_CTRL		(0x00000004)
+/* Colour Key Colour (SRC/DST)*/
+#define PSB_2D_CK_COL_MASK		(0xFFFFFFFF)
+#define PSB_2D_CK_COL_CLRMASK		(0x00000000)
+#define PSB_2D_CK_COL_SHIFT		(0)
+/* Colour Key Mask (SRC/DST)*/
+#define PSB_2D_CK_MASK_MASK		(0xFFFFFFFF)
+#define PSB_2D_CK_MASK_CLRMASK		(0x00000000)
+#define PSB_2D_CK_MASK_SHIFT		(0)
+/* Alpha Control (Alpha/RGB)*/
+#define PSB_2D_GBLALPHA_MASK		(0x000FF000)
+#define PSB_2D_GBLALPHA_CLRMASK		(0xFFF00FFF)
+#define PSB_2D_GBLALPHA_SHIFT		(12)
+#define PSB_2D_SRCALPHA_OP_MASK		(0x00700000)
+#define PSB_2D_SRCALPHA_OP_CLRMASK	(0xFF8FFFFF)
+#define PSB_2D_SRCALPHA_OP_SHIFT	(20)
+#define PSB_2D_SRCALPHA_OP_ONE		(0x00000000)
+#define PSB_2D_SRCALPHA_OP_SRC		(0x00100000)
+#define PSB_2D_SRCALPHA_OP_DST		(0x00200000)
+#define PSB_2D_SRCALPHA_OP_SG		(0x00300000)
+#define PSB_2D_SRCALPHA_OP_DG		(0x00400000)
+#define PSB_2D_SRCALPHA_OP_GBL		(0x00500000)
+#define PSB_2D_SRCALPHA_OP_ZERO		(0x00600000)
+#define PSB_2D_SRCALPHA_INVERT		(0x00800000)
+#define PSB_2D_SRCALPHA_INVERT_CLR	(0xFF7FFFFF)
+#define PSB_2D_DSTALPHA_OP_MASK		(0x07000000)
+#define PSB_2D_DSTALPHA_OP_CLRMASK	(0xF8FFFFFF)
+#define PSB_2D_DSTALPHA_OP_SHIFT	(24)
+#define PSB_2D_DSTALPHA_OP_ONE		(0x00000000)
+#define PSB_2D_DSTALPHA_OP_SRC		(0x01000000)
+#define PSB_2D_DSTALPHA_OP_DST		(0x02000000)
+#define PSB_2D_DSTALPHA_OP_SG		(0x03000000)
+#define PSB_2D_DSTALPHA_OP_DG		(0x04000000)
+#define PSB_2D_DSTALPHA_OP_GBL		(0x05000000)
+#define PSB_2D_DSTALPHA_OP_ZERO		(0x06000000)
+#define PSB_2D_DSTALPHA_INVERT		(0x08000000)
+#define PSB_2D_DSTALPHA_INVERT_CLR	(0xF7FFFFFF)
+
+#define PSB_2D_PRE_MULTIPLICATION_ENABLE	(0x10000000)
+#define PSB_2D_PRE_MULTIPLICATION_CLRMASK	(0xEFFFFFFF)
+#define PSB_2D_ZERO_SOURCE_ALPHA_ENABLE		(0x20000000)
+#define PSB_2D_ZERO_SOURCE_ALPHA_CLRMASK	(0xDFFFFFFF)
+
+/*
+ *Source Offset (PSB_2D_SRC_OFF_BH)
+ */
+#define PSB_2D_SRCOFF_XSTART_MASK	((0x00000FFF) << 12)
+#define PSB_2D_SRCOFF_XSTART_SHIFT	(12)
+#define PSB_2D_SRCOFF_YSTART_MASK	(0x00000FFF)
+#define PSB_2D_SRCOFF_YSTART_SHIFT	(0)
+
+/*
+ * Mask Offset (PSB_2D_MASK_OFF_BH)
+ */
+#define PSB_2D_MASKOFF_XSTART_MASK	((0x00000FFF) << 12)
+#define PSB_2D_MASKOFF_XSTART_SHIFT	(12)
+#define PSB_2D_MASKOFF_YSTART_MASK	(0x00000FFF)
+#define PSB_2D_MASKOFF_YSTART_SHIFT	(0)
+
+/*
+ * 2D Fence (see PSB_2D_FENCE_BH): bits 0:27 are ignored
+ */
+
+/*
+ *Blit Rectangle (PSB_2D_BLIT_BH)
+ */
+
+#define PSB_2D_ROT_MASK			(3 << 25)
+#define PSB_2D_ROT_CLRMASK		(~PSB_2D_ROT_MASK)
+#define PSB_2D_ROT_NONE			(0 << 25)
+#define PSB_2D_ROT_90DEGS		(1 << 25)
+#define PSB_2D_ROT_180DEGS		(2 << 25)
+#define PSB_2D_ROT_270DEGS		(3 << 25)
+
+#define PSB_2D_COPYORDER_MASK		(3 << 23)
+#define PSB_2D_COPYORDER_CLRMASK	(~PSB_2D_COPYORDER_MASK)
+#define PSB_2D_COPYORDER_TL2BR		(0 << 23)
+#define PSB_2D_COPYORDER_BR2TL		(1 << 23)
+#define PSB_2D_COPYORDER_TR2BL		(2 << 23)
+#define PSB_2D_COPYORDER_BL2TR		(3 << 23)
+
+#define PSB_2D_DSTCK_CLRMASK		(0xFF9FFFFF)
+#define PSB_2D_DSTCK_DISABLE		(0x00000000)
+#define PSB_2D_DSTCK_PASS		(0x00200000)
+#define PSB_2D_DSTCK_REJECT		(0x00400000)
+
+#define PSB_2D_SRCCK_CLRMASK		(0xFFE7FFFF)
+#define PSB_2D_SRCCK_DISABLE		(0x00000000)
+#define PSB_2D_SRCCK_PASS		(0x00080000)
+#define PSB_2D_SRCCK_REJECT		(0x00100000)
+
+#define PSB_2D_CLIP_ENABLE		(0x00040000)
+
+#define PSB_2D_ALPHA_ENABLE		(0x00020000)
+
+#define PSB_2D_PAT_CLRMASK		(0xFFFEFFFF)
+#define PSB_2D_PAT_MASK			(0x00010000)
+#define PSB_2D_USE_PAT			(0x00010000)
+#define PSB_2D_USE_FILL			(0x00000000)
+/*
+ * Tungsten Graphics note on rop codes: If rop A and rop B are
+ * identical, the mask surface will not be read and need not be
+ * set up.
+ */
+
+#define PSB_2D_ROP3B_MASK		(0x0000FF00)
+#define PSB_2D_ROP3B_CLRMASK		(0xFFFF00FF)
+#define PSB_2D_ROP3B_SHIFT		(8)
+/* rop code A */
+#define PSB_2D_ROP3A_MASK		(0x000000FF)
+#define PSB_2D_ROP3A_CLRMASK		(0xFFFFFF00)
+#define PSB_2D_ROP3A_SHIFT		(0)
+
+#define PSB_2D_ROP4_MASK		(0x0000FFFF)
+/*
+ *	DWORD0:	(Only pass if Pattern control == Use Fill Colour)
+ *	Fill Colour RGBA8888
+ */
+#define PSB_2D_FILLCOLOUR_MASK		(0xFFFFFFFF)
+#define PSB_2D_FILLCOLOUR_SHIFT		(0)
+/*
+ *	DWORD1: (Always Present)
+ *	X Start (Dest)
+ *	Y Start (Dest)
+ */
+#define PSB_2D_DST_XSTART_MASK		(0x00FFF000)
+#define PSB_2D_DST_XSTART_CLRMASK	(0xFF000FFF)
+#define PSB_2D_DST_XSTART_SHIFT		(12)
+#define PSB_2D_DST_YSTART_MASK		(0x00000FFF)
+#define PSB_2D_DST_YSTART_CLRMASK	(0xFFFFF000)
+#define PSB_2D_DST_YSTART_SHIFT		(0)
+/*
+ *	DWORD2: (Always Present)
+ *	X Size (Dest)
+ *	Y Size (Dest)
+ */
+#define PSB_2D_DST_XSIZE_MASK		(0x00FFF000)
+#define PSB_2D_DST_XSIZE_CLRMASK	(0xFF000FFF)
+#define PSB_2D_DST_XSIZE_SHIFT		(12)
+#define PSB_2D_DST_YSIZE_MASK		(0x00000FFF)
+#define PSB_2D_DST_YSIZE_CLRMASK	(0xFFFFF000)
+#define PSB_2D_DST_YSIZE_SHIFT		(0)
+
+/*
+ * Source Surface (PSB_2D_SRC_SURF_BH)
+ */
+/*
+ * WORD 0
+ */
+
+#define PSB_2D_SRC_FORMAT_MASK		(0x00078000)
+#define PSB_2D_SRC_1_PAL		(0x00000000)
+#define PSB_2D_SRC_2_PAL		(0x00008000)
+#define PSB_2D_SRC_4_PAL		(0x00010000)
+#define PSB_2D_SRC_8_PAL		(0x00018000)
+#define PSB_2D_SRC_8_ALPHA		(0x00020000)
+#define PSB_2D_SRC_4_ALPHA		(0x00028000)
+#define PSB_2D_SRC_332RGB		(0x00030000)
+#define PSB_2D_SRC_4444ARGB		(0x00038000)
+#define PSB_2D_SRC_555RGB		(0x00040000)
+#define PSB_2D_SRC_1555ARGB		(0x00048000)
+#define PSB_2D_SRC_565RGB		(0x00050000)
+#define PSB_2D_SRC_0888ARGB		(0x00058000)
+#define PSB_2D_SRC_8888ARGB		(0x00060000)
+#define PSB_2D_SRC_8888UYVY		(0x00068000)
+#define PSB_2D_SRC_RESERVED		(0x00070000)
+#define PSB_2D_SRC_1555ARGB_LOOKUP	(0x00078000)
+
+
+#define PSB_2D_SRC_STRIDE_MASK		(0x00007FFF)
+#define PSB_2D_SRC_STRIDE_CLRMASK	(0xFFFF8000)
+#define PSB_2D_SRC_STRIDE_SHIFT		(0)
+/*
+ *  WORD 1 - Base Address
+ */
+#define PSB_2D_SRC_ADDR_MASK		(0x0FFFFFFC)
+#define PSB_2D_SRC_ADDR_CLRMASK		(0x00000003)
+#define PSB_2D_SRC_ADDR_SHIFT		(2)
+#define PSB_2D_SRC_ADDR_ALIGNSHIFT	(2)
+
+/*
+ * Pattern Surface (PSB_2D_PAT_SURF_BH)
+ */
+/*
+ *  WORD 0
+ */
+
+#define PSB_2D_PAT_FORMAT_MASK		(0x00078000)
+#define PSB_2D_PAT_1_PAL		(0x00000000)
+#define PSB_2D_PAT_2_PAL		(0x00008000)
+#define PSB_2D_PAT_4_PAL		(0x00010000)
+#define PSB_2D_PAT_8_PAL		(0x00018000)
+#define PSB_2D_PAT_8_ALPHA		(0x00020000)
+#define PSB_2D_PAT_4_ALPHA		(0x00028000)
+#define PSB_2D_PAT_332RGB		(0x00030000)
+#define PSB_2D_PAT_4444ARGB		(0x00038000)
+#define PSB_2D_PAT_555RGB		(0x00040000)
+#define PSB_2D_PAT_1555ARGB		(0x00048000)
+#define PSB_2D_PAT_565RGB		(0x00050000)
+#define PSB_2D_PAT_0888ARGB		(0x00058000)
+#define PSB_2D_PAT_8888ARGB		(0x00060000)
+
+#define PSB_2D_PAT_STRIDE_MASK		(0x00007FFF)
+#define PSB_2D_PAT_STRIDE_CLRMASK	(0xFFFF8000)
+#define PSB_2D_PAT_STRIDE_SHIFT		(0)
+/*
+ *  WORD 1 - Base Address
+ */
+#define PSB_2D_PAT_ADDR_MASK		(0x0FFFFFFC)
+#define PSB_2D_PAT_ADDR_CLRMASK		(0x00000003)
+#define PSB_2D_PAT_ADDR_SHIFT		(2)
+#define PSB_2D_PAT_ADDR_ALIGNSHIFT	(2)
+
+/*
+ * Destination Surface (PSB_2D_DST_SURF_BH)
+ */
+/*
+ * WORD 0
+ */
+
+#define PSB_2D_DST_FORMAT_MASK		(0x00078000)
+#define PSB_2D_DST_332RGB		(0x00030000)
+#define PSB_2D_DST_4444ARGB		(0x00038000)
+#define PSB_2D_DST_555RGB		(0x00040000)
+#define PSB_2D_DST_1555ARGB		(0x00048000)
+#define PSB_2D_DST_565RGB		(0x00050000)
+#define PSB_2D_DST_0888ARGB		(0x00058000)
+#define PSB_2D_DST_8888ARGB		(0x00060000)
+#define PSB_2D_DST_8888AYUV		(0x00070000)
+
+#define PSB_2D_DST_STRIDE_MASK		(0x00007FFF)
+#define PSB_2D_DST_STRIDE_CLRMASK	(0xFFFF8000)
+#define PSB_2D_DST_STRIDE_SHIFT		(0)
+/*
+ * WORD 1 - Base Address
+ */
+#define PSB_2D_DST_ADDR_MASK		(0x0FFFFFFC)
+#define PSB_2D_DST_ADDR_CLRMASK		(0x00000003)
+#define PSB_2D_DST_ADDR_SHIFT		(2)
+#define PSB_2D_DST_ADDR_ALIGNSHIFT	(2)
+
+/*
+ * Mask Surface (PSB_2D_MASK_SURF_BH)
+ */
+/*
+ * WORD 0
+ */
+#define PSB_2D_MASK_STRIDE_MASK		(0x00007FFF)
+#define PSB_2D_MASK_STRIDE_CLRMASK	(0xFFFF8000)
+#define PSB_2D_MASK_STRIDE_SHIFT	(0)
+/*
+ *  WORD 1 - Base Address
+ */
+#define PSB_2D_MASK_ADDR_MASK		(0x0FFFFFFC)
+#define PSB_2D_MASK_ADDR_CLRMASK	(0x00000003)
+#define PSB_2D_MASK_ADDR_SHIFT		(2)
+#define PSB_2D_MASK_ADDR_ALIGNSHIFT	(2)
+
+/*
+ * Source Palette (PSB_2D_SRC_PAL_BH)
+ */
+
+#define PSB_2D_SRCPAL_ADDR_SHIFT	(0)
+#define PSB_2D_SRCPAL_ADDR_CLRMASK	(0xF0000007)
+#define PSB_2D_SRCPAL_ADDR_MASK		(0x0FFFFFF8)
+#define PSB_2D_SRCPAL_BYTEALIGN		(1024)
+
+/*
+ * Pattern Palette (PSB_2D_PAT_PAL_BH)
+ */
+
+#define PSB_2D_PATPAL_ADDR_SHIFT	(0)
+#define PSB_2D_PATPAL_ADDR_CLRMASK	(0xF0000007)
+#define PSB_2D_PATPAL_ADDR_MASK		(0x0FFFFFF8)
+#define PSB_2D_PATPAL_BYTEALIGN		(1024)
+
+/*
+ * Rop3 Codes (2 LS bytes)
+ */
+
+#define PSB_2D_ROP3_SRCCOPY		(0xCCCC)
+#define PSB_2D_ROP3_PATCOPY		(0xF0F0)
+#define PSB_2D_ROP3_WHITENESS		(0xFFFF)
+#define PSB_2D_ROP3_BLACKNESS		(0x0000)
+#define PSB_2D_ROP3_SRC			(0xCC)
+#define PSB_2D_ROP3_PAT			(0xF0)
+#define PSB_2D_ROP3_DST			(0xAA)
+
+/*
+ * Sizes.
+ */
+
+#define PSB_SCENE_HW_COOKIE_SIZE	16
+#define PSB_TA_MEM_HW_COOKIE_SIZE	16
+
+/*
+ * Scene stuff.
+ */
+
+#define PSB_NUM_HW_SCENES		2
+
+/*
+ * Scheduler completion actions.
+ */
+
+#define PSB_RASTER_BLOCK		0
+#define PSB_RASTER			1
+#define PSB_RETURN			2
+#define PSB_TA				3
+
+/* Power management */
+#define PSB_PUNIT_PORT			0x04
+#define PSB_OSPMBA			0x78
+#define PSB_APMBA			0x7a
+#define PSB_APM_CMD			0x0
+#define PSB_APM_STS			0x04
+#define PSB_PWRGT_VID_ENC_MASK		0x30
+#define PSB_PWRGT_VID_DEC_MASK		0xc
+#define PSB_PWRGT_GL3_MASK		0xc0
+
+#define PSB_PM_SSC			0x20
+#define PSB_PM_SSS			0x30
+#define PSB_PWRGT_DISPLAY_MASK		0xc /*on a different BA than video/gfx*/
+#define MDFLD_PWRGT_DISPLAY_A_CNTR	0x0000000c
+#define MDFLD_PWRGT_DISPLAY_B_CNTR	0x0000c000
+#define MDFLD_PWRGT_DISPLAY_C_CNTR	0x00030000
+#define MDFLD_PWRGT_DISP_MIPI_CNTR	0x000c0000
+#define MDFLD_PWRGT_DISPLAY_CNTR    (MDFLD_PWRGT_DISPLAY_A_CNTR | MDFLD_PWRGT_DISPLAY_B_CNTR | MDFLD_PWRGT_DISPLAY_C_CNTR | MDFLD_PWRGT_DISP_MIPI_CNTR) /* 0x000fc00c */
+/* Display SSS register bits are different in A0 vs. B0 */
+#define PSB_PWRGT_GFX_MASK		0x3
+#define MDFLD_PWRGT_DISPLAY_A_STS	0x000000c0
+#define MDFLD_PWRGT_DISPLAY_B_STS	0x00000300
+#define MDFLD_PWRGT_DISPLAY_C_STS	0x00000c00
+#define PSB_PWRGT_GFX_MASK_B0		0xc3
+#define MDFLD_PWRGT_DISPLAY_A_STS_B0	0x0000000c
+#define MDFLD_PWRGT_DISPLAY_B_STS_B0	0x0000c000
+#define MDFLD_PWRGT_DISPLAY_C_STS_B0	0x00030000
+#define MDFLD_PWRGT_DISP_MIPI_STS	0x000c0000
+#define MDFLD_PWRGT_DISPLAY_STS_A0    (MDFLD_PWRGT_DISPLAY_A_STS | MDFLD_PWRGT_DISPLAY_B_STS | MDFLD_PWRGT_DISPLAY_C_STS | MDFLD_PWRGT_DISP_MIPI_STS) /* 0x000fc00c */
+#define MDFLD_PWRGT_DISPLAY_STS_B0    (MDFLD_PWRGT_DISPLAY_A_STS_B0 | MDFLD_PWRGT_DISPLAY_B_STS_B0 | MDFLD_PWRGT_DISPLAY_C_STS_B0 | MDFLD_PWRGT_DISP_MIPI_STS) /* 0x000fc00c */
+#endif
diff --git a/drivers/gpu/drm/i810/i810_dma.c b/drivers/gpu/drm/i810/i810_dma.c
index 8f371e8d630f..f7c17b239833 100644
--- a/drivers/gpu/drm/i810/i810_dma.c
+++ b/drivers/gpu/drm/i810/i810_dma.c
@@ -222,8 +222,6 @@ static int i810_dma_cleanup(struct drm_device *dev)
 			pci_free_consistent(dev->pdev, PAGE_SIZE,
 					    dev_priv->hw_status_page,
 					    dev_priv->dma_status_page);
-			/* Need to rewrite hardware status page */
-			I810_WRITE(0x02080, 0x1ffff000);
 		}
 		kfree(dev->dev_private);
 		dev->dev_private = NULL;
@@ -888,7 +886,7 @@ static int i810_flush_queue(struct drm_device *dev)
 }
 
 /* Must be called with the lock held */
-static void i810_reclaim_buffers(struct drm_device *dev,
+void i810_driver_reclaim_buffers(struct drm_device *dev,
 				 struct drm_file *file_priv)
 {
 	struct drm_device_dma *dma = dev->dma;
@@ -1225,12 +1223,17 @@ void i810_driver_preclose(struct drm_device *dev, struct drm_file *file_priv)
 		if (dev_priv->page_flipping)
 			i810_do_cleanup_pageflip(dev);
 	}
-}
 
-void i810_driver_reclaim_buffers_locked(struct drm_device *dev,
-					struct drm_file *file_priv)
-{
-	i810_reclaim_buffers(dev, file_priv);
+	if (file_priv->master && file_priv->master->lock.hw_lock) {
+		drm_idlelock_take(&file_priv->master->lock);
+		i810_driver_reclaim_buffers(dev, file_priv);
+		drm_idlelock_release(&file_priv->master->lock);
+	} else {
+		/* master disappeared, clean up stuff anyway and hope nothing
+		 * goes wrong */
+		i810_driver_reclaim_buffers(dev, file_priv);
+	}
+
 }
 
 int i810_driver_dma_quiescent(struct drm_device *dev)
diff --git a/drivers/gpu/drm/i810/i810_drv.c b/drivers/gpu/drm/i810/i810_drv.c
index d4266bdf6fb4..053f1ee58393 100644
--- a/drivers/gpu/drm/i810/i810_drv.c
+++ b/drivers/gpu/drm/i810/i810_drv.c
@@ -43,6 +43,17 @@ static struct pci_device_id pciidlist[] = {
 	i810_PCI_IDS
 };
 
+static const struct file_operations i810_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_REQUIRE_AGP | DRIVER_USE_MTRR |
@@ -52,20 +63,9 @@ static struct drm_driver driver = {
 	.lastclose = i810_driver_lastclose,
 	.preclose = i810_driver_preclose,
 	.device_is_agp = i810_driver_device_is_agp,
-	.reclaim_buffers_locked = i810_driver_reclaim_buffers_locked,
 	.dma_quiescent = i810_driver_dma_quiescent,
 	.ioctls = i810_ioctls,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = drm_ioctl,
-		 .mmap = drm_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .llseek = noop_llseek,
-	},
-
+	.fops = &i810_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/i810/i810_drv.h b/drivers/gpu/drm/i810/i810_drv.h
index c9339f481795..6e0acad9e0f5 100644
--- a/drivers/gpu/drm/i810/i810_drv.h
+++ b/drivers/gpu/drm/i810/i810_drv.h
@@ -116,14 +116,12 @@ typedef struct drm_i810_private {
 
 				/* i810_dma.c */
 extern int i810_driver_dma_quiescent(struct drm_device *dev);
-extern void i810_driver_reclaim_buffers_locked(struct drm_device *dev,
-					       struct drm_file *file_priv);
+void i810_driver_reclaim_buffers(struct drm_device *dev,
+			         struct drm_file *file_priv);
 extern int i810_driver_load(struct drm_device *, unsigned long flags);
 extern void i810_driver_lastclose(struct drm_device *dev);
 extern void i810_driver_preclose(struct drm_device *dev,
 				 struct drm_file *file_priv);
-extern void i810_driver_reclaim_buffers_locked(struct drm_device *dev,
-					       struct drm_file *file_priv);
 extern int i810_driver_device_is_agp(struct drm_device *dev);
 
 extern long i810_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index 0ae6a7c5020f..808b255d7fc6 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -28,6 +28,7 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o i915_mem.o \
 	  intel_dvo.o \
 	  intel_ringbuffer.o \
 	  intel_overlay.o \
+	  intel_sprite.o \
 	  intel_opregion.o \
 	  dvo_ch7xxx.o \
 	  dvo_ch7017.o \
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 004b048c5192..11807989f918 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -1001,7 +1001,7 @@ static int i915_inttoext_table(struct seq_file *m, void *unused)
 	return 0;
 }
 
-static int i915_drpc_info(struct seq_file *m, void *unused)
+static int ironlake_drpc_info(struct seq_file *m)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
 	struct drm_device *dev = node->minor->dev;
@@ -1068,6 +1068,90 @@ static int i915_drpc_info(struct seq_file *m, void *unused)
 	return 0;
 }
 
+static int gen6_drpc_info(struct seq_file *m)
+{
+
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 rpmodectl1, gt_core_status, rcctl1;
+	int count=0, ret;
+
+
+	ret = mutex_lock_interruptible(&dev->struct_mutex);
+	if (ret)
+		return ret;
+
+	if (atomic_read(&dev_priv->forcewake_count)) {
+		seq_printf(m, "RC information inaccurate because userspace "
+			      "holds a reference \n");
+	} else {
+		/* NB: we cannot use forcewake, else we read the wrong values */
+		while (count++ < 50 && (I915_READ_NOTRACE(FORCEWAKE_ACK) & 1))
+			udelay(10);
+		seq_printf(m, "RC information accurate: %s\n", yesno(count < 51));
+	}
+
+	gt_core_status = readl(dev_priv->regs + GEN6_GT_CORE_STATUS);
+	trace_i915_reg_rw(false, GEN6_GT_CORE_STATUS, gt_core_status, 4);
+
+	rpmodectl1 = I915_READ(GEN6_RP_CONTROL);
+	rcctl1 = I915_READ(GEN6_RC_CONTROL);
+	mutex_unlock(&dev->struct_mutex);
+
+	seq_printf(m, "Video Turbo Mode: %s\n",
+		   yesno(rpmodectl1 & GEN6_RP_MEDIA_TURBO));
+	seq_printf(m, "HW control enabled: %s\n",
+		   yesno(rpmodectl1 & GEN6_RP_ENABLE));
+	seq_printf(m, "SW control enabled: %s\n",
+		   yesno((rpmodectl1 & GEN6_RP_MEDIA_MODE_MASK) ==
+			  GEN6_RP_MEDIA_SW_MODE));
+	seq_printf(m, "RC6 Enabled: %s\n",
+		   yesno(rcctl1 & GEN6_RC_CTL_RC1e_ENABLE));
+	seq_printf(m, "RC6 Enabled: %s\n",
+		   yesno(rcctl1 & GEN6_RC_CTL_RC6_ENABLE));
+	seq_printf(m, "Deep RC6 Enabled: %s\n",
+		   yesno(rcctl1 & GEN6_RC_CTL_RC6p_ENABLE));
+	seq_printf(m, "Deepest RC6 Enabled: %s\n",
+		   yesno(rcctl1 & GEN6_RC_CTL_RC6pp_ENABLE));
+	seq_printf(m, "Current RC state: ");
+	switch (gt_core_status & GEN6_RCn_MASK) {
+	case GEN6_RC0:
+		if (gt_core_status & GEN6_CORE_CPD_STATE_MASK)
+			seq_printf(m, "Core Power Down\n");
+		else
+			seq_printf(m, "on\n");
+		break;
+	case GEN6_RC3:
+		seq_printf(m, "RC3\n");
+		break;
+	case GEN6_RC6:
+		seq_printf(m, "RC6\n");
+		break;
+	case GEN6_RC7:
+		seq_printf(m, "RC7\n");
+		break;
+	default:
+		seq_printf(m, "Unknown\n");
+		break;
+	}
+
+	seq_printf(m, "Core Power Down: %s\n",
+		   yesno(gt_core_status & GEN6_CORE_CPD_STATE_MASK));
+	return 0;
+}
+
+static int i915_drpc_info(struct seq_file *m, void *unused)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct drm_device *dev = node->minor->dev;
+
+	if (IS_GEN6(dev) || IS_GEN7(dev))
+		return gen6_drpc_info(m);
+	else
+		return ironlake_drpc_info(m);
+}
+
 static int i915_fbc_status(struct seq_file *m, void *unused)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index a9ae374861e7..5f4d5893e983 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -781,6 +781,9 @@ static int i915_getparam(struct drm_device *dev, void *data,
 	case I915_PARAM_HAS_RELAXED_DELTA:
 		value = 1;
 		break;
+	case I915_PARAM_HAS_GEN7_SOL_RESET:
+		value = 1;
+		break;
 	default:
 		DRM_DEBUG_DRIVER("Unknown parameter %d\n",
 				 param->param);
@@ -2305,6 +2308,8 @@ struct drm_ioctl_desc i915_ioctls[] = {
 	DRM_IOCTL_DEF_DRV(I915_GEM_MADVISE, i915_gem_madvise_ioctl, DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(I915_OVERLAY_PUT_IMAGE, intel_overlay_put_image, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(I915_OVERLAY_ATTRS, intel_overlay_attrs, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(I915_SET_SPRITE_COLORKEY, intel_sprite_set_colorkey, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(I915_GET_SPRITE_COLORKEY, intel_sprite_get_colorkey, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
 };
 
 int i915_max_ioctl = DRM_ARRAY_SIZE(i915_ioctls);
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index a1103fc6597d..8f7187915b0d 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -810,6 +810,21 @@ static struct vm_operations_struct i915_gem_vm_ops = {
 	.close = drm_gem_vm_close,
 };
 
+static const struct file_operations i915_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_gem_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = i915_compat_ioctl,
+#endif
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	/* Don't use MTRRs here; the Xserver or userspace app should
 	 * deal with them for Intel hardware.
@@ -843,21 +858,7 @@ static struct drm_driver driver = {
 	.dumb_map_offset = i915_gem_mmap_gtt,
 	.dumb_destroy = i915_gem_dumb_destroy,
 	.ioctls = i915_ioctls,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = drm_ioctl,
-		 .mmap = drm_gem_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .read = drm_read,
-#ifdef CONFIG_COMPAT
-		 .compat_ioctl = i915_compat_ioctl,
-#endif
-		 .llseek = noop_llseek,
-	},
-
+	.fops = &i915_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
@@ -922,13 +923,6 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL and additional rights");
 
-/* We give fast paths for the really cool registers */
-#define NEEDS_FORCE_WAKE(dev_priv, reg) \
-	(((dev_priv)->info->gen >= 6) && \
-	 ((reg) < 0x40000) &&		 \
-	 ((reg) != FORCEWAKE) &&	 \
-	 ((reg) != ECOBUS))
-
 #define __i915_read(x, y) \
 u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg) { \
 	u##x val = 0; \
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 554bef7a3b9c..602bc80baabb 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -207,6 +207,8 @@ struct drm_i915_display_funcs {
 	int (*get_display_clock_speed)(struct drm_device *dev);
 	int (*get_fifo_size)(struct drm_device *dev, int plane);
 	void (*update_wm)(struct drm_device *dev);
+	void (*update_sprite_wm)(struct drm_device *dev, int pipe,
+				 uint32_t sprite_width, int pixel_size);
 	int (*crtc_mode_set)(struct drm_crtc *crtc,
 			     struct drm_display_mode *mode,
 			     struct drm_display_mode *adjusted_mode,
@@ -337,6 +339,8 @@ typedef struct drm_i915_private {
 	struct timer_list hangcheck_timer;
 	int hangcheck_count;
 	uint32_t last_acthd;
+	uint32_t last_acthd_bsd;
+	uint32_t last_acthd_blt;
 	uint32_t last_instdone;
 	uint32_t last_instdone1;
 
@@ -350,6 +354,7 @@ typedef struct drm_i915_private {
 
 	/* overlay */
 	struct intel_overlay *overlay;
+	bool sprite_scaling_enabled;
 
 	/* LVDS info */
 	int backlight_level;  /* restore backlight to this value */
@@ -1362,8 +1367,7 @@ void __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv);
 #define NEEDS_FORCE_WAKE(dev_priv, reg) \
 	(((dev_priv)->info->gen >= 6) && \
 	 ((reg) < 0x40000) &&		 \
-	 ((reg) != FORCEWAKE) &&	 \
-	 ((reg) != ECOBUS))
+	 ((reg) != FORCEWAKE))
 
 #define __i915_read(x, y) \
 	u##x i915_read##x(struct drm_i915_private *dev_priv, u32 reg);
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 8359dc777041..e55badb2d86d 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -2006,9 +2006,9 @@ i915_wait_request(struct intel_ring_buffer *ring,
 					   || atomic_read(&dev_priv->mm.wedged));
 
 			ring->irq_put(ring);
-		} else if (wait_for(i915_seqno_passed(ring->get_seqno(ring),
-						      seqno) ||
-				    atomic_read(&dev_priv->mm.wedged), 3000))
+		} else if (wait_for_atomic(i915_seqno_passed(ring->get_seqno(ring),
+							     seqno) ||
+					   atomic_read(&dev_priv->mm.wedged), 3000))
 			ret = -EBUSY;
 		ring->waiting_seqno = 0;
 
@@ -3309,6 +3309,10 @@ i915_gem_ring_throttle(struct drm_device *dev, struct drm_file *file)
 
 			if (ret == 0 && atomic_read(&dev_priv->mm.wedged))
 				ret = -EIO;
+		} else if (wait_for_atomic(i915_seqno_passed(ring->get_seqno(ring),
+							     seqno) ||
+				    atomic_read(&dev_priv->mm.wedged), 3000)) {
+			ret = -EBUSY;
 		}
 	}
 
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index b9da8900ae4e..65e1f0043f9d 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -971,6 +971,31 @@ i915_gem_execbuffer_retire_commands(struct drm_device *dev,
 }
 
 static int
+i915_reset_gen7_sol_offsets(struct drm_device *dev,
+			    struct intel_ring_buffer *ring)
+{
+	drm_i915_private_t *dev_priv = dev->dev_private;
+	int ret, i;
+
+	if (!IS_GEN7(dev) || ring != &dev_priv->ring[RCS])
+		return 0;
+
+	ret = intel_ring_begin(ring, 4 * 3);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < 4; i++) {
+		intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
+		intel_ring_emit(ring, GEN7_SO_WRITE_OFFSET(i));
+		intel_ring_emit(ring, 0);
+	}
+
+	intel_ring_advance(ring);
+
+	return 0;
+}
+
+static int
 i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		       struct drm_file *file,
 		       struct drm_i915_gem_execbuffer2 *args,
@@ -984,6 +1009,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 	struct intel_ring_buffer *ring;
 	u32 exec_start, exec_len;
 	u32 seqno;
+	u32 mask;
 	int ret, mode, i;
 
 	if (!i915_gem_check_execbuffer(args)) {
@@ -1021,6 +1047,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 	}
 
 	mode = args->flags & I915_EXEC_CONSTANTS_MASK;
+	mask = I915_EXEC_CONSTANTS_MASK;
 	switch (mode) {
 	case I915_EXEC_CONSTANTS_REL_GENERAL:
 	case I915_EXEC_CONSTANTS_ABSOLUTE:
@@ -1034,18 +1061,9 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 			    mode == I915_EXEC_CONSTANTS_REL_SURFACE)
 				return -EINVAL;
 
-			ret = intel_ring_begin(ring, 4);
-			if (ret)
-				return ret;
-
-			intel_ring_emit(ring, MI_NOOP);
-			intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
-			intel_ring_emit(ring, INSTPM);
-			intel_ring_emit(ring,
-					I915_EXEC_CONSTANTS_MASK << 16 | mode);
-			intel_ring_advance(ring);
-
-			dev_priv->relative_constants_mode = mode;
+			/* The HW changed the meaning on this bit on gen6 */
+			if (INTEL_INFO(dev)->gen >= 6)
+				mask &= ~I915_EXEC_CONSTANTS_REL_SURFACE;
 		}
 		break;
 	default:
@@ -1176,6 +1194,27 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		}
 	}
 
+	if (ring == &dev_priv->ring[RCS] &&
+	    mode != dev_priv->relative_constants_mode) {
+		ret = intel_ring_begin(ring, 4);
+		if (ret)
+				goto err;
+
+		intel_ring_emit(ring, MI_NOOP);
+		intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
+		intel_ring_emit(ring, INSTPM);
+		intel_ring_emit(ring, mask << 16 | mode);
+		intel_ring_advance(ring);
+
+		dev_priv->relative_constants_mode = mode;
+	}
+
+	if (args->flags & I915_EXEC_GEN7_SOL_RESET) {
+		ret = i915_reset_gen7_sol_offsets(dev, ring);
+		if (ret)
+			goto err;
+	}
+
 	trace_i915_gem_ring_dispatch(ring, seqno);
 
 	exec_start = batch_obj->gtt_offset + args->batch_start_offset;
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index b40004b55977..5d433fc11ace 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -1205,7 +1205,7 @@ static void i915_pageflip_stall_check(struct drm_device *dev, int pipe)
 	} else {
 		int dspaddr = DSPADDR(intel_crtc->plane);
 		stall_detected = I915_READ(dspaddr) == (obj->gtt_offset +
-							crtc->y * crtc->fb->pitch +
+							crtc->y * crtc->fb->pitches[0] +
 							crtc->x * crtc->fb->bits_per_pixel/8);
 	}
 
@@ -1649,13 +1649,6 @@ static bool kick_ring(struct intel_ring_buffer *ring)
 		I915_WRITE_CTL(ring, tmp);
 		return true;
 	}
-	if (IS_GEN6(dev) &&
-	    (tmp & RING_WAIT_SEMAPHORE)) {
-		DRM_ERROR("Kicking stuck semaphore on %s\n",
-			  ring->name);
-		I915_WRITE_CTL(ring, tmp);
-		return true;
-	}
 	return false;
 }
 
@@ -1669,7 +1662,7 @@ void i915_hangcheck_elapsed(unsigned long data)
 {
 	struct drm_device *dev = (struct drm_device *)data;
 	drm_i915_private_t *dev_priv = dev->dev_private;
-	uint32_t acthd, instdone, instdone1;
+	uint32_t acthd, instdone, instdone1, acthd_bsd, acthd_blt;
 	bool err = false;
 
 	if (!i915_enable_hangcheck)
@@ -1686,16 +1679,21 @@ void i915_hangcheck_elapsed(unsigned long data)
 	}
 
 	if (INTEL_INFO(dev)->gen < 4) {
-		acthd = I915_READ(ACTHD);
 		instdone = I915_READ(INSTDONE);
 		instdone1 = 0;
 	} else {
-		acthd = I915_READ(ACTHD_I965);
 		instdone = I915_READ(INSTDONE_I965);
 		instdone1 = I915_READ(INSTDONE1);
 	}
+	acthd = intel_ring_get_active_head(&dev_priv->ring[RCS]);
+	acthd_bsd = HAS_BSD(dev) ?
+		intel_ring_get_active_head(&dev_priv->ring[VCS]) : 0;
+	acthd_blt = HAS_BLT(dev) ?
+		intel_ring_get_active_head(&dev_priv->ring[BCS]) : 0;
 
 	if (dev_priv->last_acthd == acthd &&
+	    dev_priv->last_acthd_bsd == acthd_bsd &&
+	    dev_priv->last_acthd_blt == acthd_blt &&
 	    dev_priv->last_instdone == instdone &&
 	    dev_priv->last_instdone1 == instdone1) {
 		if (dev_priv->hangcheck_count++ > 1) {
@@ -1727,6 +1725,8 @@ void i915_hangcheck_elapsed(unsigned long data)
 		dev_priv->hangcheck_count = 0;
 
 		dev_priv->last_acthd = acthd;
+		dev_priv->last_acthd_bsd = acthd_bsd;
+		dev_priv->last_acthd_blt = acthd_blt;
 		dev_priv->last_instdone = instdone;
 		dev_priv->last_instdone1 = instdone1;
 	}
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index a26d5b0a3690..c3afb783cb9d 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -442,6 +442,7 @@
 #define   INSTPM_AGPBUSY_DIS (1<<11) /* gen3: when disabled, pending interrupts
 					will not assert AGPBUSY# and will only
 					be delivered when out of C3. */
+#define   INSTPM_FORCE_ORDERING				(1<<7) /* GEN6+ */
 #define ACTHD	        0x020c8
 #define FW_BLC		0x020d8
 #define FW_BLC2		0x020dc
@@ -2321,6 +2322,7 @@
 #define   PIPECONF_PROGRESSIVE	(0 << 21)
 #define   PIPECONF_INTERLACE_W_FIELD_INDICATION	(6 << 21)
 #define   PIPECONF_INTERLACE_FIELD_0_ONLY		(7 << 21)
+#define   PIPECONF_INTERLACE_MASK	(7 << 21)
 #define   PIPECONF_CXSR_DOWNCLOCK	(1<<16)
 #define   PIPECONF_BPP_MASK	(0x000000e0)
 #define   PIPECONF_BPP_8	(0<<5)
@@ -2459,6 +2461,8 @@
 #define WM3_LP_ILK		0x45110
 #define  WM3_LP_EN		(1<<31)
 #define WM1S_LP_ILK		0x45120
+#define WM2S_LP_IVB		0x45124
+#define WM3S_LP_IVB		0x45128
 #define  WM1S_LP_EN		(1<<31)
 
 /* Memory latency timer register */
@@ -2675,6 +2679,140 @@
 #define _DSPBSURF		0x7119C
 #define _DSPBTILEOFF		0x711A4
 
+/* Sprite A control */
+#define _DVSACNTR		0x72180
+#define   DVS_ENABLE		(1<<31)
+#define   DVS_GAMMA_ENABLE	(1<<30)
+#define   DVS_PIXFORMAT_MASK	(3<<25)
+#define   DVS_FORMAT_YUV422	(0<<25)
+#define   DVS_FORMAT_RGBX101010	(1<<25)
+#define   DVS_FORMAT_RGBX888	(2<<25)
+#define   DVS_FORMAT_RGBX161616	(3<<25)
+#define   DVS_SOURCE_KEY	(1<<22)
+#define   DVS_RGB_ORDER_RGBX	(1<<20)
+#define   DVS_YUV_BYTE_ORDER_MASK (3<<16)
+#define   DVS_YUV_ORDER_YUYV	(0<<16)
+#define   DVS_YUV_ORDER_UYVY	(1<<16)
+#define   DVS_YUV_ORDER_YVYU	(2<<16)
+#define   DVS_YUV_ORDER_VYUY	(3<<16)
+#define   DVS_DEST_KEY		(1<<2)
+#define   DVS_TRICKLE_FEED_DISABLE (1<<14)
+#define   DVS_TILED		(1<<10)
+#define _DVSALINOFF		0x72184
+#define _DVSASTRIDE		0x72188
+#define _DVSAPOS		0x7218c
+#define _DVSASIZE		0x72190
+#define _DVSAKEYVAL		0x72194
+#define _DVSAKEYMSK		0x72198
+#define _DVSASURF		0x7219c
+#define _DVSAKEYMAXVAL		0x721a0
+#define _DVSATILEOFF		0x721a4
+#define _DVSASURFLIVE		0x721ac
+#define _DVSASCALE		0x72204
+#define   DVS_SCALE_ENABLE	(1<<31)
+#define   DVS_FILTER_MASK	(3<<29)
+#define   DVS_FILTER_MEDIUM	(0<<29)
+#define   DVS_FILTER_ENHANCING	(1<<29)
+#define   DVS_FILTER_SOFTENING	(2<<29)
+#define   DVS_VERTICAL_OFFSET_HALF (1<<28) /* must be enabled below */
+#define   DVS_VERTICAL_OFFSET_ENABLE (1<<27)
+#define _DVSAGAMC		0x72300
+
+#define _DVSBCNTR		0x73180
+#define _DVSBLINOFF		0x73184
+#define _DVSBSTRIDE		0x73188
+#define _DVSBPOS		0x7318c
+#define _DVSBSIZE		0x73190
+#define _DVSBKEYVAL		0x73194
+#define _DVSBKEYMSK		0x73198
+#define _DVSBSURF		0x7319c
+#define _DVSBKEYMAXVAL		0x731a0
+#define _DVSBTILEOFF		0x731a4
+#define _DVSBSURFLIVE		0x731ac
+#define _DVSBSCALE		0x73204
+#define _DVSBGAMC		0x73300
+
+#define DVSCNTR(pipe) _PIPE(pipe, _DVSACNTR, _DVSBCNTR)
+#define DVSLINOFF(pipe) _PIPE(pipe, _DVSALINOFF, _DVSBLINOFF)
+#define DVSSTRIDE(pipe) _PIPE(pipe, _DVSASTRIDE, _DVSBSTRIDE)
+#define DVSPOS(pipe) _PIPE(pipe, _DVSAPOS, _DVSBPOS)
+#define DVSSURF(pipe) _PIPE(pipe, _DVSASURF, _DVSBSURF)
+#define DVSKEYMAX(pipe) _PIPE(pipe, _DVSAKEYMAXVAL, _DVSBKEYMAXVAL)
+#define DVSSIZE(pipe) _PIPE(pipe, _DVSASIZE, _DVSBSIZE)
+#define DVSSCALE(pipe) _PIPE(pipe, _DVSASCALE, _DVSBSCALE)
+#define DVSTILEOFF(pipe) _PIPE(pipe, _DVSATILEOFF, _DVSBTILEOFF)
+#define DVSKEYVAL(pipe) _PIPE(pipe, _DVSAKEYVAL, _DVSBKEYVAL)
+#define DVSKEYMSK(pipe) _PIPE(pipe, _DVSAKEYMSK, _DVSBKEYMSK)
+
+#define _SPRA_CTL		0x70280
+#define   SPRITE_ENABLE			(1<<31)
+#define   SPRITE_GAMMA_ENABLE		(1<<30)
+#define   SPRITE_PIXFORMAT_MASK		(7<<25)
+#define   SPRITE_FORMAT_YUV422		(0<<25)
+#define   SPRITE_FORMAT_RGBX101010	(1<<25)
+#define   SPRITE_FORMAT_RGBX888		(2<<25)
+#define   SPRITE_FORMAT_RGBX161616	(3<<25)
+#define   SPRITE_FORMAT_YUV444		(4<<25)
+#define   SPRITE_FORMAT_XR_BGR101010	(5<<25) /* Extended range */
+#define   SPRITE_CSC_ENABLE		(1<<24)
+#define   SPRITE_SOURCE_KEY		(1<<22)
+#define   SPRITE_RGB_ORDER_RGBX		(1<<20) /* only for 888 and 161616 */
+#define   SPRITE_YUV_TO_RGB_CSC_DISABLE	(1<<19)
+#define   SPRITE_YUV_CSC_FORMAT_BT709	(1<<18) /* 0 is BT601 */
+#define   SPRITE_YUV_BYTE_ORDER_MASK	(3<<16)
+#define   SPRITE_YUV_ORDER_YUYV		(0<<16)
+#define   SPRITE_YUV_ORDER_UYVY		(1<<16)
+#define   SPRITE_YUV_ORDER_YVYU		(2<<16)
+#define   SPRITE_YUV_ORDER_VYUY		(3<<16)
+#define   SPRITE_TRICKLE_FEED_DISABLE	(1<<14)
+#define   SPRITE_INT_GAMMA_ENABLE	(1<<13)
+#define   SPRITE_TILED			(1<<10)
+#define   SPRITE_DEST_KEY		(1<<2)
+#define _SPRA_LINOFF		0x70284
+#define _SPRA_STRIDE		0x70288
+#define _SPRA_POS		0x7028c
+#define _SPRA_SIZE		0x70290
+#define _SPRA_KEYVAL		0x70294
+#define _SPRA_KEYMSK		0x70298
+#define _SPRA_SURF		0x7029c
+#define _SPRA_KEYMAX		0x702a0
+#define _SPRA_TILEOFF		0x702a4
+#define _SPRA_SCALE		0x70304
+#define   SPRITE_SCALE_ENABLE	(1<<31)
+#define   SPRITE_FILTER_MASK	(3<<29)
+#define   SPRITE_FILTER_MEDIUM	(0<<29)
+#define   SPRITE_FILTER_ENHANCING	(1<<29)
+#define   SPRITE_FILTER_SOFTENING	(2<<29)
+#define   SPRITE_VERTICAL_OFFSET_HALF	(1<<28) /* must be enabled below */
+#define   SPRITE_VERTICAL_OFFSET_ENABLE	(1<<27)
+#define _SPRA_GAMC		0x70400
+
+#define _SPRB_CTL		0x71280
+#define _SPRB_LINOFF		0x71284
+#define _SPRB_STRIDE		0x71288
+#define _SPRB_POS		0x7128c
+#define _SPRB_SIZE		0x71290
+#define _SPRB_KEYVAL		0x71294
+#define _SPRB_KEYMSK		0x71298
+#define _SPRB_SURF		0x7129c
+#define _SPRB_KEYMAX		0x712a0
+#define _SPRB_TILEOFF		0x712a4
+#define _SPRB_SCALE		0x71304
+#define _SPRB_GAMC		0x71400
+
+#define SPRCTL(pipe) _PIPE(pipe, _SPRA_CTL, _SPRB_CTL)
+#define SPRLINOFF(pipe) _PIPE(pipe, _SPRA_LINOFF, _SPRB_LINOFF)
+#define SPRSTRIDE(pipe) _PIPE(pipe, _SPRA_STRIDE, _SPRB_STRIDE)
+#define SPRPOS(pipe) _PIPE(pipe, _SPRA_POS, _SPRB_POS)
+#define SPRSIZE(pipe) _PIPE(pipe, _SPRA_SIZE, _SPRB_SIZE)
+#define SPRKEYVAL(pipe) _PIPE(pipe, _SPRA_KEYVAL, _SPRB_KEYVAL)
+#define SPRKEYMSK(pipe) _PIPE(pipe, _SPRA_KEYMSK, _SPRB_KEYMSK)
+#define SPRSURF(pipe) _PIPE(pipe, _SPRA_SURF, _SPRB_SURF)
+#define SPRKEYMAX(pipe) _PIPE(pipe, _SPRA_KEYMAX, _SPRB_KEYMAX)
+#define SPRTILEOFF(pipe) _PIPE(pipe, _SPRA_TILEOFF, _SPRB_TILEOFF)
+#define SPRSCALE(pipe) _PIPE(pipe, _SPRA_SCALE, _SPRB_SCALE)
+#define SPRGAMC(pipe) _PIPE(pipe, _SPRA_GAMC, _SPRB_GAMC)
+
 /* VBIOS regs */
 #define VGACNTRL		0x71400
 # define VGA_DISP_DISABLE			(1 << 31)
@@ -2882,6 +3020,10 @@
 #define   ILK_DPFC_DIS1		(1<<8)
 #define   ILK_DPFC_DIS2		(1<<9)
 
+#define IVB_CHICKEN3	0x4200c
+# define CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE	(1 << 5)
+# define CHICKEN3_DGMG_DONE_FIX_DISABLE		(1 << 2)
+
 #define DISP_ARB_CTL	0x45000
 #define  DISP_TILE_SURFACE_SWIZZLING	(1<<13)
 #define  DISP_FBC_WM_DIS		(1<<15)
@@ -3500,7 +3642,11 @@
 #define   GEN6_CAGF_MASK			(0x7f << GEN6_CAGF_SHIFT)
 #define GEN6_RP_CONTROL				0xA024
 #define   GEN6_RP_MEDIA_TURBO			(1<<11)
-#define   GEN6_RP_USE_NORMAL_FREQ		(1<<9)
+#define   GEN6_RP_MEDIA_MODE_MASK		(3<<9)
+#define   GEN6_RP_MEDIA_HW_TURBO_MODE		(3<<9)
+#define   GEN6_RP_MEDIA_HW_NORMAL_MODE		(2<<9)
+#define   GEN6_RP_MEDIA_HW_MODE			(1<<9)
+#define   GEN6_RP_MEDIA_SW_MODE			(0<<9)
 #define   GEN6_RP_MEDIA_IS_GFX			(1<<8)
 #define   GEN6_RP_ENABLE			(1<<7)
 #define   GEN6_RP_UP_IDLE_MIN			(0x1<<3)
@@ -3557,6 +3703,14 @@
 #define GEN6_PCODE_DATA				0x138128
 #define   GEN6_PCODE_FREQ_IA_RATIO_SHIFT	8
 
+#define GEN6_GT_CORE_STATUS		0x138060
+#define   GEN6_CORE_CPD_STATE_MASK	(7<<4)
+#define   GEN6_RCn_MASK			7
+#define   GEN6_RC0			0
+#define   GEN6_RC3			2
+#define   GEN6_RC6			3
+#define   GEN6_RC7			4
+
 #define G4X_AUD_VID_DID			0x62020
 #define INTEL_AUDIO_DEVCL		0x808629FB
 #define INTEL_AUDIO_DEVBLC		0x80862801
@@ -3569,17 +3723,23 @@
 #define G4X_ELD_ACK			(1 << 4)
 #define G4X_HDMIW_HDMIEDID		0x6210C
 
-#define GEN5_HDMIW_HDMIEDID_A		0xE2050
-#define GEN5_AUD_CNTL_ST_A		0xE20B4
-#define GEN5_ELD_BUFFER_SIZE		(0x1f << 10)
-#define GEN5_ELD_ADDRESS		(0x1f << 5)
-#define GEN5_ELD_ACK			(1 << 4)
-#define GEN5_AUD_CNTL_ST2		0xE20C0
-#define GEN5_ELD_VALIDB			(1 << 0)
-#define GEN5_CP_READYB			(1 << 1)
-
-#define GEN7_HDMIW_HDMIEDID_A		0xE5050
-#define GEN7_AUD_CNTRL_ST_A		0xE50B4
-#define GEN7_AUD_CNTRL_ST2		0xE50C0
+#define IBX_HDMIW_HDMIEDID_A		0xE2050
+#define IBX_AUD_CNTL_ST_A		0xE20B4
+#define IBX_ELD_BUFFER_SIZE		(0x1f << 10)
+#define IBX_ELD_ADDRESS			(0x1f << 5)
+#define IBX_ELD_ACK			(1 << 4)
+#define IBX_AUD_CNTL_ST2		0xE20C0
+#define IBX_ELD_VALIDB			(1 << 0)
+#define IBX_CP_READYB			(1 << 1)
+
+#define CPT_HDMIW_HDMIEDID_A		0xE5050
+#define CPT_AUD_CNTL_ST_A		0xE50B4
+#define CPT_AUD_CNTRL_ST2		0xE50C0
+
+/* These are the 4 32-bit write offset registers for each stream
+ * output buffer.  It determines the offset from the
+ * 3DSTATE_SO_BUFFERs that the next streamed vertex output goes to.
+ */
+#define GEN7_SO_WRITE_OFFSET(n)		(0x5280 + (n) * 4)
 
 #endif /* _I915_REG_H_ */
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index daa5743ccbd6..2a3f707caab8 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -915,8 +915,8 @@ static void assert_panel_unlocked(struct drm_i915_private *dev_priv,
 	     pipe_name(pipe));
 }
 
-static void assert_pipe(struct drm_i915_private *dev_priv,
-			enum pipe pipe, bool state)
+void assert_pipe(struct drm_i915_private *dev_priv,
+		 enum pipe pipe, bool state)
 {
 	int reg;
 	u32 val;
@@ -929,8 +929,6 @@ static void assert_pipe(struct drm_i915_private *dev_priv,
 	     "pipe %c assertion failure (expected %s, current %s)\n",
 	     pipe_name(pipe), state_string(state), state_string(cur_state));
 }
-#define assert_pipe_enabled(d, p) assert_pipe(d, p, true)
-#define assert_pipe_disabled(d, p) assert_pipe(d, p, false)
 
 static void assert_plane_enabled(struct drm_i915_private *dev_priv,
 				 enum plane plane)
@@ -1206,7 +1204,8 @@ static void intel_disable_pch_pll(struct drm_i915_private *dev_priv,
 				  enum pipe pipe)
 {
 	int reg;
-	u32 val;
+	u32 val, pll_mask = TRANSC_DPLL_ENABLE | TRANSC_DPLLB_SEL,
+		pll_sel = TRANSC_DPLL_ENABLE;
 
 	if (pipe > 1)
 		return;
@@ -1217,6 +1216,15 @@ static void intel_disable_pch_pll(struct drm_i915_private *dev_priv,
 	/* Make sure transcoder isn't still depending on us */
 	assert_transcoder_disabled(dev_priv, pipe);
 
+	if (pipe == 0)
+		pll_sel |= TRANSC_DPLLA_SEL;
+	else if (pipe == 1)
+		pll_sel |= TRANSC_DPLLB_SEL;
+
+
+	if ((I915_READ(PCH_DPLL_SEL) & pll_mask) == pll_sel)
+		return;
+
 	reg = PCH_DPLL(pipe);
 	val = I915_READ(reg);
 	val &= ~DPLL_VCO_ENABLE;
@@ -1511,8 +1519,8 @@ static void i8xx_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
 	u32 fbc_ctl, fbc_ctl2;
 
 	cfb_pitch = dev_priv->cfb_size / FBC_LL_SIZE;
-	if (fb->pitch < cfb_pitch)
-		cfb_pitch = fb->pitch;
+	if (fb->pitches[0] < cfb_pitch)
+		cfb_pitch = fb->pitches[0];
 
 	/* FBC_CTL wants 64B units */
 	cfb_pitch = (cfb_pitch / 64) - 1;
@@ -2073,11 +2081,11 @@ static int i9xx_update_plane(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	I915_WRITE(reg, dspcntr);
 
 	Start = obj->gtt_offset;
-	Offset = y * fb->pitch + x * (fb->bits_per_pixel / 8);
+	Offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
 
 	DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
-		      Start, Offset, x, y, fb->pitch);
-	I915_WRITE(DSPSTRIDE(plane), fb->pitch);
+		      Start, Offset, x, y, fb->pitches[0]);
+	I915_WRITE(DSPSTRIDE(plane), fb->pitches[0]);
 	if (INTEL_INFO(dev)->gen >= 4) {
 		I915_WRITE(DSPSURF(plane), Start);
 		I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
@@ -2154,11 +2162,11 @@ static int ironlake_update_plane(struct drm_crtc *crtc,
 	I915_WRITE(reg, dspcntr);
 
 	Start = obj->gtt_offset;
-	Offset = y * fb->pitch + x * (fb->bits_per_pixel / 8);
+	Offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
 
 	DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
-		      Start, Offset, x, y, fb->pitch);
-	I915_WRITE(DSPSTRIDE(plane), fb->pitch);
+		      Start, Offset, x, y, fb->pitches[0]);
+	I915_WRITE(DSPSTRIDE(plane), fb->pitches[0]);
 	I915_WRITE(DSPSURF(plane), Start);
 	I915_WRITE(DSPTILEOFF(plane), (y << 16) | x);
 	I915_WRITE(DSPADDR(plane), Offset);
@@ -4509,7 +4517,7 @@ static void ironlake_update_wm(struct drm_device *dev)
 	 */
 }
 
-static void sandybridge_update_wm(struct drm_device *dev)
+void sandybridge_update_wm(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
@@ -4569,7 +4577,8 @@ static void sandybridge_update_wm(struct drm_device *dev)
 	I915_WRITE(WM2_LP_ILK, 0);
 	I915_WRITE(WM1_LP_ILK, 0);
 
-	if (!single_plane_enabled(enabled))
+	if (!single_plane_enabled(enabled) ||
+	    dev_priv->sprite_scaling_enabled)
 		return;
 	enabled = ffs(enabled) - 1;
 
@@ -4619,6 +4628,149 @@ static void sandybridge_update_wm(struct drm_device *dev)
 		   cursor_wm);
 }
 
+static bool
+sandybridge_compute_sprite_wm(struct drm_device *dev, int plane,
+			      uint32_t sprite_width, int pixel_size,
+			      const struct intel_watermark_params *display,
+			      int display_latency_ns, int *sprite_wm)
+{
+	struct drm_crtc *crtc;
+	int clock;
+	int entries, tlb_miss;
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	if (crtc->fb == NULL || !crtc->enabled) {
+		*sprite_wm = display->guard_size;
+		return false;
+	}
+
+	clock = crtc->mode.clock;
+
+	/* Use the small buffer method to calculate the sprite watermark */
+	entries = ((clock * pixel_size / 1000) * display_latency_ns) / 1000;
+	tlb_miss = display->fifo_size*display->cacheline_size -
+		sprite_width * 8;
+	if (tlb_miss > 0)
+		entries += tlb_miss;
+	entries = DIV_ROUND_UP(entries, display->cacheline_size);
+	*sprite_wm = entries + display->guard_size;
+	if (*sprite_wm > (int)display->max_wm)
+		*sprite_wm = display->max_wm;
+
+	return true;
+}
+
+static bool
+sandybridge_compute_sprite_srwm(struct drm_device *dev, int plane,
+				uint32_t sprite_width, int pixel_size,
+				const struct intel_watermark_params *display,
+				int latency_ns, int *sprite_wm)
+{
+	struct drm_crtc *crtc;
+	unsigned long line_time_us;
+	int clock;
+	int line_count, line_size;
+	int small, large;
+	int entries;
+
+	if (!latency_ns) {
+		*sprite_wm = 0;
+		return false;
+	}
+
+	crtc = intel_get_crtc_for_plane(dev, plane);
+	clock = crtc->mode.clock;
+
+	line_time_us = (sprite_width * 1000) / clock;
+	line_count = (latency_ns / line_time_us + 1000) / 1000;
+	line_size = sprite_width * pixel_size;
+
+	/* Use the minimum of the small and large buffer method for primary */
+	small = ((clock * pixel_size / 1000) * latency_ns) / 1000;
+	large = line_count * line_size;
+
+	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
+	*sprite_wm = entries + display->guard_size;
+
+	return *sprite_wm > 0x3ff ? false : true;
+}
+
+static void sandybridge_update_sprite_wm(struct drm_device *dev, int pipe,
+					 uint32_t sprite_width, int pixel_size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int latency = SNB_READ_WM0_LATENCY() * 100;	/* In unit 0.1us */
+	int sprite_wm, reg;
+	int ret;
+
+	switch (pipe) {
+	case 0:
+		reg = WM0_PIPEA_ILK;
+		break;
+	case 1:
+		reg = WM0_PIPEB_ILK;
+		break;
+	case 2:
+		reg = WM0_PIPEC_IVB;
+		break;
+	default:
+		return; /* bad pipe */
+	}
+
+	ret = sandybridge_compute_sprite_wm(dev, pipe, sprite_width, pixel_size,
+					    &sandybridge_display_wm_info,
+					    latency, &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite wm for pipe %d\n",
+			      pipe);
+		return;
+	}
+
+	I915_WRITE(reg, I915_READ(reg) | (sprite_wm << WM0_PIPE_SPRITE_SHIFT));
+	DRM_DEBUG_KMS("sprite watermarks For pipe %d - %d\n", pipe, sprite_wm);
+
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM1_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp1 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM1S_LP_ILK, sprite_wm);
+
+	/* Only IVB has two more LP watermarks for sprite */
+	if (!IS_IVYBRIDGE(dev))
+		return;
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM2_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp2 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM2S_LP_IVB, sprite_wm);
+
+	ret = sandybridge_compute_sprite_srwm(dev, pipe, sprite_width,
+					      pixel_size,
+					      &sandybridge_display_srwm_info,
+					      SNB_READ_WM3_LATENCY() * 500,
+					      &sprite_wm);
+	if (!ret) {
+		DRM_DEBUG_KMS("failed to compute sprite lp3 wm on pipe %d\n",
+			      pipe);
+		return;
+	}
+	I915_WRITE(WM3S_LP_IVB, sprite_wm);
+}
+
 /**
  * intel_update_watermarks - update FIFO watermark values based on current modes
  *
@@ -4659,6 +4811,16 @@ static void intel_update_watermarks(struct drm_device *dev)
 		dev_priv->display.update_wm(dev);
 }
 
+void intel_update_sprite_watermarks(struct drm_device *dev, int pipe,
+				    uint32_t sprite_width, int pixel_size)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (dev_priv->display.update_sprite_wm)
+		dev_priv->display.update_sprite_wm(dev, pipe, sprite_width,
+						   pixel_size);
+}
+
 static inline bool intel_panel_use_ssc(struct drm_i915_private *dev_priv)
 {
 	if (i915_panel_use_ssc >= 0)
@@ -5155,7 +5317,7 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 		adjusted_mode->crtc_vsync_end -= 1;
 		adjusted_mode->crtc_vsync_start -= 1;
 	} else
-		pipeconf &= ~PIPECONF_INTERLACE_W_FIELD_INDICATION; /* progressive */
+		pipeconf &= ~PIPECONF_INTERLACE_MASK; /* progressive */
 
 	I915_WRITE(HTOTAL(pipe),
 		   (adjusted_mode->crtc_hdisplay - 1) |
@@ -5822,14 +5984,45 @@ static int intel_crtc_mode_set(struct drm_crtc *crtc,
 
 	ret = dev_priv->display.crtc_mode_set(crtc, mode, adjusted_mode,
 					      x, y, old_fb);
-
 	drm_vblank_post_modeset(dev, pipe);
 
-	intel_crtc->dpms_mode = DRM_MODE_DPMS_ON;
+	if (ret)
+		intel_crtc->dpms_mode = DRM_MODE_DPMS_OFF;
+	else
+		intel_crtc->dpms_mode = DRM_MODE_DPMS_ON;
 
 	return ret;
 }
 
+static bool intel_eld_uptodate(struct drm_connector *connector,
+			       int reg_eldv, uint32_t bits_eldv,
+			       int reg_elda, uint32_t bits_elda,
+			       int reg_edid)
+{
+	struct drm_i915_private *dev_priv = connector->dev->dev_private;
+	uint8_t *eld = connector->eld;
+	uint32_t i;
+
+	i = I915_READ(reg_eldv);
+	i &= bits_eldv;
+
+	if (!eld[0])
+		return !i;
+
+	if (!i)
+		return false;
+
+	i = I915_READ(reg_elda);
+	i &= ~bits_elda;
+	I915_WRITE(reg_elda, i);
+
+	for (i = 0; i < eld[2]; i++)
+		if (I915_READ(reg_edid) != *((uint32_t *)eld + i))
+			return false;
+
+	return true;
+}
+
 static void g4x_write_eld(struct drm_connector *connector,
 			  struct drm_crtc *crtc)
 {
@@ -5846,6 +6039,12 @@ static void g4x_write_eld(struct drm_connector *connector,
 	else
 		eldv = G4X_ELDV_DEVCTG;
 
+	if (intel_eld_uptodate(connector,
+			       G4X_AUD_CNTL_ST, eldv,
+			       G4X_AUD_CNTL_ST, G4X_ELD_ADDR,
+			       G4X_HDMIW_HDMIEDID))
+		return;
+
 	i = I915_READ(G4X_AUD_CNTL_ST);
 	i &= ~(eldv | G4X_ELD_ADDR);
 	len = (i >> 9) & 0x1f;		/* ELD buffer size */
@@ -5876,14 +6075,14 @@ static void ironlake_write_eld(struct drm_connector *connector,
 	int aud_cntl_st;
 	int aud_cntrl_st2;
 
-	if (IS_IVYBRIDGE(connector->dev)) {
-		hdmiw_hdmiedid = GEN7_HDMIW_HDMIEDID_A;
-		aud_cntl_st = GEN7_AUD_CNTRL_ST_A;
-		aud_cntrl_st2 = GEN7_AUD_CNTRL_ST2;
+	if (HAS_PCH_IBX(connector->dev)) {
+		hdmiw_hdmiedid = IBX_HDMIW_HDMIEDID_A;
+		aud_cntl_st = IBX_AUD_CNTL_ST_A;
+		aud_cntrl_st2 = IBX_AUD_CNTL_ST2;
 	} else {
-		hdmiw_hdmiedid = GEN5_HDMIW_HDMIEDID_A;
-		aud_cntl_st = GEN5_AUD_CNTL_ST_A;
-		aud_cntrl_st2 = GEN5_AUD_CNTL_ST2;
+		hdmiw_hdmiedid = CPT_HDMIW_HDMIEDID_A;
+		aud_cntl_st = CPT_AUD_CNTL_ST_A;
+		aud_cntrl_st2 = CPT_AUD_CNTRL_ST2;
 	}
 
 	i = to_intel_crtc(crtc)->pipe;
@@ -5897,14 +6096,25 @@ static void ironlake_write_eld(struct drm_connector *connector,
 	if (!i) {
 		DRM_DEBUG_DRIVER("Audio directed to unknown port\n");
 		/* operate blindly on all ports */
-		eldv = GEN5_ELD_VALIDB;
-		eldv |= GEN5_ELD_VALIDB << 4;
-		eldv |= GEN5_ELD_VALIDB << 8;
+		eldv = IBX_ELD_VALIDB;
+		eldv |= IBX_ELD_VALIDB << 4;
+		eldv |= IBX_ELD_VALIDB << 8;
 	} else {
 		DRM_DEBUG_DRIVER("ELD on port %c\n", 'A' + i);
-		eldv = GEN5_ELD_VALIDB << ((i - 1) * 4);
+		eldv = IBX_ELD_VALIDB << ((i - 1) * 4);
+	}
+
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT)) {
+		DRM_DEBUG_DRIVER("ELD: DisplayPort detected\n");
+		eld[5] |= (1 << 2);	/* Conn_Type, 0x1 = DisplayPort */
 	}
 
+	if (intel_eld_uptodate(connector,
+			       aud_cntrl_st2, eldv,
+			       aud_cntl_st, IBX_ELD_ADDRESS,
+			       hdmiw_hdmiedid))
+		return;
+
 	i = I915_READ(aud_cntrl_st2);
 	i &= ~eldv;
 	I915_WRITE(aud_cntrl_st2, i);
@@ -5912,13 +6122,8 @@ static void ironlake_write_eld(struct drm_connector *connector,
 	if (!eld[0])
 		return;
 
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT)) {
-		DRM_DEBUG_DRIVER("ELD: DisplayPort detected\n");
-		eld[5] |= (1 << 2);	/* Conn_Type, 0x1 = DisplayPort */
-	}
-
 	i = I915_READ(aud_cntl_st);
-	i &= ~GEN5_ELD_ADDRESS;
+	i &= ~IBX_ELD_ADDRESS;
 	I915_WRITE(aud_cntl_st, i);
 
 	len = min_t(uint8_t, eld[2], 21);	/* 84 bytes of hw ELD buffer */
@@ -6298,7 +6503,7 @@ static struct drm_display_mode load_detect_mode = {
 
 static struct drm_framebuffer *
 intel_framebuffer_create(struct drm_device *dev,
-			 struct drm_mode_fb_cmd *mode_cmd,
+			 struct drm_mode_fb_cmd2 *mode_cmd,
 			 struct drm_i915_gem_object *obj)
 {
 	struct intel_framebuffer *intel_fb;
@@ -6340,7 +6545,7 @@ intel_framebuffer_create_for_mode(struct drm_device *dev,
 				  int depth, int bpp)
 {
 	struct drm_i915_gem_object *obj;
-	struct drm_mode_fb_cmd mode_cmd;
+	struct drm_mode_fb_cmd2 mode_cmd;
 
 	obj = i915_gem_alloc_object(dev,
 				    intel_framebuffer_size_for_mode(mode, bpp));
@@ -6349,9 +6554,9 @@ intel_framebuffer_create_for_mode(struct drm_device *dev,
 
 	mode_cmd.width = mode->hdisplay;
 	mode_cmd.height = mode->vdisplay;
-	mode_cmd.depth = depth;
-	mode_cmd.bpp = bpp;
-	mode_cmd.pitch = intel_framebuffer_pitch_for_width(mode_cmd.width, bpp);
+	mode_cmd.pitches[0] = intel_framebuffer_pitch_for_width(mode_cmd.width,
+								bpp);
+	mode_cmd.pixel_format = 0;
 
 	return intel_framebuffer_create(dev, &mode_cmd, obj);
 }
@@ -6372,11 +6577,11 @@ mode_fits_in_fbdev(struct drm_device *dev,
 		return NULL;
 
 	fb = &dev_priv->fbdev->ifb.base;
-	if (fb->pitch < intel_framebuffer_pitch_for_width(mode->hdisplay,
-							  fb->bits_per_pixel))
+	if (fb->pitches[0] < intel_framebuffer_pitch_for_width(mode->hdisplay,
+							       fb->bits_per_pixel))
 		return NULL;
 
-	if (obj->base.size < mode->vdisplay * fb->pitch)
+	if (obj->base.size < mode->vdisplay * fb->pitches[0])
 		return NULL;
 
 	return fb;
@@ -7009,7 +7214,7 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 		goto out;
 
 	/* Offset into the new buffer for cases of shared fbs between CRTCs */
-	offset = crtc->y * fb->pitch + crtc->x * fb->bits_per_pixel/8;
+	offset = crtc->y * fb->pitches[0] + crtc->x * fb->bits_per_pixel/8;
 
 	ret = BEGIN_LP_RING(6);
 	if (ret)
@@ -7026,7 +7231,7 @@ static int intel_gen2_queue_flip(struct drm_device *dev,
 	OUT_RING(MI_NOOP);
 	OUT_RING(MI_DISPLAY_FLIP |
 		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitch);
+	OUT_RING(fb->pitches[0]);
 	OUT_RING(obj->gtt_offset + offset);
 	OUT_RING(MI_NOOP);
 	ADVANCE_LP_RING();
@@ -7050,7 +7255,7 @@ static int intel_gen3_queue_flip(struct drm_device *dev,
 		goto out;
 
 	/* Offset into the new buffer for cases of shared fbs between CRTCs */
-	offset = crtc->y * fb->pitch + crtc->x * fb->bits_per_pixel/8;
+	offset = crtc->y * fb->pitches[0] + crtc->x * fb->bits_per_pixel/8;
 
 	ret = BEGIN_LP_RING(6);
 	if (ret)
@@ -7064,7 +7269,7 @@ static int intel_gen3_queue_flip(struct drm_device *dev,
 	OUT_RING(MI_NOOP);
 	OUT_RING(MI_DISPLAY_FLIP_I915 |
 		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitch);
+	OUT_RING(fb->pitches[0]);
 	OUT_RING(obj->gtt_offset + offset);
 	OUT_RING(MI_NOOP);
 
@@ -7097,7 +7302,7 @@ static int intel_gen4_queue_flip(struct drm_device *dev,
 	 */
 	OUT_RING(MI_DISPLAY_FLIP |
 		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitch);
+	OUT_RING(fb->pitches[0]);
 	OUT_RING(obj->gtt_offset | obj->tiling_mode);
 
 	/* XXX Enabling the panel-fitter across page-flip is so far
@@ -7132,7 +7337,7 @@ static int intel_gen6_queue_flip(struct drm_device *dev,
 
 	OUT_RING(MI_DISPLAY_FLIP |
 		 MI_DISPLAY_FLIP_PLANE(intel_crtc->plane));
-	OUT_RING(fb->pitch | obj->tiling_mode);
+	OUT_RING(fb->pitches[0] | obj->tiling_mode);
 	OUT_RING(obj->gtt_offset);
 
 	pf = I915_READ(PF_CTL(intel_crtc->pipe)) & PF_ENABLE;
@@ -7168,7 +7373,7 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 		goto out;
 
 	intel_ring_emit(ring, MI_DISPLAY_FLIP_I915 | (intel_crtc->plane << 19));
-	intel_ring_emit(ring, (fb->pitch | obj->tiling_mode));
+	intel_ring_emit(ring, (fb->pitches[0] | obj->tiling_mode));
 	intel_ring_emit(ring, (obj->gtt_offset));
 	intel_ring_emit(ring, (MI_NOOP));
 	intel_ring_advance(ring);
@@ -7594,7 +7799,7 @@ static const struct drm_framebuffer_funcs intel_fb_funcs = {
 
 int intel_framebuffer_init(struct drm_device *dev,
 			   struct intel_framebuffer *intel_fb,
-			   struct drm_mode_fb_cmd *mode_cmd,
+			   struct drm_mode_fb_cmd2 *mode_cmd,
 			   struct drm_i915_gem_object *obj)
 {
 	int ret;
@@ -7602,21 +7807,25 @@ int intel_framebuffer_init(struct drm_device *dev,
 	if (obj->tiling_mode == I915_TILING_Y)
 		return -EINVAL;
 
-	if (mode_cmd->pitch & 63)
+	if (mode_cmd->pitches[0] & 63)
 		return -EINVAL;
 
-	switch (mode_cmd->bpp) {
-	case 8:
-	case 16:
-		/* Only pre-ILK can handle 5:5:5 */
-		if (mode_cmd->depth == 15 && !HAS_PCH_SPLIT(dev))
-			return -EINVAL;
+	switch (mode_cmd->pixel_format) {
+	case DRM_FORMAT_RGB332:
+	case DRM_FORMAT_RGB565:
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_ARGB8888:
+	case DRM_FORMAT_XRGB2101010:
+	case DRM_FORMAT_ARGB2101010:
+		/* RGB formats are common across chipsets */
 		break;
-
-	case 24:
-	case 32:
+	case DRM_FORMAT_YUYV:
+	case DRM_FORMAT_UYVY:
+	case DRM_FORMAT_YVYU:
+	case DRM_FORMAT_VYUY:
 		break;
 	default:
+		DRM_ERROR("unsupported pixel format\n");
 		return -EINVAL;
 	}
 
@@ -7634,11 +7843,12 @@ int intel_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 intel_user_framebuffer_create(struct drm_device *dev,
 			      struct drm_file *filp,
-			      struct drm_mode_fb_cmd *mode_cmd)
+			      struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_i915_gem_object *obj;
 
-	obj = to_intel_bo(drm_gem_object_lookup(dev, filp, mode_cmd->handle));
+	obj = to_intel_bo(drm_gem_object_lookup(dev, filp,
+						mode_cmd->handles[0]));
 	if (&obj->base == NULL)
 		return ERR_PTR(-ENOENT);
 
@@ -7995,7 +8205,7 @@ void gen6_enable_rps(struct drm_i915_private *dev_priv)
 	I915_WRITE(GEN6_RP_IDLE_HYSTERSIS, 10);
 	I915_WRITE(GEN6_RP_CONTROL,
 		   GEN6_RP_MEDIA_TURBO |
-		   GEN6_RP_USE_NORMAL_FREQ |
+		   GEN6_RP_MEDIA_HW_MODE |
 		   GEN6_RP_MEDIA_IS_GFX |
 		   GEN6_RP_ENABLE |
 		   GEN6_RP_UP_BUSY_AVG |
@@ -8250,6 +8460,10 @@ static void ivybridge_init_clock_gating(struct drm_device *dev)
 
 	I915_WRITE(ILK_DSPCLK_GATE, IVB_VRHUNIT_CLK_GATE);
 
+	I915_WRITE(IVB_CHICKEN3,
+		   CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE |
+		   CHICKEN3_DGMG_DONE_FIX_DISABLE);
+
 	for_each_pipe(pipe) {
 		I915_WRITE(DSPCNTR(pipe),
 			   I915_READ(DSPCNTR(pipe)) |
@@ -8543,9 +8757,15 @@ static void intel_init_display(struct drm_device *dev)
 		if (IS_IVYBRIDGE(dev)) {
 			u32	ecobus;
 
+			/* A small trick here - if the bios hasn't configured MT forcewake,
+			 * and if the device is in RC6, then force_wake_mt_get will not wake
+			 * the device and the ECOBUS read will return zero. Which will be
+			 * (correctly) interpreted by the test below as MT forcewake being
+			 * disabled.
+			 */
 			mutex_lock(&dev->struct_mutex);
 			__gen6_gt_force_wake_mt_get(dev_priv);
-			ecobus = I915_READ(ECOBUS);
+			ecobus = I915_READ_NOTRACE(ECOBUS);
 			__gen6_gt_force_wake_mt_put(dev_priv);
 			mutex_unlock(&dev->struct_mutex);
 
@@ -8577,6 +8797,7 @@ static void intel_init_display(struct drm_device *dev)
 		} else if (IS_GEN6(dev)) {
 			if (SNB_READ_WM0_LATENCY()) {
 				dev_priv->display.update_wm = sandybridge_update_wm;
+				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
 			} else {
 				DRM_DEBUG_KMS("Failed to read display plane latency. "
 					      "Disable CxSR\n");
@@ -8590,6 +8811,7 @@ static void intel_init_display(struct drm_device *dev)
 			dev_priv->display.fdi_link_train = ivb_manual_fdi_link_train;
 			if (SNB_READ_WM0_LATENCY()) {
 				dev_priv->display.update_wm = sandybridge_update_wm;
+				dev_priv->display.update_sprite_wm = sandybridge_update_sprite_wm;
 			} else {
 				DRM_DEBUG_KMS("Failed to read display plane latency. "
 					      "Disable CxSR\n");
@@ -8773,7 +8995,7 @@ static void i915_disable_vga(struct drm_device *dev)
 void intel_modeset_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int i;
+	int i, ret;
 
 	drm_mode_config_init(dev);
 
@@ -8803,6 +9025,12 @@ void intel_modeset_init(struct drm_device *dev)
 
 	for (i = 0; i < dev_priv->num_pipe; i++) {
 		intel_crtc_init(dev, i);
+		if (HAS_PCH_SPLIT(dev)) {
+			ret = intel_plane_init(dev, i);
+			if (ret)
+				DRM_ERROR("plane %d init failed: %d\n",
+					  i, ret);
+		}
 	}
 
 	/* Just disable it once at startup */
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 92b041b66e49..db3b461ad412 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -1926,6 +1926,7 @@ intel_dp_link_down(struct intel_dp *intel_dp)
 			intel_wait_for_vblank(dev, to_intel_crtc(crtc)->pipe);
 	}
 
+	DP &= ~DP_AUDIO_OUTPUT_ENABLE;
 	I915_WRITE(intel_dp->output_reg, DP & ~DP_PORT_EN);
 	POSTING_READ(intel_dp->output_reg);
 	msleep(intel_dp->panel_power_down_delay);
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index a1b4343814e8..1348705faf6b 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -26,6 +26,7 @@
 #define __INTEL_DRV_H__
 
 #include <linux/i2c.h>
+#include "i915_drm.h"
 #include "i915_drv.h"
 #include "drm_crtc.h"
 #include "drm_crtc_helper.h"
@@ -39,7 +40,7 @@
 			ret__ = -ETIMEDOUT;				\
 			break;						\
 		}							\
-		if (W && !(in_atomic() || in_dbg_master())) msleep(W);	\
+		if (W && drm_can_sleep()) msleep(W);	\
 	}								\
 	ret__;								\
 })
@@ -47,13 +48,6 @@
 #define wait_for(COND, MS) _wait_for(COND, MS, 1)
 #define wait_for_atomic(COND, MS) _wait_for(COND, MS, 0)
 
-#define MSLEEP(x) do { \
-	if (in_dbg_master()) \
-		mdelay(x); \
-	else \
-		msleep(x); \
-} while (0)
-
 #define KHz(x) (1000*x)
 #define MHz(x) KHz(1000*x)
 
@@ -177,10 +171,32 @@ struct intel_crtc {
 	bool use_pll_a;
 };
 
+struct intel_plane {
+	struct drm_plane base;
+	enum pipe pipe;
+	struct drm_i915_gem_object *obj;
+	bool primary_disabled;
+	int max_downscale;
+	u32 lut_r[1024], lut_g[1024], lut_b[1024];
+	void (*update_plane)(struct drm_plane *plane,
+			     struct drm_framebuffer *fb,
+			     struct drm_i915_gem_object *obj,
+			     int crtc_x, int crtc_y,
+			     unsigned int crtc_w, unsigned int crtc_h,
+			     uint32_t x, uint32_t y,
+			     uint32_t src_w, uint32_t src_h);
+	void (*disable_plane)(struct drm_plane *plane);
+	int (*update_colorkey)(struct drm_plane *plane,
+			       struct drm_intel_sprite_colorkey *key);
+	void (*get_colorkey)(struct drm_plane *plane,
+			     struct drm_intel_sprite_colorkey *key);
+};
+
 #define to_intel_crtc(x) container_of(x, struct intel_crtc, base)
 #define to_intel_connector(x) container_of(x, struct intel_connector, base)
 #define to_intel_encoder(x) container_of(x, struct intel_encoder, base)
 #define to_intel_framebuffer(x) container_of(x, struct intel_framebuffer, base)
+#define to_intel_plane(x) container_of(x, struct intel_plane, base)
 
 #define DIP_HEADER_SIZE	5
 
@@ -290,6 +306,7 @@ intel_dp_set_m_n(struct drm_crtc *crtc, struct drm_display_mode *mode,
 extern bool intel_dpd_is_edp(struct drm_device *dev);
 extern void intel_edp_link_config(struct intel_encoder *, int *, int *);
 extern bool intel_encoder_is_pch_edp(struct drm_encoder *encoder);
+extern int intel_plane_init(struct drm_device *dev, enum pipe pipe);
 
 /* intel_panel.c */
 extern void intel_fixed_panel_mode(struct drm_display_mode *fixed_mode,
@@ -360,7 +377,7 @@ extern int intel_pin_and_fence_fb_obj(struct drm_device *dev,
 
 extern int intel_framebuffer_init(struct drm_device *dev,
 				  struct intel_framebuffer *ifb,
-				  struct drm_mode_fb_cmd *mode_cmd,
+				  struct drm_mode_fb_cmd2 *mode_cmd,
 				  struct drm_i915_gem_object *obj);
 extern int intel_fbdev_init(struct drm_device *dev);
 extern void intel_fbdev_fini(struct drm_device *dev);
@@ -380,9 +397,25 @@ extern int intel_overlay_attrs(struct drm_device *dev, void *data,
 extern void intel_fb_output_poll_changed(struct drm_device *dev);
 extern void intel_fb_restore_mode(struct drm_device *dev);
 
+extern void assert_pipe(struct drm_i915_private *dev_priv, enum pipe pipe,
+			bool state);
+#define assert_pipe_enabled(d, p) assert_pipe(d, p, true)
+#define assert_pipe_disabled(d, p) assert_pipe(d, p, false)
+
 extern void intel_init_clock_gating(struct drm_device *dev);
 extern void intel_write_eld(struct drm_encoder *encoder,
 			    struct drm_display_mode *mode);
 extern void intel_cpt_verify_modeset(struct drm_device *dev, int pipe);
 
+/* For use by IVB LP watermark workaround in intel_sprite.c */
+extern void sandybridge_update_wm(struct drm_device *dev);
+extern void intel_update_sprite_watermarks(struct drm_device *dev, int pipe,
+					   uint32_t sprite_width,
+					   int pixel_size);
+
+extern int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
+				     struct drm_file *file_priv);
+extern int intel_sprite_get_colorkey(struct drm_device *dev, void *data,
+				     struct drm_file *file_priv);
+
 #endif /* __INTEL_DRV_H__ */
diff --git a/drivers/gpu/drm/i915/intel_fb.c b/drivers/gpu/drm/i915/intel_fb.c
index ec49bae73382..571375a3eef4 100644
--- a/drivers/gpu/drm/i915/intel_fb.c
+++ b/drivers/gpu/drm/i915/intel_fb.c
@@ -65,7 +65,7 @@ static int intelfb_create(struct intel_fbdev *ifbdev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct fb_info *info;
 	struct drm_framebuffer *fb;
-	struct drm_mode_fb_cmd mode_cmd;
+	struct drm_mode_fb_cmd2 mode_cmd;
 	struct drm_i915_gem_object *obj;
 	struct device *device = &dev->pdev->dev;
 	int size, ret;
@@ -77,11 +77,12 @@ static int intelfb_create(struct intel_fbdev *ifbdev,
 	mode_cmd.width = sizes->surface_width;
 	mode_cmd.height = sizes->surface_height;
 
-	mode_cmd.bpp = sizes->surface_bpp;
-	mode_cmd.pitch = ALIGN(mode_cmd.width * ((mode_cmd.bpp + 7) / 8), 64);
-	mode_cmd.depth = sizes->surface_depth;
+	mode_cmd.pitches[0] = ALIGN(mode_cmd.width * ((sizes->surface_bpp + 7) /
+						      8), 64);
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
 
-	size = mode_cmd.pitch * mode_cmd.height;
+	size = mode_cmd.pitches[0] * mode_cmd.height;
 	size = ALIGN(size, PAGE_SIZE);
 	obj = i915_gem_alloc_object(dev, size);
 	if (!obj) {
@@ -148,7 +149,7 @@ static int intelfb_create(struct intel_fbdev *ifbdev,
 
 //	memset(info->screen_base, 0, size);
 
-	drm_fb_helper_fill_fix(info, fb->pitch, fb->depth);
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
 	drm_fb_helper_fill_var(info, &ifbdev->helper, sizes->fb_width, sizes->fb_height);
 
 	info->pixmap.size = 64*1024;
@@ -269,8 +270,14 @@ void intel_fb_restore_mode(struct drm_device *dev)
 {
 	int ret;
 	drm_i915_private_t *dev_priv = dev->dev_private;
+	struct drm_mode_config *config = &dev->mode_config;
+	struct drm_plane *plane;
 
 	ret = drm_fb_helper_restore_fbdev_mode(&dev_priv->fbdev->helper);
 	if (ret)
 		DRM_DEBUG("failed to restore crtc mode\n");
+
+	/* Be sure to shut off any planes that may be active */
+	list_for_each_entry(plane, &config->plane_list, head)
+		plane->funcs->disable_plane(plane);
 }
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index d4f5a0b2120d..64541f7ef900 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -269,6 +269,10 @@ static void intel_hdmi_dpms(struct drm_encoder *encoder, int mode)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
 	u32 temp;
+	u32 enable_bits = SDVO_ENABLE;
+
+	if (intel_hdmi->has_audio)
+		enable_bits |= SDVO_AUDIO_ENABLE;
 
 	temp = I915_READ(intel_hdmi->sdvox_reg);
 
@@ -281,9 +285,9 @@ static void intel_hdmi_dpms(struct drm_encoder *encoder, int mode)
 	}
 
 	if (mode != DRM_MODE_DPMS_ON) {
-		temp &= ~SDVO_ENABLE;
+		temp &= ~enable_bits;
 	} else {
-		temp |= SDVO_ENABLE;
+		temp |= enable_bits;
 	}
 
 	I915_WRITE(intel_hdmi->sdvox_reg, temp);
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index ca70e2f10445..77e729d4e4f0 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -414,6 +414,11 @@ static int init_render_ring(struct intel_ring_buffer *ring)
 			return ret;
 	}
 
+	if (INTEL_INFO(dev)->gen >= 6) {
+		I915_WRITE(INSTPM,
+			   INSTPM_FORCE_ORDERING << 16 | INSTPM_FORCE_ORDERING);
+	}
+
 	return ret;
 }
 
@@ -787,6 +792,17 @@ ring_add_request(struct intel_ring_buffer *ring,
 }
 
 static bool
+gen7_blt_ring_get_irq(struct intel_ring_buffer *ring)
+{
+	/* The BLT ring on IVB appears to have broken synchronization
+	 * between the seqno write and the interrupt, so that the
+	 * interrupt appears first.  Returning false here makes
+	 * i915_wait_request() do a polling loop, instead.
+	 */
+	return false;
+}
+
+static bool
 gen6_ring_get_irq(struct intel_ring_buffer *ring, u32 gflag, u32 rflag)
 {
 	struct drm_device *dev = ring->dev;
@@ -1119,7 +1135,16 @@ int intel_wait_ring_buffer(struct intel_ring_buffer *ring, int n)
 	}
 
 	trace_i915_ring_wait_begin(ring);
-	end = jiffies + 3 * HZ;
+	if (drm_core_check_feature(dev, DRIVER_GEM))
+		/* With GEM the hangcheck timer should kick us out of the loop,
+		 * leaving it early runs the risk of corrupting GEM state (due
+		 * to running on almost untested codepaths). But on resume
+		 * timers don't work yet, so prevent a complete hang in that
+		 * case by choosing an insanely large timeout. */
+		end = jiffies + 60 * HZ;
+	else
+		end = jiffies + 3 * HZ;
+
 	do {
 		ring->head = I915_READ_HEAD(ring);
 		ring->space = ring_space(ring);
@@ -1552,5 +1577,8 @@ int intel_init_blt_ring_buffer(struct drm_device *dev)
 
 	*ring = gen6_blt_ring;
 
+	if (IS_GEN7(dev))
+		ring->irq_get = gen7_blt_ring_get_irq;
+
 	return intel_init_ring_buffer(dev, ring);
 }
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
new file mode 100644
index 000000000000..d13989fda501
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -0,0 +1,668 @@
+/*
+ * Copyright © 2011 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Authors:
+ *   Jesse Barnes <jbarnes@virtuousgeek.org>
+ *
+ * New plane/sprite handling.
+ *
+ * The older chips had a separate interface for programming plane related
+ * registers; newer ones are much simpler and we can use the new DRM plane
+ * support.
+ */
+#include "drmP.h"
+#include "drm_crtc.h"
+#include "drm_fourcc.h"
+#include "intel_drv.h"
+#include "i915_drm.h"
+#include "i915_drv.h"
+
+static void
+ivb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
+		 struct drm_i915_gem_object *obj, int crtc_x, int crtc_y,
+		 unsigned int crtc_w, unsigned int crtc_h,
+		 uint32_t x, uint32_t y,
+		 uint32_t src_w, uint32_t src_h)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	int pipe = intel_plane->pipe;
+	u32 sprctl, sprscale = 0;
+	int pixel_size;
+
+	sprctl = I915_READ(SPRCTL(pipe));
+
+	/* Mask out pixel format bits in case we change it */
+	sprctl &= ~SPRITE_PIXFORMAT_MASK;
+	sprctl &= ~SPRITE_RGB_ORDER_RGBX;
+	sprctl &= ~SPRITE_YUV_BYTE_ORDER_MASK;
+
+	switch (fb->pixel_format) {
+	case DRM_FORMAT_XBGR8888:
+		sprctl |= SPRITE_FORMAT_RGBX888;
+		pixel_size = 4;
+		break;
+	case DRM_FORMAT_XRGB8888:
+		sprctl |= SPRITE_FORMAT_RGBX888 | SPRITE_RGB_ORDER_RGBX;
+		pixel_size = 4;
+		break;
+	case DRM_FORMAT_YUYV:
+		sprctl |= SPRITE_FORMAT_YUV422 | SPRITE_YUV_ORDER_YUYV;
+		pixel_size = 2;
+		break;
+	case DRM_FORMAT_YVYU:
+		sprctl |= SPRITE_FORMAT_YUV422 | SPRITE_YUV_ORDER_YVYU;
+		pixel_size = 2;
+		break;
+	case DRM_FORMAT_UYVY:
+		sprctl |= SPRITE_FORMAT_YUV422 | SPRITE_YUV_ORDER_UYVY;
+		pixel_size = 2;
+		break;
+	case DRM_FORMAT_VYUY:
+		sprctl |= SPRITE_FORMAT_YUV422 | SPRITE_YUV_ORDER_VYUY;
+		pixel_size = 2;
+		break;
+	default:
+		DRM_DEBUG_DRIVER("bad pixel format, assuming RGBX888\n");
+		sprctl |= DVS_FORMAT_RGBX888;
+		pixel_size = 4;
+		break;
+	}
+
+	if (obj->tiling_mode != I915_TILING_NONE)
+		sprctl |= SPRITE_TILED;
+
+	/* must disable */
+	sprctl |= SPRITE_TRICKLE_FEED_DISABLE;
+	sprctl |= SPRITE_ENABLE;
+	sprctl |= SPRITE_DEST_KEY;
+
+	/* Sizes are 0 based */
+	src_w--;
+	src_h--;
+	crtc_w--;
+	crtc_h--;
+
+	intel_update_sprite_watermarks(dev, pipe, crtc_w, pixel_size);
+
+	/*
+	 * IVB workaround: must disable low power watermarks for at least
+	 * one frame before enabling scaling.  LP watermarks can be re-enabled
+	 * when scaling is disabled.
+	 */
+	if (crtc_w != src_w || crtc_h != src_h) {
+		dev_priv->sprite_scaling_enabled = true;
+		sandybridge_update_wm(dev);
+		intel_wait_for_vblank(dev, pipe);
+		sprscale = SPRITE_SCALE_ENABLE | (src_w << 16) | src_h;
+	} else {
+		dev_priv->sprite_scaling_enabled = false;
+		/* potentially re-enable LP watermarks */
+		sandybridge_update_wm(dev);
+	}
+
+	I915_WRITE(SPRSTRIDE(pipe), fb->pitches[0]);
+	I915_WRITE(SPRPOS(pipe), (crtc_y << 16) | crtc_x);
+	if (obj->tiling_mode != I915_TILING_NONE) {
+		I915_WRITE(SPRTILEOFF(pipe), (y << 16) | x);
+	} else {
+		unsigned long offset;
+
+		offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
+		I915_WRITE(SPRLINOFF(pipe), offset);
+	}
+	I915_WRITE(SPRSIZE(pipe), (crtc_h << 16) | crtc_w);
+	I915_WRITE(SPRSCALE(pipe), sprscale);
+	I915_WRITE(SPRCTL(pipe), sprctl);
+	I915_WRITE(SPRSURF(pipe), obj->gtt_offset);
+	POSTING_READ(SPRSURF(pipe));
+}
+
+static void
+ivb_disable_plane(struct drm_plane *plane)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	int pipe = intel_plane->pipe;
+
+	I915_WRITE(SPRCTL(pipe), I915_READ(SPRCTL(pipe)) & ~SPRITE_ENABLE);
+	/* Can't leave the scaler enabled... */
+	I915_WRITE(SPRSCALE(pipe), 0);
+	/* Activate double buffered register update */
+	I915_WRITE(SPRSURF(pipe), 0);
+	POSTING_READ(SPRSURF(pipe));
+}
+
+static int
+ivb_update_colorkey(struct drm_plane *plane,
+		    struct drm_intel_sprite_colorkey *key)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane;
+	u32 sprctl;
+	int ret = 0;
+
+	intel_plane = to_intel_plane(plane);
+
+	I915_WRITE(SPRKEYVAL(intel_plane->pipe), key->min_value);
+	I915_WRITE(SPRKEYMAX(intel_plane->pipe), key->max_value);
+	I915_WRITE(SPRKEYMSK(intel_plane->pipe), key->channel_mask);
+
+	sprctl = I915_READ(SPRCTL(intel_plane->pipe));
+	sprctl &= ~(SPRITE_SOURCE_KEY | SPRITE_DEST_KEY);
+	if (key->flags & I915_SET_COLORKEY_DESTINATION)
+		sprctl |= SPRITE_DEST_KEY;
+	else if (key->flags & I915_SET_COLORKEY_SOURCE)
+		sprctl |= SPRITE_SOURCE_KEY;
+	I915_WRITE(SPRCTL(intel_plane->pipe), sprctl);
+
+	POSTING_READ(SPRKEYMSK(intel_plane->pipe));
+
+	return ret;
+}
+
+static void
+ivb_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane;
+	u32 sprctl;
+
+	intel_plane = to_intel_plane(plane);
+
+	key->min_value = I915_READ(SPRKEYVAL(intel_plane->pipe));
+	key->max_value = I915_READ(SPRKEYMAX(intel_plane->pipe));
+	key->channel_mask = I915_READ(SPRKEYMSK(intel_plane->pipe));
+	key->flags = 0;
+
+	sprctl = I915_READ(SPRCTL(intel_plane->pipe));
+
+	if (sprctl & SPRITE_DEST_KEY)
+		key->flags = I915_SET_COLORKEY_DESTINATION;
+	else if (sprctl & SPRITE_SOURCE_KEY)
+		key->flags = I915_SET_COLORKEY_SOURCE;
+	else
+		key->flags = I915_SET_COLORKEY_NONE;
+}
+
+static void
+snb_update_plane(struct drm_plane *plane, struct drm_framebuffer *fb,
+		 struct drm_i915_gem_object *obj, int crtc_x, int crtc_y,
+		 unsigned int crtc_w, unsigned int crtc_h,
+		 uint32_t x, uint32_t y,
+		 uint32_t src_w, uint32_t src_h)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	int pipe = intel_plane->pipe, pixel_size;
+	u32 dvscntr, dvsscale = 0;
+
+	dvscntr = I915_READ(DVSCNTR(pipe));
+
+	/* Mask out pixel format bits in case we change it */
+	dvscntr &= ~DVS_PIXFORMAT_MASK;
+	dvscntr &= ~DVS_RGB_ORDER_RGBX;
+	dvscntr &= ~DVS_YUV_BYTE_ORDER_MASK;
+
+	switch (fb->pixel_format) {
+	case DRM_FORMAT_XBGR8888:
+		dvscntr |= DVS_FORMAT_RGBX888;
+		pixel_size = 4;
+		break;
+	case DRM_FORMAT_XRGB8888:
+		dvscntr |= DVS_FORMAT_RGBX888 | DVS_RGB_ORDER_RGBX;
+		pixel_size = 4;
+		break;
+	case DRM_FORMAT_YUYV:
+		dvscntr |= DVS_FORMAT_YUV422 | DVS_YUV_ORDER_YUYV;
+		pixel_size = 2;
+		break;
+	case DRM_FORMAT_YVYU:
+		dvscntr |= DVS_FORMAT_YUV422 | DVS_YUV_ORDER_YVYU;
+		pixel_size = 2;
+		break;
+	case DRM_FORMAT_UYVY:
+		dvscntr |= DVS_FORMAT_YUV422 | DVS_YUV_ORDER_UYVY;
+		pixel_size = 2;
+		break;
+	case DRM_FORMAT_VYUY:
+		dvscntr |= DVS_FORMAT_YUV422 | DVS_YUV_ORDER_VYUY;
+		pixel_size = 2;
+		break;
+	default:
+		DRM_DEBUG_DRIVER("bad pixel format, assuming RGBX888\n");
+		dvscntr |= DVS_FORMAT_RGBX888;
+		pixel_size = 4;
+		break;
+	}
+
+	if (obj->tiling_mode != I915_TILING_NONE)
+		dvscntr |= DVS_TILED;
+
+	/* must disable */
+	dvscntr |= DVS_TRICKLE_FEED_DISABLE;
+	dvscntr |= DVS_ENABLE;
+
+	/* Sizes are 0 based */
+	src_w--;
+	src_h--;
+	crtc_w--;
+	crtc_h--;
+
+	intel_update_sprite_watermarks(dev, pipe, crtc_w, pixel_size);
+
+	if (crtc_w != src_w || crtc_h != src_h)
+		dvsscale = DVS_SCALE_ENABLE | (src_w << 16) | src_h;
+
+	I915_WRITE(DVSSTRIDE(pipe), fb->pitches[0]);
+	I915_WRITE(DVSPOS(pipe), (crtc_y << 16) | crtc_x);
+	if (obj->tiling_mode != I915_TILING_NONE) {
+		I915_WRITE(DVSTILEOFF(pipe), (y << 16) | x);
+	} else {
+		unsigned long offset;
+
+		offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
+		I915_WRITE(DVSLINOFF(pipe), offset);
+	}
+	I915_WRITE(DVSSIZE(pipe), (crtc_h << 16) | crtc_w);
+	I915_WRITE(DVSSCALE(pipe), dvsscale);
+	I915_WRITE(DVSCNTR(pipe), dvscntr);
+	I915_WRITE(DVSSURF(pipe), obj->gtt_offset);
+	POSTING_READ(DVSSURF(pipe));
+}
+
+static void
+snb_disable_plane(struct drm_plane *plane)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	int pipe = intel_plane->pipe;
+
+	I915_WRITE(DVSCNTR(pipe), I915_READ(DVSCNTR(pipe)) & ~DVS_ENABLE);
+	/* Disable the scaler */
+	I915_WRITE(DVSSCALE(pipe), 0);
+	/* Flush double buffered register updates */
+	I915_WRITE(DVSSURF(pipe), 0);
+	POSTING_READ(DVSSURF(pipe));
+}
+
+static void
+intel_enable_primary(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int reg = DSPCNTR(intel_crtc->plane);
+
+	I915_WRITE(reg, I915_READ(reg) | DISPLAY_PLANE_ENABLE);
+}
+
+static void
+intel_disable_primary(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int reg = DSPCNTR(intel_crtc->plane);
+
+	I915_WRITE(reg, I915_READ(reg) & ~DISPLAY_PLANE_ENABLE);
+}
+
+static int
+snb_update_colorkey(struct drm_plane *plane,
+		    struct drm_intel_sprite_colorkey *key)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane;
+	u32 dvscntr;
+	int ret = 0;
+
+	intel_plane = to_intel_plane(plane);
+
+	I915_WRITE(DVSKEYVAL(intel_plane->pipe), key->min_value);
+	I915_WRITE(DVSKEYMAX(intel_plane->pipe), key->max_value);
+	I915_WRITE(DVSKEYMSK(intel_plane->pipe), key->channel_mask);
+
+	dvscntr = I915_READ(DVSCNTR(intel_plane->pipe));
+	dvscntr &= ~(DVS_SOURCE_KEY | DVS_DEST_KEY);
+	if (key->flags & I915_SET_COLORKEY_DESTINATION)
+		dvscntr |= DVS_DEST_KEY;
+	else if (key->flags & I915_SET_COLORKEY_SOURCE)
+		dvscntr |= DVS_SOURCE_KEY;
+	I915_WRITE(DVSCNTR(intel_plane->pipe), dvscntr);
+
+	POSTING_READ(DVSKEYMSK(intel_plane->pipe));
+
+	return ret;
+}
+
+static void
+snb_get_colorkey(struct drm_plane *plane, struct drm_intel_sprite_colorkey *key)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_plane *intel_plane;
+	u32 dvscntr;
+
+	intel_plane = to_intel_plane(plane);
+
+	key->min_value = I915_READ(DVSKEYVAL(intel_plane->pipe));
+	key->max_value = I915_READ(DVSKEYMAX(intel_plane->pipe));
+	key->channel_mask = I915_READ(DVSKEYMSK(intel_plane->pipe));
+	key->flags = 0;
+
+	dvscntr = I915_READ(DVSCNTR(intel_plane->pipe));
+
+	if (dvscntr & DVS_DEST_KEY)
+		key->flags = I915_SET_COLORKEY_DESTINATION;
+	else if (dvscntr & DVS_SOURCE_KEY)
+		key->flags = I915_SET_COLORKEY_SOURCE;
+	else
+		key->flags = I915_SET_COLORKEY_NONE;
+}
+
+static int
+intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
+		   struct drm_framebuffer *fb, int crtc_x, int crtc_y,
+		   unsigned int crtc_w, unsigned int crtc_h,
+		   uint32_t src_x, uint32_t src_y,
+		   uint32_t src_w, uint32_t src_h)
+{
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	struct intel_framebuffer *intel_fb;
+	struct drm_i915_gem_object *obj, *old_obj;
+	int pipe = intel_plane->pipe;
+	int ret = 0;
+	int x = src_x >> 16, y = src_y >> 16;
+	int primary_w = crtc->mode.hdisplay, primary_h = crtc->mode.vdisplay;
+	bool disable_primary = false;
+
+	intel_fb = to_intel_framebuffer(fb);
+	obj = intel_fb->obj;
+
+	old_obj = intel_plane->obj;
+
+	/* Pipe must be running... */
+	if (!(I915_READ(PIPECONF(pipe)) & PIPECONF_ENABLE))
+		return -EINVAL;
+
+	if (crtc_x >= primary_w || crtc_y >= primary_h)
+		return -EINVAL;
+
+	/* Don't modify another pipe's plane */
+	if (intel_plane->pipe != intel_crtc->pipe)
+		return -EINVAL;
+
+	/*
+	 * Clamp the width & height into the visible area.  Note we don't
+	 * try to scale the source if part of the visible region is offscreen.
+	 * The caller must handle that by adjusting source offset and size.
+	 */
+	if ((crtc_x < 0) && ((crtc_x + crtc_w) > 0)) {
+		crtc_w += crtc_x;
+		crtc_x = 0;
+	}
+	if ((crtc_x + crtc_w) <= 0) /* Nothing to display */
+		goto out;
+	if ((crtc_x + crtc_w) > primary_w)
+		crtc_w = primary_w - crtc_x;
+
+	if ((crtc_y < 0) && ((crtc_y + crtc_h) > 0)) {
+		crtc_h += crtc_y;
+		crtc_y = 0;
+	}
+	if ((crtc_y + crtc_h) <= 0) /* Nothing to display */
+		goto out;
+	if (crtc_y + crtc_h > primary_h)
+		crtc_h = primary_h - crtc_y;
+
+	if (!crtc_w || !crtc_h) /* Again, nothing to display */
+		goto out;
+
+	/*
+	 * We can take a larger source and scale it down, but
+	 * only so much...  16x is the max on SNB.
+	 */
+	if (((src_w * src_h) / (crtc_w * crtc_h)) > intel_plane->max_downscale)
+		return -EINVAL;
+
+	/*
+	 * If the sprite is completely covering the primary plane,
+	 * we can disable the primary and save power.
+	 */
+	if ((crtc_x == 0) && (crtc_y == 0) &&
+	    (crtc_w == primary_w) && (crtc_h == primary_h))
+		disable_primary = true;
+
+	mutex_lock(&dev->struct_mutex);
+
+	ret = intel_pin_and_fence_fb_obj(dev, obj, NULL);
+	if (ret) {
+		DRM_ERROR("failed to pin object\n");
+		goto out_unlock;
+	}
+
+	intel_plane->obj = obj;
+
+	/*
+	 * Be sure to re-enable the primary before the sprite is no longer
+	 * covering it fully.
+	 */
+	if (!disable_primary && intel_plane->primary_disabled) {
+		intel_enable_primary(crtc);
+		intel_plane->primary_disabled = false;
+	}
+
+	intel_plane->update_plane(plane, fb, obj, crtc_x, crtc_y,
+				  crtc_w, crtc_h, x, y, src_w, src_h);
+
+	if (disable_primary) {
+		intel_disable_primary(crtc);
+		intel_plane->primary_disabled = true;
+	}
+
+	/* Unpin old obj after new one is active to avoid ugliness */
+	if (old_obj) {
+		/*
+		 * It's fairly common to simply update the position of
+		 * an existing object.  In that case, we don't need to
+		 * wait for vblank to avoid ugliness, we only need to
+		 * do the pin & ref bookkeeping.
+		 */
+		if (old_obj != obj) {
+			mutex_unlock(&dev->struct_mutex);
+			intel_wait_for_vblank(dev, to_intel_crtc(crtc)->pipe);
+			mutex_lock(&dev->struct_mutex);
+		}
+		i915_gem_object_unpin(old_obj);
+	}
+
+out_unlock:
+	mutex_unlock(&dev->struct_mutex);
+out:
+	return ret;
+}
+
+static int
+intel_disable_plane(struct drm_plane *plane)
+{
+	struct drm_device *dev = plane->dev;
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	int ret = 0;
+
+	if (intel_plane->primary_disabled) {
+		intel_enable_primary(plane->crtc);
+		intel_plane->primary_disabled = false;
+	}
+
+	intel_plane->disable_plane(plane);
+
+	if (!intel_plane->obj)
+		goto out;
+
+	mutex_lock(&dev->struct_mutex);
+	i915_gem_object_unpin(intel_plane->obj);
+	intel_plane->obj = NULL;
+	mutex_unlock(&dev->struct_mutex);
+out:
+
+	return ret;
+}
+
+static void intel_destroy_plane(struct drm_plane *plane)
+{
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	intel_disable_plane(plane);
+	drm_plane_cleanup(plane);
+	kfree(intel_plane);
+}
+
+int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
+			      struct drm_file *file_priv)
+{
+	struct drm_intel_sprite_colorkey *set = data;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_mode_object *obj;
+	struct drm_plane *plane;
+	struct intel_plane *intel_plane;
+	int ret = 0;
+
+	if (!dev_priv)
+		return -EINVAL;
+
+	/* Make sure we don't try to enable both src & dest simultaneously */
+	if ((set->flags & (I915_SET_COLORKEY_DESTINATION | I915_SET_COLORKEY_SOURCE)) == (I915_SET_COLORKEY_DESTINATION | I915_SET_COLORKEY_SOURCE))
+		return -EINVAL;
+
+	mutex_lock(&dev->mode_config.mutex);
+
+	obj = drm_mode_object_find(dev, set->plane_id, DRM_MODE_OBJECT_PLANE);
+	if (!obj) {
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	plane = obj_to_plane(obj);
+	intel_plane = to_intel_plane(plane);
+	ret = intel_plane->update_colorkey(plane, set);
+
+out_unlock:
+	mutex_unlock(&dev->mode_config.mutex);
+	return ret;
+}
+
+int intel_sprite_get_colorkey(struct drm_device *dev, void *data,
+			      struct drm_file *file_priv)
+{
+	struct drm_intel_sprite_colorkey *get = data;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_mode_object *obj;
+	struct drm_plane *plane;
+	struct intel_plane *intel_plane;
+	int ret = 0;
+
+	if (!dev_priv)
+		return -EINVAL;
+
+	mutex_lock(&dev->mode_config.mutex);
+
+	obj = drm_mode_object_find(dev, get->plane_id, DRM_MODE_OBJECT_PLANE);
+	if (!obj) {
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	plane = obj_to_plane(obj);
+	intel_plane = to_intel_plane(plane);
+	intel_plane->get_colorkey(plane, get);
+
+out_unlock:
+	mutex_unlock(&dev->mode_config.mutex);
+	return ret;
+}
+
+static const struct drm_plane_funcs intel_plane_funcs = {
+	.update_plane = intel_update_plane,
+	.disable_plane = intel_disable_plane,
+	.destroy = intel_destroy_plane,
+};
+
+static uint32_t snb_plane_formats[] = {
+	DRM_FORMAT_XBGR8888,
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_YUYV,
+	DRM_FORMAT_YVYU,
+	DRM_FORMAT_UYVY,
+	DRM_FORMAT_VYUY,
+};
+
+int
+intel_plane_init(struct drm_device *dev, enum pipe pipe)
+{
+	struct intel_plane *intel_plane;
+	unsigned long possible_crtcs;
+	int ret;
+
+	if (!(IS_GEN6(dev) || IS_GEN7(dev))) {
+		DRM_ERROR("new plane code only for SNB+\n");
+		return -ENODEV;
+	}
+
+	intel_plane = kzalloc(sizeof(struct intel_plane), GFP_KERNEL);
+	if (!intel_plane)
+		return -ENOMEM;
+
+	if (IS_GEN6(dev)) {
+		intel_plane->max_downscale = 16;
+		intel_plane->update_plane = snb_update_plane;
+		intel_plane->disable_plane = snb_disable_plane;
+		intel_plane->update_colorkey = snb_update_colorkey;
+		intel_plane->get_colorkey = snb_get_colorkey;
+	} else if (IS_GEN7(dev)) {
+		intel_plane->max_downscale = 2;
+		intel_plane->update_plane = ivb_update_plane;
+		intel_plane->disable_plane = ivb_disable_plane;
+		intel_plane->update_colorkey = ivb_update_colorkey;
+		intel_plane->get_colorkey = ivb_get_colorkey;
+	}
+
+	intel_plane->pipe = pipe;
+	possible_crtcs = (1 << pipe);
+	ret = drm_plane_init(dev, &intel_plane->base, possible_crtcs,
+			     &intel_plane_funcs, snb_plane_formats,
+			     ARRAY_SIZE(snb_plane_formats), false);
+	if (ret)
+		kfree(intel_plane);
+
+	return ret;
+}
+
diff --git a/drivers/gpu/drm/mga/mga_drv.c b/drivers/gpu/drm/mga/mga_drv.c
index 33daa29eea66..f9a925d58819 100644
--- a/drivers/gpu/drm/mga/mga_drv.c
+++ b/drivers/gpu/drm/mga/mga_drv.c
@@ -44,6 +44,20 @@ static struct pci_device_id pciidlist[] = {
 	mga_PCI_IDS
 };
 
+static const struct file_operations mga_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = mga_compat_ioctl,
+#endif
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_USE_MTRR | DRIVER_PCI_DMA |
@@ -64,20 +78,7 @@ static struct drm_driver driver = {
 	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = mga_ioctls,
 	.dma_ioctl = mga_dma_buffers,
-	.fops = {
-		.owner = THIS_MODULE,
-		.open = drm_open,
-		.release = drm_release,
-		.unlocked_ioctl = drm_ioctl,
-		.mmap = drm_mmap,
-		.poll = drm_poll,
-		.fasync = drm_fasync,
-#ifdef CONFIG_COMPAT
-		.compat_ioctl = mga_compat_ioctl,
-#endif
-		.llseek = noop_llseek,
-	},
-
+	.fops = &mga_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/nouveau/Makefile b/drivers/gpu/drm/nouveau/Makefile
index 35ef5b1e3566..9f27e3d9e69a 100644
--- a/drivers/gpu/drm/nouveau/Makefile
+++ b/drivers/gpu/drm/nouveau/Makefile
@@ -9,9 +9,9 @@ nouveau-y := nouveau_drv.o nouveau_state.o nouveau_channel.o nouveau_mem.o \
              nouveau_bo.o nouveau_fence.o nouveau_gem.o nouveau_ttm.o \
              nouveau_hw.o nouveau_calc.o nouveau_bios.o nouveau_i2c.o \
              nouveau_display.o nouveau_connector.o nouveau_fbcon.o \
-             nouveau_dp.o nouveau_ramht.o \
+             nouveau_hdmi.o nouveau_dp.o nouveau_ramht.o \
 	     nouveau_pm.o nouveau_volt.o nouveau_perf.o nouveau_temp.o \
-	     nouveau_mm.o nouveau_vm.o \
+	     nouveau_mm.o nouveau_vm.o nouveau_mxm.o nouveau_gpio.o \
              nv04_timer.o \
              nv04_mc.o nv40_mc.o nv50_mc.o \
              nv04_fb.o nv10_fb.o nv30_fb.o nv40_fb.o nv50_fb.o nvc0_fb.o \
@@ -19,9 +19,12 @@ nouveau-y := nouveau_drv.o nouveau_state.o nouveau_channel.o nouveau_mem.o \
              nv04_graph.o nv10_graph.o nv20_graph.o \
              nv40_graph.o nv50_graph.o nvc0_graph.o \
              nv40_grctx.o nv50_grctx.o nvc0_grctx.o \
-             nv84_crypt.o \
+             nv84_crypt.o nv98_crypt.o \
              nva3_copy.o nvc0_copy.o \
              nv31_mpeg.o nv50_mpeg.o \
+             nv84_bsp.o \
+             nv84_vp.o \
+             nv98_ppp.o \
              nv04_instmem.o nv50_instmem.o nvc0_instmem.o \
              nv04_dac.o nv04_dfp.o nv04_tv.o nv17_tv.o nv17_tv_modes.o \
              nv04_crtc.o nv04_display.o nv04_cursor.o \
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.c b/drivers/gpu/drm/nouveau/nouveau_bios.c
index 5fc201b49d30..e5cbead85e50 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.c
@@ -27,6 +27,7 @@
 #include "nouveau_drv.h"
 #include "nouveau_hw.h"
 #include "nouveau_encoder.h"
+#include "nouveau_gpio.h"
 
 #include <linux/io-mapping.h>
 
@@ -34,9 +35,6 @@
 #define NV_CIO_CRE_44_HEADA 0x0
 #define NV_CIO_CRE_44_HEADB 0x3
 #define FEATURE_MOBILE 0x10	/* also FEATURE_QUADRO for BMP */
-#define LEGACY_I2C_CRT 0x80
-#define LEGACY_I2C_PANEL 0x81
-#define LEGACY_I2C_TV 0x82
 
 #define EDID1_LEN 128
 
@@ -723,115 +721,19 @@ static int dcb_entry_idx_from_crtchead(struct drm_device *dev)
 	return dcb_entry;
 }
 
-static int
-read_dcb_i2c_entry(struct drm_device *dev, int dcb_version, uint8_t *i2ctable, int index, struct dcb_i2c_entry *i2c)
-{
-	uint8_t dcb_i2c_ver = dcb_version, headerlen = 0, entry_len = 4;
-	int i2c_entries = DCB_MAX_NUM_I2C_ENTRIES;
-	int recordoffset = 0, rdofs = 1, wrofs = 0;
-	uint8_t port_type = 0;
-
-	if (!i2ctable)
-		return -EINVAL;
-
-	if (dcb_version >= 0x30) {
-		if (i2ctable[0] != dcb_version) /* necessary? */
-			NV_WARN(dev,
-				"DCB I2C table version mismatch (%02X vs %02X)\n",
-				i2ctable[0], dcb_version);
-		dcb_i2c_ver = i2ctable[0];
-		headerlen = i2ctable[1];
-		if (i2ctable[2] <= DCB_MAX_NUM_I2C_ENTRIES)
-			i2c_entries = i2ctable[2];
-		else
-			NV_WARN(dev,
-				"DCB I2C table has more entries than indexable "
-				"(%d entries, max %d)\n", i2ctable[2],
-				DCB_MAX_NUM_I2C_ENTRIES);
-		entry_len = i2ctable[3];
-		/* [4] is i2c_default_indices, read in parse_dcb_table() */
-	}
-	/*
-	 * It's your own fault if you call this function on a DCB 1.1 BIOS --
-	 * the test below is for DCB 1.2
-	 */
-	if (dcb_version < 0x14) {
-		recordoffset = 2;
-		rdofs = 0;
-		wrofs = 1;
-	}
-
-	if (index == 0xf)
-		return 0;
-	if (index >= i2c_entries) {
-		NV_ERROR(dev, "DCB I2C index too big (%d >= %d)\n",
-			 index, i2ctable[2]);
-		return -ENOENT;
-	}
-	if (i2ctable[headerlen + entry_len * index + 3] == 0xff) {
-		NV_ERROR(dev, "DCB I2C entry invalid\n");
-		return -EINVAL;
-	}
-
-	if (dcb_i2c_ver >= 0x30) {
-		port_type = i2ctable[headerlen + recordoffset + 3 + entry_len * index];
-
-		/*
-		 * Fixup for chips using same address offset for read and
-		 * write.
-		 */
-		if (port_type == 4)	/* seen on C51 */
-			rdofs = wrofs = 1;
-		if (port_type >= 5)	/* G80+ */
-			rdofs = wrofs = 0;
-	}
-
-	if (dcb_i2c_ver >= 0x40) {
-		if (port_type != 5 && port_type != 6)
-			NV_WARN(dev, "DCB I2C table has port type %d\n", port_type);
-
-		i2c->entry = ROM32(i2ctable[headerlen + recordoffset + entry_len * index]);
-	}
-
-	i2c->port_type = port_type;
-	i2c->read = i2ctable[headerlen + recordoffset + rdofs + entry_len * index];
-	i2c->write = i2ctable[headerlen + recordoffset + wrofs + entry_len * index];
-
-	return 0;
-}
-
 static struct nouveau_i2c_chan *
 init_i2c_device_find(struct drm_device *dev, int i2c_index)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct dcb_table *dcb = &dev_priv->vbios.dcb;
-
 	if (i2c_index == 0xff) {
+		struct drm_nouveau_private *dev_priv = dev->dev_private;
+		struct dcb_table *dcb = &dev_priv->vbios.dcb;
 		/* note: dcb_entry_idx_from_crtchead needs pre-script set-up */
-		int idx = dcb_entry_idx_from_crtchead(dev), shift = 0;
-		int default_indices = dcb->i2c_default_indices;
+		int idx = dcb_entry_idx_from_crtchead(dev);
 
+		i2c_index = NV_I2C_DEFAULT(0);
 		if (idx != 0x7f && dcb->entry[idx].i2c_upper_default)
-			shift = 4;
-
-		i2c_index = (default_indices >> shift) & 0xf;
+			i2c_index = NV_I2C_DEFAULT(1);
 	}
-	if (i2c_index == 0x80)	/* g80+ */
-		i2c_index = dcb->i2c_default_indices & 0xf;
-	else
-	if (i2c_index == 0x81)
-		i2c_index = (dcb->i2c_default_indices & 0xf0) >> 4;
-
-	if (i2c_index >= DCB_MAX_NUM_I2C_ENTRIES) {
-		NV_ERROR(dev, "invalid i2c_index 0x%x\n", i2c_index);
-		return NULL;
-	}
-
-	/* Make sure i2c table entry has been parsed, it may not
-	 * have been if this is a bus not referenced by a DCB encoder
-	 */
-	read_dcb_i2c_entry(dev, dcb->version, dcb->i2c_table,
-			   i2c_index, &dcb->i2c[i2c_index]);
 
 	return nouveau_i2c_find(dev, i2c_index);
 }
@@ -1199,13 +1101,9 @@ init_dp_condition(struct nvbios *bios, uint16_t offset, struct init_exec *iexec)
 
 	switch (cond) {
 	case 0:
-	{
-		struct dcb_connector_table_entry *ent =
-			&bios->dcb.connector.entry[dcb->connector];
-
-		if (ent->type != DCB_CONNECTOR_eDP)
+		entry = dcb_conn(dev, dcb->connector);
+		if (!entry || entry[0] != DCB_CONNECTOR_eDP)
 			iexec->execute = false;
-	}
 		break;
 	case 1:
 	case 2:
@@ -3227,49 +3125,6 @@ init_8d(struct nvbios *bios, uint16_t offset, struct init_exec *iexec)
 	return 1;
 }
 
-static void
-init_gpio_unknv50(struct nvbios *bios, struct dcb_gpio_entry *gpio)
-{
-	const uint32_t nv50_gpio_ctl[2] = { 0xe100, 0xe28c };
-	u32 r, s, v;
-
-	/* Not a clue, needs de-magicing */
-	r = nv50_gpio_ctl[gpio->line >> 4];
-	s = (gpio->line & 0x0f);
-	v = bios_rd32(bios, r) & ~(0x00010001 << s);
-	switch ((gpio->entry & 0x06000000) >> 25) {
-	case 1:
-		v |= (0x00000001 << s);
-		break;
-	case 2:
-		v |= (0x00010000 << s);
-		break;
-	default:
-		break;
-	}
-
-	bios_wr32(bios, r, v);
-}
-
-static void
-init_gpio_unknvd0(struct nvbios *bios, struct dcb_gpio_entry *gpio)
-{
-	u32 v, i;
-
-	v  = bios_rd32(bios, 0x00d610 + (gpio->line * 4));
-	v &= 0xffffff00;
-	v |= (gpio->entry & 0x00ff0000) >> 16;
-	bios_wr32(bios, 0x00d610 + (gpio->line * 4), v);
-
-	i = (gpio->entry & 0x1f000000) >> 24;
-	if (i) {
-		v  = bios_rd32(bios, 0x00d640 + ((i - 1) * 4));
-		v &= 0xffffff00;
-		v |= gpio->line;
-		bios_wr32(bios, 0x00d640 + ((i - 1) * 4), v);
-	}
-}
-
 static int
 init_gpio(struct nvbios *bios, uint16_t offset, struct init_exec *iexec)
 {
@@ -3282,35 +3137,8 @@ init_gpio(struct nvbios *bios, uint16_t offset, struct init_exec *iexec)
 	 * each GPIO according to various values listed in each entry
 	 */
 
-	struct drm_nouveau_private *dev_priv = bios->dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	int i;
-
-	if (dev_priv->card_type < NV_50) {
-		NV_ERROR(bios->dev, "INIT_GPIO on unsupported chipset\n");
-		return 1;
-	}
-
-	if (!iexec->execute)
-		return 1;
-
-	for (i = 0; i < bios->dcb.gpio.entries; i++) {
-		struct dcb_gpio_entry *gpio = &bios->dcb.gpio.entry[i];
-
-		BIOSLOG(bios, "0x%04X: Entry: 0x%08X\n", offset, gpio->entry);
-
-		BIOSLOG(bios, "0x%04X: set gpio 0x%02x, state %d\n",
-			offset, gpio->tag, gpio->state_default);
-
-		if (!bios->execute)
-			continue;
-
-		pgpio->set(bios->dev, gpio->tag, gpio->state_default);
-		if (dev_priv->card_type < NV_D0)
-			init_gpio_unknv50(bios, gpio);
-		else
-			init_gpio_unknvd0(bios, gpio);
-	}
+	if (iexec->execute && bios->execute)
+		nouveau_gpio_reset(bios->dev);
 
 	return 1;
 }
@@ -4407,18 +4235,6 @@ int nouveau_bios_parse_lvds_table(struct drm_device *dev, int pxclk, bool *dl, b
 		break;
 	}
 
-	/* Dell Latitude D620 reports a too-high value for the dual-link
-	 * transition freq, causing us to program the panel incorrectly.
-	 *
-	 * It doesn't appear the VBIOS actually uses its transition freq
-	 * (90000kHz), instead it uses the "Number of LVDS channels" field
-	 * out of the panel ID structure (http://www.spwg.org/).
-	 *
-	 * For the moment, a quirk will do :)
-	 */
-	if (nv_match_device(dev, 0x01d7, 0x1028, 0x01c2))
-		bios->fp.duallink_transition_clk = 80000;
-
 	/* set dual_link flag for EDID case */
 	if (pxclk && (chip_version < 0x25 || chip_version > 0x28))
 		bios->fp.dual_link = (pxclk >= bios->fp.duallink_transition_clk);
@@ -4541,7 +4357,7 @@ nouveau_bios_run_display_table(struct drm_device *dev, u16 type, int pclk,
 	NV_DEBUG_KMS(dev, "Searching for output entry for %d %d %d\n",
 			dcbent->type, dcbent->location, dcbent->or);
 	for (i = 0; i < table[3]; i++) {
-		otable = ROMPTR(bios, table[table[1] + (i * table[2])]);
+		otable = ROMPTR(dev, table[table[1] + (i * table[2])]);
 		if (otable && bios_encoder_match(dcbent, ROM32(otable[0])))
 			break;
 	}
@@ -4719,7 +4535,7 @@ static struct pll_mapping nv84_pll_mapping[] = {
 	{ PLL_CORE  , 0x004028 },
 	{ PLL_SHADER, 0x004020 },
 	{ PLL_MEMORY, 0x004008 },
-	{ PLL_UNK05 , 0x004030 },
+	{ PLL_VDEC  , 0x004030 },
 	{ PLL_UNK41 , 0x00e818 },
 	{ PLL_VPLL0 , 0x614100 },
 	{ PLL_VPLL1 , 0x614900 },
@@ -5485,6 +5301,9 @@ bit_table(struct drm_device *dev, u8 id, struct bit_entry *bit)
 	struct nvbios *bios = &dev_priv->vbios;
 	u8 entries, *entry;
 
+	if (bios->type != NVBIOS_BIT)
+		return -ENODEV;
+
 	entries = bios->data[bios->offset + 10];
 	entry   = &bios->data[bios->offset + 12];
 	while (entries--) {
@@ -5493,7 +5312,7 @@ bit_table(struct drm_device *dev, u8 id, struct bit_entry *bit)
 			bit->version = entry[1];
 			bit->length = ROM16(entry[2]);
 			bit->offset = ROM16(entry[4]);
-			bit->data = ROMPTR(bios, entry[4]);
+			bit->data = ROMPTR(dev, entry[4]);
 			return 0;
 		}
 
@@ -5598,10 +5417,6 @@ static int parse_bmp_structure(struct drm_device *dev, struct nvbios *bios, unsi
 	uint16_t legacy_scripts_offset, legacy_i2c_offset;
 
 	/* load needed defaults in case we can't parse this info */
-	bios->dcb.i2c[0].write = NV_CIO_CRE_DDC_WR__INDEX;
-	bios->dcb.i2c[0].read = NV_CIO_CRE_DDC_STATUS__INDEX;
-	bios->dcb.i2c[1].write = NV_CIO_CRE_DDC0_WR__INDEX;
-	bios->dcb.i2c[1].read = NV_CIO_CRE_DDC0_STATUS__INDEX;
 	bios->digital_min_front_porch = 0x4b;
 	bios->fmaxvco = 256000;
 	bios->fminvco = 128000;
@@ -5709,14 +5524,6 @@ static int parse_bmp_structure(struct drm_device *dev, struct nvbios *bios, unsi
 	bios->legacy.i2c_indices.crt = bios->data[legacy_i2c_offset];
 	bios->legacy.i2c_indices.tv = bios->data[legacy_i2c_offset + 1];
 	bios->legacy.i2c_indices.panel = bios->data[legacy_i2c_offset + 2];
-	if (bios->data[legacy_i2c_offset + 4])
-		bios->dcb.i2c[0].write = bios->data[legacy_i2c_offset + 4];
-	if (bios->data[legacy_i2c_offset + 5])
-		bios->dcb.i2c[0].read = bios->data[legacy_i2c_offset + 5];
-	if (bios->data[legacy_i2c_offset + 6])
-		bios->dcb.i2c[1].write = bios->data[legacy_i2c_offset + 6];
-	if (bios->data[legacy_i2c_offset + 7])
-		bios->dcb.i2c[1].read = bios->data[legacy_i2c_offset + 7];
 
 	if (bmplength > 74) {
 		bios->fmaxvco = ROM32(bmp[67]);
@@ -5767,286 +5574,128 @@ static uint16_t findstr(uint8_t *data, int n, const uint8_t *str, int len)
 	return 0;
 }
 
-static struct dcb_gpio_entry *
-new_gpio_entry(struct nvbios *bios)
-{
-	struct drm_device *dev = bios->dev;
-	struct dcb_gpio_table *gpio = &bios->dcb.gpio;
-
-	if (gpio->entries >= DCB_MAX_NUM_GPIO_ENTRIES) {
-		NV_ERROR(dev, "exceeded maximum number of gpio entries!!\n");
-		return NULL;
-	}
-
-	return &gpio->entry[gpio->entries++];
-}
-
-struct dcb_gpio_entry *
-nouveau_bios_gpio_entry(struct drm_device *dev, enum dcb_gpio_tag tag)
+void *
+dcb_table(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nvbios *bios = &dev_priv->vbios;
-	int i;
-
-	for (i = 0; i < bios->dcb.gpio.entries; i++) {
-		if (bios->dcb.gpio.entry[i].tag != tag)
-			continue;
+	u8 *dcb = NULL;
 
-		return &bios->dcb.gpio.entry[i];
+	if (dev_priv->card_type > NV_04)
+		dcb = ROMPTR(dev, dev_priv->vbios.data[0x36]);
+	if (!dcb) {
+		NV_WARNONCE(dev, "No DCB data found in VBIOS\n");
+		return NULL;
 	}
 
-	return NULL;
-}
-
-static void
-parse_dcb_gpio_table(struct nvbios *bios)
-{
-	struct drm_device *dev = bios->dev;
-	struct dcb_gpio_entry *e;
-	u8 headerlen, entries, recordlen;
-	u8 *dcb, *gpio = NULL, *entry;
-	int i;
-
-	dcb = ROMPTR(bios, bios->data[0x36]);
+	if (dcb[0] >= 0x41) {
+		NV_WARNONCE(dev, "DCB version 0x%02x unknown\n", dcb[0]);
+		return NULL;
+	} else
 	if (dcb[0] >= 0x30) {
-		gpio = ROMPTR(bios, dcb[10]);
-		if (!gpio)
-			goto no_table;
-
-		headerlen = gpio[1];
-		entries   = gpio[2];
-		recordlen = gpio[3];
+		if (ROM32(dcb[6]) == 0x4edcbdcb)
+			return dcb;
 	} else
-	if (dcb[0] >= 0x22 && dcb[-1] >= 0x13) {
-		gpio = ROMPTR(bios, dcb[-15]);
-		if (!gpio)
-			goto no_table;
-
-		headerlen = 3;
-		entries   = gpio[2];
-		recordlen = gpio[1];
+	if (dcb[0] >= 0x20) {
+		if (ROM32(dcb[4]) == 0x4edcbdcb)
+			return dcb;
 	} else
-	if (dcb[0] >= 0x22) {
-		/* No GPIO table present, parse the TVDAC GPIO data. */
-		uint8_t *tvdac_gpio = &dcb[-5];
-
-		if (tvdac_gpio[0] & 1) {
-			e = new_gpio_entry(bios);
-			e->tag = DCB_GPIO_TVDAC0;
-			e->line = tvdac_gpio[1] >> 4;
-			e->invert = tvdac_gpio[0] & 2;
-		}
-
-		goto no_table;
+	if (dcb[0] >= 0x15) {
+		if (!memcmp(&dcb[-7], "DEV_REC", 7))
+			return dcb;
 	} else {
-		NV_DEBUG(dev, "no/unknown gpio table on DCB 0x%02x\n", dcb[0]);
-		goto no_table;
-	}
-
-	entry = gpio + headerlen;
-	for (i = 0; i < entries; i++, entry += recordlen) {
-		e = new_gpio_entry(bios);
-		if (!e)
-			break;
-
-		if (gpio[0] < 0x40) {
-			e->entry = ROM16(entry[0]);
-			e->tag = (e->entry & 0x07e0) >> 5;
-			if (e->tag == 0x3f) {
-				bios->dcb.gpio.entries--;
-				continue;
-			}
-
-			e->line = (e->entry & 0x001f);
-			e->invert = ((e->entry & 0xf800) >> 11) != 4;
-		} else {
-			e->entry = ROM32(entry[0]);
-			e->tag = (e->entry & 0x0000ff00) >> 8;
-			if (e->tag == 0xff) {
-				bios->dcb.gpio.entries--;
-				continue;
-			}
-
-			e->line = (e->entry & 0x0000001f) >> 0;
-			if (gpio[0] == 0x40) {
-				e->state_default = (e->entry & 0x01000000) >> 24;
-				e->state[0] = (e->entry & 0x18000000) >> 27;
-				e->state[1] = (e->entry & 0x60000000) >> 29;
-			} else {
-				e->state_default = (e->entry & 0x00000080) >> 7;
-				e->state[0] = (entry[4] >> 4) & 3;
-				e->state[1] = (entry[4] >> 6) & 3;
-			}
-		}
-	}
-
-no_table:
-	/* Apple iMac G4 NV18 */
-	if (nv_match_device(dev, 0x0189, 0x10de, 0x0010)) {
-		e = new_gpio_entry(bios);
-		if (e) {
-			e->tag = DCB_GPIO_TVDAC0;
-			e->line = 4;
-		}
-	}
-}
-
-struct dcb_connector_table_entry *
-nouveau_bios_connector_entry(struct drm_device *dev, int index)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nvbios *bios = &dev_priv->vbios;
-	struct dcb_connector_table_entry *cte;
-
-	if (index >= bios->dcb.connector.entries)
-		return NULL;
-
-	cte = &bios->dcb.connector.entry[index];
-	if (cte->type == 0xff)
+		/*
+		 * v1.4 (some NV15/16, NV11+) seems the same as v1.5, but
+		 * always has the same single (crt) entry, even when tv-out
+		 * present, so the conclusion is this version cannot really
+		 * be used.
+		 *
+		 * v1.2 tables (some NV6/10, and NV15+) normally have the
+		 * same 5 entries, which are not specific to the card and so
+		 * no use.
+		 *
+		 * v1.2 does have an I2C table that read_dcb_i2c_table can
+		 * handle, but cards exist (nv11 in #14821) with a bad i2c
+		 * table pointer, so use the indices parsed in
+		 * parse_bmp_structure.
+		 *
+		 * v1.1 (NV5+, maybe some NV4) is entirely unhelpful
+		 */
+		NV_WARNONCE(dev, "No useful DCB data in VBIOS\n");
 		return NULL;
+	}
 
-	return cte;
+	NV_WARNONCE(dev, "DCB header validation failed\n");
+	return NULL;
 }
 
-static enum dcb_connector_type
-divine_connector_type(struct nvbios *bios, int index)
+void *
+dcb_outp(struct drm_device *dev, u8 idx)
 {
-	struct dcb_table *dcb = &bios->dcb;
-	unsigned encoders = 0, type = DCB_CONNECTOR_NONE;
-	int i;
-
-	for (i = 0; i < dcb->entries; i++) {
-		if (dcb->entry[i].connector == index)
-			encoders |= (1 << dcb->entry[i].type);
-	}
-
-	if (encoders & (1 << OUTPUT_DP)) {
-		if (encoders & (1 << OUTPUT_TMDS))
-			type = DCB_CONNECTOR_DP;
-		else
-			type = DCB_CONNECTOR_eDP;
-	} else
-	if (encoders & (1 << OUTPUT_TMDS)) {
-		if (encoders & (1 << OUTPUT_ANALOG))
-			type = DCB_CONNECTOR_DVI_I;
-		else
-			type = DCB_CONNECTOR_DVI_D;
-	} else
-	if (encoders & (1 << OUTPUT_ANALOG)) {
-		type = DCB_CONNECTOR_VGA;
+	u8 *dcb = dcb_table(dev);
+	if (dcb && dcb[0] >= 0x30) {
+		if (idx < dcb[2])
+			return dcb + dcb[1] + (idx * dcb[3]);
 	} else
-	if (encoders & (1 << OUTPUT_LVDS)) {
-		type = DCB_CONNECTOR_LVDS;
+	if (dcb && dcb[0] >= 0x20) {
+		u8 *i2c = ROMPTR(dev, dcb[2]);
+		u8 *ent = dcb + 8 + (idx * 8);
+		if (i2c && ent < i2c)
+			return ent;
 	} else
-	if (encoders & (1 << OUTPUT_TV)) {
-		type = DCB_CONNECTOR_TV_0;
+	if (dcb && dcb[0] >= 0x15) {
+		u8 *i2c = ROMPTR(dev, dcb[2]);
+		u8 *ent = dcb + 4 + (idx * 10);
+		if (i2c && ent < i2c)
+			return ent;
 	}
 
-	return type;
+	return NULL;
 }
 
-static void
-apply_dcb_connector_quirks(struct nvbios *bios, int idx)
-{
-	struct dcb_connector_table_entry *cte = &bios->dcb.connector.entry[idx];
-	struct drm_device *dev = bios->dev;
+int
+dcb_outp_foreach(struct drm_device *dev, void *data,
+		 int (*exec)(struct drm_device *, void *, int idx, u8 *outp))
+{
+	int ret, idx = -1;
+	u8 *outp = NULL;
+	while ((outp = dcb_outp(dev, ++idx))) {
+		if (ROM32(outp[0]) == 0x00000000)
+			break; /* seen on an NV11 with DCB v1.5 */
+		if (ROM32(outp[0]) == 0xffffffff)
+			break; /* seen on an NV17 with DCB v2.0 */
+
+		if ((outp[0] & 0x0f) == OUTPUT_UNUSED)
+			continue;
+		if ((outp[0] & 0x0f) == OUTPUT_EOL)
+			break;
 
-	/* Gigabyte NX85T */
-	if (nv_match_device(dev, 0x0421, 0x1458, 0x344c)) {
-		if (cte->type == DCB_CONNECTOR_HDMI_1)
-			cte->type = DCB_CONNECTOR_DVI_I;
+		ret = exec(dev, data, idx, outp);
+		if (ret)
+			return ret;
 	}
 
-	/* Gigabyte GV-NX86T512H */
-	if (nv_match_device(dev, 0x0402, 0x1458, 0x3455)) {
-		if (cte->type == DCB_CONNECTOR_HDMI_1)
-			cte->type = DCB_CONNECTOR_DVI_I;
-	}
+	return 0;
 }
 
-static const u8 hpd_gpio[16] = {
-	0xff, 0x07, 0x08, 0xff, 0xff, 0x51, 0x52, 0xff,
-	0xff, 0xff, 0xff, 0xff, 0xff, 0x5e, 0x5f, 0x60,
-};
-
-static void
-parse_dcb_connector_table(struct nvbios *bios)
+u8 *
+dcb_conntab(struct drm_device *dev)
 {
-	struct drm_device *dev = bios->dev;
-	struct dcb_connector_table *ct = &bios->dcb.connector;
-	struct dcb_connector_table_entry *cte;
-	uint8_t *conntab = &bios->data[bios->dcb.connector_table_ptr];
-	uint8_t *entry;
-	int i;
-
-	if (!bios->dcb.connector_table_ptr) {
-		NV_DEBUG_KMS(dev, "No DCB connector table present\n");
-		return;
-	}
-
-	NV_INFO(dev, "DCB connector table: VHER 0x%02x %d %d %d\n",
-		conntab[0], conntab[1], conntab[2], conntab[3]);
-	if ((conntab[0] != 0x30 && conntab[0] != 0x40) ||
-	    (conntab[3] != 2 && conntab[3] != 4)) {
-		NV_ERROR(dev, "  Unknown!  Please report.\n");
-		return;
+	u8 *dcb = dcb_table(dev);
+	if (dcb && dcb[0] >= 0x30 && dcb[1] >= 0x16) {
+		u8 *conntab = ROMPTR(dev, dcb[0x14]);
+		if (conntab && conntab[0] >= 0x30 && conntab[0] <= 0x40)
+			return conntab;
 	}
+	return NULL;
+}
 
-	ct->entries = conntab[2];
-
-	entry = conntab + conntab[1];
-	cte = &ct->entry[0];
-	for (i = 0; i < conntab[2]; i++, entry += conntab[3], cte++) {
-		cte->index = i;
-		if (conntab[3] == 2)
-			cte->entry = ROM16(entry[0]);
-		else
-			cte->entry = ROM32(entry[0]);
-
-		cte->type  = (cte->entry & 0x000000ff) >> 0;
-		cte->index2 = (cte->entry & 0x00000f00) >> 8;
-
-		cte->gpio_tag = ffs((cte->entry & 0x07033000) >> 12);
-		cte->gpio_tag = hpd_gpio[cte->gpio_tag];
-
-		if (cte->type == 0xff)
-			continue;
-
-		apply_dcb_connector_quirks(bios, i);
-
-		NV_INFO(dev, "  %d: 0x%08x: type 0x%02x idx %d tag 0x%02x\n",
-			i, cte->entry, cte->type, cte->index, cte->gpio_tag);
-
-		/* check for known types, fallback to guessing the type
-		 * from attached encoders if we hit an unknown.
-		 */
-		switch (cte->type) {
-		case DCB_CONNECTOR_VGA:
-		case DCB_CONNECTOR_TV_0:
-		case DCB_CONNECTOR_TV_1:
-		case DCB_CONNECTOR_TV_3:
-		case DCB_CONNECTOR_DVI_I:
-		case DCB_CONNECTOR_DVI_D:
-		case DCB_CONNECTOR_LVDS:
-		case DCB_CONNECTOR_LVDS_SPWG:
-		case DCB_CONNECTOR_DP:
-		case DCB_CONNECTOR_eDP:
-		case DCB_CONNECTOR_HDMI_0:
-		case DCB_CONNECTOR_HDMI_1:
-			break;
-		default:
-			cte->type = divine_connector_type(bios, cte->index);
-			NV_WARN(dev, "unknown type, using 0x%02x\n", cte->type);
-			break;
-		}
-
-		if (nouveau_override_conntype) {
-			int type = divine_connector_type(bios, cte->index);
-			if (type != cte->type)
-				NV_WARN(dev, " -> type 0x%02x\n", cte->type);
-		}
-
-	}
+u8 *
+dcb_conn(struct drm_device *dev, u8 idx)
+{
+	u8 *conntab = dcb_conntab(dev);
+	if (conntab && idx < conntab[2])
+		return conntab + conntab[1] + (idx * conntab[3]);
+	return NULL;
 }
 
 static struct dcb_entry *new_dcb_entry(struct dcb_table *dcb)
@@ -6079,8 +5728,7 @@ parse_dcb20_entry(struct drm_device *dev, struct dcb_table *dcb,
 	entry->type = conn & 0xf;
 	entry->i2c_index = (conn >> 4) & 0xf;
 	entry->heads = (conn >> 8) & 0xf;
-	if (dcb->version >= 0x40)
-		entry->connector = (conn >> 12) & 0xf;
+	entry->connector = (conn >> 12) & 0xf;
 	entry->bus = (conn >> 16) & 0xf;
 	entry->location = (conn >> 20) & 0x3;
 	entry->or = (conn >> 24) & 0xf;
@@ -6252,25 +5900,6 @@ parse_dcb15_entry(struct drm_device *dev, struct dcb_table *dcb,
 	return true;
 }
 
-static bool parse_dcb_entry(struct drm_device *dev, struct dcb_table *dcb,
-			    uint32_t conn, uint32_t conf)
-{
-	struct dcb_entry *entry = new_dcb_entry(dcb);
-	bool ret;
-
-	if (dcb->version >= 0x20)
-		ret = parse_dcb20_entry(dev, dcb, conn, conf, entry);
-	else
-		ret = parse_dcb15_entry(dev, dcb, conn, conf, entry);
-	if (!ret)
-		return ret;
-
-	read_dcb_i2c_entry(dev, dcb->version, dcb->i2c_table,
-			   entry->i2c_index, &dcb->i2c[entry->i2c_index]);
-
-	return true;
-}
-
 static
 void merge_like_dcb_entries(struct drm_device *dev, struct dcb_table *dcb)
 {
@@ -6431,154 +6060,118 @@ fabricate_dcb_encoder_table(struct drm_device *dev, struct nvbios *bios)
 #endif
 
 	/* Make up some sane defaults */
-	fabricate_dcb_output(dcb, OUTPUT_ANALOG, LEGACY_I2C_CRT, 1, 1);
+	fabricate_dcb_output(dcb, OUTPUT_ANALOG,
+			     bios->legacy.i2c_indices.crt, 1, 1);
 
 	if (nv04_tv_identify(dev, bios->legacy.i2c_indices.tv) >= 0)
-		fabricate_dcb_output(dcb, OUTPUT_TV, LEGACY_I2C_TV,
+		fabricate_dcb_output(dcb, OUTPUT_TV,
+				     bios->legacy.i2c_indices.tv,
 				     all_heads, 0);
 
 	else if (bios->tmds.output0_script_ptr ||
 		 bios->tmds.output1_script_ptr)
-		fabricate_dcb_output(dcb, OUTPUT_TMDS, LEGACY_I2C_PANEL,
+		fabricate_dcb_output(dcb, OUTPUT_TMDS,
+				     bios->legacy.i2c_indices.panel,
 				     all_heads, 1);
 }
 
 static int
-parse_dcb_table(struct drm_device *dev, struct nvbios *bios)
+parse_dcb_entry(struct drm_device *dev, void *data, int idx, u8 *outp)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct dcb_table *dcb = &bios->dcb;
-	uint16_t dcbptr = 0, i2ctabptr = 0;
-	uint8_t *dcbtable;
-	uint8_t headerlen = 0x4, entries = DCB_MAX_NUM_ENTRIES;
-	bool configblock = true;
-	int recordlength = 8, confofs = 4;
-	int i;
-
-	/* get the offset from 0x36 */
-	if (dev_priv->card_type > NV_04) {
-		dcbptr = ROM16(bios->data[0x36]);
-		if (dcbptr == 0x0000)
-			NV_WARN(dev, "No output data (DCB) found in BIOS\n");
-	}
-
-	/* this situation likely means a really old card, pre DCB */
-	if (dcbptr == 0x0) {
-		fabricate_dcb_encoder_table(dev, bios);
-		return 0;
-	}
-
-	dcbtable = &bios->data[dcbptr];
-
-	/* get DCB version */
-	dcb->version = dcbtable[0];
-	NV_TRACE(dev, "Found Display Configuration Block version %d.%d\n",
-		 dcb->version >> 4, dcb->version & 0xf);
-
-	if (dcb->version >= 0x20) { /* NV17+ */
-		uint32_t sig;
+	struct dcb_table *dcb = &dev_priv->vbios.dcb;
+	u32 conf = (dcb->version >= 0x20) ? ROM32(outp[4]) : ROM32(outp[6]);
+	u32 conn = ROM32(outp[0]);
+	bool ret;
 
-		if (dcb->version >= 0x30) { /* NV40+ */
-			headerlen = dcbtable[1];
-			entries = dcbtable[2];
-			recordlength = dcbtable[3];
-			i2ctabptr = ROM16(dcbtable[4]);
-			sig = ROM32(dcbtable[6]);
-			dcb->gpio_table_ptr = ROM16(dcbtable[10]);
-			dcb->connector_table_ptr = ROM16(dcbtable[20]);
-		} else {
-			i2ctabptr = ROM16(dcbtable[2]);
-			sig = ROM32(dcbtable[4]);
-			headerlen = 8;
-		}
+	if (apply_dcb_encoder_quirks(dev, idx, &conn, &conf)) {
+		struct dcb_entry *entry = new_dcb_entry(dcb);
 
-		if (sig != 0x4edcbdcb) {
-			NV_ERROR(dev, "Bad Display Configuration Block "
-					"signature (%08X)\n", sig);
-			return -EINVAL;
-		}
-	} else if (dcb->version >= 0x15) { /* some NV11 and NV20 */
-		char sig[8] = { 0 };
+		NV_TRACEWARN(dev, "DCB outp %02d: %08x %08x\n", idx, conn, conf);
 
-		strncpy(sig, (char *)&dcbtable[-7], 7);
-		i2ctabptr = ROM16(dcbtable[2]);
-		recordlength = 10;
-		confofs = 6;
+		if (dcb->version >= 0x20)
+			ret = parse_dcb20_entry(dev, dcb, conn, conf, entry);
+		else
+			ret = parse_dcb15_entry(dev, dcb, conn, conf, entry);
+		if (!ret)
+			return 1; /* stop parsing */
 
-		if (strcmp(sig, "DEV_REC")) {
-			NV_ERROR(dev, "Bad Display Configuration Block "
-					"signature (%s)\n", sig);
-			return -EINVAL;
-		}
-	} else {
-		/*
-		 * v1.4 (some NV15/16, NV11+) seems the same as v1.5, but always
-		 * has the same single (crt) entry, even when tv-out present, so
-		 * the conclusion is this version cannot really be used.
-		 * v1.2 tables (some NV6/10, and NV15+) normally have the same
-		 * 5 entries, which are not specific to the card and so no use.
-		 * v1.2 does have an I2C table that read_dcb_i2c_table can
-		 * handle, but cards exist (nv11 in #14821) with a bad i2c table
-		 * pointer, so use the indices parsed in parse_bmp_structure.
-		 * v1.1 (NV5+, maybe some NV4) is entirely unhelpful
+		/* Ignore the I2C index for on-chip TV-out, as there
+		 * are cards with bogus values (nv31m in bug 23212),
+		 * and it's otherwise useless.
 		 */
-		NV_TRACEWARN(dev, "No useful information in BIOS output table; "
-				  "adding all possible outputs\n");
-		fabricate_dcb_encoder_table(dev, bios);
-		return 0;
+		if (entry->type == OUTPUT_TV &&
+		    entry->location == DCB_LOC_ON_CHIP)
+			entry->i2c_index = 0x0f;
 	}
 
-	if (!i2ctabptr)
-		NV_WARN(dev, "No pointer to DCB I2C port table\n");
-	else {
-		dcb->i2c_table = &bios->data[i2ctabptr];
-		if (dcb->version >= 0x30)
-			dcb->i2c_default_indices = dcb->i2c_table[4];
+	return 0;
+}
 
-		/*
-		 * Parse the "management" I2C bus, used for hardware
-		 * monitoring and some external TMDS transmitters.
-		 */
-		if (dcb->version >= 0x22) {
-			int idx = (dcb->version >= 0x40 ?
-				   dcb->i2c_default_indices & 0xf :
-				   2);
+static void
+dcb_fake_connectors(struct nvbios *bios)
+{
+	struct dcb_table *dcbt = &bios->dcb;
+	u8 map[16] = { };
+	int i, idx = 0;
 
-			read_dcb_i2c_entry(dev, dcb->version, dcb->i2c_table,
-					   idx, &dcb->i2c[idx]);
-		}
+	/* heuristic: if we ever get a non-zero connector field, assume
+	 * that all the indices are valid and we don't need fake them.
+	 */
+	for (i = 0; i < dcbt->entries; i++) {
+		if (dcbt->entry[i].connector)
+			return;
 	}
 
-	if (entries > DCB_MAX_NUM_ENTRIES)
-		entries = DCB_MAX_NUM_ENTRIES;
-
-	for (i = 0; i < entries; i++) {
-		uint32_t connection, config = 0;
-
-		connection = ROM32(dcbtable[headerlen + recordlength * i]);
-		if (configblock)
-			config = ROM32(dcbtable[headerlen + confofs + recordlength * i]);
-
-		/* seen on an NV11 with DCB v1.5 */
-		if (connection == 0x00000000)
-			break;
+	/* no useful connector info available, we need to make it up
+	 * ourselves.  the rule here is: anything on the same i2c bus
+	 * is considered to be on the same connector.  any output
+	 * without an associated i2c bus is assigned its own unique
+	 * connector index.
+	 */
+	for (i = 0; i < dcbt->entries; i++) {
+		u8 i2c = dcbt->entry[i].i2c_index;
+		if (i2c == 0x0f) {
+			dcbt->entry[i].connector = idx++;
+		} else {
+			if (!map[i2c])
+				map[i2c] = ++idx;
+			dcbt->entry[i].connector = map[i2c] - 1;
+		}
+	}
 
-		/* seen on an NV17 with DCB v2.0 */
-		if (connection == 0xffffffff)
-			break;
+	/* if we created more than one connector, destroy the connector
+	 * table - just in case it has random, rather than stub, entries.
+	 */
+	if (i > 1) {
+		u8 *conntab = dcb_conntab(bios->dev);
+		if (conntab)
+			conntab[0] = 0x00;
+	}
+}
 
-		if ((connection & 0x0000000f) == 0x0000000f)
-			continue;
+static int
+parse_dcb_table(struct drm_device *dev, struct nvbios *bios)
+{
+	struct dcb_table *dcb = &bios->dcb;
+	u8 *dcbt, *conn;
+	int idx;
+
+	dcbt = dcb_table(dev);
+	if (!dcbt) {
+		/* handle pre-DCB boards */
+		if (bios->type == NVBIOS_BMP) {
+			fabricate_dcb_encoder_table(dev, bios);
+			return 0;
+		}
 
-		if (!apply_dcb_encoder_quirks(dev, i, &connection, &config))
-			continue;
+		return -EINVAL;
+	}
 
-		NV_TRACEWARN(dev, "Raw DCB entry %d: %08x %08x\n",
-			     dcb->entries, connection, config);
+	NV_TRACE(dev, "DCB version %d.%d\n", dcbt[0] >> 4, dcbt[0] & 0xf);
 
-		if (!parse_dcb_entry(dev, dcb, connection, config))
-			break;
-	}
+	dcb->version = dcbt[0];
+	dcb_outp_foreach(dev, NULL, parse_dcb_entry);
 
 	/*
 	 * apart for v2.1+ not being known for requiring merging, this
@@ -6590,77 +6183,19 @@ parse_dcb_table(struct drm_device *dev, struct nvbios *bios)
 	if (!dcb->entries)
 		return -ENXIO;
 
-	parse_dcb_gpio_table(bios);
-	parse_dcb_connector_table(bios);
-	return 0;
-}
-
-static void
-fixup_legacy_connector(struct nvbios *bios)
-{
-	struct dcb_table *dcb = &bios->dcb;
-	int i, i2c, i2c_conn[DCB_MAX_NUM_I2C_ENTRIES] = { };
-
-	/*
-	 * DCB 3.0 also has the table in most cases, but there are some cards
-	 * where the table is filled with stub entries, and the DCB entriy
-	 * indices are all 0.  We don't need the connector indices on pre-G80
-	 * chips (yet?) so limit the use to DCB 4.0 and above.
-	 */
-	if (dcb->version >= 0x40)
-		return;
-
-	dcb->connector.entries = 0;
-
-	/*
-	 * No known connector info before v3.0, so make it up.  the rule here
-	 * is: anything on the same i2c bus is considered to be on the same
-	 * connector.  any output without an associated i2c bus is assigned
-	 * its own unique connector index.
-	 */
-	for (i = 0; i < dcb->entries; i++) {
-		/*
-		 * Ignore the I2C index for on-chip TV-out, as there
-		 * are cards with bogus values (nv31m in bug 23212),
-		 * and it's otherwise useless.
-		 */
-		if (dcb->entry[i].type == OUTPUT_TV &&
-		    dcb->entry[i].location == DCB_LOC_ON_CHIP)
-			dcb->entry[i].i2c_index = 0xf;
-		i2c = dcb->entry[i].i2c_index;
-
-		if (i2c_conn[i2c]) {
-			dcb->entry[i].connector = i2c_conn[i2c] - 1;
-			continue;
+	/* dump connector table entries to log, if any exist */
+	idx = -1;
+	while ((conn = dcb_conn(dev, ++idx))) {
+		if (conn[0] != 0xff) {
+			NV_TRACE(dev, "DCB conn %02d: ", idx);
+			if (dcb_conntab(dev)[3] < 4)
+				printk("%04x\n", ROM16(conn[0]));
+			else
+				printk("%08x\n", ROM32(conn[0]));
 		}
-
-		dcb->entry[i].connector = dcb->connector.entries++;
-		if (i2c != 0xf)
-			i2c_conn[i2c] = dcb->connector.entries;
-	}
-
-	/* Fake the connector table as well as just connector indices */
-	for (i = 0; i < dcb->connector.entries; i++) {
-		dcb->connector.entry[i].index = i;
-		dcb->connector.entry[i].type = divine_connector_type(bios, i);
-		dcb->connector.entry[i].gpio_tag = 0xff;
-	}
-}
-
-static void
-fixup_legacy_i2c(struct nvbios *bios)
-{
-	struct dcb_table *dcb = &bios->dcb;
-	int i;
-
-	for (i = 0; i < dcb->entries; i++) {
-		if (dcb->entry[i].i2c_index == LEGACY_I2C_CRT)
-			dcb->entry[i].i2c_index = bios->legacy.i2c_indices.crt;
-		if (dcb->entry[i].i2c_index == LEGACY_I2C_PANEL)
-			dcb->entry[i].i2c_index = bios->legacy.i2c_indices.panel;
-		if (dcb->entry[i].i2c_index == LEGACY_I2C_TV)
-			dcb->entry[i].i2c_index = bios->legacy.i2c_indices.tv;
 	}
+	dcb_fake_connectors(bios);
+	return 0;
 }
 
 static int load_nv17_hwsq_ucode_entry(struct drm_device *dev, struct nvbios *bios, uint16_t hwsq_offset, int entry)
@@ -6879,19 +6414,6 @@ nouveau_run_vbios_init(struct drm_device *dev)
 	return ret;
 }
 
-static void
-nouveau_bios_i2c_devices_takedown(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nvbios *bios = &dev_priv->vbios;
-	struct dcb_i2c_entry *entry;
-	int i;
-
-	entry = &bios->dcb.i2c[0];
-	for (i = 0; i < DCB_MAX_NUM_I2C_ENTRIES; i++, entry++)
-		nouveau_i2c_fini(dev, entry);
-}
-
 static bool
 nouveau_bios_posted(struct drm_device *dev)
 {
@@ -6928,12 +6450,17 @@ nouveau_bios_init(struct drm_device *dev)
 	if (ret)
 		return ret;
 
-	ret = parse_dcb_table(dev, bios);
+	ret = nouveau_i2c_init(dev);
 	if (ret)
 		return ret;
 
-	fixup_legacy_i2c(bios);
-	fixup_legacy_connector(bios);
+	ret = nouveau_mxm_init(dev);
+	if (ret)
+		return ret;
+
+	ret = parse_dcb_table(dev, bios);
+	if (ret)
+		return ret;
 
 	if (!bios->major_version)	/* we don't run version 0 bios */
 		return 0;
@@ -6971,5 +6498,6 @@ nouveau_bios_init(struct drm_device *dev)
 void
 nouveau_bios_takedown(struct drm_device *dev)
 {
-	nouveau_bios_i2c_devices_takedown(dev);
+	nouveau_mxm_fini(dev);
+	nouveau_i2c_fini(dev);
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.h b/drivers/gpu/drm/nouveau/nouveau_bios.h
index 8adb69e4a6b1..1e382ad5a2b8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.h
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.h
@@ -34,9 +34,14 @@
 
 #define DCB_LOC_ON_CHIP 0
 
-#define ROM16(x) le16_to_cpu(*(uint16_t *)&(x))
-#define ROM32(x) le32_to_cpu(*(uint32_t *)&(x))
-#define ROMPTR(bios, x) (ROM16(x) ? &(bios)->data[ROM16(x)] : NULL)
+#define ROM16(x) le16_to_cpu(*(u16 *)&(x))
+#define ROM32(x) le32_to_cpu(*(u32 *)&(x))
+#define ROM48(x) ({ u8 *p = &(x); (u64)ROM16(p[4]) << 32 | ROM32(p[0]); })
+#define ROM64(x) le64_to_cpu(*(u64 *)&(x))
+#define ROMPTR(d,x) ({            \
+	struct drm_nouveau_private *dev_priv = (d)->dev_private; \
+	ROM16(x) ? &dev_priv->vbios.data[ROM16(x)] : NULL; \
+})
 
 struct bit_entry {
 	uint8_t  id;
@@ -48,30 +53,12 @@ struct bit_entry {
 
 int bit_table(struct drm_device *, u8 id, struct bit_entry *);
 
-struct dcb_i2c_entry {
-	uint32_t entry;
-	uint8_t port_type;
-	uint8_t read, write;
-	struct nouveau_i2c_chan *chan;
-};
-
 enum dcb_gpio_tag {
 	DCB_GPIO_TVDAC0 = 0xc,
 	DCB_GPIO_TVDAC1 = 0x2d,
-};
-
-struct dcb_gpio_entry {
-	enum dcb_gpio_tag tag;
-	int line;
-	bool invert;
-	uint32_t entry;
-	uint8_t state_default;
-	uint8_t state[2];
-};
-
-struct dcb_gpio_table {
-	int entries;
-	struct dcb_gpio_entry entry[DCB_MAX_NUM_GPIO_ENTRIES];
+	DCB_GPIO_PWM_FAN = 0x9,
+	DCB_GPIO_FAN_SENSE = 0x3d,
+	DCB_GPIO_UNUSED = 0xff
 };
 
 enum dcb_connector_type {
@@ -90,20 +77,6 @@ enum dcb_connector_type {
 	DCB_CONNECTOR_NONE = 0xff
 };
 
-struct dcb_connector_table_entry {
-	uint8_t index;
-	uint32_t entry;
-	enum dcb_connector_type type;
-	uint8_t index2;
-	uint8_t gpio_tag;
-	void *drm;
-};
-
-struct dcb_connector_table {
-	int entries;
-	struct dcb_connector_table_entry entry[DCB_MAX_NUM_CONNECTOR_ENTRIES];
-};
-
 enum dcb_type {
 	OUTPUT_ANALOG = 0,
 	OUTPUT_TV = 1,
@@ -111,6 +84,7 @@ enum dcb_type {
 	OUTPUT_LVDS = 3,
 	OUTPUT_DP = 6,
 	OUTPUT_EOL = 14, /* DCB 4.0+, appears to be end-of-list */
+	OUTPUT_UNUSED = 15,
 	OUTPUT_ANY = -1
 };
 
@@ -155,18 +129,8 @@ struct dcb_entry {
 
 struct dcb_table {
 	uint8_t version;
-
 	int entries;
 	struct dcb_entry entry[DCB_MAX_NUM_ENTRIES];
-
-	uint8_t *i2c_table;
-	uint8_t i2c_default_indices;
-	struct dcb_i2c_entry i2c[DCB_MAX_NUM_I2C_ENTRIES];
-
-	uint16_t gpio_table_ptr;
-	struct dcb_gpio_table gpio;
-	uint16_t connector_table_ptr;
-	struct dcb_connector_table connector;
 };
 
 enum nouveau_or {
@@ -195,7 +159,7 @@ enum pll_types {
 	PLL_SHADER = 0x02,
 	PLL_UNK03  = 0x03,
 	PLL_MEMORY = 0x04,
-	PLL_UNK05  = 0x05,
+	PLL_VDEC   = 0x05,
 	PLL_UNK40  = 0x40,
 	PLL_UNK41  = 0x41,
 	PLL_UNK42  = 0x42,
@@ -333,4 +297,11 @@ struct nvbios {
 	} legacy;
 };
 
+void *dcb_table(struct drm_device *);
+void *dcb_outp(struct drm_device *, u8 idx);
+int dcb_outp_foreach(struct drm_device *, void *data,
+		     int (*)(struct drm_device *, void *, int idx, u8 *outp));
+u8 *dcb_conntab(struct drm_device *);
+u8 *dcb_conn(struct drm_device *, u8 idx);
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index 7cc37e690860..724b41a2b9e9 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -28,6 +28,7 @@
  */
 
 #include "drmP.h"
+#include "ttm/ttm_page_alloc.h"
 
 #include "nouveau_drm.h"
 #include "nouveau_drv.h"
@@ -92,6 +93,7 @@ nouveau_bo_new(struct drm_device *dev, int size, int align,
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_bo *nvbo;
+	size_t acc_size;
 	int ret;
 
 	nvbo = kzalloc(sizeof(struct nouveau_bo), GFP_KERNEL);
@@ -114,9 +116,12 @@ nouveau_bo_new(struct drm_device *dev, int size, int align,
 	nvbo->bo.mem.num_pages = size >> PAGE_SHIFT;
 	nouveau_bo_placement_set(nvbo, flags, 0);
 
+	acc_size = ttm_bo_dma_acc_size(&dev_priv->ttm.bdev, size,
+				       sizeof(struct nouveau_bo));
+
 	ret = ttm_bo_init(&dev_priv->ttm.bdev, &nvbo->bo, size,
 			  ttm_bo_type_device, &nvbo->placement,
-			  align >> PAGE_SHIFT, 0, false, NULL, size,
+			  align >> PAGE_SHIFT, 0, false, NULL, acc_size,
 			  nouveau_bo_del_ttm);
 	if (ret) {
 		/* ttm will call nouveau_bo_del_ttm if it fails.. */
@@ -343,8 +348,10 @@ nouveau_bo_wr32(struct nouveau_bo *nvbo, unsigned index, u32 val)
 		*mem = val;
 }
 
-static struct ttm_backend *
-nouveau_bo_create_ttm_backend_entry(struct ttm_bo_device *bdev)
+static struct ttm_tt *
+nouveau_ttm_tt_create(struct ttm_bo_device *bdev,
+		      unsigned long size, uint32_t page_flags,
+		      struct page *dummy_read_page)
 {
 	struct drm_nouveau_private *dev_priv = nouveau_bdev(bdev);
 	struct drm_device *dev = dev_priv->dev;
@@ -352,11 +359,13 @@ nouveau_bo_create_ttm_backend_entry(struct ttm_bo_device *bdev)
 	switch (dev_priv->gart_info.type) {
 #if __OS_HAS_AGP
 	case NOUVEAU_GART_AGP:
-		return ttm_agp_backend_init(bdev, dev->agp->bridge);
+		return ttm_agp_tt_create(bdev, dev->agp->bridge,
+					 size, page_flags, dummy_read_page);
 #endif
 	case NOUVEAU_GART_PDMA:
 	case NOUVEAU_GART_HW:
-		return nouveau_sgdma_init_ttm(dev);
+		return nouveau_sgdma_create_ttm(bdev, size, page_flags,
+						dummy_read_page);
 	default:
 		NV_ERROR(dev, "Unknown GART type %d\n",
 			 dev_priv->gart_info.type);
@@ -673,8 +682,7 @@ nouveau_vma_getmap(struct nouveau_channel *chan, struct nouveau_bo *nvbo,
 	if (mem->mem_type == TTM_PL_VRAM)
 		nouveau_vm_map(vma, node);
 	else
-		nouveau_vm_map_sg(vma, 0, mem->num_pages << PAGE_SHIFT,
-				  node, node->pages);
+		nouveau_vm_map_sg(vma, 0, mem->num_pages << PAGE_SHIFT, node);
 
 	return 0;
 }
@@ -801,19 +809,18 @@ out:
 static void
 nouveau_bo_move_ntfy(struct ttm_buffer_object *bo, struct ttm_mem_reg *new_mem)
 {
-	struct nouveau_mem *node = new_mem->mm_node;
 	struct nouveau_bo *nvbo = nouveau_bo(bo);
 	struct nouveau_vma *vma;
 
 	list_for_each_entry(vma, &nvbo->vma_list, head) {
-		if (new_mem->mem_type == TTM_PL_VRAM) {
+		if (new_mem && new_mem->mem_type == TTM_PL_VRAM) {
 			nouveau_vm_map(vma, new_mem->mm_node);
 		} else
-		if (new_mem->mem_type == TTM_PL_TT &&
+		if (new_mem && new_mem->mem_type == TTM_PL_TT &&
 		    nvbo->page_shift == vma->vm->spg_shift) {
 			nouveau_vm_map_sg(vma, 0, new_mem->
 					  num_pages << PAGE_SHIFT,
-					  node, node->pages);
+					  new_mem->mm_node);
 		} else {
 			nouveau_vm_unmap(vma);
 		}
@@ -1044,8 +1051,94 @@ nouveau_bo_fence(struct nouveau_bo *nvbo, struct nouveau_fence *fence)
 	nouveau_fence_unref(&old_fence);
 }
 
+static int
+nouveau_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	struct ttm_dma_tt *ttm_dma = (void *)ttm;
+	struct drm_nouveau_private *dev_priv;
+	struct drm_device *dev;
+	unsigned i;
+	int r;
+
+	if (ttm->state != tt_unpopulated)
+		return 0;
+
+	dev_priv = nouveau_bdev(ttm->bdev);
+	dev = dev_priv->dev;
+
+#if __OS_HAS_AGP
+	if (dev_priv->gart_info.type == NOUVEAU_GART_AGP) {
+		return ttm_agp_tt_populate(ttm);
+	}
+#endif
+
+#ifdef CONFIG_SWIOTLB
+	if (swiotlb_nr_tbl()) {
+		return ttm_dma_populate((void *)ttm, dev->dev);
+	}
+#endif
+
+	r = ttm_pool_populate(ttm);
+	if (r) {
+		return r;
+	}
+
+	for (i = 0; i < ttm->num_pages; i++) {
+		ttm_dma->dma_address[i] = pci_map_page(dev->pdev, ttm->pages[i],
+						   0, PAGE_SIZE,
+						   PCI_DMA_BIDIRECTIONAL);
+		if (pci_dma_mapping_error(dev->pdev, ttm_dma->dma_address[i])) {
+			while (--i) {
+				pci_unmap_page(dev->pdev, ttm_dma->dma_address[i],
+					       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
+				ttm_dma->dma_address[i] = 0;
+			}
+			ttm_pool_unpopulate(ttm);
+			return -EFAULT;
+		}
+	}
+	return 0;
+}
+
+static void
+nouveau_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	struct ttm_dma_tt *ttm_dma = (void *)ttm;
+	struct drm_nouveau_private *dev_priv;
+	struct drm_device *dev;
+	unsigned i;
+
+	dev_priv = nouveau_bdev(ttm->bdev);
+	dev = dev_priv->dev;
+
+#if __OS_HAS_AGP
+	if (dev_priv->gart_info.type == NOUVEAU_GART_AGP) {
+		ttm_agp_tt_unpopulate(ttm);
+		return;
+	}
+#endif
+
+#ifdef CONFIG_SWIOTLB
+	if (swiotlb_nr_tbl()) {
+		ttm_dma_unpopulate((void *)ttm, dev->dev);
+		return;
+	}
+#endif
+
+	for (i = 0; i < ttm->num_pages; i++) {
+		if (ttm_dma->dma_address[i]) {
+			pci_unmap_page(dev->pdev, ttm_dma->dma_address[i],
+				       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
+		}
+	}
+
+	ttm_pool_unpopulate(ttm);
+}
+
 struct ttm_bo_driver nouveau_bo_driver = {
-	.create_ttm_backend_entry = nouveau_bo_create_ttm_backend_entry,
+	.ttm_tt_create = &nouveau_ttm_tt_create,
+	.ttm_tt_populate = &nouveau_ttm_tt_populate,
+	.ttm_tt_unpopulate = &nouveau_ttm_tt_unpopulate,
 	.invalidate_caches = nouveau_bo_invalidate_caches,
 	.init_mem_type = nouveau_bo_init_mem_type,
 	.evict_flags = nouveau_bo_evict_flags,
@@ -1091,7 +1184,7 @@ nouveau_bo_vma_add(struct nouveau_bo *nvbo, struct nouveau_vm *vm,
 		nouveau_vm_map(vma, nvbo->bo.mem.mm_node);
 	else
 	if (nvbo->bo.mem.mem_type == TTM_PL_TT)
-		nouveau_vm_map_sg(vma, 0, size, node, node->pages);
+		nouveau_vm_map_sg(vma, 0, size, node);
 
 	list_add_tail(&vma->head, &nvbo->vma_list);
 	vma->refcount = 1;
diff --git a/drivers/gpu/drm/nouveau/nouveau_channel.c b/drivers/gpu/drm/nouveau/nouveau_channel.c
index bb6ec9ef8676..a018defb7621 100644
--- a/drivers/gpu/drm/nouveau/nouveau_channel.c
+++ b/drivers/gpu/drm/nouveau/nouveau_channel.c
@@ -187,6 +187,8 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 	nouveau_dma_pre_init(chan);
 	chan->user_put = 0x40;
 	chan->user_get = 0x44;
+	if (dev_priv->card_type >= NV_50)
+                chan->user_get_hi = 0x60;
 
 	/* disable the fifo caches */
 	pfifo->reassign(dev, false);
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.c b/drivers/gpu/drm/nouveau/nouveau_connector.c
index cea6696b1906..f3ce34be082a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.c
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.c
@@ -35,6 +35,7 @@
 #include "nouveau_encoder.h"
 #include "nouveau_crtc.h"
 #include "nouveau_connector.h"
+#include "nouveau_gpio.h"
 #include "nouveau_hw.h"
 
 static void nouveau_connector_hotplug(void *, int);
@@ -78,29 +79,11 @@ nouveau_encoder_connector_get(struct nouveau_encoder *encoder)
 	return NULL;
 }
 
-/*TODO: This could use improvement, and learn to handle the fixed
- *      BIOS tables etc.  It's fine currently, for its only user.
- */
-int
-nouveau_connector_bpp(struct drm_connector *connector)
-{
-	struct nouveau_connector *nv_connector = nouveau_connector(connector);
-
-	if (nv_connector->edid && nv_connector->edid->revision >= 4) {
-		u8 bpc = ((nv_connector->edid->input & 0x70) >> 3) + 4;
-		if (bpc > 4)
-			return bpc;
-	}
-
-	return 18;
-}
-
 static void
 nouveau_connector_destroy(struct drm_connector *connector)
 {
 	struct nouveau_connector *nv_connector = nouveau_connector(connector);
 	struct drm_nouveau_private *dev_priv;
-	struct nouveau_gpio_engine *pgpio;
 	struct drm_device *dev;
 
 	if (!nv_connector)
@@ -110,10 +93,9 @@ nouveau_connector_destroy(struct drm_connector *connector)
 	dev_priv = dev->dev_private;
 	NV_DEBUG_KMS(dev, "\n");
 
-	pgpio = &dev_priv->engine.gpio;
-	if (pgpio->irq_unregister) {
-		pgpio->irq_unregister(dev, nv_connector->dcb->gpio_tag,
-				      nouveau_connector_hotplug, connector);
+	if (nv_connector->hpd != DCB_GPIO_UNUSED) {
+		nouveau_gpio_isr_del(dev, 0, nv_connector->hpd, 0xff,
+				     nouveau_connector_hotplug, connector);
 	}
 
 	kfree(nv_connector->edid);
@@ -198,6 +180,10 @@ nouveau_connector_set_encoder(struct drm_connector *connector,
 		return;
 	nv_connector->detected_encoder = nv_encoder;
 
+	if (dev_priv->card_type >= NV_50) {
+		connector->interlace_allowed = true;
+		connector->doublescan_allowed = true;
+	} else
 	if (nv_encoder->dcb->type == OUTPUT_LVDS ||
 	    nv_encoder->dcb->type == OUTPUT_TMDS) {
 		connector->doublescan_allowed = false;
@@ -214,7 +200,7 @@ nouveau_connector_set_encoder(struct drm_connector *connector,
 			connector->interlace_allowed = true;
 	}
 
-	if (nv_connector->dcb->type == DCB_CONNECTOR_DVI_I) {
+	if (nv_connector->type == DCB_CONNECTOR_DVI_I) {
 		drm_connector_property_set_value(connector,
 			dev->mode_config.dvi_i_subconnector_property,
 			nv_encoder->dcb->type == OUTPUT_TMDS ?
@@ -397,7 +383,7 @@ nouveau_connector_force(struct drm_connector *connector)
 	struct nouveau_encoder *nv_encoder;
 	int type;
 
-	if (nv_connector->dcb->type == DCB_CONNECTOR_DVI_I) {
+	if (nv_connector->type == DCB_CONNECTOR_DVI_I) {
 		if (connector->force == DRM_FORCE_ON_DIGITAL)
 			type = OUTPUT_TMDS;
 		else
@@ -420,15 +406,21 @@ static int
 nouveau_connector_set_property(struct drm_connector *connector,
 			       struct drm_property *property, uint64_t value)
 {
+	struct drm_nouveau_private *dev_priv = connector->dev->dev_private;
+	struct nouveau_display_engine *disp = &dev_priv->engine.display;
 	struct nouveau_connector *nv_connector = nouveau_connector(connector);
 	struct nouveau_encoder *nv_encoder = nv_connector->detected_encoder;
 	struct drm_encoder *encoder = to_drm_encoder(nv_encoder);
 	struct drm_device *dev = connector->dev;
+	struct nouveau_crtc *nv_crtc;
 	int ret;
 
+	nv_crtc = NULL;
+	if (connector->encoder && connector->encoder->crtc)
+		nv_crtc = nouveau_crtc(connector->encoder->crtc);
+
 	/* Scaling mode */
 	if (property == dev->mode_config.scaling_mode_property) {
-		struct nouveau_crtc *nv_crtc = NULL;
 		bool modeset = false;
 
 		switch (value) {
@@ -454,8 +446,6 @@ nouveau_connector_set_property(struct drm_connector *connector,
 			modeset = true;
 		nv_connector->scaling_mode = value;
 
-		if (connector->encoder && connector->encoder->crtc)
-			nv_crtc = nouveau_crtc(connector->encoder->crtc);
 		if (!nv_crtc)
 			return 0;
 
@@ -467,7 +457,7 @@ nouveau_connector_set_property(struct drm_connector *connector,
 			if (!ret)
 				return -EINVAL;
 		} else {
-			ret = nv_crtc->set_scale(nv_crtc, value, true);
+			ret = nv_crtc->set_scale(nv_crtc, true);
 			if (ret)
 				return ret;
 		}
@@ -475,23 +465,58 @@ nouveau_connector_set_property(struct drm_connector *connector,
 		return 0;
 	}
 
-	/* Dithering */
-	if (property == dev->mode_config.dithering_mode_property) {
-		struct nouveau_crtc *nv_crtc = NULL;
+	/* Underscan */
+	if (property == disp->underscan_property) {
+		if (nv_connector->underscan != value) {
+			nv_connector->underscan = value;
+			if (!nv_crtc || !nv_crtc->set_scale)
+				return 0;
 
-		if (value == DRM_MODE_DITHERING_ON)
-			nv_connector->use_dithering = true;
-		else
-			nv_connector->use_dithering = false;
+			return nv_crtc->set_scale(nv_crtc, true);
+		}
+
+		return 0;
+	}
+
+	if (property == disp->underscan_hborder_property) {
+		if (nv_connector->underscan_hborder != value) {
+			nv_connector->underscan_hborder = value;
+			if (!nv_crtc || !nv_crtc->set_scale)
+				return 0;
+
+			return nv_crtc->set_scale(nv_crtc, true);
+		}
+
+		return 0;
+	}
+
+	if (property == disp->underscan_vborder_property) {
+		if (nv_connector->underscan_vborder != value) {
+			nv_connector->underscan_vborder = value;
+			if (!nv_crtc || !nv_crtc->set_scale)
+				return 0;
+
+			return nv_crtc->set_scale(nv_crtc, true);
+		}
+
+		return 0;
+	}
+
+	/* Dithering */
+	if (property == disp->dithering_mode) {
+		nv_connector->dithering_mode = value;
+		if (!nv_crtc || !nv_crtc->set_dither)
+			return 0;
 
-		if (connector->encoder && connector->encoder->crtc)
-			nv_crtc = nouveau_crtc(connector->encoder->crtc);
+		return nv_crtc->set_dither(nv_crtc, true);
+	}
 
+	if (property == disp->dithering_depth) {
+		nv_connector->dithering_depth = value;
 		if (!nv_crtc || !nv_crtc->set_dither)
 			return 0;
 
-		return nv_crtc->set_dither(nv_crtc, nv_connector->use_dithering,
-					   true);
+		return nv_crtc->set_dither(nv_crtc, true);
 	}
 
 	if (nv_encoder && nv_encoder->dcb->type == OUTPUT_TV)
@@ -602,6 +627,46 @@ nouveau_connector_scaler_modes_add(struct drm_connector *connector)
 	return modes;
 }
 
+static void
+nouveau_connector_detect_depth(struct drm_connector *connector)
+{
+	struct drm_nouveau_private *dev_priv = connector->dev->dev_private;
+	struct nouveau_connector *nv_connector = nouveau_connector(connector);
+	struct nouveau_encoder *nv_encoder = nv_connector->detected_encoder;
+	struct nvbios *bios = &dev_priv->vbios;
+	struct drm_display_mode *mode = nv_connector->native_mode;
+	bool duallink;
+
+	/* if the edid is feeling nice enough to provide this info, use it */
+	if (nv_connector->edid && connector->display_info.bpc)
+		return;
+
+	/* if not, we're out of options unless we're LVDS, default to 6bpc */
+	connector->display_info.bpc = 6;
+	if (nv_encoder->dcb->type != OUTPUT_LVDS)
+		return;
+
+	/* LVDS: panel straps */
+	if (bios->fp_no_ddc) {
+		if (bios->fp.if_is_24bit)
+			connector->display_info.bpc = 8;
+		return;
+	}
+
+	/* LVDS: DDC panel, need to first determine the number of links to
+	 * know which if_is_24bit flag to check...
+	 */
+	if (nv_connector->edid &&
+	    nv_connector->type == DCB_CONNECTOR_LVDS_SPWG)
+		duallink = ((u8 *)nv_connector->edid)[121] == 2;
+	else
+		duallink = mode->clock >= bios->fp.duallink_transition_clk;
+
+	if ((!duallink && (bios->fp.strapless_is_24bit & 1)) ||
+	    ( duallink && (bios->fp.strapless_is_24bit & 2)))
+		connector->display_info.bpc = 8;
+}
+
 static int
 nouveau_connector_get_modes(struct drm_connector *connector)
 {
@@ -631,6 +696,12 @@ nouveau_connector_get_modes(struct drm_connector *connector)
 		nv_connector->native_mode = drm_mode_duplicate(dev, &mode);
 	}
 
+	/* Determine display colour depth for everything except LVDS now,
+	 * DP requires this before mode_valid() is called.
+	 */
+	if (connector->connector_type != DRM_MODE_CONNECTOR_LVDS)
+		nouveau_connector_detect_depth(connector);
+
 	/* Find the native mode if this is a digital panel, if we didn't
 	 * find any modes through DDC previously add the native mode to
 	 * the list of modes.
@@ -646,12 +717,19 @@ nouveau_connector_get_modes(struct drm_connector *connector)
 		ret = 1;
 	}
 
+	/* Determine LVDS colour depth, must happen after determining
+	 * "native" mode as some VBIOS tables require us to use the
+	 * pixel clock as part of the lookup...
+	 */
+	if (connector->connector_type == DRM_MODE_CONNECTOR_LVDS)
+		nouveau_connector_detect_depth(connector);
+
 	if (nv_encoder->dcb->type == OUTPUT_TV)
 		ret = get_slave_funcs(encoder)->get_modes(encoder, connector);
 
-	if (nv_connector->dcb->type == DCB_CONNECTOR_LVDS ||
-	    nv_connector->dcb->type == DCB_CONNECTOR_LVDS_SPWG ||
-	    nv_connector->dcb->type == DCB_CONNECTOR_eDP)
+	if (nv_connector->type == DCB_CONNECTOR_LVDS ||
+	    nv_connector->type == DCB_CONNECTOR_LVDS_SPWG ||
+	    nv_connector->type == DCB_CONNECTOR_eDP)
 		ret += nouveau_connector_scaler_modes_add(connector);
 
 	return ret;
@@ -710,7 +788,7 @@ nouveau_connector_mode_valid(struct drm_connector *connector,
 	case OUTPUT_DP:
 		max_clock  = nv_encoder->dp.link_nr;
 		max_clock *= nv_encoder->dp.link_bw;
-		clock = clock * nouveau_connector_bpp(connector) / 10;
+		clock = clock * (connector->display_info.bpc * 3) / 10;
 		break;
 	default:
 		BUG_ON(1);
@@ -768,96 +846,175 @@ nouveau_connector_funcs_lvds = {
 	.force = nouveau_connector_force
 };
 
+static int
+drm_conntype_from_dcb(enum dcb_connector_type dcb)
+{
+	switch (dcb) {
+	case DCB_CONNECTOR_VGA      : return DRM_MODE_CONNECTOR_VGA;
+	case DCB_CONNECTOR_TV_0     :
+	case DCB_CONNECTOR_TV_1     :
+	case DCB_CONNECTOR_TV_3     : return DRM_MODE_CONNECTOR_TV;
+	case DCB_CONNECTOR_DVI_I    : return DRM_MODE_CONNECTOR_DVII;
+	case DCB_CONNECTOR_DVI_D    : return DRM_MODE_CONNECTOR_DVID;
+	case DCB_CONNECTOR_LVDS     :
+	case DCB_CONNECTOR_LVDS_SPWG: return DRM_MODE_CONNECTOR_LVDS;
+	case DCB_CONNECTOR_DP       : return DRM_MODE_CONNECTOR_DisplayPort;
+	case DCB_CONNECTOR_eDP      : return DRM_MODE_CONNECTOR_eDP;
+	case DCB_CONNECTOR_HDMI_0   :
+	case DCB_CONNECTOR_HDMI_1   : return DRM_MODE_CONNECTOR_HDMIA;
+	default:
+		break;
+	}
+
+	return DRM_MODE_CONNECTOR_Unknown;
+}
+
 struct drm_connector *
 nouveau_connector_create(struct drm_device *dev, int index)
 {
 	const struct drm_connector_funcs *funcs = &nouveau_connector_funcs;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+	struct nouveau_display_engine *disp = &dev_priv->engine.display;
 	struct nouveau_connector *nv_connector = NULL;
-	struct dcb_connector_table_entry *dcb = NULL;
 	struct drm_connector *connector;
 	int type, ret = 0;
+	bool dummy;
 
 	NV_DEBUG_KMS(dev, "\n");
 
-	if (index >= dev_priv->vbios.dcb.connector.entries)
-		return ERR_PTR(-EINVAL);
-
-	dcb = &dev_priv->vbios.dcb.connector.entry[index];
-	if (dcb->drm)
-		return dcb->drm;
-
-	switch (dcb->type) {
-	case DCB_CONNECTOR_VGA:
-		type = DRM_MODE_CONNECTOR_VGA;
-		break;
-	case DCB_CONNECTOR_TV_0:
-	case DCB_CONNECTOR_TV_1:
-	case DCB_CONNECTOR_TV_3:
-		type = DRM_MODE_CONNECTOR_TV;
-		break;
-	case DCB_CONNECTOR_DVI_I:
-		type = DRM_MODE_CONNECTOR_DVII;
-		break;
-	case DCB_CONNECTOR_DVI_D:
-		type = DRM_MODE_CONNECTOR_DVID;
-		break;
-	case DCB_CONNECTOR_HDMI_0:
-	case DCB_CONNECTOR_HDMI_1:
-		type = DRM_MODE_CONNECTOR_HDMIA;
-		break;
-	case DCB_CONNECTOR_LVDS:
-	case DCB_CONNECTOR_LVDS_SPWG:
-		type = DRM_MODE_CONNECTOR_LVDS;
-		funcs = &nouveau_connector_funcs_lvds;
-		break;
-	case DCB_CONNECTOR_DP:
-		type = DRM_MODE_CONNECTOR_DisplayPort;
-		break;
-	case DCB_CONNECTOR_eDP:
-		type = DRM_MODE_CONNECTOR_eDP;
-		break;
-	default:
-		NV_ERROR(dev, "unknown connector type: 0x%02x!!\n", dcb->type);
-		return ERR_PTR(-EINVAL);
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		nv_connector = nouveau_connector(connector);
+		if (nv_connector->index == index)
+			return connector;
 	}
 
 	nv_connector = kzalloc(sizeof(*nv_connector), GFP_KERNEL);
 	if (!nv_connector)
 		return ERR_PTR(-ENOMEM);
-	nv_connector->dcb = dcb;
+
 	connector = &nv_connector->base;
+	nv_connector->index = index;
+
+	/* attempt to parse vbios connector type and hotplug gpio */
+	nv_connector->dcb = dcb_conn(dev, index);
+	if (nv_connector->dcb) {
+		static const u8 hpd[16] = {
+			0xff, 0x07, 0x08, 0xff, 0xff, 0x51, 0x52, 0xff,
+			0xff, 0xff, 0xff, 0xff, 0xff, 0x5e, 0x5f, 0x60,
+		};
+
+		u32 entry = ROM16(nv_connector->dcb[0]);
+		if (dcb_conntab(dev)[3] >= 4)
+			entry |= (u32)ROM16(nv_connector->dcb[2]) << 16;
+
+		nv_connector->hpd = ffs((entry & 0x07033000) >> 12);
+		nv_connector->hpd = hpd[nv_connector->hpd];
+
+		nv_connector->type = nv_connector->dcb[0];
+		if (drm_conntype_from_dcb(nv_connector->type) ==
+					  DRM_MODE_CONNECTOR_Unknown) {
+			NV_WARN(dev, "unknown connector type %02x\n",
+				nv_connector->type);
+			nv_connector->type = DCB_CONNECTOR_NONE;
+		}
 
-	/* defaults, will get overridden in detect() */
-	connector->interlace_allowed = false;
-	connector->doublescan_allowed = false;
+		/* Gigabyte NX85T */
+		if (nv_match_device(dev, 0x0421, 0x1458, 0x344c)) {
+			if (nv_connector->type == DCB_CONNECTOR_HDMI_1)
+				nv_connector->type = DCB_CONNECTOR_DVI_I;
+		}
 
-	drm_connector_init(dev, connector, funcs, type);
-	drm_connector_helper_add(connector, &nouveau_connector_helper_funcs);
+		/* Gigabyte GV-NX86T512H */
+		if (nv_match_device(dev, 0x0402, 0x1458, 0x3455)) {
+			if (nv_connector->type == DCB_CONNECTOR_HDMI_1)
+				nv_connector->type = DCB_CONNECTOR_DVI_I;
+		}
+	} else {
+		nv_connector->type = DCB_CONNECTOR_NONE;
+		nv_connector->hpd = DCB_GPIO_UNUSED;
+	}
+
+	/* no vbios data, or an unknown dcb connector type - attempt to
+	 * figure out something suitable ourselves
+	 */
+	if (nv_connector->type == DCB_CONNECTOR_NONE) {
+		struct drm_nouveau_private *dev_priv = dev->dev_private;
+		struct dcb_table *dcbt = &dev_priv->vbios.dcb;
+		u32 encoders = 0;
+		int i;
+
+		for (i = 0; i < dcbt->entries; i++) {
+			if (dcbt->entry[i].connector == nv_connector->index)
+				encoders |= (1 << dcbt->entry[i].type);
+		}
 
-	/* Check if we need dithering enabled */
-	if (connector->connector_type == DRM_MODE_CONNECTOR_LVDS) {
-		bool dummy, is_24bit = false;
+		if (encoders & (1 << OUTPUT_DP)) {
+			if (encoders & (1 << OUTPUT_TMDS))
+				nv_connector->type = DCB_CONNECTOR_DP;
+			else
+				nv_connector->type = DCB_CONNECTOR_eDP;
+		} else
+		if (encoders & (1 << OUTPUT_TMDS)) {
+			if (encoders & (1 << OUTPUT_ANALOG))
+				nv_connector->type = DCB_CONNECTOR_DVI_I;
+			else
+				nv_connector->type = DCB_CONNECTOR_DVI_D;
+		} else
+		if (encoders & (1 << OUTPUT_ANALOG)) {
+			nv_connector->type = DCB_CONNECTOR_VGA;
+		} else
+		if (encoders & (1 << OUTPUT_LVDS)) {
+			nv_connector->type = DCB_CONNECTOR_LVDS;
+		} else
+		if (encoders & (1 << OUTPUT_TV)) {
+			nv_connector->type = DCB_CONNECTOR_TV_0;
+		}
+	}
 
-		ret = nouveau_bios_parse_lvds_table(dev, 0, &dummy, &is_24bit);
+	type = drm_conntype_from_dcb(nv_connector->type);
+	if (type == DRM_MODE_CONNECTOR_LVDS) {
+		ret = nouveau_bios_parse_lvds_table(dev, 0, &dummy, &dummy);
 		if (ret) {
-			NV_ERROR(dev, "Error parsing LVDS table, disabling "
-				 "LVDS\n");
-			goto fail;
+			NV_ERROR(dev, "Error parsing LVDS table, disabling\n");
+			kfree(nv_connector);
+			return ERR_PTR(ret);
 		}
 
-		nv_connector->use_dithering = !is_24bit;
+		funcs = &nouveau_connector_funcs_lvds;
+	} else {
+		funcs = &nouveau_connector_funcs;
 	}
 
+	/* defaults, will get overridden in detect() */
+	connector->interlace_allowed = false;
+	connector->doublescan_allowed = false;
+
+	drm_connector_init(dev, connector, funcs, type);
+	drm_connector_helper_add(connector, &nouveau_connector_helper_funcs);
+
 	/* Init DVI-I specific properties */
-	if (dcb->type == DCB_CONNECTOR_DVI_I) {
-		drm_mode_create_dvi_i_properties(dev);
+	if (nv_connector->type == DCB_CONNECTOR_DVI_I)
 		drm_connector_attach_property(connector, dev->mode_config.dvi_i_subconnector_property, 0);
-		drm_connector_attach_property(connector, dev->mode_config.dvi_i_select_subconnector_property, 0);
+
+	/* Add overscan compensation options to digital outputs */
+	if (disp->underscan_property &&
+	    (nv_connector->type == DCB_CONNECTOR_DVI_D ||
+	     nv_connector->type == DCB_CONNECTOR_DVI_I ||
+	     nv_connector->type == DCB_CONNECTOR_HDMI_0 ||
+	     nv_connector->type == DCB_CONNECTOR_HDMI_1 ||
+	     nv_connector->type == DCB_CONNECTOR_DP)) {
+		drm_connector_attach_property(connector,
+					      disp->underscan_property,
+					      UNDERSCAN_OFF);
+		drm_connector_attach_property(connector,
+					      disp->underscan_hborder_property,
+					      0);
+		drm_connector_attach_property(connector,
+					      disp->underscan_vborder_property,
+					      0);
 	}
 
-	switch (dcb->type) {
+	switch (nv_connector->type) {
 	case DCB_CONNECTOR_VGA:
 		if (dev_priv->card_type >= NV_50) {
 			drm_connector_attach_property(connector,
@@ -876,32 +1033,32 @@ nouveau_connector_create(struct drm_device *dev, int index)
 		drm_connector_attach_property(connector,
 				dev->mode_config.scaling_mode_property,
 				nv_connector->scaling_mode);
-		drm_connector_attach_property(connector,
-				dev->mode_config.dithering_mode_property,
-				nv_connector->use_dithering ?
-				DRM_MODE_DITHERING_ON : DRM_MODE_DITHERING_OFF);
+		if (disp->dithering_mode) {
+			nv_connector->dithering_mode = DITHERING_MODE_AUTO;
+			drm_connector_attach_property(connector,
+						disp->dithering_mode,
+						nv_connector->dithering_mode);
+		}
+		if (disp->dithering_depth) {
+			nv_connector->dithering_depth = DITHERING_DEPTH_AUTO;
+			drm_connector_attach_property(connector,
+						disp->dithering_depth,
+						nv_connector->dithering_depth);
+		}
 		break;
 	}
 
-	if (nv_connector->dcb->gpio_tag != 0xff && pgpio->irq_register) {
-		pgpio->irq_register(dev, nv_connector->dcb->gpio_tag,
-				    nouveau_connector_hotplug, connector);
-
-		connector->polled = DRM_CONNECTOR_POLL_HPD;
-	} else {
-		connector->polled = DRM_CONNECTOR_POLL_CONNECT;
+	connector->polled = DRM_CONNECTOR_POLL_CONNECT;
+	if (nv_connector->hpd != DCB_GPIO_UNUSED) {
+		ret = nouveau_gpio_isr_add(dev, 0, nv_connector->hpd, 0xff,
+					   nouveau_connector_hotplug,
+					   connector);
+		if (ret == 0)
+			connector->polled = DRM_CONNECTOR_POLL_HPD;
 	}
 
 	drm_sysfs_connector_add(connector);
-
-	dcb->drm = connector;
-	return dcb->drm;
-
-fail:
-	drm_connector_cleanup(connector);
-	kfree(connector);
-	return ERR_PTR(ret);
-
+	return connector;
 }
 
 static void
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.h b/drivers/gpu/drm/nouveau/nouveau_connector.h
index 711b1e9203af..e4857021304c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.h
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.h
@@ -30,13 +30,43 @@
 #include "drm_edid.h"
 #include "nouveau_i2c.h"
 
+enum nouveau_underscan_type {
+	UNDERSCAN_OFF,
+	UNDERSCAN_ON,
+	UNDERSCAN_AUTO,
+};
+
+/* the enum values specifically defined here match nv50/nvd0 hw values, and
+ * the code relies on this
+ */
+enum nouveau_dithering_mode {
+	DITHERING_MODE_OFF = 0x00,
+	DITHERING_MODE_ON = 0x01,
+	DITHERING_MODE_DYNAMIC2X2 = 0x10 | DITHERING_MODE_ON,
+	DITHERING_MODE_STATIC2X2 = 0x18 | DITHERING_MODE_ON,
+	DITHERING_MODE_TEMPORAL = 0x20 | DITHERING_MODE_ON,
+	DITHERING_MODE_AUTO
+};
+
+enum nouveau_dithering_depth {
+	DITHERING_DEPTH_6BPC = 0x00,
+	DITHERING_DEPTH_8BPC = 0x02,
+	DITHERING_DEPTH_AUTO
+};
+
 struct nouveau_connector {
 	struct drm_connector base;
+	enum dcb_connector_type type;
+	u8 index;
+	u8 *dcb;
+	u8 hpd;
 
-	struct dcb_connector_table_entry *dcb;
-
+	int dithering_mode;
+	int dithering_depth;
 	int scaling_mode;
-	bool use_dithering;
+	enum nouveau_underscan_type underscan;
+	u32 underscan_hborder;
+	u32 underscan_vborder;
 
 	struct nouveau_encoder *detected_encoder;
 	struct edid *edid;
diff --git a/drivers/gpu/drm/nouveau/nouveau_crtc.h b/drivers/gpu/drm/nouveau/nouveau_crtc.h
index bf8e1289953d..686f6b4a1da3 100644
--- a/drivers/gpu/drm/nouveau/nouveau_crtc.h
+++ b/drivers/gpu/drm/nouveau/nouveau_crtc.h
@@ -32,8 +32,6 @@ struct nouveau_crtc {
 
 	int index;
 
-	struct drm_display_mode *mode;
-
 	uint32_t dpms_saved_fp_control;
 	uint32_t fp_users;
 	int saturation;
@@ -67,8 +65,8 @@ struct nouveau_crtc {
 		int depth;
 	} lut;
 
-	int (*set_dither)(struct nouveau_crtc *crtc, bool on, bool update);
-	int (*set_scale)(struct nouveau_crtc *crtc, int mode, bool update);
+	int (*set_dither)(struct nouveau_crtc *crtc, bool update);
+	int (*set_scale)(struct nouveau_crtc *crtc, bool update);
 };
 
 static inline struct nouveau_crtc *nouveau_crtc(struct drm_crtc *crtc)
diff --git a/drivers/gpu/drm/nouveau/nouveau_debugfs.c b/drivers/gpu/drm/nouveau/nouveau_debugfs.c
index 8e1592368cce..fa2ec491f6a7 100644
--- a/drivers/gpu/drm/nouveau/nouveau_debugfs.c
+++ b/drivers/gpu/drm/nouveau/nouveau_debugfs.c
@@ -44,7 +44,7 @@ nouveau_debugfs_channel_info(struct seq_file *m, void *data)
 	seq_printf(m, "channel id    : %d\n", chan->id);
 
 	seq_printf(m, "cpu fifo state:\n");
-	seq_printf(m, "          base: 0x%08x\n", chan->pushbuf_base);
+	seq_printf(m, "          base: 0x%10llx\n", chan->pushbuf_base);
 	seq_printf(m, "           max: 0x%08x\n", chan->dma.max << 2);
 	seq_printf(m, "           cur: 0x%08x\n", chan->dma.cur << 2);
 	seq_printf(m, "           put: 0x%08x\n", chan->dma.put << 2);
@@ -178,6 +178,7 @@ static struct drm_info_list nouveau_debugfs_list[] = {
 	{ "memory", nouveau_debugfs_memory_info, 0, NULL },
 	{ "vbios.rom", nouveau_debugfs_vbios_image, 0, NULL },
 	{ "ttm_page_pool", ttm_page_alloc_debugfs, 0, NULL },
+	{ "ttm_dma_page_pool", ttm_dma_page_alloc_debugfs, 0, NULL },
 };
 #define NOUVEAU_DEBUGFS_ENTRIES ARRAY_SIZE(nouveau_debugfs_list)
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index b12fd2c80812..3cb52bc52b21 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -32,6 +32,8 @@
 #include "nouveau_hw.h"
 #include "nouveau_crtc.h"
 #include "nouveau_dma.h"
+#include "nouveau_connector.h"
+#include "nouveau_gpio.h"
 #include "nv50_display.h"
 
 static void
@@ -64,7 +66,7 @@ static const struct drm_framebuffer_funcs nouveau_framebuffer_funcs = {
 int
 nouveau_framebuffer_init(struct drm_device *dev,
 			 struct nouveau_framebuffer *nv_fb,
-			 struct drm_mode_fb_cmd *mode_cmd,
+			 struct drm_mode_fb_cmd2 *mode_cmd,
 			 struct nouveau_bo *nvbo)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
@@ -107,14 +109,14 @@ nouveau_framebuffer_init(struct drm_device *dev,
 
 		if (!tile_flags) {
 			if (dev_priv->card_type < NV_D0)
-				nv_fb->r_pitch = 0x00100000 | fb->pitch;
+				nv_fb->r_pitch = 0x00100000 | fb->pitches[0];
 			else
-				nv_fb->r_pitch = 0x01000000 | fb->pitch;
+				nv_fb->r_pitch = 0x01000000 | fb->pitches[0];
 		} else {
 			u32 mode = nvbo->tile_mode;
 			if (dev_priv->card_type >= NV_C0)
 				mode >>= 4;
-			nv_fb->r_pitch = ((fb->pitch / 4) << 4) | mode;
+			nv_fb->r_pitch = ((fb->pitches[0] / 4) << 4) | mode;
 		}
 	}
 
@@ -124,13 +126,13 @@ nouveau_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 nouveau_user_framebuffer_create(struct drm_device *dev,
 				struct drm_file *file_priv,
-				struct drm_mode_fb_cmd *mode_cmd)
+				struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct nouveau_framebuffer *nouveau_fb;
 	struct drm_gem_object *gem;
 	int ret;
 
-	gem = drm_gem_object_lookup(dev, file_priv, mode_cmd->handle);
+	gem = drm_gem_object_lookup(dev, file_priv, mode_cmd->handles[0]);
 	if (!gem)
 		return ERR_PTR(-ENOENT);
 
@@ -147,11 +149,186 @@ nouveau_user_framebuffer_create(struct drm_device *dev,
 	return &nouveau_fb->base;
 }
 
-const struct drm_mode_config_funcs nouveau_mode_config_funcs = {
+static const struct drm_mode_config_funcs nouveau_mode_config_funcs = {
 	.fb_create = nouveau_user_framebuffer_create,
 	.output_poll_changed = nouveau_fbcon_output_poll_changed,
 };
 
+
+struct drm_prop_enum_list {
+	u8 gen_mask;
+	int type;
+	char *name;
+};
+
+static struct drm_prop_enum_list underscan[] = {
+	{ 6, UNDERSCAN_AUTO, "auto" },
+	{ 6, UNDERSCAN_OFF, "off" },
+	{ 6, UNDERSCAN_ON, "on" },
+	{}
+};
+
+static struct drm_prop_enum_list dither_mode[] = {
+	{ 7, DITHERING_MODE_AUTO, "auto" },
+	{ 7, DITHERING_MODE_OFF, "off" },
+	{ 1, DITHERING_MODE_ON, "on" },
+	{ 6, DITHERING_MODE_STATIC2X2, "static 2x2" },
+	{ 6, DITHERING_MODE_DYNAMIC2X2, "dynamic 2x2" },
+	{ 4, DITHERING_MODE_TEMPORAL, "temporal" },
+	{}
+};
+
+static struct drm_prop_enum_list dither_depth[] = {
+	{ 6, DITHERING_DEPTH_AUTO, "auto" },
+	{ 6, DITHERING_DEPTH_6BPC, "6 bpc" },
+	{ 6, DITHERING_DEPTH_8BPC, "8 bpc" },
+	{}
+};
+
+#define PROP_ENUM(p,gen,n,list) do {                                           \
+	struct drm_prop_enum_list *l = (list);                                 \
+	int c = 0;                                                             \
+	while (l->gen_mask) {                                                  \
+		if (l->gen_mask & (1 << (gen)))                                \
+			c++;                                                   \
+		l++;                                                           \
+	}                                                                      \
+	if (c) {                                                               \
+		p = drm_property_create(dev, DRM_MODE_PROP_ENUM, n, c);        \
+		l = (list);                                                    \
+		c = 0;                                                         \
+		while (p && l->gen_mask) {                                     \
+			if (l->gen_mask & (1 << (gen))) {                      \
+				drm_property_add_enum(p, c, l->type, l->name); \
+				c++;                                           \
+			}                                                      \
+			l++;                                                   \
+		}                                                              \
+	}                                                                      \
+} while(0)
+
+int
+nouveau_display_init(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_display_engine *disp = &dev_priv->engine.display;
+	struct drm_connector *connector;
+	int ret;
+
+	ret = disp->init(dev);
+	if (ret)
+		return ret;
+
+	drm_kms_helper_poll_enable(dev);
+
+	/* enable hotplug interrupts */
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		struct nouveau_connector *conn = nouveau_connector(connector);
+		nouveau_gpio_irq(dev, 0, conn->hpd, 0xff, true);
+	}
+
+	return ret;
+}
+
+void
+nouveau_display_fini(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_display_engine *disp = &dev_priv->engine.display;
+	struct drm_connector *connector;
+
+	/* disable hotplug interrupts */
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		struct nouveau_connector *conn = nouveau_connector(connector);
+		nouveau_gpio_irq(dev, 0, conn->hpd, 0xff, false);
+	}
+
+	drm_kms_helper_poll_disable(dev);
+	disp->fini(dev);
+}
+
+int
+nouveau_display_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_display_engine *disp = &dev_priv->engine.display;
+	int ret, gen;
+
+	drm_mode_config_init(dev);
+	drm_mode_create_scaling_mode_property(dev);
+	drm_mode_create_dvi_i_properties(dev);
+
+	if (dev_priv->card_type < NV_50)
+		gen = 0;
+	else
+	if (dev_priv->card_type < NV_D0)
+		gen = 1;
+	else
+		gen = 2;
+
+	PROP_ENUM(disp->dithering_mode, gen, "dithering mode", dither_mode);
+	PROP_ENUM(disp->dithering_depth, gen, "dithering depth", dither_depth);
+	PROP_ENUM(disp->underscan_property, gen, "underscan", underscan);
+
+	disp->underscan_hborder_property =
+		drm_property_create(dev, DRM_MODE_PROP_RANGE,
+				    "underscan hborder", 2);
+	disp->underscan_hborder_property->values[0] = 0;
+	disp->underscan_hborder_property->values[1] = 128;
+
+	disp->underscan_vborder_property =
+		drm_property_create(dev, DRM_MODE_PROP_RANGE,
+				    "underscan vborder", 2);
+	disp->underscan_vborder_property->values[0] = 0;
+	disp->underscan_vborder_property->values[1] = 128;
+
+	dev->mode_config.funcs = (void *)&nouveau_mode_config_funcs;
+	dev->mode_config.fb_base = pci_resource_start(dev->pdev, 1);
+
+	dev->mode_config.min_width = 0;
+	dev->mode_config.min_height = 0;
+	if (dev_priv->card_type < NV_10) {
+		dev->mode_config.max_width = 2048;
+		dev->mode_config.max_height = 2048;
+	} else
+	if (dev_priv->card_type < NV_50) {
+		dev->mode_config.max_width = 4096;
+		dev->mode_config.max_height = 4096;
+	} else {
+		dev->mode_config.max_width = 8192;
+		dev->mode_config.max_height = 8192;
+	}
+
+	drm_kms_helper_poll_init(dev);
+	drm_kms_helper_poll_disable(dev);
+
+	ret = disp->create(dev);
+	if (ret)
+		return ret;
+
+	if (dev->mode_config.num_crtc) {
+		ret = drm_vblank_init(dev, dev->mode_config.num_crtc);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+void
+nouveau_display_destroy(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_display_engine *disp = &dev_priv->engine.display;
+
+	drm_vblank_cleanup(dev);
+
+	disp->destroy(dev);
+
+	drm_kms_helper_poll_fini(dev);
+	drm_mode_config_cleanup(dev);
+}
+
 int
 nouveau_vblank_enable(struct drm_device *dev, int crtc)
 {
@@ -294,7 +471,7 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	/* Initialize a page flip struct */
 	*s = (struct nouveau_page_flip_state)
 		{ { }, event, nouveau_crtc(crtc)->index,
-		  fb->bits_per_pixel, fb->pitch, crtc->x, crtc->y,
+		  fb->bits_per_pixel, fb->pitches[0], crtc->x, crtc->y,
 		  new_bo->bo.offset };
 
 	/* Choose the channel the flip will be handled in */
@@ -305,7 +482,10 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 
 	/* Emit a page flip */
 	if (dev_priv->card_type >= NV_50) {
-		ret = nv50_display_flip_next(crtc, fb, chan);
+		if (dev_priv->card_type >= NV_D0)
+			ret = nvd0_display_flip_next(crtc, fb, chan, 0);
+		else
+			ret = nv50_display_flip_next(crtc, fb, chan);
 		if (ret) {
 			nouveau_channel_put(&chan);
 			goto fail_unreserve;
diff --git a/drivers/gpu/drm/nouveau/nouveau_dma.c b/drivers/gpu/drm/nouveau/nouveau_dma.c
index 00bc6eaad558..4c2e4e5925fe 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dma.c
+++ b/drivers/gpu/drm/nouveau/nouveau_dma.c
@@ -134,11 +134,13 @@ OUT_RINGp(struct nouveau_channel *chan, const void *data, unsigned nr_dwords)
  *  -EBUSY if timeout exceeded
  */
 static inline int
-READ_GET(struct nouveau_channel *chan, uint32_t *prev_get, uint32_t *timeout)
+READ_GET(struct nouveau_channel *chan, uint64_t *prev_get, int *timeout)
 {
-	uint32_t val;
+	uint64_t val;
 
 	val = nvchan_rd32(chan, chan->user_get);
+        if (chan->user_get_hi)
+                val |= (uint64_t)nvchan_rd32(chan, chan->user_get_hi) << 32;
 
 	/* reset counter as long as GET is still advancing, this is
 	 * to avoid misdetecting a GPU lockup if the GPU happens to
@@ -218,8 +220,8 @@ nv50_dma_push_wait(struct nouveau_channel *chan, int count)
 static int
 nv50_dma_wait(struct nouveau_channel *chan, int slots, int count)
 {
-	uint32_t cnt = 0, prev_get = 0;
-	int ret;
+	uint64_t prev_get = 0;
+	int ret, cnt = 0;
 
 	ret = nv50_dma_push_wait(chan, slots + 1);
 	if (unlikely(ret))
@@ -261,8 +263,8 @@ nv50_dma_wait(struct nouveau_channel *chan, int slots, int count)
 int
 nouveau_dma_wait(struct nouveau_channel *chan, int slots, int size)
 {
-	uint32_t prev_get = 0, cnt = 0;
-	int get;
+	uint64_t prev_get = 0;
+	int cnt = 0, get;
 
 	if (chan->dma.ib_max)
 		return nv50_dma_wait(chan, slots, size);
diff --git a/drivers/gpu/drm/nouveau/nouveau_dp.c b/drivers/gpu/drm/nouveau/nouveau_dp.c
index de5efe71fefd..9b93b703ceab 100644
--- a/drivers/gpu/drm/nouveau/nouveau_dp.c
+++ b/drivers/gpu/drm/nouveau/nouveau_dp.c
@@ -29,6 +29,7 @@
 #include "nouveau_connector.h"
 #include "nouveau_encoder.h"
 #include "nouveau_crtc.h"
+#include "nouveau_gpio.h"
 
 /******************************************************************************
  * aux channel util functions
@@ -273,8 +274,6 @@ nouveau_dp_tu_update(struct drm_device *dev, int or, int link, u32 clk, u32 bpp)
 u8 *
 nouveau_dp_bios_data(struct drm_device *dev, struct dcb_entry *dcb, u8 **entry)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nvbios *bios = &dev_priv->vbios;
 	struct bit_entry d;
 	u8 *table;
 	int i;
@@ -289,7 +288,7 @@ nouveau_dp_bios_data(struct drm_device *dev, struct dcb_entry *dcb, u8 **entry)
 		return NULL;
 	}
 
-	table = ROMPTR(bios, d.data[0]);
+	table = ROMPTR(dev, d.data[0]);
 	if (!table) {
 		NV_ERROR(dev, "displayport table pointer invalid\n");
 		return NULL;
@@ -306,7 +305,7 @@ nouveau_dp_bios_data(struct drm_device *dev, struct dcb_entry *dcb, u8 **entry)
 	}
 
 	for (i = 0; i < table[3]; i++) {
-		*entry = ROMPTR(bios, table[table[1] + (i * table[2])]);
+		*entry = ROMPTR(dev, table[table[1] + (i * table[2])]);
 		if (*entry && bios_encoder_match(dcb, ROM32((*entry)[0])))
 			return table;
 	}
@@ -336,7 +335,6 @@ struct dp_state {
 static void
 dp_set_link_config(struct drm_device *dev, struct dp_state *dp)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	int or = dp->or, link = dp->link;
 	u8 *entry, sink[2];
 	u32 dp_ctrl;
@@ -360,7 +358,7 @@ dp_set_link_config(struct drm_device *dev, struct dp_state *dp)
 	 * table, that has (among other things) pointers to more scripts that
 	 * need to be executed, this time depending on link speed.
 	 */
-	entry = ROMPTR(&dev_priv->vbios, dp->entry[10]);
+	entry = ROMPTR(dev, dp->entry[10]);
 	if (entry) {
 		if (dp->table[0] < 0x30) {
 			while (dp->link_bw < (ROM16(entry[0]) * 10))
@@ -559,8 +557,6 @@ dp_link_train_eq(struct drm_device *dev, struct dp_state *dp)
 bool
 nouveau_dp_link_train(struct drm_encoder *encoder, u32 datarate)
 {
-	struct drm_nouveau_private *dev_priv = encoder->dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(encoder->crtc);
 	struct nouveau_connector *nv_connector =
@@ -581,7 +577,7 @@ nouveau_dp_link_train(struct drm_encoder *encoder, u32 datarate)
 
 	dp.dcb = nv_encoder->dcb;
 	dp.crtc = nv_crtc->index;
-	dp.auxch = auxch->rd;
+	dp.auxch = auxch->drive;
 	dp.or = nv_encoder->or;
 	dp.link = !(nv_encoder->dcb->sorconf.link & 1);
 	dp.dpcd = nv_encoder->dp.dpcd;
@@ -590,7 +586,7 @@ nouveau_dp_link_train(struct drm_encoder *encoder, u32 datarate)
 	 * we take during link training (DP_SET_POWER is one), we need
 	 * to ignore them for the moment to avoid races.
 	 */
-	pgpio->irq_enable(dev, nv_connector->dcb->gpio_tag, false);
+	nouveau_gpio_irq(dev, 0, nv_connector->hpd, 0xff, false);
 
 	/* enable down-spreading, if possible */
 	if (dp.table[1] >= 16) {
@@ -639,7 +635,7 @@ nouveau_dp_link_train(struct drm_encoder *encoder, u32 datarate)
 	nouveau_bios_run_init_table(dev, ROM16(dp.entry[8]), dp.dcb, dp.crtc);
 
 	/* re-enable hotplug detect */
-	pgpio->irq_enable(dev, nv_connector->dcb->gpio_tag, true);
+	nouveau_gpio_irq(dev, 0, nv_connector->hpd, 0xff, true);
 	return true;
 }
 
@@ -656,7 +652,7 @@ nouveau_dp_detect(struct drm_encoder *encoder)
 	if (!auxch)
 		return false;
 
-	ret = auxch_tx(dev, auxch->rd, 9, DP_DPCD_REV, dpcd, 8);
+	ret = auxch_tx(dev, auxch->drive, 9, DP_DPCD_REV, dpcd, 8);
 	if (ret)
 		return false;
 
@@ -684,7 +680,7 @@ int
 nouveau_dp_auxch(struct nouveau_i2c_chan *auxch, int cmd, int addr,
 		 uint8_t *data, int data_nr)
 {
-	return auxch_tx(auxch->dev, auxch->rd, cmd, addr, data, data_nr);
+	return auxch_tx(auxch->dev, auxch->drive, cmd, addr, data, data_nr);
 }
 
 static int
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.c b/drivers/gpu/drm/nouveau/nouveau_drv.c
index 9791d13c9e3b..e4a7cfe7898d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.c
@@ -124,6 +124,10 @@ MODULE_PARM_DESC(ctxfw, "Use external HUB/GPC ucode (fermi)\n");
 int nouveau_ctxfw;
 module_param_named(ctxfw, nouveau_ctxfw, int, 0400);
 
+MODULE_PARM_DESC(ctxfw, "Santise DCB table according to MXM-SIS\n");
+int nouveau_mxmdcb = 1;
+module_param_named(mxmdcb, nouveau_mxmdcb, int, 0400);
+
 int nouveau_fbpercrtc;
 #if 0
 module_param_named(fbpercrtc, nouveau_fbpercrtc, int, 0400);
@@ -178,8 +182,11 @@ nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state)
 	if (dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 		return 0;
 
-	NV_INFO(dev, "Disabling fbcon acceleration...\n");
-	nouveau_fbcon_save_disable_accel(dev);
+	NV_INFO(dev, "Disabling display...\n");
+	nouveau_display_fini(dev);
+
+	NV_INFO(dev, "Disabling fbcon...\n");
+	nouveau_fbcon_set_suspend(dev, 1);
 
 	NV_INFO(dev, "Unpinning framebuffer(s)...\n");
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
@@ -220,7 +227,7 @@ nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state)
 
 		ret = dev_priv->eng[e]->fini(dev, e, true);
 		if (ret) {
-			NV_ERROR(dev, "... engine %d failed: %d\n", i, ret);
+			NV_ERROR(dev, "... engine %d failed: %d\n", e, ret);
 			goto out_abort;
 		}
 	}
@@ -246,10 +253,6 @@ nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state)
 		pci_set_power_state(pdev, PCI_D3hot);
 	}
 
-	console_lock();
-	nouveau_fbcon_set_suspend(dev, 1);
-	console_unlock();
-	nouveau_fbcon_restore_accel(dev);
 	return 0;
 
 out_abort:
@@ -275,8 +278,6 @@ nouveau_pci_resume(struct pci_dev *pdev)
 	if (dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 		return 0;
 
-	nouveau_fbcon_save_disable_accel(dev);
-
 	NV_INFO(dev, "We're back, enabling device...\n");
 	pci_set_power_state(pdev, PCI_D0);
 	pci_restore_state(pdev);
@@ -296,8 +297,6 @@ nouveau_pci_resume(struct pci_dev *pdev)
 	if (ret)
 		return ret;
 
-	nouveau_pm_resume(dev);
-
 	if (dev_priv->gart_info.type == NOUVEAU_GART_AGP) {
 		ret = nouveau_mem_init_agp(dev);
 		if (ret) {
@@ -337,6 +336,8 @@ nouveau_pci_resume(struct pci_dev *pdev)
 		}
 	}
 
+	nouveau_pm_resume(dev);
+
 	NV_INFO(dev, "Restoring mode...\n");
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		struct nouveau_framebuffer *nouveau_fb;
@@ -358,16 +359,10 @@ nouveau_pci_resume(struct pci_dev *pdev)
 			NV_ERROR(dev, "Could not pin/map cursor.\n");
 	}
 
-	engine->display.init(dev);
-
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
-		struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
-		u32 offset = nv_crtc->cursor.nvbo->bo.offset;
+	nouveau_fbcon_set_suspend(dev, 0);
+	nouveau_fbcon_zfill_all(dev);
 
-		nv_crtc->cursor.set_offset(nv_crtc, offset);
-		nv_crtc->cursor.set_pos(nv_crtc, nv_crtc->cursor_saved_x,
-						 nv_crtc->cursor_saved_y);
-	}
+	nouveau_display_init(dev);
 
 	/* Force CLUT to get re-loaded during modeset */
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
@@ -376,18 +371,35 @@ nouveau_pci_resume(struct pci_dev *pdev)
 		nv_crtc->lut.depth = 0;
 	}
 
-	console_lock();
-	nouveau_fbcon_set_suspend(dev, 0);
-	console_unlock();
+	drm_helper_resume_force_mode(dev);
 
-	nouveau_fbcon_zfill_all(dev);
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+		u32 offset = nv_crtc->cursor.nvbo->bo.offset;
 
-	drm_helper_resume_force_mode(dev);
+		nv_crtc->cursor.set_offset(nv_crtc, offset);
+		nv_crtc->cursor.set_pos(nv_crtc, nv_crtc->cursor_saved_x,
+						 nv_crtc->cursor_saved_y);
+	}
 
-	nouveau_fbcon_restore_accel(dev);
 	return 0;
 }
 
+static const struct file_operations nouveau_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = nouveau_ttm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+#if defined(CONFIG_COMPAT)
+	.compat_ioctl = nouveau_compat_ioctl,
+#endif
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	.driver_features =
 		DRIVER_USE_AGP | DRIVER_PCI_DMA | DRIVER_SG |
@@ -413,21 +425,7 @@ static struct drm_driver driver = {
 	.disable_vblank = nouveau_vblank_disable,
 	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = nouveau_ioctls,
-	.fops = {
-		.owner = THIS_MODULE,
-		.open = drm_open,
-		.release = drm_release,
-		.unlocked_ioctl = drm_ioctl,
-		.mmap = nouveau_ttm_mmap,
-		.poll = drm_poll,
-		.fasync = drm_fasync,
-		.read = drm_read,
-#if defined(CONFIG_COMPAT)
-		.compat_ioctl = nouveau_compat_ioctl,
-#endif
-		.llseek = noop_llseek,
-	},
-
+	.fops = &nouveau_driver_fops,
 	.gem_init_object = nouveau_gem_object_new,
 	.gem_free_object = nouveau_gem_object_del,
 	.gem_open_object = nouveau_gem_object_open,
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.h b/drivers/gpu/drm/nouveau/nouveau_drv.h
index 4c0be3a4ed88..38134a9c7578 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.h
@@ -163,6 +163,9 @@ enum nouveau_flags {
 #define NVOBJ_ENGINE_COPY0	3
 #define NVOBJ_ENGINE_COPY1	4
 #define NVOBJ_ENGINE_MPEG	5
+#define NVOBJ_ENGINE_PPP	NVOBJ_ENGINE_MPEG
+#define NVOBJ_ENGINE_BSP	6
+#define NVOBJ_ENGINE_VP		7
 #define NVOBJ_ENGINE_DISPLAY	15
 #define NVOBJ_ENGINE_NR		16
 
@@ -229,6 +232,7 @@ struct nouveau_channel {
 	/* mapping of the regs controlling the fifo */
 	void __iomem *user;
 	uint32_t user_get;
+	uint32_t user_get_hi;
 	uint32_t user_put;
 
 	/* Fencing */
@@ -246,7 +250,7 @@ struct nouveau_channel {
 	struct nouveau_gpuobj *pushbuf;
 	struct nouveau_bo     *pushbuf_bo;
 	struct nouveau_vma     pushbuf_vma;
-	uint32_t               pushbuf_base;
+	uint64_t               pushbuf_base;
 
 	/* Notifier memory */
 	struct nouveau_bo *notifier_bo;
@@ -393,24 +397,25 @@ struct nouveau_display_engine {
 	int (*early_init)(struct drm_device *);
 	void (*late_takedown)(struct drm_device *);
 	int (*create)(struct drm_device *);
-	int (*init)(struct drm_device *);
 	void (*destroy)(struct drm_device *);
+	int (*init)(struct drm_device *);
+	void (*fini)(struct drm_device *);
+
+	struct drm_property *dithering_mode;
+	struct drm_property *dithering_depth;
+	struct drm_property *underscan_property;
+	struct drm_property *underscan_hborder_property;
+	struct drm_property *underscan_vborder_property;
 };
 
 struct nouveau_gpio_engine {
-	void *priv;
-
-	int  (*init)(struct drm_device *);
-	void (*takedown)(struct drm_device *);
-
-	int  (*get)(struct drm_device *, enum dcb_gpio_tag);
-	int  (*set)(struct drm_device *, enum dcb_gpio_tag, int state);
-
-	int  (*irq_register)(struct drm_device *, enum dcb_gpio_tag,
-			     void (*)(void *, int), void *);
-	void (*irq_unregister)(struct drm_device *, enum dcb_gpio_tag,
-			       void (*)(void *, int), void *);
-	bool (*irq_enable)(struct drm_device *, enum dcb_gpio_tag, bool on);
+	spinlock_t lock;
+	struct list_head isr;
+	int (*init)(struct drm_device *);
+	void (*fini)(struct drm_device *);
+	int (*drive)(struct drm_device *, int line, int dir, int out);
+	int (*sense)(struct drm_device *, int line);
+	void (*irq_enable)(struct drm_device *, int line, bool);
 };
 
 struct nouveau_pm_voltage_level {
@@ -484,7 +489,7 @@ struct nouveau_pm_level {
 	u32 copy;
 	u32 daemon;
 	u32 vdec;
-	u32 unk05;	/* nv50:nva3, roughly.. */
+	u32 dom6;
 	u32 unka0;	/* nva3:nvc0 */
 	u32 hub01;	/* nvc0- */
 	u32 hub06;	/* nvc0- */
@@ -518,6 +523,12 @@ struct nouveau_pm_memtimings {
 	int nr_timing;
 };
 
+struct nouveau_pm_fan {
+	u32 min_duty;
+	u32 max_duty;
+	u32 pwm_freq;
+};
+
 struct nouveau_pm_engine {
 	struct nouveau_pm_voltage voltage;
 	struct nouveau_pm_level perflvl[NOUVEAU_PM_MAX_LEVEL];
@@ -525,6 +536,8 @@ struct nouveau_pm_engine {
 	struct nouveau_pm_memtimings memtimings;
 	struct nouveau_pm_temp_sensor_constants sensor_constants;
 	struct nouveau_pm_threshold_temp threshold_temp;
+	struct nouveau_pm_fan fan;
+	u32 pwm_divisor;
 
 	struct nouveau_pm_level boot;
 	struct nouveau_pm_level *cur;
@@ -532,19 +545,14 @@ struct nouveau_pm_engine {
 	struct device *hwmon;
 	struct notifier_block acpi_nb;
 
-	int (*clock_get)(struct drm_device *, u32 id);
-	void *(*clock_pre)(struct drm_device *, struct nouveau_pm_level *,
-			   u32 id, int khz);
-	void (*clock_set)(struct drm_device *, void *);
-
 	int  (*clocks_get)(struct drm_device *, struct nouveau_pm_level *);
 	void *(*clocks_pre)(struct drm_device *, struct nouveau_pm_level *);
-	void (*clocks_set)(struct drm_device *, void *);
+	int (*clocks_set)(struct drm_device *, void *);
 
 	int (*voltage_get)(struct drm_device *);
 	int (*voltage_set)(struct drm_device *, int voltage);
-	int (*fanspeed_get)(struct drm_device *);
-	int (*fanspeed_set)(struct drm_device *, int fanspeed);
+	int (*pwm_get)(struct drm_device *, int line, u32*, u32*);
+	int (*pwm_set)(struct drm_device *, int line, u32, u32);
 	int (*temp_get)(struct drm_device *);
 };
 
@@ -780,6 +788,8 @@ struct drm_nouveau_private {
 	struct nouveau_vm *chan_vm;
 
 	struct nvbios vbios;
+	u8 *mxms;
+	struct list_head i2c_ports;
 
 	struct nv04_mode_state mode_reg;
 	struct nv04_mode_state saved_reg;
@@ -850,6 +860,7 @@ extern char *nouveau_perflvl;
 extern int nouveau_perflvl_wr;
 extern int nouveau_msi;
 extern int nouveau_ctxfw;
+extern int nouveau_mxmdcb;
 
 extern int nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state);
 extern int nouveau_pci_resume(struct pci_dev *pdev);
@@ -1000,7 +1011,10 @@ extern int nouveau_sgdma_init(struct drm_device *);
 extern void nouveau_sgdma_takedown(struct drm_device *);
 extern uint32_t nouveau_sgdma_get_physical(struct drm_device *,
 					   uint32_t offset);
-extern struct ttm_backend *nouveau_sgdma_init_ttm(struct drm_device *);
+extern struct ttm_tt *nouveau_sgdma_create_ttm(struct ttm_bo_device *bdev,
+					       unsigned long size,
+					       uint32_t page_flags,
+					       struct page *dummy_read_page);
 
 /* nouveau_debugfs.c */
 #if defined(CONFIG_DRM_NOUVEAU_DEBUG)
@@ -1072,8 +1086,6 @@ extern int nouveau_run_vbios_init(struct drm_device *);
 extern void nouveau_bios_run_init_table(struct drm_device *, uint16_t table,
 					struct dcb_entry *, int crtc);
 extern void nouveau_bios_init_exec(struct drm_device *, uint16_t table);
-extern struct dcb_gpio_entry *nouveau_bios_gpio_entry(struct drm_device *,
-						      enum dcb_gpio_tag);
 extern struct dcb_connector_table_entry *
 nouveau_bios_connector_entry(struct drm_device *, int index);
 extern u32 get_pll_register(struct drm_device *, enum pll_types);
@@ -1091,11 +1103,18 @@ extern int call_lvds_script(struct drm_device *, struct dcb_entry *, int head,
 			    enum LVDS_script, int pxclk);
 bool bios_encoder_match(struct dcb_entry *, u32 hash);
 
+/* nouveau_mxm.c */
+int  nouveau_mxm_init(struct drm_device *dev);
+void nouveau_mxm_fini(struct drm_device *dev);
+
 /* nouveau_ttm.c */
 int nouveau_ttm_global_init(struct drm_nouveau_private *);
 void nouveau_ttm_global_release(struct drm_nouveau_private *);
 int nouveau_ttm_mmap(struct file *, struct vm_area_struct *);
 
+/* nouveau_hdmi.c */
+void nouveau_hdmi_mode_set(struct drm_encoder *, struct drm_display_mode *);
+
 /* nouveau_dp.c */
 int nouveau_dp_auxch(struct nouveau_i2c_chan *auxch, int cmd, int addr,
 		     uint8_t *data, int data_nr);
@@ -1222,6 +1241,9 @@ extern int  nvc0_graph_isr_chid(struct drm_device *dev, u64 inst);
 /* nv84_crypt.c */
 extern int  nv84_crypt_create(struct drm_device *);
 
+/* nv98_crypt.c */
+extern int  nv98_crypt_create(struct drm_device *dev);
+
 /* nva3_copy.c */
 extern int  nva3_copy_create(struct drm_device *dev);
 
@@ -1234,6 +1256,17 @@ extern int  nv31_mpeg_create(struct drm_device *dev);
 /* nv50_mpeg.c */
 extern int  nv50_mpeg_create(struct drm_device *dev);
 
+/* nv84_bsp.c */
+/* nv98_bsp.c */
+extern int  nv84_bsp_create(struct drm_device *dev);
+
+/* nv84_vp.c */
+/* nv98_vp.c */
+extern int  nv84_vp_create(struct drm_device *dev);
+
+/* nv98_ppp.c */
+extern int  nv98_ppp_create(struct drm_device *dev);
+
 /* nv04_instmem.c */
 extern int  nv04_instmem_init(struct drm_device *);
 extern void nv04_instmem_takedown(struct drm_device *);
@@ -1311,13 +1344,19 @@ extern int nv17_tv_create(struct drm_connector *, struct dcb_entry *);
 extern int nv04_display_early_init(struct drm_device *);
 extern void nv04_display_late_takedown(struct drm_device *);
 extern int nv04_display_create(struct drm_device *);
-extern int nv04_display_init(struct drm_device *);
 extern void nv04_display_destroy(struct drm_device *);
+extern int nv04_display_init(struct drm_device *);
+extern void nv04_display_fini(struct drm_device *);
 
 /* nvd0_display.c */
 extern int nvd0_display_create(struct drm_device *);
-extern int nvd0_display_init(struct drm_device *);
 extern void nvd0_display_destroy(struct drm_device *);
+extern int nvd0_display_init(struct drm_device *);
+extern void nvd0_display_fini(struct drm_device *);
+struct nouveau_bo *nvd0_display_crtc_sema(struct drm_device *, int crtc);
+void nvd0_display_flip_stop(struct drm_crtc *);
+int nvd0_display_flip_next(struct drm_crtc *, struct drm_framebuffer *,
+			   struct nouveau_channel *, u32 swap_interval);
 
 /* nv04_crtc.c */
 extern int nv04_crtc_create(struct drm_device *, int index);
@@ -1412,6 +1451,10 @@ extern int nouveau_gem_ioctl_info(struct drm_device *, void *,
 				  struct drm_file *);
 
 /* nouveau_display.c */
+int nouveau_display_create(struct drm_device *dev);
+void nouveau_display_destroy(struct drm_device *dev);
+int nouveau_display_init(struct drm_device *dev);
+void nouveau_display_fini(struct drm_device *dev);
 int nouveau_vblank_enable(struct drm_device *dev, int crtc);
 void nouveau_vblank_disable(struct drm_device *dev, int crtc);
 int nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
@@ -1426,23 +1469,22 @@ int nouveau_display_dumb_destroy(struct drm_file *, struct drm_device *,
 				 uint32_t handle);
 
 /* nv10_gpio.c */
-int nv10_gpio_get(struct drm_device *dev, enum dcb_gpio_tag tag);
-int nv10_gpio_set(struct drm_device *dev, enum dcb_gpio_tag tag, int state);
+int nv10_gpio_init(struct drm_device *dev);
+void nv10_gpio_fini(struct drm_device *dev);
+int nv10_gpio_drive(struct drm_device *dev, int line, int dir, int out);
+int nv10_gpio_sense(struct drm_device *dev, int line);
+void nv10_gpio_irq_enable(struct drm_device *, int line, bool on);
 
 /* nv50_gpio.c */
 int nv50_gpio_init(struct drm_device *dev);
 void nv50_gpio_fini(struct drm_device *dev);
-int nv50_gpio_get(struct drm_device *dev, enum dcb_gpio_tag tag);
-int nv50_gpio_set(struct drm_device *dev, enum dcb_gpio_tag tag, int state);
-int nvd0_gpio_get(struct drm_device *dev, enum dcb_gpio_tag tag);
-int nvd0_gpio_set(struct drm_device *dev, enum dcb_gpio_tag tag, int state);
-int  nv50_gpio_irq_register(struct drm_device *, enum dcb_gpio_tag,
-			    void (*)(void *, int), void *);
-void nv50_gpio_irq_unregister(struct drm_device *, enum dcb_gpio_tag,
-			      void (*)(void *, int), void *);
-bool nv50_gpio_irq_enable(struct drm_device *, enum dcb_gpio_tag, bool on);
-
-/* nv50_calc. */
+int nv50_gpio_drive(struct drm_device *dev, int line, int dir, int out);
+int nv50_gpio_sense(struct drm_device *dev, int line);
+void nv50_gpio_irq_enable(struct drm_device *, int line, bool on);
+int nvd0_gpio_drive(struct drm_device *dev, int line, int dir, int out);
+int nvd0_gpio_sense(struct drm_device *dev, int line);
+
+/* nv50_calc.c */
 int nv50_calc_pll(struct drm_device *, struct pll_lims *, int clk,
 		  int *N1, int *M1, int *N2, int *M2, int *P);
 int nva3_calc_pll(struct drm_device *, struct pll_lims *,
@@ -1565,6 +1607,13 @@ extern void nv_wo32(struct nouveau_gpuobj *, u32 offset, u32 val);
 #define NV_TRACEWARN(d, fmt, arg...) NV_PRINTK(KERN_NOTICE, d, fmt, ##arg)
 #define NV_TRACE(d, fmt, arg...) NV_PRINTK(KERN_INFO, d, fmt, ##arg)
 #define NV_WARN(d, fmt, arg...) NV_PRINTK(KERN_WARNING, d, fmt, ##arg)
+#define NV_WARNONCE(d, fmt, arg...) do {                                       \
+	static int _warned = 0;                                                \
+	if (!_warned) {                                                        \
+		NV_WARN(d, fmt, ##arg);                                        \
+		_warned = 1;                                                   \
+	}                                                                      \
+} while(0)
 
 /* nouveau_reg_debug bitmask */
 enum {
diff --git a/drivers/gpu/drm/nouveau/nouveau_fb.h b/drivers/gpu/drm/nouveau/nouveau_fb.h
index 95c843e684bb..f3fb649fe454 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fb.h
+++ b/drivers/gpu/drm/nouveau/nouveau_fb.h
@@ -42,8 +42,6 @@ nouveau_framebuffer(struct drm_framebuffer *fb)
 	return container_of(fb, struct nouveau_framebuffer, base);
 }
 
-extern const struct drm_mode_config_funcs nouveau_mode_config_funcs;
-
 int nouveau_framebuffer_init(struct drm_device *dev, struct nouveau_framebuffer *nouveau_fb,
-			     struct drm_mode_fb_cmd *mode_cmd, struct nouveau_bo *nvbo);
+			     struct drm_mode_fb_cmd2 *mode_cmd, struct nouveau_bo *nvbo);
 #endif /* __NOUVEAU_FB_H__ */
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.c b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
index 3a4cc32b9e44..9892218d7452 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
@@ -36,6 +36,7 @@
 #include <linux/init.h>
 #include <linux/screen_info.h>
 #include <linux/vga_switcheroo.h>
+#include <linux/console.h>
 
 #include "drmP.h"
 #include "drm.h"
@@ -281,7 +282,7 @@ nouveau_fbcon_create(struct nouveau_fbdev *nfbdev,
 	struct nouveau_framebuffer *nouveau_fb;
 	struct nouveau_channel *chan;
 	struct nouveau_bo *nvbo;
-	struct drm_mode_fb_cmd mode_cmd;
+	struct drm_mode_fb_cmd2 mode_cmd;
 	struct pci_dev *pdev = dev->pdev;
 	struct device *device = &pdev->dev;
 	int size, ret;
@@ -289,12 +290,13 @@ nouveau_fbcon_create(struct nouveau_fbdev *nfbdev,
 	mode_cmd.width = sizes->surface_width;
 	mode_cmd.height = sizes->surface_height;
 
-	mode_cmd.bpp = sizes->surface_bpp;
-	mode_cmd.pitch = mode_cmd.width * (mode_cmd.bpp >> 3);
-	mode_cmd.pitch = roundup(mode_cmd.pitch, 256);
-	mode_cmd.depth = sizes->surface_depth;
+	mode_cmd.pitches[0] = mode_cmd.width * (sizes->surface_bpp >> 3);
+	mode_cmd.pitches[0] = roundup(mode_cmd.pitches[0], 256);
 
-	size = mode_cmd.pitch * mode_cmd.height;
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
+
+	size = mode_cmd.pitches[0] * mode_cmd.height;
 	size = roundup(size, PAGE_SIZE);
 
 	ret = nouveau_gem_new(dev, size, 0, NOUVEAU_GEM_DOMAIN_VRAM,
@@ -369,7 +371,7 @@ nouveau_fbcon_create(struct nouveau_fbdev *nfbdev,
 	info->screen_base = nvbo_kmap_obj_iovirtual(nouveau_fb->nvbo);
 	info->screen_size = size;
 
-	drm_fb_helper_fill_fix(info, fb->pitch, fb->depth);
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
 	drm_fb_helper_fill_var(info, &nfbdev->helper, sizes->fb_width, sizes->fb_height);
 
 	/* Set aperture base/size for vesafb takeover */
@@ -547,7 +549,13 @@ void nouveau_fbcon_restore_accel(struct drm_device *dev)
 void nouveau_fbcon_set_suspend(struct drm_device *dev, int state)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	console_lock();
+	if (state == 0)
+		nouveau_fbcon_save_disable_accel(dev);
 	fb_set_suspend(dev_priv->nfbdev->helper.fbdev, state);
+	if (state == 1)
+		nouveau_fbcon_restore_accel(dev);
+	console_unlock();
 }
 
 void nouveau_fbcon_zfill_all(struct drm_device *dev)
diff --git a/drivers/gpu/drm/nouveau/nouveau_gpio.c b/drivers/gpu/drm/nouveau/nouveau_gpio.c
new file mode 100644
index 000000000000..a580cc62337a
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_gpio.c
@@ -0,0 +1,400 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_i2c.h"
+#include "nouveau_gpio.h"
+
+static u8 *
+dcb_gpio_table(struct drm_device *dev)
+{
+	u8 *dcb = dcb_table(dev);
+	if (dcb) {
+		if (dcb[0] >= 0x30 && dcb[1] >= 0x0c)
+			return ROMPTR(dev, dcb[0x0a]);
+		if (dcb[0] >= 0x22 && dcb[-1] >= 0x13)
+			return ROMPTR(dev, dcb[-15]);
+	}
+	return NULL;
+}
+
+static u8 *
+dcb_gpio_entry(struct drm_device *dev, int idx, int ent, u8 *version)
+{
+	u8 *table = dcb_gpio_table(dev);
+	if (table) {
+		*version = table[0];
+		if (*version < 0x30 && ent < table[2])
+			return table + 3 + (ent * table[1]);
+		else if (ent < table[2])
+			return table + table[1] + (ent * table[3]);
+	}
+	return NULL;
+}
+
+int
+nouveau_gpio_drive(struct drm_device *dev, int idx, int line, int dir, int out)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+
+	return pgpio->drive ? pgpio->drive(dev, line, dir, out) : -ENODEV;
+}
+
+int
+nouveau_gpio_sense(struct drm_device *dev, int idx, int line)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+
+	return pgpio->sense ? pgpio->sense(dev, line) : -ENODEV;
+}
+
+int
+nouveau_gpio_find(struct drm_device *dev, int idx, u8 func, u8 line,
+		  struct gpio_func *gpio)
+{
+	u8 *table, *entry, version;
+	int i = -1;
+
+	if (line == 0xff && func == 0xff)
+		return -EINVAL;
+
+	while ((entry = dcb_gpio_entry(dev, idx, ++i, &version))) {
+		if (version < 0x40) {
+			u16 data = ROM16(entry[0]);
+			*gpio = (struct gpio_func) {
+				.line = (data & 0x001f) >> 0,
+				.func = (data & 0x07e0) >> 5,
+				.log[0] = (data & 0x1800) >> 11,
+				.log[1] = (data & 0x6000) >> 13,
+			};
+		} else
+		if (version < 0x41) {
+			*gpio = (struct gpio_func) {
+				.line = entry[0] & 0x1f,
+				.func = entry[1],
+				.log[0] = (entry[3] & 0x18) >> 3,
+				.log[1] = (entry[3] & 0x60) >> 5,
+			};
+		} else {
+			*gpio = (struct gpio_func) {
+				.line = entry[0] & 0x3f,
+				.func = entry[1],
+				.log[0] = (entry[4] & 0x30) >> 4,
+				.log[1] = (entry[4] & 0xc0) >> 6,
+			};
+		}
+
+		if ((line == 0xff || line == gpio->line) &&
+		    (func == 0xff || func == gpio->func))
+			return 0;
+	}
+
+	/* DCB 2.2, fixed TVDAC GPIO data */
+	if ((table = dcb_table(dev)) && table[0] >= 0x22) {
+		if (func == DCB_GPIO_TVDAC0) {
+			*gpio = (struct gpio_func) {
+				.func = DCB_GPIO_TVDAC0,
+				.line = table[-4] >> 4,
+				.log[0] = !!(table[-5] & 2),
+				.log[1] =  !(table[-5] & 2),
+			};
+			return 0;
+		}
+	}
+
+	/* Apple iMac G4 NV18 */
+	if (nv_match_device(dev, 0x0189, 0x10de, 0x0010)) {
+		if (func == DCB_GPIO_TVDAC0) {
+			*gpio = (struct gpio_func) {
+				.func = DCB_GPIO_TVDAC0,
+				.line = 4,
+				.log[0] = 0,
+				.log[1] = 1,
+			};
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+int
+nouveau_gpio_set(struct drm_device *dev, int idx, u8 tag, u8 line, int state)
+{
+	struct gpio_func gpio;
+	int ret;
+
+	ret = nouveau_gpio_find(dev, idx, tag, line, &gpio);
+	if (ret == 0) {
+		int dir = !!(gpio.log[state] & 0x02);
+		int out = !!(gpio.log[state] & 0x01);
+		ret = nouveau_gpio_drive(dev, idx, gpio.line, dir, out);
+	}
+
+	return ret;
+}
+
+int
+nouveau_gpio_get(struct drm_device *dev, int idx, u8 tag, u8 line)
+{
+	struct gpio_func gpio;
+	int ret;
+
+	ret = nouveau_gpio_find(dev, idx, tag, line, &gpio);
+	if (ret == 0) {
+		ret = nouveau_gpio_sense(dev, idx, gpio.line);
+		if (ret >= 0)
+			ret = (ret == (gpio.log[1] & 1));
+	}
+
+	return ret;
+}
+
+int
+nouveau_gpio_irq(struct drm_device *dev, int idx, u8 tag, u8 line, bool on)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+	struct gpio_func gpio;
+	int ret;
+
+	ret = nouveau_gpio_find(dev, idx, tag, line, &gpio);
+	if (ret == 0) {
+		if (idx == 0 && pgpio->irq_enable)
+			pgpio->irq_enable(dev, gpio.line, on);
+		else
+			ret = -ENODEV;
+	}
+
+	return ret;
+}
+
+struct gpio_isr {
+	struct drm_device *dev;
+	struct list_head head;
+	struct work_struct work;
+	int idx;
+	struct gpio_func func;
+	void (*handler)(void *, int);
+	void *data;
+	bool inhibit;
+};
+
+static void
+nouveau_gpio_isr_bh(struct work_struct *work)
+{
+	struct gpio_isr *isr = container_of(work, struct gpio_isr, work);
+	struct drm_device *dev = isr->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+	unsigned long flags;
+	int state;
+
+	state = nouveau_gpio_get(dev, isr->idx, isr->func.func, isr->func.line);
+	if (state >= 0)
+		isr->handler(isr->data, state);
+
+	spin_lock_irqsave(&pgpio->lock, flags);
+	isr->inhibit = false;
+	spin_unlock_irqrestore(&pgpio->lock, flags);
+}
+
+void
+nouveau_gpio_isr(struct drm_device *dev, int idx, u32 line_mask)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+	struct gpio_isr *isr;
+
+	if (idx != 0)
+		return;
+
+	spin_lock(&pgpio->lock);
+	list_for_each_entry(isr, &pgpio->isr, head) {
+		if (line_mask & (1 << isr->func.line)) {
+			if (isr->inhibit)
+				continue;
+			isr->inhibit = true;
+			schedule_work(&isr->work);
+		}
+	}
+	spin_unlock(&pgpio->lock);
+}
+
+int
+nouveau_gpio_isr_add(struct drm_device *dev, int idx, u8 tag, u8 line,
+		     void (*handler)(void *, int), void *data)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+	struct gpio_isr *isr;
+	unsigned long flags;
+	int ret;
+
+	isr = kzalloc(sizeof(*isr), GFP_KERNEL);
+	if (!isr)
+		return -ENOMEM;
+
+	ret = nouveau_gpio_find(dev, idx, tag, line, &isr->func);
+	if (ret) {
+		kfree(isr);
+		return ret;
+	}
+
+	INIT_WORK(&isr->work, nouveau_gpio_isr_bh);
+	isr->dev = dev;
+	isr->handler = handler;
+	isr->data = data;
+	isr->idx = idx;
+
+	spin_lock_irqsave(&pgpio->lock, flags);
+	list_add(&isr->head, &pgpio->isr);
+	spin_unlock_irqrestore(&pgpio->lock, flags);
+	return 0;
+}
+
+void
+nouveau_gpio_isr_del(struct drm_device *dev, int idx, u8 tag, u8 line,
+		     void (*handler)(void *, int), void *data)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+	struct gpio_isr *isr, *tmp;
+	struct gpio_func func;
+	unsigned long flags;
+	LIST_HEAD(tofree);
+	int ret;
+
+	ret = nouveau_gpio_find(dev, idx, tag, line, &func);
+	if (ret == 0) {
+		spin_lock_irqsave(&pgpio->lock, flags);
+		list_for_each_entry_safe(isr, tmp, &pgpio->isr, head) {
+			if (memcmp(&isr->func, &func, sizeof(func)) ||
+			    isr->idx != idx ||
+			    isr->handler != handler || isr->data != data)
+				continue;
+			list_move(&isr->head, &tofree);
+		}
+		spin_unlock_irqrestore(&pgpio->lock, flags);
+
+		list_for_each_entry_safe(isr, tmp, &tofree, head) {
+			flush_work_sync(&isr->work);
+			kfree(isr);
+		}
+	}
+}
+
+int
+nouveau_gpio_create(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+
+	INIT_LIST_HEAD(&pgpio->isr);
+	spin_lock_init(&pgpio->lock);
+
+	return nouveau_gpio_init(dev);
+}
+
+void
+nouveau_gpio_destroy(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+
+	nouveau_gpio_fini(dev);
+	BUG_ON(!list_empty(&pgpio->isr));
+}
+
+int
+nouveau_gpio_init(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+	int ret = 0;
+
+	if (pgpio->init)
+		ret = pgpio->init(dev);
+
+	return ret;
+}
+
+void
+nouveau_gpio_fini(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+
+	if (pgpio->fini)
+		pgpio->fini(dev);
+}
+
+void
+nouveau_gpio_reset(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	u8 *entry, version;
+	int ent = -1;
+
+	while ((entry = dcb_gpio_entry(dev, 0, ++ent, &version))) {
+		u8 func = 0xff, line, defs, unk0, unk1;
+		if (version >= 0x41) {
+			defs = !!(entry[0] & 0x80);
+			line = entry[0] & 0x3f;
+			func = entry[1];
+			unk0 = entry[2];
+			unk1 = entry[3] & 0x1f;
+		} else
+		if (version >= 0x40) {
+			line = entry[0] & 0x1f;
+			func = entry[1];
+			defs = !!(entry[3] & 0x01);
+			unk0 = !!(entry[3] & 0x02);
+			unk1 = !!(entry[3] & 0x04);
+		} else {
+			break;
+		}
+
+		if (func == 0xff)
+			continue;
+
+		nouveau_gpio_func_set(dev, func, defs);
+
+		if (dev_priv->card_type >= NV_D0) {
+			nv_mask(dev, 0x00d610 + (line * 4), 0xff, unk0);
+			if (unk1--)
+				nv_mask(dev, 0x00d640 + (unk1 * 4), 0xff, line);
+		} else
+		if (dev_priv->card_type >= NV_50) {
+			static const u32 regs[] = { 0xe100, 0xe28c };
+			u32 val = (unk1 << 16) | unk0;
+			u32 reg = regs[line >> 4]; line &= 0x0f;
+
+			nv_mask(dev, reg, 0x00010001 << line, val << line);
+		}
+	}
+}
diff --git a/drivers/gpu/drm/nouveau/nouveau_gpio.h b/drivers/gpu/drm/nouveau/nouveau_gpio.h
new file mode 100644
index 000000000000..64c5cb077ace
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_gpio.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef __NOUVEAU_GPIO_H__
+#define __NOUVEAU_GPIO_H__
+
+struct gpio_func {
+	u8 func;
+	u8 line;
+	u8 log[2];
+};
+
+/* nouveau_gpio.c */
+int  nouveau_gpio_create(struct drm_device *);
+void nouveau_gpio_destroy(struct drm_device *);
+int  nouveau_gpio_init(struct drm_device *);
+void nouveau_gpio_fini(struct drm_device *);
+void nouveau_gpio_reset(struct drm_device *);
+int  nouveau_gpio_drive(struct drm_device *, int idx, int line,
+			int dir, int out);
+int  nouveau_gpio_sense(struct drm_device *, int idx, int line);
+int  nouveau_gpio_find(struct drm_device *, int idx, u8 tag, u8 line,
+		       struct gpio_func *);
+int  nouveau_gpio_set(struct drm_device *, int idx, u8 tag, u8 line, int state);
+int  nouveau_gpio_get(struct drm_device *, int idx, u8 tag, u8 line);
+int  nouveau_gpio_irq(struct drm_device *, int idx, u8 tag, u8 line, bool on);
+void nouveau_gpio_isr(struct drm_device *, int idx, u32 mask);
+int  nouveau_gpio_isr_add(struct drm_device *, int idx, u8 tag, u8 line,
+			  void (*)(void *, int state), void *data);
+void nouveau_gpio_isr_del(struct drm_device *, int idx, u8 tag, u8 line,
+			  void (*)(void *, int state), void *data);
+
+static inline bool
+nouveau_gpio_func_valid(struct drm_device *dev, u8 tag)
+{
+	struct gpio_func func;
+	return (nouveau_gpio_find(dev, 0, tag, 0xff, &func)) == 0;
+}
+
+static inline int
+nouveau_gpio_func_set(struct drm_device *dev, u8 tag, int state)
+{
+	return nouveau_gpio_set(dev, 0, tag, 0xff, state);
+}
+
+static inline int
+nouveau_gpio_func_get(struct drm_device *dev, u8 tag)
+{
+	return nouveau_gpio_get(dev, 0, tag, 0xff);
+}
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_hdmi.c b/drivers/gpu/drm/nouveau/nouveau_hdmi.c
new file mode 100644
index 000000000000..59ea1c14eca0
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_hdmi.c
@@ -0,0 +1,258 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_connector.h"
+#include "nouveau_encoder.h"
+#include "nouveau_crtc.h"
+
+static bool
+hdmi_sor(struct drm_encoder *encoder)
+{
+	struct drm_nouveau_private *dev_priv = encoder->dev->dev_private;
+	if (dev_priv->chipset < 0xa3)
+		return false;
+	return true;
+}
+
+static inline u32
+hdmi_base(struct drm_encoder *encoder)
+{
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(nv_encoder->crtc);
+	if (!hdmi_sor(encoder))
+		return 0x616500 + (nv_crtc->index * 0x800);
+	return 0x61c500 + (nv_encoder->or * 0x800);
+}
+
+static void
+hdmi_wr32(struct drm_encoder *encoder, u32 reg, u32 val)
+{
+	nv_wr32(encoder->dev, hdmi_base(encoder) + reg, val);
+}
+
+static u32
+hdmi_rd32(struct drm_encoder *encoder, u32 reg)
+{
+	return nv_rd32(encoder->dev, hdmi_base(encoder) + reg);
+}
+
+static u32
+hdmi_mask(struct drm_encoder *encoder, u32 reg, u32 mask, u32 val)
+{
+	u32 tmp = hdmi_rd32(encoder, reg);
+	hdmi_wr32(encoder, reg, (tmp & ~mask) | val);
+	return tmp;
+}
+
+static void
+nouveau_audio_disconnect(struct drm_encoder *encoder)
+{
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct drm_device *dev = encoder->dev;
+	u32 or = nv_encoder->or * 0x800;
+
+	if (hdmi_sor(encoder)) {
+		nv_mask(dev, 0x61c448 + or, 0x00000003, 0x00000000);
+	}
+}
+
+static void
+nouveau_audio_mode_set(struct drm_encoder *encoder,
+		       struct drm_display_mode *mode)
+{
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct nouveau_connector *nv_connector;
+	struct drm_device *dev = encoder->dev;
+	u32 or = nv_encoder->or * 0x800;
+	int i;
+
+	nv_connector = nouveau_encoder_connector_get(nv_encoder);
+	if (!drm_detect_monitor_audio(nv_connector->edid)) {
+		nouveau_audio_disconnect(encoder);
+		return;
+	}
+
+	if (hdmi_sor(encoder)) {
+		nv_mask(dev, 0x61c448 + or, 0x00000001, 0x00000001);
+
+		drm_edid_to_eld(&nv_connector->base, nv_connector->edid);
+		if (nv_connector->base.eld[0]) {
+			u8 *eld = nv_connector->base.eld;
+			for (i = 0; i < eld[2] * 4; i++)
+				nv_wr32(dev, 0x61c440 + or, (i << 8) | eld[i]);
+			for (i = eld[2] * 4; i < 0x60; i++)
+				nv_wr32(dev, 0x61c440 + or, (i << 8) | 0x00);
+			nv_mask(dev, 0x61c448 + or, 0x00000002, 0x00000002);
+		}
+	}
+}
+
+static void
+nouveau_hdmi_infoframe(struct drm_encoder *encoder, u32 ctrl, u8 *frame)
+{
+	/* calculate checksum for the infoframe */
+	u8 sum = 0, i;
+	for (i = 0; i < frame[2]; i++)
+		sum += frame[i];
+	frame[3] = 256 - sum;
+
+	/* disable infoframe, and write header */
+	hdmi_mask(encoder, ctrl + 0x00, 0x00000001, 0x00000000);
+	hdmi_wr32(encoder, ctrl + 0x08, *(u32 *)frame & 0xffffff);
+
+	/* register scans tell me the audio infoframe has only one set of
+	 * subpack regs, according to tegra (gee nvidia, it'd be nice if we
+	 * could get those docs too!), the hdmi block pads out the rest of
+	 * the packet on its own.
+	 */
+	if (ctrl == 0x020)
+		frame[2] = 6;
+
+	/* write out checksum and data, weird weird 7 byte register pairs */
+	for (i = 0; i < frame[2] + 1; i += 7) {
+		u32 rsubpack = ctrl + 0x0c + ((i / 7) * 8);
+		u32 *subpack = (u32 *)&frame[3 + i];
+		hdmi_wr32(encoder, rsubpack + 0, subpack[0]);
+		hdmi_wr32(encoder, rsubpack + 4, subpack[1] & 0xffffff);
+	}
+
+	/* enable the infoframe */
+	hdmi_mask(encoder, ctrl, 0x00000001, 0x00000001);
+}
+
+static void
+nouveau_hdmi_video_infoframe(struct drm_encoder *encoder,
+			     struct drm_display_mode *mode)
+{
+	const u8 Y = 0, A = 0, B = 0, S = 0, C = 0, M = 0, R = 0;
+	const u8 ITC = 0, EC = 0, Q = 0, SC = 0, VIC = 0, PR = 0;
+	const u8 bar_top = 0, bar_bottom = 0, bar_left = 0, bar_right = 0;
+	u8 frame[20];
+
+	frame[0x00] = 0x82; /* AVI infoframe */
+	frame[0x01] = 0x02; /* version */
+	frame[0x02] = 0x0d; /* length */
+	frame[0x03] = 0x00;
+	frame[0x04] = (Y << 5) | (A << 4) | (B << 2) | S;
+	frame[0x05] = (C << 6) | (M << 4) | R;
+	frame[0x06] = (ITC << 7) | (EC << 4) | (Q << 2) | SC;
+	frame[0x07] = VIC;
+	frame[0x08] = PR;
+	frame[0x09] = bar_top & 0xff;
+	frame[0x0a] = bar_top >> 8;
+	frame[0x0b] = bar_bottom & 0xff;
+	frame[0x0c] = bar_bottom >> 8;
+	frame[0x0d] = bar_left & 0xff;
+	frame[0x0e] = bar_left >> 8;
+	frame[0x0f] = bar_right & 0xff;
+	frame[0x10] = bar_right >> 8;
+	frame[0x11] = 0x00;
+	frame[0x12] = 0x00;
+	frame[0x13] = 0x00;
+
+	nouveau_hdmi_infoframe(encoder, 0x020, frame);
+}
+
+static void
+nouveau_hdmi_audio_infoframe(struct drm_encoder *encoder,
+			     struct drm_display_mode *mode)
+{
+	const u8 CT = 0x00, CC = 0x01, ceaSS = 0x00, SF = 0x00, FMT = 0x00;
+	const u8 CA = 0x00, DM_INH = 0, LSV = 0x00;
+	u8 frame[12];
+
+	frame[0x00] = 0x84;	/* Audio infoframe */
+	frame[0x01] = 0x01;	/* version */
+	frame[0x02] = 0x0a;	/* length */
+	frame[0x03] = 0x00;
+	frame[0x04] = (CT << 4) | CC;
+	frame[0x05] = (SF << 2) | ceaSS;
+	frame[0x06] = FMT;
+	frame[0x07] = CA;
+	frame[0x08] = (DM_INH << 7) | (LSV << 3);
+	frame[0x09] = 0x00;
+	frame[0x0a] = 0x00;
+	frame[0x0b] = 0x00;
+
+	nouveau_hdmi_infoframe(encoder, 0x000, frame);
+}
+
+static void
+nouveau_hdmi_disconnect(struct drm_encoder *encoder)
+{
+	nouveau_audio_disconnect(encoder);
+
+	/* disable audio and avi infoframes */
+	hdmi_mask(encoder, 0x000, 0x00000001, 0x00000000);
+	hdmi_mask(encoder, 0x020, 0x00000001, 0x00000000);
+
+	/* disable hdmi */
+	hdmi_mask(encoder, 0x0a4, 0x40000000, 0x00000000);
+}
+
+void
+nouveau_hdmi_mode_set(struct drm_encoder *encoder,
+		      struct drm_display_mode *mode)
+{
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct nouveau_connector *nv_connector;
+	struct drm_device *dev = encoder->dev;
+	u32 max_ac_packet, rekey;
+
+	nv_connector = nouveau_encoder_connector_get(nv_encoder);
+	if (!mode || !nv_connector || !nv_connector->edid ||
+	    !drm_detect_hdmi_monitor(nv_connector->edid)) {
+		nouveau_hdmi_disconnect(encoder);
+		return;
+	}
+
+	nouveau_hdmi_video_infoframe(encoder, mode);
+	nouveau_hdmi_audio_infoframe(encoder, mode);
+
+	hdmi_mask(encoder, 0x0d0, 0x00070001, 0x00010001); /* SPARE, HW_CTS */
+	hdmi_mask(encoder, 0x068, 0x00010101, 0x00000000); /* ACR_CTRL, ?? */
+	hdmi_mask(encoder, 0x078, 0x80000000, 0x80000000); /* ACR_0441_ENABLE */
+
+	nv_mask(dev, 0x61733c, 0x00100000, 0x00100000); /* RESETF */
+	nv_mask(dev, 0x61733c, 0x10000000, 0x10000000); /* LOOKUP_EN */
+	nv_mask(dev, 0x61733c, 0x00100000, 0x00000000); /* !RESETF */
+
+	/* value matches nvidia binary driver, and tegra constant */
+	rekey = 56;
+
+	max_ac_packet  = mode->htotal - mode->hdisplay;
+	max_ac_packet -= rekey;
+	max_ac_packet -= 18; /* constant from tegra */
+	max_ac_packet /= 32;
+
+	/* enable hdmi */
+	hdmi_mask(encoder, 0x0a4, 0x5f1f003f, 0x40000000 | /* enable */
+					      0x1f000000 | /* unknown */
+					      max_ac_packet << 16 |
+					      rekey);
+
+	nouveau_audio_mode_set(encoder, mode);
+}
diff --git a/drivers/gpu/drm/nouveau/nouveau_hwsq.h b/drivers/gpu/drm/nouveau/nouveau_hwsq.h
new file mode 100644
index 000000000000..697687593a81
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_hwsq.h
@@ -0,0 +1,115 @@
+/*
+ * Copyright 2010 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#ifndef __NOUVEAU_HWSQ_H__
+#define __NOUVEAU_HWSQ_H__
+
+struct hwsq_ucode {
+	u8 data[0x200];
+	union {
+		u8  *u08;
+		u16 *u16;
+		u32 *u32;
+	} ptr;
+	u16 len;
+
+	u32 reg;
+	u32 val;
+};
+
+static inline void
+hwsq_init(struct hwsq_ucode *hwsq)
+{
+	hwsq->ptr.u08 = hwsq->data;
+	hwsq->reg = 0xffffffff;
+	hwsq->val = 0xffffffff;
+}
+
+static inline void
+hwsq_fini(struct hwsq_ucode *hwsq)
+{
+	do {
+		*hwsq->ptr.u08++ = 0x7f;
+		hwsq->len = hwsq->ptr.u08 - hwsq->data;
+	} while (hwsq->len & 3);
+	hwsq->ptr.u08 = hwsq->data;
+}
+
+static inline void
+hwsq_usec(struct hwsq_ucode *hwsq, u8 usec)
+{
+	u32 shift = 0;
+	while (usec & ~3) {
+		usec >>= 2;
+		shift++;
+	}
+
+	*hwsq->ptr.u08++ = (shift << 2) | usec;
+}
+
+static inline void
+hwsq_setf(struct hwsq_ucode *hwsq, u8 flag, int val)
+{
+	flag += 0x80;
+	if (val >= 0)
+		flag += 0x20;
+	if (val >= 1)
+		flag += 0x20;
+	*hwsq->ptr.u08++ = flag;
+}
+
+static inline void
+hwsq_op5f(struct hwsq_ucode *hwsq, u8 v0, u8 v1)
+{
+	*hwsq->ptr.u08++ = 0x5f;
+	*hwsq->ptr.u08++ = v0;
+	*hwsq->ptr.u08++ = v1;
+}
+
+static inline void
+hwsq_wr32(struct hwsq_ucode *hwsq, u32 reg, u32 val)
+{
+	if (val != hwsq->val) {
+		if ((val & 0xffff0000) == (hwsq->val & 0xffff0000)) {
+			*hwsq->ptr.u08++ = 0x42;
+			*hwsq->ptr.u16++ = (val & 0x0000ffff);
+		} else {
+			*hwsq->ptr.u08++ = 0xe2;
+			*hwsq->ptr.u32++ = val;
+		}
+
+		hwsq->val = val;
+	}
+
+	if ((reg & 0xffff0000) == (hwsq->reg & 0xffff0000)) {
+		*hwsq->ptr.u08++ = 0x40;
+		*hwsq->ptr.u16++ = (reg & 0x0000ffff);
+	} else {
+		*hwsq->ptr.u08++ = 0xe0;
+		*hwsq->ptr.u32++ = reg;
+	}
+	hwsq->reg = reg;
+}
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_i2c.c b/drivers/gpu/drm/nouveau/nouveau_i2c.c
index d39b2202b197..820ae7f52044 100644
--- a/drivers/gpu/drm/nouveau/nouveau_i2c.c
+++ b/drivers/gpu/drm/nouveau/nouveau_i2c.c
@@ -29,262 +29,465 @@
 #include "nouveau_i2c.h"
 #include "nouveau_hw.h"
 
+#define T_TIMEOUT  2200000
+#define T_RISEFALL 1000
+#define T_HOLD     5000
+
 static void
-nv04_i2c_setscl(void *data, int state)
+i2c_drive_scl(void *data, int state)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
-	uint8_t val;
-
-	val = (NVReadVgaCrtc(dev, 0, i2c->wr) & 0xd0) | (state ? 0x20 : 0);
-	NVWriteVgaCrtc(dev, 0, i2c->wr, val | 0x01);
+	struct nouveau_i2c_chan *port = data;
+	if (port->type == 0) {
+		u8 val = NVReadVgaCrtc(port->dev, 0, port->drive);
+		if (state) val |= 0x20;
+		else	   val &= 0xdf;
+		NVWriteVgaCrtc(port->dev, 0, port->drive, val | 0x01);
+	} else
+	if (port->type == 4) {
+		nv_mask(port->dev, port->drive, 0x2f, state ? 0x21 : 0x01);
+	} else
+	if (port->type == 5) {
+		if (state) port->state |= 0x01;
+		else	   port->state &= 0xfe;
+		nv_wr32(port->dev, port->drive, 4 | port->state);
+	}
 }
 
 static void
-nv04_i2c_setsda(void *data, int state)
+i2c_drive_sda(void *data, int state)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
-	uint8_t val;
-
-	val = (NVReadVgaCrtc(dev, 0, i2c->wr) & 0xe0) | (state ? 0x10 : 0);
-	NVWriteVgaCrtc(dev, 0, i2c->wr, val | 0x01);
+	struct nouveau_i2c_chan *port = data;
+	if (port->type == 0) {
+		u8 val = NVReadVgaCrtc(port->dev, 0, port->drive);
+		if (state) val |= 0x10;
+		else	   val &= 0xef;
+		NVWriteVgaCrtc(port->dev, 0, port->drive, val | 0x01);
+	} else
+	if (port->type == 4) {
+		nv_mask(port->dev, port->drive, 0x1f, state ? 0x11 : 0x01);
+	} else
+	if (port->type == 5) {
+		if (state) port->state |= 0x02;
+		else	   port->state &= 0xfd;
+		nv_wr32(port->dev, port->drive, 4 | port->state);
+	}
 }
 
 static int
-nv04_i2c_getscl(void *data)
+i2c_sense_scl(void *data)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
-
-	return !!(NVReadVgaCrtc(dev, 0, i2c->rd) & 4);
+	struct nouveau_i2c_chan *port = data;
+	struct drm_nouveau_private *dev_priv = port->dev->dev_private;
+	if (port->type == 0) {
+		return !!(NVReadVgaCrtc(port->dev, 0, port->sense) & 0x04);
+	} else
+	if (port->type == 4) {
+		return !!(nv_rd32(port->dev, port->sense) & 0x00040000);
+	} else
+	if (port->type == 5) {
+		if (dev_priv->card_type < NV_D0)
+			return !!(nv_rd32(port->dev, port->sense) & 0x01);
+		else
+			return !!(nv_rd32(port->dev, port->sense) & 0x10);
+	}
+	return 0;
 }
 
 static int
-nv04_i2c_getsda(void *data)
+i2c_sense_sda(void *data)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
-
-	return !!(NVReadVgaCrtc(dev, 0, i2c->rd) & 8);
+	struct nouveau_i2c_chan *port = data;
+	struct drm_nouveau_private *dev_priv = port->dev->dev_private;
+	if (port->type == 0) {
+		return !!(NVReadVgaCrtc(port->dev, 0, port->sense) & 0x08);
+	} else
+	if (port->type == 4) {
+		return !!(nv_rd32(port->dev, port->sense) & 0x00080000);
+	} else
+	if (port->type == 5) {
+		if (dev_priv->card_type < NV_D0)
+			return !!(nv_rd32(port->dev, port->sense) & 0x02);
+		else
+			return !!(nv_rd32(port->dev, port->sense) & 0x20);
+	}
+	return 0;
 }
 
 static void
-nv4e_i2c_setscl(void *data, int state)
+i2c_delay(struct nouveau_i2c_chan *port, u32 nsec)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
-	uint8_t val;
-
-	val = (nv_rd32(dev, i2c->wr) & 0xd0) | (state ? 0x20 : 0);
-	nv_wr32(dev, i2c->wr, val | 0x01);
+	udelay((nsec + 500) / 1000);
 }
 
-static void
-nv4e_i2c_setsda(void *data, int state)
+static bool
+i2c_raise_scl(struct nouveau_i2c_chan *port)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
-	uint8_t val;
+	u32 timeout = T_TIMEOUT / T_RISEFALL;
+
+	i2c_drive_scl(port, 1);
+	do {
+		i2c_delay(port, T_RISEFALL);
+	} while (!i2c_sense_scl(port) && --timeout);
 
-	val = (nv_rd32(dev, i2c->wr) & 0xe0) | (state ? 0x10 : 0);
-	nv_wr32(dev, i2c->wr, val | 0x01);
+	return timeout != 0;
 }
 
 static int
-nv4e_i2c_getscl(void *data)
+i2c_start(struct nouveau_i2c_chan *port)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
+	int ret = 0;
+
+	port->state  = i2c_sense_scl(port);
+	port->state |= i2c_sense_sda(port) << 1;
+	if (port->state != 3) {
+		i2c_drive_scl(port, 0);
+		i2c_drive_sda(port, 1);
+		if (!i2c_raise_scl(port))
+			ret = -EBUSY;
+	}
 
-	return !!((nv_rd32(dev, i2c->rd) >> 16) & 4);
+	i2c_drive_sda(port, 0);
+	i2c_delay(port, T_HOLD);
+	i2c_drive_scl(port, 0);
+	i2c_delay(port, T_HOLD);
+	return ret;
 }
 
-static int
-nv4e_i2c_getsda(void *data)
+static void
+i2c_stop(struct nouveau_i2c_chan *port)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
-
-	return !!((nv_rd32(dev, i2c->rd) >> 16) & 8);
+	i2c_drive_scl(port, 0);
+	i2c_drive_sda(port, 0);
+	i2c_delay(port, T_RISEFALL);
+
+	i2c_drive_scl(port, 1);
+	i2c_delay(port, T_HOLD);
+	i2c_drive_sda(port, 1);
+	i2c_delay(port, T_HOLD);
 }
 
-static const uint32_t nv50_i2c_port[] = {
-	0x00e138, 0x00e150, 0x00e168, 0x00e180,
-	0x00e254, 0x00e274, 0x00e764, 0x00e780,
-	0x00e79c, 0x00e7b8
-};
-#define NV50_I2C_PORTS ARRAY_SIZE(nv50_i2c_port)
-
 static int
-nv50_i2c_getscl(void *data)
+i2c_bitw(struct nouveau_i2c_chan *port, int sda)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
+	i2c_drive_sda(port, sda);
+	i2c_delay(port, T_RISEFALL);
 
-	return !!(nv_rd32(dev, i2c->rd) & 1);
-}
+	if (!i2c_raise_scl(port))
+		return -ETIMEDOUT;
+	i2c_delay(port, T_HOLD);
 
+	i2c_drive_scl(port, 0);
+	i2c_delay(port, T_HOLD);
+	return 0;
+}
 
 static int
-nv50_i2c_getsda(void *data)
+i2c_bitr(struct nouveau_i2c_chan *port)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	struct drm_device *dev = i2c->dev;
+	int sda;
+
+	i2c_drive_sda(port, 1);
+	i2c_delay(port, T_RISEFALL);
 
-	return !!(nv_rd32(dev, i2c->rd) & 2);
+	if (!i2c_raise_scl(port))
+		return -ETIMEDOUT;
+	i2c_delay(port, T_HOLD);
+
+	sda = i2c_sense_sda(port);
+
+	i2c_drive_scl(port, 0);
+	i2c_delay(port, T_HOLD);
+	return sda;
 }
 
-static void
-nv50_i2c_setscl(void *data, int state)
+static int
+i2c_get_byte(struct nouveau_i2c_chan *port, u8 *byte, bool last)
 {
-	struct nouveau_i2c_chan *i2c = data;
+	int i, bit;
+
+	*byte = 0;
+	for (i = 7; i >= 0; i--) {
+		bit = i2c_bitr(port);
+		if (bit < 0)
+			return bit;
+		*byte |= bit << i;
+	}
 
-	nv_wr32(i2c->dev, i2c->wr, 4 | (i2c->data ? 2 : 0) | (state ? 1 : 0));
+	return i2c_bitw(port, last ? 1 : 0);
 }
 
-static void
-nv50_i2c_setsda(void *data, int state)
+static int
+i2c_put_byte(struct nouveau_i2c_chan *port, u8 byte)
 {
-	struct nouveau_i2c_chan *i2c = data;
+	int i, ret;
+	for (i = 7; i >= 0; i--) {
+		ret = i2c_bitw(port, !!(byte & (1 << i)));
+		if (ret < 0)
+			return ret;
+	}
 
-	nv_mask(i2c->dev, i2c->wr, 0x00000006, 4 | (state ? 2 : 0));
-	i2c->data = state;
+	ret = i2c_bitr(port);
+	if (ret == 1) /* nack */
+		ret = -EIO;
+	return ret;
 }
 
 static int
-nvd0_i2c_getscl(void *data)
+i2c_addr(struct nouveau_i2c_chan *port, struct i2c_msg *msg)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	return !!(nv_rd32(i2c->dev, i2c->rd) & 0x10);
+	u32 addr = msg->addr << 1;
+	if (msg->flags & I2C_M_RD)
+		addr |= 1;
+	return i2c_put_byte(port, addr);
 }
 
 static int
-nvd0_i2c_getsda(void *data)
+i2c_bit_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
 {
-	struct nouveau_i2c_chan *i2c = data;
-	return !!(nv_rd32(i2c->dev, i2c->rd) & 0x20);
+	struct nouveau_i2c_chan *port = (struct nouveau_i2c_chan *)adap;
+	struct i2c_msg *msg = msgs;
+	int ret = 0, mcnt = num;
+
+	while (!ret && mcnt--) {
+		u8 remaining = msg->len;
+		u8 *ptr = msg->buf;
+
+		ret = i2c_start(port);
+		if (ret == 0)
+			ret = i2c_addr(port, msg);
+
+		if (msg->flags & I2C_M_RD) {
+			while (!ret && remaining--)
+				ret = i2c_get_byte(port, ptr++, !remaining);
+		} else {
+			while (!ret && remaining--)
+				ret = i2c_put_byte(port, *ptr++);
+		}
+
+		msg++;
+	}
+
+	i2c_stop(port);
+	return (ret < 0) ? ret : num;
 }
 
-int
-nouveau_i2c_init(struct drm_device *dev, struct dcb_i2c_entry *entry, int index)
+static u32
+i2c_bit_func(struct i2c_adapter *adap)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_i2c_chan *i2c;
-	int ret;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+const struct i2c_algorithm i2c_bit_algo = {
+	.master_xfer = i2c_bit_xfer,
+	.functionality = i2c_bit_func
+};
+
+static const uint32_t nv50_i2c_port[] = {
+	0x00e138, 0x00e150, 0x00e168, 0x00e180,
+	0x00e254, 0x00e274, 0x00e764, 0x00e780,
+	0x00e79c, 0x00e7b8
+};
 
-	if (entry->chan)
-		return -EEXIST;
+static u8 *
+i2c_table(struct drm_device *dev, u8 *version)
+{
+	u8 *dcb = dcb_table(dev), *i2c = NULL;
+	if (dcb) {
+		if (dcb[0] >= 0x15)
+			i2c = ROMPTR(dev, dcb[2]);
+		if (dcb[0] >= 0x30)
+			i2c = ROMPTR(dev, dcb[4]);
+	}
 
-	if (dev_priv->card_type >= NV_50 &&
-	    dev_priv->card_type <= NV_C0 && entry->read >= NV50_I2C_PORTS) {
-		NV_ERROR(dev, "unknown i2c port %d\n", entry->read);
-		return -EINVAL;
+	/* early revisions had no version number, use dcb version */
+	if (i2c) {
+		*version = dcb[0];
+		if (*version >= 0x30)
+			*version = i2c[0];
 	}
 
-	i2c = kzalloc(sizeof(*i2c), GFP_KERNEL);
-	if (i2c == NULL)
-		return -ENOMEM;
-
-	switch (entry->port_type) {
-	case 0:
-		i2c->bit.setsda = nv04_i2c_setsda;
-		i2c->bit.setscl = nv04_i2c_setscl;
-		i2c->bit.getsda = nv04_i2c_getsda;
-		i2c->bit.getscl = nv04_i2c_getscl;
-		i2c->rd = entry->read;
-		i2c->wr = entry->write;
-		break;
-	case 4:
-		i2c->bit.setsda = nv4e_i2c_setsda;
-		i2c->bit.setscl = nv4e_i2c_setscl;
-		i2c->bit.getsda = nv4e_i2c_getsda;
-		i2c->bit.getscl = nv4e_i2c_getscl;
-		i2c->rd = 0x600800 + entry->read;
-		i2c->wr = 0x600800 + entry->write;
-		break;
-	case 5:
-		i2c->bit.setsda = nv50_i2c_setsda;
-		i2c->bit.setscl = nv50_i2c_setscl;
-		if (dev_priv->card_type < NV_D0) {
-			i2c->bit.getsda = nv50_i2c_getsda;
-			i2c->bit.getscl = nv50_i2c_getscl;
-			i2c->rd = nv50_i2c_port[entry->read];
-			i2c->wr = i2c->rd;
-		} else {
-			i2c->bit.getsda = nvd0_i2c_getsda;
-			i2c->bit.getscl = nvd0_i2c_getscl;
-			i2c->rd = 0x00d014 + (entry->read * 0x20);
-			i2c->wr = i2c->rd;
-		}
-		break;
-	case 6:
-		i2c->rd = entry->read;
-		i2c->wr = entry->write;
-		break;
-	default:
-		NV_ERROR(dev, "DCB I2C port type %d unknown\n",
-			 entry->port_type);
-		kfree(i2c);
-		return -EINVAL;
+	return i2c;
+}
+
+int
+nouveau_i2c_init(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvbios *bios = &dev_priv->vbios;
+	struct nouveau_i2c_chan *port;
+	u8 *i2c, *entry, legacy[2][4] = {};
+	u8 version, entries, recordlen;
+	int ret, i;
+
+	INIT_LIST_HEAD(&dev_priv->i2c_ports);
+
+	i2c = i2c_table(dev, &version);
+	if (!i2c) {
+		u8 *bmp = &bios->data[bios->offset];
+		if (bios->type != NVBIOS_BMP)
+			return -ENODEV;
+
+		legacy[0][0] = NV_CIO_CRE_DDC_WR__INDEX;
+		legacy[0][1] = NV_CIO_CRE_DDC_STATUS__INDEX;
+		legacy[1][0] = NV_CIO_CRE_DDC0_WR__INDEX;
+		legacy[1][1] = NV_CIO_CRE_DDC0_STATUS__INDEX;
+
+		/* BMP (from v4.0) has i2c info in the structure, it's in a
+		 * fixed location on earlier VBIOS
+		 */
+		if (bmp[5] < 4)
+			i2c = &bios->data[0x48];
+		else
+			i2c = &bmp[0x36];
+
+		if (i2c[4]) legacy[0][0] = i2c[4];
+		if (i2c[5]) legacy[0][1] = i2c[5];
+		if (i2c[6]) legacy[1][0] = i2c[6];
+		if (i2c[7]) legacy[1][1] = i2c[7];
 	}
 
-	snprintf(i2c->adapter.name, sizeof(i2c->adapter.name),
-		 "nouveau-%s-%d", pci_name(dev->pdev), index);
-	i2c->adapter.owner = THIS_MODULE;
-	i2c->adapter.dev.parent = &dev->pdev->dev;
-	i2c->dev = dev;
-	i2c_set_adapdata(&i2c->adapter, i2c);
-
-	if (entry->port_type < 6) {
-		i2c->adapter.algo_data = &i2c->bit;
-		i2c->bit.udelay = 40;
-		i2c->bit.timeout = usecs_to_jiffies(5000);
-		i2c->bit.data = i2c;
-		ret = i2c_bit_add_bus(&i2c->adapter);
+	if (i2c && version >= 0x30) {
+		entry     = i2c[1] + i2c;
+		entries   = i2c[2];
+		recordlen = i2c[3];
+	} else
+	if (i2c) {
+		entry     = i2c;
+		entries   = 16;
+		recordlen = 4;
 	} else {
-		i2c->adapter.algo = &nouveau_dp_i2c_algo;
-		ret = i2c_add_adapter(&i2c->adapter);
+		entry     = legacy[0];
+		entries   = 2;
+		recordlen = 4;
 	}
 
-	if (ret) {
-		NV_ERROR(dev, "Failed to register i2c %d\n", index);
-		kfree(i2c);
-		return ret;
+	for (i = 0; i < entries; i++, entry += recordlen) {
+		port = kzalloc(sizeof(*port), GFP_KERNEL);
+		if (port == NULL) {
+			nouveau_i2c_fini(dev);
+			return -ENOMEM;
+		}
+
+		port->type = entry[3];
+		if (version < 0x30) {
+			port->type &= 0x07;
+			if (port->type == 0x07)
+				port->type = 0xff;
+		}
+
+		if (port->type == 0xff) {
+			kfree(port);
+			continue;
+		}
+
+		switch (port->type) {
+		case 0: /* NV04:NV50 */
+			port->drive = entry[0];
+			port->sense = entry[1];
+			port->adapter.algo = &i2c_bit_algo;
+			break;
+		case 4: /* NV4E */
+			port->drive = 0x600800 + entry[1];
+			port->sense = port->drive;
+			port->adapter.algo = &i2c_bit_algo;
+			break;
+		case 5: /* NV50- */
+			port->drive = entry[0] & 0x0f;
+			if (dev_priv->card_type < NV_D0) {
+				if (port->drive >= ARRAY_SIZE(nv50_i2c_port))
+					break;
+				port->drive = nv50_i2c_port[port->drive];
+				port->sense = port->drive;
+			} else {
+				port->drive = 0x00d014 + (port->drive * 0x20);
+				port->sense = port->drive;
+			}
+			port->adapter.algo = &i2c_bit_algo;
+			break;
+		case 6: /* NV50- DP AUX */
+			port->drive = entry[0];
+			port->sense = port->drive;
+			port->adapter.algo = &nouveau_dp_i2c_algo;
+			break;
+		default:
+			break;
+		}
+
+		if (!port->adapter.algo) {
+			NV_ERROR(dev, "I2C%d: type %d index %x/%x unknown\n",
+				 i, port->type, port->drive, port->sense);
+			kfree(port);
+			continue;
+		}
+
+		snprintf(port->adapter.name, sizeof(port->adapter.name),
+			 "nouveau-%s-%d", pci_name(dev->pdev), i);
+		port->adapter.owner = THIS_MODULE;
+		port->adapter.dev.parent = &dev->pdev->dev;
+		port->dev = dev;
+		port->index = i;
+		port->dcb = ROM32(entry[0]);
+		i2c_set_adapdata(&port->adapter, i2c);
+
+		ret = i2c_add_adapter(&port->adapter);
+		if (ret) {
+			NV_ERROR(dev, "I2C%d: failed register: %d\n", i, ret);
+			kfree(port);
+			continue;
+		}
+
+		list_add_tail(&port->head, &dev_priv->i2c_ports);
 	}
 
-	entry->chan = i2c;
 	return 0;
 }
 
 void
-nouveau_i2c_fini(struct drm_device *dev, struct dcb_i2c_entry *entry)
+nouveau_i2c_fini(struct drm_device *dev)
 {
-	if (!entry->chan)
-		return;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_i2c_chan *port, *tmp;
 
-	i2c_del_adapter(&entry->chan->adapter);
-	kfree(entry->chan);
-	entry->chan = NULL;
+	list_for_each_entry_safe(port, tmp, &dev_priv->i2c_ports, head) {
+		i2c_del_adapter(&port->adapter);
+		kfree(port);
+	}
 }
 
 struct nouveau_i2c_chan *
-nouveau_i2c_find(struct drm_device *dev, int index)
+nouveau_i2c_find(struct drm_device *dev, u8 index)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct dcb_i2c_entry *i2c = &dev_priv->vbios.dcb.i2c[index];
+	struct nouveau_i2c_chan *port;
+
+	if (index == NV_I2C_DEFAULT(0) ||
+	    index == NV_I2C_DEFAULT(1)) {
+		u8 version, *i2c = i2c_table(dev, &version);
+		if (i2c && version >= 0x30) {
+			if (index == NV_I2C_DEFAULT(0))
+				index = (i2c[4] & 0x0f);
+			else
+				index = (i2c[4] & 0xf0) >> 4;
+		} else {
+			index = 2;
+		}
+	}
 
-	if (index >= DCB_MAX_NUM_I2C_ENTRIES)
-		return NULL;
+	list_for_each_entry(port, &dev_priv->i2c_ports, head) {
+		if (port->index == index)
+			break;
+	}
 
-	if (dev_priv->card_type >= NV_50 && (i2c->entry & 0x00000100)) {
-		uint32_t reg = 0xe500, val;
+	if (&port->head == &dev_priv->i2c_ports)
+		return NULL;
 
-		if (i2c->port_type == 6) {
-			reg += i2c->read * 0x50;
+	if (dev_priv->card_type >= NV_50 && (port->dcb & 0x00000100)) {
+		u32 reg = 0x00e500, val;
+		if (port->type == 6) {
+			reg += port->drive * 0x50;
 			val  = 0x2002;
 		} else {
-			reg += ((i2c->entry & 0x1e00) >> 9) * 0x50;
+			reg += ((port->dcb & 0x1e00) >> 9) * 0x50;
 			val  = 0xe001;
 		}
 
@@ -294,9 +497,7 @@ nouveau_i2c_find(struct drm_device *dev, int index)
 		nv_mask(dev, reg + 0x00, 0x0000f003, val);
 	}
 
-	if (!i2c->chan && nouveau_i2c_init(dev, i2c, index))
-		return NULL;
-	return i2c->chan;
+	return port;
 }
 
 bool
@@ -331,9 +532,13 @@ nouveau_i2c_identify(struct drm_device *dev, const char *what,
 	struct nouveau_i2c_chan *i2c = nouveau_i2c_find(dev, index);
 	int i;
 
-	NV_DEBUG(dev, "Probing %ss on I2C bus: %d\n", what, index);
+	if (!i2c) {
+		NV_DEBUG(dev, "No bus when probing %s on %d\n", what, index);
+		return -ENODEV;
+	}
 
-	for (i = 0; i2c && info[i].addr; i++) {
+	NV_DEBUG(dev, "Probing %ss on I2C bus: %d\n", what, i2c->index);
+	for (i = 0; info[i].addr; i++) {
 		if (nouveau_probe_i2c_addr(i2c, info[i].addr) &&
 		    (!match || match(i2c, &info[i]))) {
 			NV_INFO(dev, "Detected %s: %s\n", what, info[i].type);
@@ -342,6 +547,5 @@ nouveau_i2c_identify(struct drm_device *dev, const char *what,
 	}
 
 	NV_DEBUG(dev, "No devices found.\n");
-
 	return -ENODEV;
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_i2c.h b/drivers/gpu/drm/nouveau/nouveau_i2c.h
index 422b62fd8272..4d2e4e9031be 100644
--- a/drivers/gpu/drm/nouveau/nouveau_i2c.h
+++ b/drivers/gpu/drm/nouveau/nouveau_i2c.h
@@ -27,20 +27,25 @@
 #include <linux/i2c-algo-bit.h>
 #include "drm_dp_helper.h"
 
-struct dcb_i2c_entry;
+#define NV_I2C_PORT(n)    (0x00 + (n))
+#define NV_I2C_PORT_NUM    0x10
+#define NV_I2C_DEFAULT(n) (0x80 + (n))
 
 struct nouveau_i2c_chan {
 	struct i2c_adapter adapter;
 	struct drm_device *dev;
-	struct i2c_algo_bit_data bit;
-	unsigned rd;
-	unsigned wr;
-	unsigned data;
+	struct list_head head;
+	u8  index;
+	u8  type;
+	u32 dcb;
+	u32 drive;
+	u32 sense;
+	u32 state;
 };
 
-int nouveau_i2c_init(struct drm_device *, struct dcb_i2c_entry *, int index);
-void nouveau_i2c_fini(struct drm_device *, struct dcb_i2c_entry *);
-struct nouveau_i2c_chan *nouveau_i2c_find(struct drm_device *, int index);
+int  nouveau_i2c_init(struct drm_device *);
+void nouveau_i2c_fini(struct drm_device *);
+struct nouveau_i2c_chan *nouveau_i2c_find(struct drm_device *, u8 index);
 bool nouveau_probe_i2c_addr(struct nouveau_i2c_chan *i2c, int addr);
 int nouveau_i2c_identify(struct drm_device *dev, const char *what,
 			 struct i2c_board_info *info,
diff --git a/drivers/gpu/drm/nouveau/nouveau_mem.c b/drivers/gpu/drm/nouveau/nouveau_mem.c
index 36bec4807701..c3a5745e9c79 100644
--- a/drivers/gpu/drm/nouveau/nouveau_mem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_mem.c
@@ -407,6 +407,12 @@ nouveau_mem_vram_init(struct drm_device *dev)
 	ret = pci_set_dma_mask(dev->pdev, DMA_BIT_MASK(dma_bits));
 	if (ret)
 		return ret;
+	ret = pci_set_consistent_dma_mask(dev->pdev, DMA_BIT_MASK(dma_bits));
+	if (ret) {
+		/* Reset to default value. */
+		pci_set_consistent_dma_mask(dev->pdev, DMA_BIT_MASK(32));
+	}
+
 
 	ret = nouveau_ttm_global_init(dev_priv);
 	if (ret)
@@ -638,10 +644,10 @@ nouveau_mem_timing_init(struct drm_device *dev)
 			return;
 
 		if (P.version == 1)
-			hdr = (struct nouveau_pm_tbl_header *) ROMPTR(bios, P.data[4]);
+			hdr = (struct nouveau_pm_tbl_header *) ROMPTR(dev, P.data[4]);
 		else
 		if (P.version == 2)
-			hdr = (struct nouveau_pm_tbl_header *) ROMPTR(bios, P.data[8]);
+			hdr = (struct nouveau_pm_tbl_header *) ROMPTR(dev, P.data[8]);
 		else {
 			NV_WARN(dev, "unknown mem for BIT P %d\n", P.version);
 		}
diff --git a/drivers/gpu/drm/nouveau/nouveau_mxm.c b/drivers/gpu/drm/nouveau/nouveau_mxm.c
new file mode 100644
index 000000000000..8bccddf4eff0
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nouveau_mxm.c
@@ -0,0 +1,677 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <linux/acpi.h>
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+
+#define MXM_DBG(dev, fmt, args...) NV_DEBUG((dev), "MXM: " fmt, ##args)
+#define MXM_MSG(dev, fmt, args...) NV_INFO((dev), "MXM: " fmt, ##args)
+
+static u8 *
+mxms_data(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	return dev_priv->mxms;
+
+}
+
+static u16
+mxms_version(struct drm_device *dev)
+{
+	u8 *mxms = mxms_data(dev);
+	u16 version = (mxms[4] << 8) | mxms[5];
+	switch (version ) {
+	case 0x0200:
+	case 0x0201:
+	case 0x0300:
+		return version;
+	default:
+		break;
+	}
+
+	MXM_DBG(dev, "unknown version %d.%d\n", mxms[4], mxms[5]);
+	return 0x0000;
+}
+
+static u16
+mxms_headerlen(struct drm_device *dev)
+{
+	return 8;
+}
+
+static u16
+mxms_structlen(struct drm_device *dev)
+{
+	return *(u16 *)&mxms_data(dev)[6];
+}
+
+static bool
+mxms_checksum(struct drm_device *dev)
+{
+	u16 size = mxms_headerlen(dev) + mxms_structlen(dev);
+	u8 *mxms = mxms_data(dev), sum = 0;
+	while (size--)
+		sum += *mxms++;
+	if (sum) {
+		MXM_DBG(dev, "checksum invalid\n");
+		return false;
+	}
+	return true;
+}
+
+static bool
+mxms_valid(struct drm_device *dev)
+{
+	u8 *mxms = mxms_data(dev);
+	if (*(u32 *)mxms != 0x5f4d584d) {
+		MXM_DBG(dev, "signature invalid\n");
+		return false;
+	}
+
+	if (!mxms_version(dev) || !mxms_checksum(dev))
+		return false;
+
+	return true;
+}
+
+static bool
+mxms_foreach(struct drm_device *dev, u8 types,
+	     bool (*exec)(struct drm_device *, u8 *, void *), void *info)
+{
+	u8 *mxms = mxms_data(dev);
+	u8 *desc = mxms + mxms_headerlen(dev);
+	u8 *fini = desc + mxms_structlen(dev) - 1;
+	while (desc < fini) {
+		u8 type = desc[0] & 0x0f;
+		u8 headerlen = 0;
+		u8 recordlen = 0;
+		u8 entries = 0;
+
+		switch (type) {
+		case 0: /* Output Device Structure */
+			if (mxms_version(dev) >= 0x0300)
+				headerlen = 8;
+			else
+				headerlen = 6;
+			break;
+		case 1: /* System Cooling Capability Structure */
+		case 2: /* Thermal Structure */
+		case 3: /* Input Power Structure */
+			headerlen = 4;
+			break;
+		case 4: /* GPIO Device Structure */
+			headerlen = 4;
+			recordlen = 2;
+			entries   = (ROM32(desc[0]) & 0x01f00000) >> 20;
+			break;
+		case 5: /* Vendor Specific Structure */
+			headerlen = 8;
+			break;
+		case 6: /* Backlight Control Structure */
+			if (mxms_version(dev) >= 0x0300) {
+				headerlen = 4;
+				recordlen = 8;
+				entries   = (desc[1] & 0xf0) >> 4;
+			} else {
+				headerlen = 8;
+			}
+			break;
+		case 7: /* Fan Control Structure */
+			headerlen = 8;
+			recordlen = 4;
+			entries   = desc[1] & 0x07;
+			break;
+		default:
+			MXM_DBG(dev, "unknown descriptor type %d\n", type);
+			return false;
+		}
+
+		if ((drm_debug & DRM_UT_DRIVER) && (exec == NULL)) {
+			static const char * mxms_desc_name[] = {
+				"ODS", "SCCS", "TS", "IPS",
+				"GSD", "VSS", "BCS", "FCS",
+			};
+			u8 *dump = desc;
+			int i, j;
+
+			MXM_DBG(dev, "%4s: ", mxms_desc_name[type]);
+			for (j = headerlen - 1; j >= 0; j--)
+				printk("%02x", dump[j]);
+			printk("\n");
+			dump += headerlen;
+
+			for (i = 0; i < entries; i++, dump += recordlen) {
+				MXM_DBG(dev, "      ");
+				for (j = recordlen - 1; j >= 0; j--)
+					printk("%02x", dump[j]);
+				printk("\n");
+			}
+		}
+
+		if (types & (1 << type)) {
+			if (!exec(dev, desc, info))
+				return false;
+		}
+
+		desc += headerlen + (entries * recordlen);
+	}
+
+	return true;
+}
+
+static u8 *
+mxm_table(struct drm_device *dev, u8 *size)
+{
+	struct bit_entry x;
+
+	if (bit_table(dev, 'x', &x)) {
+		MXM_DBG(dev, "BIT 'x' table not present\n");
+		return NULL;
+	}
+
+	if (x.version != 1 || x.length < 3) {
+		MXM_MSG(dev, "BIT x table %d/%d unknown\n",
+			x.version, x.length);
+		return NULL;
+	}
+
+	*size = x.length;
+	return x.data;
+}
+
+/* These map MXM v2.x digital connection values to the appropriate SOR/link,
+ * hopefully they're correct for all boards within the same chipset...
+ *
+ * MXM v3.x VBIOS are nicer and provide pointers to these tables.
+ */
+static u8 nv84_sor_map[16] = {
+	0x00, 0x12, 0x22, 0x11, 0x32, 0x31, 0x11, 0x31,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static u8 nv92_sor_map[16] = {
+	0x00, 0x12, 0x22, 0x11, 0x32, 0x31, 0x11, 0x31,
+	0x11, 0x31, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static u8 nv94_sor_map[16] = {
+	0x00, 0x14, 0x24, 0x11, 0x34, 0x31, 0x11, 0x31,
+	0x11, 0x31, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static u8 nv96_sor_map[16] = {
+	0x00, 0x14, 0x24, 0x00, 0x34, 0x00, 0x11, 0x31,
+	0x11, 0x31, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static u8 nv98_sor_map[16] = {
+	0x00, 0x14, 0x12, 0x11, 0x00, 0x31, 0x11, 0x31,
+	0x11, 0x31, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static u8
+mxm_sor_map(struct drm_device *dev, u8 conn)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	u8 len, *mxm = mxm_table(dev, &len);
+	if (mxm && len >= 6) {
+		u8 *map = ROMPTR(dev, mxm[4]);
+		if (map) {
+			if (map[0] == 0x10) {
+				if (conn < map[3])
+					return map[map[1] + conn];
+				return 0x00;
+			}
+
+			MXM_MSG(dev, "unknown sor map 0x%02x\n", map[0]);
+		}
+	}
+
+	if (dev_priv->chipset == 0x84 || dev_priv->chipset == 0x86)
+		return nv84_sor_map[conn];
+	if (dev_priv->chipset == 0x92)
+		return nv92_sor_map[conn];
+	if (dev_priv->chipset == 0x94)
+		return nv94_sor_map[conn];
+	if (dev_priv->chipset == 0x96)
+		return nv96_sor_map[conn];
+	if (dev_priv->chipset == 0x98)
+		return nv98_sor_map[conn];
+
+	MXM_MSG(dev, "missing sor map\n");
+	return 0x00;
+}
+
+static u8
+mxm_ddc_map(struct drm_device *dev, u8 port)
+{
+	u8 len, *mxm = mxm_table(dev, &len);
+	if (mxm && len >= 8) {
+		u8 *map = ROMPTR(dev, mxm[6]);
+		if (map) {
+			if (map[0] == 0x10) {
+				if (port < map[3])
+					return map[map[1] + port];
+				return 0x00;
+			}
+
+			MXM_MSG(dev, "unknown ddc map 0x%02x\n", map[0]);
+		}
+	}
+
+	/* v2.x: directly write port as dcb i2cidx */
+	return (port << 4) | port;
+}
+
+struct mxms_odev {
+	u8 outp_type;
+	u8 conn_type;
+	u8 ddc_port;
+	u8 dig_conn;
+};
+
+static void
+mxms_output_device(struct drm_device *dev, u8 *pdata, struct mxms_odev *desc)
+{
+	u64 data = ROM32(pdata[0]);
+	if (mxms_version(dev) >= 0x0300)
+		data |= (u64)ROM16(pdata[4]) << 32;
+
+	desc->outp_type = (data & 0x00000000000000f0ULL) >> 4;
+	desc->ddc_port  = (data & 0x0000000000000f00ULL) >> 8;
+	desc->conn_type = (data & 0x000000000001f000ULL) >> 12;
+	desc->dig_conn  = (data & 0x0000000000780000ULL) >> 19;
+}
+
+struct context {
+	u32 *outp;
+	struct mxms_odev desc;
+};
+
+static bool
+mxm_match_tmds_partner(struct drm_device *dev, u8 *data, void *info)
+{
+	struct context *ctx = info;
+	struct mxms_odev desc;
+
+	mxms_output_device(dev, data, &desc);
+	if (desc.outp_type == 2 &&
+	    desc.dig_conn == ctx->desc.dig_conn)
+		return false;
+	return true;
+}
+
+static bool
+mxm_match_dcb(struct drm_device *dev, u8 *data, void *info)
+{
+	struct context *ctx = info;
+	u64 desc = *(u64 *)data;
+
+	mxms_output_device(dev, data, &ctx->desc);
+
+	/* match dcb encoder type to mxm-ods device type */
+	if ((ctx->outp[0] & 0x0000000f) != ctx->desc.outp_type)
+		return true;
+
+	/* digital output, have some extra stuff to match here, there's a
+	 * table in the vbios that provides a mapping from the mxm digital
+	 * connection enum values to SOR/link
+	 */
+	if ((desc & 0x00000000000000f0) >= 0x20) {
+		/* check against sor index */
+		u8 link = mxm_sor_map(dev, ctx->desc.dig_conn);
+		if ((ctx->outp[0] & 0x0f000000) != (link & 0x0f) << 24)
+			return true;
+
+		/* check dcb entry has a compatible link field */
+		link = (link & 0x30) >> 4;
+		if ((link & ((ctx->outp[1] & 0x00000030) >> 4)) != link)
+			return true;
+	}
+
+	/* mark this descriptor accounted for by setting invalid device type,
+	 * except of course some manufactures don't follow specs properly and
+	 * we need to avoid killing off the TMDS function on DP connectors
+	 * if MXM-SIS is missing an entry for it.
+	 */
+	data[0] &= ~0xf0;
+	if (ctx->desc.outp_type == 6 && ctx->desc.conn_type == 6 &&
+	    mxms_foreach(dev, 0x01, mxm_match_tmds_partner, ctx)) {
+		data[0] |= 0x20; /* modify descriptor to match TMDS now */
+	} else {
+		data[0] |= 0xf0;
+	}
+
+	return false;
+}
+
+static int
+mxm_dcb_sanitise_entry(struct drm_device *dev, void *data, int idx, u8 *dcbe)
+{
+	struct context ctx = { .outp = (u32 *)dcbe };
+	u8 type, i2cidx, link;
+	u8 *conn;
+
+	/* look for an output device structure that matches this dcb entry.
+	 * if one isn't found, disable it.
+	 */
+	if (mxms_foreach(dev, 0x01, mxm_match_dcb, &ctx)) {
+		MXM_DBG(dev, "disable %d: 0x%08x 0x%08x\n",
+			idx, ctx.outp[0], ctx.outp[1]);
+		ctx.outp[0] |= 0x0000000f;
+		return 0;
+	}
+
+	/* modify the output's ddc/aux port, there's a pointer to a table
+	 * with the mapping from mxm ddc/aux port to dcb i2c_index in the
+	 * vbios mxm table
+	 */
+	i2cidx = mxm_ddc_map(dev, ctx.desc.ddc_port);
+	if ((ctx.outp[0] & 0x0000000f) != OUTPUT_DP)
+		i2cidx = (i2cidx & 0x0f) << 4;
+	else
+		i2cidx = (i2cidx & 0xf0);
+
+	if (i2cidx != 0xf0) {
+		ctx.outp[0] &= ~0x000000f0;
+		ctx.outp[0] |= i2cidx;
+	}
+
+	/* override dcb sorconf.link, based on what mxm data says */
+	switch (ctx.desc.outp_type) {
+	case 0x00: /* Analog CRT */
+	case 0x01: /* Analog TV/HDTV */
+		break;
+	default:
+		link = mxm_sor_map(dev, ctx.desc.dig_conn) & 0x30;
+		ctx.outp[1] &= ~0x00000030;
+		ctx.outp[1] |= link;
+		break;
+	}
+
+	/* we may need to fixup various other vbios tables based on what
+	 * the descriptor says the connector type should be.
+	 *
+	 * in a lot of cases, the vbios tables will claim DVI-I is possible,
+	 * and the mxm data says the connector is really HDMI.  another
+	 * common example is DP->eDP.
+	 */
+	conn = dcb_conn(dev, (ctx.outp[0] & 0x0000f000) >> 12);
+	type = conn[0];
+	switch (ctx.desc.conn_type) {
+	case 0x01: /* LVDS */
+		ctx.outp[1] |= 0x00000004; /* use_power_scripts */
+		/* XXX: modify default link width in LVDS table */
+		break;
+	case 0x02: /* HDMI */
+		type = DCB_CONNECTOR_HDMI_1;
+		break;
+	case 0x03: /* DVI-D */
+		type = DCB_CONNECTOR_DVI_D;
+		break;
+	case 0x0e: /* eDP, falls through to DPint */
+		ctx.outp[1] |= 0x00010000;
+	case 0x07: /* DP internal, wtf is this?? HP8670w */
+		ctx.outp[1] |= 0x00000004; /* use_power_scripts? */
+		type = DCB_CONNECTOR_eDP;
+		break;
+	default:
+		break;
+	}
+
+	if (mxms_version(dev) >= 0x0300)
+		conn[0] = type;
+
+	return 0;
+}
+
+static bool
+mxm_show_unmatched(struct drm_device *dev, u8 *data, void *info)
+{
+	u64 desc = *(u64 *)data;
+	if ((desc & 0xf0) != 0xf0)
+		MXM_MSG(dev, "unmatched output device 0x%016llx\n", desc);
+	return true;
+}
+
+static void
+mxm_dcb_sanitise(struct drm_device *dev)
+{
+	u8 *dcb = dcb_table(dev);
+	if (!dcb || dcb[0] != 0x40) {
+		MXM_DBG(dev, "unsupported DCB version\n");
+		return;
+	}
+
+	dcb_outp_foreach(dev, NULL, mxm_dcb_sanitise_entry);
+	mxms_foreach(dev, 0x01, mxm_show_unmatched, NULL);
+}
+
+static bool
+mxm_shadow_rom_fetch(struct nouveau_i2c_chan *i2c, u8 addr,
+		     u8 offset, u8 size, u8 *data)
+{
+	struct i2c_msg msgs[] = {
+		{ .addr = addr, .flags = 0, .len = 1, .buf = &offset },
+		{ .addr = addr, .flags = I2C_M_RD, .len = size, .buf = data, },
+	};
+
+	return i2c_transfer(&i2c->adapter, msgs, 2) == 2;
+}
+
+static bool
+mxm_shadow_rom(struct drm_device *dev, u8 version)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_i2c_chan *i2c = NULL;
+	u8 i2cidx, mxms[6], addr, size;
+
+	i2cidx = mxm_ddc_map(dev, 1 /* LVDS_DDC */) & 0x0f;
+	if (i2cidx < 0x0f)
+		i2c = nouveau_i2c_find(dev, i2cidx);
+	if (!i2c)
+		return false;
+
+	addr = 0x54;
+	if (!mxm_shadow_rom_fetch(i2c, addr, 0, 6, mxms)) {
+		addr = 0x56;
+		if (!mxm_shadow_rom_fetch(i2c, addr, 0, 6, mxms))
+			return false;
+	}
+
+	dev_priv->mxms = mxms;
+	size = mxms_headerlen(dev) + mxms_structlen(dev);
+	dev_priv->mxms = kmalloc(size, GFP_KERNEL);
+
+	if (dev_priv->mxms &&
+	    mxm_shadow_rom_fetch(i2c, addr, 0, size, dev_priv->mxms))
+		return true;
+
+	kfree(dev_priv->mxms);
+	dev_priv->mxms = NULL;
+	return false;
+}
+
+#if defined(CONFIG_ACPI)
+static bool
+mxm_shadow_dsm(struct drm_device *dev, u8 version)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	static char muid[] = {
+		0x00, 0xA4, 0x04, 0x40, 0x7D, 0x91, 0xF2, 0x4C,
+		0xB8, 0x9C, 0x79, 0xB6, 0x2F, 0xD5, 0x56, 0x65
+	};
+	u32 mxms_args[] = { 0x00000000 };
+	union acpi_object args[4] = {
+		/* _DSM MUID */
+		{ .buffer.type = 3,
+		  .buffer.length = sizeof(muid),
+		  .buffer.pointer = muid,
+		},
+		/* spec says this can be zero to mean "highest revision", but
+		 * of course there's at least one bios out there which fails
+		 * unless you pass in exactly the version it supports..
+		 */
+		{ .integer.type = ACPI_TYPE_INTEGER,
+		  .integer.value = (version & 0xf0) << 4 | (version & 0x0f),
+		},
+		/* MXMS function */
+		{ .integer.type = ACPI_TYPE_INTEGER,
+		  .integer.value = 0x00000010,
+		},
+		/* Pointer to MXMS arguments */
+		{ .buffer.type = ACPI_TYPE_BUFFER,
+		  .buffer.length = sizeof(mxms_args),
+		  .buffer.pointer = (char *)mxms_args,
+		},
+	};
+	struct acpi_object_list list = { ARRAY_SIZE(args), args };
+	struct acpi_buffer retn = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *obj;
+	acpi_handle handle;
+	int ret;
+
+	handle = DEVICE_ACPI_HANDLE(&dev->pdev->dev);
+	if (!handle)
+		return false;
+
+	ret = acpi_evaluate_object(handle, "_DSM", &list, &retn);
+	if (ret) {
+		MXM_DBG(dev, "DSM MXMS failed: %d\n", ret);
+		return false;
+	}
+
+	obj = retn.pointer;
+	if (obj->type == ACPI_TYPE_BUFFER) {
+		dev_priv->mxms = kmemdup(obj->buffer.pointer,
+					 obj->buffer.length, GFP_KERNEL);
+	} else
+	if (obj->type == ACPI_TYPE_INTEGER) {
+		MXM_DBG(dev, "DSM MXMS returned 0x%llx\n", obj->integer.value);
+	}
+
+	kfree(obj);
+	return dev_priv->mxms != NULL;
+}
+#endif
+
+#if defined(CONFIG_ACPI_WMI) || defined(CONFIG_ACPI_WMI_MODULE)
+
+#define WMI_WMMX_GUID "F6CB5C3C-9CAE-4EBD-B577-931EA32A2CC0"
+
+static bool
+mxm_shadow_wmi(struct drm_device *dev, u8 version)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	u32 mxms_args[] = { 0x534D584D /* MXMS */, version, 0 };
+	struct acpi_buffer args = { sizeof(mxms_args), mxms_args };
+	struct acpi_buffer retn = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *obj;
+	acpi_status status;
+
+	if (!wmi_has_guid(WMI_WMMX_GUID))
+		return false;
+
+	status = wmi_evaluate_method(WMI_WMMX_GUID, 0, 0, &args, &retn);
+	if (ACPI_FAILURE(status)) {
+		MXM_DBG(dev, "WMMX MXMS returned %d\n", status);
+		return false;
+	}
+
+	obj = retn.pointer;
+	if (obj->type == ACPI_TYPE_BUFFER) {
+		dev_priv->mxms = kmemdup(obj->buffer.pointer,
+					 obj->buffer.length, GFP_KERNEL);
+	}
+
+	kfree(obj);
+	return dev_priv->mxms != NULL;
+}
+#endif
+
+struct mxm_shadow_h {
+	const char *name;
+	bool (*exec)(struct drm_device *, u8 version);
+} _mxm_shadow[] = {
+	{ "ROM", mxm_shadow_rom },
+#if defined(CONFIG_ACPI)
+	{ "DSM", mxm_shadow_dsm },
+#endif
+#if defined(CONFIG_ACPI_WMI) || defined(CONFIG_ACPI_WMI_MODULE)
+	{ "WMI", mxm_shadow_wmi },
+#endif
+	{}
+};
+
+static int
+mxm_shadow(struct drm_device *dev, u8 version)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct mxm_shadow_h *shadow = _mxm_shadow;
+	do {
+		MXM_DBG(dev, "checking %s\n", shadow->name);
+		if (shadow->exec(dev, version)) {
+			if (mxms_valid(dev))
+				return 0;
+			kfree(dev_priv->mxms);
+			dev_priv->mxms = NULL;
+		}
+	} while ((++shadow)->name);
+	return -ENOENT;
+}
+
+int
+nouveau_mxm_init(struct drm_device *dev)
+{
+	u8 mxm_size, *mxm = mxm_table(dev, &mxm_size);
+	if (!mxm || !mxm[0]) {
+		MXM_MSG(dev, "no VBIOS data, nothing to do\n");
+		return 0;
+	}
+
+	MXM_MSG(dev, "BIOS version %d.%d\n", mxm[0] >> 4, mxm[0] & 0x0f);
+
+	if (mxm_shadow(dev, mxm[0])) {
+		MXM_MSG(dev, "failed to locate valid SIS\n");
+		return -EINVAL;
+	}
+
+	MXM_MSG(dev, "MXMS Version %d.%d\n",
+		mxms_version(dev) >> 8, mxms_version(dev) & 0xff);
+	mxms_foreach(dev, 0, NULL, NULL);
+
+	if (nouveau_mxmdcb)
+		mxm_dcb_sanitise(dev);
+	return 0;
+}
+
+void
+nouveau_mxm_fini(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	kfree(dev_priv->mxms);
+	dev_priv->mxms = NULL;
+}
diff --git a/drivers/gpu/drm/nouveau/nouveau_notifier.c b/drivers/gpu/drm/nouveau/nouveau_notifier.c
index 6abdbe6530a7..2ef883c4bbc1 100644
--- a/drivers/gpu/drm/nouveau/nouveau_notifier.c
+++ b/drivers/gpu/drm/nouveau/nouveau_notifier.c
@@ -115,7 +115,7 @@ nouveau_notifier_alloc(struct nouveau_channel *chan, uint32_t handle,
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *nobj = NULL;
 	struct drm_mm_node *mem;
-	uint32_t offset;
+	uint64_t offset;
 	int target, ret;
 
 	mem = drm_mm_search_free_in_range(&chan->notifier_heap, size, 0,
diff --git a/drivers/gpu/drm/nouveau/nouveau_object.c b/drivers/gpu/drm/nouveau/nouveau_object.c
index 960c0ae0c0c3..cc419fae794b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_object.c
+++ b/drivers/gpu/drm/nouveau/nouveau_object.c
@@ -723,14 +723,14 @@ nvc0_gpuobj_channel_init(struct nouveau_channel *chan, struct nouveau_vm *vm)
 	nv_wo32(chan->ramin, 0x020c, 0x000000ff);
 
 	/* map display semaphore buffers into channel's vm */
-	if (dev_priv->card_type >= NV_D0)
-		return 0;
-
-	for (i = 0; i < 2; i++) {
-		struct nv50_display_crtc *dispc = &nv50_display(dev)->crtc[i];
-
-		ret = nouveau_bo_vma_add(dispc->sem.bo, chan->vm,
-					 &chan->dispc_vma[i]);
+	for (i = 0; i < dev->mode_config.num_crtc; i++) {
+		struct nouveau_bo *bo;
+		if (dev_priv->card_type >= NV_D0)
+			bo = nvd0_display_crtc_sema(dev, i);
+		else
+			bo = nv50_display(dev)->crtc[i].sem.bo;
+
+		ret = nouveau_bo_vma_add(bo, chan->vm, &chan->dispc_vma[i]);
 		if (ret)
 			return ret;
 	}
@@ -879,9 +879,14 @@ nouveau_gpuobj_channel_takedown(struct nouveau_channel *chan)
 
 	NV_DEBUG(dev, "ch%d\n", chan->id);
 
-	if (dev_priv->card_type >= NV_50 && dev_priv->card_type <= NV_C0) {
+	if (dev_priv->card_type >= NV_D0) {
+		for (i = 0; i < dev->mode_config.num_crtc; i++) {
+			struct nouveau_bo *bo = nvd0_display_crtc_sema(dev, i);
+			nouveau_bo_vma_del(bo, &chan->dispc_vma[i]);
+		}
+	} else
+	if (dev_priv->card_type >= NV_50) {
 		struct nv50_display *disp = nv50_display(dev);
-
 		for (i = 0; i < dev->mode_config.num_crtc; i++) {
 			struct nv50_display_crtc *dispc = &disp->crtc[i];
 			nouveau_bo_vma_del(dispc->sem.bo, &chan->dispc_vma[i]);
diff --git a/drivers/gpu/drm/nouveau/nouveau_perf.c b/drivers/gpu/drm/nouveau/nouveau_perf.c
index 33d03fbf00df..58f497343cec 100644
--- a/drivers/gpu/drm/nouveau/nouveau_perf.c
+++ b/drivers/gpu/drm/nouveau/nouveau_perf.c
@@ -41,7 +41,7 @@ legacy_perf_init(struct drm_device *dev)
 		return;
 	}
 
-	perf = ROMPTR(bios, bmp[0x73]);
+	perf = ROMPTR(dev, bmp[0x73]);
 	if (!perf) {
 		NV_DEBUG(dev, "No memclock table pointer found.\n");
 		return;
@@ -87,7 +87,7 @@ nouveau_perf_timing(struct drm_device *dev, struct bit_entry *P,
 	 * ramcfg to select the correct subentry
 	 */
 	if (P->version == 2) {
-		u8 *tmap = ROMPTR(bios, P->data[4]);
+		u8 *tmap = ROMPTR(dev, P->data[4]);
 		if (!tmap) {
 			NV_DEBUG(dev, "no timing map pointer\n");
 			return NULL;
@@ -140,7 +140,6 @@ nouveau_perf_voltage(struct drm_device *dev, struct bit_entry *P,
 		     struct nouveau_pm_level *perflvl)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nvbios *bios = &dev_priv->vbios;
 	u8 *vmap;
 	int id;
 
@@ -165,7 +164,7 @@ nouveau_perf_voltage(struct drm_device *dev, struct bit_entry *P,
 		return;
 	}
 
-	vmap = ROMPTR(bios, P->data[32]);
+	vmap = ROMPTR(dev, P->data[32]);
 	if (!vmap) {
 		NV_DEBUG(dev, "volt map table pointer invalid\n");
 		return;
@@ -200,12 +199,14 @@ nouveau_perf_init(struct drm_device *dev)
 			return;
 		}
 
-		perf = ROMPTR(bios, P.data[0]);
+		perf = ROMPTR(dev, P.data[0]);
 		version   = perf[0];
 		headerlen = perf[1];
 		if (version < 0x40) {
 			recordlen = perf[3] + (perf[4] * perf[5]);
 			entries   = perf[2];
+
+			pm->pwm_divisor = ROM16(perf[6]);
 		} else {
 			recordlen = perf[2] + (perf[3] * perf[4]);
 			entries   = perf[5];
@@ -216,7 +217,7 @@ nouveau_perf_init(struct drm_device *dev)
 			return;
 		}
 
-		perf = ROMPTR(bios, bios->data[bios->offset + 0x94]);
+		perf = ROMPTR(dev, bios->data[bios->offset + 0x94]);
 		if (!perf) {
 			NV_DEBUG(dev, "perf table pointer invalid\n");
 			return;
@@ -283,7 +284,6 @@ nouveau_perf_init(struct drm_device *dev)
 				perflvl->memory = ROM16(entry[11]) * 1000;
 			else
 				perflvl->memory = ROM16(entry[11]) * 2000;
-
 			break;
 		case 0x25:
 			perflvl->fanspeed = entry[4];
@@ -300,8 +300,8 @@ nouveau_perf_init(struct drm_device *dev)
 			perflvl->core = ROM16(entry[8]) * 1000;
 			perflvl->shader = ROM16(entry[10]) * 1000;
 			perflvl->memory = ROM16(entry[12]) * 1000;
-			/*XXX: confirm on 0x35 */
-			perflvl->unk05 = ROM16(entry[16]) * 1000;
+			perflvl->vdec = ROM16(entry[16]) * 1000;
+			perflvl->dom6 = ROM16(entry[20]) * 1000;
 			break;
 		case 0x40:
 #define subent(n) (ROM16(entry[perf[2] + ((n) * perf[3])]) & 0xfff) * 1000
diff --git a/drivers/gpu/drm/nouveau/nouveau_pm.c b/drivers/gpu/drm/nouveau/nouveau_pm.c
index a539fd257921..9064d7f19794 100644
--- a/drivers/gpu/drm/nouveau/nouveau_pm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_pm.c
@@ -26,6 +26,7 @@
 
 #include "nouveau_drv.h"
 #include "nouveau_pm.h"
+#include "nouveau_gpio.h"
 
 #ifdef CONFIG_ACPI
 #include <linux/acpi.h>
@@ -35,22 +36,95 @@
 #include <linux/hwmon-sysfs.h>
 
 static int
-nouveau_pm_clock_set(struct drm_device *dev, struct nouveau_pm_level *perflvl,
-		     u8 id, u32 khz)
+nouveau_pwmfan_get(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
-	void *pre_state;
+	struct gpio_func gpio;
+	u32 divs, duty;
+	int ret;
 
-	if (khz == 0)
-		return 0;
+	if (!pm->pwm_get)
+		return -ENODEV;
+
+	ret = nouveau_gpio_find(dev, 0, DCB_GPIO_PWM_FAN, 0xff, &gpio);
+	if (ret == 0) {
+		ret = pm->pwm_get(dev, gpio.line, &divs, &duty);
+		if (ret == 0) {
+			divs = max(divs, duty);
+			if (dev_priv->card_type <= NV_40 || (gpio.log[0] & 1))
+				duty = divs - duty;
+			return (duty * 100) / divs;
+		}
+
+		return nouveau_gpio_func_get(dev, gpio.func) * 100;
+	}
+
+	return -ENODEV;
+}
+
+static int
+nouveau_pwmfan_set(struct drm_device *dev, int percent)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+	struct gpio_func gpio;
+	u32 divs, duty;
+	int ret;
+
+	if (!pm->pwm_set)
+		return -ENODEV;
+
+	ret = nouveau_gpio_find(dev, 0, DCB_GPIO_PWM_FAN, 0xff, &gpio);
+	if (ret == 0) {
+		divs = pm->pwm_divisor;
+		if (pm->fan.pwm_freq) {
+			/*XXX: PNVIO clock more than likely... */
+			divs = 135000 / pm->fan.pwm_freq;
+			if (dev_priv->chipset < 0xa3)
+				divs /= 4;
+		}
+
+		duty = ((divs * percent) + 99) / 100;
+		if (dev_priv->card_type <= NV_40 || (gpio.log[0] & 1))
+			duty = divs - duty;
 
-	pre_state = pm->clock_pre(dev, perflvl, id, khz);
-	if (IS_ERR(pre_state))
-		return PTR_ERR(pre_state);
+		return pm->pwm_set(dev, gpio.line, divs, duty);
+	}
+
+	return -ENODEV;
+}
+
+static int
+nouveau_pm_perflvl_aux(struct drm_device *dev, struct nouveau_pm_level *perflvl,
+		       struct nouveau_pm_level *a, struct nouveau_pm_level *b)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+	int ret;
+
+	/*XXX: not on all boards, we should control based on temperature
+	 *     on recent boards..  or maybe on some other factor we don't
+	 *     know about?
+	 */
+	if (a->fanspeed && b->fanspeed && b->fanspeed > a->fanspeed) {
+		ret = nouveau_pwmfan_set(dev, perflvl->fanspeed);
+		if (ret && ret != -ENODEV) {
+			NV_ERROR(dev, "fanspeed set failed: %d\n", ret);
+			return ret;
+		}
+	}
+
+	if (pm->voltage.supported && pm->voltage_set) {
+		if (perflvl->volt_min && b->volt_min > a->volt_min) {
+			ret = pm->voltage_set(dev, perflvl->volt_min);
+			if (ret) {
+				NV_ERROR(dev, "voltage set failed: %d\n", ret);
+				return ret;
+			}
+		}
+	}
 
-	if (pre_state)
-		pm->clock_set(dev, pre_state);
 	return 0;
 }
 
@@ -59,31 +133,24 @@ nouveau_pm_perflvl_set(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+	void *state;
 	int ret;
 
 	if (perflvl == pm->cur)
 		return 0;
 
-	if (pm->voltage.supported && pm->voltage_set && perflvl->volt_min) {
-		ret = pm->voltage_set(dev, perflvl->volt_min);
-		if (ret) {
-			NV_ERROR(dev, "voltage_set %d failed: %d\n",
-				 perflvl->volt_min, ret);
-		}
-	}
+	ret = nouveau_pm_perflvl_aux(dev, perflvl, pm->cur, perflvl);
+	if (ret)
+		return ret;
 
-	if (pm->clocks_pre) {
-		void *state = pm->clocks_pre(dev, perflvl);
-		if (IS_ERR(state))
-			return PTR_ERR(state);
-		pm->clocks_set(dev, state);
-	} else
-	if (pm->clock_set) {
-		nouveau_pm_clock_set(dev, perflvl, PLL_CORE, perflvl->core);
-		nouveau_pm_clock_set(dev, perflvl, PLL_SHADER, perflvl->shader);
-		nouveau_pm_clock_set(dev, perflvl, PLL_MEMORY, perflvl->memory);
-		nouveau_pm_clock_set(dev, perflvl, PLL_UNK05, perflvl->unk05);
-	}
+	state = pm->clocks_pre(dev, perflvl);
+	if (IS_ERR(state))
+		return PTR_ERR(state);
+	pm->clocks_set(dev, state);
+
+	ret = nouveau_pm_perflvl_aux(dev, perflvl, perflvl, pm->cur);
+	if (ret)
+		return ret;
 
 	pm->cur = perflvl;
 	return 0;
@@ -130,28 +197,9 @@ nouveau_pm_perflvl_get(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 
 	memset(perflvl, 0, sizeof(*perflvl));
 
-	if (pm->clocks_get) {
-		ret = pm->clocks_get(dev, perflvl);
-		if (ret)
-			return ret;
-	} else
-	if (pm->clock_get) {
-		ret = pm->clock_get(dev, PLL_CORE);
-		if (ret > 0)
-			perflvl->core = ret;
-
-		ret = pm->clock_get(dev, PLL_MEMORY);
-		if (ret > 0)
-			perflvl->memory = ret;
-
-		ret = pm->clock_get(dev, PLL_SHADER);
-		if (ret > 0)
-			perflvl->shader = ret;
-
-		ret = pm->clock_get(dev, PLL_UNK05);
-		if (ret > 0)
-			perflvl->unk05 = ret;
-	}
+	ret = pm->clocks_get(dev, perflvl);
+	if (ret)
+		return ret;
 
 	if (pm->voltage.supported && pm->voltage_get) {
 		ret = pm->voltage_get(dev);
@@ -161,6 +209,10 @@ nouveau_pm_perflvl_get(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 		}
 	}
 
+	ret = nouveau_pwmfan_get(dev);
+	if (ret > 0)
+		perflvl->fanspeed = ret;
+
 	return 0;
 }
 
@@ -412,6 +464,172 @@ static SENSOR_DEVICE_ATTR(update_rate, S_IRUGO,
 						nouveau_hwmon_show_update_rate,
 						NULL, 0);
 
+static ssize_t
+nouveau_hwmon_show_fan0_input(struct device *d, struct device_attribute *attr,
+			      char *buf)
+{
+	struct drm_device *dev = dev_get_drvdata(d);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_timer_engine *ptimer = &dev_priv->engine.timer;
+	struct gpio_func gpio;
+	u32 cycles, cur, prev;
+	u64 start;
+	int ret;
+
+	ret = nouveau_gpio_find(dev, 0, DCB_GPIO_FAN_SENSE, 0xff, &gpio);
+	if (ret)
+		return ret;
+
+	/* Monitor the GPIO input 0x3b for 250ms.
+	 * When the fan spins, it changes the value of GPIO FAN_SENSE.
+	 * We get 4 changes (0 -> 1 -> 0 -> 1 -> [...]) per complete rotation.
+	 */
+	start = ptimer->read(dev);
+	prev = nouveau_gpio_sense(dev, 0, gpio.line);
+	cycles = 0;
+	do {
+		cur = nouveau_gpio_sense(dev, 0, gpio.line);
+		if (prev != cur) {
+			cycles++;
+			prev = cur;
+		}
+
+		usleep_range(500, 1000); /* supports 0 < rpm < 7500 */
+	} while (ptimer->read(dev) - start < 250000000);
+
+	/* interpolate to get rpm */
+	return sprintf(buf, "%i\n", cycles / 4 * 4 * 60);
+}
+static SENSOR_DEVICE_ATTR(fan0_input, S_IRUGO, nouveau_hwmon_show_fan0_input,
+			  NULL, 0);
+
+static ssize_t
+nouveau_hwmon_get_pwm0(struct device *d, struct device_attribute *a, char *buf)
+{
+	struct drm_device *dev = dev_get_drvdata(d);
+	int ret;
+
+	ret = nouveau_pwmfan_get(dev);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%i\n", ret);
+}
+
+static ssize_t
+nouveau_hwmon_set_pwm0(struct device *d, struct device_attribute *a,
+		       const char *buf, size_t count)
+{
+	struct drm_device *dev = dev_get_drvdata(d);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+	int ret = -ENODEV;
+	long value;
+
+	if (nouveau_perflvl_wr != 7777)
+		return -EPERM;
+
+	if (strict_strtol(buf, 10, &value) == -EINVAL)
+		return -EINVAL;
+
+	if (value < pm->fan.min_duty)
+		value = pm->fan.min_duty;
+	if (value > pm->fan.max_duty)
+		value = pm->fan.max_duty;
+
+	ret = nouveau_pwmfan_set(dev, value);
+	if (ret)
+		return ret;
+
+	return count;
+}
+
+static SENSOR_DEVICE_ATTR(pwm0, S_IRUGO | S_IWUSR,
+			  nouveau_hwmon_get_pwm0,
+			  nouveau_hwmon_set_pwm0, 0);
+
+static ssize_t
+nouveau_hwmon_get_pwm0_min(struct device *d,
+			   struct device_attribute *a, char *buf)
+{
+	struct drm_device *dev = dev_get_drvdata(d);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+
+	return sprintf(buf, "%i\n", pm->fan.min_duty);
+}
+
+static ssize_t
+nouveau_hwmon_set_pwm0_min(struct device *d, struct device_attribute *a,
+			   const char *buf, size_t count)
+{
+	struct drm_device *dev = dev_get_drvdata(d);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+	long value;
+
+	if (strict_strtol(buf, 10, &value) == -EINVAL)
+		return -EINVAL;
+
+	if (value < 0)
+		value = 0;
+
+	if (pm->fan.max_duty - value < 10)
+		value = pm->fan.max_duty - 10;
+
+	if (value < 10)
+		pm->fan.min_duty = 10;
+	else
+		pm->fan.min_duty = value;
+
+	return count;
+}
+
+static SENSOR_DEVICE_ATTR(pwm0_min, S_IRUGO | S_IWUSR,
+			  nouveau_hwmon_get_pwm0_min,
+			  nouveau_hwmon_set_pwm0_min, 0);
+
+static ssize_t
+nouveau_hwmon_get_pwm0_max(struct device *d,
+			   struct device_attribute *a, char *buf)
+{
+	struct drm_device *dev = dev_get_drvdata(d);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+
+	return sprintf(buf, "%i\n", pm->fan.max_duty);
+}
+
+static ssize_t
+nouveau_hwmon_set_pwm0_max(struct device *d, struct device_attribute *a,
+			   const char *buf, size_t count)
+{
+	struct drm_device *dev = dev_get_drvdata(d);
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+	long value;
+
+	if (strict_strtol(buf, 10, &value) == -EINVAL)
+		return -EINVAL;
+
+	if (value < 0)
+		value = 0;
+
+	if (value - pm->fan.min_duty < 10)
+		value = pm->fan.min_duty + 10;
+
+	if (value > 100)
+		pm->fan.max_duty = 100;
+	else
+		pm->fan.max_duty = value;
+
+	return count;
+}
+
+static SENSOR_DEVICE_ATTR(pwm0_max, S_IRUGO | S_IWUSR,
+			  nouveau_hwmon_get_pwm0_max,
+			  nouveau_hwmon_set_pwm0_max, 0);
+
 static struct attribute *hwmon_attributes[] = {
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
@@ -420,20 +638,36 @@ static struct attribute *hwmon_attributes[] = {
 	&sensor_dev_attr_update_rate.dev_attr.attr,
 	NULL
 };
+static struct attribute *hwmon_fan_rpm_attributes[] = {
+	&sensor_dev_attr_fan0_input.dev_attr.attr,
+	NULL
+};
+static struct attribute *hwmon_pwm_fan_attributes[] = {
+	&sensor_dev_attr_pwm0.dev_attr.attr,
+	&sensor_dev_attr_pwm0_min.dev_attr.attr,
+	&sensor_dev_attr_pwm0_max.dev_attr.attr,
+	NULL
+};
 
 static const struct attribute_group hwmon_attrgroup = {
 	.attrs = hwmon_attributes,
 };
+static const struct attribute_group hwmon_fan_rpm_attrgroup = {
+	.attrs = hwmon_fan_rpm_attributes,
+};
+static const struct attribute_group hwmon_pwm_fan_attrgroup = {
+	.attrs = hwmon_pwm_fan_attributes,
+};
 #endif
 
 static int
 nouveau_hwmon_init(struct drm_device *dev)
 {
-#if defined(CONFIG_HWMON) || (defined(MODULE) && defined(CONFIG_HWMON_MODULE))
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_pm_engine *pm = &dev_priv->engine.pm;
+#if defined(CONFIG_HWMON) || (defined(MODULE) && defined(CONFIG_HWMON_MODULE))
 	struct device *hwmon_dev;
-	int ret;
+	int ret = 0;
 
 	if (!pm->temp_get)
 		return -ENODEV;
@@ -446,17 +680,46 @@ nouveau_hwmon_init(struct drm_device *dev)
 		return ret;
 	}
 	dev_set_drvdata(hwmon_dev, dev);
+
+	/* default sysfs entries */
 	ret = sysfs_create_group(&dev->pdev->dev.kobj, &hwmon_attrgroup);
 	if (ret) {
-		NV_ERROR(dev,
-			"Unable to create hwmon sysfs file: %d\n", ret);
-		hwmon_device_unregister(hwmon_dev);
-		return ret;
+		if (ret)
+			goto error;
+	}
+
+	/* if the card has a pwm fan */
+	/*XXX: incorrect, need better detection for this, some boards have
+	 *     the gpio entries for pwm fan control even when there's no
+	 *     actual fan connected to it... therm table? */
+	if (nouveau_pwmfan_get(dev) >= 0) {
+		ret = sysfs_create_group(&dev->pdev->dev.kobj,
+					 &hwmon_pwm_fan_attrgroup);
+		if (ret)
+			goto error;
+	}
+
+	/* if the card can read the fan rpm */
+	if (nouveau_gpio_func_valid(dev, DCB_GPIO_FAN_SENSE)) {
+		ret = sysfs_create_group(&dev->pdev->dev.kobj,
+					 &hwmon_fan_rpm_attrgroup);
+		if (ret)
+			goto error;
 	}
 
 	pm->hwmon = hwmon_dev;
-#endif
+
+	return 0;
+
+error:
+	NV_ERROR(dev, "Unable to create some hwmon sysfs files: %d\n", ret);
+	hwmon_device_unregister(hwmon_dev);
+	pm->hwmon = NULL;
+	return ret;
+#else
+	pm->hwmon = NULL;
 	return 0;
+#endif
 }
 
 static void
@@ -468,6 +731,9 @@ nouveau_hwmon_fini(struct drm_device *dev)
 
 	if (pm->hwmon) {
 		sysfs_remove_group(&dev->pdev->dev.kobj, &hwmon_attrgroup);
+		sysfs_remove_group(&dev->pdev->dev.kobj, &hwmon_pwm_fan_attrgroup);
+		sysfs_remove_group(&dev->pdev->dev.kobj, &hwmon_fan_rpm_attrgroup);
+
 		hwmon_device_unregister(pm->hwmon);
 	}
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_pm.h b/drivers/gpu/drm/nouveau/nouveau_pm.h
index 8ac02cdd03a1..2f8e14fbcff8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_pm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_pm.h
@@ -47,29 +47,33 @@ void nouveau_mem_timing_init(struct drm_device *);
 void nouveau_mem_timing_fini(struct drm_device *);
 
 /* nv04_pm.c */
-int nv04_pm_clock_get(struct drm_device *, u32 id);
-void *nv04_pm_clock_pre(struct drm_device *, struct nouveau_pm_level *,
-			u32 id, int khz);
-void nv04_pm_clock_set(struct drm_device *, void *);
+int nv04_pm_clocks_get(struct drm_device *, struct nouveau_pm_level *);
+void *nv04_pm_clocks_pre(struct drm_device *, struct nouveau_pm_level *);
+int nv04_pm_clocks_set(struct drm_device *, void *);
 
 /* nv40_pm.c */
 int nv40_pm_clocks_get(struct drm_device *, struct nouveau_pm_level *);
 void *nv40_pm_clocks_pre(struct drm_device *, struct nouveau_pm_level *);
-void nv40_pm_clocks_set(struct drm_device *, void *);
+int nv40_pm_clocks_set(struct drm_device *, void *);
+int nv40_pm_pwm_get(struct drm_device *, int, u32 *, u32 *);
+int nv40_pm_pwm_set(struct drm_device *, int, u32, u32);
 
 /* nv50_pm.c */
-int nv50_pm_clock_get(struct drm_device *, u32 id);
-void *nv50_pm_clock_pre(struct drm_device *, struct nouveau_pm_level *,
-			u32 id, int khz);
-void nv50_pm_clock_set(struct drm_device *, void *);
+int nv50_pm_clocks_get(struct drm_device *, struct nouveau_pm_level *);
+void *nv50_pm_clocks_pre(struct drm_device *, struct nouveau_pm_level *);
+int nv50_pm_clocks_set(struct drm_device *, void *);
+int nv50_pm_pwm_get(struct drm_device *, int, u32 *, u32 *);
+int nv50_pm_pwm_set(struct drm_device *, int, u32, u32);
 
 /* nva3_pm.c */
 int nva3_pm_clocks_get(struct drm_device *, struct nouveau_pm_level *);
 void *nva3_pm_clocks_pre(struct drm_device *, struct nouveau_pm_level *);
-void nva3_pm_clocks_set(struct drm_device *, void *);
+int nva3_pm_clocks_set(struct drm_device *, void *);
 
 /* nvc0_pm.c */
 int nvc0_pm_clocks_get(struct drm_device *, struct nouveau_pm_level *);
+void *nvc0_pm_clocks_pre(struct drm_device *, struct nouveau_pm_level *);
+int nvc0_pm_clocks_set(struct drm_device *, void *);
 
 /* nouveau_temp.c */
 void nouveau_temp_init(struct drm_device *dev);
diff --git a/drivers/gpu/drm/nouveau/nouveau_sgdma.c b/drivers/gpu/drm/nouveau/nouveau_sgdma.c
index c8a463b76c89..47f245edf538 100644
--- a/drivers/gpu/drm/nouveau/nouveau_sgdma.c
+++ b/drivers/gpu/drm/nouveau/nouveau_sgdma.c
@@ -8,91 +8,30 @@
 #define NV_CTXDMA_PAGE_MASK  (NV_CTXDMA_PAGE_SIZE - 1)
 
 struct nouveau_sgdma_be {
-	struct ttm_backend backend;
+	/* this has to be the first field so populate/unpopulated in
+	 * nouve_bo.c works properly, otherwise have to move them here
+	 */
+	struct ttm_dma_tt ttm;
 	struct drm_device *dev;
-
-	dma_addr_t *pages;
-	unsigned nr_pages;
-	bool unmap_pages;
-
 	u64 offset;
-	bool bound;
 };
 
-static int
-nouveau_sgdma_populate(struct ttm_backend *be, unsigned long num_pages,
-		       struct page **pages, struct page *dummy_read_page,
-		       dma_addr_t *dma_addrs)
-{
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
-	struct drm_device *dev = nvbe->dev;
-	int i;
-
-	NV_DEBUG(nvbe->dev, "num_pages = %ld\n", num_pages);
-
-	nvbe->pages = dma_addrs;
-	nvbe->nr_pages = num_pages;
-	nvbe->unmap_pages = true;
-
-	/* this code path isn't called and is incorrect anyways */
-	if (0) { /* dma_addrs[0] != DMA_ERROR_CODE) { */
-		nvbe->unmap_pages = false;
-		return 0;
-	}
-
-	for (i = 0; i < num_pages; i++) {
-		nvbe->pages[i] = pci_map_page(dev->pdev, pages[i], 0,
-					      PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-		if (pci_dma_mapping_error(dev->pdev, nvbe->pages[i])) {
-			nvbe->nr_pages = --i;
-			be->func->clear(be);
-			return -EFAULT;
-		}
-	}
-
-	return 0;
-}
-
 static void
-nouveau_sgdma_clear(struct ttm_backend *be)
+nouveau_sgdma_destroy(struct ttm_tt *ttm)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
-	struct drm_device *dev = nvbe->dev;
-
-	if (nvbe->bound)
-		be->func->unbind(be);
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 
-	if (nvbe->unmap_pages) {
-		while (nvbe->nr_pages--) {
-			pci_unmap_page(dev->pdev, nvbe->pages[nvbe->nr_pages],
-				       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-		}
-		nvbe->unmap_pages = false;
-	}
-
-	nvbe->pages = NULL;
-}
-
-static void
-nouveau_sgdma_destroy(struct ttm_backend *be)
-{
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
-
-	if (be) {
+	if (ttm) {
 		NV_DEBUG(nvbe->dev, "\n");
-
-		if (nvbe) {
-			if (nvbe->pages)
-				be->func->clear(be);
-			kfree(nvbe);
-		}
+		ttm_dma_tt_fini(&nvbe->ttm);
+		kfree(nvbe);
 	}
 }
 
 static int
-nv04_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
+nv04_sgdma_bind(struct ttm_tt *ttm, struct ttm_mem_reg *mem)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 	struct drm_device *dev = nvbe->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *gpuobj = dev_priv->gart_info.sg_ctxdma;
@@ -102,8 +41,8 @@ nv04_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
 
 	nvbe->offset = mem->start << PAGE_SHIFT;
 	pte = (nvbe->offset >> NV_CTXDMA_PAGE_SHIFT) + 2;
-	for (i = 0; i < nvbe->nr_pages; i++) {
-		dma_addr_t dma_offset = nvbe->pages[i];
+	for (i = 0; i < ttm->num_pages; i++) {
+		dma_addr_t dma_offset = nvbe->ttm.dma_address[i];
 		uint32_t offset_l = lower_32_bits(dma_offset);
 
 		for (j = 0; j < PAGE_SIZE / NV_CTXDMA_PAGE_SIZE; j++, pte++) {
@@ -112,14 +51,13 @@ nv04_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
 		}
 	}
 
-	nvbe->bound = true;
 	return 0;
 }
 
 static int
-nv04_sgdma_unbind(struct ttm_backend *be)
+nv04_sgdma_unbind(struct ttm_tt *ttm)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 	struct drm_device *dev = nvbe->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *gpuobj = dev_priv->gart_info.sg_ctxdma;
@@ -127,22 +65,19 @@ nv04_sgdma_unbind(struct ttm_backend *be)
 
 	NV_DEBUG(dev, "\n");
 
-	if (!nvbe->bound)
+	if (ttm->state != tt_bound)
 		return 0;
 
 	pte = (nvbe->offset >> NV_CTXDMA_PAGE_SHIFT) + 2;
-	for (i = 0; i < nvbe->nr_pages; i++) {
+	for (i = 0; i < ttm->num_pages; i++) {
 		for (j = 0; j < PAGE_SIZE / NV_CTXDMA_PAGE_SIZE; j++, pte++)
 			nv_wo32(gpuobj, (pte * 4) + 0, 0x00000000);
 	}
 
-	nvbe->bound = false;
 	return 0;
 }
 
 static struct ttm_backend_func nv04_sgdma_backend = {
-	.populate		= nouveau_sgdma_populate,
-	.clear			= nouveau_sgdma_clear,
 	.bind			= nv04_sgdma_bind,
 	.unbind			= nv04_sgdma_unbind,
 	.destroy		= nouveau_sgdma_destroy
@@ -161,14 +96,14 @@ nv41_sgdma_flush(struct nouveau_sgdma_be *nvbe)
 }
 
 static int
-nv41_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
+nv41_sgdma_bind(struct ttm_tt *ttm, struct ttm_mem_reg *mem)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 	struct drm_nouveau_private *dev_priv = nvbe->dev->dev_private;
 	struct nouveau_gpuobj *pgt = dev_priv->gart_info.sg_ctxdma;
-	dma_addr_t *list = nvbe->pages;
+	dma_addr_t *list = nvbe->ttm.dma_address;
 	u32 pte = mem->start << 2;
-	u32 cnt = nvbe->nr_pages;
+	u32 cnt = ttm->num_pages;
 
 	nvbe->offset = mem->start << PAGE_SHIFT;
 
@@ -178,18 +113,17 @@ nv41_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
 	}
 
 	nv41_sgdma_flush(nvbe);
-	nvbe->bound = true;
 	return 0;
 }
 
 static int
-nv41_sgdma_unbind(struct ttm_backend *be)
+nv41_sgdma_unbind(struct ttm_tt *ttm)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 	struct drm_nouveau_private *dev_priv = nvbe->dev->dev_private;
 	struct nouveau_gpuobj *pgt = dev_priv->gart_info.sg_ctxdma;
 	u32 pte = (nvbe->offset >> 12) << 2;
-	u32 cnt = nvbe->nr_pages;
+	u32 cnt = ttm->num_pages;
 
 	while (cnt--) {
 		nv_wo32(pgt, pte, 0x00000000);
@@ -197,24 +131,22 @@ nv41_sgdma_unbind(struct ttm_backend *be)
 	}
 
 	nv41_sgdma_flush(nvbe);
-	nvbe->bound = false;
 	return 0;
 }
 
 static struct ttm_backend_func nv41_sgdma_backend = {
-	.populate		= nouveau_sgdma_populate,
-	.clear			= nouveau_sgdma_clear,
 	.bind			= nv41_sgdma_bind,
 	.unbind			= nv41_sgdma_unbind,
 	.destroy		= nouveau_sgdma_destroy
 };
 
 static void
-nv44_sgdma_flush(struct nouveau_sgdma_be *nvbe)
+nv44_sgdma_flush(struct ttm_tt *ttm)
 {
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 	struct drm_device *dev = nvbe->dev;
 
-	nv_wr32(dev, 0x100814, (nvbe->nr_pages - 1) << 12);
+	nv_wr32(dev, 0x100814, (ttm->num_pages - 1) << 12);
 	nv_wr32(dev, 0x100808, nvbe->offset | 0x20);
 	if (!nv_wait(dev, 0x100808, 0x00000001, 0x00000001))
 		NV_ERROR(dev, "gart flush timeout: 0x%08x\n",
@@ -273,14 +205,14 @@ nv44_sgdma_fill(struct nouveau_gpuobj *pgt, dma_addr_t *list, u32 base, u32 cnt)
 }
 
 static int
-nv44_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
+nv44_sgdma_bind(struct ttm_tt *ttm, struct ttm_mem_reg *mem)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 	struct drm_nouveau_private *dev_priv = nvbe->dev->dev_private;
 	struct nouveau_gpuobj *pgt = dev_priv->gart_info.sg_ctxdma;
-	dma_addr_t *list = nvbe->pages;
+	dma_addr_t *list = nvbe->ttm.dma_address;
 	u32 pte = mem->start << 2, tmp[4];
-	u32 cnt = nvbe->nr_pages;
+	u32 cnt = ttm->num_pages;
 	int i;
 
 	nvbe->offset = mem->start << PAGE_SHIFT;
@@ -308,19 +240,18 @@ nv44_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
 	if (cnt)
 		nv44_sgdma_fill(pgt, list, pte, cnt);
 
-	nv44_sgdma_flush(nvbe);
-	nvbe->bound = true;
+	nv44_sgdma_flush(ttm);
 	return 0;
 }
 
 static int
-nv44_sgdma_unbind(struct ttm_backend *be)
+nv44_sgdma_unbind(struct ttm_tt *ttm)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 	struct drm_nouveau_private *dev_priv = nvbe->dev->dev_private;
 	struct nouveau_gpuobj *pgt = dev_priv->gart_info.sg_ctxdma;
 	u32 pte = (nvbe->offset >> 12) << 2;
-	u32 cnt = nvbe->nr_pages;
+	u32 cnt = ttm->num_pages;
 
 	if (pte & 0x0000000c) {
 		u32  max = 4 - ((pte >> 2) & 0x3);
@@ -342,55 +273,47 @@ nv44_sgdma_unbind(struct ttm_backend *be)
 	if (cnt)
 		nv44_sgdma_fill(pgt, NULL, pte, cnt);
 
-	nv44_sgdma_flush(nvbe);
-	nvbe->bound = false;
+	nv44_sgdma_flush(ttm);
 	return 0;
 }
 
 static struct ttm_backend_func nv44_sgdma_backend = {
-	.populate		= nouveau_sgdma_populate,
-	.clear			= nouveau_sgdma_clear,
 	.bind			= nv44_sgdma_bind,
 	.unbind			= nv44_sgdma_unbind,
 	.destroy		= nouveau_sgdma_destroy
 };
 
 static int
-nv50_sgdma_bind(struct ttm_backend *be, struct ttm_mem_reg *mem)
+nv50_sgdma_bind(struct ttm_tt *ttm, struct ttm_mem_reg *mem)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
+	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)ttm;
 	struct nouveau_mem *node = mem->mm_node;
+
 	/* noop: bound in move_notify() */
-	node->pages = nvbe->pages;
-	nvbe->pages = (dma_addr_t *)node;
-	nvbe->bound = true;
+	node->pages = nvbe->ttm.dma_address;
 	return 0;
 }
 
 static int
-nv50_sgdma_unbind(struct ttm_backend *be)
+nv50_sgdma_unbind(struct ttm_tt *ttm)
 {
-	struct nouveau_sgdma_be *nvbe = (struct nouveau_sgdma_be *)be;
-	struct nouveau_mem *node = (struct nouveau_mem *)nvbe->pages;
 	/* noop: unbound in move_notify() */
-	nvbe->pages = node->pages;
-	node->pages = NULL;
-	nvbe->bound = false;
 	return 0;
 }
 
 static struct ttm_backend_func nv50_sgdma_backend = {
-	.populate		= nouveau_sgdma_populate,
-	.clear			= nouveau_sgdma_clear,
 	.bind			= nv50_sgdma_bind,
 	.unbind			= nv50_sgdma_unbind,
 	.destroy		= nouveau_sgdma_destroy
 };
 
-struct ttm_backend *
-nouveau_sgdma_init_ttm(struct drm_device *dev)
+struct ttm_tt *
+nouveau_sgdma_create_ttm(struct ttm_bo_device *bdev,
+			 unsigned long size, uint32_t page_flags,
+			 struct page *dummy_read_page)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct drm_nouveau_private *dev_priv = nouveau_bdev(bdev);
+	struct drm_device *dev = dev_priv->dev;
 	struct nouveau_sgdma_be *nvbe;
 
 	nvbe = kzalloc(sizeof(*nvbe), GFP_KERNEL);
@@ -398,9 +321,13 @@ nouveau_sgdma_init_ttm(struct drm_device *dev)
 		return NULL;
 
 	nvbe->dev = dev;
+	nvbe->ttm.ttm.func = dev_priv->gart_info.func;
 
-	nvbe->backend.func = dev_priv->gart_info.func;
-	return &nvbe->backend;
+	if (ttm_dma_tt_init(&nvbe->ttm, bdev, size, page_flags, dummy_read_page)) {
+		kfree(nvbe);
+		return NULL;
+	}
+	return &nvbe->ttm.ttm;
 }
 
 int
diff --git a/drivers/gpu/drm/nouveau/nouveau_state.c b/drivers/gpu/drm/nouveau/nouveau_state.c
index d8831ab42bb9..f5e98910d17f 100644
--- a/drivers/gpu/drm/nouveau/nouveau_state.c
+++ b/drivers/gpu/drm/nouveau/nouveau_state.c
@@ -36,6 +36,7 @@
 #include "nouveau_drm.h"
 #include "nouveau_fbcon.h"
 #include "nouveau_ramht.h"
+#include "nouveau_gpio.h"
 #include "nouveau_pm.h"
 #include "nv50_display.h"
 
@@ -80,16 +81,12 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
-		engine->display.init		= nv04_display_init;
 		engine->display.destroy		= nv04_display_destroy;
-		engine->gpio.init		= nouveau_stub_init;
-		engine->gpio.takedown		= nouveau_stub_takedown;
-		engine->gpio.get		= NULL;
-		engine->gpio.set		= NULL;
-		engine->gpio.irq_enable		= NULL;
-		engine->pm.clock_get		= nv04_pm_clock_get;
-		engine->pm.clock_pre		= nv04_pm_clock_pre;
-		engine->pm.clock_set		= nv04_pm_clock_set;
+		engine->display.init		= nv04_display_init;
+		engine->display.fini		= nv04_display_fini;
+		engine->pm.clocks_get		= nv04_pm_clocks_get;
+		engine->pm.clocks_pre		= nv04_pm_clocks_pre;
+		engine->pm.clocks_set		= nv04_pm_clocks_set;
 		engine->vram.init		= nouveau_mem_detect;
 		engine->vram.takedown		= nouveau_stub_takedown;
 		engine->vram.flags_valid	= nouveau_mem_flags_valid;
@@ -129,16 +126,14 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
-		engine->display.init		= nv04_display_init;
 		engine->display.destroy		= nv04_display_destroy;
-		engine->gpio.init		= nouveau_stub_init;
-		engine->gpio.takedown		= nouveau_stub_takedown;
-		engine->gpio.get		= nv10_gpio_get;
-		engine->gpio.set		= nv10_gpio_set;
-		engine->gpio.irq_enable		= NULL;
-		engine->pm.clock_get		= nv04_pm_clock_get;
-		engine->pm.clock_pre		= nv04_pm_clock_pre;
-		engine->pm.clock_set		= nv04_pm_clock_set;
+		engine->display.init		= nv04_display_init;
+		engine->display.fini		= nv04_display_fini;
+		engine->gpio.drive		= nv10_gpio_drive;
+		engine->gpio.sense		= nv10_gpio_sense;
+		engine->pm.clocks_get		= nv04_pm_clocks_get;
+		engine->pm.clocks_pre		= nv04_pm_clocks_pre;
+		engine->pm.clocks_set		= nv04_pm_clocks_set;
 		engine->vram.init		= nouveau_mem_detect;
 		engine->vram.takedown		= nouveau_stub_takedown;
 		engine->vram.flags_valid	= nouveau_mem_flags_valid;
@@ -178,16 +173,14 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
-		engine->display.init		= nv04_display_init;
 		engine->display.destroy		= nv04_display_destroy;
-		engine->gpio.init		= nouveau_stub_init;
-		engine->gpio.takedown		= nouveau_stub_takedown;
-		engine->gpio.get		= nv10_gpio_get;
-		engine->gpio.set		= nv10_gpio_set;
-		engine->gpio.irq_enable		= NULL;
-		engine->pm.clock_get		= nv04_pm_clock_get;
-		engine->pm.clock_pre		= nv04_pm_clock_pre;
-		engine->pm.clock_set		= nv04_pm_clock_set;
+		engine->display.init		= nv04_display_init;
+		engine->display.fini		= nv04_display_fini;
+		engine->gpio.drive		= nv10_gpio_drive;
+		engine->gpio.sense		= nv10_gpio_sense;
+		engine->pm.clocks_get		= nv04_pm_clocks_get;
+		engine->pm.clocks_pre		= nv04_pm_clocks_pre;
+		engine->pm.clocks_set		= nv04_pm_clocks_set;
 		engine->vram.init		= nouveau_mem_detect;
 		engine->vram.takedown		= nouveau_stub_takedown;
 		engine->vram.flags_valid	= nouveau_mem_flags_valid;
@@ -227,16 +220,14 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
-		engine->display.init		= nv04_display_init;
 		engine->display.destroy		= nv04_display_destroy;
-		engine->gpio.init		= nouveau_stub_init;
-		engine->gpio.takedown		= nouveau_stub_takedown;
-		engine->gpio.get		= nv10_gpio_get;
-		engine->gpio.set		= nv10_gpio_set;
-		engine->gpio.irq_enable		= NULL;
-		engine->pm.clock_get		= nv04_pm_clock_get;
-		engine->pm.clock_pre		= nv04_pm_clock_pre;
-		engine->pm.clock_set		= nv04_pm_clock_set;
+		engine->display.init		= nv04_display_init;
+		engine->display.fini		= nv04_display_fini;
+		engine->gpio.drive		= nv10_gpio_drive;
+		engine->gpio.sense		= nv10_gpio_sense;
+		engine->pm.clocks_get		= nv04_pm_clocks_get;
+		engine->pm.clocks_pre		= nv04_pm_clocks_pre;
+		engine->pm.clocks_set		= nv04_pm_clocks_set;
 		engine->pm.voltage_get		= nouveau_voltage_gpio_get;
 		engine->pm.voltage_set		= nouveau_voltage_gpio_set;
 		engine->vram.init		= nouveau_mem_detect;
@@ -279,19 +270,22 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->display.early_init	= nv04_display_early_init;
 		engine->display.late_takedown	= nv04_display_late_takedown;
 		engine->display.create		= nv04_display_create;
-		engine->display.init		= nv04_display_init;
 		engine->display.destroy		= nv04_display_destroy;
-		engine->gpio.init		= nouveau_stub_init;
-		engine->gpio.takedown		= nouveau_stub_takedown;
-		engine->gpio.get		= nv10_gpio_get;
-		engine->gpio.set		= nv10_gpio_set;
-		engine->gpio.irq_enable		= NULL;
+		engine->display.init		= nv04_display_init;
+		engine->display.fini		= nv04_display_fini;
+		engine->gpio.init		= nv10_gpio_init;
+		engine->gpio.fini		= nv10_gpio_fini;
+		engine->gpio.drive		= nv10_gpio_drive;
+		engine->gpio.sense		= nv10_gpio_sense;
+		engine->gpio.irq_enable		= nv10_gpio_irq_enable;
 		engine->pm.clocks_get		= nv40_pm_clocks_get;
 		engine->pm.clocks_pre		= nv40_pm_clocks_pre;
 		engine->pm.clocks_set		= nv40_pm_clocks_set;
 		engine->pm.voltage_get		= nouveau_voltage_gpio_get;
 		engine->pm.voltage_set		= nouveau_voltage_gpio_set;
 		engine->pm.temp_get		= nv40_temp_get;
+		engine->pm.pwm_get		= nv40_pm_pwm_get;
+		engine->pm.pwm_set		= nv40_pm_pwm_set;
 		engine->vram.init		= nouveau_mem_detect;
 		engine->vram.takedown		= nouveau_stub_takedown;
 		engine->vram.flags_valid	= nouveau_mem_flags_valid;
@@ -334,14 +328,13 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->display.early_init	= nv50_display_early_init;
 		engine->display.late_takedown	= nv50_display_late_takedown;
 		engine->display.create		= nv50_display_create;
-		engine->display.init		= nv50_display_init;
 		engine->display.destroy		= nv50_display_destroy;
+		engine->display.init		= nv50_display_init;
+		engine->display.fini		= nv50_display_fini;
 		engine->gpio.init		= nv50_gpio_init;
-		engine->gpio.takedown		= nv50_gpio_fini;
-		engine->gpio.get		= nv50_gpio_get;
-		engine->gpio.set		= nv50_gpio_set;
-		engine->gpio.irq_register	= nv50_gpio_irq_register;
-		engine->gpio.irq_unregister	= nv50_gpio_irq_unregister;
+		engine->gpio.fini		= nv50_gpio_fini;
+		engine->gpio.drive		= nv50_gpio_drive;
+		engine->gpio.sense		= nv50_gpio_sense;
 		engine->gpio.irq_enable		= nv50_gpio_irq_enable;
 		switch (dev_priv->chipset) {
 		case 0x84:
@@ -354,9 +347,9 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		case 0xaa:
 		case 0xac:
 		case 0x50:
-			engine->pm.clock_get	= nv50_pm_clock_get;
-			engine->pm.clock_pre	= nv50_pm_clock_pre;
-			engine->pm.clock_set	= nv50_pm_clock_set;
+			engine->pm.clocks_get	= nv50_pm_clocks_get;
+			engine->pm.clocks_pre	= nv50_pm_clocks_pre;
+			engine->pm.clocks_set	= nv50_pm_clocks_set;
 			break;
 		default:
 			engine->pm.clocks_get	= nva3_pm_clocks_get;
@@ -370,6 +363,8 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 			engine->pm.temp_get	= nv84_temp_get;
 		else
 			engine->pm.temp_get	= nv40_temp_get;
+		engine->pm.pwm_get		= nv50_pm_pwm_get;
+		engine->pm.pwm_set		= nv50_pm_pwm_set;
 		engine->vram.init		= nv50_vram_init;
 		engine->vram.takedown		= nv50_vram_fini;
 		engine->vram.get		= nv50_vram_new;
@@ -407,14 +402,13 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->display.early_init	= nv50_display_early_init;
 		engine->display.late_takedown	= nv50_display_late_takedown;
 		engine->display.create		= nv50_display_create;
-		engine->display.init		= nv50_display_init;
 		engine->display.destroy		= nv50_display_destroy;
+		engine->display.init		= nv50_display_init;
+		engine->display.fini		= nv50_display_fini;
 		engine->gpio.init		= nv50_gpio_init;
-		engine->gpio.takedown		= nouveau_stub_takedown;
-		engine->gpio.get		= nv50_gpio_get;
-		engine->gpio.set		= nv50_gpio_set;
-		engine->gpio.irq_register	= nv50_gpio_irq_register;
-		engine->gpio.irq_unregister	= nv50_gpio_irq_unregister;
+		engine->gpio.fini		= nv50_gpio_fini;
+		engine->gpio.drive		= nv50_gpio_drive;
+		engine->gpio.sense		= nv50_gpio_sense;
 		engine->gpio.irq_enable		= nv50_gpio_irq_enable;
 		engine->vram.init		= nvc0_vram_init;
 		engine->vram.takedown		= nv50_vram_fini;
@@ -423,8 +417,12 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->vram.flags_valid	= nvc0_vram_flags_valid;
 		engine->pm.temp_get		= nv84_temp_get;
 		engine->pm.clocks_get		= nvc0_pm_clocks_get;
+		engine->pm.clocks_pre		= nvc0_pm_clocks_pre;
+		engine->pm.clocks_set		= nvc0_pm_clocks_set;
 		engine->pm.voltage_get		= nouveau_voltage_gpio_get;
 		engine->pm.voltage_set		= nouveau_voltage_gpio_set;
+		engine->pm.pwm_get		= nv50_pm_pwm_get;
+		engine->pm.pwm_set		= nv50_pm_pwm_set;
 		break;
 	case 0xd0:
 		engine->instmem.init		= nvc0_instmem_init;
@@ -457,21 +455,23 @@ static int nouveau_init_engine_ptrs(struct drm_device *dev)
 		engine->display.early_init	= nouveau_stub_init;
 		engine->display.late_takedown	= nouveau_stub_takedown;
 		engine->display.create		= nvd0_display_create;
-		engine->display.init		= nvd0_display_init;
 		engine->display.destroy		= nvd0_display_destroy;
+		engine->display.init		= nvd0_display_init;
+		engine->display.fini		= nvd0_display_fini;
 		engine->gpio.init		= nv50_gpio_init;
-		engine->gpio.takedown		= nouveau_stub_takedown;
-		engine->gpio.get		= nvd0_gpio_get;
-		engine->gpio.set		= nvd0_gpio_set;
-		engine->gpio.irq_register	= nv50_gpio_irq_register;
-		engine->gpio.irq_unregister	= nv50_gpio_irq_unregister;
+		engine->gpio.fini		= nv50_gpio_fini;
+		engine->gpio.drive		= nvd0_gpio_drive;
+		engine->gpio.sense		= nvd0_gpio_sense;
 		engine->gpio.irq_enable		= nv50_gpio_irq_enable;
 		engine->vram.init		= nvc0_vram_init;
 		engine->vram.takedown		= nv50_vram_fini;
 		engine->vram.get		= nvc0_vram_new;
 		engine->vram.put		= nv50_vram_del;
 		engine->vram.flags_valid	= nvc0_vram_flags_valid;
+		engine->pm.temp_get		= nv84_temp_get;
 		engine->pm.clocks_get		= nvc0_pm_clocks_get;
+		engine->pm.clocks_pre		= nvc0_pm_clocks_pre;
+		engine->pm.clocks_set		= nvc0_pm_clocks_set;
 		engine->pm.voltage_get		= nouveau_voltage_gpio_get;
 		engine->pm.voltage_set		= nouveau_voltage_gpio_set;
 		break;
@@ -615,7 +615,7 @@ nouveau_card_init(struct drm_device *dev)
 		goto out_gart;
 
 	/* PGPIO */
-	ret = engine->gpio.init(dev);
+	ret = nouveau_gpio_create(dev);
 	if (ret)
 		goto out_mc;
 
@@ -648,6 +648,7 @@ nouveau_card_init(struct drm_device *dev)
 			nv50_graph_create(dev);
 			break;
 		case NV_C0:
+		case NV_D0:
 			nvc0_graph_create(dev);
 			break;
 		default:
@@ -663,6 +664,11 @@ nouveau_card_init(struct drm_device *dev)
 		case 0xa0:
 			nv84_crypt_create(dev);
 			break;
+		case 0x98:
+		case 0xaa:
+		case 0xac:
+			nv98_crypt_create(dev);
+			break;
 		}
 
 		switch (dev_priv->card_type) {
@@ -684,15 +690,25 @@ nouveau_card_init(struct drm_device *dev)
 			break;
 		}
 
+		if (dev_priv->chipset >= 0xa3 || dev_priv->chipset == 0x98) {
+			nv84_bsp_create(dev);
+			nv84_vp_create(dev);
+			nv98_ppp_create(dev);
+		} else
+		if (dev_priv->chipset >= 0x84) {
+			nv50_mpeg_create(dev);
+			nv84_bsp_create(dev);
+			nv84_vp_create(dev);
+		} else
+		if (dev_priv->chipset >= 0x50) {
+			nv50_mpeg_create(dev);
+		} else
 		if (dev_priv->card_type == NV_40 ||
 		    dev_priv->chipset == 0x31 ||
 		    dev_priv->chipset == 0x34 ||
-		    dev_priv->chipset == 0x36)
+		    dev_priv->chipset == 0x36) {
 			nv31_mpeg_create(dev);
-		else
-		if (dev_priv->card_type == NV_50 &&
-		    (dev_priv->chipset < 0x98 || dev_priv->chipset == 0xa0))
-			nv50_mpeg_create(dev);
+		}
 
 		for (e = 0; e < NVOBJ_ENGINE_NR; e++) {
 			if (dev_priv->eng[e]) {
@@ -712,27 +728,7 @@ nouveau_card_init(struct drm_device *dev)
 	if (ret)
 		goto out_fifo;
 
-	/* initialise general modesetting */
-	drm_mode_config_init(dev);
-	drm_mode_create_scaling_mode_property(dev);
-	drm_mode_create_dithering_property(dev);
-	dev->mode_config.funcs = (void *)&nouveau_mode_config_funcs;
-	dev->mode_config.fb_base = pci_resource_start(dev->pdev, 1);
-	dev->mode_config.min_width = 0;
-	dev->mode_config.min_height = 0;
-	if (dev_priv->card_type < NV_10) {
-		dev->mode_config.max_width = 2048;
-		dev->mode_config.max_height = 2048;
-	} else
-	if (dev_priv->card_type < NV_50) {
-		dev->mode_config.max_width = 4096;
-		dev->mode_config.max_height = 4096;
-	} else {
-		dev->mode_config.max_width = 8192;
-		dev->mode_config.max_height = 8192;
-	}
-
-	ret = engine->display.create(dev);
+	ret = nouveau_display_create(dev);
 	if (ret)
 		goto out_irq;
 
@@ -752,12 +748,11 @@ nouveau_card_init(struct drm_device *dev)
 	}
 
 	if (dev->mode_config.num_crtc) {
-		ret = drm_vblank_init(dev, dev->mode_config.num_crtc);
+		ret = nouveau_display_init(dev);
 		if (ret)
 			goto out_chan;
 
 		nouveau_fbcon_init(dev);
-		drm_kms_helper_poll_init(dev);
 	}
 
 	return 0;
@@ -768,7 +763,7 @@ out_fence:
 	nouveau_fence_fini(dev);
 out_disp:
 	nouveau_backlight_exit(dev);
-	engine->display.destroy(dev);
+	nouveau_display_destroy(dev);
 out_irq:
 	nouveau_irq_fini(dev);
 out_fifo:
@@ -788,7 +783,7 @@ out_engine:
 out_timer:
 	engine->timer.takedown(dev);
 out_gpio:
-	engine->gpio.takedown(dev);
+	nouveau_gpio_destroy(dev);
 out_mc:
 	engine->mc.takedown(dev);
 out_gart:
@@ -818,9 +813,8 @@ static void nouveau_card_takedown(struct drm_device *dev)
 	int e;
 
 	if (dev->mode_config.num_crtc) {
-		drm_kms_helper_poll_fini(dev);
 		nouveau_fbcon_fini(dev);
-		drm_vblank_cleanup(dev);
+		nouveau_display_fini(dev);
 	}
 
 	if (dev_priv->channel) {
@@ -829,8 +823,7 @@ static void nouveau_card_takedown(struct drm_device *dev)
 	}
 
 	nouveau_backlight_exit(dev);
-	engine->display.destroy(dev);
-	drm_mode_config_cleanup(dev);
+	nouveau_display_destroy(dev);
 
 	if (!dev_priv->noaccel) {
 		engine->fifo.takedown(dev);
@@ -843,7 +836,7 @@ static void nouveau_card_takedown(struct drm_device *dev)
 	}
 	engine->fb.takedown(dev);
 	engine->timer.takedown(dev);
-	engine->gpio.takedown(dev);
+	nouveau_gpio_destroy(dev);
 	engine->mc.takedown(dev);
 	engine->display.late_takedown(dev);
 
@@ -1110,13 +1103,11 @@ int nouveau_load(struct drm_device *dev, unsigned long flags)
 	dev_priv->noaccel = !!nouveau_noaccel;
 	if (nouveau_noaccel == -1) {
 		switch (dev_priv->chipset) {
-#if 0
-		case 0xXX: /* known broken */
+		case 0xd9: /* known broken */
 			NV_INFO(dev, "acceleration disabled by default, pass "
 				     "noaccel=0 to force enable\n");
 			dev_priv->noaccel = true;
 			break;
-#endif
 		default:
 			dev_priv->noaccel = false;
 			break;
@@ -1238,7 +1229,7 @@ int nouveau_ioctl_getparam(struct drm_device *dev, void *data,
 		getparam->value = 1;
 		break;
 	case NOUVEAU_GETPARAM_HAS_PAGEFLIP:
-		getparam->value = dev_priv->card_type < NV_D0;
+		getparam->value = 1;
 		break;
 	case NOUVEAU_GETPARAM_GRAPH_UNITS:
 		/* NV40 and NV50 versions are quite different, but register
diff --git a/drivers/gpu/drm/nouveau/nouveau_temp.c b/drivers/gpu/drm/nouveau/nouveau_temp.c
index 5a46446dd5a8..0f5a30160556 100644
--- a/drivers/gpu/drm/nouveau/nouveau_temp.c
+++ b/drivers/gpu/drm/nouveau/nouveau_temp.c
@@ -55,6 +55,10 @@ nouveau_temp_vbios_parse(struct drm_device *dev, u8 *temp)
 	temps->down_clock = 100;
 	temps->fan_boost = 90;
 
+	/* Set the default range for the pwm fan */
+	pm->fan.min_duty = 30;
+	pm->fan.max_duty = 100;
+
 	/* Set the known default values to setup the temperature sensor */
 	if (dev_priv->card_type >= NV_40) {
 		switch (dev_priv->chipset) {
@@ -156,11 +160,26 @@ nouveau_temp_vbios_parse(struct drm_device *dev, u8 *temp)
 		case 0x13:
 			sensor->slope_div = value;
 			break;
+		case 0x22:
+			pm->fan.min_duty = value & 0xff;
+			pm->fan.max_duty = (value & 0xff00) >> 8;
+			break;
+		case 0x26:
+			pm->fan.pwm_freq = value;
+			break;
 		}
 		temp += recordlen;
 	}
 
 	nouveau_temp_safety_checks(dev);
+
+	/* check the fan min/max settings */
+	if (pm->fan.min_duty < 10)
+		pm->fan.min_duty = 10;
+	if (pm->fan.max_duty > 100)
+		pm->fan.max_duty = 100;
+	if (pm->fan.max_duty < pm->fan.min_duty)
+		pm->fan.max_duty = pm->fan.min_duty;
 }
 
 static int
@@ -267,8 +286,6 @@ probe_monitoring_device(struct nouveau_i2c_chan *i2c,
 static void
 nouveau_temp_probe_i2c(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct dcb_table *dcb = &dev_priv->vbios.dcb;
 	struct i2c_board_info info[] = {
 		{ I2C_BOARD_INFO("w83l785ts", 0x2d) },
 		{ I2C_BOARD_INFO("w83781d", 0x2d) },
@@ -277,11 +294,9 @@ nouveau_temp_probe_i2c(struct drm_device *dev)
 		{ I2C_BOARD_INFO("lm99", 0x4c) },
 		{ }
 	};
-	int idx = (dcb->version >= 0x40 ?
-		   dcb->i2c_default_indices & 0xf : 2);
 
 	nouveau_i2c_identify(dev, "monitoring device", info,
-			     probe_monitoring_device, idx);
+			     probe_monitoring_device, NV_I2C_DEFAULT(0));
 }
 
 void
@@ -297,9 +312,9 @@ nouveau_temp_init(struct drm_device *dev)
 			return;
 
 		if (P.version == 1)
-			temp = ROMPTR(bios, P.data[12]);
+			temp = ROMPTR(dev, P.data[12]);
 		else if (P.version == 2)
-			temp = ROMPTR(bios, P.data[16]);
+			temp = ROMPTR(dev, P.data[16]);
 		else
 			NV_WARN(dev, "unknown temp for BIT P %d\n", P.version);
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_vm.c b/drivers/gpu/drm/nouveau/nouveau_vm.c
index ef0832b29ad2..2bf6c0350b4b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_vm.c
@@ -78,9 +78,10 @@ nouveau_vm_map(struct nouveau_vma *vma, struct nouveau_mem *node)
 
 void
 nouveau_vm_map_sg(struct nouveau_vma *vma, u64 delta, u64 length,
-		  struct nouveau_mem *mem, dma_addr_t *list)
+		  struct nouveau_mem *mem)
 {
 	struct nouveau_vm *vm = vma->vm;
+	dma_addr_t *list = mem->pages;
 	int big = vma->node->type != vm->spg_shift;
 	u32 offset = vma->node->offset + (delta >> 12);
 	u32 bits = vma->node->type - 12;
diff --git a/drivers/gpu/drm/nouveau/nouveau_vm.h b/drivers/gpu/drm/nouveau/nouveau_vm.h
index 6ce995f7797e..4fb6e728734d 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_vm.h
@@ -89,7 +89,7 @@ void nouveau_vm_map_at(struct nouveau_vma *, u64 offset, struct nouveau_mem *);
 void nouveau_vm_unmap(struct nouveau_vma *);
 void nouveau_vm_unmap_at(struct nouveau_vma *, u64 offset, u64 length);
 void nouveau_vm_map_sg(struct nouveau_vma *, u64 offset, u64 length,
-		       struct nouveau_mem *, dma_addr_t *);
+		       struct nouveau_mem *);
 
 /* nv50_vm.c */
 void nv50_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 pde,
diff --git a/drivers/gpu/drm/nouveau/nouveau_volt.c b/drivers/gpu/drm/nouveau/nouveau_volt.c
index 86d03e15735d..b010cb997b34 100644
--- a/drivers/gpu/drm/nouveau/nouveau_volt.c
+++ b/drivers/gpu/drm/nouveau/nouveau_volt.c
@@ -26,6 +26,7 @@
 
 #include "nouveau_drv.h"
 #include "nouveau_pm.h"
+#include "nouveau_gpio.h"
 
 static const enum dcb_gpio_tag vidtag[] = { 0x04, 0x05, 0x06, 0x1a, 0x73 };
 static int nr_vidtag = sizeof(vidtag) / sizeof(vidtag[0]);
@@ -34,7 +35,6 @@ int
 nouveau_voltage_gpio_get(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *gpio = &dev_priv->engine.gpio;
 	struct nouveau_pm_voltage *volt = &dev_priv->engine.pm.voltage;
 	u8 vid = 0;
 	int i;
@@ -43,7 +43,7 @@ nouveau_voltage_gpio_get(struct drm_device *dev)
 		if (!(volt->vid_mask & (1 << i)))
 			continue;
 
-		vid |= gpio->get(dev, vidtag[i]) << i;
+		vid |= nouveau_gpio_func_get(dev, vidtag[i]) << i;
 	}
 
 	return nouveau_volt_lvl_lookup(dev, vid);
@@ -53,7 +53,6 @@ int
 nouveau_voltage_gpio_set(struct drm_device *dev, int voltage)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *gpio = &dev_priv->engine.gpio;
 	struct nouveau_pm_voltage *volt = &dev_priv->engine.pm.voltage;
 	int vid, i;
 
@@ -65,7 +64,7 @@ nouveau_voltage_gpio_set(struct drm_device *dev, int voltage)
 		if (!(volt->vid_mask & (1 << i)))
 			continue;
 
-		gpio->set(dev, vidtag[i], !!(vid & (1 << i)));
+		nouveau_gpio_func_set(dev, vidtag[i], !!(vid & (1 << i)));
 	}
 
 	return 0;
@@ -117,10 +116,10 @@ nouveau_volt_init(struct drm_device *dev)
 			return;
 
 		if (P.version == 1)
-			volt = ROMPTR(bios, P.data[16]);
+			volt = ROMPTR(dev, P.data[16]);
 		else
 		if (P.version == 2)
-			volt = ROMPTR(bios, P.data[12]);
+			volt = ROMPTR(dev, P.data[12]);
 		else {
 			NV_WARN(dev, "unknown volt for BIT P %d\n", P.version);
 		}
@@ -130,7 +129,7 @@ nouveau_volt_init(struct drm_device *dev)
 			return;
 		}
 
-		volt = ROMPTR(bios, bios->data[bios->offset + 0x98]);
+		volt = ROMPTR(dev, bios->data[bios->offset + 0x98]);
 	}
 
 	if (!volt) {
@@ -194,7 +193,7 @@ nouveau_volt_init(struct drm_device *dev)
 			return;
 		}
 
-		if (!nouveau_bios_gpio_entry(dev, vidtag[i])) {
+		if (!nouveau_gpio_func_valid(dev, vidtag[i])) {
 			NV_DEBUG(dev, "vid bit %d has no gpio tag\n", i);
 			return;
 		}
diff --git a/drivers/gpu/drm/nouveau/nv04_crtc.c b/drivers/gpu/drm/nouveau/nv04_crtc.c
index 5e45398a9e2d..728d07584d39 100644
--- a/drivers/gpu/drm/nouveau/nv04_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv04_crtc.c
@@ -364,7 +364,7 @@ nv_crtc_mode_set_vga(struct drm_crtc *crtc, struct drm_display_mode *mode)
 	regp->CRTC[NV_CIO_CR_VRE_INDEX] = 1 << 5 | XLATE(vertEnd, 0, NV_CIO_CR_VRE_3_0);
 	regp->CRTC[NV_CIO_CR_VDE_INDEX] = vertDisplay;
 	/* framebuffer can be larger than crtc scanout area. */
-	regp->CRTC[NV_CIO_CR_OFFSET_INDEX] = fb->pitch / 8;
+	regp->CRTC[NV_CIO_CR_OFFSET_INDEX] = fb->pitches[0] / 8;
 	regp->CRTC[NV_CIO_CR_ULINE_INDEX] = 0x00;
 	regp->CRTC[NV_CIO_CR_VBS_INDEX] = vertBlankStart;
 	regp->CRTC[NV_CIO_CR_VBE_INDEX] = vertBlankEnd;
@@ -377,9 +377,9 @@ nv_crtc_mode_set_vga(struct drm_crtc *crtc, struct drm_display_mode *mode)
 
 	/* framebuffer can be larger than crtc scanout area. */
 	regp->CRTC[NV_CIO_CRE_RPC0_INDEX] =
-		XLATE(fb->pitch / 8, 8, NV_CIO_CRE_RPC0_OFFSET_10_8);
+		XLATE(fb->pitches[0] / 8, 8, NV_CIO_CRE_RPC0_OFFSET_10_8);
 	regp->CRTC[NV_CIO_CRE_42] =
-		XLATE(fb->pitch / 8, 11, NV_CIO_CRE_42_OFFSET_11);
+		XLATE(fb->pitches[0] / 8, 11, NV_CIO_CRE_42_OFFSET_11);
 	regp->CRTC[NV_CIO_CRE_RPC1_INDEX] = mode->crtc_hdisplay < 1280 ?
 					    MASK(NV_CIO_CRE_RPC1_LARGE) : 0x00;
 	regp->CRTC[NV_CIO_CRE_LSR_INDEX] = XLATE(horizBlankEnd, 6, NV_CIO_CRE_LSR_HBE_6) |
@@ -835,18 +835,18 @@ nv04_crtc_do_mode_set_base(struct drm_crtc *crtc,
 	NVWriteRAMDAC(dev, nv_crtc->index, NV_PRAMDAC_GENERAL_CONTROL,
 		      regp->ramdac_gen_ctrl);
 
-	regp->CRTC[NV_CIO_CR_OFFSET_INDEX] = drm_fb->pitch >> 3;
+	regp->CRTC[NV_CIO_CR_OFFSET_INDEX] = drm_fb->pitches[0] >> 3;
 	regp->CRTC[NV_CIO_CRE_RPC0_INDEX] =
-		XLATE(drm_fb->pitch >> 3, 8, NV_CIO_CRE_RPC0_OFFSET_10_8);
+		XLATE(drm_fb->pitches[0] >> 3, 8, NV_CIO_CRE_RPC0_OFFSET_10_8);
 	regp->CRTC[NV_CIO_CRE_42] =
-		XLATE(drm_fb->pitch / 8, 11, NV_CIO_CRE_42_OFFSET_11);
+		XLATE(drm_fb->pitches[0] / 8, 11, NV_CIO_CRE_42_OFFSET_11);
 	crtc_wr_cio_state(crtc, regp, NV_CIO_CRE_RPC0_INDEX);
 	crtc_wr_cio_state(crtc, regp, NV_CIO_CR_OFFSET_INDEX);
 	crtc_wr_cio_state(crtc, regp, NV_CIO_CRE_42);
 
 	/* Update the framebuffer location. */
 	regp->fb_start = nv_crtc->fb.offset & ~3;
-	regp->fb_start += (y * drm_fb->pitch) + (x * drm_fb->bits_per_pixel / 8);
+	regp->fb_start += (y * drm_fb->pitches[0]) + (x * drm_fb->bits_per_pixel / 8);
 	nv_set_crtc_base(dev, nv_crtc->index, regp->fb_start);
 
 	/* Update the arbitration parameters. */
diff --git a/drivers/gpu/drm/nouveau/nv04_dac.c b/drivers/gpu/drm/nouveau/nv04_dac.c
index e000455e06d0..8300266ffaea 100644
--- a/drivers/gpu/drm/nouveau/nv04_dac.c
+++ b/drivers/gpu/drm/nouveau/nv04_dac.c
@@ -32,6 +32,7 @@
 #include "nouveau_connector.h"
 #include "nouveau_crtc.h"
 #include "nouveau_hw.h"
+#include "nouveau_gpio.h"
 #include "nvreg.h"
 
 int nv04_dac_output_offset(struct drm_encoder *encoder)
@@ -220,7 +221,6 @@ uint32_t nv17_dac_sample_load(struct drm_encoder *encoder)
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *gpio = &dev_priv->engine.gpio;
 	struct dcb_entry *dcb = nouveau_encoder(encoder)->dcb;
 	uint32_t sample, testval, regoffset = nv04_dac_output_offset(encoder);
 	uint32_t saved_powerctrl_2 = 0, saved_powerctrl_4 = 0, saved_routput,
@@ -252,11 +252,11 @@ uint32_t nv17_dac_sample_load(struct drm_encoder *encoder)
 		nvWriteMC(dev, NV_PBUS_POWERCTRL_4, saved_powerctrl_4 & 0xffffffcf);
 	}
 
-	saved_gpio1 = gpio->get(dev, DCB_GPIO_TVDAC1);
-	saved_gpio0 = gpio->get(dev, DCB_GPIO_TVDAC0);
+	saved_gpio1 = nouveau_gpio_func_get(dev, DCB_GPIO_TVDAC1);
+	saved_gpio0 = nouveau_gpio_func_get(dev, DCB_GPIO_TVDAC0);
 
-	gpio->set(dev, DCB_GPIO_TVDAC1, dcb->type == OUTPUT_TV);
-	gpio->set(dev, DCB_GPIO_TVDAC0, dcb->type == OUTPUT_TV);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC1, dcb->type == OUTPUT_TV);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC0, dcb->type == OUTPUT_TV);
 
 	msleep(4);
 
@@ -306,8 +306,8 @@ uint32_t nv17_dac_sample_load(struct drm_encoder *encoder)
 		nvWriteMC(dev, NV_PBUS_POWERCTRL_4, saved_powerctrl_4);
 	nvWriteMC(dev, NV_PBUS_POWERCTRL_2, saved_powerctrl_2);
 
-	gpio->set(dev, DCB_GPIO_TVDAC1, saved_gpio1);
-	gpio->set(dev, DCB_GPIO_TVDAC0, saved_gpio0);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC1, saved_gpio1);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC0, saved_gpio0);
 
 	return sample;
 }
diff --git a/drivers/gpu/drm/nouveau/nv04_dfp.c b/drivers/gpu/drm/nouveau/nv04_dfp.c
index 12098bf839c4..2258746016f8 100644
--- a/drivers/gpu/drm/nouveau/nv04_dfp.c
+++ b/drivers/gpu/drm/nouveau/nv04_dfp.c
@@ -289,6 +289,7 @@ static void nv04_dfp_mode_set(struct drm_encoder *encoder,
 	struct nouveau_connector *nv_connector = nouveau_crtc_connector_get(nv_crtc);
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct drm_display_mode *output_mode = &nv_encoder->mode;
+	struct drm_connector *connector = &nv_connector->base;
 	uint32_t mode_ratio, panel_ratio;
 
 	NV_DEBUG_KMS(dev, "Output mode on CRTC %d:\n", nv_crtc->index);
@@ -340,10 +341,15 @@ static void nv04_dfp_mode_set(struct drm_encoder *encoder,
 	    output_mode->clock > 165000)
 		regp->fp_control |= (2 << 24);
 	if (nv_encoder->dcb->type == OUTPUT_LVDS) {
-		bool duallink, dummy;
+		bool duallink = false, dummy;
+		if (nv_connector->edid &&
+		    nv_connector->type == DCB_CONNECTOR_LVDS_SPWG) {
+			duallink = (((u8 *)nv_connector->edid)[121] == 2);
+		} else {
+			nouveau_bios_parse_lvds_table(dev, output_mode->clock,
+						      &duallink, &dummy);
+		}
 
-		nouveau_bios_parse_lvds_table(dev, output_mode->clock,
-					      &duallink, &dummy);
 		if (duallink)
 			regp->fp_control |= (8 << 28);
 	} else
@@ -407,7 +413,9 @@ static void nv04_dfp_mode_set(struct drm_encoder *encoder,
 	}
 
 	/* Output property. */
-	if (nv_connector->use_dithering) {
+	if ((nv_connector->dithering_mode == DITHERING_MODE_ON) ||
+	    (nv_connector->dithering_mode == DITHERING_MODE_AUTO &&
+	     encoder->crtc->fb->depth > connector->display_info.bpc * 3)) {
 		if (dev_priv->chipset == 0x11)
 			regp->dither = savep->dither | 0x00010000;
 		else {
diff --git a/drivers/gpu/drm/nouveau/nv04_display.c b/drivers/gpu/drm/nouveau/nv04_display.c
index 6bd8518d7b2e..7047d37e8dab 100644
--- a/drivers/gpu/drm/nouveau/nv04_display.c
+++ b/drivers/gpu/drm/nouveau/nv04_display.c
@@ -243,6 +243,11 @@ nv04_display_init(struct drm_device *dev)
 	return 0;
 }
 
+void
+nv04_display_fini(struct drm_device *dev)
+{
+}
+
 static void
 nv04_vblank_crtc0_isr(struct drm_device *dev)
 {
diff --git a/drivers/gpu/drm/nouveau/nv04_pm.c b/drivers/gpu/drm/nouveau/nv04_pm.c
index 9ae92a87b8cc..6e7589918fa9 100644
--- a/drivers/gpu/drm/nouveau/nv04_pm.c
+++ b/drivers/gpu/drm/nouveau/nv04_pm.c
@@ -27,68 +27,111 @@
 #include "nouveau_hw.h"
 #include "nouveau_pm.h"
 
-struct nv04_pm_state {
+int
+nv04_pm_clocks_get(struct drm_device *dev, struct nouveau_pm_level *perflvl)
+{
+	int ret;
+
+	ret = nouveau_hw_get_clock(dev, PLL_CORE);
+	if (ret < 0)
+		return ret;
+	perflvl->core = ret;
+
+	ret = nouveau_hw_get_clock(dev, PLL_MEMORY);
+	if (ret < 0)
+		return ret;
+	perflvl->memory = ret;
+
+	return 0;
+}
+
+struct nv04_pm_clock {
 	struct pll_lims pll;
 	struct nouveau_pll_vals calc;
 };
 
-int
-nv04_pm_clock_get(struct drm_device *dev, u32 id)
+struct nv04_pm_state {
+	struct nv04_pm_clock core;
+	struct nv04_pm_clock memory;
+};
+
+static int
+calc_pll(struct drm_device *dev, u32 id, int khz, struct nv04_pm_clock *clk)
 {
-	return nouveau_hw_get_clock(dev, id);
+	int ret;
+
+	ret = get_pll_limits(dev, id, &clk->pll);
+	if (ret)
+		return ret;
+
+	ret = nouveau_calc_pll_mnp(dev, &clk->pll, khz, &clk->calc);
+	if (!ret)
+		return -EINVAL;
+
+	return 0;
 }
 
 void *
-nv04_pm_clock_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl,
-		  u32 id, int khz)
+nv04_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 {
-	struct nv04_pm_state *state;
+	struct nv04_pm_state *info;
 	int ret;
 
-	state = kzalloc(sizeof(*state), GFP_KERNEL);
-	if (!state)
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
 		return ERR_PTR(-ENOMEM);
 
-	ret = get_pll_limits(dev, id, &state->pll);
-	if (ret) {
-		kfree(state);
-		return (ret == -ENOENT) ? NULL : ERR_PTR(ret);
-	}
+	ret = calc_pll(dev, PLL_CORE, perflvl->core, &info->core);
+	if (ret)
+		goto error;
 
-	ret = nouveau_calc_pll_mnp(dev, &state->pll, khz, &state->calc);
-	if (!ret) {
-		kfree(state);
-		return ERR_PTR(-EINVAL);
+	if (perflvl->memory) {
+		ret = calc_pll(dev, PLL_MEMORY, perflvl->memory, &info->memory);
+		if (ret)
+			goto error;
 	}
 
-	return state;
+	return info;
+error:
+	kfree(info);
+	return ERR_PTR(ret);
 }
 
-void
-nv04_pm_clock_set(struct drm_device *dev, void *pre_state)
+static void
+prog_pll(struct drm_device *dev, struct nv04_pm_clock *clk)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_timer_engine *ptimer = &dev_priv->engine.timer;
-	struct nv04_pm_state *state = pre_state;
-	u32 reg = state->pll.reg;
+	u32 reg = clk->pll.reg;
 
 	/* thank the insane nouveau_hw_setpll() interface for this */
 	if (dev_priv->card_type >= NV_40)
 		reg += 4;
 
-	nouveau_hw_setpll(dev, reg, &state->calc);
+	nouveau_hw_setpll(dev, reg, &clk->calc);
+}
+
+int
+nv04_pm_clocks_set(struct drm_device *dev, void *pre_state)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nouveau_timer_engine *ptimer = &dev_priv->engine.timer;
+	struct nv04_pm_state *state = pre_state;
+
+	prog_pll(dev, &state->core);
 
-	if (dev_priv->card_type < NV_30 && reg == NV_PRAMDAC_MPLL_COEFF) {
-		if (dev_priv->card_type == NV_20)
-			nv_mask(dev, 0x1002c4, 0, 1 << 20);
+	if (state->memory.pll.reg) {
+		prog_pll(dev, &state->memory);
+		if (dev_priv->card_type < NV_30) {
+			if (dev_priv->card_type == NV_20)
+				nv_mask(dev, 0x1002c4, 0, 1 << 20);
 
-		/* Reset the DLLs */
-		nv_mask(dev, 0x1002c0, 0, 1 << 8);
+			/* Reset the DLLs */
+			nv_mask(dev, 0x1002c0, 0, 1 << 8);
+		}
 	}
 
-	if (reg == NV_PRAMDAC_NVPLL_COEFF)
-		ptimer->init(dev);
+	ptimer->init(dev);
 
 	kfree(state);
+	return 0;
 }
-
diff --git a/drivers/gpu/drm/nouveau/nv04_timer.c b/drivers/gpu/drm/nouveau/nv04_timer.c
index 263301b809dd..55c945290e52 100644
--- a/drivers/gpu/drm/nouveau/nv04_timer.c
+++ b/drivers/gpu/drm/nouveau/nv04_timer.c
@@ -2,6 +2,7 @@
 #include "drm.h"
 #include "nouveau_drv.h"
 #include "nouveau_drm.h"
+#include "nouveau_hw.h"
 
 int
 nv04_timer_init(struct drm_device *dev)
@@ -17,7 +18,7 @@ nv04_timer_init(struct drm_device *dev)
 
 	/* determine base clock for timer source */
 	if (dev_priv->chipset < 0x40) {
-		n = dev_priv->engine.pm.clock_get(dev, PLL_CORE);
+		n = nouveau_hw_get_clock(dev, PLL_CORE);
 	} else
 	if (dev_priv->chipset == 0x40) {
 		/*XXX: figure this out */
diff --git a/drivers/gpu/drm/nouveau/nv10_gpio.c b/drivers/gpu/drm/nouveau/nv10_gpio.c
index 007fc29e2f86..550ad3fcf0af 100644
--- a/drivers/gpu/drm/nouveau/nv10_gpio.c
+++ b/drivers/gpu/drm/nouveau/nv10_gpio.c
@@ -27,66 +27,97 @@
 #include "drmP.h"
 #include "nouveau_drv.h"
 #include "nouveau_hw.h"
+#include "nouveau_gpio.h"
 
-static bool
-get_gpio_location(struct dcb_gpio_entry *ent, uint32_t *reg, uint32_t *shift,
-		  uint32_t *mask)
+int
+nv10_gpio_sense(struct drm_device *dev, int line)
 {
-	if (ent->line < 2) {
-		*reg = NV_PCRTC_GPIO;
-		*shift = ent->line * 16;
-		*mask = 0x11;
-
-	} else if (ent->line < 10) {
-		*reg = NV_PCRTC_GPIO_EXT;
-		*shift = (ent->line - 2) * 4;
-		*mask = 0x3;
+	if (line < 2) {
+		line = line * 16;
+		line = NVReadCRTC(dev, 0, NV_PCRTC_GPIO) >> line;
+		return !!(line & 0x0100);
+	} else
+	if (line < 10) {
+		line = (line - 2) * 4;
+		line = NVReadCRTC(dev, 0, NV_PCRTC_GPIO_EXT) >> line;
+		return !!(line & 0x04);
+	} else
+	if (line < 14) {
+		line = (line - 10) * 4;
+		line = NVReadCRTC(dev, 0, NV_PCRTC_850) >> line;
+		return !!(line & 0x04);
+	}
 
-	} else if (ent->line < 14) {
-		*reg = NV_PCRTC_850;
-		*shift = (ent->line - 10) * 4;
-		*mask = 0x3;
+	return -EINVAL;
+}
 
+int
+nv10_gpio_drive(struct drm_device *dev, int line, int dir, int out)
+{
+	u32 reg, mask, data;
+
+	if (line < 2) {
+		line = line * 16;
+		reg  = NV_PCRTC_GPIO;
+		mask = 0x00000011;
+		data = (dir << 4) | out;
+	} else
+	if (line < 10) {
+		line = (line - 2) * 4;
+		reg  = NV_PCRTC_GPIO_EXT;
+		mask = 0x00000003 << ((line - 2) * 4);
+		data = (dir << 1) | out;
+	} else
+	if (line < 14) {
+		line = (line - 10) * 4;
+		reg  = NV_PCRTC_850;
+		mask = 0x00000003;
+		data = (dir << 1) | out;
 	} else {
-		return false;
+		return -EINVAL;
 	}
 
-	return true;
+	mask = NVReadCRTC(dev, 0, reg) & ~(mask << line);
+	NVWriteCRTC(dev, 0, reg, mask | (data << line));
+	return 0;
 }
 
-int
-nv10_gpio_get(struct drm_device *dev, enum dcb_gpio_tag tag)
+void
+nv10_gpio_irq_enable(struct drm_device *dev, int line, bool on)
 {
-	struct dcb_gpio_entry *ent = nouveau_bios_gpio_entry(dev, tag);
-	uint32_t reg, shift, mask, value;
+	u32 mask = 0x00010001 << line;
 
-	if (!ent)
-		return -ENODEV;
+	nv_wr32(dev, 0x001104, mask);
+	nv_mask(dev, 0x001144, mask, on ? mask : 0);
+}
 
-	if (!get_gpio_location(ent, &reg, &shift, &mask))
-		return -ENODEV;
+static void
+nv10_gpio_isr(struct drm_device *dev)
+{
+	u32 intr = nv_rd32(dev, 0x1104);
+	u32 hi = (intr & 0x0000ffff) >> 0;
+	u32 lo = (intr & 0xffff0000) >> 16;
 
-	value = NVReadCRTC(dev, 0, reg) >> shift;
+	nouveau_gpio_isr(dev, 0, hi | lo);
 
-	return (ent->invert ? 1 : 0) ^ (value & 1);
+	nv_wr32(dev, 0x001104, intr);
 }
 
 int
-nv10_gpio_set(struct drm_device *dev, enum dcb_gpio_tag tag, int state)
+nv10_gpio_init(struct drm_device *dev)
 {
-	struct dcb_gpio_entry *ent = nouveau_bios_gpio_entry(dev, tag);
-	uint32_t reg, shift, mask, value;
-
-	if (!ent)
-		return -ENODEV;
-
-	if (!get_gpio_location(ent, &reg, &shift, &mask))
-		return -ENODEV;
-
-	value = ((ent->invert ? 1 : 0) ^ (state ? 1 : 0)) << shift;
-	mask = ~(mask << shift);
-
-	NVWriteCRTC(dev, 0, reg, value | (NVReadCRTC(dev, 0, reg) & mask));
-
+	nv_wr32(dev, 0x001140, 0x00000000);
+	nv_wr32(dev, 0x001100, 0xffffffff);
+	nv_wr32(dev, 0x001144, 0x00000000);
+	nv_wr32(dev, 0x001104, 0xffffffff);
+	nouveau_irq_register(dev, 28, nv10_gpio_isr); /* PBUS */
 	return 0;
 }
+
+void
+nv10_gpio_fini(struct drm_device *dev)
+{
+	nv_wr32(dev, 0x001140, 0x00000000);
+	nv_wr32(dev, 0x001144, 0x00000000);
+	nouveau_irq_unregister(dev, 28);
+}
diff --git a/drivers/gpu/drm/nouveau/nv17_tv.c b/drivers/gpu/drm/nouveau/nv17_tv.c
index 3900cebba560..696d7e7dc2a0 100644
--- a/drivers/gpu/drm/nouveau/nv17_tv.c
+++ b/drivers/gpu/drm/nouveau/nv17_tv.c
@@ -30,6 +30,7 @@
 #include "nouveau_encoder.h"
 #include "nouveau_connector.h"
 #include "nouveau_crtc.h"
+#include "nouveau_gpio.h"
 #include "nouveau_hw.h"
 #include "nv17_tv.h"
 
@@ -37,7 +38,6 @@ static uint32_t nv42_tv_sample_load(struct drm_encoder *encoder)
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *gpio = &dev_priv->engine.gpio;
 	uint32_t testval, regoffset = nv04_dac_output_offset(encoder);
 	uint32_t gpio0, gpio1, fp_htotal, fp_hsync_start, fp_hsync_end,
 		fp_control, test_ctrl, dacclk, ctv_14, ctv_1c, ctv_6c;
@@ -53,8 +53,8 @@ static uint32_t nv42_tv_sample_load(struct drm_encoder *encoder)
 	head = (dacclk & 0x100) >> 8;
 
 	/* Save the previous state. */
-	gpio1 = gpio->get(dev, DCB_GPIO_TVDAC1);
-	gpio0 = gpio->get(dev, DCB_GPIO_TVDAC0);
+	gpio1 = nouveau_gpio_func_get(dev, DCB_GPIO_TVDAC1);
+	gpio0 = nouveau_gpio_func_get(dev, DCB_GPIO_TVDAC0);
 	fp_htotal = NVReadRAMDAC(dev, head, NV_PRAMDAC_FP_HTOTAL);
 	fp_hsync_start = NVReadRAMDAC(dev, head, NV_PRAMDAC_FP_HSYNC_START);
 	fp_hsync_end = NVReadRAMDAC(dev, head, NV_PRAMDAC_FP_HSYNC_END);
@@ -65,8 +65,8 @@ static uint32_t nv42_tv_sample_load(struct drm_encoder *encoder)
 	ctv_6c = NVReadRAMDAC(dev, head, 0x680c6c);
 
 	/* Prepare the DAC for load detection.  */
-	gpio->set(dev, DCB_GPIO_TVDAC1, true);
-	gpio->set(dev, DCB_GPIO_TVDAC0, true);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC1, true);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC0, true);
 
 	NVWriteRAMDAC(dev, head, NV_PRAMDAC_FP_HTOTAL, 1343);
 	NVWriteRAMDAC(dev, head, NV_PRAMDAC_FP_HSYNC_START, 1047);
@@ -111,8 +111,8 @@ static uint32_t nv42_tv_sample_load(struct drm_encoder *encoder)
 	NVWriteRAMDAC(dev, head, NV_PRAMDAC_FP_HSYNC_END, fp_hsync_end);
 	NVWriteRAMDAC(dev, head, NV_PRAMDAC_FP_HSYNC_START, fp_hsync_start);
 	NVWriteRAMDAC(dev, head, NV_PRAMDAC_FP_HTOTAL, fp_htotal);
-	gpio->set(dev, DCB_GPIO_TVDAC1, gpio1);
-	gpio->set(dev, DCB_GPIO_TVDAC0, gpio0);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC1, gpio1);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC0, gpio0);
 
 	return sample;
 }
@@ -357,8 +357,6 @@ static bool nv17_tv_mode_fixup(struct drm_encoder *encoder,
 static void  nv17_tv_dpms(struct drm_encoder *encoder, int mode)
 {
 	struct drm_device *dev = encoder->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *gpio = &dev_priv->engine.gpio;
 	struct nv17_tv_state *regs = &to_tv_enc(encoder)->state;
 	struct nv17_tv_norm_params *tv_norm = get_tv_norm(encoder);
 
@@ -383,8 +381,8 @@ static void  nv17_tv_dpms(struct drm_encoder *encoder, int mode)
 
 	nv_load_ptv(dev, regs, 200);
 
-	gpio->set(dev, DCB_GPIO_TVDAC1, mode == DRM_MODE_DPMS_ON);
-	gpio->set(dev, DCB_GPIO_TVDAC0, mode == DRM_MODE_DPMS_ON);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC1, mode == DRM_MODE_DPMS_ON);
+	nouveau_gpio_func_set(dev, DCB_GPIO_TVDAC0, mode == DRM_MODE_DPMS_ON);
 
 	nv04_dac_update_dacclk(encoder, mode == DRM_MODE_DPMS_ON);
 }
diff --git a/drivers/gpu/drm/nouveau/nv40_pm.c b/drivers/gpu/drm/nouveau/nv40_pm.c
index e676b0d53478..c7615381c5d9 100644
--- a/drivers/gpu/drm/nouveau/nv40_pm.c
+++ b/drivers/gpu/drm/nouveau/nv40_pm.c
@@ -222,7 +222,7 @@ nv40_pm_gr_idle(void *data)
 	return true;
 }
 
-void
+int
 nv40_pm_clocks_set(struct drm_device *dev, void *pre_state)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
@@ -231,7 +231,7 @@ nv40_pm_clocks_set(struct drm_device *dev, void *pre_state)
 	struct bit_entry M;
 	u32 crtc_mask = 0;
 	u8 sr1[2];
-	int i;
+	int i, ret = -EAGAIN;
 
 	/* determine which CRTCs are active, fetch VGA_SR1 for each */
 	for (i = 0; i < 2; i++) {
@@ -263,6 +263,8 @@ nv40_pm_clocks_set(struct drm_device *dev, void *pre_state)
 	if (!nv_wait_cb(dev, nv40_pm_gr_idle, dev))
 		goto resume;
 
+	ret = 0;
+
 	/* set engine clocks */
 	nv_mask(dev, 0x00c040, 0x00000333, 0x00000000);
 	nv_wr32(dev, 0x004004, info->npll_coef);
@@ -345,4 +347,48 @@ resume:
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 
 	kfree(info);
+	return ret;
+}
+
+int
+nv40_pm_pwm_get(struct drm_device *dev, int line, u32 *divs, u32 *duty)
+{
+	if (line == 2) {
+		u32 reg = nv_rd32(dev, 0x0010f0);
+		if (reg & 0x80000000) {
+			*duty = (reg & 0x7fff0000) >> 16;
+			*divs = (reg & 0x00007fff);
+			return 0;
+		}
+	} else
+	if (line == 9) {
+		u32 reg = nv_rd32(dev, 0x0015f4);
+		if (reg & 0x80000000) {
+			*divs = nv_rd32(dev, 0x0015f8);
+			*duty = (reg & 0x7fffffff);
+			return 0;
+		}
+	} else {
+		NV_ERROR(dev, "unknown pwm ctrl for gpio %d\n", line);
+		return -ENODEV;
+	}
+
+	return -EINVAL;
+}
+
+int
+nv40_pm_pwm_set(struct drm_device *dev, int line, u32 divs, u32 duty)
+{
+	if (line == 2) {
+		nv_wr32(dev, 0x0010f0, 0x80000000 | (duty << 16) | divs);
+	} else
+	if (line == 9) {
+		nv_wr32(dev, 0x0015f8, divs);
+		nv_wr32(dev, 0x0015f4, duty | 0x80000000);
+	} else {
+		NV_ERROR(dev, "unknown pwm ctrl for gpio %d\n", line);
+		return -ENODEV;
+	}
+
+	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_crtc.c b/drivers/gpu/drm/nouveau/nv50_crtc.c
index 882080e0b4f5..8f6c2ace3adf 100644
--- a/drivers/gpu/drm/nouveau/nv50_crtc.c
+++ b/drivers/gpu/drm/nouveau/nv50_crtc.c
@@ -132,33 +132,42 @@ nv50_crtc_blank(struct nouveau_crtc *nv_crtc, bool blanked)
 }
 
 static int
-nv50_crtc_set_dither(struct nouveau_crtc *nv_crtc, bool on, bool update)
+nv50_crtc_set_dither(struct nouveau_crtc *nv_crtc, bool update)
 {
-	struct drm_device *dev = nv_crtc->base.dev;
-	struct nouveau_channel *evo = nv50_display(dev)->master;
-	int ret;
-
-	NV_DEBUG_KMS(dev, "\n");
+	struct nouveau_channel *evo = nv50_display(nv_crtc->base.dev)->master;
+	struct nouveau_connector *nv_connector;
+	struct drm_connector *connector;
+	int head = nv_crtc->index, ret;
+	u32 mode = 0x00;
 
-	ret = RING_SPACE(evo, 2 + (update ? 2 : 0));
-	if (ret) {
-		NV_ERROR(dev, "no space while setting dither\n");
-		return ret;
+	nv_connector = nouveau_crtc_connector_get(nv_crtc);
+	connector = &nv_connector->base;
+	if (nv_connector->dithering_mode == DITHERING_MODE_AUTO) {
+		if (nv_crtc->base.fb->depth > connector->display_info.bpc * 3)
+			mode = DITHERING_MODE_DYNAMIC2X2;
+	} else {
+		mode = nv_connector->dithering_mode;
 	}
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, DITHER_CTRL), 1);
-	if (on)
-		OUT_RING(evo, NV50_EVO_CRTC_DITHER_CTRL_ON);
-	else
-		OUT_RING(evo, NV50_EVO_CRTC_DITHER_CTRL_OFF);
+	if (nv_connector->dithering_depth == DITHERING_DEPTH_AUTO) {
+		if (connector->display_info.bpc >= 8)
+			mode |= DITHERING_DEPTH_8BPC;
+	} else {
+		mode |= nv_connector->dithering_depth;
+	}
 
-	if (update) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
-		OUT_RING(evo, 0);
-		FIRE_RING(evo);
+	ret = RING_SPACE(evo, 2 + (update ? 2 : 0));
+	if (ret == 0) {
+		BEGIN_RING(evo, 0, NV50_EVO_CRTC(head, DITHER_CTRL), 1);
+		OUT_RING  (evo, mode);
+		if (update) {
+			BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
+			OUT_RING  (evo, 0);
+			FIRE_RING (evo);
+		}
 	}
 
-	return 0;
+	return ret;
 }
 
 struct nouveau_connector *
@@ -180,80 +189,103 @@ nouveau_crtc_connector_get(struct nouveau_crtc *nv_crtc)
 }
 
 static int
-nv50_crtc_set_scale(struct nouveau_crtc *nv_crtc, int scaling_mode, bool update)
+nv50_crtc_set_scale(struct nouveau_crtc *nv_crtc, bool update)
 {
-	struct nouveau_connector *nv_connector =
-		nouveau_crtc_connector_get(nv_crtc);
-	struct drm_device *dev = nv_crtc->base.dev;
+	struct nouveau_connector *nv_connector;
+	struct drm_crtc *crtc = &nv_crtc->base;
+	struct drm_device *dev = crtc->dev;
 	struct nouveau_channel *evo = nv50_display(dev)->master;
-	struct drm_display_mode *native_mode = NULL;
-	struct drm_display_mode *mode = &nv_crtc->base.mode;
-	uint32_t outX, outY, horiz, vert;
-	int ret;
+	struct drm_display_mode *umode = &crtc->mode;
+	struct drm_display_mode *omode;
+	int scaling_mode, ret;
+	u32 ctrl = 0, oX, oY;
 
 	NV_DEBUG_KMS(dev, "\n");
 
-	switch (scaling_mode) {
-	case DRM_MODE_SCALE_NONE:
-		break;
-	default:
-		if (!nv_connector || !nv_connector->native_mode) {
-			NV_ERROR(dev, "No native mode, forcing panel scaling\n");
-			scaling_mode = DRM_MODE_SCALE_NONE;
+	nv_connector = nouveau_crtc_connector_get(nv_crtc);
+	if (!nv_connector || !nv_connector->native_mode) {
+		NV_ERROR(dev, "no native mode, forcing panel scaling\n");
+		scaling_mode = DRM_MODE_SCALE_NONE;
+	} else {
+		scaling_mode = nv_connector->scaling_mode;
+	}
+
+	/* start off at the resolution we programmed the crtc for, this
+	 * effectively handles NONE/FULL scaling
+	 */
+	if (scaling_mode != DRM_MODE_SCALE_NONE)
+		omode = nv_connector->native_mode;
+	else
+		omode = umode;
+
+	oX = omode->hdisplay;
+	oY = omode->vdisplay;
+	if (omode->flags & DRM_MODE_FLAG_DBLSCAN)
+		oY *= 2;
+
+	/* add overscan compensation if necessary, will keep the aspect
+	 * ratio the same as the backend mode unless overridden by the
+	 * user setting both hborder and vborder properties.
+	 */
+	if (nv_connector && ( nv_connector->underscan == UNDERSCAN_ON ||
+			     (nv_connector->underscan == UNDERSCAN_AUTO &&
+			      nv_connector->edid &&
+			      drm_detect_hdmi_monitor(nv_connector->edid)))) {
+		u32 bX = nv_connector->underscan_hborder;
+		u32 bY = nv_connector->underscan_vborder;
+		u32 aspect = (oY << 19) / oX;
+
+		if (bX) {
+			oX -= (bX * 2);
+			if (bY) oY -= (bY * 2);
+			else    oY  = ((oX * aspect) + (aspect / 2)) >> 19;
 		} else {
-			native_mode = nv_connector->native_mode;
+			oX -= (oX >> 4) + 32;
+			if (bY) oY -= (bY * 2);
+			else    oY  = ((oX * aspect) + (aspect / 2)) >> 19;
 		}
-		break;
 	}
 
+	/* handle CENTER/ASPECT scaling, taking into account the areas
+	 * removed already for overscan compensation
+	 */
 	switch (scaling_mode) {
+	case DRM_MODE_SCALE_CENTER:
+		oX = min((u32)umode->hdisplay, oX);
+		oY = min((u32)umode->vdisplay, oY);
+		/* fall-through */
 	case DRM_MODE_SCALE_ASPECT:
-		horiz = (native_mode->hdisplay << 19) / mode->hdisplay;
-		vert = (native_mode->vdisplay << 19) / mode->vdisplay;
-
-		if (vert > horiz) {
-			outX = (mode->hdisplay * horiz) >> 19;
-			outY = (mode->vdisplay * horiz) >> 19;
+		if (oY < oX) {
+			u32 aspect = (umode->hdisplay << 19) / umode->vdisplay;
+			oX = ((oY * aspect) + (aspect / 2)) >> 19;
 		} else {
-			outX = (mode->hdisplay * vert) >> 19;
-			outY = (mode->vdisplay * vert) >> 19;
+			u32 aspect = (umode->vdisplay << 19) / umode->hdisplay;
+			oY = ((oX * aspect) + (aspect / 2)) >> 19;
 		}
 		break;
-	case DRM_MODE_SCALE_FULLSCREEN:
-		outX = native_mode->hdisplay;
-		outY = native_mode->vdisplay;
-		break;
-	case DRM_MODE_SCALE_CENTER:
-	case DRM_MODE_SCALE_NONE:
 	default:
-		outX = mode->hdisplay;
-		outY = mode->vdisplay;
 		break;
 	}
 
-	ret = RING_SPACE(evo, update ? 7 : 5);
+	if (umode->hdisplay != oX || umode->vdisplay != oY ||
+	    umode->flags & DRM_MODE_FLAG_INTERLACE ||
+	    umode->flags & DRM_MODE_FLAG_DBLSCAN)
+		ctrl |= NV50_EVO_CRTC_SCALE_CTRL_ACTIVE;
+
+	ret = RING_SPACE(evo, 5);
 	if (ret)
 		return ret;
 
-	/* Got a better name for SCALER_ACTIVE? */
-	/* One day i've got to really figure out why this is needed. */
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_CTRL), 1);
-	if ((mode->flags & DRM_MODE_FLAG_DBLSCAN) ||
-	    (mode->flags & DRM_MODE_FLAG_INTERLACE) ||
-	    mode->hdisplay != outX || mode->vdisplay != outY) {
-		OUT_RING(evo, NV50_EVO_CRTC_SCALE_CTRL_ACTIVE);
-	} else {
-		OUT_RING(evo, NV50_EVO_CRTC_SCALE_CTRL_INACTIVE);
-	}
-
+	OUT_RING  (evo, ctrl);
 	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_RES1), 2);
-	OUT_RING(evo, outY << 16 | outX);
-	OUT_RING(evo, outY << 16 | outX);
+	OUT_RING  (evo, oY << 16 | oX);
+	OUT_RING  (evo, oY << 16 | oX);
 
 	if (update) {
-		BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
-		OUT_RING(evo, 0);
-		FIRE_RING(evo);
+		nv50_display_flip_stop(crtc);
+		nv50_display_sync(dev);
+		nv50_display_flip_next(crtc, crtc->fb, NULL);
 	}
 
 	return 0;
@@ -333,7 +365,6 @@ nv50_crtc_destroy(struct drm_crtc *crtc)
 	nouveau_bo_ref(NULL, &nv_crtc->lut.nvbo);
 	nouveau_bo_unmap(nv_crtc->cursor.nvbo);
 	nouveau_bo_ref(NULL, &nv_crtc->cursor.nvbo);
-	kfree(nv_crtc->mode);
 	kfree(nv_crtc);
 }
 
@@ -441,39 +472,6 @@ nv50_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
 }
 
-static int
-nv50_crtc_wait_complete(struct drm_crtc *crtc)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_timer_engine *ptimer = &dev_priv->engine.timer;
-	struct nv50_display *disp = nv50_display(dev);
-	struct nouveau_channel *evo = disp->master;
-	u64 start;
-	int ret;
-
-	ret = RING_SPACE(evo, 6);
-	if (ret)
-		return ret;
-	BEGIN_RING(evo, 0, 0x0084, 1);
-	OUT_RING  (evo, 0x80000000);
-	BEGIN_RING(evo, 0, 0x0080, 1);
-	OUT_RING  (evo, 0);
-	BEGIN_RING(evo, 0, 0x0084, 1);
-	OUT_RING  (evo, 0x00000000);
-
-	nv_wo32(disp->ntfy, 0x000, 0x00000000);
-	FIRE_RING (evo);
-
-	start = ptimer->read(dev);
-	do {
-		if (nv_ro32(disp->ntfy, 0x000))
-			return 0;
-	} while (ptimer->read(dev) - start < 2000000000ULL);
-
-	return -EBUSY;
-}
-
 static void
 nv50_crtc_prepare(struct drm_crtc *crtc)
 {
@@ -497,7 +495,7 @@ nv50_crtc_commit(struct drm_crtc *crtc)
 
 	nv50_crtc_blank(nv_crtc, false);
 	drm_vblank_post_modeset(dev, nv_crtc->index);
-	nv50_crtc_wait_complete(crtc);
+	nv50_display_sync(dev);
 	nv50_display_flip_next(crtc, crtc->fb, NULL);
 }
 
@@ -593,90 +591,76 @@ nv50_crtc_do_mode_set_base(struct drm_crtc *crtc,
 }
 
 static int
-nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
-		   struct drm_display_mode *adjusted_mode, int x, int y,
+nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *umode,
+		   struct drm_display_mode *mode, int x, int y,
 		   struct drm_framebuffer *old_fb)
 {
 	struct drm_device *dev = crtc->dev;
 	struct nouveau_channel *evo = nv50_display(dev)->master;
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
-	struct nouveau_connector *nv_connector = NULL;
-	uint32_t hsync_dur,  vsync_dur, hsync_start_to_end, vsync_start_to_end;
-	uint32_t hunk1, vunk1, vunk2a, vunk2b;
+	u32 head = nv_crtc->index * 0x400;
+	u32 ilace = (mode->flags & DRM_MODE_FLAG_INTERLACE) ? 2 : 1;
+	u32 vscan = (mode->flags & DRM_MODE_FLAG_DBLSCAN) ? 2 : 1;
+	u32 hactive, hsynce, hbackp, hfrontp, hblanke, hblanks;
+	u32 vactive, vsynce, vbackp, vfrontp, vblanke, vblanks;
+	u32 vblan2e = 0, vblan2s = 1;
 	int ret;
 
-	/* Find the connector attached to this CRTC */
-	nv_connector = nouveau_crtc_connector_get(nv_crtc);
-
-	*nv_crtc->mode = *adjusted_mode;
-
-	NV_DEBUG_KMS(dev, "index %d\n", nv_crtc->index);
+	/* hw timing description looks like this:
+	 *
+	 * <sync> <back porch> <---------display---------> <front porch>
+	 * ______
+	 *       |____________|---------------------------|____________|
+	 *
+	 *       ^ synce      ^ blanke                    ^ blanks     ^ active
+	 *
+	 * interlaced modes also have 2 additional values pointing at the end
+	 * and start of the next field's blanking period.
+	 */
 
-	hsync_dur = adjusted_mode->hsync_end - adjusted_mode->hsync_start;
-	vsync_dur = adjusted_mode->vsync_end - adjusted_mode->vsync_start;
-	hsync_start_to_end = adjusted_mode->htotal - adjusted_mode->hsync_start;
-	vsync_start_to_end = adjusted_mode->vtotal - adjusted_mode->vsync_start;
-	/* I can't give this a proper name, anyone else can? */
-	hunk1 = adjusted_mode->htotal -
-		adjusted_mode->hsync_start + adjusted_mode->hdisplay;
-	vunk1 = adjusted_mode->vtotal -
-		adjusted_mode->vsync_start + adjusted_mode->vdisplay;
-	/* Another strange value, this time only for interlaced adjusted_modes. */
-	vunk2a = 2 * adjusted_mode->vtotal -
-		 adjusted_mode->vsync_start + adjusted_mode->vdisplay;
-	vunk2b = adjusted_mode->vtotal -
-		 adjusted_mode->vsync_start + adjusted_mode->vtotal;
-
-	if (adjusted_mode->flags & DRM_MODE_FLAG_INTERLACE) {
-		vsync_dur /= 2;
-		vsync_start_to_end  /= 2;
-		vunk1 /= 2;
-		vunk2a /= 2;
-		vunk2b /= 2;
-		/* magic */
-		if (adjusted_mode->flags & DRM_MODE_FLAG_DBLSCAN) {
-			vsync_start_to_end -= 1;
-			vunk1 -= 1;
-			vunk2a -= 1;
-			vunk2b -= 1;
-		}
+	hactive = mode->htotal;
+	hsynce  = mode->hsync_end - mode->hsync_start - 1;
+	hbackp  = mode->htotal - mode->hsync_end;
+	hblanke = hsynce + hbackp;
+	hfrontp = mode->hsync_start - mode->hdisplay;
+	hblanks = mode->htotal - hfrontp - 1;
+
+	vactive = mode->vtotal * vscan / ilace;
+	vsynce  = ((mode->vsync_end - mode->vsync_start) * vscan / ilace) - 1;
+	vbackp  = (mode->vtotal - mode->vsync_end) * vscan / ilace;
+	vblanke = vsynce + vbackp;
+	vfrontp = (mode->vsync_start - mode->vdisplay) * vscan / ilace;
+	vblanks = vactive - vfrontp - 1;
+	if (mode->flags & DRM_MODE_FLAG_INTERLACE) {
+		vblan2e = vactive + vsynce + vbackp;
+		vblan2s = vblan2e + (mode->vdisplay * vscan / ilace);
+		vactive = (vactive * 2) + 1;
 	}
 
-	ret = RING_SPACE(evo, 17);
-	if (ret)
-		return ret;
-
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, CLOCK), 2);
-	OUT_RING(evo, adjusted_mode->clock | 0x800000);
-	OUT_RING(evo, (adjusted_mode->flags & DRM_MODE_FLAG_INTERLACE) ? 2 : 0);
-
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, DISPLAY_START), 5);
-	OUT_RING(evo, 0);
-	OUT_RING(evo, (adjusted_mode->vtotal << 16) | adjusted_mode->htotal);
-	OUT_RING(evo, (vsync_dur - 1) << 16 | (hsync_dur - 1));
-	OUT_RING(evo, (vsync_start_to_end - 1) << 16 |
-			(hsync_start_to_end - 1));
-	OUT_RING(evo, (vunk1 - 1) << 16 | (hunk1 - 1));
-
-	if (adjusted_mode->flags & DRM_MODE_FLAG_INTERLACE) {
-		BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, UNK0824), 1);
-		OUT_RING(evo, (vunk2b - 1) << 16 | (vunk2a - 1));
-	} else {
-		OUT_RING(evo, 0);
-		OUT_RING(evo, 0);
+	ret = RING_SPACE(evo, 18);
+	if (ret == 0) {
+		BEGIN_RING(evo, 0, 0x0804 + head, 2);
+		OUT_RING  (evo, 0x00800000 | mode->clock);
+		OUT_RING  (evo, (ilace == 2) ? 2 : 0);
+		BEGIN_RING(evo, 0, 0x0810 + head, 6);
+		OUT_RING  (evo, 0x00000000); /* border colour */
+		OUT_RING  (evo, (vactive << 16) | hactive);
+		OUT_RING  (evo, ( vsynce << 16) | hsynce);
+		OUT_RING  (evo, (vblanke << 16) | hblanke);
+		OUT_RING  (evo, (vblanks << 16) | hblanks);
+		OUT_RING  (evo, (vblan2e << 16) | vblan2s);
+		BEGIN_RING(evo, 0, 0x082c + head, 1);
+		OUT_RING  (evo, 0x00000000);
+		BEGIN_RING(evo, 0, 0x0900 + head, 1);
+		OUT_RING  (evo, 0x00000311); /* makes sync channel work */
+		BEGIN_RING(evo, 0, 0x08c8 + head, 1);
+		OUT_RING  (evo, (umode->vdisplay << 16) | umode->hdisplay);
+		BEGIN_RING(evo, 0, 0x08d4 + head, 1);
+		OUT_RING  (evo, 0x00000000); /* screen position */
 	}
 
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, UNK082C), 1);
-	OUT_RING(evo, 0);
-
-	/* This is the actual resolution of the mode. */
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, REAL_RES), 1);
-	OUT_RING(evo, (mode->vdisplay << 16) | mode->hdisplay);
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(nv_crtc->index, SCALE_CENTER_OFFSET), 1);
-	OUT_RING(evo, NV50_EVO_CRTC_SCALE_CENTER_OFFSET_VAL(0, 0));
-
-	nv_crtc->set_dither(nv_crtc, nv_connector->use_dithering, false);
-	nv_crtc->set_scale(nv_crtc, nv_connector->scaling_mode, false);
+	nv_crtc->set_dither(nv_crtc, false);
+	nv_crtc->set_scale(nv_crtc, false);
 
 	return nv50_crtc_do_mode_set_base(crtc, old_fb, x, y, false);
 }
@@ -692,7 +676,7 @@ nv50_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
 	if (ret)
 		return ret;
 
-	ret = nv50_crtc_wait_complete(crtc);
+	ret = nv50_display_sync(crtc->dev);
 	if (ret)
 		return ret;
 
@@ -711,7 +695,7 @@ nv50_crtc_mode_set_base_atomic(struct drm_crtc *crtc,
 	if (ret)
 		return ret;
 
-	return nv50_crtc_wait_complete(crtc);
+	return nv50_display_sync(crtc->dev);
 }
 
 static const struct drm_crtc_helper_funcs nv50_crtc_helper_funcs = {
@@ -737,12 +721,6 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	if (!nv_crtc)
 		return -ENOMEM;
 
-	nv_crtc->mode = kzalloc(sizeof(*nv_crtc->mode), GFP_KERNEL);
-	if (!nv_crtc->mode) {
-		kfree(nv_crtc);
-		return -ENOMEM;
-	}
-
 	/* Default CLUT parameters, will be activated on the hw upon
 	 * first mode set.
 	 */
@@ -764,7 +742,6 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	}
 
 	if (ret) {
-		kfree(nv_crtc->mode);
 		kfree(nv_crtc);
 		return ret;
 	}
diff --git a/drivers/gpu/drm/nouveau/nv50_dac.c b/drivers/gpu/drm/nouveau/nv50_dac.c
index 808f3ec8f827..a0f2bebf49e3 100644
--- a/drivers/gpu/drm/nouveau/nv50_dac.c
+++ b/drivers/gpu/drm/nouveau/nv50_dac.c
@@ -200,11 +200,6 @@ nv50_dac_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
 }
 
 static void
-nv50_dac_prepare(struct drm_encoder *encoder)
-{
-}
-
-static void
 nv50_dac_commit(struct drm_encoder *encoder)
 {
 }
@@ -266,7 +261,7 @@ static const struct drm_encoder_helper_funcs nv50_dac_helper_funcs = {
 	.save = nv50_dac_save,
 	.restore = nv50_dac_restore,
 	.mode_fixup = nv50_dac_mode_fixup,
-	.prepare = nv50_dac_prepare,
+	.prepare = nv50_dac_disconnect,
 	.commit = nv50_dac_commit,
 	.mode_set = nv50_dac_mode_set,
 	.get_crtc = nv50_dac_crtc_get,
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index 06de250fe617..7ba28e08ee31 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -50,9 +50,53 @@ nv50_sor_nr(struct drm_device *dev)
 	return 4;
 }
 
+static int
+evo_icmd(struct drm_device *dev, int ch, u32 mthd, u32 data)
+{
+	int ret = 0;
+	nv_mask(dev, 0x610300 + (ch * 0x08), 0x00000001, 0x00000001);
+	nv_wr32(dev, 0x610304 + (ch * 0x08), data);
+	nv_wr32(dev, 0x610300 + (ch * 0x08), 0x80000001 | mthd);
+	if (!nv_wait(dev, 0x610300 + (ch * 0x08), 0x80000000, 0x00000000))
+		ret = -EBUSY;
+	if (ret || (nouveau_reg_debug & NOUVEAU_REG_DEBUG_EVO))
+		NV_INFO(dev, "EvoPIO: %d 0x%04x 0x%08x\n", ch, mthd, data);
+	nv_mask(dev, 0x610300 + (ch * 0x08), 0x00000001, 0x00000000);
+	return ret;
+}
+
 int
 nv50_display_early_init(struct drm_device *dev)
 {
+	u32 ctrl = nv_rd32(dev, 0x610200);
+	int i;
+
+	/* check if master evo channel is already active, a good a sign as any
+	 * that the display engine is in a weird state (hibernate/kexec), if
+	 * it is, do our best to reset the display engine...
+	 */
+	if ((ctrl & 0x00000003) == 0x00000003) {
+		NV_INFO(dev, "PDISP: EVO(0) 0x%08x, resetting...\n", ctrl);
+
+		/* deactivate both heads first, PDISP will disappear forever
+		 * (well, until you power cycle) on some boards as soon as
+		 * PMC_ENABLE is hit unless they are..
+		 */
+		for (i = 0; i < 2; i++) {
+			evo_icmd(dev, 0, 0x0880 + (i * 0x400), 0x05000000);
+			evo_icmd(dev, 0, 0x089c + (i * 0x400), 0);
+			evo_icmd(dev, 0, 0x0840 + (i * 0x400), 0);
+			evo_icmd(dev, 0, 0x0844 + (i * 0x400), 0);
+			evo_icmd(dev, 0, 0x085c + (i * 0x400), 0);
+			evo_icmd(dev, 0, 0x0874 + (i * 0x400), 0);
+		}
+		evo_icmd(dev, 0, 0x0080, 0);
+
+		/* reset PDISP */
+		nv_mask(dev, 0x000200, 0x40000000, 0x00000000);
+		nv_mask(dev, 0x000200, 0x40000000, 0x40000000);
+	}
+
 	return 0;
 }
 
@@ -62,11 +106,40 @@ nv50_display_late_takedown(struct drm_device *dev)
 }
 
 int
-nv50_display_init(struct drm_device *dev)
+nv50_display_sync(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	struct drm_connector *connector;
+	struct nouveau_timer_engine *ptimer = &dev_priv->engine.timer;
+	struct nv50_display *disp = nv50_display(dev);
+	struct nouveau_channel *evo = disp->master;
+	u64 start;
+	int ret;
+
+	ret = RING_SPACE(evo, 6);
+	if (ret == 0) {
+		BEGIN_RING(evo, 0, 0x0084, 1);
+		OUT_RING  (evo, 0x80000000);
+		BEGIN_RING(evo, 0, 0x0080, 1);
+		OUT_RING  (evo, 0);
+		BEGIN_RING(evo, 0, 0x0084, 1);
+		OUT_RING  (evo, 0x00000000);
+
+		nv_wo32(disp->ntfy, 0x000, 0x00000000);
+		FIRE_RING (evo);
+
+		start = ptimer->read(dev);
+		do {
+			if (nv_ro32(disp->ntfy, 0x000))
+				return 0;
+		} while (ptimer->read(dev) - start < 2000000000ULL);
+	}
+
+	return -EBUSY;
+}
+
+int
+nv50_display_init(struct drm_device *dev)
+{
 	struct nouveau_channel *evo;
 	int ret, i;
 	u32 val;
@@ -161,16 +234,6 @@ nv50_display_init(struct drm_device *dev)
 		     NV50_PDISPLAY_INTR_EN_1_CLK_UNK20 |
 		     NV50_PDISPLAY_INTR_EN_1_CLK_UNK40);
 
-	/* enable hotplug interrupts */
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		struct nouveau_connector *conn = nouveau_connector(connector);
-
-		if (conn->dcb->gpio_tag == 0xff)
-			continue;
-
-		pgpio->irq_enable(dev, conn->dcb->gpio_tag, true);
-	}
-
 	ret = nv50_evo_init(dev);
 	if (ret)
 		return ret;
@@ -178,36 +241,19 @@ nv50_display_init(struct drm_device *dev)
 
 	nv_wr32(dev, NV50_PDISPLAY_OBJECTS, (evo->ramin->vinst >> 8) | 9);
 
-	ret = RING_SPACE(evo, 15);
+	ret = RING_SPACE(evo, 3);
 	if (ret)
 		return ret;
 	BEGIN_RING(evo, 0, NV50_EVO_UNK84, 2);
-	OUT_RING(evo, NV50_EVO_UNK84_NOTIFY_DISABLED);
-	OUT_RING(evo, NvEvoSync);
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, FB_DMA), 1);
-	OUT_RING(evo, NV50_EVO_CRTC_FB_DMA_HANDLE_NONE);
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, UNK0800), 1);
-	OUT_RING(evo, 0);
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, DISPLAY_START), 1);
-	OUT_RING(evo, 0);
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, UNK082C), 1);
-	OUT_RING(evo, 0);
-	/* required to make display sync channels not hate life */
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(0, UNK900), 1);
-	OUT_RING  (evo, 0x00000311);
-	BEGIN_RING(evo, 0, NV50_EVO_CRTC(1, UNK900), 1);
-	OUT_RING  (evo, 0x00000311);
-	FIRE_RING(evo);
-	if (!nv_wait(dev, 0x640004, 0xffffffff, evo->dma.put << 2))
-		NV_ERROR(dev, "evo pushbuf stalled\n");
-
+	OUT_RING  (evo, NV50_EVO_UNK84_NOTIFY_DISABLED);
+	OUT_RING  (evo, NvEvoSync);
 
-	return 0;
+	return nv50_display_sync(dev);
 }
 
-static int nv50_display_disable(struct drm_device *dev)
+void
+nv50_display_fini(struct drm_device *dev)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nv50_display *disp = nv50_display(dev);
 	struct nouveau_channel *evo = disp->master;
 	struct drm_crtc *drm_crtc;
@@ -270,18 +316,10 @@ static int nv50_display_disable(struct drm_device *dev)
 
 	/* disable interrupts. */
 	nv_wr32(dev, NV50_PDISPLAY_INTR_EN_1, 0x00000000);
-
-	/* disable hotplug interrupts */
-	nv_wr32(dev, 0xe054, 0xffffffff);
-	nv_wr32(dev, 0xe050, 0x00000000);
-	if (dev_priv->chipset >= 0x90) {
-		nv_wr32(dev, 0xe074, 0xffffffff);
-		nv_wr32(dev, 0xe070, 0x00000000);
-	}
-	return 0;
 }
 
-int nv50_display_create(struct drm_device *dev)
+int
+nv50_display_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct dcb_table *dcb = &dev_priv->vbios.dcb;
@@ -341,7 +379,7 @@ int nv50_display_create(struct drm_device *dev)
 	tasklet_init(&priv->tasklet, nv50_display_bh, (unsigned long)dev);
 	nouveau_irq_register(dev, 26, nv50_display_isr);
 
-	ret = nv50_display_init(dev);
+	ret = nv50_evo_create(dev);
 	if (ret) {
 		nv50_display_destroy(dev);
 		return ret;
@@ -357,7 +395,7 @@ nv50_display_destroy(struct drm_device *dev)
 
 	NV_DEBUG_KMS(dev, "\n");
 
-	nv50_display_disable(dev);
+	nv50_evo_destroy(dev);
 	nouveau_irq_unregister(dev, 26);
 	kfree(disp);
 }
@@ -521,7 +559,7 @@ nv50_display_script_select(struct drm_device *dev, struct dcb_entry *dcb,
 		} else {
 			/* determine number of lvds links */
 			if (nv_connector && nv_connector->edid &&
-			    nv_connector->dcb->type == DCB_CONNECTOR_LVDS_SPWG) {
+			    nv_connector->type == DCB_CONNECTOR_LVDS_SPWG) {
 				/* http://www.spwg.org */
 				if (((u8 *)nv_connector->edid)[121] == 2)
 					script |= 0x0100;
@@ -722,8 +760,8 @@ nv50_display_unk20_handler(struct drm_device *dev)
 	if (crtc >= 0) {
 		pclk  = nv_rd32(dev, NV50_PDISPLAY_CRTC_P(crtc, CLOCK));
 		pclk &= 0x003fffff;
-
-		nv50_crtc_set_clock(dev, crtc, pclk);
+		if (pclk)
+			nv50_crtc_set_clock(dev, crtc, pclk);
 
 		tmp = nv_rd32(dev, NV50_PDISPLAY_CRTC_CLK_CTRL2(crtc));
 		tmp &= ~0x000000f;
diff --git a/drivers/gpu/drm/nouveau/nv50_display.h b/drivers/gpu/drm/nouveau/nv50_display.h
index c2da503a22aa..95874f7c043c 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.h
+++ b/drivers/gpu/drm/nouveau/nv50_display.h
@@ -69,14 +69,18 @@ int nv50_display_early_init(struct drm_device *dev);
 void nv50_display_late_takedown(struct drm_device *dev);
 int nv50_display_create(struct drm_device *dev);
 int nv50_display_init(struct drm_device *dev);
+void nv50_display_fini(struct drm_device *dev);
 void nv50_display_destroy(struct drm_device *dev);
 int nv50_crtc_blank(struct nouveau_crtc *, bool blank);
 int nv50_crtc_set_clock(struct drm_device *, int head, int pclk);
 
+int  nv50_display_sync(struct drm_device *);
 int  nv50_display_flip_next(struct drm_crtc *, struct drm_framebuffer *,
 			    struct nouveau_channel *chan);
 void nv50_display_flip_stop(struct drm_crtc *);
 
+int  nv50_evo_create(struct drm_device *dev);
+void nv50_evo_destroy(struct drm_device *dev);
 int  nv50_evo_init(struct drm_device *dev);
 void nv50_evo_fini(struct drm_device *dev);
 void nv50_evo_dmaobj_init(struct nouveau_gpuobj *, u32 memtype, u64 base,
diff --git a/drivers/gpu/drm/nouveau/nv50_evo.c b/drivers/gpu/drm/nouveau/nv50_evo.c
index c99d9751880c..9b962e989d7c 100644
--- a/drivers/gpu/drm/nouveau/nv50_evo.c
+++ b/drivers/gpu/drm/nouveau/nv50_evo.c
@@ -218,7 +218,7 @@ nv50_evo_channel_fini(struct nouveau_channel *evo)
 	}
 }
 
-static void
+void
 nv50_evo_destroy(struct drm_device *dev)
 {
 	struct nv50_display *disp = nv50_display(dev);
@@ -235,7 +235,7 @@ nv50_evo_destroy(struct drm_device *dev)
 	nv50_evo_channel_del(&disp->master);
 }
 
-static int
+int
 nv50_evo_create(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
@@ -388,12 +388,6 @@ nv50_evo_init(struct drm_device *dev)
 	struct nv50_display *disp = nv50_display(dev);
 	int ret, i;
 
-	if (!disp->master) {
-		ret = nv50_evo_create(dev);
-		if (ret)
-			return ret;
-	}
-
 	ret = nv50_evo_channel_init(disp->master);
 	if (ret)
 		return ret;
@@ -420,6 +414,4 @@ nv50_evo_fini(struct drm_device *dev)
 
 	if (disp->master)
 		nv50_evo_channel_fini(disp->master);
-
-	nv50_evo_destroy(dev);
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_fifo.c b/drivers/gpu/drm/nouveau/nv50_fifo.c
index c34a074f7ea1..3bc2a565c20b 100644
--- a/drivers/gpu/drm/nouveau/nv50_fifo.c
+++ b/drivers/gpu/drm/nouveau/nv50_fifo.c
@@ -230,6 +230,7 @@ nv50_fifo_create_context(struct nouveau_channel *chan)
 	struct drm_device *dev = chan->dev;
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_gpuobj *ramfc = NULL;
+        uint64_t ib_offset = chan->pushbuf_base + chan->dma.ib_base * 4;
 	unsigned long flags;
 	int ret;
 
@@ -280,8 +281,9 @@ nv50_fifo_create_context(struct nouveau_channel *chan)
 	nv_wo32(ramfc, 0x7c, 0x30000001);
 	nv_wo32(ramfc, 0x78, 0x00000000);
 	nv_wo32(ramfc, 0x3c, 0x403f6078);
-	nv_wo32(ramfc, 0x50, chan->pushbuf_base + chan->dma.ib_base * 4);
-	nv_wo32(ramfc, 0x54, drm_order(chan->dma.ib_max + 1) << 16);
+	nv_wo32(ramfc, 0x50, lower_32_bits(ib_offset));
+	nv_wo32(ramfc, 0x54, upper_32_bits(ib_offset) |
+                drm_order(chan->dma.ib_max + 1) << 16);
 
 	if (dev_priv->chipset != 0x50) {
 		nv_wo32(chan->ramin, 0, chan->id);
diff --git a/drivers/gpu/drm/nouveau/nv50_gpio.c b/drivers/gpu/drm/nouveau/nv50_gpio.c
index 793a5ccca121..f429e6a8ca7a 100644
--- a/drivers/gpu/drm/nouveau/nv50_gpio.c
+++ b/drivers/gpu/drm/nouveau/nv50_gpio.c
@@ -25,229 +25,95 @@
 #include "drmP.h"
 #include "nouveau_drv.h"
 #include "nouveau_hw.h"
+#include "nouveau_gpio.h"
 
 #include "nv50_display.h"
 
-static void nv50_gpio_isr(struct drm_device *dev);
-static void nv50_gpio_isr_bh(struct work_struct *work);
-
-struct nv50_gpio_priv {
-	struct list_head handlers;
-	spinlock_t lock;
-};
-
-struct nv50_gpio_handler {
-	struct drm_device *dev;
-	struct list_head head;
-	struct work_struct work;
-	bool inhibit;
-
-	struct dcb_gpio_entry *gpio;
-
-	void (*handler)(void *data, int state);
-	void *data;
-};
-
 static int
-nv50_gpio_location(struct dcb_gpio_entry *gpio, uint32_t *reg, uint32_t *shift)
+nv50_gpio_location(int line, u32 *reg, u32 *shift)
 {
 	const uint32_t nv50_gpio_reg[4] = { 0xe104, 0xe108, 0xe280, 0xe284 };
 
-	if (gpio->line >= 32)
+	if (line >= 32)
 		return -EINVAL;
 
-	*reg = nv50_gpio_reg[gpio->line >> 3];
-	*shift = (gpio->line & 7) << 2;
+	*reg = nv50_gpio_reg[line >> 3];
+	*shift = (line & 7) << 2;
 	return 0;
 }
 
 int
-nv50_gpio_get(struct drm_device *dev, enum dcb_gpio_tag tag)
+nv50_gpio_drive(struct drm_device *dev, int line, int dir, int out)
 {
-	struct dcb_gpio_entry *gpio;
-	uint32_t r, s, v;
-
-	gpio = nouveau_bios_gpio_entry(dev, tag);
-	if (!gpio)
-		return -ENOENT;
+	u32 reg, shift;
 
-	if (nv50_gpio_location(gpio, &r, &s))
+	if (nv50_gpio_location(line, &reg, &shift))
 		return -EINVAL;
 
-	v = nv_rd32(dev, r) >> (s + 2);
-	return ((v & 1) == (gpio->state[1] & 1));
+	nv_mask(dev, reg, 7 << shift, (((dir ^ 1) << 1) | out) << shift);
+	return 0;
 }
 
 int
-nv50_gpio_set(struct drm_device *dev, enum dcb_gpio_tag tag, int state)
+nv50_gpio_sense(struct drm_device *dev, int line)
 {
-	struct dcb_gpio_entry *gpio;
-	uint32_t r, s, v;
-
-	gpio = nouveau_bios_gpio_entry(dev, tag);
-	if (!gpio)
-		return -ENOENT;
+	u32 reg, shift;
 
-	if (nv50_gpio_location(gpio, &r, &s))
+	if (nv50_gpio_location(line, &reg, &shift))
 		return -EINVAL;
 
-	v  = nv_rd32(dev, r) & ~(0x3 << s);
-	v |= (gpio->state[state] ^ 2) << s;
-	nv_wr32(dev, r, v);
-	return 0;
+	return !!(nv_rd32(dev, reg) & (4 << shift));
 }
 
-int
-nvd0_gpio_get(struct drm_device *dev, enum dcb_gpio_tag tag)
+void
+nv50_gpio_irq_enable(struct drm_device *dev, int line, bool on)
 {
-	struct dcb_gpio_entry *gpio;
-	u32 v;
-
-	gpio = nouveau_bios_gpio_entry(dev, tag);
-	if (!gpio)
-		return -ENOENT;
+	u32 reg  = line < 16 ? 0xe050 : 0xe070;
+	u32 mask = 0x00010001 << (line & 0xf);
 
-	v  = nv_rd32(dev, 0x00d610 + (gpio->line * 4));
-	v &= 0x00004000;
-	return (!!v == (gpio->state[1] & 1));
+	nv_wr32(dev, reg + 4, mask);
+	nv_mask(dev, reg + 0, mask, on ? mask : 0);
 }
 
 int
-nvd0_gpio_set(struct drm_device *dev, enum dcb_gpio_tag tag, int state)
+nvd0_gpio_drive(struct drm_device *dev, int line, int dir, int out)
 {
-	struct dcb_gpio_entry *gpio;
-	u32 v;
-
-	gpio = nouveau_bios_gpio_entry(dev, tag);
-	if (!gpio)
-		return -ENOENT;
-
-	v = gpio->state[state] ^ 2;
-
-	nv_mask(dev, 0x00d610 + (gpio->line * 4), 0x00003000, v << 12);
+	u32 data = ((dir ^ 1) << 13) | (out << 12);
+	nv_mask(dev, 0x00d610 + (line * 4), 0x00003000, data);
+	nv_mask(dev, 0x00d604, 0x00000001, 0x00000001); /* update? */
 	return 0;
 }
 
 int
-nv50_gpio_irq_register(struct drm_device *dev, enum dcb_gpio_tag tag,
-		       void (*handler)(void *, int), void *data)
+nvd0_gpio_sense(struct drm_device *dev, int line)
 {
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	struct nv50_gpio_priv *priv = pgpio->priv;
-	struct nv50_gpio_handler *gpioh;
-	struct dcb_gpio_entry *gpio;
-	unsigned long flags;
-
-	gpio = nouveau_bios_gpio_entry(dev, tag);
-	if (!gpio)
-		return -ENOENT;
-
-	gpioh = kzalloc(sizeof(*gpioh), GFP_KERNEL);
-	if (!gpioh)
-		return -ENOMEM;
-
-	INIT_WORK(&gpioh->work, nv50_gpio_isr_bh);
-	gpioh->dev  = dev;
-	gpioh->gpio = gpio;
-	gpioh->handler = handler;
-	gpioh->data = data;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	list_add(&gpioh->head, &priv->handlers);
-	spin_unlock_irqrestore(&priv->lock, flags);
-	return 0;
+	return !!(nv_rd32(dev, 0x00d610 + (line * 4)) & 0x00004000);
 }
 
-void
-nv50_gpio_irq_unregister(struct drm_device *dev, enum dcb_gpio_tag tag,
-			 void (*handler)(void *, int), void *data)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	struct nv50_gpio_priv *priv = pgpio->priv;
-	struct nv50_gpio_handler *gpioh, *tmp;
-	struct dcb_gpio_entry *gpio;
-	LIST_HEAD(tofree);
-	unsigned long flags;
-
-	gpio = nouveau_bios_gpio_entry(dev, tag);
-	if (!gpio)
-		return;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	list_for_each_entry_safe(gpioh, tmp, &priv->handlers, head) {
-		if (gpioh->gpio != gpio ||
-		    gpioh->handler != handler ||
-		    gpioh->data != data)
-			continue;
-		list_move(&gpioh->head, &tofree);
-	}
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	list_for_each_entry_safe(gpioh, tmp, &tofree, head) {
-		flush_work_sync(&gpioh->work);
-		kfree(gpioh);
-	}
-}
-
-bool
-nv50_gpio_irq_enable(struct drm_device *dev, enum dcb_gpio_tag tag, bool on)
-{
-	struct dcb_gpio_entry *gpio;
-	u32 reg, mask;
-
-	gpio = nouveau_bios_gpio_entry(dev, tag);
-	if (!gpio)
-		return false;
-
-	reg  = gpio->line < 16 ? 0xe050 : 0xe070;
-	mask = 0x00010001 << (gpio->line & 0xf);
-
-	nv_wr32(dev, reg + 4, mask);
-	reg = nv_mask(dev, reg + 0, mask, on ? mask : 0);
-	return (reg & mask) == mask;
-}
-
-static int
-nv50_gpio_create(struct drm_device *dev)
+static void
+nv50_gpio_isr(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	struct nv50_gpio_priv *priv;
-
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
+	u32 intr0, intr1 = 0;
+	u32 hi, lo;
 
-	INIT_LIST_HEAD(&priv->handlers);
-	spin_lock_init(&priv->lock);
-	pgpio->priv = priv;
-	return 0;
-}
+	intr0 = nv_rd32(dev, 0xe054) & nv_rd32(dev, 0xe050);
+	if (dev_priv->chipset >= 0x90)
+		intr1 = nv_rd32(dev, 0xe074) & nv_rd32(dev, 0xe070);
 
-static void
-nv50_gpio_destroy(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
+	hi = (intr0 & 0x0000ffff) | (intr1 << 16);
+	lo = (intr0 >> 16) | (intr1 & 0xffff0000);
+	nouveau_gpio_isr(dev, 0, hi | lo);
 
-	kfree(pgpio->priv);
-	pgpio->priv = NULL;
+	nv_wr32(dev, 0xe054, intr0);
+	if (dev_priv->chipset >= 0x90)
+		nv_wr32(dev, 0xe074, intr1);
 }
 
 int
 nv50_gpio_init(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	int ret;
-
-	if (!pgpio->priv) {
-		ret = nv50_gpio_create(dev);
-		if (ret)
-			return ret;
-	}
 
 	/* disable, and ack any pending gpio interrupts */
 	nv_wr32(dev, 0xe050, 0x00000000);
@@ -270,64 +136,4 @@ nv50_gpio_fini(struct drm_device *dev)
 	if (dev_priv->chipset >= 0x90)
 		nv_wr32(dev, 0xe070, 0x00000000);
 	nouveau_irq_unregister(dev, 21);
-
-	nv50_gpio_destroy(dev);
-}
-
-static void
-nv50_gpio_isr_bh(struct work_struct *work)
-{
-	struct nv50_gpio_handler *gpioh =
-		container_of(work, struct nv50_gpio_handler, work);
-	struct drm_nouveau_private *dev_priv = gpioh->dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	struct nv50_gpio_priv *priv = pgpio->priv;
-	unsigned long flags;
-	int state;
-
-	state = pgpio->get(gpioh->dev, gpioh->gpio->tag);
-	if (state < 0)
-		return;
-
-	gpioh->handler(gpioh->data, state);
-
-	spin_lock_irqsave(&priv->lock, flags);
-	gpioh->inhibit = false;
-	spin_unlock_irqrestore(&priv->lock, flags);
-}
-
-static void
-nv50_gpio_isr(struct drm_device *dev)
-{
-	struct drm_nouveau_private *dev_priv = dev->dev_private;
-	struct nouveau_gpio_engine *pgpio = &dev_priv->engine.gpio;
-	struct nv50_gpio_priv *priv = pgpio->priv;
-	struct nv50_gpio_handler *gpioh;
-	u32 intr0, intr1 = 0;
-	u32 hi, lo, ch;
-
-	intr0 = nv_rd32(dev, 0xe054) & nv_rd32(dev, 0xe050);
-	if (dev_priv->chipset >= 0x90)
-		intr1 = nv_rd32(dev, 0xe074) & nv_rd32(dev, 0xe070);
-
-	hi = (intr0 & 0x0000ffff) | (intr1 << 16);
-	lo = (intr0 >> 16) | (intr1 & 0xffff0000);
-	ch = hi | lo;
-
-	nv_wr32(dev, 0xe054, intr0);
-	if (dev_priv->chipset >= 0x90)
-		nv_wr32(dev, 0xe074, intr1);
-
-	spin_lock(&priv->lock);
-	list_for_each_entry(gpioh, &priv->handlers, head) {
-		if (!(ch & (1 << gpioh->gpio->line)))
-			continue;
-
-		if (gpioh->inhibit)
-			continue;
-		gpioh->inhibit = true;
-
-		schedule_work(&gpioh->work);
-	}
-	spin_unlock(&priv->lock);
 }
diff --git a/drivers/gpu/drm/nouveau/nv50_graph.c b/drivers/gpu/drm/nouveau/nv50_graph.c
index ac601f7c4e1a..33d5711a918d 100644
--- a/drivers/gpu/drm/nouveau/nv50_graph.c
+++ b/drivers/gpu/drm/nouveau/nv50_graph.c
@@ -616,9 +616,9 @@ nv50_pgraph_tp_trap(struct drm_device *dev, int type, uint32_t ustatus_old,
 			}
 			break;
 		case 7: /* MP error */
-			if (ustatus & 0x00010000) {
+			if (ustatus & 0x04030000) {
 				nv50_pgraph_mp_trap(dev, i, display);
-				ustatus &= ~0x00010000;
+				ustatus &= ~0x04030000;
 			}
 			break;
 		case 8: /* TPDMA error */
diff --git a/drivers/gpu/drm/nouveau/nv50_pm.c b/drivers/gpu/drm/nouveau/nv50_pm.c
index 3d5a86b98282..03937212e9d8 100644
--- a/drivers/gpu/drm/nouveau/nv50_pm.c
+++ b/drivers/gpu/drm/nouveau/nv50_pm.c
@@ -25,122 +25,745 @@
 #include "drmP.h"
 #include "nouveau_drv.h"
 #include "nouveau_bios.h"
+#include "nouveau_hw.h"
 #include "nouveau_pm.h"
+#include "nouveau_hwsq.h"
 
-struct nv50_pm_state {
-	struct nouveau_pm_level *perflvl;
-	struct pll_lims pll;
-	enum pll_types type;
-	int N, M, P;
+enum clk_src {
+	clk_src_crystal,
+	clk_src_href,
+	clk_src_hclk,
+	clk_src_hclkm3,
+	clk_src_hclkm3d2,
+	clk_src_host,
+	clk_src_nvclk,
+	clk_src_sclk,
+	clk_src_mclk,
+	clk_src_vdec,
+	clk_src_dom6
 };
 
+static u32 read_clk(struct drm_device *, enum clk_src);
+
+static u32
+read_div(struct drm_device *dev)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+
+	switch (dev_priv->chipset) {
+	case 0x50: /* it exists, but only has bit 31, not the dividers.. */
+	case 0x84:
+	case 0x86:
+	case 0x98:
+	case 0xa0:
+		return nv_rd32(dev, 0x004700);
+	case 0x92:
+	case 0x94:
+	case 0x96:
+		return nv_rd32(dev, 0x004800);
+	default:
+		return 0x00000000;
+	}
+}
+
+static u32
+read_pll_src(struct drm_device *dev, u32 base)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	u32 coef, ref = read_clk(dev, clk_src_crystal);
+	u32 rsel = nv_rd32(dev, 0x00e18c);
+	int P, N, M, id;
+
+	switch (dev_priv->chipset) {
+	case 0x50:
+	case 0xa0:
+		switch (base) {
+		case 0x4020:
+		case 0x4028: id = !!(rsel & 0x00000004); break;
+		case 0x4008: id = !!(rsel & 0x00000008); break;
+		case 0x4030: id = 0; break;
+		default:
+			NV_ERROR(dev, "ref: bad pll 0x%06x\n", base);
+			return 0;
+		}
+
+		coef = nv_rd32(dev, 0x00e81c + (id * 0x0c));
+		ref *=  (coef & 0x01000000) ? 2 : 4;
+		P    =  (coef & 0x00070000) >> 16;
+		N    = ((coef & 0x0000ff00) >> 8) + 1;
+		M    = ((coef & 0x000000ff) >> 0) + 1;
+		break;
+	case 0x84:
+	case 0x86:
+	case 0x92:
+		coef = nv_rd32(dev, 0x00e81c);
+		P    = (coef & 0x00070000) >> 16;
+		N    = (coef & 0x0000ff00) >> 8;
+		M    = (coef & 0x000000ff) >> 0;
+		break;
+	case 0x94:
+	case 0x96:
+	case 0x98:
+		rsel = nv_rd32(dev, 0x00c050);
+		switch (base) {
+		case 0x4020: rsel = (rsel & 0x00000003) >> 0; break;
+		case 0x4008: rsel = (rsel & 0x0000000c) >> 2; break;
+		case 0x4028: rsel = (rsel & 0x00001800) >> 11; break;
+		case 0x4030: rsel = 3; break;
+		default:
+			NV_ERROR(dev, "ref: bad pll 0x%06x\n", base);
+			return 0;
+		}
+
+		switch (rsel) {
+		case 0: id = 1; break;
+		case 1: return read_clk(dev, clk_src_crystal);
+		case 2: return read_clk(dev, clk_src_href);
+		case 3: id = 0; break;
+		}
+
+		coef =  nv_rd32(dev, 0x00e81c + (id * 0x28));
+		P    = (nv_rd32(dev, 0x00e824 + (id * 0x28)) >> 16) & 7;
+		P   += (coef & 0x00070000) >> 16;
+		N    = (coef & 0x0000ff00) >> 8;
+		M    = (coef & 0x000000ff) >> 0;
+		break;
+	default:
+		BUG_ON(1);
+	}
+
+	if (M)
+		return (ref * N / M) >> P;
+	return 0;
+}
+
+static u32
+read_pll_ref(struct drm_device *dev, u32 base)
+{
+	u32 src, mast = nv_rd32(dev, 0x00c040);
+
+	switch (base) {
+	case 0x004028:
+		src = !!(mast & 0x00200000);
+		break;
+	case 0x004020:
+		src = !!(mast & 0x00400000);
+		break;
+	case 0x004008:
+		src = !!(mast & 0x00010000);
+		break;
+	case 0x004030:
+		src = !!(mast & 0x02000000);
+		break;
+	case 0x00e810:
+		return read_clk(dev, clk_src_crystal);
+	default:
+		NV_ERROR(dev, "bad pll 0x%06x\n", base);
+		return 0;
+	}
+
+	if (src)
+		return read_clk(dev, clk_src_href);
+	return read_pll_src(dev, base);
+}
+
+static u32
+read_pll(struct drm_device *dev, u32 base)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	u32 mast = nv_rd32(dev, 0x00c040);
+	u32 ctrl = nv_rd32(dev, base + 0);
+	u32 coef = nv_rd32(dev, base + 4);
+	u32 ref = read_pll_ref(dev, base);
+	u32 clk = 0;
+	int N1, N2, M1, M2;
+
+	if (base == 0x004028 && (mast & 0x00100000)) {
+		/* wtf, appears to only disable post-divider on nva0 */
+		if (dev_priv->chipset != 0xa0)
+			return read_clk(dev, clk_src_dom6);
+	}
+
+	N2 = (coef & 0xff000000) >> 24;
+	M2 = (coef & 0x00ff0000) >> 16;
+	N1 = (coef & 0x0000ff00) >> 8;
+	M1 = (coef & 0x000000ff);
+	if ((ctrl & 0x80000000) && M1) {
+		clk = ref * N1 / M1;
+		if ((ctrl & 0x40000100) == 0x40000000) {
+			if (M2)
+				clk = clk * N2 / M2;
+			else
+				clk = 0;
+		}
+	}
+
+	return clk;
+}
+
+static u32
+read_clk(struct drm_device *dev, enum clk_src src)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	u32 mast = nv_rd32(dev, 0x00c040);
+	u32 P = 0;
+
+	switch (src) {
+	case clk_src_crystal:
+		return dev_priv->crystal;
+	case clk_src_href:
+		return 100000; /* PCIE reference clock */
+	case clk_src_hclk:
+		return read_clk(dev, clk_src_href) * 27778 / 10000;
+	case clk_src_hclkm3:
+		return read_clk(dev, clk_src_hclk) * 3;
+	case clk_src_hclkm3d2:
+		return read_clk(dev, clk_src_hclk) * 3 / 2;
+	case clk_src_host:
+		switch (mast & 0x30000000) {
+		case 0x00000000: return read_clk(dev, clk_src_href);
+		case 0x10000000: break;
+		case 0x20000000: /* !0x50 */
+		case 0x30000000: return read_clk(dev, clk_src_hclk);
+		}
+		break;
+	case clk_src_nvclk:
+		if (!(mast & 0x00100000))
+			P = (nv_rd32(dev, 0x004028) & 0x00070000) >> 16;
+		switch (mast & 0x00000003) {
+		case 0x00000000: return read_clk(dev, clk_src_crystal) >> P;
+		case 0x00000001: return read_clk(dev, clk_src_dom6);
+		case 0x00000002: return read_pll(dev, 0x004020) >> P;
+		case 0x00000003: return read_pll(dev, 0x004028) >> P;
+		}
+		break;
+	case clk_src_sclk:
+		P = (nv_rd32(dev, 0x004020) & 0x00070000) >> 16;
+		switch (mast & 0x00000030) {
+		case 0x00000000:
+			if (mast & 0x00000080)
+				return read_clk(dev, clk_src_host) >> P;
+			return read_clk(dev, clk_src_crystal) >> P;
+		case 0x00000010: break;
+		case 0x00000020: return read_pll(dev, 0x004028) >> P;
+		case 0x00000030: return read_pll(dev, 0x004020) >> P;
+		}
+		break;
+	case clk_src_mclk:
+		P = (nv_rd32(dev, 0x004008) & 0x00070000) >> 16;
+		if (nv_rd32(dev, 0x004008) & 0x00000200) {
+			switch (mast & 0x0000c000) {
+			case 0x00000000:
+				return read_clk(dev, clk_src_crystal) >> P;
+			case 0x00008000:
+			case 0x0000c000:
+				return read_clk(dev, clk_src_href) >> P;
+			}
+		} else {
+			return read_pll(dev, 0x004008) >> P;
+		}
+		break;
+	case clk_src_vdec:
+		P = (read_div(dev) & 0x00000700) >> 8;
+		switch (dev_priv->chipset) {
+		case 0x84:
+		case 0x86:
+		case 0x92:
+		case 0x94:
+		case 0x96:
+		case 0xa0:
+			switch (mast & 0x00000c00) {
+			case 0x00000000:
+				if (dev_priv->chipset == 0xa0) /* wtf?? */
+					return read_clk(dev, clk_src_nvclk) >> P;
+				return read_clk(dev, clk_src_crystal) >> P;
+			case 0x00000400:
+				return 0;
+			case 0x00000800:
+				if (mast & 0x01000000)
+					return read_pll(dev, 0x004028) >> P;
+				return read_pll(dev, 0x004030) >> P;
+			case 0x00000c00:
+				return read_clk(dev, clk_src_nvclk) >> P;
+			}
+			break;
+		case 0x98:
+			switch (mast & 0x00000c00) {
+			case 0x00000000:
+				return read_clk(dev, clk_src_nvclk) >> P;
+			case 0x00000400:
+				return 0;
+			case 0x00000800:
+				return read_clk(dev, clk_src_hclkm3d2) >> P;
+			case 0x00000c00:
+				return read_clk(dev, clk_src_mclk) >> P;
+			}
+			break;
+		}
+		break;
+	case clk_src_dom6:
+		switch (dev_priv->chipset) {
+		case 0x50:
+		case 0xa0:
+			return read_pll(dev, 0x00e810) >> 2;
+		case 0x84:
+		case 0x86:
+		case 0x92:
+		case 0x94:
+		case 0x96:
+		case 0x98:
+			P = (read_div(dev) & 0x00000007) >> 0;
+			switch (mast & 0x0c000000) {
+			case 0x00000000: return read_clk(dev, clk_src_href);
+			case 0x04000000: break;
+			case 0x08000000: return read_clk(dev, clk_src_hclk);
+			case 0x0c000000:
+				return read_clk(dev, clk_src_hclkm3) >> P;
+			}
+			break;
+		default:
+			break;
+		}
+	default:
+		break;
+	}
+
+	NV_DEBUG(dev, "unknown clock source %d 0x%08x\n", src, mast);
+	return 0;
+}
+
 int
-nv50_pm_clock_get(struct drm_device *dev, u32 id)
+nv50_pm_clocks_get(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 {
-	struct pll_lims pll;
-	int P, N, M, ret;
-	u32 reg0, reg1;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	if (dev_priv->chipset == 0xaa ||
+	    dev_priv->chipset == 0xac)
+		return 0;
 
-	ret = get_pll_limits(dev, id, &pll);
+	perflvl->core   = read_clk(dev, clk_src_nvclk);
+	perflvl->shader = read_clk(dev, clk_src_sclk);
+	perflvl->memory = read_clk(dev, clk_src_mclk);
+	if (dev_priv->chipset != 0x50) {
+		perflvl->vdec = read_clk(dev, clk_src_vdec);
+		perflvl->dom6 = read_clk(dev, clk_src_dom6);
+	}
+
+	return 0;
+}
+
+struct nv50_pm_state {
+	struct hwsq_ucode mclk_hwsq;
+	u32 mscript;
+
+	u32 emast;
+	u32 nctrl;
+	u32 ncoef;
+	u32 sctrl;
+	u32 scoef;
+
+	u32 amast;
+	u32 pdivs;
+};
+
+static u32
+calc_pll(struct drm_device *dev, u32 reg, struct pll_lims *pll,
+	 u32 clk, int *N1, int *M1, int *log2P)
+{
+	struct nouveau_pll_vals coef;
+	int ret;
+
+	ret = get_pll_limits(dev, reg, pll);
 	if (ret)
-		return ret;
+		return 0;
+
+	pll->vco2.maxfreq = 0;
+	pll->refclk = read_pll_ref(dev, reg);
+	if (!pll->refclk)
+		return 0;
 
-	reg0 = nv_rd32(dev, pll.reg + 0);
-	reg1 = nv_rd32(dev, pll.reg + 4);
+	ret = nouveau_calc_pll_mnp(dev, pll, clk, &coef);
+	if (ret == 0)
+		return 0;
 
-	if ((reg0 & 0x80000000) == 0) {
-		if (id == PLL_SHADER) {
-			NV_DEBUG(dev, "Shader PLL is disabled. "
-				"Shader clock is twice the core\n");
-			ret = nv50_pm_clock_get(dev, PLL_CORE);
-			if (ret > 0)
-				return ret << 1;
-		} else if (id == PLL_MEMORY) {
-			NV_DEBUG(dev, "Memory PLL is disabled. "
-				"Memory clock is equal to the ref_clk\n");
-			return pll.refclk;
+	*N1 = coef.N1;
+	*M1 = coef.M1;
+	*log2P = coef.log2P;
+	return ret;
+}
+
+static inline u32
+calc_div(u32 src, u32 target, int *div)
+{
+	u32 clk0 = src, clk1 = src;
+	for (*div = 0; *div <= 7; (*div)++) {
+		if (clk0 <= target) {
+			clk1 = clk0 << (*div ? 1 : 0);
+			break;
 		}
+		clk0 >>= 1;
+	}
+
+	if (target - clk0 <= clk1 - target)
+		return clk0;
+	(*div)--;
+	return clk1;
+}
+
+static inline u32
+clk_same(u32 a, u32 b)
+{
+	return ((a / 1000) == (b / 1000));
+}
+
+static int
+calc_mclk(struct drm_device *dev, u32 freq, struct hwsq_ucode *hwsq)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct pll_lims pll;
+	u32 mast = nv_rd32(dev, 0x00c040);
+	u32 ctrl = nv_rd32(dev, 0x004008);
+	u32 coef = nv_rd32(dev, 0x00400c);
+	u32 orig = ctrl;
+	u32 crtc_mask = 0;
+	int N, M, P;
+	int ret, i;
+
+	/* use pcie refclock if possible, otherwise use mpll */
+	ctrl &= ~0x81ff0200;
+	if (clk_same(freq, read_clk(dev, clk_src_href))) {
+		ctrl |= 0x00000200 | (pll.log2p_bias << 19);
+	} else {
+		ret = calc_pll(dev, 0x4008, &pll, freq, &N, &M, &P);
+		if (ret == 0)
+			return -EINVAL;
+
+		ctrl |= 0x80000000 | (P << 22) | (P << 16);
+		ctrl |= pll.log2p_bias << 19;
+		coef  = (N << 8) | M;
+	}
+
+	mast &= ~0xc0000000; /* get MCLK_2 from HREF */
+	mast |=  0x0000c000; /* use MCLK_2 as MPLL_BYPASS clock */
+
+	/* determine active crtcs */
+	for (i = 0; i < 2; i++) {
+		if (nv_rd32(dev, NV50_PDISPLAY_CRTC_C(i, CLOCK)))
+			crtc_mask |= (1 << i);
+	}
+
+	/* build the ucode which will reclock the memory for us */
+	hwsq_init(hwsq);
+	if (crtc_mask) {
+		hwsq_op5f(hwsq, crtc_mask, 0x00); /* wait for scanout */
+		hwsq_op5f(hwsq, crtc_mask, 0x01); /* wait for vblank */
 	}
+	if (dev_priv->chipset >= 0x92)
+		hwsq_wr32(hwsq, 0x611200, 0x00003300); /* disable scanout */
+	hwsq_setf(hwsq, 0x10, 0); /* disable bus access */
+	hwsq_op5f(hwsq, 0x00, 0x01); /* no idea :s */
+
+	/* prepare memory controller */
+	hwsq_wr32(hwsq, 0x1002d4, 0x00000001); /* precharge banks and idle */
+	hwsq_wr32(hwsq, 0x1002d0, 0x00000001); /* force refresh */
+	hwsq_wr32(hwsq, 0x100210, 0x00000000); /* stop the automatic refresh */
+	hwsq_wr32(hwsq, 0x1002dc, 0x00000001); /* start self refresh mode */
 
-	P = (reg0 & 0x00070000) >> 16;
-	N = (reg1 & 0x0000ff00) >> 8;
-	M = (reg1 & 0x000000ff);
+	/* reclock memory */
+	hwsq_wr32(hwsq, 0xc040, mast);
+	hwsq_wr32(hwsq, 0x4008, orig | 0x00000200); /* bypass MPLL */
+	hwsq_wr32(hwsq, 0x400c, coef);
+	hwsq_wr32(hwsq, 0x4008, ctrl);
 
-	return ((pll.refclk * N / M) >> P);
+	/* restart memory controller */
+	hwsq_wr32(hwsq, 0x1002d4, 0x00000001); /* precharge banks and idle */
+	hwsq_wr32(hwsq, 0x1002dc, 0x00000000); /* stop self refresh mode */
+	hwsq_wr32(hwsq, 0x100210, 0x80000000); /* restart automatic refresh */
+	hwsq_usec(hwsq, 12); /* wait for the PLL to stabilize */
+
+	hwsq_usec(hwsq, 48); /* may be unnecessary: causes flickering */
+	hwsq_setf(hwsq, 0x10, 1); /* enable bus access */
+	hwsq_op5f(hwsq, 0x00, 0x00); /* no idea, reverse of 0x00, 0x01? */
+	if (dev_priv->chipset >= 0x92)
+		hwsq_wr32(hwsq, 0x611200, 0x00003330); /* enable scanout */
+	hwsq_fini(hwsq);
+	return 0;
 }
 
 void *
-nv50_pm_clock_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl,
-		  u32 id, int khz)
+nv50_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 {
-	struct nv50_pm_state *state;
-	int dummy, ret;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv50_pm_state *info;
+	struct pll_lims pll;
+	int ret = -EINVAL;
+	int N, M, P1, P2;
+	u32 clk, out;
 
-	state = kzalloc(sizeof(*state), GFP_KERNEL);
-	if (!state)
+	if (dev_priv->chipset == 0xaa ||
+	    dev_priv->chipset == 0xac)
+		return ERR_PTR(-ENODEV);
+
+	info = kmalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
 		return ERR_PTR(-ENOMEM);
-	state->type = id;
-	state->perflvl = perflvl;
 
-	ret = get_pll_limits(dev, id, &state->pll);
-	if (ret < 0) {
-		kfree(state);
-		return (ret == -ENOENT) ? NULL : ERR_PTR(ret);
+	/* core: for the moment at least, always use nvpll */
+	clk = calc_pll(dev, 0x4028, &pll, perflvl->core, &N, &M, &P1);
+	if (clk == 0)
+		goto error;
+
+	info->emast = 0x00000003;
+	info->nctrl = 0x80000000 | (P1 << 19) | (P1 << 16);
+	info->ncoef = (N << 8) | M;
+
+	/* shader: tie to nvclk if possible, otherwise use spll.  have to be
+	 * very careful that the shader clock is at least twice the core, or
+	 * some chipsets will be very unhappy.  i expect most or all of these
+	 * cases will be handled by tying to nvclk, but it's possible there's
+	 * corners
+	 */
+	if (P1-- && perflvl->shader == (perflvl->core << 1)) {
+		info->emast |= 0x00000020;
+		info->sctrl  = 0x00000000 | (P1 << 19) | (P1 << 16);
+		info->scoef  = nv_rd32(dev, 0x004024);
+	} else {
+		clk = calc_pll(dev, 0x4020, &pll, perflvl->shader, &N, &M, &P1);
+		if (clk == 0)
+			goto error;
+
+		info->emast |= 0x00000030;
+		info->sctrl  = 0x80000000 | (P1 << 19) | (P1 << 16);
+		info->scoef  = (N << 8) | M;
+	}
+
+	/* memory: build hwsq ucode which we'll use to reclock memory */
+	info->mclk_hwsq.len = 0;
+	if (perflvl->memory) {
+		clk = calc_mclk(dev, perflvl->memory, &info->mclk_hwsq);
+		if (clk < 0) {
+			ret = clk;
+			goto error;
+		}
+
+		info->mscript = perflvl->memscript;
+	}
+
+	/* vdec: avoid modifying xpll until we know exactly how the other
+	 * clock domains work, i suspect at least some of them can also be
+	 * tied to xpll...
+	 */
+	info->amast = nv_rd32(dev, 0x00c040);
+	info->pdivs = read_div(dev);
+	if (perflvl->vdec) {
+		/* see how close we can get using nvclk as a source */
+		clk = calc_div(perflvl->core, perflvl->vdec, &P1);
+
+		/* see how close we can get using xpll/hclk as a source */
+		if (dev_priv->chipset != 0x98)
+			out = read_pll(dev, 0x004030);
+		else
+			out = read_clk(dev, clk_src_hclkm3d2);
+		out = calc_div(out, perflvl->vdec, &P2);
+
+		/* select whichever gets us closest */
+		info->amast &= ~0x00000c00;
+		info->pdivs &= ~0x00000700;
+		if (abs((int)perflvl->vdec - clk) <=
+		    abs((int)perflvl->vdec - out)) {
+			if (dev_priv->chipset != 0x98)
+				info->amast |= 0x00000c00;
+			info->pdivs |= P1 << 8;
+		} else {
+			info->amast |= 0x00000800;
+			info->pdivs |= P2 << 8;
+		}
+	}
+
+	/* dom6: nfi what this is, but we're limited to various combinations
+	 * of the host clock frequency
+	 */
+	if (perflvl->dom6) {
+		info->amast &= ~0x0c000000;
+		if (clk_same(perflvl->dom6, read_clk(dev, clk_src_href))) {
+			info->amast |= 0x00000000;
+		} else
+		if (clk_same(perflvl->dom6, read_clk(dev, clk_src_hclk))) {
+			info->amast |= 0x08000000;
+		} else {
+			clk = read_clk(dev, clk_src_hclk) * 3;
+			clk = calc_div(clk, perflvl->dom6, &P1);
+
+			info->amast |= 0x0c000000;
+			info->pdivs  = (info->pdivs & ~0x00000007) | P1;
+		}
 	}
 
-	ret = nv50_calc_pll(dev, &state->pll, khz, &state->N, &state->M,
-			    &dummy, &dummy, &state->P);
-	if (ret < 0) {
-		kfree(state);
-		return ERR_PTR(ret);
+	return info;
+error:
+	kfree(info);
+	return ERR_PTR(ret);
+}
+
+static int
+prog_mclk(struct drm_device *dev, struct hwsq_ucode *hwsq)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	u32 hwsq_data, hwsq_kick;
+	int i;
+
+	if (dev_priv->chipset < 0x90) {
+		hwsq_data = 0x001400;
+		hwsq_kick = 0x00000003;
+	} else {
+		hwsq_data = 0x080000;
+		hwsq_kick = 0x00000001;
 	}
 
-	return state;
+	/* upload hwsq ucode */
+	nv_mask(dev, 0x001098, 0x00000008, 0x00000000);
+	nv_wr32(dev, 0x001304, 0x00000000);
+	for (i = 0; i < hwsq->len / 4; i++)
+		nv_wr32(dev, hwsq_data + (i * 4), hwsq->ptr.u32[i]);
+	nv_mask(dev, 0x001098, 0x00000018, 0x00000018);
+
+	/* launch, and wait for completion */
+	nv_wr32(dev, 0x00130c, hwsq_kick);
+	if (!nv_wait(dev, 0x001308, 0x00000100, 0x00000000)) {
+		NV_ERROR(dev, "hwsq ucode exec timed out\n");
+		NV_ERROR(dev, "0x001308: 0x%08x\n", nv_rd32(dev, 0x001308));
+		for (i = 0; i < hwsq->len / 4; i++) {
+			NV_ERROR(dev, "0x%06x: 0x%08x\n", 0x1400 + (i * 4),
+				 nv_rd32(dev, 0x001400 + (i * 4)));
+		}
+
+		return -EIO;
+	}
+
+	return 0;
 }
 
-void
-nv50_pm_clock_set(struct drm_device *dev, void *pre_state)
+int
+nv50_pm_clocks_set(struct drm_device *dev, void *data)
 {
-	struct nv50_pm_state *state = pre_state;
-	struct nouveau_pm_level *perflvl = state->perflvl;
-	u32 reg = state->pll.reg, tmp;
-	struct bit_entry BIT_M;
-	u16 script;
-	int N = state->N;
-	int M = state->M;
-	int P = state->P;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nv50_pm_state *info = data;
+	struct bit_entry M;
+	int ret = 0;
 
-	if (state->type == PLL_MEMORY && perflvl->memscript &&
-	    bit_table(dev, 'M', &BIT_M) == 0 &&
-	    BIT_M.version == 1 && BIT_M.length >= 0x0b) {
-		script = ROM16(BIT_M.data[0x05]);
-		if (script)
-			nouveau_bios_run_init_table(dev, script, NULL, -1);
-		script = ROM16(BIT_M.data[0x07]);
-		if (script)
-			nouveau_bios_run_init_table(dev, script, NULL, -1);
-		script = ROM16(BIT_M.data[0x09]);
-		if (script)
-			nouveau_bios_run_init_table(dev, script, NULL, -1);
+	/* halt and idle execution engines */
+	nv_mask(dev, 0x002504, 0x00000001, 0x00000001);
+	if (!nv_wait(dev, 0x002504, 0x00000010, 0x00000010))
+		goto error;
 
-		nouveau_bios_run_init_table(dev, perflvl->memscript, NULL, -1);
+	/* memory: it is *very* important we change this first, the ucode
+	 * we build in pre() now has hardcoded 0xc040 values, which can't
+	 * change before we execute it or the engine clocks may end up
+	 * messed up.
+	 */
+	if (info->mclk_hwsq.len) {
+		/* execute some scripts that do ??? from the vbios.. */
+		if (!bit_table(dev, 'M', &M) && M.version == 1) {
+			if (M.length >= 6)
+				nouveau_bios_init_exec(dev, ROM16(M.data[5]));
+			if (M.length >= 8)
+				nouveau_bios_init_exec(dev, ROM16(M.data[7]));
+			if (M.length >= 10)
+				nouveau_bios_init_exec(dev, ROM16(M.data[9]));
+			nouveau_bios_init_exec(dev, info->mscript);
+		}
+
+		ret = prog_mclk(dev, &info->mclk_hwsq);
+		if (ret)
+			goto resume;
 	}
 
-	if (state->type == PLL_MEMORY) {
-		nv_wr32(dev, 0x100210, 0);
-		nv_wr32(dev, 0x1002dc, 1);
+	/* reclock vdec/dom6 */
+	nv_mask(dev, 0x00c040, 0x00000c00, 0x00000000);
+	switch (dev_priv->chipset) {
+	case 0x92:
+	case 0x94:
+	case 0x96:
+		nv_mask(dev, 0x004800, 0x00000707, info->pdivs);
+		break;
+	default:
+		nv_mask(dev, 0x004700, 0x00000707, info->pdivs);
+		break;
 	}
+	nv_mask(dev, 0x00c040, 0x0c000c00, info->amast);
 
-	tmp  = nv_rd32(dev, reg + 0) & 0xfff8ffff;
-	tmp |= 0x80000000 | (P << 16);
-	nv_wr32(dev, reg + 0, tmp);
-	nv_wr32(dev, reg + 4, (N << 8) | M);
+	/* core/shader: make sure sclk/nvclk are disconnected from their
+	 * plls (nvclk to dom6, sclk to hclk), modify the plls, and
+	 * reconnect sclk/nvclk to their new clock source
+	 */
+	if (dev_priv->chipset < 0x92)
+		nv_mask(dev, 0x00c040, 0x001000b0, 0x00100080); /* grrr! */
+	else
+		nv_mask(dev, 0x00c040, 0x000000b3, 0x00000081);
+	nv_mask(dev, 0x004020, 0xc03f0100, info->sctrl);
+	nv_wr32(dev, 0x004024, info->scoef);
+	nv_mask(dev, 0x004028, 0xc03f0100, info->nctrl);
+	nv_wr32(dev, 0x00402c, info->ncoef);
+	nv_mask(dev, 0x00c040, 0x00100033, info->emast);
+
+	goto resume;
+error:
+	ret = -EBUSY;
+resume:
+	nv_mask(dev, 0x002504, 0x00000001, 0x00000000);
+	kfree(info);
+	return ret;
+}
 
-	if (state->type == PLL_MEMORY) {
-		nv_wr32(dev, 0x1002dc, 0);
-		nv_wr32(dev, 0x100210, 0x80000000);
+static int
+pwm_info(struct drm_device *dev, int *line, int *ctrl, int *indx)
+{
+	if (*line == 0x04) {
+		*ctrl = 0x00e100;
+		*line = 4;
+		*indx = 0;
+	} else
+	if (*line == 0x09) {
+		*ctrl = 0x00e100;
+		*line = 9;
+		*indx = 1;
+	} else
+	if (*line == 0x10) {
+		*ctrl = 0x00e28c;
+		*line = 0;
+		*indx = 0;
+	} else {
+		NV_ERROR(dev, "unknown pwm ctrl for gpio %d\n", *line);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+int
+nv50_pm_pwm_get(struct drm_device *dev, int line, u32 *divs, u32 *duty)
+{
+	int ctrl, id, ret = pwm_info(dev, &line, &ctrl, &id);
+	if (ret)
+		return ret;
+
+	if (nv_rd32(dev, ctrl) & (1 << line)) {
+		*divs = nv_rd32(dev, 0x00e114 + (id * 8));
+		*duty = nv_rd32(dev, 0x00e118 + (id * 8));
+		return 0;
 	}
 
-	kfree(state);
+	return -EINVAL;
 }
 
+int
+nv50_pm_pwm_set(struct drm_device *dev, int line, u32 divs, u32 duty)
+{
+	int ctrl, id, ret = pwm_info(dev, &line, &ctrl, &id);
+	if (ret)
+		return ret;
+
+	nv_mask(dev, ctrl, 0x00010001 << line, 0x00000001 << line);
+	nv_wr32(dev, 0x00e114 + (id * 8), divs);
+	nv_wr32(dev, 0x00e118 + (id * 8), duty | 0x80000000);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv50_sor.c b/drivers/gpu/drm/nouveau/nv50_sor.c
index 2633aa8554eb..c4423ba9c9bf 100644
--- a/drivers/gpu/drm/nouveau/nv50_sor.c
+++ b/drivers/gpu/drm/nouveau/nv50_sor.c
@@ -60,6 +60,8 @@ nv50_sor_disconnect(struct drm_encoder *encoder)
 	BEGIN_RING(evo, 0, NV50_EVO_UPDATE, 1);
 	OUT_RING  (evo, 0);
 
+	nouveau_hdmi_mode_set(encoder, NULL);
+
 	nv_encoder->crtc = NULL;
 	nv_encoder->last_dpms = DRM_MODE_DPMS_OFF;
 }
@@ -172,6 +174,12 @@ nv50_sor_mode_fixup(struct drm_encoder *encoder, struct drm_display_mode *mode,
 static void
 nv50_sor_prepare(struct drm_encoder *encoder)
 {
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	nv50_sor_disconnect(encoder);
+	if (nv_encoder->dcb->type == OUTPUT_DP) {
+		/* avoid race between link training and supervisor intr */
+		nv50_display_sync(encoder->dev);
+	}
 }
 
 static void
@@ -180,8 +188,8 @@ nv50_sor_commit(struct drm_encoder *encoder)
 }
 
 static void
-nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
-		  struct drm_display_mode *adjusted_mode)
+nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
+		  struct drm_display_mode *mode)
 {
 	struct nouveau_channel *evo = nv50_display(encoder->dev)->master;
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
@@ -193,24 +201,27 @@ nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 
 	NV_DEBUG_KMS(dev, "or %d type %d -> crtc %d\n",
 		     nv_encoder->or, nv_encoder->dcb->type, crtc->index);
+	nv_encoder->crtc = encoder->crtc;
 
 	switch (nv_encoder->dcb->type) {
 	case OUTPUT_TMDS:
 		if (nv_encoder->dcb->sorconf.link & 1) {
-			if (adjusted_mode->clock < 165000)
+			if (mode->clock < 165000)
 				mode_ctl = 0x0100;
 			else
 				mode_ctl = 0x0500;
 		} else
 			mode_ctl = 0x0200;
+
+		nouveau_hdmi_mode_set(encoder, mode);
 		break;
 	case OUTPUT_DP:
 		nv_connector = nouveau_encoder_connector_get(nv_encoder);
 		if (nv_connector && nv_connector->base.display_info.bpc == 6) {
-			nv_encoder->dp.datarate = crtc->mode->clock * 18 / 8;
+			nv_encoder->dp.datarate = mode->clock * 18 / 8;
 			mode_ctl |= 0x00020000;
 		} else {
-			nv_encoder->dp.datarate = crtc->mode->clock * 24 / 8;
+			nv_encoder->dp.datarate = mode->clock * 24 / 8;
 			mode_ctl |= 0x00050000;
 		}
 
@@ -228,10 +239,10 @@ nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 	else
 		mode_ctl |= NV50_EVO_SOR_MODE_CTRL_CRTC0;
 
-	if (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC)
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
 		mode_ctl |= NV50_EVO_SOR_MODE_CTRL_NHSYNC;
 
-	if (adjusted_mode->flags & DRM_MODE_FLAG_NVSYNC)
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
 		mode_ctl |= NV50_EVO_SOR_MODE_CTRL_NVSYNC;
 
 	nv50_sor_dpms(encoder, DRM_MODE_DPMS_ON);
@@ -239,12 +250,11 @@ nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 	ret = RING_SPACE(evo, 2);
 	if (ret) {
 		NV_ERROR(dev, "no space while connecting SOR\n");
+		nv_encoder->crtc = NULL;
 		return;
 	}
 	BEGIN_RING(evo, 0, NV50_EVO_SOR(nv_encoder->or, MODE_CTRL), 1);
 	OUT_RING(evo, mode_ctl);
-
-	nv_encoder->crtc = encoder->crtc;
 }
 
 static struct drm_crtc *
diff --git a/drivers/gpu/drm/nouveau/nv50_vm.c b/drivers/gpu/drm/nouveau/nv50_vm.c
index 40b84f22d819..6f38ceae3aa4 100644
--- a/drivers/gpu/drm/nouveau/nv50_vm.c
+++ b/drivers/gpu/drm/nouveau/nv50_vm.c
@@ -48,7 +48,7 @@ nv50_vm_map_pgt(struct nouveau_gpuobj *pgd, u32 pde,
 			phys |= 0x60;
 		else if (coverage <= 64 * 1024 * 1024)
 			phys |= 0x40;
-		else if (coverage < 128 * 1024 * 1024)
+		else if (coverage <= 128 * 1024 * 1024)
 			phys |= 0x20;
 	}
 
diff --git a/drivers/gpu/drm/nouveau/nv84_bsp.c b/drivers/gpu/drm/nouveau/nv84_bsp.c
new file mode 100644
index 000000000000..74875739bcc0
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv84_bsp.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_util.h"
+#include "nouveau_vm.h"
+#include "nouveau_ramht.h"
+
+/*XXX: This stub is currently used on NV98+ also, as soon as this becomes
+ *     more than just an enable/disable stub this needs to be split out to
+ *     nv98_bsp.c...
+ */
+
+struct nv84_bsp_engine {
+	struct nouveau_exec_engine base;
+};
+
+static int
+nv84_bsp_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	if (!(nv_rd32(dev, 0x000200) & 0x00008000))
+		return 0;
+
+	nv_mask(dev, 0x000200, 0x00008000, 0x00000000);
+	return 0;
+}
+
+static int
+nv84_bsp_init(struct drm_device *dev, int engine)
+{
+	nv_mask(dev, 0x000200, 0x00008000, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00008000, 0x00008000);
+	return 0;
+}
+
+static void
+nv84_bsp_destroy(struct drm_device *dev, int engine)
+{
+	struct nv84_bsp_engine *pbsp = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, BSP);
+
+	kfree(pbsp);
+}
+
+int
+nv84_bsp_create(struct drm_device *dev)
+{
+	struct nv84_bsp_engine *pbsp;
+
+	pbsp = kzalloc(sizeof(*pbsp), GFP_KERNEL);
+	if (!pbsp)
+		return -ENOMEM;
+
+	pbsp->base.destroy = nv84_bsp_destroy;
+	pbsp->base.init = nv84_bsp_init;
+	pbsp->base.fini = nv84_bsp_fini;
+
+	NVOBJ_ENGINE_ADD(dev, BSP, &pbsp->base);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv84_vp.c b/drivers/gpu/drm/nouveau/nv84_vp.c
new file mode 100644
index 000000000000..6570d300ab85
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv84_vp.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_util.h"
+#include "nouveau_vm.h"
+#include "nouveau_ramht.h"
+
+/*XXX: This stub is currently used on NV98+ also, as soon as this becomes
+ *     more than just an enable/disable stub this needs to be split out to
+ *     nv98_vp.c...
+ */
+
+struct nv84_vp_engine {
+	struct nouveau_exec_engine base;
+};
+
+static int
+nv84_vp_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	if (!(nv_rd32(dev, 0x000200) & 0x00020000))
+		return 0;
+
+	nv_mask(dev, 0x000200, 0x00020000, 0x00000000);
+	return 0;
+}
+
+static int
+nv84_vp_init(struct drm_device *dev, int engine)
+{
+	nv_mask(dev, 0x000200, 0x00020000, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00020000, 0x00020000);
+	return 0;
+}
+
+static void
+nv84_vp_destroy(struct drm_device *dev, int engine)
+{
+	struct nv84_vp_engine *pvp = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, VP);
+
+	kfree(pvp);
+}
+
+int
+nv84_vp_create(struct drm_device *dev)
+{
+	struct nv84_vp_engine *pvp;
+
+	pvp = kzalloc(sizeof(*pvp), GFP_KERNEL);
+	if (!pvp)
+		return -ENOMEM;
+
+	pvp->base.destroy = nv84_vp_destroy;
+	pvp->base.init = nv84_vp_init;
+	pvp->base.fini = nv84_vp_fini;
+
+	NVOBJ_ENGINE_ADD(dev, VP, &pvp->base);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv98_crypt.c b/drivers/gpu/drm/nouveau/nv98_crypt.c
new file mode 100644
index 000000000000..db94ff0a9fab
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv98_crypt.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_util.h"
+#include "nouveau_vm.h"
+#include "nouveau_ramht.h"
+
+struct nv98_crypt_engine {
+	struct nouveau_exec_engine base;
+};
+
+static int
+nv98_crypt_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	if (!(nv_rd32(dev, 0x000200) & 0x00004000))
+		return 0;
+
+	nv_mask(dev, 0x000200, 0x00004000, 0x00000000);
+	return 0;
+}
+
+static int
+nv98_crypt_init(struct drm_device *dev, int engine)
+{
+	nv_mask(dev, 0x000200, 0x00004000, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00004000, 0x00004000);
+	return 0;
+}
+
+static void
+nv98_crypt_destroy(struct drm_device *dev, int engine)
+{
+	struct nv98_crypt_engine *pcrypt = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, CRYPT);
+
+	kfree(pcrypt);
+}
+
+int
+nv98_crypt_create(struct drm_device *dev)
+{
+	struct nv98_crypt_engine *pcrypt;
+
+	pcrypt = kzalloc(sizeof(*pcrypt), GFP_KERNEL);
+	if (!pcrypt)
+		return -ENOMEM;
+
+	pcrypt->base.destroy = nv98_crypt_destroy;
+	pcrypt->base.init = nv98_crypt_init;
+	pcrypt->base.fini = nv98_crypt_fini;
+
+	NVOBJ_ENGINE_ADD(dev, CRYPT, &pcrypt->base);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nv98_ppp.c b/drivers/gpu/drm/nouveau/nv98_ppp.c
new file mode 100644
index 000000000000..a987dd6e0036
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nv98_ppp.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include "drmP.h"
+#include "nouveau_drv.h"
+#include "nouveau_util.h"
+#include "nouveau_vm.h"
+#include "nouveau_ramht.h"
+
+struct nv98_ppp_engine {
+	struct nouveau_exec_engine base;
+};
+
+static int
+nv98_ppp_fini(struct drm_device *dev, int engine, bool suspend)
+{
+	if (!(nv_rd32(dev, 0x000200) & 0x00000002))
+		return 0;
+
+	nv_mask(dev, 0x000200, 0x00000002, 0x00000000);
+	return 0;
+}
+
+static int
+nv98_ppp_init(struct drm_device *dev, int engine)
+{
+	nv_mask(dev, 0x000200, 0x00000002, 0x00000000);
+	nv_mask(dev, 0x000200, 0x00000002, 0x00000002);
+	return 0;
+}
+
+static void
+nv98_ppp_destroy(struct drm_device *dev, int engine)
+{
+	struct nv98_ppp_engine *pppp = nv_engine(dev, engine);
+
+	NVOBJ_ENGINE_DEL(dev, PPP);
+
+	kfree(pppp);
+}
+
+int
+nv98_ppp_create(struct drm_device *dev)
+{
+	struct nv98_ppp_engine *pppp;
+
+	pppp = kzalloc(sizeof(*pppp), GFP_KERNEL);
+	if (!pppp)
+		return -ENOMEM;
+
+	pppp->base.destroy = nv98_ppp_destroy;
+	pppp->base.init = nv98_ppp_init;
+	pppp->base.fini = nv98_ppp_fini;
+
+	NVOBJ_ENGINE_ADD(dev, PPP, &pppp->base);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nva3_copy.fuc b/drivers/gpu/drm/nouveau/nva3_copy.fuc
index eaf35f8321ee..abc36626fef0 100644
--- a/drivers/gpu/drm/nouveau/nva3_copy.fuc
+++ b/drivers/gpu/drm/nouveau/nva3_copy.fuc
@@ -31,8 +31,9 @@
  */
 
 ifdef(`NVA3',
-.section nva3_pcopy_data,
-.section nvc0_pcopy_data
+.section #nva3_pcopy_data
+,
+.section #nvc0_pcopy_data
 )
 
 ctx_object:                   .b32 0
@@ -42,7 +43,7 @@ ctx_dma_query:                .b32 0
 ctx_dma_src:                  .b32 0
 ctx_dma_dst:                  .b32 0
 ,)
-.equ ctx_dma_count 3
+.equ #ctx_dma_count 3
 ctx_query_address_high:       .b32 0
 ctx_query_address_low:        .b32 0
 ctx_query_counter:            .b32 0
@@ -78,64 +79,65 @@ ctx_ycnt:                     .b32 0
 dispatch_table:
 // mthd 0x0000, NAME
 .b16 0x000 1
-.b32 ctx_object                     ~0xffffffff
+.b32 #ctx_object                     ~0xffffffff
 // mthd 0x0100, NOP
 .b16 0x040 1
-.b32 0x00010000 + cmd_nop           ~0xffffffff
+.b32 0x00010000 + #cmd_nop           ~0xffffffff
 // mthd 0x0140, PM_TRIGGER
 .b16 0x050 1
-.b32 0x00010000 + cmd_pm_trigger    ~0xffffffff
+.b32 0x00010000 + #cmd_pm_trigger    ~0xffffffff
 ifdef(`NVA3', `
 // mthd 0x0180-0x018c, DMA_
-.b16 0x060 ctx_dma_count
+.b16 0x060 #ctx_dma_count
 dispatch_dma:
-.b32 0x00010000 + cmd_dma           ~0xffffffff
-.b32 0x00010000 + cmd_dma           ~0xffffffff
-.b32 0x00010000 + cmd_dma           ~0xffffffff
+.b32 0x00010000 + #cmd_dma           ~0xffffffff
+.b32 0x00010000 + #cmd_dma           ~0xffffffff
+.b32 0x00010000 + #cmd_dma           ~0xffffffff
 ',)
 // mthd 0x0200-0x0218, SRC_TILE
 .b16 0x80 7
-.b32 ctx_src_tile_mode              ~0x00000fff
-.b32 ctx_src_xsize                  ~0x0007ffff
-.b32 ctx_src_ysize                  ~0x00001fff
-.b32 ctx_src_zsize                  ~0x000007ff
-.b32 ctx_src_zoff                   ~0x00000fff
-.b32 ctx_src_xoff                   ~0x0007ffff
-.b32 ctx_src_yoff                   ~0x00001fff
+.b32 #ctx_src_tile_mode              ~0x00000fff
+.b32 #ctx_src_xsize                  ~0x0007ffff
+.b32 #ctx_src_ysize                  ~0x00001fff
+.b32 #ctx_src_zsize                  ~0x000007ff
+.b32 #ctx_src_zoff                   ~0x00000fff
+.b32 #ctx_src_xoff                   ~0x0007ffff
+.b32 #ctx_src_yoff                   ~0x00001fff
 // mthd 0x0220-0x0238, DST_TILE
 .b16 0x88 7
-.b32 ctx_dst_tile_mode              ~0x00000fff
-.b32 ctx_dst_xsize                  ~0x0007ffff
-.b32 ctx_dst_ysize                  ~0x00001fff
-.b32 ctx_dst_zsize                  ~0x000007ff
-.b32 ctx_dst_zoff                   ~0x00000fff
-.b32 ctx_dst_xoff                   ~0x0007ffff
-.b32 ctx_dst_yoff                   ~0x00001fff
+.b32 #ctx_dst_tile_mode              ~0x00000fff
+.b32 #ctx_dst_xsize                  ~0x0007ffff
+.b32 #ctx_dst_ysize                  ~0x00001fff
+.b32 #ctx_dst_zsize                  ~0x000007ff
+.b32 #ctx_dst_zoff                   ~0x00000fff
+.b32 #ctx_dst_xoff                   ~0x0007ffff
+.b32 #ctx_dst_yoff                   ~0x00001fff
 // mthd 0x0300-0x0304, EXEC, WRCACHE_FLUSH
 .b16 0xc0 2
-.b32 0x00010000 + cmd_exec          ~0xffffffff
-.b32 0x00010000 + cmd_wrcache_flush ~0xffffffff
+.b32 0x00010000 + #cmd_exec          ~0xffffffff
+.b32 0x00010000 + #cmd_wrcache_flush ~0xffffffff
 // mthd 0x030c-0x0340, various stuff
 .b16 0xc3 14
-.b32 ctx_src_address_high           ~0x000000ff
-.b32 ctx_src_address_low            ~0xfffffff0
-.b32 ctx_dst_address_high           ~0x000000ff
-.b32 ctx_dst_address_low            ~0xfffffff0
-.b32 ctx_src_pitch                  ~0x0007ffff
-.b32 ctx_dst_pitch                  ~0x0007ffff
-.b32 ctx_xcnt                       ~0x0000ffff
-.b32 ctx_ycnt                       ~0x00001fff
-.b32 ctx_format                     ~0x0333ffff
-.b32 ctx_swz_const0                 ~0xffffffff
-.b32 ctx_swz_const1                 ~0xffffffff
-.b32 ctx_query_address_high         ~0x000000ff
-.b32 ctx_query_address_low          ~0xffffffff
-.b32 ctx_query_counter              ~0xffffffff
+.b32 #ctx_src_address_high           ~0x000000ff
+.b32 #ctx_src_address_low            ~0xfffffff0
+.b32 #ctx_dst_address_high           ~0x000000ff
+.b32 #ctx_dst_address_low            ~0xfffffff0
+.b32 #ctx_src_pitch                  ~0x0007ffff
+.b32 #ctx_dst_pitch                  ~0x0007ffff
+.b32 #ctx_xcnt                       ~0x0000ffff
+.b32 #ctx_ycnt                       ~0x00001fff
+.b32 #ctx_format                     ~0x0333ffff
+.b32 #ctx_swz_const0                 ~0xffffffff
+.b32 #ctx_swz_const1                 ~0xffffffff
+.b32 #ctx_query_address_high         ~0x000000ff
+.b32 #ctx_query_address_low          ~0xffffffff
+.b32 #ctx_query_counter              ~0xffffffff
 .b16 0x800 0
 
 ifdef(`NVA3',
-.section nva3_pcopy_code,
-.section nvc0_pcopy_code
+.section #nva3_pcopy_code
+,
+.section #nvc0_pcopy_code
 )
 
 main:
@@ -143,12 +145,12 @@ main:
    mov $sp $r0
 
    // setup i0 handler and route fifo and ctxswitch to it
-   mov $r1 ih
+   mov $r1 #ih
    mov $iv0 $r1
    mov $r1 0x400
    movw $r2 0xfff3
    sethi $r2 0
-   iowr I[$r2 + 0x300] $r2
+   iowr I[$r1 + 0x300] $r2
 
    // enable interrupts
    or $r2 0xc
@@ -164,19 +166,19 @@ main:
    bset $flags $p0
    spin:
       sleep $p0
-      bra spin
+      bra #spin
 
 // i0 handler
 ih:
    iord $r1 I[$r0 + 0x200]
 
    and $r2 $r1 0x00000008
-   bra e ih_no_chsw
-      call chsw
+   bra e #ih_no_chsw
+      call #chsw
    ih_no_chsw:
    and $r2 $r1 0x00000004
-   bra e ih_no_cmd
-      call dispatch
+   bra e #ih_no_cmd
+      call #dispatch
 
    ih_no_cmd:
    and $r1 $r1 0x0000000c
@@ -235,9 +237,9 @@ ifdef(`NVA3', `
    sethi $r4 0x60000
 
    // swap!
-   bra $p1 swctx_load
+   bra $p1 #swctx_load
       xdst $r0 $r4
-      bra swctx_done
+      bra #swctx_done
    swctx_load:
       xdld $r0 $r4
    swctx_done:
@@ -251,9 +253,9 @@ chsw:
 
    // if it's active, unload it and return
    xbit $r15 $r3 0x1e
-   bra e chsw_no_unload
+   bra e #chsw_no_unload
       bclr $flags $p1
-      call swctx
+      call #swctx
       bclr $r3 0x1e
       iowr I[$r2] $r3
       mov $r4 1
@@ -266,20 +268,20 @@ chsw:
 
    // is there a channel waiting to be loaded?
    xbit $r13 $r3 0x1e
-   bra e chsw_finish_load
+   bra e #chsw_finish_load
       bset $flags $p1
-      call swctx
+      call #swctx
 ifdef(`NVA3',
       // load dma objects back into TARGET regs
-      mov $r5 ctx_dma
-      mov $r6 ctx_dma_count
+      mov $r5 #ctx_dma
+      mov $r6 #ctx_dma_count
       chsw_load_ctx_dma:
          ld b32 $r7 D[$r5 + $r6 * 4]
          add b32 $r8 $r6 0x180
          shl b32 $r8 8
          iowr I[$r8] $r7
          sub b32 $r6 1
-         bra nc chsw_load_ctx_dma
+         bra nc #chsw_load_ctx_dma
 ,)
 
    chsw_finish_load:
@@ -297,7 +299,7 @@ dispatch:
    shl b32 $r2 0x10
 
    // lookup method in the dispatch table, ILLEGAL_MTHD if not found
-   mov $r5 dispatch_table
+   mov $r5 #dispatch_table
    clear b32 $r6
    clear b32 $r7
    dispatch_loop:
@@ -305,14 +307,14 @@ dispatch:
       ld b16 $r7 D[$r5 + 2]
       add b32 $r5 4
       cmpu b32 $r4 $r6
-      bra c dispatch_illegal_mthd
+      bra c #dispatch_illegal_mthd
       add b32 $r7 $r6
       cmpu b32 $r4 $r7
-      bra c dispatch_valid_mthd
+      bra c #dispatch_valid_mthd
       sub b32 $r7 $r6
       shl b32 $r7 3
       add b32 $r5 $r7
-      bra dispatch_loop
+      bra #dispatch_loop
 
    // ensure no bits set in reserved fields, INVALID_BITFIELD
    dispatch_valid_mthd:
@@ -322,20 +324,20 @@ dispatch:
    ld b32 $r5 D[$r4 + 4]
    and $r5 $r3
    cmpu b32 $r5 0
-   bra ne dispatch_invalid_bitfield
+   bra ne #dispatch_invalid_bitfield
 
    // depending on dispatch flags: execute method, or save data as state
    ld b16 $r5 D[$r4 + 0]
    ld b16 $r6 D[$r4 + 2]
    cmpu b32 $r6 0
-   bra ne dispatch_cmd
+   bra ne #dispatch_cmd
       st b32 D[$r5] $r3
-      bra dispatch_done
+      bra #dispatch_done
    dispatch_cmd:
       bclr $flags $p1
       call $r5
-      bra $p1 dispatch_error
-      bra dispatch_done
+      bra $p1 #dispatch_error
+      bra #dispatch_done
 
    dispatch_invalid_bitfield:
    or $r2 2
@@ -353,7 +355,7 @@ dispatch:
       iord $r2 I[$r0 + 0x200]
       and $r2 0x40
       cmpu b32 $r2 0
-      bra ne hostirq_wait
+      bra ne #hostirq_wait
 
    dispatch_done:
    mov $r2 0x1d00
@@ -409,10 +411,10 @@ ifdef(`NVA3',
 //       $r2: hostirq state
 //       $r3: data
 cmd_dma:
-   sub b32 $r4 dispatch_dma
+   sub b32 $r4 #dispatch_dma
    shr b32 $r4 1
    bset $r3 0x1e
-   st b32 D[$r4 + ctx_dma] $r3
+   st b32 D[$r4 + #ctx_dma] $r3
    add b32 $r4 0x600
    shl b32 $r4 6
    iowr I[$r4] $r3
@@ -430,7 +432,7 @@ cmd_exec_set_format:
    st b32 D[$sp + 0x0c] $r0
 
    // extract cpp, src_ncomp and dst_ncomp from FORMAT
-   ld b32 $r4 D[$r0 + ctx_format]
+   ld b32 $r4 D[$r0 + #ctx_format]
    extr $r5 $r4 16:17
    add b32 $r5 1
    extr $r6 $r4 20:21
@@ -448,22 +450,22 @@ cmd_exec_set_format:
       clear b32 $r11
       bpc_loop:
          cmpu b8 $r10 4
-         bra nc cmp_c0
+         bra nc #cmp_c0
             mulu $r12 $r10 $r5
             add b32 $r12 $r11
             bset $flags $p2
-            bra bpc_next
+            bra #bpc_next
          cmp_c0:
-         bra ne cmp_c1
+         bra ne #cmp_c1
             mov $r12 0x10
             add b32 $r12 $r11
-            bra bpc_next
+            bra #bpc_next
          cmp_c1:
          cmpu b8 $r10 6
-         bra nc cmp_zero
+         bra nc #cmp_zero
             mov $r12 0x14
             add b32 $r12 $r11
-            bra bpc_next
+            bra #bpc_next
          cmp_zero:
             mov $r12 0x80
          bpc_next:
@@ -471,22 +473,22 @@ cmd_exec_set_format:
          add b32 $r8 1
          add b32 $r11 1
          cmpu b32 $r11 $r5
-         bra c bpc_loop
+         bra c #bpc_loop
       add b32 $r9 1
       cmpu b32 $r9 $r7
-      bra c ncomp_loop
+      bra c #ncomp_loop
 
    // SRC_XCNT = (xcnt * src_cpp), or 0 if no src ref in swz (hw will hang)
    mulu $r6 $r5
-   st b32 D[$r0 + ctx_src_cpp] $r6
-   ld b32 $r8 D[$r0 + ctx_xcnt]
+   st b32 D[$r0 + #ctx_src_cpp] $r6
+   ld b32 $r8 D[$r0 + #ctx_xcnt]
    mulu $r6 $r8
-   bra $p2 dst_xcnt
+   bra $p2 #dst_xcnt
    clear b32 $r6
 
    dst_xcnt:
    mulu $r7 $r5
-   st b32 D[$r0 + ctx_dst_cpp] $r7
+   st b32 D[$r0 + #ctx_dst_cpp] $r7
    mulu $r7 $r8
 
    mov $r5 0x810
@@ -494,10 +496,10 @@ cmd_exec_set_format:
    iowr I[$r5 + 0x000] $r6
    iowr I[$r5 + 0x100] $r7
    add b32 $r5 0x800
-   ld b32 $r6 D[$r0 + ctx_dst_cpp]
+   ld b32 $r6 D[$r0 + #ctx_dst_cpp]
    sub b32 $r6 1
    shl b32 $r6 8
-   ld b32 $r7 D[$r0 + ctx_src_cpp]
+   ld b32 $r7 D[$r0 + #ctx_src_cpp]
    sub b32 $r7 1
    or $r6 $r7
    iowr I[$r5 + 0x000] $r6
@@ -511,9 +513,9 @@ cmd_exec_set_format:
    ld b32 $r6 D[$sp + 0x0c]
    iowr I[$r5 + 0x300] $r6
    add b32 $r5 0x400
-   ld b32 $r6 D[$r0 + ctx_swz_const0]
+   ld b32 $r6 D[$r0 + #ctx_swz_const0]
    iowr I[$r5 + 0x000] $r6
-   ld b32 $r6 D[$r0 + ctx_swz_const1]
+   ld b32 $r6 D[$r0 + #ctx_swz_const1]
    iowr I[$r5 + 0x100] $r6
    add $sp 0x10
    ret
@@ -543,7 +545,7 @@ cmd_exec_set_format:
 //
 cmd_exec_set_surface_tiled:
    // translate TILE_MODE into Tp, Th, Td shift values
-   ld b32 $r7 D[$r5 + ctx_src_tile_mode]
+   ld b32 $r7 D[$r5 + #ctx_src_tile_mode]
    extr $r9 $r7 8:11
    extr $r8 $r7 4:7
 ifdef(`NVA3',
@@ -553,9 +555,9 @@ ifdef(`NVA3',
 )
    extr $r7 $r7 0:3
    cmp b32 $r7 0xe
-   bra ne xtile64
+   bra ne #xtile64
    mov $r7 4
-   bra xtileok
+   bra #xtileok
    xtile64:
    xbit $r7 $flags $p2
    add b32 $r7 17
@@ -565,8 +567,8 @@ ifdef(`NVA3',
 
    // Op = (x * cpp) & ((1 << Tp) - 1)
    // Tx = (x * cpp) >> Tp
-   ld b32 $r10 D[$r5 + ctx_src_xoff]
-   ld b32 $r11 D[$r5 + ctx_src_cpp]
+   ld b32 $r10 D[$r5 + #ctx_src_xoff]
+   ld b32 $r11 D[$r5 + #ctx_src_cpp]
    mulu $r10 $r11
    mov $r11 1
    shl b32 $r11 $r7
@@ -576,7 +578,7 @@ ifdef(`NVA3',
 
    // Tyo = y & ((1 << Th) - 1)
    // Ty  = y >> Th
-   ld b32 $r13 D[$r5 + ctx_src_yoff]
+   ld b32 $r13 D[$r5 + #ctx_src_yoff]
    mov $r14 1
    shl b32 $r14 $r8
    sub b32 $r14 1
@@ -598,8 +600,8 @@ ifdef(`NVA3',
    add b32 $r12 $r11
 
    // nTx = ((w * cpp) + ((1 << Tp) - 1) >> Tp)
-   ld b32 $r15 D[$r5 + ctx_src_xsize]
-   ld b32 $r11 D[$r5 + ctx_src_cpp]
+   ld b32 $r15 D[$r5 + #ctx_src_xsize]
+   ld b32 $r11 D[$r5 + #ctx_src_cpp]
    mulu $r15 $r11
    mov $r11 1
    shl b32 $r11 $r7
@@ -609,7 +611,7 @@ ifdef(`NVA3',
    push $r15
 
    // nTy = (h + ((1 << Th) - 1)) >> Th
-   ld b32 $r15 D[$r5 + ctx_src_ysize]
+   ld b32 $r15 D[$r5 + #ctx_src_ysize]
    mov $r11 1
    shl b32 $r11 $r8
    sub b32 $r11 1
@@ -629,7 +631,7 @@ ifdef(`NVA3',
    // Tz  = z >> Td
    // Op += Tzo << Tys
    // Ts  = Tys + Td
-   ld b32 $r8 D[$r5 + ctx_src_zoff]
+   ld b32 $r8 D[$r5 + #ctx_src_zoff]
    mov $r14 1
    shl b32 $r14 $r9
    sub b32 $r14 1
@@ -656,8 +658,8 @@ ifdef(`NVA3',
 
    // SRC_ADDRESS_LOW   = (Ot + Op) & 0xffffffff
    // CFG_ADDRESS_HIGH |= ((Ot + Op) >> 32) << 16
-   ld b32 $r7 D[$r5 + ctx_src_address_low]
-   ld b32 $r8 D[$r5 + ctx_src_address_high]
+   ld b32 $r7 D[$r5 + #ctx_src_address_low]
+   ld b32 $r8 D[$r5 + #ctx_src_address_high]
    add b32 $r10 $r12
    add b32 $r7 $r10
    adc b32 $r8 0
@@ -677,14 +679,14 @@ cmd_exec_set_surface_linear:
    xbit $r6 $flags $p2
    add b32 $r6 0x202
    shl b32 $r6 8
-   ld b32 $r7 D[$r5 + ctx_src_address_low]
+   ld b32 $r7 D[$r5 + #ctx_src_address_low]
    iowr I[$r6 + 0x000] $r7
    add b32 $r6 0x400
-   ld b32 $r7 D[$r5 + ctx_src_address_high]
+   ld b32 $r7 D[$r5 + #ctx_src_address_high]
    shl b32 $r7 16
    iowr I[$r6 + 0x000] $r7
    add b32 $r6 0x400
-   ld b32 $r7 D[$r5 + ctx_src_pitch]
+   ld b32 $r7 D[$r5 + #ctx_src_pitch]
    iowr I[$r6 + 0x000] $r7
    ret
 
@@ -697,7 +699,7 @@ cmd_exec_wait:
    loop:
       iord $r1 I[$r0]
       and $r1 1
-      bra ne loop
+      bra ne #loop
    pop $r1
    pop $r0
    ret
@@ -705,18 +707,18 @@ cmd_exec_wait:
 cmd_exec_query:
    // if QUERY_SHORT not set, write out { -, 0, TIME_LO, TIME_HI }
    xbit $r4 $r3 13
-   bra ne query_counter
-      call cmd_exec_wait
+   bra ne #query_counter
+      call #cmd_exec_wait
       mov $r4 0x80c
       shl b32 $r4 6
-      ld b32 $r5 D[$r0 + ctx_query_address_low]
+      ld b32 $r5 D[$r0 + #ctx_query_address_low]
       add b32 $r5 4
       iowr I[$r4 + 0x000] $r5
       iowr I[$r4 + 0x100] $r0
       mov $r5 0xc
       iowr I[$r4 + 0x200] $r5
       add b32 $r4 0x400
-      ld b32 $r5 D[$r0 + ctx_query_address_high]
+      ld b32 $r5 D[$r0 + #ctx_query_address_high]
       shl b32 $r5 16
       iowr I[$r4 + 0x000] $r5
       add b32 $r4 0x500
@@ -741,16 +743,16 @@ cmd_exec_query:
 
    // write COUNTER
    query_counter:
-   call cmd_exec_wait
+   call #cmd_exec_wait
    mov $r4 0x80c
    shl b32 $r4 6
-   ld b32 $r5 D[$r0 + ctx_query_address_low]
+   ld b32 $r5 D[$r0 + #ctx_query_address_low]
    iowr I[$r4 + 0x000] $r5
    iowr I[$r4 + 0x100] $r0
    mov $r5 0x4
    iowr I[$r4 + 0x200] $r5
    add b32 $r4 0x400
-   ld b32 $r5 D[$r0 + ctx_query_address_high]
+   ld b32 $r5 D[$r0 + #ctx_query_address_high]
    shl b32 $r5 16
    iowr I[$r4 + 0x000] $r5
    add b32 $r4 0x500
@@ -759,7 +761,7 @@ cmd_exec_query:
    mov $r5 0x00001110
    sethi $r5 0x13120000
    iowr I[$r4 + 0x100] $r5
-   ld b32 $r5 D[$r0 + ctx_query_counter]
+   ld b32 $r5 D[$r0 + #ctx_query_counter]
    add b32 $r4 0x500
    iowr I[$r4 + 0x000] $r5
    mov $r5 0x00002601
@@ -787,22 +789,22 @@ cmd_exec_query:
 //       $r2: hostirq state
 //       $r3: data
 cmd_exec:
-   call cmd_exec_wait
+   call #cmd_exec_wait
 
    // if format requested, call function to calculate it, otherwise
    // fill in cpp/xcnt for both surfaces as if (cpp == 1)
    xbit $r15 $r3 0
-   bra e cmd_exec_no_format
-      call cmd_exec_set_format
+   bra e #cmd_exec_no_format
+      call #cmd_exec_set_format
       mov $r4 0x200
-      bra cmd_exec_init_src_surface
+      bra #cmd_exec_init_src_surface
    cmd_exec_no_format:
       mov $r6 0x810
       shl b32 $r6 6
       mov $r7 1
-      st b32 D[$r0 + ctx_src_cpp] $r7
-      st b32 D[$r0 + ctx_dst_cpp] $r7
-      ld b32 $r7 D[$r0 + ctx_xcnt]
+      st b32 D[$r0 + #ctx_src_cpp] $r7
+      st b32 D[$r0 + #ctx_dst_cpp] $r7
+      ld b32 $r7 D[$r0 + #ctx_xcnt]
       iowr I[$r6 + 0x000] $r7
       iowr I[$r6 + 0x100] $r7
       clear b32 $r4
@@ -811,28 +813,28 @@ cmd_exec:
    bclr $flags $p2
    clear b32 $r5
    xbit $r15 $r3 4
-   bra e src_tiled
-      call cmd_exec_set_surface_linear
-      bra cmd_exec_init_dst_surface
+   bra e #src_tiled
+      call #cmd_exec_set_surface_linear
+      bra #cmd_exec_init_dst_surface
    src_tiled:
-      call cmd_exec_set_surface_tiled
+      call #cmd_exec_set_surface_tiled
       bset $r4 7
 
    cmd_exec_init_dst_surface:
    bset $flags $p2
-   mov $r5 ctx_dst_address_high - ctx_src_address_high
+   mov $r5 #ctx_dst_address_high - #ctx_src_address_high
    xbit $r15 $r3 8
-   bra e dst_tiled
-      call cmd_exec_set_surface_linear
-      bra cmd_exec_kick
+   bra e #dst_tiled
+      call #cmd_exec_set_surface_linear
+      bra #cmd_exec_kick
    dst_tiled:
-      call cmd_exec_set_surface_tiled
+      call #cmd_exec_set_surface_tiled
       bset $r4 8
 
    cmd_exec_kick:
    mov $r5 0x800
    shl b32 $r5 6
-   ld b32 $r6 D[$r0 + ctx_ycnt]
+   ld b32 $r6 D[$r0 + #ctx_ycnt]
    iowr I[$r5 + 0x100] $r6
    mov $r6 0x0041
    // SRC_TARGET = 1, DST_TARGET = 2
@@ -842,8 +844,8 @@ cmd_exec:
 
    // if requested, queue up a QUERY write after the copy has completed
    xbit $r15 $r3 12
-   bra e cmd_exec_done
-      call cmd_exec_query
+   bra e #cmd_exec_done
+      call #cmd_exec_query
 
    cmd_exec_done:
    ret
diff --git a/drivers/gpu/drm/nouveau/nva3_copy.fuc.h b/drivers/gpu/drm/nouveau/nva3_copy.fuc.h
index 2731de22ebe9..1f33fbdc00be 100644
--- a/drivers/gpu/drm/nouveau/nva3_copy.fuc.h
+++ b/drivers/gpu/drm/nouveau/nva3_copy.fuc.h
@@ -152,7 +152,7 @@ uint32_t nva3_pcopy_code[] = {
 	0xf10010fe,
 	0xf1040017,
 	0xf0fff327,
-	0x22d00023,
+	0x12d00023,
 	0x0c25f0c0,
 	0xf40012d0,
 	0x17f11031,
diff --git a/drivers/gpu/drm/nouveau/nva3_pm.c b/drivers/gpu/drm/nouveau/nva3_pm.c
index 618c144b7a30..9e636e6ef6d7 100644
--- a/drivers/gpu/drm/nouveau/nva3_pm.c
+++ b/drivers/gpu/drm/nouveau/nva3_pm.c
@@ -287,12 +287,13 @@ nva3_pm_grcp_idle(void *data)
 	return false;
 }
 
-void
+int
 nva3_pm_clocks_set(struct drm_device *dev, void *pre_state)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nva3_pm_state *info = pre_state;
 	unsigned long flags;
+	int ret = -EAGAIN;
 
 	/* prevent any new grctx switches from starting */
 	spin_lock_irqsave(&dev_priv->context_switch_lock, flags);
@@ -328,6 +329,8 @@ nva3_pm_clocks_set(struct drm_device *dev, void *pre_state)
 		nv_wr32(dev, 0x100210, 0x80000000);
 	}
 
+	ret = 0;
+
 cleanup:
 	/* unfreeze PFIFO */
 	nv_mask(dev, 0x002504, 0x00000001, 0x00000000);
@@ -339,4 +342,5 @@ cleanup:
 		nv_mask(dev, 0x400824, 0x10000000, 0x10000000);
 	spin_unlock_irqrestore(&dev_priv->context_switch_lock, flags);
 	kfree(info);
+	return ret;
 }
diff --git a/drivers/gpu/drm/nouveau/nvc0_copy.fuc.h b/drivers/gpu/drm/nouveau/nvc0_copy.fuc.h
index 419903880e9d..a8d17458ced1 100644
--- a/drivers/gpu/drm/nouveau/nvc0_copy.fuc.h
+++ b/drivers/gpu/drm/nouveau/nvc0_copy.fuc.h
@@ -145,7 +145,7 @@ uint32_t nvc0_pcopy_code[] = {
 	0xf10010fe,
 	0xf1040017,
 	0xf0fff327,
-	0x22d00023,
+	0x12d00023,
 	0x0c25f0c0,
 	0xf40012d0,
 	0x17f11031,
diff --git a/drivers/gpu/drm/nouveau/nvc0_graph.c b/drivers/gpu/drm/nouveau/nvc0_graph.c
index ecfafd70cf0e..8ee3963f9030 100644
--- a/drivers/gpu/drm/nouveau/nvc0_graph.c
+++ b/drivers/gpu/drm/nouveau/nvc0_graph.c
@@ -875,14 +875,16 @@ nvc0_graph_create(struct drm_device *dev)
 	case 0xcf: /* 4/0/0/0, 3 */
 		priv->magic_not_rop_nr = 0x03;
 		break;
+	case 0xd9: /* 1/0/0/0, 1 */
+		priv->magic_not_rop_nr = 0x01;
+		break;
 	}
 
 	if (!priv->magic_not_rop_nr) {
 		NV_ERROR(dev, "PGRAPH: unknown config: %d/%d/%d/%d, %d\n",
 			 priv->tp_nr[0], priv->tp_nr[1], priv->tp_nr[2],
 			 priv->tp_nr[3], priv->rop_nr);
-		/* use 0xc3's values... */
-		priv->magic_not_rop_nr = 0x03;
+		priv->magic_not_rop_nr = 0x00;
 	}
 
 	NVOBJ_CLASS(dev, 0x902d, GR); /* 2D */
diff --git a/drivers/gpu/drm/nouveau/nvc0_graph.fuc b/drivers/gpu/drm/nouveau/nvc0_graph.fuc
index 2a4b6dc8f9de..e6b228844a32 100644
--- a/drivers/gpu/drm/nouveau/nvc0_graph.fuc
+++ b/drivers/gpu/drm/nouveau/nvc0_graph.fuc
@@ -71,9 +71,9 @@ queue_put:
 	ld b32 $r9 D[$r13 + 0x4]	// PUT
 	xor $r8 8
 	cmpu b32 $r8 $r9
-	bra ne queue_put_next
+	bra ne #queue_put_next
 		mov $r15 E_CMD_OVERFLOW
-		call error
+		call #error
 		ret
 
 	// store cmd/data on queue
@@ -104,7 +104,7 @@ queue_get:
 	ld b32 $r8 D[$r13 + 0x0]	// GET
 	ld b32 $r9 D[$r13 + 0x4]	// PUT
 	cmpu b32 $r8 $r9
-	bra e queue_get_done
+	bra e #queue_get_done
 		// fetch first cmd/data pair
 		and $r9 $r8 7
 		shl b32 $r9 3
@@ -135,9 +135,9 @@ nv_rd32:
 	nv_rd32_wait:
 		iord $r12 I[$r11 + 0x000]
 		xbit $r12 $r12 31
-		bra ne nv_rd32_wait
+		bra ne #nv_rd32_wait
 	mov $r10 6			// DONE_MMIO_RD
-	call wait_doneo
+	call #wait_doneo
 	iord $r15 I[$r11 + 0x100]	// MMIO_RDVAL
 	ret
 
@@ -157,7 +157,7 @@ nv_wr32:
 	nv_wr32_wait:
 		iord $r12 I[$r11 + 0x000]
 		xbit $r12 $r12 31
-		bra ne nv_wr32_wait
+		bra ne #nv_wr32_wait
 	ret
 
 // (re)set watchdog timer
@@ -193,7 +193,7 @@ $1:
 		shl b32 $r8 6
 		iord $r8 I[$r8 + 0x000]	// DONE
 		xbit $r8 $r8 $r10
-		bra $2 wait_done_$1
+		bra $2 #wait_done_$1
 	trace_clr(T_WAIT)
 	ret
 ')
@@ -216,7 +216,7 @@ mmctx_size:
 		add b32 $r9 $r8
 		add b32 $r14 4
 		cmpu b32 $r14 $r15
-		bra ne nv_mmctx_size_loop
+		bra ne #nv_mmctx_size_loop
 	mov b32 $r15 $r9
 	ret
 
@@ -238,12 +238,12 @@ mmctx_xfer:
 	shl b32 $r8 6
 	clear b32 $r9
 	or $r11 $r11
-	bra e mmctx_base_disabled
+	bra e #mmctx_base_disabled
 		iowr I[$r8 + 0x000] $r11	// MMCTX_BASE
 		bset $r9 0			// BASE_EN
 	mmctx_base_disabled:
 	or $r14 $r14
-	bra e mmctx_multi_disabled
+	bra e #mmctx_multi_disabled
 		iowr I[$r8 + 0x200] $r14 	// MMCTX_MULTI_STRIDE
 		iowr I[$r8 + 0x300] $r15 	// MMCTX_MULTI_MASK
 		bset $r9 1			// MULTI_EN
@@ -264,7 +264,7 @@ mmctx_xfer:
 		mmctx_wait_free:
 			iord $r14 I[$r8 + 0x000] // MMCTX_CTRL
 			and $r14 0x1f
-			bra e mmctx_wait_free
+			bra e #mmctx_wait_free
 
 		// queue up an entry
 		ld b32 $r14 D[$r12]
@@ -272,19 +272,19 @@ mmctx_xfer:
 		iowr I[$r8 + 0x300] $r14
 		add b32 $r12 4
 		cmpu b32 $r12 $r13
-		bra ne mmctx_exec_loop
+		bra ne #mmctx_exec_loop
 
 	xbit $r11 $r10 2
-	bra ne mmctx_stop
+	bra ne #mmctx_stop
 		// wait for queue to empty
 		mmctx_fini_wait:
 			iord $r11 I[$r8 + 0x000]	// MMCTX_CTRL
 			and $r11 0x1f
 			cmpu b32 $r11 0x10
-			bra ne mmctx_fini_wait
+			bra ne #mmctx_fini_wait
 		mov $r10 2				// DONE_MMCTX
-		call wait_donez
-		bra mmctx_done
+		call #wait_donez
+		bra #mmctx_done
 	mmctx_stop:
 		xbit $r11 $r10 0
 		shl b32 $r11 16			// DIR
@@ -295,7 +295,7 @@ mmctx_xfer:
 			// wait for STOP_TRIGGER to clear
 			iord $r11 I[$r8 + 0x000] // MMCTX_CTRL
 			xbit $r11 $r11 18
-			bra ne mmctx_stop_wait
+			bra ne #mmctx_stop_wait
 	mmctx_done:
 	trace_clr(T_MMCTX)
 	ret
@@ -305,7 +305,7 @@ mmctx_xfer:
 strand_wait:
 	push $r10
 	mov $r10 2
-	call wait_donez
+	call #wait_donez
 	pop $r10
 	ret
 
@@ -316,7 +316,7 @@ strand_pre:
 	sethi $r8 0x20000
 	mov $r9 0xc
 	iowr I[$r8] $r9
-	call strand_wait
+	call #strand_wait
 	ret
 
 // unknown - call after issuing strand commands
@@ -326,7 +326,7 @@ strand_post:
 	sethi $r8 0x20000
 	mov $r9 0xd
 	iowr I[$r8] $r9
-	call strand_wait
+	call #strand_wait
 	ret
 
 // Selects strand set?!
@@ -341,11 +341,11 @@ strand_set:
 	iowr I[$r10 + 0x000] $r12		// 0x93c = 0xf
 	mov $r12 0xb
 	iowr I[$r11 + 0x000] $r12		// 0x928 = 0xb
-	call strand_wait
+	call #strand_wait
 	iowr I[$r10 + 0x000] $r14		// 0x93c = <id>
 	mov $r12 0xa
 	iowr I[$r11 + 0x000] $r12		// 0x928 = 0xa
-	call strand_wait
+	call #strand_wait
 	ret
 
 // Initialise strand context data
@@ -357,22 +357,22 @@ strand_set:
 //
 strand_ctx_init:
 	trace_set(T_STRINIT)
-	call strand_pre
+	call #strand_pre
 	mov $r14 3
-	call strand_set
+	call #strand_set
 	mov $r10 0x46fc
 	sethi $r10 0x20000
 	add b32 $r11 $r10 0x400
 	iowr I[$r10 + 0x100] $r0	// STRAND_FIRST_GENE = 0
 	mov $r12 1
 	iowr I[$r11 + 0x000] $r12	// STRAND_CMD = LATCH_FIRST_GENE
-	call strand_wait
+	call #strand_wait
 	sub b32 $r12 $r0 1
 	iowr I[$r10 + 0x000] $r12	// STRAND_GENE_CNT = 0xffffffff
 	mov $r12 2
 	iowr I[$r11 + 0x000] $r12	// STRAND_CMD = LATCH_GENE_CNT
-	call strand_wait
-	call strand_post
+	call #strand_wait
+	call #strand_post
 
 	// read the size of each strand, poke the context offset of
 	// each into STRAND_{SAVE,LOAD}_SWBASE now, no need to worry
@@ -391,7 +391,7 @@ strand_ctx_init:
 		add b32 $r14 $r10
 		add b32 $r8 4
 		sub b32 $r9 1
-		bra ne ctx_init_strand_loop
+		bra ne #ctx_init_strand_loop
 
 	shl b32 $r14 8
 	sub b32 $r15 $r14 $r15
diff --git a/drivers/gpu/drm/nouveau/nvc0_graph.h b/drivers/gpu/drm/nouveau/nvc0_graph.h
index 636fe9812f79..91d44ea662d9 100644
--- a/drivers/gpu/drm/nouveau/nvc0_graph.h
+++ b/drivers/gpu/drm/nouveau/nvc0_graph.h
@@ -87,6 +87,7 @@ nvc0_graph_class(struct drm_device *dev)
 	case 0xc1:
 		return 0x9197;
 	case 0xc8:
+	case 0xd9:
 		return 0x9297;
 	default:
 		return 0;
diff --git a/drivers/gpu/drm/nouveau/nvc0_grctx.c b/drivers/gpu/drm/nouveau/nvc0_grctx.c
index 96b0b93d94ca..de77842b31c0 100644
--- a/drivers/gpu/drm/nouveau/nvc0_grctx.c
+++ b/drivers/gpu/drm/nouveau/nvc0_grctx.c
@@ -1268,6 +1268,17 @@ nvc0_grctx_generate_9039(struct drm_device *dev)
 static void
 nvc0_grctx_generate_90c0(struct drm_device *dev)
 {
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	int i;
+
+	for (i = 0; dev_priv->chipset == 0xd9 && i < 4; i++) {
+		nv_mthd(dev, 0x90c0, 0x2700 + (i * 0x40), 0x00000000);
+		nv_mthd(dev, 0x90c0, 0x2720 + (i * 0x40), 0x00000000);
+		nv_mthd(dev, 0x90c0, 0x2704 + (i * 0x40), 0x00000000);
+		nv_mthd(dev, 0x90c0, 0x2724 + (i * 0x40), 0x00000000);
+		nv_mthd(dev, 0x90c0, 0x2708 + (i * 0x40), 0x00000000);
+		nv_mthd(dev, 0x90c0, 0x2728 + (i * 0x40), 0x00000000);
+	}
 	nv_mthd(dev, 0x90c0, 0x270c, 0x00000000);
 	nv_mthd(dev, 0x90c0, 0x272c, 0x00000000);
 	nv_mthd(dev, 0x90c0, 0x274c, 0x00000000);
@@ -1276,6 +1287,12 @@ nvc0_grctx_generate_90c0(struct drm_device *dev)
 	nv_mthd(dev, 0x90c0, 0x27ac, 0x00000000);
 	nv_mthd(dev, 0x90c0, 0x27cc, 0x00000000);
 	nv_mthd(dev, 0x90c0, 0x27ec, 0x00000000);
+	for (i = 0; dev_priv->chipset == 0xd9 && i < 4; i++) {
+		nv_mthd(dev, 0x90c0, 0x2710 + (i * 0x40), 0x00014000);
+		nv_mthd(dev, 0x90c0, 0x2730 + (i * 0x40), 0x00014000);
+		nv_mthd(dev, 0x90c0, 0x2714 + (i * 0x40), 0x00000040);
+		nv_mthd(dev, 0x90c0, 0x2734 + (i * 0x40), 0x00000040);
+	}
 	nv_mthd(dev, 0x90c0, 0x030c, 0x00000001);
 	nv_mthd(dev, 0x90c0, 0x1944, 0x00000000);
 	nv_mthd(dev, 0x90c0, 0x0758, 0x00000100);
@@ -1471,14 +1488,20 @@ nvc0_grctx_generate_shaders(struct drm_device *dev)
 {
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 
-	if (dev_priv->chipset != 0xc1) {
-		nv_wr32(dev, 0x405800, 0x078000bf);
-		nv_wr32(dev, 0x405830, 0x02180000);
-	} else {
+	if (dev_priv->chipset == 0xd9) {
 		nv_wr32(dev, 0x405800, 0x0f8000bf);
 		nv_wr32(dev, 0x405830, 0x02180218);
+		nv_wr32(dev, 0x405834, 0x08000000);
+	} else
+	if (dev_priv->chipset == 0xc1) {
+		nv_wr32(dev, 0x405800, 0x0f8000bf);
+		nv_wr32(dev, 0x405830, 0x02180218);
+		nv_wr32(dev, 0x405834, 0x00000000);
+	} else {
+		nv_wr32(dev, 0x405800, 0x078000bf);
+		nv_wr32(dev, 0x405830, 0x02180000);
+		nv_wr32(dev, 0x405834, 0x00000000);
 	}
-	nv_wr32(dev, 0x405834, 0x00000000);
 	nv_wr32(dev, 0x405838, 0x00000000);
 	nv_wr32(dev, 0x405854, 0x00000000);
 	nv_wr32(dev, 0x405870, 0x00000001);
@@ -1509,7 +1532,10 @@ nvc0_grctx_generate_unk64xx(struct drm_device *dev)
 	nv_wr32(dev, 0x4064ac, 0x00003fff);
 	nv_wr32(dev, 0x4064b4, 0x00000000);
 	nv_wr32(dev, 0x4064b8, 0x00000000);
-	if (dev_priv->chipset == 0xc1) {
+	if (dev_priv->chipset == 0xd9)
+		nv_wr32(dev, 0x4064bc, 0x00000000);
+	if (dev_priv->chipset == 0xc1 ||
+	    dev_priv->chipset == 0xd9) {
 		nv_wr32(dev, 0x4064c0, 0x80140078);
 		nv_wr32(dev, 0x4064c4, 0x0086ffff);
 	}
@@ -1550,10 +1576,23 @@ nvc0_grctx_generate_rop(struct drm_device *dev)
 	/* ROPC_BROADCAST */
 	nv_wr32(dev, 0x408800, 0x02802a3c);
 	nv_wr32(dev, 0x408804, 0x00000040);
-	nv_wr32(dev, 0x408808, chipset != 0xc1 ? 0x0003e00d : 0x1003e005);
-	nv_wr32(dev, 0x408900, 0x3080b801);
-	nv_wr32(dev, 0x408904, chipset != 0xc1 ? 0x02000001 : 0x62000001);
-	nv_wr32(dev, 0x408908, 0x00c80929);
+	if (chipset == 0xd9) {
+		nv_wr32(dev, 0x408808, 0x1043e005);
+		nv_wr32(dev, 0x408900, 0x3080b801);
+		nv_wr32(dev, 0x408904, 0x1043e005);
+		nv_wr32(dev, 0x408908, 0x00c8102f);
+	} else
+	if (chipset == 0xc1) {
+		nv_wr32(dev, 0x408808, 0x1003e005);
+		nv_wr32(dev, 0x408900, 0x3080b801);
+		nv_wr32(dev, 0x408904, 0x62000001);
+		nv_wr32(dev, 0x408908, 0x00c80929);
+	} else {
+		nv_wr32(dev, 0x408808, 0x0003e00d);
+		nv_wr32(dev, 0x408900, 0x3080b801);
+		nv_wr32(dev, 0x408904, 0x02000001);
+		nv_wr32(dev, 0x408908, 0x00c80929);
+	}
 	nv_wr32(dev, 0x40890c, 0x00000000);
 	nv_wr32(dev, 0x408980, 0x0000011d);
 }
@@ -1572,7 +1611,7 @@ nvc0_grctx_generate_gpc(struct drm_device *dev)
 	nv_wr32(dev, 0x418408, 0x00000000);
 	nv_wr32(dev, 0x41840c, 0x00001008);
 	nv_wr32(dev, 0x418410, 0x0fff0fff);
-	nv_wr32(dev, 0x418414, 0x00200fff);
+	nv_wr32(dev, 0x418414, chipset != 0xd9 ? 0x00200fff : 0x02200fff);
 	nv_wr32(dev, 0x418450, 0x00000000);
 	nv_wr32(dev, 0x418454, 0x00000000);
 	nv_wr32(dev, 0x418458, 0x00000000);
@@ -1587,14 +1626,17 @@ nvc0_grctx_generate_gpc(struct drm_device *dev)
 	nv_wr32(dev, 0x418700, 0x00000002);
 	nv_wr32(dev, 0x418704, 0x00000080);
 	nv_wr32(dev, 0x418708, 0x00000000);
-	nv_wr32(dev, 0x41870c, 0x07c80000);
+	nv_wr32(dev, 0x41870c, chipset != 0xd9 ? 0x07c80000 : 0x00000000);
 	nv_wr32(dev, 0x418710, 0x00000000);
-	nv_wr32(dev, 0x418800, 0x0006860a);
+	nv_wr32(dev, 0x418800, chipset != 0xd9 ? 0x0006860a : 0x7006860a);
 	nv_wr32(dev, 0x418808, 0x00000000);
 	nv_wr32(dev, 0x41880c, 0x00000000);
 	nv_wr32(dev, 0x418810, 0x00000000);
 	nv_wr32(dev, 0x418828, 0x00008442);
-	nv_wr32(dev, 0x418830, chipset != 0xc1 ? 0x00000001 : 0x10000001);
+	if (chipset == 0xc1 || chipset == 0xd9)
+		nv_wr32(dev, 0x418830, 0x10000001);
+	else
+		nv_wr32(dev, 0x418830, 0x00000001);
 	nv_wr32(dev, 0x4188d8, 0x00000008);
 	nv_wr32(dev, 0x4188e0, 0x01000000);
 	nv_wr32(dev, 0x4188e8, 0x00000000);
@@ -1602,7 +1644,12 @@ nvc0_grctx_generate_gpc(struct drm_device *dev)
 	nv_wr32(dev, 0x4188f0, 0x00000000);
 	nv_wr32(dev, 0x4188f4, 0x00000000);
 	nv_wr32(dev, 0x4188f8, 0x00000000);
-	nv_wr32(dev, 0x4188fc, chipset != 0xc1 ? 0x00100000 : 0x00100018);
+	if (chipset == 0xd9)
+		nv_wr32(dev, 0x4188fc, 0x20100008);
+	else if (chipset == 0xc1)
+		nv_wr32(dev, 0x4188fc, 0x00100018);
+	else
+		nv_wr32(dev, 0x4188fc, 0x00100000);
 	nv_wr32(dev, 0x41891c, 0x00ff00ff);
 	nv_wr32(dev, 0x418924, 0x00000000);
 	nv_wr32(dev, 0x418928, 0x00ffff00);
@@ -1616,7 +1663,7 @@ nvc0_grctx_generate_gpc(struct drm_device *dev)
 		nv_wr32(dev, 0x418a14 + (i * 0x20), 0x00000000);
 		nv_wr32(dev, 0x418a18 + (i * 0x20), 0x00000000);
 	}
-	nv_wr32(dev, 0x418b00, 0x00000000);
+	nv_wr32(dev, 0x418b00, chipset != 0xd9 ? 0x00000000 : 0x00000006);
 	nv_wr32(dev, 0x418b08, 0x0a418820);
 	nv_wr32(dev, 0x418b0c, 0x062080e6);
 	nv_wr32(dev, 0x418b10, 0x020398a4);
@@ -1633,7 +1680,7 @@ nvc0_grctx_generate_gpc(struct drm_device *dev)
 	nv_wr32(dev, 0x418c24, 0x00000000);
 	nv_wr32(dev, 0x418c28, 0x00000000);
 	nv_wr32(dev, 0x418c2c, 0x00000000);
-	if (chipset == 0xc1)
+	if (chipset == 0xc1 || chipset == 0xd9)
 		nv_wr32(dev, 0x418c6c, 0x00000001);
 	nv_wr32(dev, 0x418c80, 0x20200004);
 	nv_wr32(dev, 0x418c8c, 0x00000001);
@@ -1653,7 +1700,10 @@ nvc0_grctx_generate_tp(struct drm_device *dev)
 	nv_wr32(dev, 0x419818, 0x00000000);
 	nv_wr32(dev, 0x41983c, 0x00038bc7);
 	nv_wr32(dev, 0x419848, 0x00000000);
-	nv_wr32(dev, 0x419864, chipset != 0xc1 ? 0x0000012a : 0x00000129);
+	if (chipset == 0xc1 || chipset == 0xd9)
+		nv_wr32(dev, 0x419864, 0x00000129);
+	else
+		nv_wr32(dev, 0x419864, 0x0000012a);
 	nv_wr32(dev, 0x419888, 0x00000000);
 	nv_wr32(dev, 0x419a00, 0x000001f0);
 	nv_wr32(dev, 0x419a04, 0x00000001);
@@ -1663,7 +1713,9 @@ nvc0_grctx_generate_tp(struct drm_device *dev)
 	nv_wr32(dev, 0x419a14, 0x00000200);
 	nv_wr32(dev, 0x419a1c, 0x00000000);
 	nv_wr32(dev, 0x419a20, 0x00000800);
-	if (chipset != 0xc0 && chipset != 0xc8)
+	if (chipset == 0xd9)
+		nv_wr32(dev, 0x00419ac4, 0x0017f440);
+	else if (chipset != 0xc0 && chipset != 0xc8)
 		nv_wr32(dev, 0x00419ac4, 0x0007f440);
 	nv_wr32(dev, 0x419b00, 0x0a418820);
 	nv_wr32(dev, 0x419b04, 0x062080e6);
@@ -1672,21 +1724,33 @@ nvc0_grctx_generate_tp(struct drm_device *dev)
 	nv_wr32(dev, 0x419b10, 0x0a418820);
 	nv_wr32(dev, 0x419b14, 0x000000e6);
 	nv_wr32(dev, 0x419bd0, 0x00900103);
-	nv_wr32(dev, 0x419be0, chipset != 0xc1 ? 0x00000001 : 0x00400001);
+	if (chipset == 0xc1 || chipset == 0xd9)
+		nv_wr32(dev, 0x419be0, 0x00400001);
+	else
+		nv_wr32(dev, 0x419be0, 0x00000001);
 	nv_wr32(dev, 0x419be4, 0x00000000);
-	nv_wr32(dev, 0x419c00, 0x00000002);
+	nv_wr32(dev, 0x419c00, chipset != 0xd9 ? 0x00000002 : 0x0000000a);
 	nv_wr32(dev, 0x419c04, 0x00000006);
 	nv_wr32(dev, 0x419c08, 0x00000002);
 	nv_wr32(dev, 0x419c20, 0x00000000);
-	if (chipset == 0xce || chipset == 0xcf)
+	if (dev_priv->chipset == 0xd9) {
+		nv_wr32(dev, 0x419c24, 0x00084210);
+		nv_wr32(dev, 0x419c28, 0x3cf3cf3c);
 		nv_wr32(dev, 0x419cb0, 0x00020048);
-	else
+	} else
+	if (chipset == 0xce || chipset == 0xcf) {
+		nv_wr32(dev, 0x419cb0, 0x00020048);
+	} else {
 		nv_wr32(dev, 0x419cb0, 0x00060048);
+	}
 	nv_wr32(dev, 0x419ce8, 0x00000000);
 	nv_wr32(dev, 0x419cf4, 0x00000183);
-	nv_wr32(dev, 0x419d20, chipset != 0xc1 ? 0x02180000 : 0x12180000);
+	if (chipset == 0xc1 || chipset == 0xd9)
+		nv_wr32(dev, 0x419d20, 0x12180000);
+	else
+		nv_wr32(dev, 0x419d20, 0x02180000);
 	nv_wr32(dev, 0x419d24, 0x00001fff);
-	if (chipset == 0xc1)
+	if (chipset == 0xc1 || chipset == 0xd9)
 		nv_wr32(dev, 0x419d44, 0x02180218);
 	nv_wr32(dev, 0x419e04, 0x00000000);
 	nv_wr32(dev, 0x419e08, 0x00000000);
@@ -1986,6 +2050,10 @@ nvc0_grctx_generate(struct nouveau_channel *chan)
 	nv_icmd(dev, 0x00000215, 0x00000040);
 	nv_icmd(dev, 0x00000216, 0x00000040);
 	nv_icmd(dev, 0x00000217, 0x00000040);
+	if (dev_priv->chipset == 0xd9) {
+		for (i = 0x0400; i <= 0x0417; i++)
+			nv_icmd(dev, i, 0x00000040);
+	}
 	nv_icmd(dev, 0x00000218, 0x0000c080);
 	nv_icmd(dev, 0x00000219, 0x0000c080);
 	nv_icmd(dev, 0x0000021a, 0x0000c080);
@@ -1994,6 +2062,10 @@ nvc0_grctx_generate(struct nouveau_channel *chan)
 	nv_icmd(dev, 0x0000021d, 0x0000c080);
 	nv_icmd(dev, 0x0000021e, 0x0000c080);
 	nv_icmd(dev, 0x0000021f, 0x0000c080);
+	if (dev_priv->chipset == 0xd9) {
+		for (i = 0x0440; i <= 0x0457; i++)
+			nv_icmd(dev, i, 0x0000c080);
+	}
 	nv_icmd(dev, 0x000000ad, 0x0000013e);
 	nv_icmd(dev, 0x000000e1, 0x00000010);
 	nv_icmd(dev, 0x00000290, 0x00000000);
@@ -2556,7 +2628,8 @@ nvc0_grctx_generate(struct nouveau_channel *chan)
 	nv_icmd(dev, 0x0000053f, 0xffff0000);
 	nv_icmd(dev, 0x00000585, 0x0000003f);
 	nv_icmd(dev, 0x00000576, 0x00000003);
-	if (dev_priv->chipset == 0xc1)
+	if (dev_priv->chipset == 0xc1 ||
+	    dev_priv->chipset == 0xd9)
 		nv_icmd(dev, 0x0000057b, 0x00000059);
 	nv_icmd(dev, 0x00000586, 0x00000040);
 	nv_icmd(dev, 0x00000582, 0x00000080);
@@ -2658,6 +2731,8 @@ nvc0_grctx_generate(struct nouveau_channel *chan)
 	nv_icmd(dev, 0x00000957, 0x00000003);
 	nv_icmd(dev, 0x0000095e, 0x20164010);
 	nv_icmd(dev, 0x0000095f, 0x00000020);
+	if (dev_priv->chipset == 0xd9)
+		nv_icmd(dev, 0x0000097d, 0x00000020);
 	nv_icmd(dev, 0x00000683, 0x00000006);
 	nv_icmd(dev, 0x00000685, 0x003fffff);
 	nv_icmd(dev, 0x00000687, 0x00000c48);
diff --git a/drivers/gpu/drm/nouveau/nvc0_grgpc.fuc b/drivers/gpu/drm/nouveau/nvc0_grgpc.fuc
index 06f5e26d1e0f..15272be33b66 100644
--- a/drivers/gpu/drm/nouveau/nvc0_grgpc.fuc
+++ b/drivers/gpu/drm/nouveau/nvc0_grgpc.fuc
@@ -32,7 +32,7 @@
  * - watchdog timer around ctx operations
  */
 
-.section nvc0_grgpc_data
+.section #nvc0_grgpc_data
 include(`nvc0_graph.fuc')
 gpc_id:			.b32 0
 gpc_mmio_list_head:	.b32 0
@@ -48,40 +48,45 @@ cmd_queue:		queue_init
 // chipset descriptions
 chipsets:
 .b8  0xc0 0 0 0
-.b16 nvc0_gpc_mmio_head
-.b16 nvc0_gpc_mmio_tail
-.b16 nvc0_tpc_mmio_head
-.b16 nvc0_tpc_mmio_tail
+.b16 #nvc0_gpc_mmio_head
+.b16 #nvc0_gpc_mmio_tail
+.b16 #nvc0_tpc_mmio_head
+.b16 #nvc0_tpc_mmio_tail
 .b8  0xc1 0 0 0
-.b16 nvc0_gpc_mmio_head
-.b16 nvc1_gpc_mmio_tail
-.b16 nvc0_tpc_mmio_head
-.b16 nvc1_tpc_mmio_tail
+.b16 #nvc0_gpc_mmio_head
+.b16 #nvc1_gpc_mmio_tail
+.b16 #nvc0_tpc_mmio_head
+.b16 #nvc1_tpc_mmio_tail
 .b8  0xc3 0 0 0
-.b16 nvc0_gpc_mmio_head
-.b16 nvc0_gpc_mmio_tail
-.b16 nvc0_tpc_mmio_head
-.b16 nvc3_tpc_mmio_tail
+.b16 #nvc0_gpc_mmio_head
+.b16 #nvc0_gpc_mmio_tail
+.b16 #nvc0_tpc_mmio_head
+.b16 #nvc3_tpc_mmio_tail
 .b8  0xc4 0 0 0
-.b16 nvc0_gpc_mmio_head
-.b16 nvc0_gpc_mmio_tail
-.b16 nvc0_tpc_mmio_head
-.b16 nvc3_tpc_mmio_tail
+.b16 #nvc0_gpc_mmio_head
+.b16 #nvc0_gpc_mmio_tail
+.b16 #nvc0_tpc_mmio_head
+.b16 #nvc3_tpc_mmio_tail
 .b8  0xc8 0 0 0
-.b16 nvc0_gpc_mmio_head
-.b16 nvc0_gpc_mmio_tail
-.b16 nvc0_tpc_mmio_head
-.b16 nvc0_tpc_mmio_tail
+.b16 #nvc0_gpc_mmio_head
+.b16 #nvc0_gpc_mmio_tail
+.b16 #nvc0_tpc_mmio_head
+.b16 #nvc0_tpc_mmio_tail
 .b8  0xce 0 0 0
-.b16 nvc0_gpc_mmio_head
-.b16 nvc0_gpc_mmio_tail
-.b16 nvc0_tpc_mmio_head
-.b16 nvc3_tpc_mmio_tail
+.b16 #nvc0_gpc_mmio_head
+.b16 #nvc0_gpc_mmio_tail
+.b16 #nvc0_tpc_mmio_head
+.b16 #nvc3_tpc_mmio_tail
 .b8  0xcf 0 0 0
-.b16 nvc0_gpc_mmio_head
-.b16 nvc0_gpc_mmio_tail
-.b16 nvc0_tpc_mmio_head
-.b16 nvcf_tpc_mmio_tail
+.b16 #nvc0_gpc_mmio_head
+.b16 #nvc0_gpc_mmio_tail
+.b16 #nvc0_tpc_mmio_head
+.b16 #nvcf_tpc_mmio_tail
+.b8  0xd9 0 0 0
+.b16 #nvd9_gpc_mmio_head
+.b16 #nvd9_gpc_mmio_tail
+.b16 #nvd9_tpc_mmio_head
+.b16 #nvd9_tpc_mmio_tail
 .b8  0 0 0 0
 
 // GPC mmio lists
@@ -114,6 +119,35 @@ nvc0_gpc_mmio_tail:
 mmctx_data(0x000c6c, 1);
 nvc1_gpc_mmio_tail:
 
+nvd9_gpc_mmio_head:
+mmctx_data(0x000380, 1)
+mmctx_data(0x000400, 2)
+mmctx_data(0x00040c, 3)
+mmctx_data(0x000450, 9)
+mmctx_data(0x000600, 1)
+mmctx_data(0x000684, 1)
+mmctx_data(0x000700, 5)
+mmctx_data(0x000800, 1)
+mmctx_data(0x000808, 3)
+mmctx_data(0x000828, 1)
+mmctx_data(0x000830, 1)
+mmctx_data(0x0008d8, 1)
+mmctx_data(0x0008e0, 1)
+mmctx_data(0x0008e8, 6)
+mmctx_data(0x00091c, 1)
+mmctx_data(0x000924, 3)
+mmctx_data(0x000b00, 1)
+mmctx_data(0x000b08, 6)
+mmctx_data(0x000bb8, 1)
+mmctx_data(0x000c08, 1)
+mmctx_data(0x000c10, 8)
+mmctx_data(0x000c6c, 1)
+mmctx_data(0x000c80, 1)
+mmctx_data(0x000c8c, 1)
+mmctx_data(0x001000, 3)
+mmctx_data(0x001014, 1)
+nvd9_gpc_mmio_tail:
+
 // TPC mmio lists
 nvc0_tpc_mmio_head:
 mmctx_data(0x000018, 1)
@@ -146,9 +180,34 @@ nvc3_tpc_mmio_tail:
 mmctx_data(0x000544, 1)
 nvc1_tpc_mmio_tail:
 
+nvd9_tpc_mmio_head:
+mmctx_data(0x000018, 1)
+mmctx_data(0x00003c, 1)
+mmctx_data(0x000048, 1)
+mmctx_data(0x000064, 1)
+mmctx_data(0x000088, 1)
+mmctx_data(0x000200, 6)
+mmctx_data(0x00021c, 2)
+mmctx_data(0x0002c4, 1)
+mmctx_data(0x000300, 6)
+mmctx_data(0x0003d0, 1)
+mmctx_data(0x0003e0, 2)
+mmctx_data(0x000400, 3)
+mmctx_data(0x000420, 3)
+mmctx_data(0x0004b0, 1)
+mmctx_data(0x0004e8, 1)
+mmctx_data(0x0004f4, 1)
+mmctx_data(0x000520, 2)
+mmctx_data(0x000544, 1)
+mmctx_data(0x000604, 4)
+mmctx_data(0x000644, 20)
+mmctx_data(0x000698, 1)
+mmctx_data(0x0006e0, 1)
+mmctx_data(0x000750, 3)
+nvd9_tpc_mmio_tail:
 
-.section nvc0_grgpc_code
-bra init
+.section #nvc0_grgpc_code
+bra #init
 define(`include_code')
 include(`nvc0_graph.fuc')
 
@@ -160,10 +219,10 @@ error:
 	push $r14
 	mov $r14 -0x67ec 	// 0x9814
 	sethi $r14 0x400000
-	call nv_wr32		// HUB_CTXCTL_CC_SCRATCH[5] = error code
+	call #nv_wr32		// HUB_CTXCTL_CC_SCRATCH[5] = error code
 	add b32 $r14 0x41c
 	mov $r15 1
-	call nv_wr32		// HUB_CTXCTL_INTR_UP_SET
+	call #nv_wr32		// HUB_CTXCTL_INTR_UP_SET
 	pop $r14
 	ret
 
@@ -190,7 +249,7 @@ init:
 	iowr I[$r1 + 0x000] $r2		// FIFO_ENABLE
 
 	// setup i0 handler, and route all interrupts to it
-	mov $r1 ih
+	mov $r1 #ih
 	mov $iv0 $r1
 	mov $r1 0x400
 	iowr I[$r1 + 0x300] $r0		// INTR_DISPATCH
@@ -210,24 +269,24 @@ init:
 	and $r2 0x1f
 	shl b32 $r3 $r2
 	sub b32 $r3 1
-	st b32 D[$r0 + tpc_count] $r2
-	st b32 D[$r0 + tpc_mask] $r3
+	st b32 D[$r0 + #tpc_count] $r2
+	st b32 D[$r0 + #tpc_mask] $r3
 	add b32 $r1 0x400
 	iord $r2 I[$r1 + 0x000]		// MYINDEX
-	st b32 D[$r0 + gpc_id] $r2
+	st b32 D[$r0 + #gpc_id] $r2
 
 	// find context data for this chipset
 	mov $r2 0x800
 	shl b32 $r2 6
 	iord $r2 I[$r2 + 0x000]		// CC_SCRATCH[0]
-	mov $r1 chipsets - 12
+	mov $r1 #chipsets - 12
 	init_find_chipset:
 		add b32 $r1 12
 		ld b32 $r3 D[$r1 + 0x00]
 		cmpu b32 $r3 $r2
-		bra e init_context
+		bra e #init_context
 		cmpu b32 $r3 0
-		bra ne init_find_chipset
+		bra ne #init_find_chipset
 		// unknown chipset
 		ret
 
@@ -253,19 +312,19 @@ init:
 	clear b32 $r15
 	ld b16 $r14 D[$r1 + 4]
 	ld b16 $r15 D[$r1 + 6]
-	st b16 D[$r0 + gpc_mmio_list_head] $r14
-	st b16 D[$r0 + gpc_mmio_list_tail] $r15
-	call mmctx_size
+	st b16 D[$r0 + #gpc_mmio_list_head] $r14
+	st b16 D[$r0 + #gpc_mmio_list_tail] $r15
+	call #mmctx_size
 	add b32 $r2 $r15
 	add b32 $r3 $r15
 
 	// calculate per-TPC mmio context size, store the list pointers
 	ld b16 $r14 D[$r1 + 8]
 	ld b16 $r15 D[$r1 + 10]
-	st b16 D[$r0 + tpc_mmio_list_head] $r14
-	st b16 D[$r0 + tpc_mmio_list_tail] $r15
-	call mmctx_size
-	ld b32 $r14 D[$r0 + tpc_count]
+	st b16 D[$r0 + #tpc_mmio_list_head] $r14
+	st b16 D[$r0 + #tpc_mmio_list_tail] $r15
+	call #mmctx_size
+	ld b32 $r14 D[$r0 + #tpc_count]
 	mulu $r14 $r15
 	add b32 $r2 $r14
 	add b32 $r3 $r14
@@ -283,7 +342,7 @@ init:
 
 	// calculate size of strand context data
 	mov b32 $r15 $r2
-	call strand_ctx_init
+	call #strand_ctx_init
 	add b32 $r3 $r15
 
 	// save context size, and tell HUB we're done
@@ -301,13 +360,13 @@ init:
 main:
 	bset $flags $p0
 	sleep $p0
-	mov $r13 cmd_queue
-	call queue_get
-	bra $p1 main
+	mov $r13 #cmd_queue
+	call #queue_get
+	bra $p1 #main
 
 	// 0x0000-0x0003 are all context transfers
 	cmpu b32 $r14 0x04
-	bra nc main_not_ctx_xfer
+	bra nc #main_not_ctx_xfer
 		// fetch $flags and mask off $p1/$p2
 		mov $r1 $flags
 		mov $r2 0x0006
@@ -318,14 +377,14 @@ main:
 		or $r1 $r14
 		mov $flags $r1
 		// transfer context data
-		call ctx_xfer
-		bra main
+		call #ctx_xfer
+		bra #main
 
 	main_not_ctx_xfer:
 	shl b32 $r15 $r14 16
 	or $r15 E_BAD_COMMAND
-	call error
-	bra main
+	call #error
+	bra #main
 
 // interrupt handler
 ih:
@@ -342,13 +401,13 @@ ih:
 	// incoming fifo command?
 	iord $r10 I[$r0 + 0x200]	// INTR
 	and $r11 $r10 0x00000004
-	bra e ih_no_fifo
+	bra e #ih_no_fifo
 		// queue incoming fifo command for later processing
 		mov $r11 0x1900
-		mov $r13 cmd_queue
+		mov $r13 #cmd_queue
 		iord $r14 I[$r11 + 0x100]	// FIFO_CMD
 		iord $r15 I[$r11 + 0x000]	// FIFO_DATA
-		call queue_put
+		call #queue_put
 		add b32 $r11 0x400
 		mov $r14 1
 		iowr I[$r11 + 0x000] $r14	// FIFO_ACK
@@ -374,11 +433,11 @@ ih:
 //
 hub_barrier_done:
 	mov $r15 1
-	ld b32 $r14 D[$r0 + gpc_id]
+	ld b32 $r14 D[$r0 + #gpc_id]
 	shl b32 $r15 $r14
 	mov $r14 -0x6be8 	// 0x409418 - HUB_BAR_SET
 	sethi $r14 0x400000
-	call nv_wr32
+	call #nv_wr32
 	ret
 
 // Disables various things, waits a bit, and re-enables them..
@@ -395,7 +454,7 @@ ctx_redswitch:
 	mov $r15 8
 	ctx_redswitch_delay:
 		sub b32 $r15 1
-		bra ne ctx_redswitch_delay
+		bra ne #ctx_redswitch_delay
 	mov $r15 0xa20
 	iowr I[$r14] $r15	// GPC_RED_SWITCH = UNK11, ENABLE, POWER
 	ret
@@ -413,8 +472,8 @@ ctx_xfer:
 	mov $r1 0xa04
 	shl b32 $r1 6
 	iowr I[$r1 + 0x000] $r15// MEM_BASE
-	bra not $p1 ctx_xfer_not_load
-		call ctx_redswitch
+	bra not $p1 #ctx_xfer_not_load
+		call #ctx_redswitch
 	ctx_xfer_not_load:
 
 	// strands
@@ -422,7 +481,7 @@ ctx_xfer:
 	sethi $r1 0x20000
 	mov $r2 0xc
 	iowr I[$r1] $r2		// STRAND_CMD(0x3f) = 0x0c
-	call strand_wait
+	call #strand_wait
 	mov $r2 0x47fc
 	sethi $r2 0x20000
 	iowr I[$r2] $r0		// STRAND_FIRST_GENE(0x3f) = 0x00
@@ -435,46 +494,46 @@ ctx_xfer:
 	or $r10 2		// first
 	mov $r11 0x0000
 	sethi $r11 0x500000
-	ld b32 $r12 D[$r0 + gpc_id]
+	ld b32 $r12 D[$r0 + #gpc_id]
 	shl b32 $r12 15
 	add b32 $r11 $r12	// base = NV_PGRAPH_GPCn
-	ld b32 $r12 D[$r0 + gpc_mmio_list_head]
-	ld b32 $r13 D[$r0 + gpc_mmio_list_tail]
+	ld b32 $r12 D[$r0 + #gpc_mmio_list_head]
+	ld b32 $r13 D[$r0 + #gpc_mmio_list_tail]
 	mov $r14 0		// not multi
-	call mmctx_xfer
+	call #mmctx_xfer
 
 	// per-TPC mmio context
 	xbit $r10 $flags $p1	// direction
 	or $r10 4		// last
 	mov $r11 0x4000
 	sethi $r11 0x500000	// base = NV_PGRAPH_GPC0_TPC0
-	ld b32 $r12 D[$r0 + gpc_id]
+	ld b32 $r12 D[$r0 + #gpc_id]
 	shl b32 $r12 15
 	add b32 $r11 $r12	// base = NV_PGRAPH_GPCn_TPC0
-	ld b32 $r12 D[$r0 + tpc_mmio_list_head]
-	ld b32 $r13 D[$r0 + tpc_mmio_list_tail]
-	ld b32 $r15 D[$r0 + tpc_mask]
+	ld b32 $r12 D[$r0 + #tpc_mmio_list_head]
+	ld b32 $r13 D[$r0 + #tpc_mmio_list_tail]
+	ld b32 $r15 D[$r0 + #tpc_mask]
 	mov $r14 0x800		// stride = 0x800
-	call mmctx_xfer
+	call #mmctx_xfer
 
 	// wait for strands to finish
-	call strand_wait
+	call #strand_wait
 
 	// if load, or a save without a load following, do some
 	// unknown stuff that's done after finishing a block of
 	// strand commands
-	bra $p1 ctx_xfer_post
-	bra not $p2 ctx_xfer_done
+	bra $p1 #ctx_xfer_post
+	bra not $p2 #ctx_xfer_done
 	ctx_xfer_post:
 		mov $r1 0x4afc
 		sethi $r1 0x20000
 		mov $r2 0xd
 		iowr I[$r1] $r2		// STRAND_CMD(0x3f) = 0x0d
-		call strand_wait
+		call #strand_wait
 
 	// mark completion in HUB's barrier
 	ctx_xfer_done:
-	call hub_barrier_done
+	call #hub_barrier_done
 	ret
 
 .align 256
diff --git a/drivers/gpu/drm/nouveau/nvc0_grgpc.fuc.h b/drivers/gpu/drm/nouveau/nvc0_grgpc.fuc.h
index 6f820324480e..a988b8ad00ac 100644
--- a/drivers/gpu/drm/nouveau/nvc0_grgpc.fuc.h
+++ b/drivers/gpu/drm/nouveau/nvc0_grgpc.fuc.h
@@ -25,26 +25,29 @@ uint32_t nvc0_grgpc_data[] = {
 	0x00000000,
 	0x00000000,
 	0x000000c0,
-	0x011c00bc,
-	0x01700120,
+	0x012800c8,
+	0x01e40194,
 	0x000000c1,
-	0x012000bc,
-	0x01840120,
+	0x012c00c8,
+	0x01f80194,
 	0x000000c3,
-	0x011c00bc,
-	0x01800120,
+	0x012800c8,
+	0x01f40194,
 	0x000000c4,
-	0x011c00bc,
-	0x01800120,
+	0x012800c8,
+	0x01f40194,
 	0x000000c8,
-	0x011c00bc,
-	0x01700120,
+	0x012800c8,
+	0x01e40194,
 	0x000000ce,
-	0x011c00bc,
-	0x01800120,
+	0x012800c8,
+	0x01f40194,
 	0x000000cf,
-	0x011c00bc,
-	0x017c0120,
+	0x012800c8,
+	0x01f00194,
+	0x000000d9,
+	0x0194012c,
+	0x025401f8,
 	0x00000000,
 	0x00000380,
 	0x14000400,
@@ -71,6 +74,32 @@ uint32_t nvc0_grgpc_data[] = {
 	0x08001000,
 	0x00001014,
 	0x00000c6c,
+	0x00000380,
+	0x04000400,
+	0x0800040c,
+	0x20000450,
+	0x00000600,
+	0x00000684,
+	0x10000700,
+	0x00000800,
+	0x08000808,
+	0x00000828,
+	0x00000830,
+	0x000008d8,
+	0x000008e0,
+	0x140008e8,
+	0x0000091c,
+	0x08000924,
+	0x00000b00,
+	0x14000b08,
+	0x00000bb8,
+	0x00000c08,
+	0x1c000c10,
+	0x00000c6c,
+	0x00000c80,
+	0x00000c8c,
+	0x08001000,
+	0x00001014,
 	0x00000018,
 	0x0000003c,
 	0x00000048,
@@ -96,6 +125,29 @@ uint32_t nvc0_grgpc_data[] = {
 	0x000006e0,
 	0x000004bc,
 	0x00000544,
+	0x00000018,
+	0x0000003c,
+	0x00000048,
+	0x00000064,
+	0x00000088,
+	0x14000200,
+	0x0400021c,
+	0x000002c4,
+	0x14000300,
+	0x000003d0,
+	0x040003e0,
+	0x08000400,
+	0x08000420,
+	0x000004b0,
+	0x000004e8,
+	0x000004f4,
+	0x04000520,
+	0x00000544,
+	0x0c000604,
+	0x4c000644,
+	0x00000698,
+	0x000006e0,
+	0x08000750,
 };
 
 uint32_t nvc0_grgpc_code[] = {
diff --git a/drivers/gpu/drm/nouveau/nvc0_grhub.fuc b/drivers/gpu/drm/nouveau/nvc0_grhub.fuc
index e4f8c7e89ddd..98acddb2c5bb 100644
--- a/drivers/gpu/drm/nouveau/nvc0_grhub.fuc
+++ b/drivers/gpu/drm/nouveau/nvc0_grhub.fuc
@@ -27,7 +27,7 @@
  *    m4 nvc0_grhub.fuc | envyas -a -w -m fuc -V nva3 -o nvc0_grhub.fuc.h
  */
 
-.section nvc0_grhub_data
+.section #nvc0_grhub_data
 include(`nvc0_graph.fuc')
 gpc_count:		.b32 0
 rop_count:		.b32 0
@@ -39,26 +39,29 @@ ctx_current:		.b32 0
 
 chipsets:
 .b8  0xc0 0 0 0
-.b16 nvc0_hub_mmio_head
-.b16 nvc0_hub_mmio_tail
+.b16 #nvc0_hub_mmio_head
+.b16 #nvc0_hub_mmio_tail
 .b8  0xc1 0 0 0
-.b16 nvc0_hub_mmio_head
-.b16 nvc1_hub_mmio_tail
+.b16 #nvc0_hub_mmio_head
+.b16 #nvc1_hub_mmio_tail
 .b8  0xc3 0 0 0
-.b16 nvc0_hub_mmio_head
-.b16 nvc0_hub_mmio_tail
+.b16 #nvc0_hub_mmio_head
+.b16 #nvc0_hub_mmio_tail
 .b8  0xc4 0 0 0
-.b16 nvc0_hub_mmio_head
-.b16 nvc0_hub_mmio_tail
+.b16 #nvc0_hub_mmio_head
+.b16 #nvc0_hub_mmio_tail
 .b8  0xc8 0 0 0
-.b16 nvc0_hub_mmio_head
-.b16 nvc0_hub_mmio_tail
+.b16 #nvc0_hub_mmio_head
+.b16 #nvc0_hub_mmio_tail
 .b8  0xce 0 0 0
-.b16 nvc0_hub_mmio_head
-.b16 nvc0_hub_mmio_tail
+.b16 #nvc0_hub_mmio_head
+.b16 #nvc0_hub_mmio_tail
 .b8  0xcf 0 0 0
-.b16 nvc0_hub_mmio_head
-.b16 nvc0_hub_mmio_tail
+.b16 #nvc0_hub_mmio_head
+.b16 #nvc0_hub_mmio_tail
+.b8  0xd9 0 0 0
+.b16 #nvd9_hub_mmio_head
+.b16 #nvd9_hub_mmio_tail
 .b8  0 0 0 0
 
 nvc0_hub_mmio_head:
@@ -105,6 +108,48 @@ nvc0_hub_mmio_tail:
 mmctx_data(0x4064c0, 2)
 nvc1_hub_mmio_tail:
 
+nvd9_hub_mmio_head:
+mmctx_data(0x17e91c, 2)
+mmctx_data(0x400204, 2)
+mmctx_data(0x404004, 10)
+mmctx_data(0x404044, 1)
+mmctx_data(0x404094, 14)
+mmctx_data(0x4040d0, 7)
+mmctx_data(0x4040f8, 1)
+mmctx_data(0x404130, 3)
+mmctx_data(0x404150, 3)
+mmctx_data(0x404164, 2)
+mmctx_data(0x404178, 2)
+mmctx_data(0x404200, 8)
+mmctx_data(0x404404, 14)
+mmctx_data(0x404460, 4)
+mmctx_data(0x404480, 1)
+mmctx_data(0x404498, 1)
+mmctx_data(0x404604, 4)
+mmctx_data(0x404618, 32)
+mmctx_data(0x404698, 21)
+mmctx_data(0x4046f0, 2)
+mmctx_data(0x404700, 22)
+mmctx_data(0x405800, 1)
+mmctx_data(0x405830, 3)
+mmctx_data(0x405854, 1)
+mmctx_data(0x405870, 4)
+mmctx_data(0x405a00, 2)
+mmctx_data(0x405a18, 1)
+mmctx_data(0x406020, 1)
+mmctx_data(0x406028, 4)
+mmctx_data(0x4064a8, 2)
+mmctx_data(0x4064b4, 5)
+mmctx_data(0x407804, 1)
+mmctx_data(0x40780c, 6)
+mmctx_data(0x4078bc, 1)
+mmctx_data(0x408000, 7)
+mmctx_data(0x408064, 1)
+mmctx_data(0x408800, 3)
+mmctx_data(0x408900, 4)
+mmctx_data(0x408980, 1)
+nvd9_hub_mmio_tail:
+
 .align 256
 chan_data:
 chan_mmio_count:	.b32 0
@@ -113,8 +158,8 @@ chan_mmio_address:	.b32 0
 .align 256
 xfer_data: 		.b32 0
 
-.section nvc0_grhub_code
-bra init
+.section #nvc0_grhub_code
+bra #init
 define(`include_code')
 include(`nvc0_graph.fuc')
 
@@ -157,7 +202,7 @@ init:
 	iowr I[$r1 + 0x000] $r2	// FIFO_ENABLE
 
 	// setup i0 handler, and route all interrupts to it
-	mov $r1 ih
+	mov $r1 #ih
 	mov $iv0 $r1
 	mov $r1 0x400
 	iowr I[$r1 + 0x300] $r0	// INTR_DISPATCH
@@ -201,11 +246,11 @@ init:
 	// fetch enabled GPC/ROP counts
 	mov $r14 -0x69fc	// 0x409604
 	sethi $r14 0x400000
-	call nv_rd32
+	call #nv_rd32
 	extr $r1 $r15 16:20
-	st b32 D[$r0 + rop_count] $r1
+	st b32 D[$r0 + #rop_count] $r1
 	and $r15 0x1f
-	st b32 D[$r0 + gpc_count] $r15
+	st b32 D[$r0 + #gpc_count] $r15
 
 	// set BAR_REQMASK to GPC mask
 	mov $r1 1
@@ -220,14 +265,14 @@ init:
 	mov $r2 0x800
 	shl b32 $r2 6
 	iord $r2 I[$r2 + 0x000]		// CC_SCRATCH[0]
-	mov $r15 chipsets - 8
+	mov $r15 #chipsets - 8
 	init_find_chipset:
 		add b32 $r15 8
 		ld b32 $r3 D[$r15 + 0x00]
 		cmpu b32 $r3 $r2
-		bra e init_context
+		bra e #init_context
 		cmpu b32 $r3 0
-		bra ne init_find_chipset
+		bra ne #init_find_chipset
 		// unknown chipset
 		ret
 
@@ -239,9 +284,9 @@ init:
 	ld b16 $r14 D[$r15 + 4]
 	ld b16 $r15 D[$r15 + 6]
 	sethi $r14 0
-	st b32 D[$r0 + hub_mmio_list_head] $r14
-	st b32 D[$r0 + hub_mmio_list_tail] $r15
-	call mmctx_size
+	st b32 D[$r0 + #hub_mmio_list_head] $r14
+	st b32 D[$r0 + #hub_mmio_list_tail] $r15
+	call #mmctx_size
 
 	// set mmctx base addresses now so we don't have to do it later,
 	// they don't (currently) ever change
@@ -260,7 +305,7 @@ init:
 	add b32 $r1 1
 	shl b32 $r1 8
 	mov b32 $r15 $r1
-	call strand_ctx_init
+	call #strand_ctx_init
 	add b32 $r1 $r15
 
 	// initialise each GPC in sequence by passing in the offset of its
@@ -271,40 +316,40 @@ init:
 	// when it has completed, and return the size of its context data
 	// in GPCn_CC_SCRATCH[1]
 	//
-	ld b32 $r3 D[$r0 + gpc_count]
+	ld b32 $r3 D[$r0 + #gpc_count]
 	mov $r4 0x2000
 	sethi $r4 0x500000
 	init_gpc:
 		// setup, and start GPC ucode running
 		add b32 $r14 $r4 0x804
 		mov b32 $r15 $r1
-		call nv_wr32			// CC_SCRATCH[1] = ctx offset
+		call #nv_wr32			// CC_SCRATCH[1] = ctx offset
 		add b32 $r14 $r4 0x800
 		mov b32 $r15 $r2
-		call nv_wr32			// CC_SCRATCH[0] = chipset
+		call #nv_wr32			// CC_SCRATCH[0] = chipset
 		add b32 $r14 $r4 0x10c
 		clear b32 $r15
-		call nv_wr32
+		call #nv_wr32
 		add b32 $r14 $r4 0x104
-		call nv_wr32			// ENTRY
+		call #nv_wr32			// ENTRY
 		add b32 $r14 $r4 0x100
 		mov $r15 2			// CTRL_START_TRIGGER
-		call nv_wr32			// CTRL
+		call #nv_wr32			// CTRL
 
 		// wait for it to complete, and adjust context size
 		add b32 $r14 $r4 0x800
 		init_gpc_wait:
-			call nv_rd32
+			call #nv_rd32
 			xbit $r15 $r15 31
-			bra e init_gpc_wait
+			bra e #init_gpc_wait
 		add b32 $r14 $r4 0x804
-		call nv_rd32
+		call #nv_rd32
 		add b32 $r1 $r15
 
 		// next!
 		add b32 $r4 0x8000
 		sub b32 $r3 1
-		bra ne init_gpc
+		bra ne #init_gpc
 
 	// save context size, and tell host we're ready
 	mov $r2 0x800
@@ -322,13 +367,13 @@ main:
 	// sleep until we have something to do
 	bset $flags $p0
 	sleep $p0
-	mov $r13 cmd_queue
-	call queue_get
-	bra $p1 main
+	mov $r13 #cmd_queue
+	call #queue_get
+	bra $p1 #main
 
 	// context switch, requested by GPU?
 	cmpu b32 $r14 0x4001
-	bra ne main_not_ctx_switch
+	bra ne #main_not_ctx_switch
 		trace_set(T_AUTO)
 		mov $r1 0xb00
 		shl b32 $r1 6
@@ -336,39 +381,39 @@ main:
 		iord $r1 I[$r1 + 0x000]		// CHAN_CUR
 
 		xbit $r3 $r1 31
-		bra e chsw_no_prev
+		bra e #chsw_no_prev
 			xbit $r3 $r2 31
-			bra e chsw_prev_no_next
+			bra e #chsw_prev_no_next
 				push $r2
 				mov b32 $r2 $r1
 				trace_set(T_SAVE)
 				bclr $flags $p1
 				bset $flags $p2
-				call ctx_xfer
+				call #ctx_xfer
 				trace_clr(T_SAVE);
 				pop $r2
 				trace_set(T_LOAD);
 				bset $flags $p1
-				call ctx_xfer
+				call #ctx_xfer
 				trace_clr(T_LOAD);
-				bra chsw_done
+				bra #chsw_done
 			chsw_prev_no_next:
 				push $r2
 				mov b32 $r2 $r1
 				bclr $flags $p1
 				bclr $flags $p2
-				call ctx_xfer
+				call #ctx_xfer
 				pop $r2
 				mov $r1 0xb00
 				shl b32 $r1 6
 				iowr I[$r1] $r2
-				bra chsw_done
+				bra #chsw_done
 		chsw_no_prev:
 			xbit $r3 $r2 31
-			bra e chsw_done
+			bra e #chsw_done
 				bset $flags $p1
 				bclr $flags $p2
-				call ctx_xfer
+				call #ctx_xfer
 
 		// ack the context switch request
 		chsw_done:
@@ -377,32 +422,32 @@ main:
 		mov $r2 1
 		iowr I[$r1 + 0x000] $r2		// 0x409b0c
 		trace_clr(T_AUTO)
-		bra main
+		bra #main
 
 	// request to set current channel? (*not* a context switch)
 	main_not_ctx_switch:
 	cmpu b32 $r14 0x0001
-	bra ne main_not_ctx_chan
+	bra ne #main_not_ctx_chan
 		mov b32 $r2 $r15
-		call ctx_chan
-		bra main_done
+		call #ctx_chan
+		bra #main_done
 
 	// request to store current channel context?
 	main_not_ctx_chan:
 	cmpu b32 $r14 0x0002
-	bra ne main_not_ctx_save
+	bra ne #main_not_ctx_save
 		trace_set(T_SAVE)
 		bclr $flags $p1
 		bclr $flags $p2
-		call ctx_xfer
+		call #ctx_xfer
 		trace_clr(T_SAVE)
-		bra main_done
+		bra #main_done
 
 	main_not_ctx_save:
 		shl b32 $r15 $r14 16
 		or $r15 E_BAD_COMMAND
-		call error
-		bra main
+		call #error
+		bra #main
 
 	main_done:
 	mov $r1 0x820
@@ -410,7 +455,7 @@ main:
 	clear b32 $r2
 	bset $r2 31
 	iowr I[$r1 + 0x000] $r2		// CC_SCRATCH[0] |= 0x80000000
-	bra main
+	bra #main
 
 // interrupt handler
 ih:
@@ -427,13 +472,13 @@ ih:
 	// incoming fifo command?
 	iord $r10 I[$r0 + 0x200]	// INTR
 	and $r11 $r10 0x00000004
-	bra e ih_no_fifo
+	bra e #ih_no_fifo
 		// queue incoming fifo command for later processing
 		mov $r11 0x1900
-		mov $r13 cmd_queue
+		mov $r13 #cmd_queue
 		iord $r14 I[$r11 + 0x100]	// FIFO_CMD
 		iord $r15 I[$r11 + 0x000]	// FIFO_DATA
-		call queue_put
+		call #queue_put
 		add b32 $r11 0x400
 		mov $r14 1
 		iowr I[$r11 + 0x000] $r14	// FIFO_ACK
@@ -441,18 +486,18 @@ ih:
 	// context switch request?
 	ih_no_fifo:
 	and $r11 $r10 0x00000100
-	bra e ih_no_ctxsw
+	bra e #ih_no_ctxsw
 		// enqueue a context switch for later processing
-		mov $r13 cmd_queue
+		mov $r13 #cmd_queue
 		mov $r14 0x4001
-		call queue_put
+		call #queue_put
 
 	// anything we didn't handle, bring it to the host's attention
 	ih_no_ctxsw:
 	mov $r11 0x104
 	not b32 $r11
 	and $r11 $r10 $r11
-	bra e ih_no_other
+	bra e #ih_no_other
 		mov $r10 0xc1c
 		shl b32 $r10 6
 		iowr I[$r10] $r11	// INTR_UP_SET
@@ -478,11 +523,11 @@ ctx_4160s:
 	mov $r14 0x4160
 	sethi $r14 0x400000
 	mov $r15 1
-	call nv_wr32
+	call #nv_wr32
 	ctx_4160s_wait:
-		call nv_rd32
+		call #nv_rd32
 		xbit $r15 $r15 4
-		bra e ctx_4160s_wait
+		bra e #ctx_4160s_wait
 	ret
 
 // Without clearing again at end of xfer, some things cause PGRAPH
@@ -492,7 +537,7 @@ ctx_4160c:
 	mov $r14 0x4160
 	sethi $r14 0x400000
 	clear b32 $r15
-	call nv_wr32
+	call #nv_wr32
 	ret
 
 // Again, not real sure
@@ -503,7 +548,7 @@ ctx_4170s:
 	mov $r14 0x4170
 	sethi $r14 0x400000
 	or $r15 0x10
-	call nv_wr32
+	call #nv_wr32
 	ret
 
 // Waits for a ctx_4170s() call to complete
@@ -511,9 +556,9 @@ ctx_4170s:
 ctx_4170w:
 	mov $r14 0x4170
 	sethi $r14 0x400000
-	call nv_rd32
+	call #nv_rd32
 	and $r15 0x10
-	bra ne ctx_4170w
+	bra ne #ctx_4170w
 	ret
 
 // Disables various things, waits a bit, and re-enables them..
@@ -530,7 +575,7 @@ ctx_redswitch:
 	mov $r15 8
 	ctx_redswitch_delay:
 		sub b32 $r15 1
-		bra ne ctx_redswitch_delay
+		bra ne #ctx_redswitch_delay
 	mov $r15 0x770
 	iowr I[$r14] $r15	// HUB_RED_SWITCH = ENABLE_ALL, POWER_ALL
 	ret
@@ -546,10 +591,10 @@ ctx_86c:
 	iowr I[$r14] $r15	// HUB(0x86c) = val
 	mov $r14 -0x75ec
 	sethi $r14 0x400000
-	call nv_wr32		// ROP(0xa14) = val
+	call #nv_wr32		// ROP(0xa14) = val
 	mov $r14 -0x5794
 	sethi $r14 0x410000
-	call nv_wr32		// GPC(0x86c) = val
+	call #nv_wr32		// GPC(0x86c) = val
 	ret
 
 // ctx_load - load's a channel's ctxctl data, and selects its vm
@@ -561,7 +606,7 @@ ctx_load:
 
 	// switch to channel, somewhat magic in parts..
 	mov $r10 12		// DONE_UNK12
-	call wait_donez
+	call #wait_donez
 	mov $r1 0xa24
 	shl b32 $r1 6
 	iowr I[$r1 + 0x000] $r0	// 0x409a24
@@ -576,7 +621,7 @@ ctx_load:
 	ctx_chan_wait_0:
 		iord $r4 I[$r1 + 0x100]
 		and $r4 0x1f
-		bra ne ctx_chan_wait_0
+		bra ne #ctx_chan_wait_0
 	iowr I[$r3 + 0x000] $r2	// CHAN_CUR
 
 	// load channel header, fetch PGRAPH context pointer
@@ -595,19 +640,19 @@ ctx_load:
 	sethi $r2 0x80000000
 	iowr I[$r1 + 0x000] $r2		// MEM_TARGET = vram
 	mov $r1 0x10			// chan + 0x0210
-	mov $r2 xfer_data
+	mov $r2 #xfer_data
 	sethi $r2 0x00020000		// 16 bytes
 	xdld $r1 $r2
 	xdwait
 	trace_clr(T_LCHAN)
 
 	// update current context
-	ld b32 $r1 D[$r0 + xfer_data + 4]
+	ld b32 $r1 D[$r0 + #xfer_data + 4]
 	shl b32 $r1 24
-	ld b32 $r2 D[$r0 + xfer_data + 0]
+	ld b32 $r2 D[$r0 + #xfer_data + 0]
 	shr b32 $r2 8
 	or $r1 $r2
-	st b32 D[$r0 + ctx_current] $r1
+	st b32 D[$r0 + #ctx_current] $r1
 
 	// set transfer base to start of context, and fetch context header
 	trace_set(T_LCTXH)
@@ -618,7 +663,7 @@ ctx_load:
 	mov $r1 0xa20
 	shl b32 $r1 6
 	iowr I[$r1 + 0x000] $r2		// MEM_TARGET = vm
-	mov $r1 chan_data
+	mov $r1 #chan_data
 	sethi $r1 0x00060000		// 256 bytes
 	xdld $r0 $r1
 	xdwait
@@ -635,10 +680,10 @@ ctx_load:
 // In: $r2 channel address
 //
 ctx_chan:
-	call ctx_4160s
-	call ctx_load
+	call #ctx_4160s
+	call #ctx_load
 	mov $r10 12			// DONE_UNK12
-	call wait_donez
+	call #wait_donez
 	mov $r1 0xa10
 	shl b32 $r1 6
 	mov $r2 5
@@ -646,8 +691,8 @@ ctx_chan:
 	ctx_chan_wait:
 		iord $r2 I[$r1 + 0x000]
 		or $r2 $r2
-		bra ne ctx_chan_wait
-	call ctx_4160c
+		bra ne #ctx_chan_wait
+	call #ctx_4160c
 	ret
 
 // Execute per-context state overrides list
@@ -661,7 +706,7 @@ ctx_chan:
 //
 ctx_mmio_exec:
 	// set transfer base to be the mmio list
-	ld b32 $r3 D[$r0 + chan_mmio_address]
+	ld b32 $r3 D[$r0 + #chan_mmio_address]
 	mov $r2 0xa04
 	shl b32 $r2 6
 	iowr I[$r2 + 0x000] $r3		// MEM_BASE
@@ -670,31 +715,31 @@ ctx_mmio_exec:
 	ctx_mmio_loop:
 		// fetch next 256 bytes of mmio list if necessary
 		and $r4 $r3 0xff
-		bra ne ctx_mmio_pull
-			mov $r5 xfer_data
+		bra ne #ctx_mmio_pull
+			mov $r5 #xfer_data
 			sethi $r5 0x00060000	// 256 bytes
 			xdld $r3 $r5
 			xdwait
 
 		// execute a single list entry
 		ctx_mmio_pull:
-		ld b32 $r14 D[$r4 + xfer_data + 0x00]
-		ld b32 $r15 D[$r4 + xfer_data + 0x04]
-		call nv_wr32
+		ld b32 $r14 D[$r4 + #xfer_data + 0x00]
+		ld b32 $r15 D[$r4 + #xfer_data + 0x04]
+		call #nv_wr32
 
 		// next!
 		add b32 $r3 8
 		sub b32 $r1 1
-		bra ne ctx_mmio_loop
+		bra ne #ctx_mmio_loop
 
 	// set transfer base back to the current context
 	ctx_mmio_done:
-	ld b32 $r3 D[$r0 + ctx_current]
+	ld b32 $r3 D[$r0 + #ctx_current]
 	iowr I[$r2 + 0x000] $r3		// MEM_BASE
 
 	// disable the mmio list now, we don't need/want to execute it again
-	st b32 D[$r0 + chan_mmio_count] $r0
-	mov $r1 chan_data
+	st b32 D[$r0 + #chan_mmio_count] $r0
+	mov $r1 #chan_data
 	sethi $r1 0x00060000		// 256 bytes
 	xdst $r0 $r1
 	xdwait
@@ -709,46 +754,46 @@ ctx_mmio_exec:
 //		on load it means: "a save preceeded this load"
 //
 ctx_xfer:
-	bra not $p1 ctx_xfer_pre
-	bra $p2 ctx_xfer_pre_load
+	bra not $p1 #ctx_xfer_pre
+	bra $p2 #ctx_xfer_pre_load
 	ctx_xfer_pre:
 		mov $r15 0x10
-		call ctx_86c
-		call ctx_4160s
-		bra not $p1 ctx_xfer_exec
+		call #ctx_86c
+		call #ctx_4160s
+		bra not $p1 #ctx_xfer_exec
 
 	ctx_xfer_pre_load:
 		mov $r15 2
-		call ctx_4170s
-		call ctx_4170w
-		call ctx_redswitch
+		call #ctx_4170s
+		call #ctx_4170w
+		call #ctx_redswitch
 		clear b32 $r15
-		call ctx_4170s
-		call ctx_load
+		call #ctx_4170s
+		call #ctx_load
 
 	// fetch context pointer, and initiate xfer on all GPCs
 	ctx_xfer_exec:
-	ld b32 $r1 D[$r0 + ctx_current]
+	ld b32 $r1 D[$r0 + #ctx_current]
 	mov $r2 0x414
 	shl b32 $r2 6
 	iowr I[$r2 + 0x000] $r0	// BAR_STATUS = reset
 	mov $r14 -0x5b00
 	sethi $r14 0x410000
 	mov b32 $r15 $r1
-	call nv_wr32		// GPC_BCAST_WRCMD_DATA = ctx pointer
+	call #nv_wr32		// GPC_BCAST_WRCMD_DATA = ctx pointer
 	add b32 $r14 4
 	xbit $r15 $flags $p1
 	xbit $r2 $flags $p2
 	shl b32 $r2 1
 	or $r15 $r2
-	call nv_wr32		// GPC_BCAST_WRCMD_CMD = GPC_XFER(type)
+	call #nv_wr32		// GPC_BCAST_WRCMD_CMD = GPC_XFER(type)
 
 	// strands
 	mov $r1 0x4afc
 	sethi $r1 0x20000
 	mov $r2 0xc
 	iowr I[$r1] $r2		// STRAND_CMD(0x3f) = 0x0c
-	call strand_wait
+	call #strand_wait
 	mov $r2 0x47fc
 	sethi $r2 0x20000
 	iowr I[$r2] $r0		// STRAND_FIRST_GENE(0x3f) = 0x00
@@ -760,22 +805,22 @@ ctx_xfer:
 	xbit $r10 $flags $p1	// direction
 	or $r10 6		// first, last
 	mov $r11 0		// base = 0
-	ld b32 $r12 D[$r0 + hub_mmio_list_head]
-	ld b32 $r13 D[$r0 + hub_mmio_list_tail]
+	ld b32 $r12 D[$r0 + #hub_mmio_list_head]
+	ld b32 $r13 D[$r0 + #hub_mmio_list_tail]
 	mov $r14 0		// not multi
-	call mmctx_xfer
+	call #mmctx_xfer
 
 	// wait for GPCs to all complete
 	mov $r10 8		// DONE_BAR
-	call wait_doneo
+	call #wait_doneo
 
 	// wait for strand xfer to complete
-	call strand_wait
+	call #strand_wait
 
 	// post-op
-	bra $p1 ctx_xfer_post
+	bra $p1 #ctx_xfer_post
 		mov $r10 12		// DONE_UNK12
-		call wait_donez
+		call #wait_donez
 		mov $r1 0xa10
 		shl b32 $r1 6
 		mov $r2 5
@@ -783,27 +828,27 @@ ctx_xfer:
 		ctx_xfer_post_save_wait:
 			iord $r2 I[$r1]
 			or $r2 $r2
-			bra ne ctx_xfer_post_save_wait
+			bra ne #ctx_xfer_post_save_wait
 
-	bra $p2 ctx_xfer_done
+	bra $p2 #ctx_xfer_done
 	ctx_xfer_post:
 		mov $r15 2
-		call ctx_4170s
+		call #ctx_4170s
 		clear b32 $r15
-		call ctx_86c
-		call strand_post
-		call ctx_4170w
+		call #ctx_86c
+		call #strand_post
+		call #ctx_4170w
 		clear b32 $r15
-		call ctx_4170s
+		call #ctx_4170s
 
-		bra not $p1 ctx_xfer_no_post_mmio
-		ld b32 $r1 D[$r0 + chan_mmio_count]
+		bra not $p1 #ctx_xfer_no_post_mmio
+		ld b32 $r1 D[$r0 + #chan_mmio_count]
 		or $r1 $r1
-		bra e ctx_xfer_no_post_mmio
-			call ctx_mmio_exec
+		bra e #ctx_xfer_no_post_mmio
+			call #ctx_mmio_exec
 
 		ctx_xfer_no_post_mmio:
-		call ctx_4160c
+		call #ctx_4160c
 
 	ctx_xfer_done:
 	ret
diff --git a/drivers/gpu/drm/nouveau/nvc0_grhub.fuc.h b/drivers/gpu/drm/nouveau/nvc0_grhub.fuc.h
index 241d3263f1e5..c5ed307abeb9 100644
--- a/drivers/gpu/drm/nouveau/nvc0_grhub.fuc.h
+++ b/drivers/gpu/drm/nouveau/nvc0_grhub.fuc.h
@@ -23,19 +23,21 @@ uint32_t nvc0_grhub_data[] = {
 	0x00000000,
 	0x00000000,
 	0x000000c0,
-	0x01340098,
+	0x013c00a0,
 	0x000000c1,
-	0x01380098,
+	0x014000a0,
 	0x000000c3,
-	0x01340098,
+	0x013c00a0,
 	0x000000c4,
-	0x01340098,
+	0x013c00a0,
 	0x000000c8,
-	0x01340098,
+	0x013c00a0,
 	0x000000ce,
-	0x01340098,
+	0x013c00a0,
 	0x000000cf,
-	0x01340098,
+	0x013c00a0,
+	0x000000d9,
+	0x01dc0140,
 	0x00000000,
 	0x0417e91c,
 	0x04400204,
@@ -77,47 +79,45 @@ uint32_t nvc0_grhub_data[] = {
 	0x0c408900,
 	0x00408980,
 	0x044064c0,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
+	0x0417e91c,
+	0x04400204,
+	0x24404004,
+	0x00404044,
+	0x34404094,
+	0x184040d0,
+	0x004040f8,
+	0x08404130,
+	0x08404150,
+	0x04404164,
+	0x04404178,
+	0x1c404200,
+	0x34404404,
+	0x0c404460,
+	0x00404480,
+	0x00404498,
+	0x0c404604,
+	0x7c404618,
+	0x50404698,
+	0x044046f0,
+	0x54404700,
+	0x00405800,
+	0x08405830,
+	0x00405854,
+	0x0c405870,
+	0x04405a00,
+	0x00405a18,
+	0x00406020,
+	0x0c406028,
+	0x044064a8,
+	0x104064b4,
+	0x00407804,
+	0x1440780c,
+	0x004078bc,
+	0x18408000,
+	0x00408064,
+	0x08408800,
+	0x0c408900,
+	0x00408980,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/nvc0_pm.c b/drivers/gpu/drm/nouveau/nvc0_pm.c
index 929aded35cb5..e9992f62c1c0 100644
--- a/drivers/gpu/drm/nouveau/nvc0_pm.c
+++ b/drivers/gpu/drm/nouveau/nvc0_pm.c
@@ -153,3 +153,240 @@ nvc0_pm_clocks_get(struct drm_device *dev, struct nouveau_pm_level *perflvl)
 	perflvl->vdec   = read_clk(dev, 0x0e);
 	return 0;
 }
+
+struct nvc0_pm_clock {
+	u32 freq;
+	u32 ssel;
+	u32 mdiv;
+	u32 dsrc;
+	u32 ddiv;
+	u32 coef;
+};
+
+struct nvc0_pm_state {
+	struct nvc0_pm_clock eng[16];
+};
+
+static u32
+calc_div(struct drm_device *dev, int clk, u32 ref, u32 freq, u32 *ddiv)
+{
+	u32 div = min((ref * 2) / freq, (u32)65);
+	if (div < 2)
+		div = 2;
+
+	*ddiv = div - 2;
+	return (ref * 2) / div;
+}
+
+static u32
+calc_src(struct drm_device *dev, int clk, u32 freq, u32 *dsrc, u32 *ddiv)
+{
+	u32 sclk;
+
+	/* use one of the fixed frequencies if possible */
+	*ddiv = 0x00000000;
+	switch (freq) {
+	case  27000:
+	case 108000:
+		*dsrc = 0x00000000;
+		if (freq == 108000)
+			*dsrc |= 0x00030000;
+		return freq;
+	case 100000:
+		*dsrc = 0x00000002;
+		return freq;
+	default:
+		*dsrc = 0x00000003;
+		break;
+	}
+
+	/* otherwise, calculate the closest divider */
+	sclk = read_vco(dev, clk);
+	if (clk < 7)
+		sclk = calc_div(dev, clk, sclk, freq, ddiv);
+	return sclk;
+}
+
+static u32
+calc_pll(struct drm_device *dev, int clk, u32 freq, u32 *coef)
+{
+	struct pll_lims limits;
+	int N, M, P, ret;
+
+	ret = get_pll_limits(dev, 0x137000 + (clk * 0x20), &limits);
+	if (ret)
+		return 0;
+
+	limits.refclk = read_div(dev, clk, 0x137120, 0x137140);
+	if (!limits.refclk)
+		return 0;
+
+	ret = nva3_calc_pll(dev, &limits, freq, &N, NULL, &M, &P);
+	if (ret <= 0)
+		return 0;
+
+	*coef = (P << 16) | (N << 8) | M;
+	return ret;
+}
+
+/* A (likely rather simplified and incomplete) view of the clock tree
+ *
+ * Key:
+ *
+ * S: source select
+ * D: divider
+ * P: pll
+ * F: switch
+ *
+ * Engine clocks:
+ *
+ * 137250(D) ---- 137100(F0) ---- 137160(S)/1371d0(D) ------------------- ref
+ *                      (F1) ---- 1370X0(P) ---- 137120(S)/137140(D) ---- ref
+ *
+ * Not all registers exist for all clocks.  For example: clocks >= 8 don't
+ * have their own PLL (all tied to clock 7's PLL when in PLL mode), nor do
+ * they have the divider at 1371d0, though the source selection at 137160
+ * still exists.  You must use the divider at 137250 for these instead.
+ *
+ * Memory clock:
+ *
+ * TBD, read_mem() above is likely very wrong...
+ *
+ */
+
+static int
+calc_clk(struct drm_device *dev, int clk, struct nvc0_pm_clock *info, u32 freq)
+{
+	u32 src0, div0, div1D, div1P = 0;
+	u32 clk0, clk1 = 0;
+
+	/* invalid clock domain */
+	if (!freq)
+		return 0;
+
+	/* first possible path, using only dividers */
+	clk0 = calc_src(dev, clk, freq, &src0, &div0);
+	clk0 = calc_div(dev, clk, clk0, freq, &div1D);
+
+	/* see if we can get any closer using PLLs */
+	if (clk0 != freq) {
+		if (clk < 7)
+			clk1 = calc_pll(dev, clk, freq, &info->coef);
+		else
+			clk1 = read_pll(dev, 0x1370e0);
+		clk1 = calc_div(dev, clk, clk1, freq, &div1P);
+	}
+
+	/* select the method which gets closest to target freq */
+	if (abs((int)freq - clk0) <= abs((int)freq - clk1)) {
+		info->dsrc = src0;
+		if (div0) {
+			info->ddiv |= 0x80000000;
+			info->ddiv |= div0 << 8;
+			info->ddiv |= div0;
+		}
+		if (div1D) {
+			info->mdiv |= 0x80000000;
+			info->mdiv |= div1D;
+		}
+		info->ssel = 0;
+		info->freq = clk0;
+	} else {
+		if (div1P) {
+			info->mdiv |= 0x80000000;
+			info->mdiv |= div1P << 8;
+		}
+		info->ssel = (1 << clk);
+		info->freq = clk1;
+	}
+
+	return 0;
+}
+
+void *
+nvc0_pm_clocks_pre(struct drm_device *dev, struct nouveau_pm_level *perflvl)
+{
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
+	struct nvc0_pm_state *info;
+	int ret;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return ERR_PTR(-ENOMEM);
+
+	/* NFI why this is still in the performance table, the ROPCs appear
+	 * to get their clock from clock 2 ("hub07", actually hub05 on this
+	 * chip, but, anyway...) as well.  nvatiming confirms hub05 and ROP
+	 * are always the same freq with the binary driver even when the
+	 * performance table says they should differ.
+	 */
+	if (dev_priv->chipset == 0xd9)
+		perflvl->rop = 0;
+
+	if ((ret = calc_clk(dev, 0x00, &info->eng[0x00], perflvl->shader)) ||
+	    (ret = calc_clk(dev, 0x01, &info->eng[0x01], perflvl->rop)) ||
+	    (ret = calc_clk(dev, 0x02, &info->eng[0x02], perflvl->hub07)) ||
+	    (ret = calc_clk(dev, 0x07, &info->eng[0x07], perflvl->hub06)) ||
+	    (ret = calc_clk(dev, 0x08, &info->eng[0x08], perflvl->hub01)) ||
+	    (ret = calc_clk(dev, 0x09, &info->eng[0x09], perflvl->copy)) ||
+	    (ret = calc_clk(dev, 0x0c, &info->eng[0x0c], perflvl->daemon)) ||
+	    (ret = calc_clk(dev, 0x0e, &info->eng[0x0e], perflvl->vdec))) {
+		kfree(info);
+		return ERR_PTR(ret);
+	}
+
+	return info;
+}
+
+static void
+prog_clk(struct drm_device *dev, int clk, struct nvc0_pm_clock *info)
+{
+	/* program dividers at 137160/1371d0 first */
+	if (clk < 7 && !info->ssel) {
+		nv_mask(dev, 0x1371d0 + (clk * 0x04), 0x80003f3f, info->ddiv);
+		nv_wr32(dev, 0x137160 + (clk * 0x04), info->dsrc);
+	}
+
+	/* switch clock to non-pll mode */
+	nv_mask(dev, 0x137100, (1 << clk), 0x00000000);
+	nv_wait(dev, 0x137100, (1 << clk), 0x00000000);
+
+	/* reprogram pll */
+	if (clk < 7) {
+		/* make sure it's disabled first... */
+		u32 base = 0x137000 + (clk * 0x20);
+		u32 ctrl = nv_rd32(dev, base + 0x00);
+		if (ctrl & 0x00000001) {
+			nv_mask(dev, base + 0x00, 0x00000004, 0x00000000);
+			nv_mask(dev, base + 0x00, 0x00000001, 0x00000000);
+		}
+		/* program it to new values, if necessary */
+		if (info->ssel) {
+			nv_wr32(dev, base + 0x04, info->coef);
+			nv_mask(dev, base + 0x00, 0x00000001, 0x00000001);
+			nv_wait(dev, base + 0x00, 0x00020000, 0x00020000);
+			nv_mask(dev, base + 0x00, 0x00020004, 0x00000004);
+		}
+	}
+
+	/* select pll/non-pll mode, and program final clock divider */
+	nv_mask(dev, 0x137100, (1 << clk), info->ssel);
+	nv_wait(dev, 0x137100, (1 << clk), info->ssel);
+	nv_mask(dev, 0x137250 + (clk * 0x04), 0x00003f3f, info->mdiv);
+}
+
+int
+nvc0_pm_clocks_set(struct drm_device *dev, void *data)
+{
+	struct nvc0_pm_state *info = data;
+	int i;
+
+	for (i = 0; i < 16; i++) {
+		if (!info->eng[i].freq)
+			continue;
+		prog_clk(dev, i, &info->eng[i]);
+	}
+
+	kfree(info);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nvd0_display.c b/drivers/gpu/drm/nouveau/nvd0_display.c
index cb006a718e70..d2ba2f07400b 100644
--- a/drivers/gpu/drm/nouveau/nvd0_display.c
+++ b/drivers/gpu/drm/nouveau/nvd0_display.c
@@ -35,12 +35,34 @@
 #include "nouveau_fb.h"
 #include "nv50_display.h"
 
+#define EVO_DMA_NR 9
+
+#define EVO_MASTER  (0x00)
+#define EVO_FLIP(c) (0x01 + (c))
+#define EVO_OVLY(c) (0x05 + (c))
+#define EVO_OIMM(c) (0x09 + (c))
+#define EVO_CURS(c) (0x0d + (c))
+
+/* offsets in shared sync bo of various structures */
+#define EVO_SYNC(c, o) ((c) * 0x0100 + (o))
+#define EVO_MAST_NTFY     EVO_SYNC(  0, 0x00)
+#define EVO_FLIP_SEM0(c)  EVO_SYNC((c), 0x00)
+#define EVO_FLIP_SEM1(c)  EVO_SYNC((c), 0x10)
+
+struct evo {
+	int idx;
+	dma_addr_t handle;
+	u32 *ptr;
+	struct {
+		u32 offset;
+		u16 value;
+	} sem;
+};
+
 struct nvd0_display {
 	struct nouveau_gpuobj *mem;
-	struct {
-		dma_addr_t handle;
-		u32 *ptr;
-	} evo[1];
+	struct nouveau_bo *sync;
+	struct evo evo[9];
 
 	struct tasklet_struct tasklet;
 	u32 modeset;
@@ -53,6 +75,15 @@ nvd0_display(struct drm_device *dev)
 	return dev_priv->engine.display.priv;
 }
 
+static struct drm_crtc *
+nvd0_display_crtc_get(struct drm_encoder *encoder)
+{
+	return nouveau_encoder(encoder)->crtc;
+}
+
+/******************************************************************************
+ * EVO channel helpers
+ *****************************************************************************/
 static inline int
 evo_icmd(struct drm_device *dev, int id, u32 mthd, u32 data)
 {
@@ -84,6 +115,9 @@ evo_wait(struct drm_device *dev, int id, int nr)
 		put = 0;
 	}
 
+	if (nouveau_reg_debug & NOUVEAU_REG_DEBUG_EVO)
+		NV_INFO(dev, "Evo%d: %p START\n", id, disp->evo[id].ptr + put);
+
 	return disp->evo[id].ptr + put;
 }
 
@@ -91,40 +125,264 @@ static void
 evo_kick(u32 *push, struct drm_device *dev, int id)
 {
 	struct nvd0_display *disp = nvd0_display(dev);
+
+	if (nouveau_reg_debug & NOUVEAU_REG_DEBUG_EVO) {
+		u32 curp = nv_rd32(dev, 0x640000 + (id * 0x1000)) >> 2;
+		u32 *cur = disp->evo[id].ptr + curp;
+
+		while (cur < push)
+			NV_INFO(dev, "Evo%d: 0x%08x\n", id, *cur++);
+		NV_INFO(dev, "Evo%d: %p KICK!\n", id, push);
+	}
+
 	nv_wr32(dev, 0x640000 + (id * 0x1000), (push - disp->evo[id].ptr) << 2);
 }
 
 #define evo_mthd(p,m,s) *((p)++) = (((s) << 18) | (m))
 #define evo_data(p,d)   *((p)++) = (d)
 
-static struct drm_crtc *
-nvd0_display_crtc_get(struct drm_encoder *encoder)
+static int
+evo_init_dma(struct drm_device *dev, int ch)
 {
-	return nouveau_encoder(encoder)->crtc;
+	struct nvd0_display *disp = nvd0_display(dev);
+	u32 flags;
+
+	flags = 0x00000000;
+	if (ch == EVO_MASTER)
+		flags |= 0x01000000;
+
+	nv_wr32(dev, 0x610494 + (ch * 0x0010), (disp->evo[ch].handle >> 8) | 3);
+	nv_wr32(dev, 0x610498 + (ch * 0x0010), 0x00010000);
+	nv_wr32(dev, 0x61049c + (ch * 0x0010), 0x00000001);
+	nv_mask(dev, 0x610490 + (ch * 0x0010), 0x00000010, 0x00000010);
+	nv_wr32(dev, 0x640000 + (ch * 0x1000), 0x00000000);
+	nv_wr32(dev, 0x610490 + (ch * 0x0010), 0x00000013 | flags);
+	if (!nv_wait(dev, 0x610490 + (ch * 0x0010), 0x80000000, 0x00000000)) {
+		NV_ERROR(dev, "PDISP: ch%d 0x%08x\n", ch,
+			      nv_rd32(dev, 0x610490 + (ch * 0x0010)));
+		return -EBUSY;
+	}
+
+	nv_mask(dev, 0x610090, (1 << ch), (1 << ch));
+	nv_mask(dev, 0x6100a0, (1 << ch), (1 << ch));
+	return 0;
+}
+
+static void
+evo_fini_dma(struct drm_device *dev, int ch)
+{
+	if (!(nv_rd32(dev, 0x610490 + (ch * 0x0010)) & 0x00000010))
+		return;
+
+	nv_mask(dev, 0x610490 + (ch * 0x0010), 0x00000010, 0x00000000);
+	nv_mask(dev, 0x610490 + (ch * 0x0010), 0x00000003, 0x00000000);
+	nv_wait(dev, 0x610490 + (ch * 0x0010), 0x80000000, 0x00000000);
+	nv_mask(dev, 0x610090, (1 << ch), 0x00000000);
+	nv_mask(dev, 0x6100a0, (1 << ch), 0x00000000);
+}
+
+static inline void
+evo_piow(struct drm_device *dev, int ch, u16 mthd, u32 data)
+{
+	nv_wr32(dev, 0x640000 + (ch * 0x1000) + mthd, data);
+}
+
+static int
+evo_init_pio(struct drm_device *dev, int ch)
+{
+	nv_wr32(dev, 0x610490 + (ch * 0x0010), 0x00000001);
+	if (!nv_wait(dev, 0x610490 + (ch * 0x0010), 0x00010000, 0x00010000)) {
+		NV_ERROR(dev, "PDISP: ch%d 0x%08x\n", ch,
+			      nv_rd32(dev, 0x610490 + (ch * 0x0010)));
+		return -EBUSY;
+	}
+
+	nv_mask(dev, 0x610090, (1 << ch), (1 << ch));
+	nv_mask(dev, 0x6100a0, (1 << ch), (1 << ch));
+	return 0;
+}
+
+static void
+evo_fini_pio(struct drm_device *dev, int ch)
+{
+	if (!(nv_rd32(dev, 0x610490 + (ch * 0x0010)) & 0x00000001))
+		return;
+
+	nv_mask(dev, 0x610490 + (ch * 0x0010), 0x00000010, 0x00000010);
+	nv_mask(dev, 0x610490 + (ch * 0x0010), 0x00000001, 0x00000000);
+	nv_wait(dev, 0x610490 + (ch * 0x0010), 0x00010000, 0x00000000);
+	nv_mask(dev, 0x610090, (1 << ch), 0x00000000);
+	nv_mask(dev, 0x6100a0, (1 << ch), 0x00000000);
+}
+
+static bool
+evo_sync_wait(void *data)
+{
+	return nouveau_bo_rd32(data, EVO_MAST_NTFY) != 0x00000000;
+}
+
+static int
+evo_sync(struct drm_device *dev, int ch)
+{
+	struct nvd0_display *disp = nvd0_display(dev);
+	u32 *push = evo_wait(dev, ch, 8);
+	if (push) {
+		nouveau_bo_wr32(disp->sync, EVO_MAST_NTFY, 0x00000000);
+		evo_mthd(push, 0x0084, 1);
+		evo_data(push, 0x80000000 | EVO_MAST_NTFY);
+		evo_mthd(push, 0x0080, 2);
+		evo_data(push, 0x00000000);
+		evo_data(push, 0x00000000);
+		evo_kick(push, dev, ch);
+		if (nv_wait_cb(dev, evo_sync_wait, disp->sync))
+			return 0;
+	}
+
+	return -EBUSY;
+}
+
+/******************************************************************************
+ * Page flipping channel
+ *****************************************************************************/
+struct nouveau_bo *
+nvd0_display_crtc_sema(struct drm_device *dev, int crtc)
+{
+	return nvd0_display(dev)->sync;
+}
+
+void
+nvd0_display_flip_stop(struct drm_crtc *crtc)
+{
+	struct nvd0_display *disp = nvd0_display(crtc->dev);
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+	struct evo *evo = &disp->evo[EVO_FLIP(nv_crtc->index)];
+	u32 *push;
+
+	push = evo_wait(crtc->dev, evo->idx, 8);
+	if (push) {
+		evo_mthd(push, 0x0084, 1);
+		evo_data(push, 0x00000000);
+		evo_mthd(push, 0x0094, 1);
+		evo_data(push, 0x00000000);
+		evo_mthd(push, 0x00c0, 1);
+		evo_data(push, 0x00000000);
+		evo_mthd(push, 0x0080, 1);
+		evo_data(push, 0x00000000);
+		evo_kick(push, crtc->dev, evo->idx);
+	}
+}
+
+int
+nvd0_display_flip_next(struct drm_crtc *crtc, struct drm_framebuffer *fb,
+		       struct nouveau_channel *chan, u32 swap_interval)
+{
+	struct nouveau_framebuffer *nv_fb = nouveau_framebuffer(fb);
+	struct nvd0_display *disp = nvd0_display(crtc->dev);
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+	struct evo *evo = &disp->evo[EVO_FLIP(nv_crtc->index)];
+	u64 offset;
+	u32 *push;
+	int ret;
+
+	swap_interval <<= 4;
+	if (swap_interval == 0)
+		swap_interval |= 0x100;
+
+	push = evo_wait(crtc->dev, evo->idx, 128);
+	if (unlikely(push == NULL))
+		return -EBUSY;
+
+	/* synchronise with the rendering channel, if necessary */
+	if (likely(chan)) {
+		ret = RING_SPACE(chan, 10);
+		if (ret)
+			return ret;
+
+		offset  = chan->dispc_vma[nv_crtc->index].offset;
+		offset += evo->sem.offset;
+
+		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0010, 4);
+		OUT_RING  (chan, upper_32_bits(offset));
+		OUT_RING  (chan, lower_32_bits(offset));
+		OUT_RING  (chan, 0xf00d0000 | evo->sem.value);
+		OUT_RING  (chan, 0x1002);
+		BEGIN_NVC0(chan, 2, NvSubM2MF, 0x0010, 4);
+		OUT_RING  (chan, upper_32_bits(offset));
+		OUT_RING  (chan, lower_32_bits(offset ^ 0x10));
+		OUT_RING  (chan, 0x74b1e000);
+		OUT_RING  (chan, 0x1001);
+		FIRE_RING (chan);
+	} else {
+		nouveau_bo_wr32(disp->sync, evo->sem.offset / 4,
+				0xf00d0000 | evo->sem.value);
+		evo_sync(crtc->dev, EVO_MASTER);
+	}
+
+	/* queue the flip */
+	evo_mthd(push, 0x0100, 1);
+	evo_data(push, 0xfffe0000);
+	evo_mthd(push, 0x0084, 1);
+	evo_data(push, swap_interval);
+	if (!(swap_interval & 0x00000100)) {
+		evo_mthd(push, 0x00e0, 1);
+		evo_data(push, 0x40000000);
+	}
+	evo_mthd(push, 0x0088, 4);
+	evo_data(push, evo->sem.offset);
+	evo_data(push, 0xf00d0000 | evo->sem.value);
+	evo_data(push, 0x74b1e000);
+	evo_data(push, NvEvoSync);
+	evo_mthd(push, 0x00a0, 2);
+	evo_data(push, 0x00000000);
+	evo_data(push, 0x00000000);
+	evo_mthd(push, 0x00c0, 1);
+	evo_data(push, nv_fb->r_dma);
+	evo_mthd(push, 0x0110, 2);
+	evo_data(push, 0x00000000);
+	evo_data(push, 0x00000000);
+	evo_mthd(push, 0x0400, 5);
+	evo_data(push, nv_fb->nvbo->bo.offset >> 8);
+	evo_data(push, 0);
+	evo_data(push, (fb->height << 16) | fb->width);
+	evo_data(push, nv_fb->r_pitch);
+	evo_data(push, nv_fb->r_format);
+	evo_mthd(push, 0x0080, 1);
+	evo_data(push, 0x00000000);
+	evo_kick(push, crtc->dev, evo->idx);
+
+	evo->sem.offset ^= 0x10;
+	evo->sem.value++;
+	return 0;
 }
 
 /******************************************************************************
  * CRTC
  *****************************************************************************/
 static int
-nvd0_crtc_set_dither(struct nouveau_crtc *nv_crtc, bool on, bool update)
+nvd0_crtc_set_dither(struct nouveau_crtc *nv_crtc, bool update)
 {
 	struct drm_device *dev = nv_crtc->base.dev;
-	u32 *push, mode;
+	struct nouveau_connector *nv_connector;
+	struct drm_connector *connector;
+	u32 *push, mode = 0x00;
 
-	mode = 0x00000000;
-	if (on) {
-		/* 0x11: 6bpc dynamic 2x2
-		 * 0x13: 8bpc dynamic 2x2
-		 * 0x19: 6bpc static 2x2
-		 * 0x1b: 8bpc static 2x2
-		 * 0x21: 6bpc temporal
-		 * 0x23: 8bpc temporal
-		 */
-		mode = 0x00000011;
+	nv_connector = nouveau_crtc_connector_get(nv_crtc);
+	connector = &nv_connector->base;
+	if (nv_connector->dithering_mode == DITHERING_MODE_AUTO) {
+		if (nv_crtc->base.fb->depth > connector->display_info.bpc * 3)
+			mode = DITHERING_MODE_DYNAMIC2X2;
+	} else {
+		mode = nv_connector->dithering_mode;
+	}
+
+	if (nv_connector->dithering_depth == DITHERING_DEPTH_AUTO) {
+		if (connector->display_info.bpc >= 8)
+			mode |= DITHERING_DEPTH_8BPC;
+	} else {
+		mode |= nv_connector->dithering_depth;
 	}
 
-	push = evo_wait(dev, 0, 4);
+	push = evo_wait(dev, EVO_MASTER, 4);
 	if (push) {
 		evo_mthd(push, 0x0490 + (nv_crtc->index * 0x300), 1);
 		evo_data(push, mode);
@@ -132,63 +390,98 @@ nvd0_crtc_set_dither(struct nouveau_crtc *nv_crtc, bool on, bool update)
 			evo_mthd(push, 0x0080, 1);
 			evo_data(push, 0x00000000);
 		}
-		evo_kick(push, dev, 0);
+		evo_kick(push, dev, EVO_MASTER);
 	}
 
 	return 0;
 }
 
 static int
-nvd0_crtc_set_scale(struct nouveau_crtc *nv_crtc, int type, bool update)
+nvd0_crtc_set_scale(struct nouveau_crtc *nv_crtc, bool update)
 {
-	struct drm_display_mode *mode = &nv_crtc->base.mode;
+	struct drm_display_mode *omode, *umode = &nv_crtc->base.mode;
 	struct drm_device *dev = nv_crtc->base.dev;
+	struct drm_crtc *crtc = &nv_crtc->base;
 	struct nouveau_connector *nv_connector;
-	u32 *push, outX, outY;
-
-	outX = mode->hdisplay;
-	outY = mode->vdisplay;
+	int mode = DRM_MODE_SCALE_NONE;
+	u32 oX, oY, *push;
 
+	/* start off at the resolution we programmed the crtc for, this
+	 * effectively handles NONE/FULL scaling
+	 */
 	nv_connector = nouveau_crtc_connector_get(nv_crtc);
-	if (nv_connector && nv_connector->native_mode) {
-		struct drm_display_mode *native = nv_connector->native_mode;
-		u32 xratio = (native->hdisplay << 19) / mode->hdisplay;
-		u32 yratio = (native->vdisplay << 19) / mode->vdisplay;
-
-		switch (type) {
-		case DRM_MODE_SCALE_ASPECT:
-			if (xratio > yratio) {
-				outX = (mode->hdisplay * yratio) >> 19;
-				outY = (mode->vdisplay * yratio) >> 19;
-			} else {
-				outX = (mode->hdisplay * xratio) >> 19;
-				outY = (mode->vdisplay * xratio) >> 19;
-			}
-			break;
-		case DRM_MODE_SCALE_FULLSCREEN:
-			outX = native->hdisplay;
-			outY = native->vdisplay;
-			break;
-		default:
-			break;
+	if (nv_connector && nv_connector->native_mode)
+		mode = nv_connector->scaling_mode;
+
+	if (mode != DRM_MODE_SCALE_NONE)
+		omode = nv_connector->native_mode;
+	else
+		omode = umode;
+
+	oX = omode->hdisplay;
+	oY = omode->vdisplay;
+	if (omode->flags & DRM_MODE_FLAG_DBLSCAN)
+		oY *= 2;
+
+	/* add overscan compensation if necessary, will keep the aspect
+	 * ratio the same as the backend mode unless overridden by the
+	 * user setting both hborder and vborder properties.
+	 */
+	if (nv_connector && ( nv_connector->underscan == UNDERSCAN_ON ||
+			     (nv_connector->underscan == UNDERSCAN_AUTO &&
+			      nv_connector->edid &&
+			      drm_detect_hdmi_monitor(nv_connector->edid)))) {
+		u32 bX = nv_connector->underscan_hborder;
+		u32 bY = nv_connector->underscan_vborder;
+		u32 aspect = (oY << 19) / oX;
+
+		if (bX) {
+			oX -= (bX * 2);
+			if (bY) oY -= (bY * 2);
+			else    oY  = ((oX * aspect) + (aspect / 2)) >> 19;
+		} else {
+			oX -= (oX >> 4) + 32;
+			if (bY) oY -= (bY * 2);
+			else    oY  = ((oX * aspect) + (aspect / 2)) >> 19;
+		}
+	}
+
+	/* handle CENTER/ASPECT scaling, taking into account the areas
+	 * removed already for overscan compensation
+	 */
+	switch (mode) {
+	case DRM_MODE_SCALE_CENTER:
+		oX = min((u32)umode->hdisplay, oX);
+		oY = min((u32)umode->vdisplay, oY);
+		/* fall-through */
+	case DRM_MODE_SCALE_ASPECT:
+		if (oY < oX) {
+			u32 aspect = (umode->hdisplay << 19) / umode->vdisplay;
+			oX = ((oY * aspect) + (aspect / 2)) >> 19;
+		} else {
+			u32 aspect = (umode->vdisplay << 19) / umode->hdisplay;
+			oY = ((oX * aspect) + (aspect / 2)) >> 19;
 		}
+		break;
+	default:
+		break;
 	}
 
-	push = evo_wait(dev, 0, 16);
+	push = evo_wait(dev, EVO_MASTER, 8);
 	if (push) {
 		evo_mthd(push, 0x04c0 + (nv_crtc->index * 0x300), 3);
-		evo_data(push, (outY << 16) | outX);
-		evo_data(push, (outY << 16) | outX);
-		evo_data(push, (outY << 16) | outX);
+		evo_data(push, (oY << 16) | oX);
+		evo_data(push, (oY << 16) | oX);
+		evo_data(push, (oY << 16) | oX);
 		evo_mthd(push, 0x0494 + (nv_crtc->index * 0x300), 1);
 		evo_data(push, 0x00000000);
 		evo_mthd(push, 0x04b8 + (nv_crtc->index * 0x300), 1);
-		evo_data(push, (mode->vdisplay << 16) | mode->hdisplay);
+		evo_data(push, (umode->vdisplay << 16) | umode->hdisplay);
+		evo_kick(push, dev, EVO_MASTER);
 		if (update) {
-			evo_mthd(push, 0x0080, 1);
-			evo_data(push, 0x00000000);
+			nvd0_display_flip_stop(crtc);
+			nvd0_display_flip_next(crtc, crtc->fb, NULL, 1);
 		}
-		evo_kick(push, dev, 0);
 	}
 
 	return 0;
@@ -201,7 +494,7 @@ nvd0_crtc_set_image(struct nouveau_crtc *nv_crtc, struct drm_framebuffer *fb,
 	struct nouveau_framebuffer *nvfb = nouveau_framebuffer(fb);
 	u32 *push;
 
-	push = evo_wait(fb->dev, 0, 16);
+	push = evo_wait(fb->dev, EVO_MASTER, 16);
 	if (push) {
 		evo_mthd(push, 0x0460 + (nv_crtc->index * 0x300), 1);
 		evo_data(push, nvfb->nvbo->bo.offset >> 8);
@@ -216,7 +509,7 @@ nvd0_crtc_set_image(struct nouveau_crtc *nv_crtc, struct drm_framebuffer *fb,
 			evo_mthd(push, 0x0080, 1);
 			evo_data(push, 0x00000000);
 		}
-		evo_kick(push, fb->dev, 0);
+		evo_kick(push, fb->dev, EVO_MASTER);
 	}
 
 	nv_crtc->fb.tile_flags = nvfb->r_dma;
@@ -227,7 +520,7 @@ static void
 nvd0_crtc_cursor_show(struct nouveau_crtc *nv_crtc, bool show, bool update)
 {
 	struct drm_device *dev = nv_crtc->base.dev;
-	u32 *push = evo_wait(dev, 0, 16);
+	u32 *push = evo_wait(dev, EVO_MASTER, 16);
 	if (push) {
 		if (show) {
 			evo_mthd(push, 0x0480 + (nv_crtc->index * 0x300), 2);
@@ -247,7 +540,7 @@ nvd0_crtc_cursor_show(struct nouveau_crtc *nv_crtc, bool show, bool update)
 			evo_data(push, 0x00000000);
 		}
 
-		evo_kick(push, dev, 0);
+		evo_kick(push, dev, EVO_MASTER);
 	}
 }
 
@@ -262,7 +555,9 @@ nvd0_crtc_prepare(struct drm_crtc *crtc)
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 	u32 *push;
 
-	push = evo_wait(crtc->dev, 0, 2);
+	nvd0_display_flip_stop(crtc);
+
+	push = evo_wait(crtc->dev, EVO_MASTER, 2);
 	if (push) {
 		evo_mthd(push, 0x0474 + (nv_crtc->index * 0x300), 1);
 		evo_data(push, 0x00000000);
@@ -270,7 +565,7 @@ nvd0_crtc_prepare(struct drm_crtc *crtc)
 		evo_data(push, 0x03000000);
 		evo_mthd(push, 0x045c + (nv_crtc->index * 0x300), 1);
 		evo_data(push, 0x00000000);
-		evo_kick(push, crtc->dev, 0);
+		evo_kick(push, crtc->dev, EVO_MASTER);
 	}
 
 	nvd0_crtc_cursor_show(nv_crtc, false, false);
@@ -282,7 +577,7 @@ nvd0_crtc_commit(struct drm_crtc *crtc)
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 	u32 *push;
 
-	push = evo_wait(crtc->dev, 0, 32);
+	push = evo_wait(crtc->dev, EVO_MASTER, 32);
 	if (push) {
 		evo_mthd(push, 0x0474 + (nv_crtc->index * 0x300), 1);
 		evo_data(push, nv_crtc->fb.tile_flags);
@@ -295,10 +590,11 @@ nvd0_crtc_commit(struct drm_crtc *crtc)
 		evo_data(push, NvEvoVRAM);
 		evo_mthd(push, 0x0430 + (nv_crtc->index * 0x300), 1);
 		evo_data(push, 0xffffff00);
-		evo_kick(push, crtc->dev, 0);
+		evo_kick(push, crtc->dev, EVO_MASTER);
 	}
 
-	nvd0_crtc_cursor_show(nv_crtc, nv_crtc->cursor.visible, true);
+	nvd0_crtc_cursor_show(nv_crtc, nv_crtc->cursor.visible, false);
+	nvd0_display_flip_next(crtc, crtc->fb, NULL, 1);
 }
 
 static bool
@@ -333,21 +629,35 @@ nvd0_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *umode,
 {
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 	struct nouveau_connector *nv_connector;
-	u32 htotal = mode->htotal;
-	u32 vtotal = mode->vtotal;
-	u32 hsyncw = mode->hsync_end - mode->hsync_start - 1;
-	u32 vsyncw = mode->vsync_end - mode->vsync_start - 1;
-	u32 hfrntp = mode->hsync_start - mode->hdisplay;
-	u32 vfrntp = mode->vsync_start - mode->vdisplay;
-	u32 hbackp = mode->htotal - mode->hsync_end;
-	u32 vbackp = mode->vtotal - mode->vsync_end;
-	u32 hss2be = hsyncw + hbackp;
-	u32 vss2be = vsyncw + vbackp;
-	u32 hss2de = htotal - hfrntp;
-	u32 vss2de = vtotal - vfrntp;
+	u32 ilace = (mode->flags & DRM_MODE_FLAG_INTERLACE) ? 2 : 1;
+	u32 vscan = (mode->flags & DRM_MODE_FLAG_DBLSCAN) ? 2 : 1;
+	u32 hactive, hsynce, hbackp, hfrontp, hblanke, hblanks;
+	u32 vactive, vsynce, vbackp, vfrontp, vblanke, vblanks;
+	u32 vblan2e = 0, vblan2s = 1;
+	u32 magic = 0x31ec6000;
 	u32 syncs, *push;
 	int ret;
 
+	hactive = mode->htotal;
+	hsynce  = mode->hsync_end - mode->hsync_start - 1;
+	hbackp  = mode->htotal - mode->hsync_end;
+	hblanke = hsynce + hbackp;
+	hfrontp = mode->hsync_start - mode->hdisplay;
+	hblanks = mode->htotal - hfrontp - 1;
+
+	vactive = mode->vtotal * vscan / ilace;
+	vsynce  = ((mode->vsync_end - mode->vsync_start) * vscan / ilace) - 1;
+	vbackp  = (mode->vtotal - mode->vsync_end) * vscan / ilace;
+	vblanke = vsynce + vbackp;
+	vfrontp = (mode->vsync_start - mode->vdisplay) * vscan / ilace;
+	vblanks = vactive - vfrontp - 1;
+	if (mode->flags & DRM_MODE_FLAG_INTERLACE) {
+		vblan2e = vactive + vsynce + vbackp;
+		vblan2s = vblan2e + (mode->vdisplay * vscan / ilace);
+		vactive = (vactive * 2) + 1;
+		magic  |= 0x00000001;
+	}
+
 	syncs = 0x00000001;
 	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
 		syncs |= 0x00000008;
@@ -358,28 +668,33 @@ nvd0_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *umode,
 	if (ret)
 		return ret;
 
-	push = evo_wait(crtc->dev, 0, 64);
+	push = evo_wait(crtc->dev, EVO_MASTER, 64);
 	if (push) {
-		evo_mthd(push, 0x0410 + (nv_crtc->index * 0x300), 5);
+		evo_mthd(push, 0x0410 + (nv_crtc->index * 0x300), 6);
 		evo_data(push, 0x00000000);
-		evo_data(push, (vtotal << 16) | htotal);
-		evo_data(push, (vsyncw << 16) | hsyncw);
-		evo_data(push, (vss2be << 16) | hss2be);
-		evo_data(push, (vss2de << 16) | hss2de);
+		evo_data(push, (vactive << 16) | hactive);
+		evo_data(push, ( vsynce << 16) | hsynce);
+		evo_data(push, (vblanke << 16) | hblanke);
+		evo_data(push, (vblanks << 16) | hblanks);
+		evo_data(push, (vblan2e << 16) | vblan2s);
 		evo_mthd(push, 0x042c + (nv_crtc->index * 0x300), 1);
 		evo_data(push, 0x00000000); /* ??? */
 		evo_mthd(push, 0x0450 + (nv_crtc->index * 0x300), 3);
 		evo_data(push, mode->clock * 1000);
 		evo_data(push, 0x00200000); /* ??? */
 		evo_data(push, mode->clock * 1000);
-		evo_mthd(push, 0x0404 + (nv_crtc->index * 0x300), 1);
+		evo_mthd(push, 0x0404 + (nv_crtc->index * 0x300), 2);
 		evo_data(push, syncs);
-		evo_kick(push, crtc->dev, 0);
+		evo_data(push, magic);
+		evo_mthd(push, 0x04d0 + (nv_crtc->index * 0x300), 2);
+		evo_data(push, 0x00000311);
+		evo_data(push, 0x00000100);
+		evo_kick(push, crtc->dev, EVO_MASTER);
 	}
 
 	nv_connector = nouveau_crtc_connector_get(nv_crtc);
-	nvd0_crtc_set_dither(nv_crtc, nv_connector->use_dithering, false);
-	nvd0_crtc_set_scale(nv_crtc, nv_connector->scaling_mode, false);
+	nvd0_crtc_set_dither(nv_crtc, false);
+	nvd0_crtc_set_scale(nv_crtc, false);
 	nvd0_crtc_set_image(nv_crtc, crtc->fb, x, y, false);
 	return 0;
 }
@@ -400,7 +715,9 @@ nvd0_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
 	if (ret)
 		return ret;
 
+	nvd0_display_flip_stop(crtc);
 	nvd0_crtc_set_image(nv_crtc, crtc->fb, x, y, true);
+	nvd0_display_flip_next(crtc, crtc->fb, NULL, 1);
 	return 0;
 }
 
@@ -410,6 +727,7 @@ nvd0_crtc_mode_set_base_atomic(struct drm_crtc *crtc,
 			       enum mode_set_atomic state)
 {
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+	nvd0_display_flip_stop(crtc);
 	nvd0_crtc_set_image(nv_crtc, fb, x, y, true);
 	return 0;
 }
@@ -472,10 +790,10 @@ static int
 nvd0_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
 {
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
-	const u32 data = (y << 16) | x;
+	int ch = EVO_CURS(nv_crtc->index);
 
-	nv_wr32(crtc->dev, 0x64d084 + (nv_crtc->index * 0x1000), data);
-	nv_wr32(crtc->dev, 0x64d080 + (nv_crtc->index * 0x1000), 0x00000000);
+	evo_piow(crtc->dev, ch, 0x0084, (y << 16) | x);
+	evo_piow(crtc->dev, ch, 0x0080, 0x00000000);
 	return 0;
 }
 
@@ -525,6 +843,7 @@ static const struct drm_crtc_funcs nvd0_crtc_func = {
 	.gamma_set = nvd0_crtc_gamma_set,
 	.set_config = drm_crtc_helper_set_config,
 	.destroy = nvd0_crtc_destroy,
+	.page_flip = nouveau_crtc_page_flip,
 };
 
 static void
@@ -659,12 +978,12 @@ nvd0_dac_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
 
 	nvd0_dac_dpms(encoder, DRM_MODE_DPMS_ON);
 
-	push = evo_wait(encoder->dev, 0, 4);
+	push = evo_wait(encoder->dev, EVO_MASTER, 4);
 	if (push) {
 		evo_mthd(push, 0x0180 + (nv_encoder->or * 0x20), 2);
 		evo_data(push, 1 << nv_crtc->index);
 		evo_data(push, 0x00ff);
-		evo_kick(push, encoder->dev, 0);
+		evo_kick(push, encoder->dev, EVO_MASTER);
 	}
 
 	nv_encoder->crtc = encoder->crtc;
@@ -680,13 +999,13 @@ nvd0_dac_disconnect(struct drm_encoder *encoder)
 	if (nv_encoder->crtc) {
 		nvd0_crtc_prepare(nv_encoder->crtc);
 
-		push = evo_wait(dev, 0, 4);
+		push = evo_wait(dev, EVO_MASTER, 4);
 		if (push) {
 			evo_mthd(push, 0x0180 + (nv_encoder->or * 0x20), 1);
 			evo_data(push, 0x00000000);
 			evo_mthd(push, 0x0080, 1);
 			evo_data(push, 0x00000000);
-			evo_kick(push, dev, 0);
+			evo_kick(push, dev, EVO_MASTER);
 		}
 
 		nv_encoder->crtc = NULL;
@@ -760,6 +1079,108 @@ nvd0_dac_create(struct drm_connector *connector, struct dcb_entry *dcbe)
 }
 
 /******************************************************************************
+ * Audio
+ *****************************************************************************/
+static void
+nvd0_audio_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode)
+{
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct nouveau_connector *nv_connector;
+	struct drm_device *dev = encoder->dev;
+	int i, or = nv_encoder->or * 0x30;
+
+	nv_connector = nouveau_encoder_connector_get(nv_encoder);
+	if (!drm_detect_monitor_audio(nv_connector->edid))
+		return;
+
+	nv_mask(dev, 0x10ec10 + or, 0x80000003, 0x80000001);
+
+	drm_edid_to_eld(&nv_connector->base, nv_connector->edid);
+	if (nv_connector->base.eld[0]) {
+		u8 *eld = nv_connector->base.eld;
+
+		for (i = 0; i < eld[2] * 4; i++)
+			nv_wr32(dev, 0x10ec00 + or, (i << 8) | eld[i]);
+		for (i = eld[2] * 4; i < 0x60; i++)
+			nv_wr32(dev, 0x10ec00 + or, (i << 8) | 0x00);
+
+		nv_mask(dev, 0x10ec10 + or, 0x80000002, 0x80000002);
+	}
+}
+
+static void
+nvd0_audio_disconnect(struct drm_encoder *encoder)
+{
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct drm_device *dev = encoder->dev;
+	int or = nv_encoder->or * 0x30;
+
+	nv_mask(dev, 0x10ec10 + or, 0x80000003, 0x80000000);
+}
+
+/******************************************************************************
+ * HDMI
+ *****************************************************************************/
+static void
+nvd0_hdmi_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode)
+{
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(encoder->crtc);
+	struct nouveau_connector *nv_connector;
+	struct drm_device *dev = encoder->dev;
+	int head = nv_crtc->index * 0x800;
+	u32 rekey = 56; /* binary driver, and tegra constant */
+	u32 max_ac_packet;
+
+	nv_connector = nouveau_encoder_connector_get(nv_encoder);
+	if (!drm_detect_hdmi_monitor(nv_connector->edid))
+		return;
+
+	max_ac_packet  = mode->htotal - mode->hdisplay;
+	max_ac_packet -= rekey;
+	max_ac_packet -= 18; /* constant from tegra */
+	max_ac_packet /= 32;
+
+	/* AVI InfoFrame */
+	nv_mask(dev, 0x616714 + head, 0x00000001, 0x00000000);
+	nv_wr32(dev, 0x61671c + head, 0x000d0282);
+	nv_wr32(dev, 0x616720 + head, 0x0000006f);
+	nv_wr32(dev, 0x616724 + head, 0x00000000);
+	nv_wr32(dev, 0x616728 + head, 0x00000000);
+	nv_wr32(dev, 0x61672c + head, 0x00000000);
+	nv_mask(dev, 0x616714 + head, 0x00000001, 0x00000001);
+
+	/* ??? InfoFrame? */
+	nv_mask(dev, 0x6167a4 + head, 0x00000001, 0x00000000);
+	nv_wr32(dev, 0x6167ac + head, 0x00000010);
+	nv_mask(dev, 0x6167a4 + head, 0x00000001, 0x00000001);
+
+	/* HDMI_CTRL */
+	nv_mask(dev, 0x616798 + head, 0x401f007f, 0x40000000 | rekey |
+						  max_ac_packet << 16);
+
+	/* NFI, audio doesn't work without it though.. */
+	nv_mask(dev, 0x616548 + head, 0x00000070, 0x00000000);
+
+	nvd0_audio_mode_set(encoder, mode);
+}
+
+static void
+nvd0_hdmi_disconnect(struct drm_encoder *encoder)
+{
+	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(nv_encoder->crtc);
+	struct drm_device *dev = encoder->dev;
+	int head = nv_crtc->index * 0x800;
+
+	nvd0_audio_disconnect(encoder);
+
+	nv_mask(dev, 0x616798 + head, 0x40000000, 0x00000000);
+	nv_mask(dev, 0x6167a4 + head, 0x00000001, 0x00000000);
+	nv_mask(dev, 0x616714 + head, 0x00000001, 0x00000000);
+}
+
+/******************************************************************************
  * SOR
  *****************************************************************************/
 static void
@@ -829,7 +1250,8 @@ static void
 nvd0_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
 		  struct drm_display_mode *mode)
 {
-	struct drm_nouveau_private *dev_priv = encoder->dev->dev_private;
+	struct drm_device *dev = encoder->dev;
+	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(encoder->crtc);
 	struct nouveau_connector *nv_connector;
@@ -852,6 +1274,8 @@ nvd0_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
 		or_config = (mode_ctrl & 0x00000f00) >> 8;
 		if (mode->clock >= 165000)
 			or_config |= 0x0100;
+
+		nvd0_hdmi_mode_set(encoder, mode);
 		break;
 	case OUTPUT_LVDS:
 		or_config = (mode_ctrl & 0x00000f00) >> 8;
@@ -861,7 +1285,7 @@ nvd0_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
 			if (bios->fp.if_is_24bit)
 				or_config |= 0x0200;
 		} else {
-			if (nv_connector->dcb->type == DCB_CONNECTOR_LVDS_SPWG) {
+			if (nv_connector->type == DCB_CONNECTOR_LVDS_SPWG) {
 				if (((u8 *)nv_connector->edid)[121] == 2)
 					or_config |= 0x0100;
 			} else
@@ -889,12 +1313,12 @@ nvd0_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
 
 	nvd0_sor_dpms(encoder, DRM_MODE_DPMS_ON);
 
-	push = evo_wait(encoder->dev, 0, 4);
+	push = evo_wait(dev, EVO_MASTER, 4);
 	if (push) {
 		evo_mthd(push, 0x0200 + (nv_encoder->or * 0x20), 2);
 		evo_data(push, mode_ctrl);
 		evo_data(push, or_config);
-		evo_kick(push, encoder->dev, 0);
+		evo_kick(push, dev, EVO_MASTER);
 	}
 
 	nv_encoder->crtc = encoder->crtc;
@@ -910,15 +1334,17 @@ nvd0_sor_disconnect(struct drm_encoder *encoder)
 	if (nv_encoder->crtc) {
 		nvd0_crtc_prepare(nv_encoder->crtc);
 
-		push = evo_wait(dev, 0, 4);
+		push = evo_wait(dev, EVO_MASTER, 4);
 		if (push) {
 			evo_mthd(push, 0x0200 + (nv_encoder->or * 0x20), 1);
 			evo_data(push, 0x00000000);
 			evo_mthd(push, 0x0080, 1);
 			evo_data(push, 0x00000000);
-			evo_kick(push, dev, 0);
+			evo_kick(push, dev, EVO_MASTER);
 		}
 
+		nvd0_hdmi_disconnect(encoder);
+
 		nv_encoder->crtc = NULL;
 		nv_encoder->last_dpms = DRM_MODE_DPMS_OFF;
 	}
@@ -1159,6 +1585,12 @@ nvd0_display_intr(struct drm_device *dev)
 	struct nvd0_display *disp = nvd0_display(dev);
 	u32 intr = nv_rd32(dev, 0x610088);
 
+	if (intr & 0x00000001) {
+		u32 stat = nv_rd32(dev, 0x61008c);
+		nv_wr32(dev, 0x61008c, stat);
+		intr &= ~0x00000001;
+	}
+
 	if (intr & 0x00000002) {
 		u32 stat = nv_rd32(dev, 0x61009c);
 		int chid = ffs(stat) - 1;
@@ -1215,38 +1647,29 @@ nvd0_display_intr(struct drm_device *dev)
 /******************************************************************************
  * Init
  *****************************************************************************/
-static void
+void
 nvd0_display_fini(struct drm_device *dev)
 {
 	int i;
 
-	/* fini cursors */
-	for (i = 14; i >= 13; i--) {
-		if (!(nv_rd32(dev, 0x610490 + (i * 0x10)) & 0x00000001))
-			continue;
-
-		nv_mask(dev, 0x610490 + (i * 0x10), 0x00000001, 0x00000000);
-		nv_wait(dev, 0x610490 + (i * 0x10), 0x00010000, 0x00000000);
-		nv_mask(dev, 0x610090, 1 << i, 0x00000000);
-		nv_mask(dev, 0x6100a0, 1 << i, 0x00000000);
+	/* fini cursors + overlays + flips */
+	for (i = 1; i >= 0; i--) {
+		evo_fini_pio(dev, EVO_CURS(i));
+		evo_fini_pio(dev, EVO_OIMM(i));
+		evo_fini_dma(dev, EVO_OVLY(i));
+		evo_fini_dma(dev, EVO_FLIP(i));
 	}
 
 	/* fini master */
-	if (nv_rd32(dev, 0x610490) & 0x00000010) {
-		nv_mask(dev, 0x610490, 0x00000010, 0x00000000);
-		nv_mask(dev, 0x610490, 0x00000003, 0x00000000);
-		nv_wait(dev, 0x610490, 0x80000000, 0x00000000);
-		nv_mask(dev, 0x610090, 0x00000001, 0x00000000);
-		nv_mask(dev, 0x6100a0, 0x00000001, 0x00000000);
-	}
+	evo_fini_dma(dev, EVO_MASTER);
 }
 
 int
 nvd0_display_init(struct drm_device *dev)
 {
 	struct nvd0_display *disp = nvd0_display(dev);
+	int ret, i;
 	u32 *push;
-	int i;
 
 	if (nv_rd32(dev, 0x6100ac) & 0x00000100) {
 		nv_wr32(dev, 0x6100ac, 0x00000100);
@@ -1271,7 +1694,7 @@ nvd0_display_init(struct drm_device *dev)
 		nv_wr32(dev, 0x6301c4 + (i * 0x800), sor);
 	}
 
-	for (i = 0; i < 2; i++) {
+	for (i = 0; i < dev->mode_config.num_crtc; i++) {
 		u32 crtc0 = nv_rd32(dev, 0x616104 + (i * 0x800));
 		u32 crtc1 = nv_rd32(dev, 0x616108 + (i * 0x800));
 		u32 crtc2 = nv_rd32(dev, 0x61610c + (i * 0x800));
@@ -1285,36 +1708,24 @@ nvd0_display_init(struct drm_device *dev)
 	nv_mask(dev, 0x6100b0, 0x00000307, 0x00000307);
 
 	/* init master */
-	nv_wr32(dev, 0x610494, (disp->evo[0].handle >> 8) | 3);
-	nv_wr32(dev, 0x610498, 0x00010000);
-	nv_wr32(dev, 0x61049c, 0x00000001);
-	nv_mask(dev, 0x610490, 0x00000010, 0x00000010);
-	nv_wr32(dev, 0x640000, 0x00000000);
-	nv_wr32(dev, 0x610490, 0x01000013);
-	if (!nv_wait(dev, 0x610490, 0x80000000, 0x00000000)) {
-		NV_ERROR(dev, "PDISP: master 0x%08x\n",
-			 nv_rd32(dev, 0x610490));
-		return -EBUSY;
+	ret = evo_init_dma(dev, EVO_MASTER);
+	if (ret)
+		goto error;
+
+	/* init flips + overlays + cursors */
+	for (i = 0; i < dev->mode_config.num_crtc; i++) {
+		if ((ret = evo_init_dma(dev, EVO_FLIP(i))) ||
+		    (ret = evo_init_dma(dev, EVO_OVLY(i))) ||
+		    (ret = evo_init_pio(dev, EVO_OIMM(i))) ||
+		    (ret = evo_init_pio(dev, EVO_CURS(i))))
+			goto error;
 	}
-	nv_mask(dev, 0x610090, 0x00000001, 0x00000001);
-	nv_mask(dev, 0x6100a0, 0x00000001, 0x00000001);
 
-	/* init cursors */
-	for (i = 13; i <= 14; i++) {
-		nv_wr32(dev, 0x610490 + (i * 0x10), 0x00000001);
-		if (!nv_wait(dev, 0x610490 + (i * 0x10), 0x00010000, 0x00010000)) {
-			NV_ERROR(dev, "PDISP: curs%d 0x%08x\n", i,
-				 nv_rd32(dev, 0x610490 + (i * 0x10)));
-			return -EBUSY;
-		}
-
-		nv_mask(dev, 0x610090, 1 << i, 1 << i);
-		nv_mask(dev, 0x6100a0, 1 << i, 1 << i);
+	push = evo_wait(dev, EVO_MASTER, 32);
+	if (!push) {
+		ret = -EBUSY;
+		goto error;
 	}
-
-	push = evo_wait(dev, 0, 32);
-	if (!push)
-		return -EBUSY;
 	evo_mthd(push, 0x0088, 1);
 	evo_data(push, NvEvoSync);
 	evo_mthd(push, 0x0084, 1);
@@ -1323,9 +1734,12 @@ nvd0_display_init(struct drm_device *dev)
 	evo_data(push, 0x80000000);
 	evo_mthd(push, 0x008c, 1);
 	evo_data(push, 0x00000000);
-	evo_kick(push, dev, 0);
+	evo_kick(push, dev, EVO_MASTER);
 
-	return 0;
+error:
+	if (ret)
+		nvd0_display_fini(dev);
+	return ret;
 }
 
 void
@@ -1334,11 +1748,16 @@ nvd0_display_destroy(struct drm_device *dev)
 	struct drm_nouveau_private *dev_priv = dev->dev_private;
 	struct nvd0_display *disp = nvd0_display(dev);
 	struct pci_dev *pdev = dev->pdev;
+	int i;
 
-	nvd0_display_fini(dev);
+	for (i = 0; i < EVO_DMA_NR; i++) {
+		struct evo *evo = &disp->evo[i];
+		pci_free_consistent(pdev, PAGE_SIZE, evo->ptr, evo->handle);
+	}
 
-	pci_free_consistent(pdev, PAGE_SIZE, disp->evo[0].ptr, disp->evo[0].handle);
 	nouveau_gpuobj_ref(NULL, &disp->mem);
+	nouveau_bo_unmap(disp->sync);
+	nouveau_bo_ref(NULL, &disp->sync);
 	nouveau_irq_unregister(dev, 26);
 
 	dev_priv->engine.display.priv = NULL;
@@ -1410,61 +1829,83 @@ nvd0_display_create(struct drm_device *dev)
 	tasklet_init(&disp->tasklet, nvd0_display_bh, (unsigned long)dev);
 	nouveau_irq_register(dev, 26, nvd0_display_intr);
 
+	/* small shared memory area we use for notifiers and semaphores */
+	ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
+			     0, 0x0000, &disp->sync);
+	if (!ret) {
+		ret = nouveau_bo_pin(disp->sync, TTM_PL_FLAG_VRAM);
+		if (!ret)
+			ret = nouveau_bo_map(disp->sync);
+		if (ret)
+			nouveau_bo_ref(NULL, &disp->sync);
+	}
+
+	if (ret)
+		goto out;
+
 	/* hash table and dma objects for the memory areas we care about */
 	ret = nouveau_gpuobj_new(dev, NULL, 0x4000, 0x10000,
 				 NVOBJ_FLAG_ZERO_ALLOC, &disp->mem);
 	if (ret)
 		goto out;
 
-	nv_wo32(disp->mem, 0x1000, 0x00000049);
-	nv_wo32(disp->mem, 0x1004, (disp->mem->vinst + 0x2000) >> 8);
-	nv_wo32(disp->mem, 0x1008, (disp->mem->vinst + 0x2fff) >> 8);
-	nv_wo32(disp->mem, 0x100c, 0x00000000);
-	nv_wo32(disp->mem, 0x1010, 0x00000000);
-	nv_wo32(disp->mem, 0x1014, 0x00000000);
-	nv_wo32(disp->mem, 0x0000, NvEvoSync);
-	nv_wo32(disp->mem, 0x0004, (0x1000 << 9) | 0x00000001);
-
-	nv_wo32(disp->mem, 0x1020, 0x00000049);
-	nv_wo32(disp->mem, 0x1024, 0x00000000);
-	nv_wo32(disp->mem, 0x1028, (dev_priv->vram_size - 1) >> 8);
-	nv_wo32(disp->mem, 0x102c, 0x00000000);
-	nv_wo32(disp->mem, 0x1030, 0x00000000);
-	nv_wo32(disp->mem, 0x1034, 0x00000000);
-	nv_wo32(disp->mem, 0x0008, NvEvoVRAM);
-	nv_wo32(disp->mem, 0x000c, (0x1020 << 9) | 0x00000001);
-
-	nv_wo32(disp->mem, 0x1040, 0x00000009);
-	nv_wo32(disp->mem, 0x1044, 0x00000000);
-	nv_wo32(disp->mem, 0x1048, (dev_priv->vram_size - 1) >> 8);
-	nv_wo32(disp->mem, 0x104c, 0x00000000);
-	nv_wo32(disp->mem, 0x1050, 0x00000000);
-	nv_wo32(disp->mem, 0x1054, 0x00000000);
-	nv_wo32(disp->mem, 0x0010, NvEvoVRAM_LP);
-	nv_wo32(disp->mem, 0x0014, (0x1040 << 9) | 0x00000001);
-
-	nv_wo32(disp->mem, 0x1060, 0x0fe00009);
-	nv_wo32(disp->mem, 0x1064, 0x00000000);
-	nv_wo32(disp->mem, 0x1068, (dev_priv->vram_size - 1) >> 8);
-	nv_wo32(disp->mem, 0x106c, 0x00000000);
-	nv_wo32(disp->mem, 0x1070, 0x00000000);
-	nv_wo32(disp->mem, 0x1074, 0x00000000);
-	nv_wo32(disp->mem, 0x0018, NvEvoFB32);
-	nv_wo32(disp->mem, 0x001c, (0x1060 << 9) | 0x00000001);
-
-	pinstmem->flush(dev);
+	/* create evo dma channels */
+	for (i = 0; i < EVO_DMA_NR; i++) {
+		struct evo *evo = &disp->evo[i];
+		u64 offset = disp->sync->bo.offset;
+		u32 dmao = 0x1000 + (i * 0x100);
+		u32 hash = 0x0000 + (i * 0x040);
+
+		evo->idx = i;
+		evo->sem.offset = EVO_SYNC(evo->idx, 0x00);
+		evo->ptr = pci_alloc_consistent(pdev, PAGE_SIZE, &evo->handle);
+		if (!evo->ptr) {
+			ret = -ENOMEM;
+			goto out;
+		}
 
-	/* push buffers for evo channels */
-	disp->evo[0].ptr =
-		pci_alloc_consistent(pdev, PAGE_SIZE, &disp->evo[0].handle);
-	if (!disp->evo[0].ptr) {
-		ret = -ENOMEM;
-		goto out;
+		nv_wo32(disp->mem, dmao + 0x00, 0x00000049);
+		nv_wo32(disp->mem, dmao + 0x04, (offset + 0x0000) >> 8);
+		nv_wo32(disp->mem, dmao + 0x08, (offset + 0x0fff) >> 8);
+		nv_wo32(disp->mem, dmao + 0x0c, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x10, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x14, 0x00000000);
+		nv_wo32(disp->mem, hash + 0x00, NvEvoSync);
+		nv_wo32(disp->mem, hash + 0x04, 0x00000001 | (i << 27) |
+						((dmao + 0x00) << 9));
+
+		nv_wo32(disp->mem, dmao + 0x20, 0x00000049);
+		nv_wo32(disp->mem, dmao + 0x24, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x28, (dev_priv->vram_size - 1) >> 8);
+		nv_wo32(disp->mem, dmao + 0x2c, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x30, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x34, 0x00000000);
+		nv_wo32(disp->mem, hash + 0x08, NvEvoVRAM);
+		nv_wo32(disp->mem, hash + 0x0c, 0x00000001 | (i << 27) |
+						((dmao + 0x20) << 9));
+
+		nv_wo32(disp->mem, dmao + 0x40, 0x00000009);
+		nv_wo32(disp->mem, dmao + 0x44, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x48, (dev_priv->vram_size - 1) >> 8);
+		nv_wo32(disp->mem, dmao + 0x4c, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x50, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x54, 0x00000000);
+		nv_wo32(disp->mem, hash + 0x10, NvEvoVRAM_LP);
+		nv_wo32(disp->mem, hash + 0x14, 0x00000001 | (i << 27) |
+						((dmao + 0x40) << 9));
+
+		nv_wo32(disp->mem, dmao + 0x60, 0x0fe00009);
+		nv_wo32(disp->mem, dmao + 0x64, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x68, (dev_priv->vram_size - 1) >> 8);
+		nv_wo32(disp->mem, dmao + 0x6c, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x70, 0x00000000);
+		nv_wo32(disp->mem, dmao + 0x74, 0x00000000);
+		nv_wo32(disp->mem, hash + 0x18, NvEvoFB32);
+		nv_wo32(disp->mem, hash + 0x1c, 0x00000001 | (i << 27) |
+						((dmao + 0x60) << 9));
 	}
 
-	ret = nvd0_display_init(dev);
-	if (ret)
-		goto out;
+	pinstmem->flush(dev);
 
 out:
 	if (ret)
diff --git a/drivers/gpu/drm/r128/r128_drv.c b/drivers/gpu/drm/r128/r128_drv.c
index 4c8796ba6dd8..6a5f4395838f 100644
--- a/drivers/gpu/drm/r128/r128_drv.c
+++ b/drivers/gpu/drm/r128/r128_drv.c
@@ -42,6 +42,20 @@ static struct pci_device_id pciidlist[] = {
 	r128_PCI_IDS
 };
 
+static const struct file_operations r128_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = r128_compat_ioctl,
+#endif
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_USE_MTRR | DRIVER_PCI_DMA | DRIVER_SG |
@@ -60,21 +74,7 @@ static struct drm_driver driver = {
 	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = r128_ioctls,
 	.dma_ioctl = r128_cce_buffers,
-	.fops = {
-		.owner = THIS_MODULE,
-		.open = drm_open,
-		.release = drm_release,
-		.unlocked_ioctl = drm_ioctl,
-		.mmap = drm_mmap,
-		.poll = drm_poll,
-		.fasync = drm_fasync,
-#ifdef CONFIG_COMPAT
-		.compat_ioctl = r128_compat_ioctl,
-#endif
-		.llseek = noop_llseek,
-	},
-
-
+	.fops = &r128_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index cf8b4bc3e73d..2139fe893ec5 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -70,7 +70,8 @@ radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
 	r200.o radeon_legacy_tv.o r600_cs.o r600_blit.o r600_blit_shaders.o \
 	r600_blit_kms.o radeon_pm.o atombios_dp.o r600_audio.o r600_hdmi.o \
 	evergreen.o evergreen_cs.o evergreen_blit_shaders.o evergreen_blit_kms.o \
-	radeon_trace_points.o ni.o cayman_blit_shaders.o atombios_encoders.o
+	radeon_trace_points.o ni.o cayman_blit_shaders.o atombios_encoders.o \
+	radeon_semaphore.o radeon_sa.o
 
 radeon-$(CONFIG_COMPAT) += radeon_ioc32.o
 radeon-$(CONFIG_VGA_SWITCHEROO) += radeon_atpx_handler.o
@@ -78,4 +79,4 @@ radeon-$(CONFIG_ACPI) += radeon_acpi.o
 
 obj-$(CONFIG_DRM_RADEON)+= radeon.o
 
-CFLAGS_radeon_trace_points.o := -I$(src)
\ No newline at end of file
+CFLAGS_radeon_trace_points.o := -I$(src)
diff --git a/drivers/gpu/drm/radeon/atom.c b/drivers/gpu/drm/radeon/atom.c
index 14cc88aaf3a7..d1bd239cd9e9 100644
--- a/drivers/gpu/drm/radeon/atom.c
+++ b/drivers/gpu/drm/radeon/atom.c
@@ -665,6 +665,8 @@ static void atom_op_delay(atom_exec_context *ctx, int *ptr, int arg)
 	SDEBUG("   count: %d\n", count);
 	if (arg == ATOM_UNIT_MICROSEC)
 		udelay(count);
+	else if (!drm_can_sleep())
+		mdelay(count);
 	else
 		msleep(count);
 }
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index 2b97262e3ab1..0fda830ef806 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -554,7 +554,7 @@ static u32 atombios_adjust_pll(struct drm_crtc *crtc,
 		if (encoder->crtc == crtc) {
 			radeon_encoder = to_radeon_encoder(encoder);
 			connector = radeon_get_connector_for_encoder(encoder);
-			if (connector)
+			if (connector && connector->display_info.bpc)
 				bpc = connector->display_info.bpc;
 			encoder_mode = atombios_get_encoder_mode(encoder);
 			if ((radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT | ATOM_DEVICE_DFP_SUPPORT)) ||
@@ -1184,7 +1184,7 @@ static int dce4_crtc_do_set_base(struct drm_crtc *crtc,
 	WREG32(EVERGREEN_GRPH_X_END + radeon_crtc->crtc_offset, target_fb->width);
 	WREG32(EVERGREEN_GRPH_Y_END + radeon_crtc->crtc_offset, target_fb->height);
 
-	fb_pitch_pixels = target_fb->pitch / (target_fb->bits_per_pixel / 8);
+	fb_pitch_pixels = target_fb->pitches[0] / (target_fb->bits_per_pixel / 8);
 	WREG32(EVERGREEN_GRPH_PITCH + radeon_crtc->crtc_offset, fb_pitch_pixels);
 	WREG32(EVERGREEN_GRPH_ENABLE + radeon_crtc->crtc_offset, 1);
 
@@ -1353,7 +1353,7 @@ static int avivo_crtc_do_set_base(struct drm_crtc *crtc,
 	WREG32(AVIVO_D1GRPH_X_END + radeon_crtc->crtc_offset, target_fb->width);
 	WREG32(AVIVO_D1GRPH_Y_END + radeon_crtc->crtc_offset, target_fb->height);
 
-	fb_pitch_pixels = target_fb->pitch / (target_fb->bits_per_pixel / 8);
+	fb_pitch_pixels = target_fb->pitches[0] / (target_fb->bits_per_pixel / 8);
 	WREG32(AVIVO_D1GRPH_PITCH + radeon_crtc->crtc_offset, fb_pitch_pixels);
 	WREG32(AVIVO_D1GRPH_ENABLE + radeon_crtc->crtc_offset, 1);
 
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index 39c04c1b8472..f1f06ca9f1f5 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -409,8 +409,6 @@ int
 atombios_get_encoder_mode(struct drm_encoder *encoder)
 {
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	struct drm_device *dev = encoder->dev;
-	struct radeon_device *rdev = dev->dev_private;
 	struct drm_connector *connector;
 	struct radeon_connector *radeon_connector;
 	struct radeon_connector_atom_dig *dig_connector;
@@ -434,13 +432,10 @@ atombios_get_encoder_mode(struct drm_encoder *encoder)
 	switch (connector->connector_type) {
 	case DRM_MODE_CONNECTOR_DVII:
 	case DRM_MODE_CONNECTOR_HDMIB: /* HDMI-B is basically DL-DVI; analog works fine */
-		if (drm_detect_monitor_audio(radeon_connector->edid) && radeon_audio) {
-			/* fix me */
-			if (ASIC_IS_DCE4(rdev))
-				return ATOM_ENCODER_MODE_DVI;
-			else
-				return ATOM_ENCODER_MODE_HDMI;
-		} else if (radeon_connector->use_digital)
+		if (drm_detect_monitor_audio(radeon_connector->edid) &&
+		    radeon_audio)
+			return ATOM_ENCODER_MODE_HDMI;
+		else if (radeon_connector->use_digital)
 			return ATOM_ENCODER_MODE_DVI;
 		else
 			return ATOM_ENCODER_MODE_CRT;
@@ -448,13 +443,10 @@ atombios_get_encoder_mode(struct drm_encoder *encoder)
 	case DRM_MODE_CONNECTOR_DVID:
 	case DRM_MODE_CONNECTOR_HDMIA:
 	default:
-		if (drm_detect_monitor_audio(radeon_connector->edid) && radeon_audio) {
-			/* fix me */
-			if (ASIC_IS_DCE4(rdev))
-				return ATOM_ENCODER_MODE_DVI;
-			else
-				return ATOM_ENCODER_MODE_HDMI;
-		} else
+		if (drm_detect_monitor_audio(radeon_connector->edid) &&
+		    radeon_audio)
+			return ATOM_ENCODER_MODE_HDMI;
+		else
 			return ATOM_ENCODER_MODE_DVI;
 		break;
 	case DRM_MODE_CONNECTOR_LVDS:
@@ -465,13 +457,10 @@ atombios_get_encoder_mode(struct drm_encoder *encoder)
 		if ((dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT) ||
 		    (dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_eDP))
 			return ATOM_ENCODER_MODE_DP;
-		else if (drm_detect_monitor_audio(radeon_connector->edid) && radeon_audio) {
-			/* fix me */
-			if (ASIC_IS_DCE4(rdev))
-				return ATOM_ENCODER_MODE_DVI;
-			else
-				return ATOM_ENCODER_MODE_HDMI;
-		} else
+		else if (drm_detect_monitor_audio(radeon_connector->edid) &&
+			 radeon_audio)
+			return ATOM_ENCODER_MODE_HDMI;
+		else
 			return ATOM_ENCODER_MODE_DVI;
 		break;
 	case DRM_MODE_CONNECTOR_eDP:
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index 92c9628c572d..636660fca8c2 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -40,6 +40,8 @@
 static void evergreen_gpu_init(struct radeon_device *rdev);
 void evergreen_fini(struct radeon_device *rdev);
 void evergreen_pcie_gen2_enable(struct radeon_device *rdev);
+extern void cayman_cp_int_cntl_setup(struct radeon_device *rdev,
+				     int ring, u32 cp_int_cntl);
 
 void evergreen_fix_pci_max_read_req_size(struct radeon_device *rdev)
 {
@@ -1311,18 +1313,20 @@ void evergreen_mc_program(struct radeon_device *rdev)
  */
 void evergreen_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
+	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
+
 	/* set to DX10/11 mode */
-	radeon_ring_write(rdev, PACKET3(PACKET3_MODE_CONTROL, 0));
-	radeon_ring_write(rdev, 1);
+	radeon_ring_write(ring, PACKET3(PACKET3_MODE_CONTROL, 0));
+	radeon_ring_write(ring, 1);
 	/* FIXME: implement */
-	radeon_ring_write(rdev, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
+	radeon_ring_write(ring,
 #ifdef __BIG_ENDIAN
 			  (2 << 0) |
 #endif
 			  (ib->gpu_addr & 0xFFFFFFFC));
-	radeon_ring_write(rdev, upper_32_bits(ib->gpu_addr) & 0xFF);
-	radeon_ring_write(rdev, ib->length_dw);
+	radeon_ring_write(ring, upper_32_bits(ib->gpu_addr) & 0xFF);
+	radeon_ring_write(ring, ib->length_dw);
 }
 
 
@@ -1360,71 +1364,73 @@ static int evergreen_cp_load_microcode(struct radeon_device *rdev)
 
 static int evergreen_cp_start(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r, i;
 	uint32_t cp_me;
 
-	r = radeon_ring_lock(rdev, 7);
+	r = radeon_ring_lock(rdev, ring, 7);
 	if (r) {
 		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
 		return r;
 	}
-	radeon_ring_write(rdev, PACKET3(PACKET3_ME_INITIALIZE, 5));
-	radeon_ring_write(rdev, 0x1);
-	radeon_ring_write(rdev, 0x0);
-	radeon_ring_write(rdev, rdev->config.evergreen.max_hw_contexts - 1);
-	radeon_ring_write(rdev, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_write(ring, PACKET3(PACKET3_ME_INITIALIZE, 5));
+	radeon_ring_write(ring, 0x1);
+	radeon_ring_write(ring, 0x0);
+	radeon_ring_write(ring, rdev->config.evergreen.max_hw_contexts - 1);
+	radeon_ring_write(ring, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_unlock_commit(rdev, ring);
 
 	cp_me = 0xff;
 	WREG32(CP_ME_CNTL, cp_me);
 
-	r = radeon_ring_lock(rdev, evergreen_default_size + 19);
+	r = radeon_ring_lock(rdev, ring, evergreen_default_size + 19);
 	if (r) {
 		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
 		return r;
 	}
 
 	/* setup clear context state */
-	radeon_ring_write(rdev, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
-	radeon_ring_write(rdev, PACKET3_PREAMBLE_BEGIN_CLEAR_STATE);
+	radeon_ring_write(ring, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
+	radeon_ring_write(ring, PACKET3_PREAMBLE_BEGIN_CLEAR_STATE);
 
 	for (i = 0; i < evergreen_default_size; i++)
-		radeon_ring_write(rdev, evergreen_default_state[i]);
+		radeon_ring_write(ring, evergreen_default_state[i]);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
-	radeon_ring_write(rdev, PACKET3_PREAMBLE_END_CLEAR_STATE);
+	radeon_ring_write(ring, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
+	radeon_ring_write(ring, PACKET3_PREAMBLE_END_CLEAR_STATE);
 
 	/* set clear context state */
-	radeon_ring_write(rdev, PACKET3(PACKET3_CLEAR_STATE, 0));
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_CLEAR_STATE, 0));
+	radeon_ring_write(ring, 0);
 
 	/* SQ_VTX_BASE_VTX_LOC */
-	radeon_ring_write(rdev, 0xc0026f00);
-	radeon_ring_write(rdev, 0x00000000);
-	radeon_ring_write(rdev, 0x00000000);
-	radeon_ring_write(rdev, 0x00000000);
+	radeon_ring_write(ring, 0xc0026f00);
+	radeon_ring_write(ring, 0x00000000);
+	radeon_ring_write(ring, 0x00000000);
+	radeon_ring_write(ring, 0x00000000);
 
 	/* Clear consts */
-	radeon_ring_write(rdev, 0xc0036f00);
-	radeon_ring_write(rdev, 0x00000bc4);
-	radeon_ring_write(rdev, 0xffffffff);
-	radeon_ring_write(rdev, 0xffffffff);
-	radeon_ring_write(rdev, 0xffffffff);
+	radeon_ring_write(ring, 0xc0036f00);
+	radeon_ring_write(ring, 0x00000bc4);
+	radeon_ring_write(ring, 0xffffffff);
+	radeon_ring_write(ring, 0xffffffff);
+	radeon_ring_write(ring, 0xffffffff);
 
-	radeon_ring_write(rdev, 0xc0026900);
-	radeon_ring_write(rdev, 0x00000316);
-	radeon_ring_write(rdev, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
-	radeon_ring_write(rdev, 0x00000010); /*  */
+	radeon_ring_write(ring, 0xc0026900);
+	radeon_ring_write(ring, 0x00000316);
+	radeon_ring_write(ring, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
+	radeon_ring_write(ring, 0x00000010); /*  */
 
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_unlock_commit(rdev, ring);
 
 	return 0;
 }
 
 int evergreen_cp_resume(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 tmp;
 	u32 rb_bufsz;
 	int r;
@@ -1442,13 +1448,13 @@ int evergreen_cp_resume(struct radeon_device *rdev)
 	RREG32(GRBM_SOFT_RESET);
 
 	/* Set ring buffer size */
-	rb_bufsz = drm_order(rdev->cp.ring_size / 8);
+	rb_bufsz = drm_order(ring->ring_size / 8);
 	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
 #ifdef __BIG_ENDIAN
 	tmp |= BUF_SWAP_32BIT;
 #endif
 	WREG32(CP_RB_CNTL, tmp);
-	WREG32(CP_SEM_WAIT_TIMER, 0x4);
+	WREG32(CP_SEM_WAIT_TIMER, 0x0);
 
 	/* Set the write pointer delay */
 	WREG32(CP_RB_WPTR_DELAY, 0);
@@ -1456,8 +1462,8 @@ int evergreen_cp_resume(struct radeon_device *rdev)
 	/* Initialize the ring buffer's read and write pointers */
 	WREG32(CP_RB_CNTL, tmp | RB_RPTR_WR_ENA);
 	WREG32(CP_RB_RPTR_WR, 0);
-	rdev->cp.wptr = 0;
-	WREG32(CP_RB_WPTR, rdev->cp.wptr);
+	ring->wptr = 0;
+	WREG32(CP_RB_WPTR, ring->wptr);
 
 	/* set the wb address wether it's enabled or not */
 	WREG32(CP_RB_RPTR_ADDR,
@@ -1475,16 +1481,16 @@ int evergreen_cp_resume(struct radeon_device *rdev)
 	mdelay(1);
 	WREG32(CP_RB_CNTL, tmp);
 
-	WREG32(CP_RB_BASE, rdev->cp.gpu_addr >> 8);
+	WREG32(CP_RB_BASE, ring->gpu_addr >> 8);
 	WREG32(CP_DEBUG, (1 << 27) | (1 << 28));
 
-	rdev->cp.rptr = RREG32(CP_RB_RPTR);
+	ring->rptr = RREG32(CP_RB_RPTR);
 
 	evergreen_cp_start(rdev);
-	rdev->cp.ready = true;
-	r = radeon_ring_test(rdev);
+	ring->ready = true;
+	r = radeon_ring_test(rdev, ring);
 	if (r) {
-		rdev->cp.ready = false;
+		ring->ready = false;
 		return r;
 	}
 	return 0;
@@ -2353,7 +2359,7 @@ int evergreen_mc_init(struct radeon_device *rdev)
 	return 0;
 }
 
-bool evergreen_gpu_is_lockup(struct radeon_device *rdev)
+bool evergreen_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 srbm_status;
 	u32 grbm_status;
@@ -2366,19 +2372,19 @@ bool evergreen_gpu_is_lockup(struct radeon_device *rdev)
 	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
 	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
 	if (!(grbm_status & GUI_ACTIVE)) {
-		r100_gpu_lockup_update(lockup, &rdev->cp);
+		r100_gpu_lockup_update(lockup, ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, 2);
+	r = radeon_ring_lock(rdev, ring, 2);
 	if (!r) {
 		/* PACKET2 NOP */
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_unlock_commit(rdev);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_unlock_commit(rdev, ring);
 	}
-	rdev->cp.rptr = RREG32(CP_RB_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, lockup, &rdev->cp);
+	ring->rptr = RREG32(CP_RB_RPTR);
+	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
 }
 
 static int evergreen_gpu_soft_reset(struct radeon_device *rdev)
@@ -2470,7 +2476,13 @@ void evergreen_disable_interrupt_state(struct radeon_device *rdev)
 {
 	u32 tmp;
 
-	WREG32(CP_INT_CNTL, CNTX_BUSY_INT_ENABLE | CNTX_EMPTY_INT_ENABLE);
+	if (rdev->family >= CHIP_CAYMAN) {
+		cayman_cp_int_cntl_setup(rdev, 0,
+					 CNTX_BUSY_INT_ENABLE | CNTX_EMPTY_INT_ENABLE);
+		cayman_cp_int_cntl_setup(rdev, 1, 0);
+		cayman_cp_int_cntl_setup(rdev, 2, 0);
+	} else
+		WREG32(CP_INT_CNTL, CNTX_BUSY_INT_ENABLE | CNTX_EMPTY_INT_ENABLE);
 	WREG32(GRBM_INT_CNTL, 0);
 	WREG32(INT_MASK + EVERGREEN_CRTC0_REGISTER_OFFSET, 0);
 	WREG32(INT_MASK + EVERGREEN_CRTC1_REGISTER_OFFSET, 0);
@@ -2515,6 +2527,7 @@ void evergreen_disable_interrupt_state(struct radeon_device *rdev)
 int evergreen_irq_set(struct radeon_device *rdev)
 {
 	u32 cp_int_cntl = CNTX_BUSY_INT_ENABLE | CNTX_EMPTY_INT_ENABLE;
+	u32 cp_int_cntl1 = 0, cp_int_cntl2 = 0;
 	u32 crtc1 = 0, crtc2 = 0, crtc3 = 0, crtc4 = 0, crtc5 = 0, crtc6 = 0;
 	u32 hpd1, hpd2, hpd3, hpd4, hpd5, hpd6;
 	u32 grbm_int_cntl = 0;
@@ -2539,11 +2552,28 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	hpd5 = RREG32(DC_HPD5_INT_CONTROL) & ~DC_HPDx_INT_EN;
 	hpd6 = RREG32(DC_HPD6_INT_CONTROL) & ~DC_HPDx_INT_EN;
 
-	if (rdev->irq.sw_int) {
-		DRM_DEBUG("evergreen_irq_set: sw int\n");
-		cp_int_cntl |= RB_INT_ENABLE;
-		cp_int_cntl |= TIME_STAMP_INT_ENABLE;
+	if (rdev->family >= CHIP_CAYMAN) {
+		/* enable CP interrupts on all rings */
+		if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
+			DRM_DEBUG("evergreen_irq_set: sw int gfx\n");
+			cp_int_cntl |= TIME_STAMP_INT_ENABLE;
+		}
+		if (rdev->irq.sw_int[CAYMAN_RING_TYPE_CP1_INDEX]) {
+			DRM_DEBUG("evergreen_irq_set: sw int cp1\n");
+			cp_int_cntl1 |= TIME_STAMP_INT_ENABLE;
+		}
+		if (rdev->irq.sw_int[CAYMAN_RING_TYPE_CP2_INDEX]) {
+			DRM_DEBUG("evergreen_irq_set: sw int cp2\n");
+			cp_int_cntl2 |= TIME_STAMP_INT_ENABLE;
+		}
+	} else {
+		if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
+			DRM_DEBUG("evergreen_irq_set: sw int gfx\n");
+			cp_int_cntl |= RB_INT_ENABLE;
+			cp_int_cntl |= TIME_STAMP_INT_ENABLE;
+		}
 	}
+
 	if (rdev->irq.crtc_vblank_int[0] ||
 	    rdev->irq.pflip[0]) {
 		DRM_DEBUG("evergreen_irq_set: vblank 0\n");
@@ -2603,7 +2633,12 @@ int evergreen_irq_set(struct radeon_device *rdev)
 		grbm_int_cntl |= GUI_IDLE_INT_ENABLE;
 	}
 
-	WREG32(CP_INT_CNTL, cp_int_cntl);
+	if (rdev->family >= CHIP_CAYMAN) {
+		cayman_cp_int_cntl_setup(rdev, 0, cp_int_cntl);
+		cayman_cp_int_cntl_setup(rdev, 1, cp_int_cntl1);
+		cayman_cp_int_cntl_setup(rdev, 2, cp_int_cntl2);
+	} else
+		WREG32(CP_INT_CNTL, cp_int_cntl);
 	WREG32(GRBM_INT_CNTL, grbm_int_cntl);
 
 	WREG32(INT_MASK + EVERGREEN_CRTC0_REGISTER_OFFSET, crtc1);
@@ -3018,11 +3053,24 @@ restart_ih:
 		case 177: /* CP_INT in IB1 */
 		case 178: /* CP_INT in IB2 */
 			DRM_DEBUG("IH: CP int: 0x%08x\n", src_data);
-			radeon_fence_process(rdev);
+			radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
 			break;
 		case 181: /* CP EOP event */
 			DRM_DEBUG("IH: CP EOP\n");
-			radeon_fence_process(rdev);
+			if (rdev->family >= CHIP_CAYMAN) {
+				switch (src_data) {
+				case 0:
+					radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
+					break;
+				case 1:
+					radeon_fence_process(rdev, CAYMAN_RING_TYPE_CP1_INDEX);
+					break;
+				case 2:
+					radeon_fence_process(rdev, CAYMAN_RING_TYPE_CP2_INDEX);
+					break;
+				}
+			} else
+				radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
 			break;
 		case 233: /* GUI IDLE */
 			DRM_DEBUG("IH: GUI idle\n");
@@ -3052,6 +3100,7 @@ restart_ih:
 
 static int evergreen_startup(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
 	/* enable pcie gen2 link */
@@ -3106,6 +3155,12 @@ static int evergreen_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	r = r600_irq_init(rdev);
 	if (r) {
@@ -3115,7 +3170,9 @@ static int evergreen_startup(struct radeon_device *rdev)
 	}
 	evergreen_irq_set(rdev);
 
-	r = radeon_ring_init(rdev, rdev->cp.ring_size);
+	r = radeon_ring_init(rdev, ring, ring->ring_size, RADEON_WB_CP_RPTR_OFFSET,
+			     R600_CP_RB_RPTR, R600_CP_RB_WPTR,
+			     0, 0xfffff, RADEON_CP_PACKET2);
 	if (r)
 		return r;
 	r = evergreen_cp_load_microcode(rdev);
@@ -3125,6 +3182,22 @@ static int evergreen_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r600_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
+	r = r600_audio_init(rdev);
+	if (r) {
+		DRM_ERROR("radeon: audio init failed\n");
+		return r;
+	}
+
 	return 0;
 }
 
@@ -3144,31 +3217,30 @@ int evergreen_resume(struct radeon_device *rdev)
 	/* post card */
 	atom_asic_init(rdev->mode_info.atom_context);
 
+	rdev->accel_working = true;
 	r = evergreen_startup(rdev);
 	if (r) {
 		DRM_ERROR("evergreen startup failed on resume\n");
 		return r;
 	}
 
-	r = r600_ib_test(rdev);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		return r;
-	}
-
 	return r;
 
 }
 
 int evergreen_suspend(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+
+	r600_audio_fini(rdev);
 	/* FIXME: we should wait for ring to be empty */
+	radeon_ib_pool_suspend(rdev);
+	r600_blit_suspend(rdev);
 	r700_cp_stop(rdev);
-	rdev->cp.ready = false;
+	ring->ready = false;
 	evergreen_irq_suspend(rdev);
 	radeon_wb_disable(rdev);
 	evergreen_pcie_gart_disable(rdev);
-	r600_blit_suspend(rdev);
 
 	return 0;
 }
@@ -3243,8 +3315,8 @@ int evergreen_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	rdev->cp.ring_obj = NULL;
-	r600_ring_init(rdev, 1024 * 1024);
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ring_obj = NULL;
+	r600_ring_init(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX], 1024 * 1024);
 
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
@@ -3253,29 +3325,24 @@ int evergreen_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = evergreen_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
 		r700_cp_fini(rdev);
 		r600_irq_fini(rdev);
 		radeon_wb_fini(rdev);
+		r100_ib_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		evergreen_pcie_gart_fini(rdev);
 		rdev->accel_working = false;
 	}
-	if (rdev->accel_working) {
-		r = radeon_ib_pool_init(rdev);
-		if (r) {
-			DRM_ERROR("radeon: failed initializing IB pool (%d).\n", r);
-			rdev->accel_working = false;
-		}
-		r = r600_ib_test(rdev);
-		if (r) {
-			DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-			rdev->accel_working = false;
-		}
-	}
 
 	/* Don't start up if the MC ucode is missing on BTC parts.
 	 * The default clocks and voltages before the MC ucode
@@ -3293,15 +3360,17 @@ int evergreen_init(struct radeon_device *rdev)
 
 void evergreen_fini(struct radeon_device *rdev)
 {
+	r600_audio_fini(rdev);
 	r600_blit_fini(rdev);
 	r700_cp_fini(rdev);
 	r600_irq_fini(rdev);
 	radeon_wb_fini(rdev);
-	radeon_ib_pool_fini(rdev);
+	r100_ib_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	evergreen_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
+	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_bo_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/evergreen_blit_kms.c b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
index 914e5af84163..2379849515c7 100644
--- a/drivers/gpu/drm/radeon/evergreen_blit_kms.c
+++ b/drivers/gpu/drm/radeon/evergreen_blit_kms.c
@@ -49,6 +49,7 @@ static void
 set_render_target(struct radeon_device *rdev, int format,
 		  int w, int h, u64 gpu_addr)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 cb_color_info;
 	int pitch, slice;
 
@@ -62,23 +63,23 @@ set_render_target(struct radeon_device *rdev, int format,
 	pitch = (w / 8) - 1;
 	slice = ((w * h) / 64) - 1;
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 15));
-	radeon_ring_write(rdev, (CB_COLOR0_BASE - PACKET3_SET_CONTEXT_REG_START) >> 2);
-	radeon_ring_write(rdev, gpu_addr >> 8);
-	radeon_ring_write(rdev, pitch);
-	radeon_ring_write(rdev, slice);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, cb_color_info);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, (w - 1) | ((h - 1) << 16));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 15));
+	radeon_ring_write(ring, (CB_COLOR0_BASE - PACKET3_SET_CONTEXT_REG_START) >> 2);
+	radeon_ring_write(ring, gpu_addr >> 8);
+	radeon_ring_write(ring, pitch);
+	radeon_ring_write(ring, slice);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, cb_color_info);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, (w - 1) | ((h - 1) << 16));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
 }
 
 /* emits 5dw */
@@ -87,6 +88,7 @@ cp_set_surface_sync(struct radeon_device *rdev,
 		    u32 sync_type, u32 size,
 		    u64 mc_addr)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 cp_coher_size;
 
 	if (size == 0xffffffff)
@@ -99,39 +101,40 @@ cp_set_surface_sync(struct radeon_device *rdev,
 		 * to the RB directly. For IBs, the CP programs this as part of the
 		 * surface_sync packet.
 		 */
-		radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-		radeon_ring_write(rdev, (0x85e8 - PACKET3_SET_CONFIG_REG_START) >> 2);
-		radeon_ring_write(rdev, 0); /* CP_COHER_CNTL2 */
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, (0x85e8 - PACKET3_SET_CONFIG_REG_START) >> 2);
+		radeon_ring_write(ring, 0); /* CP_COHER_CNTL2 */
 	}
-	radeon_ring_write(rdev, PACKET3(PACKET3_SURFACE_SYNC, 3));
-	radeon_ring_write(rdev, sync_type);
-	radeon_ring_write(rdev, cp_coher_size);
-	radeon_ring_write(rdev, mc_addr >> 8);
-	radeon_ring_write(rdev, 10); /* poll interval */
+	radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
+	radeon_ring_write(ring, sync_type);
+	radeon_ring_write(ring, cp_coher_size);
+	radeon_ring_write(ring, mc_addr >> 8);
+	radeon_ring_write(ring, 10); /* poll interval */
 }
 
 /* emits 11dw + 1 surface sync = 16dw */
 static void
 set_shaders(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u64 gpu_addr;
 
 	/* VS */
 	gpu_addr = rdev->r600_blit.shader_gpu_addr + rdev->r600_blit.vs_offset;
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 3));
-	radeon_ring_write(rdev, (SQ_PGM_START_VS - PACKET3_SET_CONTEXT_REG_START) >> 2);
-	radeon_ring_write(rdev, gpu_addr >> 8);
-	radeon_ring_write(rdev, 2);
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 3));
+	radeon_ring_write(ring, (SQ_PGM_START_VS - PACKET3_SET_CONTEXT_REG_START) >> 2);
+	radeon_ring_write(ring, gpu_addr >> 8);
+	radeon_ring_write(ring, 2);
+	radeon_ring_write(ring, 0);
 
 	/* PS */
 	gpu_addr = rdev->r600_blit.shader_gpu_addr + rdev->r600_blit.ps_offset;
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 4));
-	radeon_ring_write(rdev, (SQ_PGM_START_PS - PACKET3_SET_CONTEXT_REG_START) >> 2);
-	radeon_ring_write(rdev, gpu_addr >> 8);
-	radeon_ring_write(rdev, 1);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 2);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 4));
+	radeon_ring_write(ring, (SQ_PGM_START_PS - PACKET3_SET_CONTEXT_REG_START) >> 2);
+	radeon_ring_write(ring, gpu_addr >> 8);
+	radeon_ring_write(ring, 1);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 2);
 
 	gpu_addr = rdev->r600_blit.shader_gpu_addr + rdev->r600_blit.vs_offset;
 	cp_set_surface_sync(rdev, PACKET3_SH_ACTION_ENA, 512, gpu_addr);
@@ -141,6 +144,7 @@ set_shaders(struct radeon_device *rdev)
 static void
 set_vtx_resource(struct radeon_device *rdev, u64 gpu_addr)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 sq_vtx_constant_word2, sq_vtx_constant_word3;
 
 	/* high addr, stride */
@@ -155,16 +159,16 @@ set_vtx_resource(struct radeon_device *rdev, u64 gpu_addr)
 		SQ_VTCX_SEL_Z(SQ_SEL_Z) |
 		SQ_VTCX_SEL_W(SQ_SEL_W);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_RESOURCE, 8));
-	radeon_ring_write(rdev, 0x580);
-	radeon_ring_write(rdev, gpu_addr & 0xffffffff);
-	radeon_ring_write(rdev, 48 - 1); /* size */
-	radeon_ring_write(rdev, sq_vtx_constant_word2);
-	radeon_ring_write(rdev, sq_vtx_constant_word3);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, S__SQ_CONSTANT_TYPE(SQ_TEX_VTX_VALID_BUFFER));
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_RESOURCE, 8));
+	radeon_ring_write(ring, 0x580);
+	radeon_ring_write(ring, gpu_addr & 0xffffffff);
+	radeon_ring_write(ring, 48 - 1); /* size */
+	radeon_ring_write(ring, sq_vtx_constant_word2);
+	radeon_ring_write(ring, sq_vtx_constant_word3);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, S__SQ_CONSTANT_TYPE(SQ_TEX_VTX_VALID_BUFFER));
 
 	if ((rdev->family == CHIP_CEDAR) ||
 	    (rdev->family == CHIP_PALM) ||
@@ -185,6 +189,7 @@ set_tex_resource(struct radeon_device *rdev,
 		 int format, int w, int h, int pitch,
 		 u64 gpu_addr, u32 size)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 sq_tex_resource_word0, sq_tex_resource_word1;
 	u32 sq_tex_resource_word4, sq_tex_resource_word7;
 
@@ -208,16 +213,16 @@ set_tex_resource(struct radeon_device *rdev,
 	cp_set_surface_sync(rdev,
 			    PACKET3_TC_ACTION_ENA, size, gpu_addr);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_RESOURCE, 8));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, sq_tex_resource_word0);
-	radeon_ring_write(rdev, sq_tex_resource_word1);
-	radeon_ring_write(rdev, gpu_addr >> 8);
-	radeon_ring_write(rdev, gpu_addr >> 8);
-	radeon_ring_write(rdev, sq_tex_resource_word4);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, sq_tex_resource_word7);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_RESOURCE, 8));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, sq_tex_resource_word0);
+	radeon_ring_write(ring, sq_tex_resource_word1);
+	radeon_ring_write(ring, gpu_addr >> 8);
+	radeon_ring_write(ring, gpu_addr >> 8);
+	radeon_ring_write(ring, sq_tex_resource_word4);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, sq_tex_resource_word7);
 }
 
 /* emits 12 */
@@ -225,6 +230,7 @@ static void
 set_scissors(struct radeon_device *rdev, int x1, int y1,
 	     int x2, int y2)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	/* workaround some hw bugs */
 	if (x2 == 0)
 		x1 = 1;
@@ -235,43 +241,44 @@ set_scissors(struct radeon_device *rdev, int x1, int y1,
 			x2 = 2;
 	}
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
-	radeon_ring_write(rdev, (PA_SC_SCREEN_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_START) >> 2);
-	radeon_ring_write(rdev, (x1 << 0) | (y1 << 16));
-	radeon_ring_write(rdev, (x2 << 0) | (y2 << 16));
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
+	radeon_ring_write(ring, (PA_SC_SCREEN_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_START) >> 2);
+	radeon_ring_write(ring, (x1 << 0) | (y1 << 16));
+	radeon_ring_write(ring, (x2 << 0) | (y2 << 16));
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
-	radeon_ring_write(rdev, (PA_SC_GENERIC_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_START) >> 2);
-	radeon_ring_write(rdev, (x1 << 0) | (y1 << 16) | (1 << 31));
-	radeon_ring_write(rdev, (x2 << 0) | (y2 << 16));
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
+	radeon_ring_write(ring, (PA_SC_GENERIC_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_START) >> 2);
+	radeon_ring_write(ring, (x1 << 0) | (y1 << 16) | (1 << 31));
+	radeon_ring_write(ring, (x2 << 0) | (y2 << 16));
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
-	radeon_ring_write(rdev, (PA_SC_WINDOW_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_START) >> 2);
-	radeon_ring_write(rdev, (x1 << 0) | (y1 << 16) | (1 << 31));
-	radeon_ring_write(rdev, (x2 << 0) | (y2 << 16));
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
+	radeon_ring_write(ring, (PA_SC_WINDOW_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_START) >> 2);
+	radeon_ring_write(ring, (x1 << 0) | (y1 << 16) | (1 << 31));
+	radeon_ring_write(ring, (x2 << 0) | (y2 << 16));
 }
 
 /* emits 10 */
 static void
 draw_auto(struct radeon_device *rdev)
 {
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-	radeon_ring_write(rdev, (VGT_PRIMITIVE_TYPE - PACKET3_SET_CONFIG_REG_START) >> 2);
-	radeon_ring_write(rdev, DI_PT_RECTLIST);
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+	radeon_ring_write(ring, (VGT_PRIMITIVE_TYPE - PACKET3_SET_CONFIG_REG_START) >> 2);
+	radeon_ring_write(ring, DI_PT_RECTLIST);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_INDEX_TYPE, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET3(PACKET3_INDEX_TYPE, 0));
+	radeon_ring_write(ring,
 #ifdef __BIG_ENDIAN
 			  (2 << 2) |
 #endif
 			  DI_INDEX_SIZE_16_BIT);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_NUM_INSTANCES, 0));
-	radeon_ring_write(rdev, 1);
+	radeon_ring_write(ring, PACKET3(PACKET3_NUM_INSTANCES, 0));
+	radeon_ring_write(ring, 1);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_DRAW_INDEX_AUTO, 1));
-	radeon_ring_write(rdev, 3);
-	radeon_ring_write(rdev, DI_SRC_SEL_AUTO_INDEX);
+	radeon_ring_write(ring, PACKET3(PACKET3_DRAW_INDEX_AUTO, 1));
+	radeon_ring_write(ring, 3);
+	radeon_ring_write(ring, DI_SRC_SEL_AUTO_INDEX);
 
 }
 
@@ -279,6 +286,7 @@ draw_auto(struct radeon_device *rdev)
 static void
 set_default_state(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 sq_config, sq_gpr_resource_mgmt_1, sq_gpr_resource_mgmt_2, sq_gpr_resource_mgmt_3;
 	u32 sq_thread_resource_mgmt, sq_thread_resource_mgmt_2;
 	u32 sq_stack_resource_mgmt_1, sq_stack_resource_mgmt_2, sq_stack_resource_mgmt_3;
@@ -292,8 +300,8 @@ set_default_state(struct radeon_device *rdev)
 	int dwords;
 
 	/* set clear context state */
-	radeon_ring_write(rdev, PACKET3(PACKET3_CLEAR_STATE, 0));
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_CLEAR_STATE, 0));
+	radeon_ring_write(ring, 0);
 
 	if (rdev->family < CHIP_CAYMAN) {
 		switch (rdev->family) {
@@ -550,60 +558,60 @@ set_default_state(struct radeon_device *rdev)
 					    NUM_LS_STACK_ENTRIES(num_ls_stack_entries));
 
 		/* disable dyn gprs */
-		radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-		radeon_ring_write(rdev, (SQ_DYN_GPR_CNTL_PS_FLUSH_REQ - PACKET3_SET_CONFIG_REG_START) >> 2);
-		radeon_ring_write(rdev, 0);
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, (SQ_DYN_GPR_CNTL_PS_FLUSH_REQ - PACKET3_SET_CONFIG_REG_START) >> 2);
+		radeon_ring_write(ring, 0);
 
 		/* setup LDS */
-		radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-		radeon_ring_write(rdev, (SQ_LDS_RESOURCE_MGMT - PACKET3_SET_CONFIG_REG_START) >> 2);
-		radeon_ring_write(rdev, 0x10001000);
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, (SQ_LDS_RESOURCE_MGMT - PACKET3_SET_CONFIG_REG_START) >> 2);
+		radeon_ring_write(ring, 0x10001000);
 
 		/* SQ config */
-		radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 11));
-		radeon_ring_write(rdev, (SQ_CONFIG - PACKET3_SET_CONFIG_REG_START) >> 2);
-		radeon_ring_write(rdev, sq_config);
-		radeon_ring_write(rdev, sq_gpr_resource_mgmt_1);
-		radeon_ring_write(rdev, sq_gpr_resource_mgmt_2);
-		radeon_ring_write(rdev, sq_gpr_resource_mgmt_3);
-		radeon_ring_write(rdev, 0);
-		radeon_ring_write(rdev, 0);
-		radeon_ring_write(rdev, sq_thread_resource_mgmt);
-		radeon_ring_write(rdev, sq_thread_resource_mgmt_2);
-		radeon_ring_write(rdev, sq_stack_resource_mgmt_1);
-		radeon_ring_write(rdev, sq_stack_resource_mgmt_2);
-		radeon_ring_write(rdev, sq_stack_resource_mgmt_3);
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 11));
+		radeon_ring_write(ring, (SQ_CONFIG - PACKET3_SET_CONFIG_REG_START) >> 2);
+		radeon_ring_write(ring, sq_config);
+		radeon_ring_write(ring, sq_gpr_resource_mgmt_1);
+		radeon_ring_write(ring, sq_gpr_resource_mgmt_2);
+		radeon_ring_write(ring, sq_gpr_resource_mgmt_3);
+		radeon_ring_write(ring, 0);
+		radeon_ring_write(ring, 0);
+		radeon_ring_write(ring, sq_thread_resource_mgmt);
+		radeon_ring_write(ring, sq_thread_resource_mgmt_2);
+		radeon_ring_write(ring, sq_stack_resource_mgmt_1);
+		radeon_ring_write(ring, sq_stack_resource_mgmt_2);
+		radeon_ring_write(ring, sq_stack_resource_mgmt_3);
 	}
 
 	/* CONTEXT_CONTROL */
-	radeon_ring_write(rdev, 0xc0012800);
-	radeon_ring_write(rdev, 0x80000000);
-	radeon_ring_write(rdev, 0x80000000);
+	radeon_ring_write(ring, 0xc0012800);
+	radeon_ring_write(ring, 0x80000000);
+	radeon_ring_write(ring, 0x80000000);
 
 	/* SQ_VTX_BASE_VTX_LOC */
-	radeon_ring_write(rdev, 0xc0026f00);
-	radeon_ring_write(rdev, 0x00000000);
-	radeon_ring_write(rdev, 0x00000000);
-	radeon_ring_write(rdev, 0x00000000);
+	radeon_ring_write(ring, 0xc0026f00);
+	radeon_ring_write(ring, 0x00000000);
+	radeon_ring_write(ring, 0x00000000);
+	radeon_ring_write(ring, 0x00000000);
 
 	/* SET_SAMPLER */
-	radeon_ring_write(rdev, 0xc0036e00);
-	radeon_ring_write(rdev, 0x00000000);
-	radeon_ring_write(rdev, 0x00000012);
-	radeon_ring_write(rdev, 0x00000000);
-	radeon_ring_write(rdev, 0x00000000);
+	radeon_ring_write(ring, 0xc0036e00);
+	radeon_ring_write(ring, 0x00000000);
+	radeon_ring_write(ring, 0x00000012);
+	radeon_ring_write(ring, 0x00000000);
+	radeon_ring_write(ring, 0x00000000);
 
 	/* set to DX10/11 mode */
-	radeon_ring_write(rdev, PACKET3(PACKET3_MODE_CONTROL, 0));
-	radeon_ring_write(rdev, 1);
+	radeon_ring_write(ring, PACKET3(PACKET3_MODE_CONTROL, 0));
+	radeon_ring_write(ring, 1);
 
 	/* emit an IB pointing at default state */
 	dwords = ALIGN(rdev->r600_blit.state_len, 0x10);
 	gpu_addr = rdev->r600_blit.shader_gpu_addr + rdev->r600_blit.state_offset;
-	radeon_ring_write(rdev, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
-	radeon_ring_write(rdev, gpu_addr & 0xFFFFFFFC);
-	radeon_ring_write(rdev, upper_32_bits(gpu_addr) & 0xFF);
-	radeon_ring_write(rdev, dwords);
+	radeon_ring_write(ring, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
+	radeon_ring_write(ring, gpu_addr & 0xFFFFFFFC);
+	radeon_ring_write(ring, upper_32_bits(gpu_addr) & 0xFF);
+	radeon_ring_write(ring, dwords);
 
 }
 
diff --git a/drivers/gpu/drm/radeon/evergreen_cs.c b/drivers/gpu/drm/radeon/evergreen_cs.c
index cd4590aae154..f7442e62c03f 100644
--- a/drivers/gpu/drm/radeon/evergreen_cs.c
+++ b/drivers/gpu/drm/radeon/evergreen_cs.c
@@ -520,7 +520,7 @@ static int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 		break;
 	case DB_Z_INFO:
 		track->db_z_info = radeon_get_ib_value(p, idx);
-		if (!p->keep_tiling_flags) {
+		if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
 			r = evergreen_cs_packet_next_reloc(p, &reloc);
 			if (r) {
 				dev_warn(p->dev, "bad SET_CONTEXT_REG "
@@ -649,7 +649,7 @@ static int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 	case CB_COLOR7_INFO:
 		tmp = (reg - CB_COLOR0_INFO) / 0x3c;
 		track->cb_color_info[tmp] = radeon_get_ib_value(p, idx);
-		if (!p->keep_tiling_flags) {
+		if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
 			r = evergreen_cs_packet_next_reloc(p, &reloc);
 			if (r) {
 				dev_warn(p->dev, "bad SET_CONTEXT_REG "
@@ -666,7 +666,7 @@ static int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 	case CB_COLOR11_INFO:
 		tmp = ((reg - CB_COLOR8_INFO) / 0x1c) + 8;
 		track->cb_color_info[tmp] = radeon_get_ib_value(p, idx);
-		if (!p->keep_tiling_flags) {
+		if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
 			r = evergreen_cs_packet_next_reloc(p, &reloc);
 			if (r) {
 				dev_warn(p->dev, "bad SET_CONTEXT_REG "
@@ -1355,7 +1355,7 @@ static int evergreen_packet3_check(struct radeon_cs_parser *p,
 					return -EINVAL;
 				}
 				ib[idx+1+(i*8)+2] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
-				if (!p->keep_tiling_flags) {
+				if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
 					ib[idx+1+(i*8)+1] |=
 						TEX_ARRAY_MODE(evergreen_cs_get_aray_mode(reloc->lobj.tiling_flags));
 					if (reloc->lobj.tiling_flags & RADEON_TILING_MACRO) {
@@ -1572,3 +1572,241 @@ int evergreen_cs_parse(struct radeon_cs_parser *p)
 	return 0;
 }
 
+/* vm parser */
+static bool evergreen_vm_reg_valid(u32 reg)
+{
+	/* context regs are fine */
+	if (reg >= 0x28000)
+		return true;
+
+	/* check config regs */
+	switch (reg) {
+	case GRBM_GFX_INDEX:
+	case VGT_VTX_VECT_EJECT_REG:
+	case VGT_CACHE_INVALIDATION:
+	case VGT_GS_VERTEX_REUSE:
+	case VGT_PRIMITIVE_TYPE:
+	case VGT_INDEX_TYPE:
+	case VGT_NUM_INDICES:
+	case VGT_NUM_INSTANCES:
+	case VGT_COMPUTE_DIM_X:
+	case VGT_COMPUTE_DIM_Y:
+	case VGT_COMPUTE_DIM_Z:
+	case VGT_COMPUTE_START_X:
+	case VGT_COMPUTE_START_Y:
+	case VGT_COMPUTE_START_Z:
+	case VGT_COMPUTE_INDEX:
+	case VGT_COMPUTE_THREAD_GROUP_SIZE:
+	case VGT_HS_OFFCHIP_PARAM:
+	case PA_CL_ENHANCE:
+	case PA_SU_LINE_STIPPLE_VALUE:
+	case PA_SC_LINE_STIPPLE_STATE:
+	case PA_SC_ENHANCE:
+	case SQ_DYN_GPR_CNTL_PS_FLUSH_REQ:
+	case SQ_DYN_GPR_SIMD_LOCK_EN:
+	case SQ_CONFIG:
+	case SQ_GPR_RESOURCE_MGMT_1:
+	case SQ_GLOBAL_GPR_RESOURCE_MGMT_1:
+	case SQ_GLOBAL_GPR_RESOURCE_MGMT_2:
+	case SQ_CONST_MEM_BASE:
+	case SQ_STATIC_THREAD_MGMT_1:
+	case SQ_STATIC_THREAD_MGMT_2:
+	case SQ_STATIC_THREAD_MGMT_3:
+	case SPI_CONFIG_CNTL:
+	case SPI_CONFIG_CNTL_1:
+	case TA_CNTL_AUX:
+	case DB_DEBUG:
+	case DB_DEBUG2:
+	case DB_DEBUG3:
+	case DB_DEBUG4:
+	case DB_WATERMARKS:
+	case TD_PS_BORDER_COLOR_INDEX:
+	case TD_PS_BORDER_COLOR_RED:
+	case TD_PS_BORDER_COLOR_GREEN:
+	case TD_PS_BORDER_COLOR_BLUE:
+	case TD_PS_BORDER_COLOR_ALPHA:
+	case TD_VS_BORDER_COLOR_INDEX:
+	case TD_VS_BORDER_COLOR_RED:
+	case TD_VS_BORDER_COLOR_GREEN:
+	case TD_VS_BORDER_COLOR_BLUE:
+	case TD_VS_BORDER_COLOR_ALPHA:
+	case TD_GS_BORDER_COLOR_INDEX:
+	case TD_GS_BORDER_COLOR_RED:
+	case TD_GS_BORDER_COLOR_GREEN:
+	case TD_GS_BORDER_COLOR_BLUE:
+	case TD_GS_BORDER_COLOR_ALPHA:
+	case TD_HS_BORDER_COLOR_INDEX:
+	case TD_HS_BORDER_COLOR_RED:
+	case TD_HS_BORDER_COLOR_GREEN:
+	case TD_HS_BORDER_COLOR_BLUE:
+	case TD_HS_BORDER_COLOR_ALPHA:
+	case TD_LS_BORDER_COLOR_INDEX:
+	case TD_LS_BORDER_COLOR_RED:
+	case TD_LS_BORDER_COLOR_GREEN:
+	case TD_LS_BORDER_COLOR_BLUE:
+	case TD_LS_BORDER_COLOR_ALPHA:
+	case TD_CS_BORDER_COLOR_INDEX:
+	case TD_CS_BORDER_COLOR_RED:
+	case TD_CS_BORDER_COLOR_GREEN:
+	case TD_CS_BORDER_COLOR_BLUE:
+	case TD_CS_BORDER_COLOR_ALPHA:
+	case SQ_ESGS_RING_SIZE:
+	case SQ_GSVS_RING_SIZE:
+	case SQ_ESTMP_RING_SIZE:
+	case SQ_GSTMP_RING_SIZE:
+	case SQ_HSTMP_RING_SIZE:
+	case SQ_LSTMP_RING_SIZE:
+	case SQ_PSTMP_RING_SIZE:
+	case SQ_VSTMP_RING_SIZE:
+	case SQ_ESGS_RING_ITEMSIZE:
+	case SQ_ESTMP_RING_ITEMSIZE:
+	case SQ_GSTMP_RING_ITEMSIZE:
+	case SQ_GSVS_RING_ITEMSIZE:
+	case SQ_GS_VERT_ITEMSIZE:
+	case SQ_GS_VERT_ITEMSIZE_1:
+	case SQ_GS_VERT_ITEMSIZE_2:
+	case SQ_GS_VERT_ITEMSIZE_3:
+	case SQ_GSVS_RING_OFFSET_1:
+	case SQ_GSVS_RING_OFFSET_2:
+	case SQ_GSVS_RING_OFFSET_3:
+	case SQ_HSTMP_RING_ITEMSIZE:
+	case SQ_LSTMP_RING_ITEMSIZE:
+	case SQ_PSTMP_RING_ITEMSIZE:
+	case SQ_VSTMP_RING_ITEMSIZE:
+	case VGT_TF_RING_SIZE:
+	case SQ_ESGS_RING_BASE:
+	case SQ_GSVS_RING_BASE:
+	case SQ_ESTMP_RING_BASE:
+	case SQ_GSTMP_RING_BASE:
+	case SQ_HSTMP_RING_BASE:
+	case SQ_LSTMP_RING_BASE:
+	case SQ_PSTMP_RING_BASE:
+	case SQ_VSTMP_RING_BASE:
+	case CAYMAN_VGT_OFFCHIP_LDS_BASE:
+	case CAYMAN_SQ_EX_ALLOC_TABLE_SLOTS:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static int evergreen_vm_packet3_check(struct radeon_device *rdev,
+				      u32 *ib, struct radeon_cs_packet *pkt)
+{
+	u32 idx = pkt->idx + 1;
+	u32 idx_value = ib[idx];
+	u32 start_reg, end_reg, reg, i;
+
+	switch (pkt->opcode) {
+	case PACKET3_NOP:
+	case PACKET3_SET_BASE:
+	case PACKET3_CLEAR_STATE:
+	case PACKET3_INDEX_BUFFER_SIZE:
+	case PACKET3_DISPATCH_DIRECT:
+	case PACKET3_DISPATCH_INDIRECT:
+	case PACKET3_MODE_CONTROL:
+	case PACKET3_SET_PREDICATION:
+	case PACKET3_COND_EXEC:
+	case PACKET3_PRED_EXEC:
+	case PACKET3_DRAW_INDIRECT:
+	case PACKET3_DRAW_INDEX_INDIRECT:
+	case PACKET3_INDEX_BASE:
+	case PACKET3_DRAW_INDEX_2:
+	case PACKET3_CONTEXT_CONTROL:
+	case PACKET3_DRAW_INDEX_OFFSET:
+	case PACKET3_INDEX_TYPE:
+	case PACKET3_DRAW_INDEX:
+	case PACKET3_DRAW_INDEX_AUTO:
+	case PACKET3_DRAW_INDEX_IMMD:
+	case PACKET3_NUM_INSTANCES:
+	case PACKET3_DRAW_INDEX_MULTI_AUTO:
+	case PACKET3_STRMOUT_BUFFER_UPDATE:
+	case PACKET3_DRAW_INDEX_OFFSET_2:
+	case PACKET3_DRAW_INDEX_MULTI_ELEMENT:
+	case PACKET3_MPEG_INDEX:
+	case PACKET3_WAIT_REG_MEM:
+	case PACKET3_MEM_WRITE:
+	case PACKET3_SURFACE_SYNC:
+	case PACKET3_EVENT_WRITE:
+	case PACKET3_EVENT_WRITE_EOP:
+	case PACKET3_EVENT_WRITE_EOS:
+	case PACKET3_SET_CONTEXT_REG:
+	case PACKET3_SET_BOOL_CONST:
+	case PACKET3_SET_LOOP_CONST:
+	case PACKET3_SET_RESOURCE:
+	case PACKET3_SET_SAMPLER:
+	case PACKET3_SET_CTL_CONST:
+	case PACKET3_SET_RESOURCE_OFFSET:
+	case PACKET3_SET_CONTEXT_REG_INDIRECT:
+	case PACKET3_SET_RESOURCE_INDIRECT:
+	case CAYMAN_PACKET3_DEALLOC_STATE:
+		break;
+	case PACKET3_COND_WRITE:
+		if (idx_value & 0x100) {
+			reg = ib[idx + 5] * 4;
+			if (!evergreen_vm_reg_valid(reg))
+				return -EINVAL;
+		}
+		break;
+	case PACKET3_COPY_DW:
+		if (idx_value & 0x2) {
+			reg = ib[idx + 3] * 4;
+			if (!evergreen_vm_reg_valid(reg))
+				return -EINVAL;
+		}
+		break;
+	case PACKET3_SET_CONFIG_REG:
+		start_reg = (idx_value << 2) + PACKET3_SET_CONFIG_REG_START;
+		end_reg = 4 * pkt->count + start_reg - 4;
+		if ((start_reg < PACKET3_SET_CONFIG_REG_START) ||
+		    (start_reg >= PACKET3_SET_CONFIG_REG_END) ||
+		    (end_reg >= PACKET3_SET_CONFIG_REG_END)) {
+			DRM_ERROR("bad PACKET3_SET_CONFIG_REG\n");
+			return -EINVAL;
+		}
+		for (i = 0; i < pkt->count; i++) {
+			reg = start_reg + (4 * i);
+			if (!evergreen_vm_reg_valid(reg))
+				return -EINVAL;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int evergreen_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib)
+{
+	int ret = 0;
+	u32 idx = 0;
+	struct radeon_cs_packet pkt;
+
+	do {
+		pkt.idx = idx;
+		pkt.type = CP_PACKET_GET_TYPE(ib->ptr[idx]);
+		pkt.count = CP_PACKET_GET_COUNT(ib->ptr[idx]);
+		pkt.one_reg_wr = 0;
+		switch (pkt.type) {
+		case PACKET_TYPE0:
+			dev_err(rdev->dev, "Packet0 not allowed!\n");
+			ret = -EINVAL;
+			break;
+		case PACKET_TYPE2:
+			break;
+		case PACKET_TYPE3:
+			pkt.opcode = CP_PACKET3_GET_OPCODE(ib->ptr[idx]);
+			ret = evergreen_vm_packet3_check(rdev, ib->ptr, &pkt);
+			break;
+		default:
+			dev_err(rdev->dev, "Unknown packet type %d !\n", pkt.type);
+			ret = -EINVAL;
+			break;
+		}
+		if (ret)
+			break;
+		idx += pkt.count + 2;
+	} while (idx < ib->length_dw);
+
+	return ret;
+}
diff --git a/drivers/gpu/drm/radeon/evergreen_reg.h b/drivers/gpu/drm/radeon/evergreen_reg.h
index 7d7f2155e34c..4215de95477e 100644
--- a/drivers/gpu/drm/radeon/evergreen_reg.h
+++ b/drivers/gpu/drm/radeon/evergreen_reg.h
@@ -35,6 +35,14 @@
 #define EVERGREEN_P1PLL_SS_CNTL                         0x414
 #define EVERGREEN_P2PLL_SS_CNTL                         0x454
 #       define EVERGREEN_PxPLL_SS_EN                    (1 << 12)
+
+#define EVERGREEN_AUDIO_PLL1_MUL			0x5b0
+#define EVERGREEN_AUDIO_PLL1_DIV			0x5b4
+#define EVERGREEN_AUDIO_PLL1_UNK			0x5bc
+
+#define EVERGREEN_AUDIO_ENABLE				0x5e78
+#define EVERGREEN_AUDIO_VENDOR_ID			0x5ec0
+
 /* GRPH blocks at 0x6800, 0x7400, 0x10000, 0x10c00, 0x11800, 0x12400 */
 #define EVERGREEN_GRPH_ENABLE                           0x6800
 #define EVERGREEN_GRPH_CONTROL                          0x6804
@@ -220,4 +228,9 @@
 #define EVERGREEN_DC_GPIO_HPD_EN                        0x64b8
 #define EVERGREEN_DC_GPIO_HPD_Y                         0x64bc
 
+/* HDMI blocks at 0x7030, 0x7c30, 0x10830, 0x11430, 0x12030, 0x12c30 */
+#define EVERGREEN_HDMI_BASE				0x7030
+
+#define EVERGREEN_HDMI_CONFIG_OFFSET			0xf0
+
 #endif
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index e00039e59a75..b502216d42af 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -242,6 +242,7 @@
 #define	PA_CL_ENHANCE					0x8A14
 #define		CLIP_VTX_REORDER_ENA				(1 << 0)
 #define		NUM_CLIP_SEQ(x)					((x) << 1)
+#define	PA_SC_ENHANCE					0x8BF0
 #define PA_SC_AA_CONFIG					0x28C04
 #define         MSAA_NUM_SAMPLES_SHIFT                  0
 #define         MSAA_NUM_SAMPLES_MASK                   0x3
@@ -319,6 +320,8 @@
 #define	SQ_GPR_RESOURCE_MGMT_3				0x8C0C
 #define		NUM_HS_GPRS(x)					((x) << 0)
 #define		NUM_LS_GPRS(x)					((x) << 16)
+#define	SQ_GLOBAL_GPR_RESOURCE_MGMT_1			0x8C10
+#define	SQ_GLOBAL_GPR_RESOURCE_MGMT_2			0x8C14
 #define	SQ_THREAD_RESOURCE_MGMT				0x8C18
 #define		NUM_PS_THREADS(x)				((x) << 0)
 #define		NUM_VS_THREADS(x)				((x) << 8)
@@ -337,6 +340,10 @@
 #define		NUM_HS_STACK_ENTRIES(x)				((x) << 0)
 #define		NUM_LS_STACK_ENTRIES(x)				((x) << 16)
 #define	SQ_DYN_GPR_CNTL_PS_FLUSH_REQ    		0x8D8C
+#define	SQ_DYN_GPR_SIMD_LOCK_EN    			0x8D94
+#define	SQ_STATIC_THREAD_MGMT_1    			0x8E20
+#define	SQ_STATIC_THREAD_MGMT_2    			0x8E24
+#define	SQ_STATIC_THREAD_MGMT_3    			0x8E28
 #define	SQ_LDS_RESOURCE_MGMT    			0x8E2C
 
 #define	SQ_MS_FIFO_SIZES				0x8CF0
@@ -691,6 +698,7 @@
 #define	PACKET3_DRAW_INDEX_MULTI_ELEMENT		0x36
 #define	PACKET3_MEM_SEMAPHORE				0x39
 #define	PACKET3_MPEG_INDEX				0x3A
+#define	PACKET3_COPY_DW					0x3B
 #define	PACKET3_WAIT_REG_MEM				0x3C
 #define	PACKET3_MEM_WRITE				0x3D
 #define	PACKET3_INDIRECT_BUFFER				0x32
@@ -768,6 +776,8 @@
 #define			SQ_TEX_VTX_VALID_TEXTURE			0x2
 #define			SQ_TEX_VTX_VALID_BUFFER				0x3
 
+#define VGT_VTX_VECT_EJECT_REG				0x88b0
+
 #define SQ_CONST_MEM_BASE				0x8df8
 
 #define SQ_ESGS_RING_BASE				0x8c40
@@ -892,8 +902,27 @@
 #define PA_SC_SCREEN_SCISSOR_TL                         0x28030
 #define PA_SC_GENERIC_SCISSOR_TL                        0x28240
 #define PA_SC_WINDOW_SCISSOR_TL                         0x28204
-#define VGT_PRIMITIVE_TYPE                              0x8958
 
+#define VGT_PRIMITIVE_TYPE                              0x8958
+#define VGT_INDEX_TYPE                                  0x895C
+
+#define VGT_NUM_INDICES                                 0x8970
+
+#define VGT_COMPUTE_DIM_X                               0x8990
+#define VGT_COMPUTE_DIM_Y                               0x8994
+#define VGT_COMPUTE_DIM_Z                               0x8998
+#define VGT_COMPUTE_START_X                             0x899C
+#define VGT_COMPUTE_START_Y                             0x89A0
+#define VGT_COMPUTE_START_Z                             0x89A4
+#define VGT_COMPUTE_INDEX                               0x89A8
+#define VGT_COMPUTE_THREAD_GROUP_SIZE                   0x89AC
+#define VGT_HS_OFFCHIP_PARAM                            0x89B0
+
+#define DB_DEBUG					0x9830
+#define DB_DEBUG2					0x9834
+#define DB_DEBUG3					0x9838
+#define DB_DEBUG4					0x983C
+#define DB_WATERMARKS					0x9854
 #define DB_DEPTH_CONTROL				0x28800
 #define DB_DEPTH_VIEW					0x28008
 #define DB_HTILE_DATA_BASE				0x28014
@@ -1189,8 +1218,40 @@
 #define SQ_VTX_CONSTANT_WORD6_0                         0x30018
 #define SQ_VTX_CONSTANT_WORD7_0                         0x3001c
 
+#define TD_PS_BORDER_COLOR_INDEX                        0xA400
+#define TD_PS_BORDER_COLOR_RED                          0xA404
+#define TD_PS_BORDER_COLOR_GREEN                        0xA408
+#define TD_PS_BORDER_COLOR_BLUE                         0xA40C
+#define TD_PS_BORDER_COLOR_ALPHA                        0xA410
+#define TD_VS_BORDER_COLOR_INDEX                        0xA414
+#define TD_VS_BORDER_COLOR_RED                          0xA418
+#define TD_VS_BORDER_COLOR_GREEN                        0xA41C
+#define TD_VS_BORDER_COLOR_BLUE                         0xA420
+#define TD_VS_BORDER_COLOR_ALPHA                        0xA424
+#define TD_GS_BORDER_COLOR_INDEX                        0xA428
+#define TD_GS_BORDER_COLOR_RED                          0xA42C
+#define TD_GS_BORDER_COLOR_GREEN                        0xA430
+#define TD_GS_BORDER_COLOR_BLUE                         0xA434
+#define TD_GS_BORDER_COLOR_ALPHA                        0xA438
+#define TD_HS_BORDER_COLOR_INDEX                        0xA43C
+#define TD_HS_BORDER_COLOR_RED                          0xA440
+#define TD_HS_BORDER_COLOR_GREEN                        0xA444
+#define TD_HS_BORDER_COLOR_BLUE                         0xA448
+#define TD_HS_BORDER_COLOR_ALPHA                        0xA44C
+#define TD_LS_BORDER_COLOR_INDEX                        0xA450
+#define TD_LS_BORDER_COLOR_RED                          0xA454
+#define TD_LS_BORDER_COLOR_GREEN                        0xA458
+#define TD_LS_BORDER_COLOR_BLUE                         0xA45C
+#define TD_LS_BORDER_COLOR_ALPHA                        0xA460
+#define TD_CS_BORDER_COLOR_INDEX                        0xA464
+#define TD_CS_BORDER_COLOR_RED                          0xA468
+#define TD_CS_BORDER_COLOR_GREEN                        0xA46C
+#define TD_CS_BORDER_COLOR_BLUE                         0xA470
+#define TD_CS_BORDER_COLOR_ALPHA                        0xA474
+
 /* cayman 3D regs */
-#define CAYMAN_VGT_OFFCHIP_LDS_BASE			0x89B0
+#define CAYMAN_VGT_OFFCHIP_LDS_BASE			0x89B4
+#define CAYMAN_SQ_EX_ALLOC_TABLE_SLOTS			0x8E48
 #define CAYMAN_DB_EQAA					0x28804
 #define CAYMAN_DB_DEPTH_INFO				0x2803C
 #define CAYMAN_PA_SC_AA_CONFIG				0x28BE0
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index 0e5799857465..321137295400 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -934,7 +934,7 @@ void cayman_pcie_gart_tlb_flush(struct radeon_device *rdev)
 
 int cayman_pcie_gart_enable(struct radeon_device *rdev)
 {
-	int r;
+	int i, r;
 
 	if (rdev->gart.robj == NULL) {
 		dev_err(rdev->dev, "No VRAM object for PCIE GART.\n");
@@ -945,9 +945,12 @@ int cayman_pcie_gart_enable(struct radeon_device *rdev)
 		return r;
 	radeon_gart_restore(rdev);
 	/* Setup TLB control */
-	WREG32(MC_VM_MX_L1_TLB_CNTL, ENABLE_L1_TLB |
+	WREG32(MC_VM_MX_L1_TLB_CNTL,
+	       (0xA << 7) |
+	       ENABLE_L1_TLB |
 	       ENABLE_L1_FRAGMENT_PROCESSING |
 	       SYSTEM_ACCESS_MODE_NOT_IN_SYS |
+	       ENABLE_ADVANCED_DRIVER_MODEL |
 	       SYSTEM_APERTURE_UNMAPPED_ACCESS_PASS_THRU);
 	/* Setup L2 cache */
 	WREG32(VM_L2_CNTL, ENABLE_L2_CACHE |
@@ -967,9 +970,26 @@ int cayman_pcie_gart_enable(struct radeon_device *rdev)
 	WREG32(VM_CONTEXT0_CNTL2, 0);
 	WREG32(VM_CONTEXT0_CNTL, ENABLE_CONTEXT | PAGE_TABLE_DEPTH(0) |
 				RANGE_PROTECTION_FAULT_ENABLE_DEFAULT);
-	/* disable context1-7 */
+
+	WREG32(0x15D4, 0);
+	WREG32(0x15D8, 0);
+	WREG32(0x15DC, 0);
+
+	/* empty context1-7 */
+	for (i = 1; i < 8; i++) {
+		WREG32(VM_CONTEXT0_PAGE_TABLE_START_ADDR + (i << 2), 0);
+		WREG32(VM_CONTEXT0_PAGE_TABLE_END_ADDR + (i << 2), 0);
+		WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2),
+			rdev->gart.table_addr >> 12);
+	}
+
+	/* enable context1-7 */
+	WREG32(VM_CONTEXT1_PROTECTION_FAULT_DEFAULT_ADDR,
+	       (u32)(rdev->dummy_page.addr >> 12));
 	WREG32(VM_CONTEXT1_CNTL2, 0);
 	WREG32(VM_CONTEXT1_CNTL, 0);
+	WREG32(VM_CONTEXT1_CNTL, ENABLE_CONTEXT | PAGE_TABLE_DEPTH(0) |
+				RANGE_PROTECTION_FAULT_ENABLE_DEFAULT);
 
 	cayman_pcie_gart_tlb_flush(rdev);
 	DRM_INFO("PCIE GART of %uM enabled (table at 0x%016llX).\n",
@@ -1006,9 +1026,69 @@ void cayman_pcie_gart_fini(struct radeon_device *rdev)
 	radeon_gart_fini(rdev);
 }
 
+void cayman_cp_int_cntl_setup(struct radeon_device *rdev,
+			      int ring, u32 cp_int_cntl)
+{
+	u32 srbm_gfx_cntl = RREG32(SRBM_GFX_CNTL) & ~3;
+
+	WREG32(SRBM_GFX_CNTL, srbm_gfx_cntl | (ring & 3));
+	WREG32(CP_INT_CNTL, cp_int_cntl);
+}
+
 /*
  * CP.
  */
+void cayman_fence_ring_emit(struct radeon_device *rdev,
+			    struct radeon_fence *fence)
+{
+	struct radeon_ring *ring = &rdev->ring[fence->ring];
+	u64 addr = rdev->fence_drv[fence->ring].gpu_addr;
+
+	/* flush read cache over gart for this vmid */
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+	radeon_ring_write(ring, (CP_COHER_CNTL2 - PACKET3_SET_CONFIG_REG_START) >> 2);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
+	radeon_ring_write(ring, PACKET3_TC_ACTION_ENA | PACKET3_SH_ACTION_ENA);
+	radeon_ring_write(ring, 0xFFFFFFFF);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 10); /* poll interval */
+	/* EVENT_WRITE_EOP - flush caches, send int */
+	radeon_ring_write(ring, PACKET3(PACKET3_EVENT_WRITE_EOP, 4));
+	radeon_ring_write(ring, EVENT_TYPE(CACHE_FLUSH_AND_INV_EVENT_TS) | EVENT_INDEX(5));
+	radeon_ring_write(ring, addr & 0xffffffff);
+	radeon_ring_write(ring, (upper_32_bits(addr) & 0xff) | DATA_SEL(1) | INT_SEL(2));
+	radeon_ring_write(ring, fence->seq);
+	radeon_ring_write(ring, 0);
+}
+
+void cayman_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
+{
+	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
+
+	/* set to DX10/11 mode */
+	radeon_ring_write(ring, PACKET3(PACKET3_MODE_CONTROL, 0));
+	radeon_ring_write(ring, 1);
+	radeon_ring_write(ring, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
+	radeon_ring_write(ring,
+#ifdef __BIG_ENDIAN
+			  (2 << 0) |
+#endif
+			  (ib->gpu_addr & 0xFFFFFFFC));
+	radeon_ring_write(ring, upper_32_bits(ib->gpu_addr) & 0xFF);
+	radeon_ring_write(ring, ib->length_dw | (ib->vm_id << 24));
+
+	/* flush read cache over gart for this vmid */
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+	radeon_ring_write(ring, (CP_COHER_CNTL2 - PACKET3_SET_CONFIG_REG_START) >> 2);
+	radeon_ring_write(ring, ib->vm_id);
+	radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
+	radeon_ring_write(ring, PACKET3_TC_ACTION_ENA | PACKET3_SH_ACTION_ENA);
+	radeon_ring_write(ring, 0xFFFFFFFF);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 10); /* poll interval */
+}
+
 static void cayman_cp_enable(struct radeon_device *rdev, bool enable)
 {
 	if (enable)
@@ -1049,63 +1129,64 @@ static int cayman_cp_load_microcode(struct radeon_device *rdev)
 
 static int cayman_cp_start(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r, i;
 
-	r = radeon_ring_lock(rdev, 7);
+	r = radeon_ring_lock(rdev, ring, 7);
 	if (r) {
 		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
 		return r;
 	}
-	radeon_ring_write(rdev, PACKET3(PACKET3_ME_INITIALIZE, 5));
-	radeon_ring_write(rdev, 0x1);
-	radeon_ring_write(rdev, 0x0);
-	radeon_ring_write(rdev, rdev->config.cayman.max_hw_contexts - 1);
-	radeon_ring_write(rdev, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_write(ring, PACKET3(PACKET3_ME_INITIALIZE, 5));
+	radeon_ring_write(ring, 0x1);
+	radeon_ring_write(ring, 0x0);
+	radeon_ring_write(ring, rdev->config.cayman.max_hw_contexts - 1);
+	radeon_ring_write(ring, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_unlock_commit(rdev, ring);
 
 	cayman_cp_enable(rdev, true);
 
-	r = radeon_ring_lock(rdev, cayman_default_size + 19);
+	r = radeon_ring_lock(rdev, ring, cayman_default_size + 19);
 	if (r) {
 		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
 		return r;
 	}
 
 	/* setup clear context state */
-	radeon_ring_write(rdev, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
-	radeon_ring_write(rdev, PACKET3_PREAMBLE_BEGIN_CLEAR_STATE);
+	radeon_ring_write(ring, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
+	radeon_ring_write(ring, PACKET3_PREAMBLE_BEGIN_CLEAR_STATE);
 
 	for (i = 0; i < cayman_default_size; i++)
-		radeon_ring_write(rdev, cayman_default_state[i]);
+		radeon_ring_write(ring, cayman_default_state[i]);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
-	radeon_ring_write(rdev, PACKET3_PREAMBLE_END_CLEAR_STATE);
+	radeon_ring_write(ring, PACKET3(PACKET3_PREAMBLE_CNTL, 0));
+	radeon_ring_write(ring, PACKET3_PREAMBLE_END_CLEAR_STATE);
 
 	/* set clear context state */
-	radeon_ring_write(rdev, PACKET3(PACKET3_CLEAR_STATE, 0));
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_CLEAR_STATE, 0));
+	radeon_ring_write(ring, 0);
 
 	/* SQ_VTX_BASE_VTX_LOC */
-	radeon_ring_write(rdev, 0xc0026f00);
-	radeon_ring_write(rdev, 0x00000000);
-	radeon_ring_write(rdev, 0x00000000);
-	radeon_ring_write(rdev, 0x00000000);
+	radeon_ring_write(ring, 0xc0026f00);
+	radeon_ring_write(ring, 0x00000000);
+	radeon_ring_write(ring, 0x00000000);
+	radeon_ring_write(ring, 0x00000000);
 
 	/* Clear consts */
-	radeon_ring_write(rdev, 0xc0036f00);
-	radeon_ring_write(rdev, 0x00000bc4);
-	radeon_ring_write(rdev, 0xffffffff);
-	radeon_ring_write(rdev, 0xffffffff);
-	radeon_ring_write(rdev, 0xffffffff);
+	radeon_ring_write(ring, 0xc0036f00);
+	radeon_ring_write(ring, 0x00000bc4);
+	radeon_ring_write(ring, 0xffffffff);
+	radeon_ring_write(ring, 0xffffffff);
+	radeon_ring_write(ring, 0xffffffff);
 
-	radeon_ring_write(rdev, 0xc0026900);
-	radeon_ring_write(rdev, 0x00000316);
-	radeon_ring_write(rdev, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
-	radeon_ring_write(rdev, 0x00000010); /*  */
+	radeon_ring_write(ring, 0xc0026900);
+	radeon_ring_write(ring, 0x00000316);
+	radeon_ring_write(ring, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
+	radeon_ring_write(ring, 0x00000010); /*  */
 
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_unlock_commit(rdev, ring);
 
 	/* XXX init other rings */
 
@@ -1115,11 +1196,12 @@ static int cayman_cp_start(struct radeon_device *rdev)
 static void cayman_cp_fini(struct radeon_device *rdev)
 {
 	cayman_cp_enable(rdev, false);
-	radeon_ring_fini(rdev);
+	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 }
 
 int cayman_cp_resume(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring;
 	u32 tmp;
 	u32 rb_bufsz;
 	int r;
@@ -1136,7 +1218,7 @@ int cayman_cp_resume(struct radeon_device *rdev)
 	WREG32(GRBM_SOFT_RESET, 0);
 	RREG32(GRBM_SOFT_RESET);
 
-	WREG32(CP_SEM_WAIT_TIMER, 0x4);
+	WREG32(CP_SEM_WAIT_TIMER, 0x0);
 
 	/* Set the write pointer delay */
 	WREG32(CP_RB_WPTR_DELAY, 0);
@@ -1145,7 +1227,8 @@ int cayman_cp_resume(struct radeon_device *rdev)
 
 	/* ring 0 - compute and gfx */
 	/* Set ring buffer size */
-	rb_bufsz = drm_order(rdev->cp.ring_size / 8);
+	ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+	rb_bufsz = drm_order(ring->ring_size / 8);
 	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
 #ifdef __BIG_ENDIAN
 	tmp |= BUF_SWAP_32BIT;
@@ -1154,8 +1237,8 @@ int cayman_cp_resume(struct radeon_device *rdev)
 
 	/* Initialize the ring buffer's read and write pointers */
 	WREG32(CP_RB0_CNTL, tmp | RB_RPTR_WR_ENA);
-	rdev->cp.wptr = 0;
-	WREG32(CP_RB0_WPTR, rdev->cp.wptr);
+	ring->wptr = 0;
+	WREG32(CP_RB0_WPTR, ring->wptr);
 
 	/* set the wb address wether it's enabled or not */
 	WREG32(CP_RB0_RPTR_ADDR, (rdev->wb.gpu_addr + RADEON_WB_CP_RPTR_OFFSET) & 0xFFFFFFFC);
@@ -1172,13 +1255,14 @@ int cayman_cp_resume(struct radeon_device *rdev)
 	mdelay(1);
 	WREG32(CP_RB0_CNTL, tmp);
 
-	WREG32(CP_RB0_BASE, rdev->cp.gpu_addr >> 8);
+	WREG32(CP_RB0_BASE, ring->gpu_addr >> 8);
 
-	rdev->cp.rptr = RREG32(CP_RB0_RPTR);
+	ring->rptr = RREG32(CP_RB0_RPTR);
 
 	/* ring1  - compute only */
 	/* Set ring buffer size */
-	rb_bufsz = drm_order(rdev->cp1.ring_size / 8);
+	ring = &rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX];
+	rb_bufsz = drm_order(ring->ring_size / 8);
 	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
 #ifdef __BIG_ENDIAN
 	tmp |= BUF_SWAP_32BIT;
@@ -1187,8 +1271,8 @@ int cayman_cp_resume(struct radeon_device *rdev)
 
 	/* Initialize the ring buffer's read and write pointers */
 	WREG32(CP_RB1_CNTL, tmp | RB_RPTR_WR_ENA);
-	rdev->cp1.wptr = 0;
-	WREG32(CP_RB1_WPTR, rdev->cp1.wptr);
+	ring->wptr = 0;
+	WREG32(CP_RB1_WPTR, ring->wptr);
 
 	/* set the wb address wether it's enabled or not */
 	WREG32(CP_RB1_RPTR_ADDR, (rdev->wb.gpu_addr + RADEON_WB_CP1_RPTR_OFFSET) & 0xFFFFFFFC);
@@ -1197,13 +1281,14 @@ int cayman_cp_resume(struct radeon_device *rdev)
 	mdelay(1);
 	WREG32(CP_RB1_CNTL, tmp);
 
-	WREG32(CP_RB1_BASE, rdev->cp1.gpu_addr >> 8);
+	WREG32(CP_RB1_BASE, ring->gpu_addr >> 8);
 
-	rdev->cp1.rptr = RREG32(CP_RB1_RPTR);
+	ring->rptr = RREG32(CP_RB1_RPTR);
 
 	/* ring2 - compute only */
 	/* Set ring buffer size */
-	rb_bufsz = drm_order(rdev->cp2.ring_size / 8);
+	ring = &rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX];
+	rb_bufsz = drm_order(ring->ring_size / 8);
 	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
 #ifdef __BIG_ENDIAN
 	tmp |= BUF_SWAP_32BIT;
@@ -1212,8 +1297,8 @@ int cayman_cp_resume(struct radeon_device *rdev)
 
 	/* Initialize the ring buffer's read and write pointers */
 	WREG32(CP_RB2_CNTL, tmp | RB_RPTR_WR_ENA);
-	rdev->cp2.wptr = 0;
-	WREG32(CP_RB2_WPTR, rdev->cp2.wptr);
+	ring->wptr = 0;
+	WREG32(CP_RB2_WPTR, ring->wptr);
 
 	/* set the wb address wether it's enabled or not */
 	WREG32(CP_RB2_RPTR_ADDR, (rdev->wb.gpu_addr + RADEON_WB_CP2_RPTR_OFFSET) & 0xFFFFFFFC);
@@ -1222,28 +1307,28 @@ int cayman_cp_resume(struct radeon_device *rdev)
 	mdelay(1);
 	WREG32(CP_RB2_CNTL, tmp);
 
-	WREG32(CP_RB2_BASE, rdev->cp2.gpu_addr >> 8);
+	WREG32(CP_RB2_BASE, ring->gpu_addr >> 8);
 
-	rdev->cp2.rptr = RREG32(CP_RB2_RPTR);
+	ring->rptr = RREG32(CP_RB2_RPTR);
 
 	/* start the rings */
 	cayman_cp_start(rdev);
-	rdev->cp.ready = true;
-	rdev->cp1.ready = true;
-	rdev->cp2.ready = true;
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = true;
+	rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX].ready = false;
+	rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX].ready = false;
 	/* this only test cp0 */
-	r = radeon_ring_test(rdev);
+	r = radeon_ring_test(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 	if (r) {
-		rdev->cp.ready = false;
-		rdev->cp1.ready = false;
-		rdev->cp2.ready = false;
+		rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
+		rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX].ready = false;
+		rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX].ready = false;
 		return r;
 	}
 
 	return 0;
 }
 
-bool cayman_gpu_is_lockup(struct radeon_device *rdev)
+bool cayman_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 srbm_status;
 	u32 grbm_status;
@@ -1256,20 +1341,20 @@ bool cayman_gpu_is_lockup(struct radeon_device *rdev)
 	grbm_status_se0 = RREG32(GRBM_STATUS_SE0);
 	grbm_status_se1 = RREG32(GRBM_STATUS_SE1);
 	if (!(grbm_status & GUI_ACTIVE)) {
-		r100_gpu_lockup_update(lockup, &rdev->cp);
+		r100_gpu_lockup_update(lockup, ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, 2);
+	r = radeon_ring_lock(rdev, ring, 2);
 	if (!r) {
 		/* PACKET2 NOP */
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_unlock_commit(rdev);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_unlock_commit(rdev, ring);
 	}
 	/* XXX deal with CP0,1,2 */
-	rdev->cp.rptr = RREG32(CP_RB0_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, lockup, &rdev->cp);
+	ring->rptr = RREG32(ring->rptr_reg);
+	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
 }
 
 static int cayman_gpu_soft_reset(struct radeon_device *rdev)
@@ -1289,6 +1374,15 @@ static int cayman_gpu_soft_reset(struct radeon_device *rdev)
 		RREG32(GRBM_STATUS_SE1));
 	dev_info(rdev->dev, "  SRBM_STATUS=0x%08X\n",
 		RREG32(SRBM_STATUS));
+	dev_info(rdev->dev, "  VM_CONTEXT0_PROTECTION_FAULT_ADDR   0x%08X\n",
+		 RREG32(0x14F8));
+	dev_info(rdev->dev, "  VM_CONTEXT0_PROTECTION_FAULT_STATUS 0x%08X\n",
+		 RREG32(0x14D8));
+	dev_info(rdev->dev, "  VM_CONTEXT1_PROTECTION_FAULT_ADDR   0x%08X\n",
+		 RREG32(0x14FC));
+	dev_info(rdev->dev, "  VM_CONTEXT1_PROTECTION_FAULT_STATUS 0x%08X\n",
+		 RREG32(0x14DC));
+
 	evergreen_mc_stop(rdev, &save);
 	if (evergreen_mc_wait_for_idle(rdev)) {
 		dev_warn(rdev->dev, "Wait for MC idle timedout !\n");
@@ -1319,6 +1413,7 @@ static int cayman_gpu_soft_reset(struct radeon_device *rdev)
 	(void)RREG32(GRBM_SOFT_RESET);
 	/* Wait a little for things to settle down */
 	udelay(50);
+
 	dev_info(rdev->dev, "  GRBM_STATUS=0x%08X\n",
 		RREG32(GRBM_STATUS));
 	dev_info(rdev->dev, "  GRBM_STATUS_SE0=0x%08X\n",
@@ -1338,6 +1433,7 @@ int cayman_asic_reset(struct radeon_device *rdev)
 
 static int cayman_startup(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
 	/* enable pcie gen2 link */
@@ -1378,6 +1474,24 @@ static int cayman_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
+	r = radeon_fence_driver_start_ring(rdev, CAYMAN_RING_TYPE_CP1_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
+	r = radeon_fence_driver_start_ring(rdev, CAYMAN_RING_TYPE_CP2_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	r = r600_irq_init(rdev);
 	if (r) {
@@ -1387,7 +1501,9 @@ static int cayman_startup(struct radeon_device *rdev)
 	}
 	evergreen_irq_set(rdev);
 
-	r = radeon_ring_init(rdev, rdev->cp.ring_size);
+	r = radeon_ring_init(rdev, ring, ring->ring_size, RADEON_WB_CP_RPTR_OFFSET,
+			     CP_RB0_RPTR, CP_RB0_WPTR,
+			     0, 0xfffff, RADEON_CP_PACKET2);
 	if (r)
 		return r;
 	r = cayman_cp_load_microcode(rdev);
@@ -1397,6 +1513,21 @@ static int cayman_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r600_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
+		return r;
+	}
+
+	r = radeon_vm_manager_start(rdev);
+	if (r)
+		return r;
+
 	return 0;
 }
 
@@ -1411,32 +1542,26 @@ int cayman_resume(struct radeon_device *rdev)
 	/* post card */
 	atom_asic_init(rdev->mode_info.atom_context);
 
+	rdev->accel_working = true;
 	r = cayman_startup(rdev);
 	if (r) {
 		DRM_ERROR("cayman startup failed on resume\n");
 		return r;
 	}
-
-	r = r600_ib_test(rdev);
-	if (r) {
-		DRM_ERROR("radeon: failled testing IB (%d).\n", r);
-		return r;
-	}
-
 	return r;
-
 }
 
 int cayman_suspend(struct radeon_device *rdev)
 {
 	/* FIXME: we should wait for ring to be empty */
+	radeon_ib_pool_suspend(rdev);
+	radeon_vm_manager_suspend(rdev);
+	r600_blit_suspend(rdev);
 	cayman_cp_enable(rdev, false);
-	rdev->cp.ready = false;
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	evergreen_irq_suspend(rdev);
 	radeon_wb_disable(rdev);
 	cayman_pcie_gart_disable(rdev);
-	r600_blit_suspend(rdev);
-
 	return 0;
 }
 
@@ -1448,6 +1573,7 @@ int cayman_suspend(struct radeon_device *rdev)
  */
 int cayman_init(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
 	/* This don't do much */
@@ -1500,8 +1626,8 @@ int cayman_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	rdev->cp.ring_obj = NULL;
-	r600_ring_init(rdev, 1024 * 1024);
+	ring->ring_obj = NULL;
+	r600_ring_init(rdev, ring, 1024 * 1024);
 
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
@@ -1510,29 +1636,29 @@ int cayman_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+	r = radeon_vm_manager_init(rdev);
+	if (r) {
+		dev_err(rdev->dev, "vm manager initialization failed (%d).\n", r);
+	}
+
 	r = cayman_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
 		cayman_cp_fini(rdev);
 		r600_irq_fini(rdev);
 		radeon_wb_fini(rdev);
+		r100_ib_fini(rdev);
+		radeon_vm_manager_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		cayman_pcie_gart_fini(rdev);
 		rdev->accel_working = false;
 	}
-	if (rdev->accel_working) {
-		r = radeon_ib_pool_init(rdev);
-		if (r) {
-			DRM_ERROR("radeon: failed initializing IB pool (%d).\n", r);
-			rdev->accel_working = false;
-		}
-		r = r600_ib_test(rdev);
-		if (r) {
-			DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-			rdev->accel_working = false;
-		}
-	}
 
 	/* Don't start up if the MC ucode is missing.
 	 * The default clocks and voltages before the MC ucode
@@ -1552,11 +1678,13 @@ void cayman_fini(struct radeon_device *rdev)
 	cayman_cp_fini(rdev);
 	r600_irq_fini(rdev);
 	radeon_wb_fini(rdev);
-	radeon_ib_pool_fini(rdev);
+	radeon_vm_manager_fini(rdev);
+	r100_ib_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	cayman_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
+	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_bo_fini(rdev);
 	radeon_atombios_fini(rdev);
@@ -1564,3 +1692,84 @@ void cayman_fini(struct radeon_device *rdev)
 	rdev->bios = NULL;
 }
 
+/*
+ * vm
+ */
+int cayman_vm_init(struct radeon_device *rdev)
+{
+	/* number of VMs */
+	rdev->vm_manager.nvm = 8;
+	/* base offset of vram pages */
+	rdev->vm_manager.vram_base_offset = 0;
+	return 0;
+}
+
+void cayman_vm_fini(struct radeon_device *rdev)
+{
+}
+
+int cayman_vm_bind(struct radeon_device *rdev, struct radeon_vm *vm, int id)
+{
+	WREG32(VM_CONTEXT0_PAGE_TABLE_START_ADDR + (id << 2), 0);
+	WREG32(VM_CONTEXT0_PAGE_TABLE_END_ADDR + (id << 2), vm->last_pfn);
+	WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (id << 2), vm->pt_gpu_addr >> 12);
+	/* flush hdp cache */
+	WREG32(HDP_MEM_COHERENCY_FLUSH_CNTL, 0x1);
+	/* bits 0-7 are the VM contexts0-7 */
+	WREG32(VM_INVALIDATE_REQUEST, 1 << id);
+	return 0;
+}
+
+void cayman_vm_unbind(struct radeon_device *rdev, struct radeon_vm *vm)
+{
+	WREG32(VM_CONTEXT0_PAGE_TABLE_START_ADDR + (vm->id << 2), 0);
+	WREG32(VM_CONTEXT0_PAGE_TABLE_END_ADDR + (vm->id << 2), 0);
+	WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (vm->id << 2), 0);
+	/* flush hdp cache */
+	WREG32(HDP_MEM_COHERENCY_FLUSH_CNTL, 0x1);
+	/* bits 0-7 are the VM contexts0-7 */
+	WREG32(VM_INVALIDATE_REQUEST, 1 << vm->id);
+}
+
+void cayman_vm_tlb_flush(struct radeon_device *rdev, struct radeon_vm *vm)
+{
+	if (vm->id == -1)
+		return;
+
+	/* flush hdp cache */
+	WREG32(HDP_MEM_COHERENCY_FLUSH_CNTL, 0x1);
+	/* bits 0-7 are the VM contexts0-7 */
+	WREG32(VM_INVALIDATE_REQUEST, 1 << vm->id);
+}
+
+#define R600_PTE_VALID     (1 << 0)
+#define R600_PTE_SYSTEM    (1 << 1)
+#define R600_PTE_SNOOPED   (1 << 2)
+#define R600_PTE_READABLE  (1 << 5)
+#define R600_PTE_WRITEABLE (1 << 6)
+
+uint32_t cayman_vm_page_flags(struct radeon_device *rdev,
+			      struct radeon_vm *vm,
+			      uint32_t flags)
+{
+	uint32_t r600_flags = 0;
+
+	r600_flags |= (flags & RADEON_VM_PAGE_VALID) ? R600_PTE_VALID : 0;
+	r600_flags |= (flags & RADEON_VM_PAGE_READABLE) ? R600_PTE_READABLE : 0;
+	r600_flags |= (flags & RADEON_VM_PAGE_WRITEABLE) ? R600_PTE_WRITEABLE : 0;
+	if (flags & RADEON_VM_PAGE_SYSTEM) {
+		r600_flags |= R600_PTE_SYSTEM;
+		r600_flags |= (flags & RADEON_VM_PAGE_SNOOPED) ? R600_PTE_SNOOPED : 0;
+	}
+	return r600_flags;
+}
+
+void cayman_vm_set_page(struct radeon_device *rdev, struct radeon_vm *vm,
+			unsigned pfn, uint64_t addr, uint32_t flags)
+{
+	void __iomem *ptr = (void *)vm->pt;
+
+	addr = addr & 0xFFFFFFFFFFFFF000ULL;
+	addr |= flags;
+	writeq(addr, ptr + (pfn * 8));
+}
diff --git a/drivers/gpu/drm/radeon/nid.h b/drivers/gpu/drm/radeon/nid.h
index 4672869cdb26..f9df2a645e79 100644
--- a/drivers/gpu/drm/radeon/nid.h
+++ b/drivers/gpu/drm/radeon/nid.h
@@ -42,6 +42,9 @@
 #define CAYMAN_MAX_TCC_MASK          0xFF
 
 #define DMIF_ADDR_CONFIG  				0xBD4
+#define	SRBM_GFX_CNTL				        0x0E44
+#define		RINGID(x)					(((x) & 0x3) << 0)
+#define		VMID(x)						(((x) & 0x7) << 0)
 #define	SRBM_STATUS				        0x0E50
 
 #define VM_CONTEXT0_REQUEST_RESPONSE			0x1470
@@ -219,6 +222,7 @@
 #define	SCRATCH_UMSK					0x8540
 #define	SCRATCH_ADDR					0x8544
 #define	CP_SEM_WAIT_TIMER				0x85BC
+#define	CP_COHER_CNTL2					0x85E8
 #define CP_ME_CNTL					0x86D8
 #define		CP_ME_HALT					(1 << 28)
 #define		CP_PFP_HALT					(1 << 26)
@@ -394,6 +398,12 @@
 #define	CP_RB0_RPTR_ADDR				0xC10C
 #define	CP_RB0_RPTR_ADDR_HI				0xC110
 #define	CP_RB0_WPTR					0xC114
+
+#define CP_INT_CNTL                                     0xC124
+#       define CNTX_BUSY_INT_ENABLE                     (1 << 19)
+#       define CNTX_EMPTY_INT_ENABLE                    (1 << 20)
+#       define TIME_STAMP_INT_ENABLE                    (1 << 26)
+
 #define	CP_RB1_BASE					0xC180
 #define	CP_RB1_CNTL					0xC184
 #define	CP_RB1_RPTR_ADDR				0xC188
@@ -411,6 +421,10 @@
 #define	CP_ME_RAM_DATA					0xC160
 #define	CP_DEBUG					0xC1FC
 
+#define VGT_EVENT_INITIATOR                             0x28a90
+#       define CACHE_FLUSH_AND_INV_EVENT_TS                     (0x14 << 0)
+#       define CACHE_FLUSH_AND_INV_EVENT                        (0x16 << 0)
+
 /*
  * PM4
  */
@@ -445,6 +459,7 @@
 #define	PACKET3_DISPATCH_DIRECT				0x15
 #define	PACKET3_DISPATCH_INDIRECT			0x16
 #define	PACKET3_INDIRECT_BUFFER_END			0x17
+#define	PACKET3_MODE_CONTROL				0x18
 #define	PACKET3_SET_PREDICATION				0x20
 #define	PACKET3_REG_RMW					0x21
 #define	PACKET3_COND_EXEC				0x22
@@ -494,7 +509,27 @@
 #define		PACKET3_ME_INITIALIZE_DEVICE_ID(x) ((x) << 16)
 #define	PACKET3_COND_WRITE				0x45
 #define	PACKET3_EVENT_WRITE				0x46
+#define		EVENT_TYPE(x)                           ((x) << 0)
+#define		EVENT_INDEX(x)                          ((x) << 8)
+                /* 0 - any non-TS event
+		 * 1 - ZPASS_DONE
+		 * 2 - SAMPLE_PIPELINESTAT
+		 * 3 - SAMPLE_STREAMOUTSTAT*
+		 * 4 - *S_PARTIAL_FLUSH
+		 * 5 - TS events
+		 */
 #define	PACKET3_EVENT_WRITE_EOP				0x47
+#define		DATA_SEL(x)                             ((x) << 29)
+                /* 0 - discard
+		 * 1 - send low 32bit data
+		 * 2 - send 64bit data
+		 * 3 - send 64bit counter value
+		 */
+#define		INT_SEL(x)                              ((x) << 24)
+                /* 0 - none
+		 * 1 - interrupt only (DATA_SEL = 0)
+		 * 2 - interrupt when data write is confirmed
+		 */
 #define	PACKET3_EVENT_WRITE_EOS				0x48
 #define	PACKET3_PREAMBLE_CNTL				0x4A
 #              define PACKET3_PREAMBLE_BEGIN_CLEAR_STATE     (2 << 28)
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index bfc08f6320f8..3ec81c3d5108 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -667,7 +667,7 @@ int r100_irq_set(struct radeon_device *rdev)
 		WREG32(R_000040_GEN_INT_CNTL, 0);
 		return -EINVAL;
 	}
-	if (rdev->irq.sw_int) {
+	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
 		tmp |= RADEON_SW_INT_ENABLE;
 	}
 	if (rdev->irq.gui_idle) {
@@ -739,7 +739,7 @@ int r100_irq_process(struct radeon_device *rdev)
 	while (status) {
 		/* SW interrupt */
 		if (status & RADEON_SW_INT_TEST) {
-			radeon_fence_process(rdev);
+			radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
 		}
 		/* gui idle interrupt */
 		if (status & RADEON_GUI_IDLE_STAT) {
@@ -811,25 +811,36 @@ u32 r100_get_vblank_counter(struct radeon_device *rdev, int crtc)
 void r100_fence_ring_emit(struct radeon_device *rdev,
 			  struct radeon_fence *fence)
 {
+	struct radeon_ring *ring = &rdev->ring[fence->ring];
+
 	/* We have to make sure that caches are flushed before
 	 * CPU might read something from VRAM. */
-	radeon_ring_write(rdev, PACKET0(RADEON_RB3D_DSTCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, RADEON_RB3D_DC_FLUSH_ALL);
-	radeon_ring_write(rdev, PACKET0(RADEON_RB3D_ZCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, RADEON_RB3D_ZC_FLUSH_ALL);
+	radeon_ring_write(ring, PACKET0(RADEON_RB3D_DSTCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, RADEON_RB3D_DC_FLUSH_ALL);
+	radeon_ring_write(ring, PACKET0(RADEON_RB3D_ZCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, RADEON_RB3D_ZC_FLUSH_ALL);
 	/* Wait until IDLE & CLEAN */
-	radeon_ring_write(rdev, PACKET0(RADEON_WAIT_UNTIL, 0));
-	radeon_ring_write(rdev, RADEON_WAIT_2D_IDLECLEAN | RADEON_WAIT_3D_IDLECLEAN);
-	radeon_ring_write(rdev, PACKET0(RADEON_HOST_PATH_CNTL, 0));
-	radeon_ring_write(rdev, rdev->config.r100.hdp_cntl |
+	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
+	radeon_ring_write(ring, RADEON_WAIT_2D_IDLECLEAN | RADEON_WAIT_3D_IDLECLEAN);
+	radeon_ring_write(ring, PACKET0(RADEON_HOST_PATH_CNTL, 0));
+	radeon_ring_write(ring, rdev->config.r100.hdp_cntl |
 				RADEON_HDP_READ_BUFFER_INVALIDATE);
-	radeon_ring_write(rdev, PACKET0(RADEON_HOST_PATH_CNTL, 0));
-	radeon_ring_write(rdev, rdev->config.r100.hdp_cntl);
+	radeon_ring_write(ring, PACKET0(RADEON_HOST_PATH_CNTL, 0));
+	radeon_ring_write(ring, rdev->config.r100.hdp_cntl);
 	/* Emit fence sequence & fire IRQ */
-	radeon_ring_write(rdev, PACKET0(rdev->fence_drv.scratch_reg, 0));
-	radeon_ring_write(rdev, fence->seq);
-	radeon_ring_write(rdev, PACKET0(RADEON_GEN_INT_STATUS, 0));
-	radeon_ring_write(rdev, RADEON_SW_INT_FIRE);
+	radeon_ring_write(ring, PACKET0(rdev->fence_drv[fence->ring].scratch_reg, 0));
+	radeon_ring_write(ring, fence->seq);
+	radeon_ring_write(ring, PACKET0(RADEON_GEN_INT_STATUS, 0));
+	radeon_ring_write(ring, RADEON_SW_INT_FIRE);
+}
+
+void r100_semaphore_ring_emit(struct radeon_device *rdev,
+			      struct radeon_ring *ring,
+			      struct radeon_semaphore *semaphore,
+			      bool emit_wait)
+{
+	/* Unused on older asics, since we don't have semaphores or multiple rings */
+	BUG();
 }
 
 int r100_copy_blit(struct radeon_device *rdev,
@@ -838,6 +849,7 @@ int r100_copy_blit(struct radeon_device *rdev,
 		   unsigned num_gpu_pages,
 		   struct radeon_fence *fence)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	uint32_t cur_pages;
 	uint32_t stride_bytes = RADEON_GPU_PAGE_SIZE;
 	uint32_t pitch;
@@ -855,7 +867,7 @@ int r100_copy_blit(struct radeon_device *rdev,
 
 	/* Ask for enough room for blit + flush + fence */
 	ndw = 64 + (10 * num_loops);
-	r = radeon_ring_lock(rdev, ndw);
+	r = radeon_ring_lock(rdev, ring, ndw);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d) asking for %u dw.\n", r, ndw);
 		return -EINVAL;
@@ -869,8 +881,8 @@ int r100_copy_blit(struct radeon_device *rdev,
 
 		/* pages are in Y direction - height
 		   page width in X direction - width */
-		radeon_ring_write(rdev, PACKET3(PACKET3_BITBLT_MULTI, 8));
-		radeon_ring_write(rdev,
+		radeon_ring_write(ring, PACKET3(PACKET3_BITBLT_MULTI, 8));
+		radeon_ring_write(ring,
 				  RADEON_GMC_SRC_PITCH_OFFSET_CNTL |
 				  RADEON_GMC_DST_PITCH_OFFSET_CNTL |
 				  RADEON_GMC_SRC_CLIPPING |
@@ -882,26 +894,26 @@ int r100_copy_blit(struct radeon_device *rdev,
 				  RADEON_DP_SRC_SOURCE_MEMORY |
 				  RADEON_GMC_CLR_CMP_CNTL_DIS |
 				  RADEON_GMC_WR_MSK_DIS);
-		radeon_ring_write(rdev, (pitch << 22) | (src_offset >> 10));
-		radeon_ring_write(rdev, (pitch << 22) | (dst_offset >> 10));
-		radeon_ring_write(rdev, (0x1fff) | (0x1fff << 16));
-		radeon_ring_write(rdev, 0);
-		radeon_ring_write(rdev, (0x1fff) | (0x1fff << 16));
-		radeon_ring_write(rdev, num_gpu_pages);
-		radeon_ring_write(rdev, num_gpu_pages);
-		radeon_ring_write(rdev, cur_pages | (stride_pixels << 16));
-	}
-	radeon_ring_write(rdev, PACKET0(RADEON_DSTCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, RADEON_RB2D_DC_FLUSH_ALL);
-	radeon_ring_write(rdev, PACKET0(RADEON_WAIT_UNTIL, 0));
-	radeon_ring_write(rdev,
+		radeon_ring_write(ring, (pitch << 22) | (src_offset >> 10));
+		radeon_ring_write(ring, (pitch << 22) | (dst_offset >> 10));
+		radeon_ring_write(ring, (0x1fff) | (0x1fff << 16));
+		radeon_ring_write(ring, 0);
+		radeon_ring_write(ring, (0x1fff) | (0x1fff << 16));
+		radeon_ring_write(ring, num_gpu_pages);
+		radeon_ring_write(ring, num_gpu_pages);
+		radeon_ring_write(ring, cur_pages | (stride_pixels << 16));
+	}
+	radeon_ring_write(ring, PACKET0(RADEON_DSTCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, RADEON_RB2D_DC_FLUSH_ALL);
+	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
+	radeon_ring_write(ring,
 			  RADEON_WAIT_2D_IDLECLEAN |
 			  RADEON_WAIT_HOST_IDLECLEAN |
 			  RADEON_WAIT_DMA_GUI_IDLE);
 	if (fence) {
 		r = radeon_fence_emit(rdev, fence);
 	}
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_unlock_commit(rdev, ring);
 	return r;
 }
 
@@ -922,19 +934,20 @@ static int r100_cp_wait_for_idle(struct radeon_device *rdev)
 
 void r100_ring_start(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	r = radeon_ring_lock(rdev, 2);
+	r = radeon_ring_lock(rdev, ring, 2);
 	if (r) {
 		return;
 	}
-	radeon_ring_write(rdev, PACKET0(RADEON_ISYNC_CNTL, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(RADEON_ISYNC_CNTL, 0));
+	radeon_ring_write(ring,
 			  RADEON_ISYNC_ANY2D_IDLE3D |
 			  RADEON_ISYNC_ANY3D_IDLE2D |
 			  RADEON_ISYNC_WAIT_IDLEGUI |
 			  RADEON_ISYNC_CPSCRATCH_IDLEGUI);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_unlock_commit(rdev, ring);
 }
 
 
@@ -1035,6 +1048,7 @@ static void r100_cp_load_microcode(struct radeon_device *rdev)
 
 int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	unsigned rb_bufsz;
 	unsigned rb_blksz;
 	unsigned max_fetch;
@@ -1060,7 +1074,9 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 	rb_bufsz = drm_order(ring_size / 8);
 	ring_size = (1 << (rb_bufsz + 1)) * 4;
 	r100_cp_load_microcode(rdev);
-	r = radeon_ring_init(rdev, ring_size);
+	r = radeon_ring_init(rdev, ring, ring_size, RADEON_WB_CP_RPTR_OFFSET,
+			     RADEON_CP_RB_RPTR, RADEON_CP_RB_WPTR,
+			     0, 0x7fffff, RADEON_CP_PACKET2);
 	if (r) {
 		return r;
 	}
@@ -1069,7 +1085,7 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 	rb_blksz = 9;
 	/* cp will read 128bytes at a time (4 dwords) */
 	max_fetch = 1;
-	rdev->cp.align_mask = 16 - 1;
+	ring->align_mask = 16 - 1;
 	/* Write to CP_RB_WPTR will be delayed for pre_write_timer clocks */
 	pre_write_timer = 64;
 	/* Force CP_RB_WPTR write if written more than one time before the
@@ -1099,13 +1115,13 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 	WREG32(RADEON_CP_RB_CNTL, tmp | RADEON_RB_NO_UPDATE);
 
 	/* Set ring address */
-	DRM_INFO("radeon: ring at 0x%016lX\n", (unsigned long)rdev->cp.gpu_addr);
-	WREG32(RADEON_CP_RB_BASE, rdev->cp.gpu_addr);
+	DRM_INFO("radeon: ring at 0x%016lX\n", (unsigned long)ring->gpu_addr);
+	WREG32(RADEON_CP_RB_BASE, ring->gpu_addr);
 	/* Force read & write ptr to 0 */
 	WREG32(RADEON_CP_RB_CNTL, tmp | RADEON_RB_RPTR_WR_ENA | RADEON_RB_NO_UPDATE);
 	WREG32(RADEON_CP_RB_RPTR_WR, 0);
-	rdev->cp.wptr = 0;
-	WREG32(RADEON_CP_RB_WPTR, rdev->cp.wptr);
+	ring->wptr = 0;
+	WREG32(RADEON_CP_RB_WPTR, ring->wptr);
 
 	/* set the wb address whether it's enabled or not */
 	WREG32(R_00070C_CP_RB_RPTR_ADDR,
@@ -1121,7 +1137,7 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 
 	WREG32(RADEON_CP_RB_CNTL, tmp);
 	udelay(10);
-	rdev->cp.rptr = RREG32(RADEON_CP_RB_RPTR);
+	ring->rptr = RREG32(RADEON_CP_RB_RPTR);
 	/* Set cp mode to bus mastering & enable cp*/
 	WREG32(RADEON_CP_CSQ_MODE,
 	       REG_SET(RADEON_INDIRECT2_START, indirect2_start) |
@@ -1130,12 +1146,12 @@ int r100_cp_init(struct radeon_device *rdev, unsigned ring_size)
 	WREG32(RADEON_CP_CSQ_MODE, 0x00004D4D);
 	WREG32(RADEON_CP_CSQ_CNTL, RADEON_CSQ_PRIBM_INDBM);
 	radeon_ring_start(rdev);
-	r = radeon_ring_test(rdev);
+	r = radeon_ring_test(rdev, ring);
 	if (r) {
 		DRM_ERROR("radeon: cp isn't working (%d).\n", r);
 		return r;
 	}
-	rdev->cp.ready = true;
+	ring->ready = true;
 	radeon_ttm_set_active_vram_size(rdev, rdev->mc.real_vram_size);
 	return 0;
 }
@@ -1147,7 +1163,7 @@ void r100_cp_fini(struct radeon_device *rdev)
 	}
 	/* Disable ring */
 	r100_cp_disable(rdev);
-	radeon_ring_fini(rdev);
+	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 	DRM_INFO("radeon: cp finalized\n");
 }
 
@@ -1155,7 +1171,7 @@ void r100_cp_disable(struct radeon_device *rdev)
 {
 	/* Disable ring */
 	radeon_ttm_set_active_vram_size(rdev, rdev->mc.visible_vram_size);
-	rdev->cp.ready = false;
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	WREG32(RADEON_CP_CSQ_MODE, 0);
 	WREG32(RADEON_CP_CSQ_CNTL, 0);
 	WREG32(R_000770_SCRATCH_UMSK, 0);
@@ -1165,13 +1181,6 @@ void r100_cp_disable(struct radeon_device *rdev)
 	}
 }
 
-void r100_cp_commit(struct radeon_device *rdev)
-{
-	WREG32(RADEON_CP_RB_WPTR, rdev->cp.wptr);
-	(void)RREG32(RADEON_CP_RB_WPTR);
-}
-
-
 /*
  * CS functions
  */
@@ -2099,9 +2108,9 @@ int r100_mc_wait_for_idle(struct radeon_device *rdev)
 	return -1;
 }
 
-void r100_gpu_lockup_update(struct r100_gpu_lockup *lockup, struct radeon_cp *cp)
+void r100_gpu_lockup_update(struct r100_gpu_lockup *lockup, struct radeon_ring *ring)
 {
-	lockup->last_cp_rptr = cp->rptr;
+	lockup->last_cp_rptr = ring->rptr;
 	lockup->last_jiffies = jiffies;
 }
 
@@ -2126,20 +2135,20 @@ void r100_gpu_lockup_update(struct r100_gpu_lockup *lockup, struct radeon_cp *cp
  * false positive when CP is just gived nothing to do.
  *
  **/
-bool r100_gpu_cp_is_lockup(struct radeon_device *rdev, struct r100_gpu_lockup *lockup, struct radeon_cp *cp)
+bool r100_gpu_cp_is_lockup(struct radeon_device *rdev, struct r100_gpu_lockup *lockup, struct radeon_ring *ring)
 {
 	unsigned long cjiffies, elapsed;
 
 	cjiffies = jiffies;
 	if (!time_after(cjiffies, lockup->last_jiffies)) {
 		/* likely a wrap around */
-		lockup->last_cp_rptr = cp->rptr;
+		lockup->last_cp_rptr = ring->rptr;
 		lockup->last_jiffies = jiffies;
 		return false;
 	}
-	if (cp->rptr != lockup->last_cp_rptr) {
+	if (ring->rptr != lockup->last_cp_rptr) {
 		/* CP is still working no lockup */
-		lockup->last_cp_rptr = cp->rptr;
+		lockup->last_cp_rptr = ring->rptr;
 		lockup->last_jiffies = jiffies;
 		return false;
 	}
@@ -2152,31 +2161,32 @@ bool r100_gpu_cp_is_lockup(struct radeon_device *rdev, struct r100_gpu_lockup *l
 	return false;
 }
 
-bool r100_gpu_is_lockup(struct radeon_device *rdev)
+bool r100_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 rbbm_status;
 	int r;
 
 	rbbm_status = RREG32(R_000E40_RBBM_STATUS);
 	if (!G_000E40_GUI_ACTIVE(rbbm_status)) {
-		r100_gpu_lockup_update(&rdev->config.r100.lockup, &rdev->cp);
+		r100_gpu_lockup_update(&rdev->config.r100.lockup, ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, 2);
+	r = radeon_ring_lock(rdev, ring, 2);
 	if (!r) {
 		/* PACKET2 NOP */
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_unlock_commit(rdev);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_unlock_commit(rdev, ring);
 	}
-	rdev->cp.rptr = RREG32(RADEON_CP_RB_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, &rdev->config.r100.lockup, &rdev->cp);
+	ring->rptr = RREG32(ring->rptr_reg);
+	return r100_gpu_cp_is_lockup(rdev, &rdev->config.r100.lockup, ring);
 }
 
 void r100_bm_disable(struct radeon_device *rdev)
 {
 	u32 tmp;
+	u16 tmp16;
 
 	/* disable bus mastering */
 	tmp = RREG32(R_000030_BUS_CNTL);
@@ -2187,8 +2197,8 @@ void r100_bm_disable(struct radeon_device *rdev)
 	WREG32(R_000030_BUS_CNTL, (tmp & 0xFFFFFFFF) | 0x00000040);
 	tmp = RREG32(RADEON_BUS_CNTL);
 	mdelay(1);
-	pci_read_config_word(rdev->pdev, 0x4, (u16*)&tmp);
-	pci_write_config_word(rdev->pdev, 0x4, tmp & 0xFFFB);
+	pci_read_config_word(rdev->pdev, 0x4, &tmp16);
+	pci_write_config_word(rdev->pdev, 0x4, tmp16 & 0xFFFB);
 	mdelay(1);
 }
 
@@ -2579,21 +2589,22 @@ static int r100_debugfs_cp_ring_info(struct seq_file *m, void *data)
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
 	struct drm_device *dev = node->minor->dev;
 	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	uint32_t rdp, wdp;
 	unsigned count, i, j;
 
-	radeon_ring_free_size(rdev);
+	radeon_ring_free_size(rdev, ring);
 	rdp = RREG32(RADEON_CP_RB_RPTR);
 	wdp = RREG32(RADEON_CP_RB_WPTR);
-	count = (rdp + rdev->cp.ring_size - wdp) & rdev->cp.ptr_mask;
+	count = (rdp + ring->ring_size - wdp) & ring->ptr_mask;
 	seq_printf(m, "CP_STAT 0x%08x\n", RREG32(RADEON_CP_STAT));
 	seq_printf(m, "CP_RB_WPTR 0x%08x\n", wdp);
 	seq_printf(m, "CP_RB_RPTR 0x%08x\n", rdp);
-	seq_printf(m, "%u free dwords in ring\n", rdev->cp.ring_free_dw);
+	seq_printf(m, "%u free dwords in ring\n", ring->ring_free_dw);
 	seq_printf(m, "%u dwords in ring\n", count);
 	for (j = 0; j <= count; j++) {
-		i = (rdp + j) & rdev->cp.ptr_mask;
-		seq_printf(m, "r[%04d]=0x%08x\n", i, rdev->cp.ring[i]);
+		i = (rdp + j) & ring->ptr_mask;
+		seq_printf(m, "r[%04d]=0x%08x\n", i, ring->ring[i]);
 	}
 	return 0;
 }
@@ -3635,7 +3646,7 @@ void r100_cs_track_clear(struct radeon_device *rdev, struct r100_cs_track *track
 	}
 }
 
-int r100_ring_test(struct radeon_device *rdev)
+int r100_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	uint32_t scratch;
 	uint32_t tmp = 0;
@@ -3648,15 +3659,15 @@ int r100_ring_test(struct radeon_device *rdev)
 		return r;
 	}
 	WREG32(scratch, 0xCAFEDEAD);
-	r = radeon_ring_lock(rdev, 2);
+	r = radeon_ring_lock(rdev, ring, 2);
 	if (r) {
 		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
 		radeon_scratch_free(rdev, scratch);
 		return r;
 	}
-	radeon_ring_write(rdev, PACKET0(scratch, 0));
-	radeon_ring_write(rdev, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_write(ring, PACKET0(scratch, 0));
+	radeon_ring_write(ring, 0xDEADBEEF);
+	radeon_ring_unlock_commit(rdev, ring);
 	for (i = 0; i < rdev->usec_timeout; i++) {
 		tmp = RREG32(scratch);
 		if (tmp == 0xDEADBEEF) {
@@ -3677,9 +3688,11 @@ int r100_ring_test(struct radeon_device *rdev)
 
 void r100_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	radeon_ring_write(rdev, PACKET0(RADEON_CP_IB_BASE, 1));
-	radeon_ring_write(rdev, ib->gpu_addr);
-	radeon_ring_write(rdev, ib->length_dw);
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+
+	radeon_ring_write(ring, PACKET0(RADEON_CP_IB_BASE, 1));
+	radeon_ring_write(ring, ib->gpu_addr);
+	radeon_ring_write(ring, ib->length_dw);
 }
 
 int r100_ib_test(struct radeon_device *rdev)
@@ -3696,7 +3709,7 @@ int r100_ib_test(struct radeon_device *rdev)
 		return r;
 	}
 	WREG32(scratch, 0xCAFEDEAD);
-	r = radeon_ib_get(rdev, &ib);
+	r = radeon_ib_get(rdev, RADEON_RING_TYPE_GFX_INDEX, &ib, 256);
 	if (r) {
 		return r;
 	}
@@ -3740,34 +3753,16 @@ int r100_ib_test(struct radeon_device *rdev)
 
 void r100_ib_fini(struct radeon_device *rdev)
 {
+	radeon_ib_pool_suspend(rdev);
 	radeon_ib_pool_fini(rdev);
 }
 
-int r100_ib_init(struct radeon_device *rdev)
-{
-	int r;
-
-	r = radeon_ib_pool_init(rdev);
-	if (r) {
-		dev_err(rdev->dev, "failed initializing IB pool (%d).\n", r);
-		r100_ib_fini(rdev);
-		return r;
-	}
-	r = r100_ib_test(rdev);
-	if (r) {
-		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
-		r100_ib_fini(rdev);
-		return r;
-	}
-	return 0;
-}
-
 void r100_mc_stop(struct radeon_device *rdev, struct r100_mc_save *save)
 {
 	/* Shutdown CP we shouldn't need to do that but better be safe than
 	 * sorry
 	 */
-	rdev->cp.ready = false;
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	WREG32(R_000740_CP_CSQ_CNTL, 0);
 
 	/* Save few CRTC registers */
@@ -3905,6 +3900,12 @@ static int r100_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	r100_irq_set(rdev);
 	rdev->config.r100.hdp_cntl = RREG32(RADEON_HOST_PATH_CNTL);
@@ -3914,11 +3915,18 @@ static int r100_startup(struct radeon_device *rdev)
 		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
-	r = r100_ib_init(rdev);
+
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
 		return r;
 	}
+
 	return 0;
 }
 
@@ -3941,11 +3949,14 @@ int r100_resume(struct radeon_device *rdev)
 	r100_clock_startup(rdev);
 	/* Initialize surface registers */
 	radeon_surface_init(rdev);
+
+	rdev->accel_working = true;
 	return r100_startup(rdev);
 }
 
 int r100_suspend(struct radeon_device *rdev)
 {
+	radeon_ib_pool_suspend(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
 	r100_irq_disable(rdev);
@@ -4064,7 +4075,14 @@ int r100_init(struct radeon_device *rdev)
 			return r;
 	}
 	r100_set_safe_registers(rdev);
+
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = r100_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
diff --git a/drivers/gpu/drm/radeon/r200.c b/drivers/gpu/drm/radeon/r200.c
index a1f3ba063c2d..eba4cbfa78f6 100644
--- a/drivers/gpu/drm/radeon/r200.c
+++ b/drivers/gpu/drm/radeon/r200.c
@@ -87,6 +87,7 @@ int r200_copy_dma(struct radeon_device *rdev,
 		  unsigned num_gpu_pages,
 		  struct radeon_fence *fence)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	uint32_t size;
 	uint32_t cur_size;
 	int i, num_loops;
@@ -95,33 +96,33 @@ int r200_copy_dma(struct radeon_device *rdev,
 	/* radeon pitch is /64 */
 	size = num_gpu_pages << RADEON_GPU_PAGE_SHIFT;
 	num_loops = DIV_ROUND_UP(size, 0x1FFFFF);
-	r = radeon_ring_lock(rdev, num_loops * 4 + 64);
+	r = radeon_ring_lock(rdev, ring, num_loops * 4 + 64);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
 		return r;
 	}
 	/* Must wait for 2D idle & clean before DMA or hangs might happen */
-	radeon_ring_write(rdev, PACKET0(RADEON_WAIT_UNTIL, 0));
-	radeon_ring_write(rdev, (1 << 16));
+	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
+	radeon_ring_write(ring, (1 << 16));
 	for (i = 0; i < num_loops; i++) {
 		cur_size = size;
 		if (cur_size > 0x1FFFFF) {
 			cur_size = 0x1FFFFF;
 		}
 		size -= cur_size;
-		radeon_ring_write(rdev, PACKET0(0x720, 2));
-		radeon_ring_write(rdev, src_offset);
-		radeon_ring_write(rdev, dst_offset);
-		radeon_ring_write(rdev, cur_size | (1 << 31) | (1 << 30));
+		radeon_ring_write(ring, PACKET0(0x720, 2));
+		radeon_ring_write(ring, src_offset);
+		radeon_ring_write(ring, dst_offset);
+		radeon_ring_write(ring, cur_size | (1 << 31) | (1 << 30));
 		src_offset += cur_size;
 		dst_offset += cur_size;
 	}
-	radeon_ring_write(rdev, PACKET0(RADEON_WAIT_UNTIL, 0));
-	radeon_ring_write(rdev, RADEON_WAIT_DMA_GUI_IDLE);
+	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
+	radeon_ring_write(ring, RADEON_WAIT_DMA_GUI_IDLE);
 	if (fence) {
 		r = radeon_fence_emit(rdev, fence);
 	}
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_unlock_commit(rdev, ring);
 	return r;
 }
 
diff --git a/drivers/gpu/drm/radeon/r300.c b/drivers/gpu/drm/radeon/r300.c
index c93bc64707e1..3fc0d29a5f39 100644
--- a/drivers/gpu/drm/radeon/r300.c
+++ b/drivers/gpu/drm/radeon/r300.c
@@ -175,37 +175,40 @@ void rv370_pcie_gart_fini(struct radeon_device *rdev)
 void r300_fence_ring_emit(struct radeon_device *rdev,
 			  struct radeon_fence *fence)
 {
+	struct radeon_ring *ring = &rdev->ring[fence->ring];
+
 	/* Who ever call radeon_fence_emit should call ring_lock and ask
 	 * for enough space (today caller are ib schedule and buffer move) */
 	/* Write SC register so SC & US assert idle */
-	radeon_ring_write(rdev, PACKET0(R300_RE_SCISSORS_TL, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, PACKET0(R300_RE_SCISSORS_BR, 0));
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET0(R300_RE_SCISSORS_TL, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(R300_RE_SCISSORS_BR, 0));
+	radeon_ring_write(ring, 0);
 	/* Flush 3D cache */
-	radeon_ring_write(rdev, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, R300_RB3D_DC_FLUSH);
-	radeon_ring_write(rdev, PACKET0(R300_RB3D_ZCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, R300_ZC_FLUSH);
+	radeon_ring_write(ring, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, R300_RB3D_DC_FLUSH);
+	radeon_ring_write(ring, PACKET0(R300_RB3D_ZCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, R300_ZC_FLUSH);
 	/* Wait until IDLE & CLEAN */
-	radeon_ring_write(rdev, PACKET0(RADEON_WAIT_UNTIL, 0));
-	radeon_ring_write(rdev, (RADEON_WAIT_3D_IDLECLEAN |
+	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
+	radeon_ring_write(ring, (RADEON_WAIT_3D_IDLECLEAN |
 				 RADEON_WAIT_2D_IDLECLEAN |
 				 RADEON_WAIT_DMA_GUI_IDLE));
-	radeon_ring_write(rdev, PACKET0(RADEON_HOST_PATH_CNTL, 0));
-	radeon_ring_write(rdev, rdev->config.r300.hdp_cntl |
+	radeon_ring_write(ring, PACKET0(RADEON_HOST_PATH_CNTL, 0));
+	radeon_ring_write(ring, rdev->config.r300.hdp_cntl |
 				RADEON_HDP_READ_BUFFER_INVALIDATE);
-	radeon_ring_write(rdev, PACKET0(RADEON_HOST_PATH_CNTL, 0));
-	radeon_ring_write(rdev, rdev->config.r300.hdp_cntl);
+	radeon_ring_write(ring, PACKET0(RADEON_HOST_PATH_CNTL, 0));
+	radeon_ring_write(ring, rdev->config.r300.hdp_cntl);
 	/* Emit fence sequence & fire IRQ */
-	radeon_ring_write(rdev, PACKET0(rdev->fence_drv.scratch_reg, 0));
-	radeon_ring_write(rdev, fence->seq);
-	radeon_ring_write(rdev, PACKET0(RADEON_GEN_INT_STATUS, 0));
-	radeon_ring_write(rdev, RADEON_SW_INT_FIRE);
+	radeon_ring_write(ring, PACKET0(rdev->fence_drv[fence->ring].scratch_reg, 0));
+	radeon_ring_write(ring, fence->seq);
+	radeon_ring_write(ring, PACKET0(RADEON_GEN_INT_STATUS, 0));
+	radeon_ring_write(ring, RADEON_SW_INT_FIRE);
 }
 
 void r300_ring_start(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	unsigned gb_tile_config;
 	int r;
 
@@ -227,44 +230,44 @@ void r300_ring_start(struct radeon_device *rdev)
 		break;
 	}
 
-	r = radeon_ring_lock(rdev, 64);
+	r = radeon_ring_lock(rdev, ring, 64);
 	if (r) {
 		return;
 	}
-	radeon_ring_write(rdev, PACKET0(RADEON_ISYNC_CNTL, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(RADEON_ISYNC_CNTL, 0));
+	radeon_ring_write(ring,
 			  RADEON_ISYNC_ANY2D_IDLE3D |
 			  RADEON_ISYNC_ANY3D_IDLE2D |
 			  RADEON_ISYNC_WAIT_IDLEGUI |
 			  RADEON_ISYNC_CPSCRATCH_IDLEGUI);
-	radeon_ring_write(rdev, PACKET0(R300_GB_TILE_CONFIG, 0));
-	radeon_ring_write(rdev, gb_tile_config);
-	radeon_ring_write(rdev, PACKET0(RADEON_WAIT_UNTIL, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(R300_GB_TILE_CONFIG, 0));
+	radeon_ring_write(ring, gb_tile_config);
+	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
+	radeon_ring_write(ring,
 			  RADEON_WAIT_2D_IDLECLEAN |
 			  RADEON_WAIT_3D_IDLECLEAN);
-	radeon_ring_write(rdev, PACKET0(R300_DST_PIPE_CONFIG, 0));
-	radeon_ring_write(rdev, R300_PIPE_AUTO_CONFIG);
-	radeon_ring_write(rdev, PACKET0(R300_GB_SELECT, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, PACKET0(R300_GB_ENABLE, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, R300_RB3D_DC_FLUSH | R300_RB3D_DC_FREE);
-	radeon_ring_write(rdev, PACKET0(R300_RB3D_ZCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, R300_ZC_FLUSH | R300_ZC_FREE);
-	radeon_ring_write(rdev, PACKET0(RADEON_WAIT_UNTIL, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(R300_DST_PIPE_CONFIG, 0));
+	radeon_ring_write(ring, R300_PIPE_AUTO_CONFIG);
+	radeon_ring_write(ring, PACKET0(R300_GB_SELECT, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(R300_GB_ENABLE, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, R300_RB3D_DC_FLUSH | R300_RB3D_DC_FREE);
+	radeon_ring_write(ring, PACKET0(R300_RB3D_ZCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, R300_ZC_FLUSH | R300_ZC_FREE);
+	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
+	radeon_ring_write(ring,
 			  RADEON_WAIT_2D_IDLECLEAN |
 			  RADEON_WAIT_3D_IDLECLEAN);
-	radeon_ring_write(rdev, PACKET0(R300_GB_AA_CONFIG, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, R300_RB3D_DC_FLUSH | R300_RB3D_DC_FREE);
-	radeon_ring_write(rdev, PACKET0(R300_RB3D_ZCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, R300_ZC_FLUSH | R300_ZC_FREE);
-	radeon_ring_write(rdev, PACKET0(R300_GB_MSPOS0, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(R300_GB_AA_CONFIG, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, R300_RB3D_DC_FLUSH | R300_RB3D_DC_FREE);
+	radeon_ring_write(ring, PACKET0(R300_RB3D_ZCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, R300_ZC_FLUSH | R300_ZC_FREE);
+	radeon_ring_write(ring, PACKET0(R300_GB_MSPOS0, 0));
+	radeon_ring_write(ring,
 			  ((6 << R300_MS_X0_SHIFT) |
 			   (6 << R300_MS_Y0_SHIFT) |
 			   (6 << R300_MS_X1_SHIFT) |
@@ -273,8 +276,8 @@ void r300_ring_start(struct radeon_device *rdev)
 			   (6 << R300_MS_Y2_SHIFT) |
 			   (6 << R300_MSBD0_Y_SHIFT) |
 			   (6 << R300_MSBD0_X_SHIFT)));
-	radeon_ring_write(rdev, PACKET0(R300_GB_MSPOS1, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(R300_GB_MSPOS1, 0));
+	radeon_ring_write(ring,
 			  ((6 << R300_MS_X3_SHIFT) |
 			   (6 << R300_MS_Y3_SHIFT) |
 			   (6 << R300_MS_X4_SHIFT) |
@@ -282,16 +285,16 @@ void r300_ring_start(struct radeon_device *rdev)
 			   (6 << R300_MS_X5_SHIFT) |
 			   (6 << R300_MS_Y5_SHIFT) |
 			   (6 << R300_MSBD1_SHIFT)));
-	radeon_ring_write(rdev, PACKET0(R300_GA_ENHANCE, 0));
-	radeon_ring_write(rdev, R300_GA_DEADLOCK_CNTL | R300_GA_FASTSYNC_CNTL);
-	radeon_ring_write(rdev, PACKET0(R300_GA_POLY_MODE, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(R300_GA_ENHANCE, 0));
+	radeon_ring_write(ring, R300_GA_DEADLOCK_CNTL | R300_GA_FASTSYNC_CNTL);
+	radeon_ring_write(ring, PACKET0(R300_GA_POLY_MODE, 0));
+	radeon_ring_write(ring,
 			  R300_FRONT_PTYPE_TRIANGE | R300_BACK_PTYPE_TRIANGE);
-	radeon_ring_write(rdev, PACKET0(R300_GA_ROUND_MODE, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(R300_GA_ROUND_MODE, 0));
+	radeon_ring_write(ring,
 			  R300_GEOMETRY_ROUND_NEAREST |
 			  R300_COLOR_ROUND_NEAREST);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_unlock_commit(rdev, ring);
 }
 
 void r300_errata(struct radeon_device *rdev)
@@ -375,26 +378,26 @@ void r300_gpu_init(struct radeon_device *rdev)
 		 rdev->num_gb_pipes, rdev->num_z_pipes);
 }
 
-bool r300_gpu_is_lockup(struct radeon_device *rdev)
+bool r300_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 rbbm_status;
 	int r;
 
 	rbbm_status = RREG32(R_000E40_RBBM_STATUS);
 	if (!G_000E40_GUI_ACTIVE(rbbm_status)) {
-		r100_gpu_lockup_update(&rdev->config.r300.lockup, &rdev->cp);
+		r100_gpu_lockup_update(&rdev->config.r300.lockup, ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, 2);
+	r = radeon_ring_lock(rdev, ring, 2);
 	if (!r) {
 		/* PACKET2 NOP */
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_unlock_commit(rdev);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_unlock_commit(rdev, ring);
 	}
-	rdev->cp.rptr = RREG32(RADEON_CP_RB_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, &rdev->config.r300.lockup, &rdev->cp);
+	ring->rptr = RREG32(RADEON_CP_RB_RPTR);
+	return r100_gpu_cp_is_lockup(rdev, &rdev->config.r300.lockup, ring);
 }
 
 int r300_asic_reset(struct radeon_device *rdev)
@@ -701,7 +704,7 @@ static int r300_packet0_check(struct radeon_cs_parser *p,
 			return r;
 		}
 
-		if (p->keep_tiling_flags) {
+		if (p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS) {
 			ib[idx] = (idx_value & 31) | /* keep the 1st 5 bits */
 				  ((idx_value & ~31) + (u32)reloc->lobj.gpu_offset);
 		} else {
@@ -765,7 +768,7 @@ static int r300_packet0_check(struct radeon_cs_parser *p,
 		/* RB3D_COLORPITCH1 */
 		/* RB3D_COLORPITCH2 */
 		/* RB3D_COLORPITCH3 */
-		if (!p->keep_tiling_flags) {
+		if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
 			r = r100_cs_packet_next_reloc(p, &reloc);
 			if (r) {
 				DRM_ERROR("No reloc for ib[%d]=0x%04X\n",
@@ -850,7 +853,7 @@ static int r300_packet0_check(struct radeon_cs_parser *p,
 		break;
 	case 0x4F24:
 		/* ZB_DEPTHPITCH */
-		if (!p->keep_tiling_flags) {
+		if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
 			r = r100_cs_packet_next_reloc(p, &reloc);
 			if (r) {
 				DRM_ERROR("No reloc for ib[%d]=0x%04X\n",
@@ -1396,6 +1399,12 @@ static int r300_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	r100_irq_set(rdev);
 	rdev->config.r300.hdp_cntl = RREG32(RADEON_HOST_PATH_CNTL);
@@ -1405,11 +1414,18 @@ static int r300_startup(struct radeon_device *rdev)
 		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
-	r = r100_ib_init(rdev);
+
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
 		return r;
 	}
+
 	return 0;
 }
 
@@ -1434,11 +1450,14 @@ int r300_resume(struct radeon_device *rdev)
 	r300_clock_startup(rdev);
 	/* Initialize surface registers */
 	radeon_surface_init(rdev);
+
+	rdev->accel_working = true;
 	return r300_startup(rdev);
 }
 
 int r300_suspend(struct radeon_device *rdev)
 {
+	radeon_ib_pool_suspend(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
 	r100_irq_disable(rdev);
@@ -1539,7 +1558,14 @@ int r300_init(struct radeon_device *rdev)
 			return r;
 	}
 	r300_set_reg_safe(rdev);
+
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = r300_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
diff --git a/drivers/gpu/drm/radeon/r420.c b/drivers/gpu/drm/radeon/r420.c
index 417fab81812f..666e28fe509c 100644
--- a/drivers/gpu/drm/radeon/r420.c
+++ b/drivers/gpu/drm/radeon/r420.c
@@ -199,6 +199,8 @@ static void r420_clock_resume(struct radeon_device *rdev)
 
 static void r420_cp_errata_init(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+
 	/* RV410 and R420 can lock up if CP DMA to host memory happens
 	 * while the 2D engine is busy.
 	 *
@@ -206,22 +208,24 @@ static void r420_cp_errata_init(struct radeon_device *rdev)
 	 * of the CP init, apparently.
 	 */
 	radeon_scratch_get(rdev, &rdev->config.r300.resync_scratch);
-	radeon_ring_lock(rdev, 8);
-	radeon_ring_write(rdev, PACKET0(R300_CP_RESYNC_ADDR, 1));
-	radeon_ring_write(rdev, rdev->config.r300.resync_scratch);
-	radeon_ring_write(rdev, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_lock(rdev, ring, 8);
+	radeon_ring_write(ring, PACKET0(R300_CP_RESYNC_ADDR, 1));
+	radeon_ring_write(ring, rdev->config.r300.resync_scratch);
+	radeon_ring_write(ring, 0xDEADBEEF);
+	radeon_ring_unlock_commit(rdev, ring);
 }
 
 static void r420_cp_errata_fini(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+
 	/* Catch the RESYNC we dispatched all the way back,
 	 * at the very beginning of the CP init.
 	 */
-	radeon_ring_lock(rdev, 8);
-	radeon_ring_write(rdev, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, R300_RB3D_DC_FINISH);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_lock(rdev, ring, 8);
+	radeon_ring_write(ring, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, R300_RB3D_DC_FINISH);
+	radeon_ring_unlock_commit(rdev, ring);
 	radeon_scratch_free(rdev, rdev->config.r300.resync_scratch);
 }
 
@@ -254,6 +258,12 @@ static int r420_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	r100_irq_set(rdev);
 	rdev->config.r300.hdp_cntl = RREG32(RADEON_HOST_PATH_CNTL);
@@ -264,11 +274,18 @@ static int r420_startup(struct radeon_device *rdev)
 		return r;
 	}
 	r420_cp_errata_init(rdev);
-	r = r100_ib_init(rdev);
+
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
 		return r;
 	}
+
 	return 0;
 }
 
@@ -297,11 +314,14 @@ int r420_resume(struct radeon_device *rdev)
 	r420_clock_resume(rdev);
 	/* Initialize surface registers */
 	radeon_surface_init(rdev);
+
+	rdev->accel_working = true;
 	return r420_startup(rdev);
 }
 
 int r420_suspend(struct radeon_device *rdev)
 {
+	radeon_ib_pool_suspend(rdev);
 	r420_cp_errata_fini(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
@@ -414,7 +434,14 @@ int r420_init(struct radeon_device *rdev)
 			return r;
 	}
 	r420_set_reg_safe(rdev);
+
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = r420_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
diff --git a/drivers/gpu/drm/radeon/r500_reg.h b/drivers/gpu/drm/radeon/r500_reg.h
index fc437059918f..3bd8f1b1c606 100644
--- a/drivers/gpu/drm/radeon/r500_reg.h
+++ b/drivers/gpu/drm/radeon/r500_reg.h
@@ -573,6 +573,7 @@
 
 #define AVIVO_TMDSA_CNTL                    0x7880
 #   define AVIVO_TMDSA_CNTL_ENABLE               (1 << 0)
+#   define AVIVO_TMDSA_CNTL_HDMI_EN              (1 << 2)
 #   define AVIVO_TMDSA_CNTL_HPD_MASK             (1 << 4)
 #   define AVIVO_TMDSA_CNTL_HPD_SELECT           (1 << 8)
 #   define AVIVO_TMDSA_CNTL_SYNC_PHASE           (1 << 12)
@@ -633,6 +634,7 @@
 
 #define AVIVO_LVTMA_CNTL					0x7a80
 #   define AVIVO_LVTMA_CNTL_ENABLE               (1 << 0)
+#   define AVIVO_LVTMA_CNTL_HDMI_EN              (1 << 2)
 #   define AVIVO_LVTMA_CNTL_HPD_MASK             (1 << 4)
 #   define AVIVO_LVTMA_CNTL_HPD_SELECT           (1 << 8)
 #   define AVIVO_LVTMA_CNTL_SYNC_PHASE           (1 << 12)
diff --git a/drivers/gpu/drm/radeon/r520.c b/drivers/gpu/drm/radeon/r520.c
index 3081d07f8de5..4ae1615e752f 100644
--- a/drivers/gpu/drm/radeon/r520.c
+++ b/drivers/gpu/drm/radeon/r520.c
@@ -187,6 +187,12 @@ static int r520_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	rs600_irq_set(rdev);
 	rdev->config.r300.hdp_cntl = RREG32(RADEON_HOST_PATH_CNTL);
@@ -196,9 +202,15 @@ static int r520_startup(struct radeon_device *rdev)
 		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
-	r = r100_ib_init(rdev);
+
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
 		return r;
 	}
 	return 0;
@@ -223,6 +235,8 @@ int r520_resume(struct radeon_device *rdev)
 	rv515_clock_startup(rdev);
 	/* Initialize surface registers */
 	radeon_surface_init(rdev);
+
+	rdev->accel_working = true;
 	return r520_startup(rdev);
 }
 
@@ -292,7 +306,14 @@ int r520_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 	rv515_set_safe_registers(rdev);
+
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = r520_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 9cdda0b3b081..4f08e5e6ee9d 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -1344,7 +1344,7 @@ int r600_gpu_soft_reset(struct radeon_device *rdev)
 	return 0;
 }
 
-bool r600_gpu_is_lockup(struct radeon_device *rdev)
+bool r600_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	u32 srbm_status;
 	u32 grbm_status;
@@ -1361,19 +1361,19 @@ bool r600_gpu_is_lockup(struct radeon_device *rdev)
 	grbm_status = RREG32(R_008010_GRBM_STATUS);
 	grbm_status2 = RREG32(R_008014_GRBM_STATUS2);
 	if (!G_008010_GUI_ACTIVE(grbm_status)) {
-		r100_gpu_lockup_update(lockup, &rdev->cp);
+		r100_gpu_lockup_update(lockup, ring);
 		return false;
 	}
 	/* force CP activities */
-	r = radeon_ring_lock(rdev, 2);
+	r = radeon_ring_lock(rdev, ring, 2);
 	if (!r) {
 		/* PACKET2 NOP */
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_write(rdev, 0x80000000);
-		radeon_ring_unlock_commit(rdev);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_write(ring, 0x80000000);
+		radeon_ring_unlock_commit(rdev, ring);
 	}
-	rdev->cp.rptr = RREG32(R600_CP_RB_RPTR);
-	return r100_gpu_cp_is_lockup(rdev, lockup, &rdev->cp);
+	ring->rptr = RREG32(ring->rptr_reg);
+	return r100_gpu_cp_is_lockup(rdev, lockup, ring);
 }
 
 int r600_asic_reset(struct radeon_device *rdev)
@@ -2144,27 +2144,28 @@ static int r600_cp_load_microcode(struct radeon_device *rdev)
 
 int r600_cp_start(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 	uint32_t cp_me;
 
-	r = radeon_ring_lock(rdev, 7);
+	r = radeon_ring_lock(rdev, ring, 7);
 	if (r) {
 		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
 		return r;
 	}
-	radeon_ring_write(rdev, PACKET3(PACKET3_ME_INITIALIZE, 5));
-	radeon_ring_write(rdev, 0x1);
+	radeon_ring_write(ring, PACKET3(PACKET3_ME_INITIALIZE, 5));
+	radeon_ring_write(ring, 0x1);
 	if (rdev->family >= CHIP_RV770) {
-		radeon_ring_write(rdev, 0x0);
-		radeon_ring_write(rdev, rdev->config.rv770.max_hw_contexts - 1);
+		radeon_ring_write(ring, 0x0);
+		radeon_ring_write(ring, rdev->config.rv770.max_hw_contexts - 1);
 	} else {
-		radeon_ring_write(rdev, 0x3);
-		radeon_ring_write(rdev, rdev->config.r600.max_hw_contexts - 1);
+		radeon_ring_write(ring, 0x3);
+		radeon_ring_write(ring, rdev->config.r600.max_hw_contexts - 1);
 	}
-	radeon_ring_write(rdev, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_write(ring, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_unlock_commit(rdev, ring);
 
 	cp_me = 0xff;
 	WREG32(R_0086D8_CP_ME_CNTL, cp_me);
@@ -2173,6 +2174,7 @@ int r600_cp_start(struct radeon_device *rdev)
 
 int r600_cp_resume(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 tmp;
 	u32 rb_bufsz;
 	int r;
@@ -2184,13 +2186,13 @@ int r600_cp_resume(struct radeon_device *rdev)
 	WREG32(GRBM_SOFT_RESET, 0);
 
 	/* Set ring buffer size */
-	rb_bufsz = drm_order(rdev->cp.ring_size / 8);
+	rb_bufsz = drm_order(ring->ring_size / 8);
 	tmp = (drm_order(RADEON_GPU_PAGE_SIZE/8) << 8) | rb_bufsz;
 #ifdef __BIG_ENDIAN
 	tmp |= BUF_SWAP_32BIT;
 #endif
 	WREG32(CP_RB_CNTL, tmp);
-	WREG32(CP_SEM_WAIT_TIMER, 0x4);
+	WREG32(CP_SEM_WAIT_TIMER, 0x0);
 
 	/* Set the write pointer delay */
 	WREG32(CP_RB_WPTR_DELAY, 0);
@@ -2198,8 +2200,8 @@ int r600_cp_resume(struct radeon_device *rdev)
 	/* Initialize the ring buffer's read and write pointers */
 	WREG32(CP_RB_CNTL, tmp | RB_RPTR_WR_ENA);
 	WREG32(CP_RB_RPTR_WR, 0);
-	rdev->cp.wptr = 0;
-	WREG32(CP_RB_WPTR, rdev->cp.wptr);
+	ring->wptr = 0;
+	WREG32(CP_RB_WPTR, ring->wptr);
 
 	/* set the wb address whether it's enabled or not */
 	WREG32(CP_RB_RPTR_ADDR,
@@ -2217,42 +2219,36 @@ int r600_cp_resume(struct radeon_device *rdev)
 	mdelay(1);
 	WREG32(CP_RB_CNTL, tmp);
 
-	WREG32(CP_RB_BASE, rdev->cp.gpu_addr >> 8);
+	WREG32(CP_RB_BASE, ring->gpu_addr >> 8);
 	WREG32(CP_DEBUG, (1 << 27) | (1 << 28));
 
-	rdev->cp.rptr = RREG32(CP_RB_RPTR);
+	ring->rptr = RREG32(CP_RB_RPTR);
 
 	r600_cp_start(rdev);
-	rdev->cp.ready = true;
-	r = radeon_ring_test(rdev);
+	ring->ready = true;
+	r = radeon_ring_test(rdev, ring);
 	if (r) {
-		rdev->cp.ready = false;
+		ring->ready = false;
 		return r;
 	}
 	return 0;
 }
 
-void r600_cp_commit(struct radeon_device *rdev)
-{
-	WREG32(CP_RB_WPTR, rdev->cp.wptr);
-	(void)RREG32(CP_RB_WPTR);
-}
-
-void r600_ring_init(struct radeon_device *rdev, unsigned ring_size)
+void r600_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ring_size)
 {
 	u32 rb_bufsz;
 
 	/* Align ring size */
 	rb_bufsz = drm_order(ring_size / 8);
 	ring_size = (1 << (rb_bufsz + 1)) * 4;
-	rdev->cp.ring_size = ring_size;
-	rdev->cp.align_mask = 16 - 1;
+	ring->ring_size = ring_size;
+	ring->align_mask = 16 - 1;
 }
 
 void r600_cp_fini(struct radeon_device *rdev)
 {
 	r600_cp_stop(rdev);
-	radeon_ring_fini(rdev);
+	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 }
 
 
@@ -2271,11 +2267,11 @@ void r600_scratch_init(struct radeon_device *rdev)
 	}
 }
 
-int r600_ring_test(struct radeon_device *rdev)
+int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	uint32_t scratch;
 	uint32_t tmp = 0;
-	unsigned i;
+	unsigned i, ridx = radeon_ring_index(rdev, ring);
 	int r;
 
 	r = radeon_scratch_get(rdev, &scratch);
@@ -2284,16 +2280,16 @@ int r600_ring_test(struct radeon_device *rdev)
 		return r;
 	}
 	WREG32(scratch, 0xCAFEDEAD);
-	r = radeon_ring_lock(rdev, 3);
+	r = radeon_ring_lock(rdev, ring, 3);
 	if (r) {
-		DRM_ERROR("radeon: cp failed to lock ring (%d).\n", r);
+		DRM_ERROR("radeon: cp failed to lock ring %d (%d).\n", ridx, r);
 		radeon_scratch_free(rdev, scratch);
 		return r;
 	}
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-	radeon_ring_write(rdev, ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2));
-	radeon_ring_write(rdev, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+	radeon_ring_write(ring, ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2));
+	radeon_ring_write(ring, 0xDEADBEEF);
+	radeon_ring_unlock_commit(rdev, ring);
 	for (i = 0; i < rdev->usec_timeout; i++) {
 		tmp = RREG32(scratch);
 		if (tmp == 0xDEADBEEF)
@@ -2301,10 +2297,10 @@ int r600_ring_test(struct radeon_device *rdev)
 		DRM_UDELAY(1);
 	}
 	if (i < rdev->usec_timeout) {
-		DRM_INFO("ring test succeeded in %d usecs\n", i);
+		DRM_INFO("ring test on %d succeeded in %d usecs\n", ridx, i);
 	} else {
-		DRM_ERROR("radeon: ring test failed (scratch(0x%04X)=0x%08X)\n",
-			  scratch, tmp);
+		DRM_ERROR("radeon: ring %d test failed (scratch(0x%04X)=0x%08X)\n",
+			  ridx, scratch, tmp);
 		r = -EINVAL;
 	}
 	radeon_scratch_free(rdev, scratch);
@@ -2314,49 +2310,63 @@ int r600_ring_test(struct radeon_device *rdev)
 void r600_fence_ring_emit(struct radeon_device *rdev,
 			  struct radeon_fence *fence)
 {
+	struct radeon_ring *ring = &rdev->ring[fence->ring];
+
 	if (rdev->wb.use_event) {
-		u64 addr = rdev->wb.gpu_addr + R600_WB_EVENT_OFFSET +
-			(u64)(rdev->fence_drv.scratch_reg - rdev->scratch.reg_base);
+		u64 addr = rdev->fence_drv[fence->ring].gpu_addr;
 		/* flush read cache over gart */
-		radeon_ring_write(rdev, PACKET3(PACKET3_SURFACE_SYNC, 3));
-		radeon_ring_write(rdev, PACKET3_TC_ACTION_ENA |
+		radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
+		radeon_ring_write(ring, PACKET3_TC_ACTION_ENA |
 					PACKET3_VC_ACTION_ENA |
 					PACKET3_SH_ACTION_ENA);
-		radeon_ring_write(rdev, 0xFFFFFFFF);
-		radeon_ring_write(rdev, 0);
-		radeon_ring_write(rdev, 10); /* poll interval */
+		radeon_ring_write(ring, 0xFFFFFFFF);
+		radeon_ring_write(ring, 0);
+		radeon_ring_write(ring, 10); /* poll interval */
 		/* EVENT_WRITE_EOP - flush caches, send int */
-		radeon_ring_write(rdev, PACKET3(PACKET3_EVENT_WRITE_EOP, 4));
-		radeon_ring_write(rdev, EVENT_TYPE(CACHE_FLUSH_AND_INV_EVENT_TS) | EVENT_INDEX(5));
-		radeon_ring_write(rdev, addr & 0xffffffff);
-		radeon_ring_write(rdev, (upper_32_bits(addr) & 0xff) | DATA_SEL(1) | INT_SEL(2));
-		radeon_ring_write(rdev, fence->seq);
-		radeon_ring_write(rdev, 0);
+		radeon_ring_write(ring, PACKET3(PACKET3_EVENT_WRITE_EOP, 4));
+		radeon_ring_write(ring, EVENT_TYPE(CACHE_FLUSH_AND_INV_EVENT_TS) | EVENT_INDEX(5));
+		radeon_ring_write(ring, addr & 0xffffffff);
+		radeon_ring_write(ring, (upper_32_bits(addr) & 0xff) | DATA_SEL(1) | INT_SEL(2));
+		radeon_ring_write(ring, fence->seq);
+		radeon_ring_write(ring, 0);
 	} else {
 		/* flush read cache over gart */
-		radeon_ring_write(rdev, PACKET3(PACKET3_SURFACE_SYNC, 3));
-		radeon_ring_write(rdev, PACKET3_TC_ACTION_ENA |
+		radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
+		radeon_ring_write(ring, PACKET3_TC_ACTION_ENA |
 					PACKET3_VC_ACTION_ENA |
 					PACKET3_SH_ACTION_ENA);
-		radeon_ring_write(rdev, 0xFFFFFFFF);
-		radeon_ring_write(rdev, 0);
-		radeon_ring_write(rdev, 10); /* poll interval */
-		radeon_ring_write(rdev, PACKET3(PACKET3_EVENT_WRITE, 0));
-		radeon_ring_write(rdev, EVENT_TYPE(CACHE_FLUSH_AND_INV_EVENT) | EVENT_INDEX(0));
+		radeon_ring_write(ring, 0xFFFFFFFF);
+		radeon_ring_write(ring, 0);
+		radeon_ring_write(ring, 10); /* poll interval */
+		radeon_ring_write(ring, PACKET3(PACKET3_EVENT_WRITE, 0));
+		radeon_ring_write(ring, EVENT_TYPE(CACHE_FLUSH_AND_INV_EVENT) | EVENT_INDEX(0));
 		/* wait for 3D idle clean */
-		radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-		radeon_ring_write(rdev, (WAIT_UNTIL - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
-		radeon_ring_write(rdev, WAIT_3D_IDLE_bit | WAIT_3D_IDLECLEAN_bit);
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, (WAIT_UNTIL - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
+		radeon_ring_write(ring, WAIT_3D_IDLE_bit | WAIT_3D_IDLECLEAN_bit);
 		/* Emit fence sequence & fire IRQ */
-		radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-		radeon_ring_write(rdev, ((rdev->fence_drv.scratch_reg - PACKET3_SET_CONFIG_REG_OFFSET) >> 2));
-		radeon_ring_write(rdev, fence->seq);
+		radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+		radeon_ring_write(ring, ((rdev->fence_drv[fence->ring].scratch_reg - PACKET3_SET_CONFIG_REG_OFFSET) >> 2));
+		radeon_ring_write(ring, fence->seq);
 		/* CP_INTERRUPT packet 3 no longer exists, use packet 0 */
-		radeon_ring_write(rdev, PACKET0(CP_INT_STATUS, 0));
-		radeon_ring_write(rdev, RB_INT_STAT);
+		radeon_ring_write(ring, PACKET0(CP_INT_STATUS, 0));
+		radeon_ring_write(ring, RB_INT_STAT);
 	}
 }
 
+void r600_semaphore_ring_emit(struct radeon_device *rdev,
+			      struct radeon_ring *ring,
+			      struct radeon_semaphore *semaphore,
+			      bool emit_wait)
+{
+	uint64_t addr = semaphore->gpu_addr;
+	unsigned sel = emit_wait ? PACKET3_SEM_SEL_WAIT : PACKET3_SEM_SEL_SIGNAL;
+
+	radeon_ring_write(ring, PACKET3(PACKET3_MEM_SEMAPHORE, 1));
+	radeon_ring_write(ring, addr & 0xffffffff);
+	radeon_ring_write(ring, (upper_32_bits(addr) & 0xff) | sel);
+}
+
 int r600_copy_blit(struct radeon_device *rdev,
 		   uint64_t src_offset,
 		   uint64_t dst_offset,
@@ -2409,6 +2419,7 @@ void r600_clear_surface_reg(struct radeon_device *rdev, int reg)
 
 int r600_startup(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
 	/* enable pcie gen2 link */
@@ -2447,6 +2458,12 @@ int r600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	r = r600_irq_init(rdev);
 	if (r) {
@@ -2456,7 +2473,10 @@ int r600_startup(struct radeon_device *rdev)
 	}
 	r600_irq_set(rdev);
 
-	r = radeon_ring_init(rdev, rdev->cp.ring_size);
+	r = radeon_ring_init(rdev, ring, ring->ring_size, RADEON_WB_CP_RPTR_OFFSET,
+			     R600_CP_RB_RPTR, R600_CP_RB_WPTR,
+			     0, 0xfffff, RADEON_CP_PACKET2);
+
 	if (r)
 		return r;
 	r = r600_cp_load_microcode(rdev);
@@ -2466,6 +2486,17 @@ int r600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r600_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
+		return r;
+	}
+
 	return 0;
 }
 
@@ -2494,18 +2525,13 @@ int r600_resume(struct radeon_device *rdev)
 	/* post card */
 	atom_asic_init(rdev->mode_info.atom_context);
 
+	rdev->accel_working = true;
 	r = r600_startup(rdev);
 	if (r) {
 		DRM_ERROR("r600 startup failed on resume\n");
 		return r;
 	}
 
-	r = r600_ib_test(rdev);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		return r;
-	}
-
 	r = r600_audio_init(rdev);
 	if (r) {
 		DRM_ERROR("radeon: audio resume failed\n");
@@ -2518,13 +2544,14 @@ int r600_resume(struct radeon_device *rdev)
 int r600_suspend(struct radeon_device *rdev)
 {
 	r600_audio_fini(rdev);
+	radeon_ib_pool_suspend(rdev);
+	r600_blit_suspend(rdev);
 	/* FIXME: we should wait for ring to be empty */
 	r600_cp_stop(rdev);
-	rdev->cp.ready = false;
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	r600_irq_suspend(rdev);
 	radeon_wb_disable(rdev);
 	r600_pcie_gart_disable(rdev);
-	r600_blit_suspend(rdev);
 
 	return 0;
 }
@@ -2595,8 +2622,8 @@ int r600_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	rdev->cp.ring_obj = NULL;
-	r600_ring_init(rdev, 1024 * 1024);
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ring_obj = NULL;
+	r600_ring_init(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX], 1024 * 1024);
 
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
@@ -2605,30 +2632,24 @@ int r600_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = r600_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
 		r600_cp_fini(rdev);
 		r600_irq_fini(rdev);
 		radeon_wb_fini(rdev);
+		r100_ib_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		r600_pcie_gart_fini(rdev);
 		rdev->accel_working = false;
 	}
-	if (rdev->accel_working) {
-		r = radeon_ib_pool_init(rdev);
-		if (r) {
-			dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-			rdev->accel_working = false;
-		} else {
-			r = r600_ib_test(rdev);
-			if (r) {
-				dev_err(rdev->dev, "IB test failed (%d).\n", r);
-				rdev->accel_working = false;
-			}
-		}
-	}
 
 	r = r600_audio_init(rdev);
 	if (r)
@@ -2643,12 +2664,13 @@ void r600_fini(struct radeon_device *rdev)
 	r600_cp_fini(rdev);
 	r600_irq_fini(rdev);
 	radeon_wb_fini(rdev);
-	radeon_ib_pool_fini(rdev);
+	r100_ib_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	r600_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_gem_fini(rdev);
+	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_bo_fini(rdev);
 	radeon_atombios_fini(rdev);
@@ -2662,18 +2684,20 @@ void r600_fini(struct radeon_device *rdev)
  */
 void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
 {
+	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
+
 	/* FIXME: implement */
-	radeon_ring_write(rdev, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
+	radeon_ring_write(ring,
 #ifdef __BIG_ENDIAN
 			  (2 << 0) |
 #endif
 			  (ib->gpu_addr & 0xFFFFFFFC));
-	radeon_ring_write(rdev, upper_32_bits(ib->gpu_addr) & 0xFF);
-	radeon_ring_write(rdev, ib->length_dw);
+	radeon_ring_write(ring, upper_32_bits(ib->gpu_addr) & 0xFF);
+	radeon_ring_write(ring, ib->length_dw);
 }
 
-int r600_ib_test(struct radeon_device *rdev)
+int r600_ib_test(struct radeon_device *rdev, int ring)
 {
 	struct radeon_ib *ib;
 	uint32_t scratch;
@@ -2687,7 +2711,7 @@ int r600_ib_test(struct radeon_device *rdev)
 		return r;
 	}
 	WREG32(scratch, 0xCAFEDEAD);
-	r = radeon_ib_get(rdev, &ib);
+	r = radeon_ib_get(rdev, ring, &ib, 256);
 	if (r) {
 		DRM_ERROR("radeon: failed to get ib (%d).\n", r);
 		return r;
@@ -2728,7 +2752,7 @@ int r600_ib_test(struct radeon_device *rdev)
 		DRM_UDELAY(1);
 	}
 	if (i < rdev->usec_timeout) {
-		DRM_INFO("ib test succeeded in %u usecs\n", i);
+		DRM_INFO("ib test on ring %d succeeded in %u usecs\n", ib->fence->ring, i);
 	} else {
 		DRM_ERROR("radeon: ib test failed (scratch(0x%04X)=0x%08X)\n",
 			  scratch, tmp);
@@ -3075,7 +3099,7 @@ int r600_irq_set(struct radeon_device *rdev)
 		hpd3 = RREG32(DC_HOT_PLUG_DETECT3_INT_CONTROL) & ~DC_HPDx_INT_EN;
 	}
 
-	if (rdev->irq.sw_int) {
+	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
 		DRM_DEBUG("r600_irq_set: sw int\n");
 		cp_int_cntl |= RB_INT_ENABLE;
 		cp_int_cntl |= TIME_STAMP_INT_ENABLE;
@@ -3459,11 +3483,11 @@ restart_ih:
 		case 177: /* CP_INT in IB1 */
 		case 178: /* CP_INT in IB2 */
 			DRM_DEBUG("IH: CP int: 0x%08x\n", src_data);
-			radeon_fence_process(rdev);
+			radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
 			break;
 		case 181: /* CP EOP event */
 			DRM_DEBUG("IH: CP EOP\n");
-			radeon_fence_process(rdev);
+			radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
 			break;
 		case 233: /* GUI IDLE */
 			DRM_DEBUG("IH: GUI idle\n");
@@ -3496,30 +3520,6 @@ restart_ih:
  */
 #if defined(CONFIG_DEBUG_FS)
 
-static int r600_debugfs_cp_ring_info(struct seq_file *m, void *data)
-{
-	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct drm_device *dev = node->minor->dev;
-	struct radeon_device *rdev = dev->dev_private;
-	unsigned count, i, j;
-
-	radeon_ring_free_size(rdev);
-	count = (rdev->cp.ring_size / 4) - rdev->cp.ring_free_dw;
-	seq_printf(m, "CP_STAT 0x%08x\n", RREG32(CP_STAT));
-	seq_printf(m, "CP_RB_WPTR 0x%08x\n", RREG32(CP_RB_WPTR));
-	seq_printf(m, "CP_RB_RPTR 0x%08x\n", RREG32(CP_RB_RPTR));
-	seq_printf(m, "driver's copy of the CP_RB_WPTR 0x%08x\n", rdev->cp.wptr);
-	seq_printf(m, "driver's copy of the CP_RB_RPTR 0x%08x\n", rdev->cp.rptr);
-	seq_printf(m, "%u free dwords in ring\n", rdev->cp.ring_free_dw);
-	seq_printf(m, "%u dwords in ring\n", count);
-	i = rdev->cp.rptr;
-	for (j = 0; j <= count; j++) {
-		seq_printf(m, "r[%04d]=0x%08x\n", i, rdev->cp.ring[i]);
-		i = (i + 1) & rdev->cp.ptr_mask;
-	}
-	return 0;
-}
-
 static int r600_debugfs_mc_info(struct seq_file *m, void *data)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
@@ -3533,7 +3533,6 @@ static int r600_debugfs_mc_info(struct seq_file *m, void *data)
 
 static struct drm_info_list r600_mc_info_list[] = {
 	{"r600_mc_info", r600_debugfs_mc_info, 0, NULL},
-	{"r600_ring_info", r600_debugfs_cp_ring_info, 0, NULL},
 };
 #endif
 
diff --git a/drivers/gpu/drm/radeon/r600_audio.c b/drivers/gpu/drm/radeon/r600_audio.c
index 846fae576399..ba66f3093d46 100644
--- a/drivers/gpu/drm/radeon/r600_audio.c
+++ b/drivers/gpu/drm/radeon/r600_audio.c
@@ -36,7 +36,7 @@
  */
 static int r600_audio_chipset_supported(struct radeon_device *rdev)
 {
-	return (rdev->family >= CHIP_R600 && rdev->family < CHIP_CEDAR)
+	return (rdev->family >= CHIP_R600 && !ASIC_IS_DCE5(rdev))
 		|| rdev->family == CHIP_RS600
 		|| rdev->family == CHIP_RS690
 		|| rdev->family == CHIP_RS740;
@@ -161,8 +161,18 @@ static void r600_audio_update_hdmi(unsigned long param)
  */
 static void r600_audio_engine_enable(struct radeon_device *rdev, bool enable)
 {
+	u32 value = 0;
 	DRM_INFO("%s audio support\n", enable ? "Enabling" : "Disabling");
-	WREG32_P(R600_AUDIO_ENABLE, enable ? 0x81000000 : 0x0, ~0x81000000);
+	if (ASIC_IS_DCE4(rdev)) {
+		if (enable) {
+			value |= 0x81000000; /* Required to enable audio */
+			value |= 0x0e1000f0; /* fglrx sets that too */
+		}
+		WREG32(EVERGREEN_AUDIO_ENABLE, value);
+	} else {
+		WREG32_P(R600_AUDIO_ENABLE,
+			 enable ? 0x81000000 : 0x0, ~0x81000000);
+	}
 	rdev->audio_enabled = enable;
 }
 
@@ -248,22 +258,33 @@ void r600_audio_set_clock(struct drm_encoder *encoder, int clock)
 		return;
 	}
 
-	switch (dig->dig_encoder) {
-	case 0:
-		WREG32(R600_AUDIO_PLL1_MUL, base_rate * 50);
-		WREG32(R600_AUDIO_PLL1_DIV, clock * 100);
-		WREG32(R600_AUDIO_CLK_SRCSEL, 0);
-		break;
-
-	case 1:
-		WREG32(R600_AUDIO_PLL2_MUL, base_rate * 50);
-		WREG32(R600_AUDIO_PLL2_DIV, clock * 100);
-		WREG32(R600_AUDIO_CLK_SRCSEL, 1);
-		break;
-	default:
-		dev_err(rdev->dev, "Unsupported DIG on encoder 0x%02X\n",
-			  radeon_encoder->encoder_id);
-		return;
+	if (ASIC_IS_DCE4(rdev)) {
+		/* TODO: other PLLs? */
+		WREG32(EVERGREEN_AUDIO_PLL1_MUL, base_rate * 10);
+		WREG32(EVERGREEN_AUDIO_PLL1_DIV, clock * 10);
+		WREG32(EVERGREEN_AUDIO_PLL1_UNK, 0x00000071);
+
+		/* Some magic trigger or src sel? */
+		WREG32_P(0x5ac, 0x01, ~0x77);
+	} else {
+		switch (dig->dig_encoder) {
+		case 0:
+			WREG32(R600_AUDIO_PLL1_MUL, base_rate * 50);
+			WREG32(R600_AUDIO_PLL1_DIV, clock * 100);
+			WREG32(R600_AUDIO_CLK_SRCSEL, 0);
+			break;
+
+		case 1:
+			WREG32(R600_AUDIO_PLL2_MUL, base_rate * 50);
+			WREG32(R600_AUDIO_PLL2_DIV, clock * 100);
+			WREG32(R600_AUDIO_CLK_SRCSEL, 1);
+			break;
+		default:
+			dev_err(rdev->dev,
+				"Unsupported DIG on encoder 0x%02X\n",
+				radeon_encoder->encoder_id);
+			return;
+		}
 	}
 }
 
diff --git a/drivers/gpu/drm/radeon/r600_blit_kms.c b/drivers/gpu/drm/radeon/r600_blit_kms.c
index e09d2818f949..d996f4381130 100644
--- a/drivers/gpu/drm/radeon/r600_blit_kms.c
+++ b/drivers/gpu/drm/radeon/r600_blit_kms.c
@@ -50,6 +50,7 @@ static void
 set_render_target(struct radeon_device *rdev, int format,
 		  int w, int h, u64 gpu_addr)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 cb_color_info;
 	int pitch, slice;
 
@@ -63,38 +64,38 @@ set_render_target(struct radeon_device *rdev, int format,
 	pitch = (w / 8) - 1;
 	slice = ((w * h) / 64) - 1;
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (CB_COLOR0_BASE - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, gpu_addr >> 8);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (CB_COLOR0_BASE - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, gpu_addr >> 8);
 
 	if (rdev->family > CHIP_R600 && rdev->family < CHIP_RV770) {
-		radeon_ring_write(rdev, PACKET3(PACKET3_SURFACE_BASE_UPDATE, 0));
-		radeon_ring_write(rdev, 2 << 0);
+		radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_BASE_UPDATE, 0));
+		radeon_ring_write(ring, 2 << 0);
 	}
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (CB_COLOR0_SIZE - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, (pitch << 0) | (slice << 10));
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (CB_COLOR0_SIZE - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, (pitch << 0) | (slice << 10));
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (CB_COLOR0_VIEW - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (CB_COLOR0_VIEW - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, 0);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (CB_COLOR0_INFO - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, cb_color_info);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (CB_COLOR0_INFO - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, cb_color_info);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (CB_COLOR0_TILE - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (CB_COLOR0_TILE - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, 0);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (CB_COLOR0_FRAG - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (CB_COLOR0_FRAG - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, 0);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (CB_COLOR0_MASK - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (CB_COLOR0_MASK - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, 0);
 }
 
 /* emits 5dw */
@@ -103,6 +104,7 @@ cp_set_surface_sync(struct radeon_device *rdev,
 		    u32 sync_type, u32 size,
 		    u64 mc_addr)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 cp_coher_size;
 
 	if (size == 0xffffffff)
@@ -110,17 +112,18 @@ cp_set_surface_sync(struct radeon_device *rdev,
 	else
 		cp_coher_size = ((size + 255) >> 8);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SURFACE_SYNC, 3));
-	radeon_ring_write(rdev, sync_type);
-	radeon_ring_write(rdev, cp_coher_size);
-	radeon_ring_write(rdev, mc_addr >> 8);
-	radeon_ring_write(rdev, 10); /* poll interval */
+	radeon_ring_write(ring, PACKET3(PACKET3_SURFACE_SYNC, 3));
+	radeon_ring_write(ring, sync_type);
+	radeon_ring_write(ring, cp_coher_size);
+	radeon_ring_write(ring, mc_addr >> 8);
+	radeon_ring_write(ring, 10); /* poll interval */
 }
 
 /* emits 21dw + 1 surface sync = 26dw */
 static void
 set_shaders(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u64 gpu_addr;
 	u32 sq_pgm_resources;
 
@@ -129,35 +132,35 @@ set_shaders(struct radeon_device *rdev)
 
 	/* VS */
 	gpu_addr = rdev->r600_blit.shader_gpu_addr + rdev->r600_blit.vs_offset;
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (SQ_PGM_START_VS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, gpu_addr >> 8);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (SQ_PGM_START_VS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, gpu_addr >> 8);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (SQ_PGM_RESOURCES_VS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, sq_pgm_resources);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (SQ_PGM_RESOURCES_VS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, sq_pgm_resources);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (SQ_PGM_CF_OFFSET_VS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (SQ_PGM_CF_OFFSET_VS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, 0);
 
 	/* PS */
 	gpu_addr = rdev->r600_blit.shader_gpu_addr + rdev->r600_blit.ps_offset;
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (SQ_PGM_START_PS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, gpu_addr >> 8);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (SQ_PGM_START_PS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, gpu_addr >> 8);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (SQ_PGM_RESOURCES_PS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, sq_pgm_resources | (1 << 28));
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (SQ_PGM_RESOURCES_PS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, sq_pgm_resources | (1 << 28));
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (SQ_PGM_EXPORTS_PS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, 2);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (SQ_PGM_EXPORTS_PS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, 2);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
-	radeon_ring_write(rdev, (SQ_PGM_CF_OFFSET_PS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, 0);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 1));
+	radeon_ring_write(ring, (SQ_PGM_CF_OFFSET_PS - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, 0);
 
 	gpu_addr = rdev->r600_blit.shader_gpu_addr + rdev->r600_blit.vs_offset;
 	cp_set_surface_sync(rdev, PACKET3_SH_ACTION_ENA, 512, gpu_addr);
@@ -167,6 +170,7 @@ set_shaders(struct radeon_device *rdev)
 static void
 set_vtx_resource(struct radeon_device *rdev, u64 gpu_addr)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 sq_vtx_constant_word2;
 
 	sq_vtx_constant_word2 = SQ_VTXC_BASE_ADDR_HI(upper_32_bits(gpu_addr) & 0xff) |
@@ -175,15 +179,15 @@ set_vtx_resource(struct radeon_device *rdev, u64 gpu_addr)
 	sq_vtx_constant_word2 |=  SQ_VTXC_ENDIAN_SWAP(SQ_ENDIAN_8IN32);
 #endif
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_RESOURCE, 7));
-	radeon_ring_write(rdev, 0x460);
-	radeon_ring_write(rdev, gpu_addr & 0xffffffff);
-	radeon_ring_write(rdev, 48 - 1);
-	radeon_ring_write(rdev, sq_vtx_constant_word2);
-	radeon_ring_write(rdev, 1 << 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, SQ_TEX_VTX_VALID_BUFFER << 30);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_RESOURCE, 7));
+	radeon_ring_write(ring, 0x460);
+	radeon_ring_write(ring, gpu_addr & 0xffffffff);
+	radeon_ring_write(ring, 48 - 1);
+	radeon_ring_write(ring, sq_vtx_constant_word2);
+	radeon_ring_write(ring, 1 << 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, SQ_TEX_VTX_VALID_BUFFER << 30);
 
 	if ((rdev->family == CHIP_RV610) ||
 	    (rdev->family == CHIP_RV620) ||
@@ -203,6 +207,7 @@ set_tex_resource(struct radeon_device *rdev,
 		 int format, int w, int h, int pitch,
 		 u64 gpu_addr, u32 size)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	uint32_t sq_tex_resource_word0, sq_tex_resource_word1, sq_tex_resource_word4;
 
 	if (h < 1)
@@ -225,15 +230,15 @@ set_tex_resource(struct radeon_device *rdev,
 	cp_set_surface_sync(rdev,
 			    PACKET3_TC_ACTION_ENA, size, gpu_addr);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_RESOURCE, 7));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, sq_tex_resource_word0);
-	radeon_ring_write(rdev, sq_tex_resource_word1);
-	radeon_ring_write(rdev, gpu_addr >> 8);
-	radeon_ring_write(rdev, gpu_addr >> 8);
-	radeon_ring_write(rdev, sq_tex_resource_word4);
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, SQ_TEX_VTX_VALID_TEXTURE << 30);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_RESOURCE, 7));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, sq_tex_resource_word0);
+	radeon_ring_write(ring, sq_tex_resource_word1);
+	radeon_ring_write(ring, gpu_addr >> 8);
+	radeon_ring_write(ring, gpu_addr >> 8);
+	radeon_ring_write(ring, sq_tex_resource_word4);
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, SQ_TEX_VTX_VALID_TEXTURE << 30);
 }
 
 /* emits 12 */
@@ -241,43 +246,45 @@ static void
 set_scissors(struct radeon_device *rdev, int x1, int y1,
 	     int x2, int y2)
 {
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
-	radeon_ring_write(rdev, (PA_SC_SCREEN_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, (x1 << 0) | (y1 << 16));
-	radeon_ring_write(rdev, (x2 << 0) | (y2 << 16));
-
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
-	radeon_ring_write(rdev, (PA_SC_GENERIC_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, (x1 << 0) | (y1 << 16) | (1 << 31));
-	radeon_ring_write(rdev, (x2 << 0) | (y2 << 16));
-
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
-	radeon_ring_write(rdev, (PA_SC_WINDOW_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, (x1 << 0) | (y1 << 16) | (1 << 31));
-	radeon_ring_write(rdev, (x2 << 0) | (y2 << 16));
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
+	radeon_ring_write(ring, (PA_SC_SCREEN_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, (x1 << 0) | (y1 << 16));
+	radeon_ring_write(ring, (x2 << 0) | (y2 << 16));
+
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
+	radeon_ring_write(ring, (PA_SC_GENERIC_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, (x1 << 0) | (y1 << 16) | (1 << 31));
+	radeon_ring_write(ring, (x2 << 0) | (y2 << 16));
+
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONTEXT_REG, 2));
+	radeon_ring_write(ring, (PA_SC_WINDOW_SCISSOR_TL - PACKET3_SET_CONTEXT_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, (x1 << 0) | (y1 << 16) | (1 << 31));
+	radeon_ring_write(ring, (x2 << 0) | (y2 << 16));
 }
 
 /* emits 10 */
 static void
 draw_auto(struct radeon_device *rdev)
 {
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 1));
-	radeon_ring_write(rdev, (VGT_PRIMITIVE_TYPE - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, DI_PT_RECTLIST);
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
+	radeon_ring_write(ring, (VGT_PRIMITIVE_TYPE - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, DI_PT_RECTLIST);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_INDEX_TYPE, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET3(PACKET3_INDEX_TYPE, 0));
+	radeon_ring_write(ring,
 #ifdef __BIG_ENDIAN
 			  (2 << 2) |
 #endif
 			  DI_INDEX_SIZE_16_BIT);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_NUM_INSTANCES, 0));
-	radeon_ring_write(rdev, 1);
+	radeon_ring_write(ring, PACKET3(PACKET3_NUM_INSTANCES, 0));
+	radeon_ring_write(ring, 1);
 
-	radeon_ring_write(rdev, PACKET3(PACKET3_DRAW_INDEX_AUTO, 1));
-	radeon_ring_write(rdev, 3);
-	radeon_ring_write(rdev, DI_SRC_SEL_AUTO_INDEX);
+	radeon_ring_write(ring, PACKET3(PACKET3_DRAW_INDEX_AUTO, 1));
+	radeon_ring_write(ring, 3);
+	radeon_ring_write(ring, DI_SRC_SEL_AUTO_INDEX);
 
 }
 
@@ -285,6 +292,7 @@ draw_auto(struct radeon_device *rdev)
 static void
 set_default_state(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	u32 sq_config, sq_gpr_resource_mgmt_1, sq_gpr_resource_mgmt_2;
 	u32 sq_thread_resource_mgmt, sq_stack_resource_mgmt_1, sq_stack_resource_mgmt_2;
 	int num_ps_gprs, num_vs_gprs, num_temp_gprs, num_gs_gprs, num_es_gprs;
@@ -440,24 +448,24 @@ set_default_state(struct radeon_device *rdev)
 	/* emit an IB pointing at default state */
 	dwords = ALIGN(rdev->r600_blit.state_len, 0x10);
 	gpu_addr = rdev->r600_blit.shader_gpu_addr + rdev->r600_blit.state_offset;
-	radeon_ring_write(rdev, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET3(PACKET3_INDIRECT_BUFFER, 2));
+	radeon_ring_write(ring,
 #ifdef __BIG_ENDIAN
 			  (2 << 0) |
 #endif
 			  (gpu_addr & 0xFFFFFFFC));
-	radeon_ring_write(rdev, upper_32_bits(gpu_addr) & 0xFF);
-	radeon_ring_write(rdev, dwords);
+	radeon_ring_write(ring, upper_32_bits(gpu_addr) & 0xFF);
+	radeon_ring_write(ring, dwords);
 
 	/* SQ config */
-	radeon_ring_write(rdev, PACKET3(PACKET3_SET_CONFIG_REG, 6));
-	radeon_ring_write(rdev, (SQ_CONFIG - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
-	radeon_ring_write(rdev, sq_config);
-	radeon_ring_write(rdev, sq_gpr_resource_mgmt_1);
-	radeon_ring_write(rdev, sq_gpr_resource_mgmt_2);
-	radeon_ring_write(rdev, sq_thread_resource_mgmt);
-	radeon_ring_write(rdev, sq_stack_resource_mgmt_1);
-	radeon_ring_write(rdev, sq_stack_resource_mgmt_2);
+	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 6));
+	radeon_ring_write(ring, (SQ_CONFIG - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
+	radeon_ring_write(ring, sq_config);
+	radeon_ring_write(ring, sq_gpr_resource_mgmt_1);
+	radeon_ring_write(ring, sq_gpr_resource_mgmt_2);
+	radeon_ring_write(ring, sq_thread_resource_mgmt);
+	radeon_ring_write(ring, sq_stack_resource_mgmt_1);
+	radeon_ring_write(ring, sq_stack_resource_mgmt_2);
 }
 
 static uint32_t i2f(uint32_t input)
@@ -611,16 +619,17 @@ void r600_blit_fini(struct radeon_device *rdev)
 	radeon_bo_unref(&rdev->r600_blit.shader_obj);
 }
 
-static int r600_vb_ib_get(struct radeon_device *rdev)
+static int r600_vb_ib_get(struct radeon_device *rdev, unsigned size)
 {
 	int r;
-	r = radeon_ib_get(rdev, &rdev->r600_blit.vb_ib);
+	r = radeon_ib_get(rdev, RADEON_RING_TYPE_GFX_INDEX,
+			  &rdev->r600_blit.vb_ib, size);
 	if (r) {
 		DRM_ERROR("failed to get IB for vertex buffer\n");
 		return r;
 	}
 
-	rdev->r600_blit.vb_total = 64*1024;
+	rdev->r600_blit.vb_total = size;
 	rdev->r600_blit.vb_used = 0;
 	return 0;
 }
@@ -679,15 +688,12 @@ static unsigned r600_blit_create_rect(unsigned num_gpu_pages,
 
 int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 	int ring_size;
 	int num_loops = 0;
 	int dwords_per_loop = rdev->r600_blit.ring_size_per_loop;
 
-	r = r600_vb_ib_get(rdev);
-	if (r)
-		return r;
-
 	/* num loops */
 	while (num_gpu_pages) {
 		num_gpu_pages -=
@@ -696,10 +702,15 @@ int r600_blit_prepare_copy(struct radeon_device *rdev, unsigned num_gpu_pages)
 		num_loops++;
 	}
 
+	/* 48 bytes for vertex per loop */
+	r = r600_vb_ib_get(rdev, (num_loops*48)+256);
+	if (r)
+		return r;
+
 	/* calculate number of loops correctly */
 	ring_size = num_loops * dwords_per_loop;
 	ring_size += rdev->r600_blit.ring_size_common;
-	r = radeon_ring_lock(rdev, ring_size);
+	r = radeon_ring_lock(rdev, ring, ring_size);
 	if (r)
 		return r;
 
@@ -718,7 +729,7 @@ void r600_blit_done_copy(struct radeon_device *rdev, struct radeon_fence *fence)
 	if (fence)
 		r = radeon_fence_emit(rdev, fence);
 
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_unlock_commit(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 }
 
 void r600_kms_blit_copy(struct radeon_device *rdev,
diff --git a/drivers/gpu/drm/radeon/r600_cp.c b/drivers/gpu/drm/radeon/r600_cp.c
index c9db4931913f..84c546250955 100644
--- a/drivers/gpu/drm/radeon/r600_cp.c
+++ b/drivers/gpu/drm/radeon/r600_cp.c
@@ -1815,7 +1815,7 @@ static void r600_cp_init_ring_buffer(struct drm_device *dev,
 		     dev_priv->ring.size_l2qw);
 #endif
 
-	RADEON_WRITE(R600_CP_SEM_WAIT_TIMER, 0x4);
+	RADEON_WRITE(R600_CP_SEM_WAIT_TIMER, 0x0);
 
 	/* Set the write pointer delay */
 	RADEON_WRITE(R600_CP_RB_WPTR_DELAY, 0);
diff --git a/drivers/gpu/drm/radeon/r600_cs.c b/drivers/gpu/drm/radeon/r600_cs.c
index cb1acffd2430..38ce5d0427e3 100644
--- a/drivers/gpu/drm/radeon/r600_cs.c
+++ b/drivers/gpu/drm/radeon/r600_cs.c
@@ -941,7 +941,7 @@ static int r600_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 		track->db_depth_control = radeon_get_ib_value(p, idx);
 		break;
 	case R_028010_DB_DEPTH_INFO:
-		if (!p->keep_tiling_flags &&
+		if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS) &&
 		    r600_cs_packet_next_is_pkt3_nop(p)) {
 			r = r600_cs_packet_next_reloc(p, &reloc);
 			if (r) {
@@ -993,7 +993,7 @@ static int r600_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
 	case R_0280B4_CB_COLOR5_INFO:
 	case R_0280B8_CB_COLOR6_INFO:
 	case R_0280BC_CB_COLOR7_INFO:
-		if (!p->keep_tiling_flags &&
+		if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS) &&
 		     r600_cs_packet_next_is_pkt3_nop(p)) {
 			r = r600_cs_packet_next_reloc(p, &reloc);
 			if (r) {
@@ -1293,7 +1293,7 @@ static int r600_check_texture_resource(struct radeon_cs_parser *p,  u32 idx,
 	mip_offset <<= 8;
 
 	word0 = radeon_get_ib_value(p, idx + 0);
-	if (!p->keep_tiling_flags) {
+	if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
 		if (tiling_flags & RADEON_TILING_MACRO)
 			word0 |= S_038000_TILE_MODE(V_038000_ARRAY_2D_TILED_THIN1);
 		else if (tiling_flags & RADEON_TILING_MICRO)
@@ -1625,7 +1625,7 @@ static int r600_packet3_check(struct radeon_cs_parser *p,
 					return -EINVAL;
 				}
 				base_offset = (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
-				if (!p->keep_tiling_flags) {
+				if (!(p->cs_flags & RADEON_CS_KEEP_TILING_FLAGS)) {
 					if (reloc->lobj.tiling_flags & RADEON_TILING_MACRO)
 						ib[idx+1+(i*7)+0] |= S_038000_TILE_MODE(V_038000_ARRAY_2D_TILED_THIN1);
 					else if (reloc->lobj.tiling_flags & RADEON_TILING_MICRO)
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index f5ac7e788d81..0b5920671450 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -196,6 +196,13 @@ static void r600_hdmi_videoinfoframe(
 	frame[0xD] = (right_bar >> 8);
 
 	r600_hdmi_infoframe_checksum(0x82, 0x02, 0x0D, frame);
+	/* Our header values (type, version, length) should be alright, Intel
+	 * is using the same. Checksum function also seems to be OK, it works
+	 * fine for audio infoframe. However calculated value is always lower
+	 * by 2 in comparison to fglrx. It breaks displaying anything in case
+	 * of TVs that strictly check the checksum. Hack it manually here to
+	 * workaround this issue. */
+	frame[0x0] += 2;
 
 	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_0,
 		frame[0x0] | (frame[0x1] << 8) | (frame[0x2] << 16) | (frame[0x3] << 24));
@@ -313,7 +320,7 @@ void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mod
 	struct radeon_device *rdev = dev->dev_private;
 	uint32_t offset = to_radeon_encoder(encoder)->hdmi_offset;
 
-	if (ASIC_IS_DCE4(rdev))
+	if (ASIC_IS_DCE5(rdev))
 		return;
 
 	if (!offset)
@@ -455,13 +462,31 @@ static void r600_hdmi_assign_block(struct drm_encoder *encoder)
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 
+	u16 eg_offsets[] = {
+		EVERGREEN_CRTC0_REGISTER_OFFSET,
+		EVERGREEN_CRTC1_REGISTER_OFFSET,
+		EVERGREEN_CRTC2_REGISTER_OFFSET,
+		EVERGREEN_CRTC3_REGISTER_OFFSET,
+		EVERGREEN_CRTC4_REGISTER_OFFSET,
+		EVERGREEN_CRTC5_REGISTER_OFFSET,
+	};
+
 	if (!dig) {
 		dev_err(rdev->dev, "Enabling HDMI on non-dig encoder\n");
 		return;
 	}
 
-	if (ASIC_IS_DCE4(rdev)) {
+	if (ASIC_IS_DCE5(rdev)) {
 		/* TODO */
+	} else if (ASIC_IS_DCE4(rdev)) {
+		if (dig->dig_encoder >= ARRAY_SIZE(eg_offsets)) {
+			dev_err(rdev->dev, "Enabling HDMI on unknown dig\n");
+			return;
+		}
+		radeon_encoder->hdmi_offset = EVERGREEN_HDMI_BASE +
+						eg_offsets[dig->dig_encoder];
+		radeon_encoder->hdmi_config_offset = radeon_encoder->hdmi_offset
+						+ EVERGREEN_HDMI_CONFIG_OFFSET;
 	} else if (ASIC_IS_DCE3(rdev)) {
 		radeon_encoder->hdmi_offset = dig->dig_encoder ?
 			R600_HDMI_BLOCK3 : R600_HDMI_BLOCK1;
@@ -484,7 +509,7 @@ void r600_hdmi_enable(struct drm_encoder *encoder)
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	uint32_t offset;
 
-	if (ASIC_IS_DCE4(rdev))
+	if (ASIC_IS_DCE5(rdev))
 		return;
 
 	if (!radeon_encoder->hdmi_offset) {
@@ -497,16 +522,24 @@ void r600_hdmi_enable(struct drm_encoder *encoder)
 	}
 
 	offset = radeon_encoder->hdmi_offset;
-	if (ASIC_IS_DCE32(rdev) && !ASIC_IS_DCE4(rdev)) {
+	if (ASIC_IS_DCE5(rdev)) {
+		/* TODO */
+	} else if (ASIC_IS_DCE4(rdev)) {
+		WREG32_P(radeon_encoder->hdmi_config_offset + 0xc, 0x1, ~0x1);
+	} else if (ASIC_IS_DCE32(rdev)) {
 		WREG32_P(radeon_encoder->hdmi_config_offset + 0x4, 0x1, ~0x1);
-	} else if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
+	} else if (ASIC_IS_DCE3(rdev)) {
+		/* TODO */
+	} else if (rdev->family >= CHIP_R600) {
 		switch (radeon_encoder->encoder_id) {
 		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
-			WREG32_P(AVIVO_TMDSA_CNTL, 0x4, ~0x4);
+			WREG32_P(AVIVO_TMDSA_CNTL, AVIVO_TMDSA_CNTL_HDMI_EN,
+				 ~AVIVO_TMDSA_CNTL_HDMI_EN);
 			WREG32(offset + R600_HDMI_ENABLE, 0x101);
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
-			WREG32_P(AVIVO_LVTMA_CNTL, 0x4, ~0x4);
+			WREG32_P(AVIVO_LVTMA_CNTL, AVIVO_LVTMA_CNTL_HDMI_EN,
+				 ~AVIVO_LVTMA_CNTL_HDMI_EN);
 			WREG32(offset + R600_HDMI_ENABLE, 0x105);
 			break;
 		default:
@@ -518,8 +551,8 @@ void r600_hdmi_enable(struct drm_encoder *encoder)
 	if (rdev->irq.installed
 	    && rdev->family != CHIP_RS600
 	    && rdev->family != CHIP_RS690
-	    && rdev->family != CHIP_RS740) {
-
+	    && rdev->family != CHIP_RS740
+	    && !ASIC_IS_DCE4(rdev)) {
 		/* if irq is available use it */
 		rdev->irq.hdmi[offset == R600_HDMI_BLOCK1 ? 0 : 1] = true;
 		radeon_irq_set(rdev);
@@ -544,7 +577,7 @@ void r600_hdmi_disable(struct drm_encoder *encoder)
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	uint32_t offset;
 
-	if (ASIC_IS_DCE4(rdev))
+	if (ASIC_IS_DCE5(rdev))
 		return;
 
 	offset = radeon_encoder->hdmi_offset;
@@ -563,16 +596,22 @@ void r600_hdmi_disable(struct drm_encoder *encoder)
 	/* disable polling */
 	r600_audio_disable_polling(encoder);
 
-	if (ASIC_IS_DCE32(rdev) && !ASIC_IS_DCE4(rdev)) {
+	if (ASIC_IS_DCE5(rdev)) {
+		/* TODO */
+	} else if (ASIC_IS_DCE4(rdev)) {
+		WREG32_P(radeon_encoder->hdmi_config_offset + 0xc, 0, ~0x1);
+	} else if (ASIC_IS_DCE32(rdev)) {
 		WREG32_P(radeon_encoder->hdmi_config_offset + 0x4, 0, ~0x1);
 	} else if (rdev->family >= CHIP_R600 && !ASIC_IS_DCE3(rdev)) {
 		switch (radeon_encoder->encoder_id) {
 		case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
-			WREG32_P(AVIVO_TMDSA_CNTL, 0, ~0x4);
+			WREG32_P(AVIVO_TMDSA_CNTL, 0,
+				 ~AVIVO_TMDSA_CNTL_HDMI_EN);
 			WREG32(offset + R600_HDMI_ENABLE, 0);
 			break;
 		case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
-			WREG32_P(AVIVO_LVTMA_CNTL, 0, ~0x4);
+			WREG32_P(AVIVO_LVTMA_CNTL, 0,
+				 ~AVIVO_LVTMA_CNTL_HDMI_EN);
 			WREG32(offset + R600_HDMI_ENABLE, 0);
 			break;
 		default:
diff --git a/drivers/gpu/drm/radeon/r600d.h b/drivers/gpu/drm/radeon/r600d.h
index bfe1b5d92afe..3ee1fd7ef394 100644
--- a/drivers/gpu/drm/radeon/r600d.h
+++ b/drivers/gpu/drm/radeon/r600d.h
@@ -831,6 +831,8 @@
 #define	PACKET3_STRMOUT_BUFFER_UPDATE			0x34
 #define	PACKET3_INDIRECT_BUFFER_MP			0x38
 #define	PACKET3_MEM_SEMAPHORE				0x39
+#              define PACKET3_SEM_SEL_SIGNAL	    (0x6 << 29)
+#              define PACKET3_SEM_SEL_WAIT	    (0x7 << 29)
 #define	PACKET3_MPEG_INDEX				0x3A
 #define	PACKET3_WAIT_REG_MEM				0x3C
 #define	PACKET3_MEM_WRITE				0x3D
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 8227e76b5c70..73e05cb85eca 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -107,6 +107,21 @@ extern int radeon_msi;
 #define RADEONFB_CONN_LIMIT		4
 #define RADEON_BIOS_NUM_SCRATCH		8
 
+/* max number of rings */
+#define RADEON_NUM_RINGS 3
+
+/* internal ring indices */
+/* r1xx+ has gfx CP ring */
+#define RADEON_RING_TYPE_GFX_INDEX  0
+
+/* cayman has 2 compute CP rings */
+#define CAYMAN_RING_TYPE_CP1_INDEX 1
+#define CAYMAN_RING_TYPE_CP2_INDEX 2
+
+/* hardcode those limit for now */
+#define RADEON_VA_RESERVED_SIZE		(8 << 20)
+#define RADEON_IB_VM_MAX_SIZE		(64 << 10)
+
 /*
  * Errata workarounds.
  */
@@ -192,14 +207,15 @@ extern int sumo_get_temp(struct radeon_device *rdev);
  */
 struct radeon_fence_driver {
 	uint32_t			scratch_reg;
+	uint64_t			gpu_addr;
+	volatile uint32_t		*cpu_addr;
 	atomic_t			seq;
 	uint32_t			last_seq;
 	unsigned long			last_jiffies;
 	unsigned long			last_timeout;
 	wait_queue_head_t		queue;
-	rwlock_t			lock;
 	struct list_head		created;
-	struct list_head		emited;
+	struct list_head		emitted;
 	struct list_head		signaled;
 	bool				initialized;
 };
@@ -210,21 +226,26 @@ struct radeon_fence {
 	struct list_head		list;
 	/* protected by radeon_fence.lock */
 	uint32_t			seq;
-	bool				emited;
+	bool				emitted;
 	bool				signaled;
+	/* RB, DMA, etc. */
+	int				ring;
+	struct radeon_semaphore		*semaphore;
 };
 
+int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring);
 int radeon_fence_driver_init(struct radeon_device *rdev);
 void radeon_fence_driver_fini(struct radeon_device *rdev);
-int radeon_fence_create(struct radeon_device *rdev, struct radeon_fence **fence);
+int radeon_fence_create(struct radeon_device *rdev, struct radeon_fence **fence, int ring);
 int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence *fence);
-void radeon_fence_process(struct radeon_device *rdev);
+void radeon_fence_process(struct radeon_device *rdev, int ring);
 bool radeon_fence_signaled(struct radeon_fence *fence);
 int radeon_fence_wait(struct radeon_fence *fence, bool interruptible);
-int radeon_fence_wait_next(struct radeon_device *rdev);
-int radeon_fence_wait_last(struct radeon_device *rdev);
+int radeon_fence_wait_next(struct radeon_device *rdev, int ring);
+int radeon_fence_wait_last(struct radeon_device *rdev, int ring);
 struct radeon_fence *radeon_fence_ref(struct radeon_fence *fence);
 void radeon_fence_unref(struct radeon_fence **fence);
+int radeon_fence_count_emitted(struct radeon_device *rdev, int ring);
 
 /*
  * Tiling registers
@@ -246,6 +267,21 @@ struct radeon_mman {
 	bool				initialized;
 };
 
+/* bo virtual address in a specific vm */
+struct radeon_bo_va {
+	/* bo list is protected by bo being reserved */
+	struct list_head		bo_list;
+	/* vm list is protected by vm mutex */
+	struct list_head		vm_list;
+	/* constant after initialization */
+	struct radeon_vm		*vm;
+	struct radeon_bo		*bo;
+	uint64_t			soffset;
+	uint64_t			eoffset;
+	uint32_t			flags;
+	bool				valid;
+};
+
 struct radeon_bo {
 	/* Protected by gem.mutex */
 	struct list_head		list;
@@ -259,6 +295,10 @@ struct radeon_bo {
 	u32				tiling_flags;
 	u32				pitch;
 	int				surface_reg;
+	/* list of all virtual address to which this bo
+	 * is associated to
+	 */
+	struct list_head		va;
 	/* Constant after initialization */
 	struct radeon_device		*rdev;
 	struct drm_gem_object		gem_base;
@@ -274,6 +314,48 @@ struct radeon_bo_list {
 	u32			tiling_flags;
 };
 
+/* sub-allocation manager, it has to be protected by another lock.
+ * By conception this is an helper for other part of the driver
+ * like the indirect buffer or semaphore, which both have their
+ * locking.
+ *
+ * Principe is simple, we keep a list of sub allocation in offset
+ * order (first entry has offset == 0, last entry has the highest
+ * offset).
+ *
+ * When allocating new object we first check if there is room at
+ * the end total_size - (last_object_offset + last_object_size) >=
+ * alloc_size. If so we allocate new object there.
+ *
+ * When there is not enough room at the end, we start waiting for
+ * each sub object until we reach object_offset+object_size >=
+ * alloc_size, this object then become the sub object we return.
+ *
+ * Alignment can't be bigger than page size.
+ *
+ * Hole are not considered for allocation to keep things simple.
+ * Assumption is that there won't be hole (all object on same
+ * alignment).
+ */
+struct radeon_sa_manager {
+	struct radeon_bo	*bo;
+	struct list_head	sa_bo;
+	unsigned		size;
+	uint64_t		gpu_addr;
+	void			*cpu_ptr;
+	uint32_t		domain;
+};
+
+struct radeon_sa_bo;
+
+/* sub-allocation buffer */
+struct radeon_sa_bo {
+	struct list_head		list;
+	struct radeon_sa_manager	*manager;
+	unsigned			offset;
+	unsigned			size;
+};
+
 /*
  * GEM objects.
  */
@@ -303,6 +385,46 @@ int radeon_mode_dumb_destroy(struct drm_file *file_priv,
 			     uint32_t handle);
 
 /*
+ * Semaphores.
+ */
+struct radeon_ring;
+
+#define	RADEON_SEMAPHORE_BO_SIZE	256
+
+struct radeon_semaphore_driver {
+	rwlock_t			lock;
+	struct list_head		bo;
+};
+
+struct radeon_semaphore_bo;
+
+/* everything here is constant */
+struct radeon_semaphore {
+	struct list_head		list;
+	uint64_t			gpu_addr;
+	uint32_t			*cpu_ptr;
+	struct radeon_semaphore_bo	*bo;
+};
+
+struct radeon_semaphore_bo {
+	struct list_head		list;
+	struct radeon_ib		*ib;
+	struct list_head		free;
+	struct radeon_semaphore		semaphores[RADEON_SEMAPHORE_BO_SIZE/8];
+	unsigned			nused;
+};
+
+void radeon_semaphore_driver_fini(struct radeon_device *rdev);
+int radeon_semaphore_create(struct radeon_device *rdev,
+			    struct radeon_semaphore **semaphore);
+void radeon_semaphore_emit_signal(struct radeon_device *rdev, int ring,
+				  struct radeon_semaphore *semaphore);
+void radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
+				struct radeon_semaphore *semaphore);
+void radeon_semaphore_free(struct radeon_device *rdev,
+			   struct radeon_semaphore *semaphore);
+
+/*
  * GART structures, functions & helpers
  */
 struct radeon_mc;
@@ -310,6 +432,7 @@ struct radeon_mc;
 #define RADEON_GPU_PAGE_SIZE 4096
 #define RADEON_GPU_PAGE_MASK (RADEON_GPU_PAGE_SIZE - 1)
 #define RADEON_GPU_PAGE_SHIFT 12
+#define RADEON_GPU_PAGE_ALIGN(a) (((a) + RADEON_GPU_PAGE_MASK) & ~RADEON_GPU_PAGE_MASK)
 
 struct radeon_gart {
 	dma_addr_t			table_addr;
@@ -320,7 +443,6 @@ struct radeon_gart {
 	unsigned			table_size;
 	struct page			**pages;
 	dma_addr_t			*pages_addr;
-	bool				*ttm_alloced;
 	bool				ready;
 };
 
@@ -434,7 +556,7 @@ union radeon_irq_stat_regs {
 
 struct radeon_irq {
 	bool		installed;
-	bool		sw_int;
+	bool		sw_int[RADEON_NUM_RINGS];
 	bool		crtc_vblank_int[RADEON_MAX_CRTCS];
 	bool		pflip[RADEON_MAX_CRTCS];
 	wait_queue_head_t	vblank_queue;
@@ -444,7 +566,7 @@ struct radeon_irq {
 	wait_queue_head_t	idle_queue;
 	bool		hdmi[RADEON_MAX_HDMI_BLOCKS];
 	spinlock_t sw_lock;
-	int sw_refcount;
+	int sw_refcount[RADEON_NUM_RINGS];
 	union radeon_irq_stat_regs stat_regs;
 	spinlock_t pflip_lock[RADEON_MAX_CRTCS];
 	int pflip_refcount[RADEON_MAX_CRTCS];
@@ -452,22 +574,23 @@ struct radeon_irq {
 
 int radeon_irq_kms_init(struct radeon_device *rdev);
 void radeon_irq_kms_fini(struct radeon_device *rdev);
-void radeon_irq_kms_sw_irq_get(struct radeon_device *rdev);
-void radeon_irq_kms_sw_irq_put(struct radeon_device *rdev);
+void radeon_irq_kms_sw_irq_get(struct radeon_device *rdev, int ring);
+void radeon_irq_kms_sw_irq_put(struct radeon_device *rdev, int ring);
 void radeon_irq_kms_pflip_irq_get(struct radeon_device *rdev, int crtc);
 void radeon_irq_kms_pflip_irq_put(struct radeon_device *rdev, int crtc);
 
 /*
- * CP & ring.
+ * CP & rings.
  */
+
 struct radeon_ib {
-	struct list_head	list;
+	struct radeon_sa_bo	sa_bo;
 	unsigned		idx;
+	uint32_t		length_dw;
 	uint64_t		gpu_addr;
-	struct radeon_fence	*fence;
 	uint32_t		*ptr;
-	uint32_t		length_dw;
-	bool			free;
+	struct radeon_fence	*fence;
+	unsigned		vm_id;
 };
 
 /*
@@ -475,20 +598,22 @@ struct radeon_ib {
  * mutex protects scheduled_ibs, ready, alloc_bm
  */
 struct radeon_ib_pool {
-	struct mutex		mutex;
-	struct radeon_bo	*robj;
-	struct list_head	bogus_ib;
-	struct radeon_ib	ibs[RADEON_IB_POOL_SIZE];
-	bool			ready;
-	unsigned		head_id;
+	struct mutex			mutex;
+	struct radeon_sa_manager	sa_manager;
+	struct radeon_ib		ibs[RADEON_IB_POOL_SIZE];
+	bool				ready;
+	unsigned			head_id;
 };
 
-struct radeon_cp {
+struct radeon_ring {
 	struct radeon_bo	*ring_obj;
 	volatile uint32_t	*ring;
 	unsigned		rptr;
+	unsigned		rptr_offs;
+	unsigned		rptr_reg;
 	unsigned		wptr;
 	unsigned		wptr_old;
+	unsigned		wptr_reg;
 	unsigned		ring_size;
 	unsigned		ring_free_dw;
 	int			count_dw;
@@ -497,6 +622,61 @@ struct radeon_cp {
 	uint32_t		ptr_mask;
 	struct mutex		mutex;
 	bool			ready;
+	u32			ptr_reg_shift;
+	u32			ptr_reg_mask;
+	u32			nop;
+};
+
+/*
+ * VM
+ */
+struct radeon_vm {
+	struct list_head		list;
+	struct list_head		va;
+	int				id;
+	unsigned			last_pfn;
+	u64				pt_gpu_addr;
+	u64				*pt;
+	struct radeon_sa_bo		sa_bo;
+	struct mutex			mutex;
+	/* last fence for cs using this vm */
+	struct radeon_fence		*fence;
+};
+
+struct radeon_vm_funcs {
+	int (*init)(struct radeon_device *rdev);
+	void (*fini)(struct radeon_device *rdev);
+	/* cs mutex must be lock for schedule_ib */
+	int (*bind)(struct radeon_device *rdev, struct radeon_vm *vm, int id);
+	void (*unbind)(struct radeon_device *rdev, struct radeon_vm *vm);
+	void (*tlb_flush)(struct radeon_device *rdev, struct radeon_vm *vm);
+	uint32_t (*page_flags)(struct radeon_device *rdev,
+			       struct radeon_vm *vm,
+			       uint32_t flags);
+	void (*set_page)(struct radeon_device *rdev, struct radeon_vm *vm,
+			unsigned pfn, uint64_t addr, uint32_t flags);
+};
+
+struct radeon_vm_manager {
+	struct list_head		lru_vm;
+	uint32_t			use_bitmap;
+	struct radeon_sa_manager	sa_manager;
+	uint32_t			max_pfn;
+	/* fields constant after init */
+	const struct radeon_vm_funcs	*funcs;
+	/* number of VMIDs */
+	unsigned			nvm;
+	/* vram base address for page table entry  */
+	u64				vram_base_offset;
+	/* is vm enabled? */
+	bool				enabled;
+};
+
+/*
+ * file private structure
+ */
+struct radeon_fpriv {
+	struct radeon_vm		vm;
 };
 
 /*
@@ -506,6 +686,7 @@ struct r600_ih {
 	struct radeon_bo	*ring_obj;
 	volatile uint32_t	*ring;
 	unsigned		rptr;
+	unsigned		rptr_offs;
 	unsigned		wptr;
 	unsigned		wptr_old;
 	unsigned		ring_size;
@@ -549,23 +730,29 @@ struct r600_blit {
 
 void r600_blit_suspend(struct radeon_device *rdev);
 
-int radeon_ib_get(struct radeon_device *rdev, struct radeon_ib **ib);
+int radeon_ib_get(struct radeon_device *rdev, int ring,
+		  struct radeon_ib **ib, unsigned size);
 void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib **ib);
+bool radeon_ib_try_free(struct radeon_device *rdev, struct radeon_ib *ib);
 int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib);
 int radeon_ib_pool_init(struct radeon_device *rdev);
 void radeon_ib_pool_fini(struct radeon_device *rdev);
+int radeon_ib_pool_start(struct radeon_device *rdev);
+int radeon_ib_pool_suspend(struct radeon_device *rdev);
 int radeon_ib_test(struct radeon_device *rdev);
-extern void radeon_ib_bogus_add(struct radeon_device *rdev, struct radeon_ib *ib);
 /* Ring access between begin & end cannot sleep */
-void radeon_ring_free_size(struct radeon_device *rdev);
-int radeon_ring_alloc(struct radeon_device *rdev, unsigned ndw);
-int radeon_ring_lock(struct radeon_device *rdev, unsigned ndw);
-void radeon_ring_commit(struct radeon_device *rdev);
-void radeon_ring_unlock_commit(struct radeon_device *rdev);
-void radeon_ring_unlock_undo(struct radeon_device *rdev);
-int radeon_ring_test(struct radeon_device *rdev);
-int radeon_ring_init(struct radeon_device *rdev, unsigned ring_size);
-void radeon_ring_fini(struct radeon_device *rdev);
+int radeon_ring_index(struct radeon_device *rdev, struct radeon_ring *cp);
+void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *cp);
+int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ndw);
+int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ndw);
+void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *cp);
+void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *cp);
+void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *cp);
+int radeon_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
+int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ring_size,
+		     unsigned rptr_offs, unsigned rptr_reg, unsigned wptr_reg,
+		     u32 ptr_reg_shift, u32 ptr_reg_mask, u32 nop);
+void radeon_ring_fini(struct radeon_device *rdev, struct radeon_ring *cp);
 
 
 /*
@@ -582,12 +769,12 @@ struct radeon_cs_reloc {
 struct radeon_cs_chunk {
 	uint32_t		chunk_id;
 	uint32_t		length_dw;
-	int kpage_idx[2];
-	uint32_t                *kpage[2];
+	int			kpage_idx[2];
+	uint32_t		*kpage[2];
 	uint32_t		*kdata;
-	void __user *user_ptr;
-	int last_copied_page;
-	int last_page_index;
+	void __user		*user_ptr;
+	int			last_copied_page;
+	int			last_page_index;
 };
 
 struct radeon_cs_parser {
@@ -605,14 +792,18 @@ struct radeon_cs_parser {
 	struct radeon_cs_reloc	*relocs;
 	struct radeon_cs_reloc	**relocs_ptr;
 	struct list_head	validated;
+	bool			sync_to_ring[RADEON_NUM_RINGS];
 	/* indices of various chunks */
 	int			chunk_ib_idx;
 	int			chunk_relocs_idx;
+	int			chunk_flags_idx;
 	struct radeon_ib	*ib;
 	void			*track;
 	unsigned		family;
 	int			parser_error;
-	bool			keep_tiling_flags;
+	u32			cs_flags;
+	u32			ring;
+	s32			priority;
 };
 
 extern int radeon_cs_update_pages(struct radeon_cs_parser *p, int pg_idx);
@@ -869,11 +1060,20 @@ void radeon_benchmark(struct radeon_device *rdev, int test_number);
  * Testing
  */
 void radeon_test_moves(struct radeon_device *rdev);
+void radeon_test_ring_sync(struct radeon_device *rdev,
+			   struct radeon_ring *cpA,
+			   struct radeon_ring *cpB);
+void radeon_test_syncing(struct radeon_device *rdev);
 
 
 /*
  * Debugfs
  */
+struct radeon_debugfs {
+	struct drm_info_list	*files;
+	unsigned		num_files;
+};
+
 int radeon_debugfs_add_files(struct radeon_device *rdev,
 			     struct drm_info_list *files,
 			     unsigned nfiles);
@@ -889,21 +1089,27 @@ struct radeon_asic {
 	int (*resume)(struct radeon_device *rdev);
 	int (*suspend)(struct radeon_device *rdev);
 	void (*vga_set_state)(struct radeon_device *rdev, bool state);
-	bool (*gpu_is_lockup)(struct radeon_device *rdev);
+	bool (*gpu_is_lockup)(struct radeon_device *rdev, struct radeon_ring *cp);
 	int (*asic_reset)(struct radeon_device *rdev);
 	void (*gart_tlb_flush)(struct radeon_device *rdev);
 	int (*gart_set_page)(struct radeon_device *rdev, int i, uint64_t addr);
 	int (*cp_init)(struct radeon_device *rdev, unsigned ring_size);
 	void (*cp_fini)(struct radeon_device *rdev);
 	void (*cp_disable)(struct radeon_device *rdev);
-	void (*cp_commit)(struct radeon_device *rdev);
 	void (*ring_start)(struct radeon_device *rdev);
-	int (*ring_test)(struct radeon_device *rdev);
-	void (*ring_ib_execute)(struct radeon_device *rdev, struct radeon_ib *ib);
+
+	struct {
+		void (*ib_execute)(struct radeon_device *rdev, struct radeon_ib *ib);
+		int (*ib_parse)(struct radeon_device *rdev, struct radeon_ib *ib);
+		void (*emit_fence)(struct radeon_device *rdev, struct radeon_fence *fence);
+		void (*emit_semaphore)(struct radeon_device *rdev, struct radeon_ring *cp,
+				       struct radeon_semaphore *semaphore, bool emit_wait);
+	} ring[RADEON_NUM_RINGS];
+
+	int (*ring_test)(struct radeon_device *rdev, struct radeon_ring *cp);
 	int (*irq_set)(struct radeon_device *rdev);
 	int (*irq_process)(struct radeon_device *rdev);
 	u32 (*get_vblank_counter)(struct radeon_device *rdev, int crtc);
-	void (*fence_ring_emit)(struct radeon_device *rdev, struct radeon_fence *fence);
 	int (*cs_parse)(struct radeon_cs_parser *p);
 	int (*copy_blit)(struct radeon_device *rdev,
 			 uint64_t src_offset,
@@ -1132,6 +1338,8 @@ int radeon_gem_busy_ioctl(struct drm_device *dev, void *data,
 			  struct drm_file *filp);
 int radeon_gem_wait_idle_ioctl(struct drm_device *dev, void *data,
 			      struct drm_file *filp);
+int radeon_gem_va_ioctl(struct drm_device *dev, void *data,
+			  struct drm_file *filp);
 int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp);
 int radeon_gem_set_tiling_ioctl(struct drm_device *dev, void *data,
 				struct drm_file *filp);
@@ -1231,11 +1439,10 @@ struct radeon_device {
 	struct radeon_mode_info		mode_info;
 	struct radeon_scratch		scratch;
 	struct radeon_mman		mman;
-	struct radeon_fence_driver	fence_drv;
-	struct radeon_cp		cp;
-	/* cayman compute rings */
-	struct radeon_cp		cp1;
-	struct radeon_cp		cp2;
+	rwlock_t			fence_lock;
+	struct radeon_fence_driver	fence_drv[RADEON_NUM_RINGS];
+	struct radeon_semaphore_driver	semaphore_drv;
+	struct radeon_ring		ring[RADEON_NUM_RINGS];
 	struct radeon_ib_pool		ib_pool;
 	struct radeon_irq		irq;
 	struct radeon_asic		*asic;
@@ -1279,6 +1486,13 @@ struct radeon_device {
 	struct drm_file *cmask_filp;
 	/* i2c buses */
 	struct radeon_i2c_chan *i2c_bus[RADEON_MAX_I2C_BUS];
+	/* debugfs */
+	struct radeon_debugfs	debugfs[RADEON_DEBUGFS_MAX_COMPONENTS];
+	unsigned 		debugfs_count;
+	/* virtual memory */
+	struct radeon_vm_manager	vm_manager;
+	/* ring used for bo copies */
+	u32				copy_ring;
 };
 
 int radeon_device_init(struct radeon_device *rdev,
@@ -1414,18 +1628,17 @@ void radeon_atombios_fini(struct radeon_device *rdev);
 /*
  * RING helpers.
  */
-
 #if DRM_DEBUG_CODE == 0
-static inline void radeon_ring_write(struct radeon_device *rdev, uint32_t v)
+static inline void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
 {
-	rdev->cp.ring[rdev->cp.wptr++] = v;
-	rdev->cp.wptr &= rdev->cp.ptr_mask;
-	rdev->cp.count_dw--;
-	rdev->cp.ring_free_dw--;
+	ring->ring[ring->wptr++] = v;
+	ring->wptr &= ring->ptr_mask;
+	ring->count_dw--;
+	ring->ring_free_dw--;
 }
 #else
 /* With debugging this is just too big to inline */
-void radeon_ring_write(struct radeon_device *rdev, uint32_t v);
+void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
 #endif
 
 /*
@@ -1437,18 +1650,19 @@ void radeon_ring_write(struct radeon_device *rdev, uint32_t v);
 #define radeon_suspend(rdev) (rdev)->asic->suspend((rdev))
 #define radeon_cs_parse(p) rdev->asic->cs_parse((p))
 #define radeon_vga_set_state(rdev, state) (rdev)->asic->vga_set_state((rdev), (state))
-#define radeon_gpu_is_lockup(rdev) (rdev)->asic->gpu_is_lockup((rdev))
+#define radeon_gpu_is_lockup(rdev, cp) (rdev)->asic->gpu_is_lockup((rdev), (cp))
 #define radeon_asic_reset(rdev) (rdev)->asic->asic_reset((rdev))
 #define radeon_gart_tlb_flush(rdev) (rdev)->asic->gart_tlb_flush((rdev))
 #define radeon_gart_set_page(rdev, i, p) (rdev)->asic->gart_set_page((rdev), (i), (p))
-#define radeon_cp_commit(rdev) (rdev)->asic->cp_commit((rdev))
 #define radeon_ring_start(rdev) (rdev)->asic->ring_start((rdev))
-#define radeon_ring_test(rdev) (rdev)->asic->ring_test((rdev))
-#define radeon_ring_ib_execute(rdev, ib) (rdev)->asic->ring_ib_execute((rdev), (ib))
+#define radeon_ring_test(rdev, cp) (rdev)->asic->ring_test((rdev), (cp))
+#define radeon_ring_ib_execute(rdev, r, ib) (rdev)->asic->ring[(r)].ib_execute((rdev), (ib))
+#define radeon_ring_ib_parse(rdev, r, ib) (rdev)->asic->ring[(r)].ib_parse((rdev), (ib))
 #define radeon_irq_set(rdev) (rdev)->asic->irq_set((rdev))
 #define radeon_irq_process(rdev) (rdev)->asic->irq_process((rdev))
 #define radeon_get_vblank_counter(rdev, crtc) (rdev)->asic->get_vblank_counter((rdev), (crtc))
-#define radeon_fence_ring_emit(rdev, fence) (rdev)->asic->fence_ring_emit((rdev), (fence))
+#define radeon_fence_ring_emit(rdev, r, fence) (rdev)->asic->ring[(r)].emit_fence((rdev), (fence))
+#define radeon_semaphore_ring_emit(rdev, r, cp, semaphore, emit_wait) (rdev)->asic->ring[(r)].emit_semaphore((rdev), (cp), (semaphore), (emit_wait))
 #define radeon_copy_blit(rdev, s, d, np, f) (rdev)->asic->copy_blit((rdev), (s), (d), (np), (f))
 #define radeon_copy_dma(rdev, s, d, np, f) (rdev)->asic->copy_dma((rdev), (s), (d), (np), (f))
 #define radeon_copy(rdev, s, d, np, f) (rdev)->asic->copy((rdev), (s), (d), (np), (f))
@@ -1503,6 +1717,33 @@ extern int radeon_suspend_kms(struct drm_device *dev, pm_message_t state);
 extern void radeon_ttm_set_active_vram_size(struct radeon_device *rdev, u64 size);
 
 /*
+ * vm
+ */
+int radeon_vm_manager_init(struct radeon_device *rdev);
+void radeon_vm_manager_fini(struct radeon_device *rdev);
+int radeon_vm_manager_start(struct radeon_device *rdev);
+int radeon_vm_manager_suspend(struct radeon_device *rdev);
+int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm);
+void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm);
+int radeon_vm_bind(struct radeon_device *rdev, struct radeon_vm *vm);
+void radeon_vm_unbind(struct radeon_device *rdev, struct radeon_vm *vm);
+int radeon_vm_bo_update_pte(struct radeon_device *rdev,
+			    struct radeon_vm *vm,
+			    struct radeon_bo *bo,
+			    struct ttm_mem_reg *mem);
+void radeon_vm_bo_invalidate(struct radeon_device *rdev,
+			     struct radeon_bo *bo);
+int radeon_vm_bo_add(struct radeon_device *rdev,
+		     struct radeon_vm *vm,
+		     struct radeon_bo *bo,
+		     uint64_t offset,
+		     uint32_t flags);
+int radeon_vm_bo_rmv(struct radeon_device *rdev,
+		     struct radeon_vm *vm,
+		     struct radeon_bo *bo);
+
+
+/*
  * R600 vram scratch functions
  */
 int r600_vram_scratch_init(struct radeon_device *rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index a2e1eae114ef..36a6192ce862 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -138,14 +138,18 @@ static struct radeon_asic r100_asic = {
 	.asic_reset = &r100_asic_reset,
 	.gart_tlb_flush = &r100_pci_gart_tlb_flush,
 	.gart_set_page = &r100_pci_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &r100_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r100_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r100_irq_set,
 	.irq_process = &r100_irq_process,
 	.get_vblank_counter = &r100_get_vblank_counter,
-	.fence_ring_emit = &r100_fence_ring_emit,
 	.cs_parse = &r100_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = NULL,
@@ -186,14 +190,18 @@ static struct radeon_asic r200_asic = {
 	.asic_reset = &r100_asic_reset,
 	.gart_tlb_flush = &r100_pci_gart_tlb_flush,
 	.gart_set_page = &r100_pci_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &r100_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r100_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r100_irq_set,
 	.irq_process = &r100_irq_process,
 	.get_vblank_counter = &r100_get_vblank_counter,
-	.fence_ring_emit = &r100_fence_ring_emit,
 	.cs_parse = &r100_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -233,14 +241,18 @@ static struct radeon_asic r300_asic = {
 	.asic_reset = &r300_asic_reset,
 	.gart_tlb_flush = &r100_pci_gart_tlb_flush,
 	.gart_set_page = &r100_pci_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &r300_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r300_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r100_irq_set,
 	.irq_process = &r100_irq_process,
 	.get_vblank_counter = &r100_get_vblank_counter,
-	.fence_ring_emit = &r300_fence_ring_emit,
 	.cs_parse = &r300_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -281,14 +293,18 @@ static struct radeon_asic r300_asic_pcie = {
 	.asic_reset = &r300_asic_reset,
 	.gart_tlb_flush = &rv370_pcie_gart_tlb_flush,
 	.gart_set_page = &rv370_pcie_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &r300_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r300_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r100_irq_set,
 	.irq_process = &r100_irq_process,
 	.get_vblank_counter = &r100_get_vblank_counter,
-	.fence_ring_emit = &r300_fence_ring_emit,
 	.cs_parse = &r300_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -328,14 +344,18 @@ static struct radeon_asic r420_asic = {
 	.asic_reset = &r300_asic_reset,
 	.gart_tlb_flush = &rv370_pcie_gart_tlb_flush,
 	.gart_set_page = &rv370_pcie_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &r300_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r300_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r100_irq_set,
 	.irq_process = &r100_irq_process,
 	.get_vblank_counter = &r100_get_vblank_counter,
-	.fence_ring_emit = &r300_fence_ring_emit,
 	.cs_parse = &r300_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -376,14 +396,18 @@ static struct radeon_asic rs400_asic = {
 	.asic_reset = &r300_asic_reset,
 	.gart_tlb_flush = &rs400_gart_tlb_flush,
 	.gart_set_page = &rs400_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &r300_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r300_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r100_irq_set,
 	.irq_process = &r100_irq_process,
 	.get_vblank_counter = &r100_get_vblank_counter,
-	.fence_ring_emit = &r300_fence_ring_emit,
 	.cs_parse = &r300_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -424,14 +448,18 @@ static struct radeon_asic rs600_asic = {
 	.asic_reset = &rs600_asic_reset,
 	.gart_tlb_flush = &rs600_gart_tlb_flush,
 	.gart_set_page = &rs600_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &r300_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r300_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &rs600_irq_set,
 	.irq_process = &rs600_irq_process,
 	.get_vblank_counter = &rs600_get_vblank_counter,
-	.fence_ring_emit = &r300_fence_ring_emit,
 	.cs_parse = &r300_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -472,14 +500,18 @@ static struct radeon_asic rs690_asic = {
 	.asic_reset = &rs600_asic_reset,
 	.gart_tlb_flush = &rs400_gart_tlb_flush,
 	.gart_set_page = &rs400_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &r300_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r300_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &rs600_irq_set,
 	.irq_process = &rs600_irq_process,
 	.get_vblank_counter = &rs600_get_vblank_counter,
-	.fence_ring_emit = &r300_fence_ring_emit,
 	.cs_parse = &r300_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -520,14 +552,18 @@ static struct radeon_asic rv515_asic = {
 	.asic_reset = &rs600_asic_reset,
 	.gart_tlb_flush = &rv370_pcie_gart_tlb_flush,
 	.gart_set_page = &rv370_pcie_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &rv515_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r300_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &rs600_irq_set,
 	.irq_process = &rs600_irq_process,
 	.get_vblank_counter = &rs600_get_vblank_counter,
-	.fence_ring_emit = &r300_fence_ring_emit,
 	.cs_parse = &r300_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -568,14 +604,18 @@ static struct radeon_asic r520_asic = {
 	.asic_reset = &rs600_asic_reset,
 	.gart_tlb_flush = &rv370_pcie_gart_tlb_flush,
 	.gart_set_page = &rv370_pcie_gart_set_page,
-	.cp_commit = &r100_cp_commit,
 	.ring_start = &rv515_ring_start,
 	.ring_test = &r100_ring_test,
-	.ring_ib_execute = &r100_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r100_ring_ib_execute,
+			.emit_fence = &r300_fence_ring_emit,
+			.emit_semaphore = &r100_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &rs600_irq_set,
 	.irq_process = &rs600_irq_process,
 	.get_vblank_counter = &rs600_get_vblank_counter,
-	.fence_ring_emit = &r300_fence_ring_emit,
 	.cs_parse = &r300_cs_parse,
 	.copy_blit = &r100_copy_blit,
 	.copy_dma = &r200_copy_dma,
@@ -611,18 +651,22 @@ static struct radeon_asic r600_asic = {
 	.fini = &r600_fini,
 	.suspend = &r600_suspend,
 	.resume = &r600_resume,
-	.cp_commit = &r600_cp_commit,
 	.vga_set_state = &r600_vga_set_state,
 	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.asic_reset = &r600_asic_reset,
 	.gart_tlb_flush = &r600_pcie_gart_tlb_flush,
 	.gart_set_page = &rs600_gart_set_page,
 	.ring_test = &r600_ring_test,
-	.ring_ib_execute = &r600_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r600_ring_ib_execute,
+			.emit_fence = &r600_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r600_irq_set,
 	.irq_process = &r600_irq_process,
 	.get_vblank_counter = &rs600_get_vblank_counter,
-	.fence_ring_emit = &r600_fence_ring_emit,
 	.cs_parse = &r600_cs_parse,
 	.copy_blit = &r600_copy_blit,
 	.copy_dma = NULL,
@@ -658,18 +702,22 @@ static struct radeon_asic rs780_asic = {
 	.fini = &r600_fini,
 	.suspend = &r600_suspend,
 	.resume = &r600_resume,
-	.cp_commit = &r600_cp_commit,
 	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.vga_set_state = &r600_vga_set_state,
 	.asic_reset = &r600_asic_reset,
 	.gart_tlb_flush = &r600_pcie_gart_tlb_flush,
 	.gart_set_page = &rs600_gart_set_page,
 	.ring_test = &r600_ring_test,
-	.ring_ib_execute = &r600_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r600_ring_ib_execute,
+			.emit_fence = &r600_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r600_irq_set,
 	.irq_process = &r600_irq_process,
 	.get_vblank_counter = &rs600_get_vblank_counter,
-	.fence_ring_emit = &r600_fence_ring_emit,
 	.cs_parse = &r600_cs_parse,
 	.copy_blit = &r600_copy_blit,
 	.copy_dma = NULL,
@@ -705,18 +753,22 @@ static struct radeon_asic rv770_asic = {
 	.fini = &rv770_fini,
 	.suspend = &rv770_suspend,
 	.resume = &rv770_resume,
-	.cp_commit = &r600_cp_commit,
 	.asic_reset = &r600_asic_reset,
 	.gpu_is_lockup = &r600_gpu_is_lockup,
 	.vga_set_state = &r600_vga_set_state,
 	.gart_tlb_flush = &r600_pcie_gart_tlb_flush,
 	.gart_set_page = &rs600_gart_set_page,
 	.ring_test = &r600_ring_test,
-	.ring_ib_execute = &r600_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &r600_ring_ib_execute,
+			.emit_fence = &r600_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &r600_irq_set,
 	.irq_process = &r600_irq_process,
 	.get_vblank_counter = &rs600_get_vblank_counter,
-	.fence_ring_emit = &r600_fence_ring_emit,
 	.cs_parse = &r600_cs_parse,
 	.copy_blit = &r600_copy_blit,
 	.copy_dma = NULL,
@@ -752,18 +804,22 @@ static struct radeon_asic evergreen_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.cp_commit = &r600_cp_commit,
 	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.gart_tlb_flush = &evergreen_pcie_gart_tlb_flush,
 	.gart_set_page = &rs600_gart_set_page,
 	.ring_test = &r600_ring_test,
-	.ring_ib_execute = &evergreen_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &evergreen_ring_ib_execute,
+			.emit_fence = &r600_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &evergreen_irq_set,
 	.irq_process = &evergreen_irq_process,
 	.get_vblank_counter = &evergreen_get_vblank_counter,
-	.fence_ring_emit = &r600_fence_ring_emit,
 	.cs_parse = &evergreen_cs_parse,
 	.copy_blit = &r600_copy_blit,
 	.copy_dma = NULL,
@@ -799,18 +855,22 @@ static struct radeon_asic sumo_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.cp_commit = &r600_cp_commit,
 	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.gart_tlb_flush = &evergreen_pcie_gart_tlb_flush,
 	.gart_set_page = &rs600_gart_set_page,
 	.ring_test = &r600_ring_test,
-	.ring_ib_execute = &evergreen_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &evergreen_ring_ib_execute,
+			.emit_fence = &r600_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &evergreen_irq_set,
 	.irq_process = &evergreen_irq_process,
 	.get_vblank_counter = &evergreen_get_vblank_counter,
-	.fence_ring_emit = &r600_fence_ring_emit,
 	.cs_parse = &evergreen_cs_parse,
 	.copy_blit = &r600_copy_blit,
 	.copy_dma = NULL,
@@ -846,18 +906,22 @@ static struct radeon_asic btc_asic = {
 	.fini = &evergreen_fini,
 	.suspend = &evergreen_suspend,
 	.resume = &evergreen_resume,
-	.cp_commit = &r600_cp_commit,
 	.gpu_is_lockup = &evergreen_gpu_is_lockup,
 	.asic_reset = &evergreen_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.gart_tlb_flush = &evergreen_pcie_gart_tlb_flush,
 	.gart_set_page = &rs600_gart_set_page,
 	.ring_test = &r600_ring_test,
-	.ring_ib_execute = &evergreen_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &evergreen_ring_ib_execute,
+			.emit_fence = &r600_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &evergreen_irq_set,
 	.irq_process = &evergreen_irq_process,
 	.get_vblank_counter = &evergreen_get_vblank_counter,
-	.fence_ring_emit = &r600_fence_ring_emit,
 	.cs_parse = &evergreen_cs_parse,
 	.copy_blit = &r600_copy_blit,
 	.copy_dma = NULL,
@@ -888,23 +952,50 @@ static struct radeon_asic btc_asic = {
 	.post_page_flip = &evergreen_post_page_flip,
 };
 
+static const struct radeon_vm_funcs cayman_vm_funcs = {
+	.init = &cayman_vm_init,
+	.fini = &cayman_vm_fini,
+	.bind = &cayman_vm_bind,
+	.unbind = &cayman_vm_unbind,
+	.tlb_flush = &cayman_vm_tlb_flush,
+	.page_flags = &cayman_vm_page_flags,
+	.set_page = &cayman_vm_set_page,
+};
+
 static struct radeon_asic cayman_asic = {
 	.init = &cayman_init,
 	.fini = &cayman_fini,
 	.suspend = &cayman_suspend,
 	.resume = &cayman_resume,
-	.cp_commit = &r600_cp_commit,
 	.gpu_is_lockup = &cayman_gpu_is_lockup,
 	.asic_reset = &cayman_asic_reset,
 	.vga_set_state = &r600_vga_set_state,
 	.gart_tlb_flush = &cayman_pcie_gart_tlb_flush,
 	.gart_set_page = &rs600_gart_set_page,
 	.ring_test = &r600_ring_test,
-	.ring_ib_execute = &evergreen_ring_ib_execute,
+	.ring = {
+		[RADEON_RING_TYPE_GFX_INDEX] = {
+			.ib_execute = &cayman_ring_ib_execute,
+			.ib_parse = &evergreen_ib_parse,
+			.emit_fence = &cayman_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		},
+		[CAYMAN_RING_TYPE_CP1_INDEX] = {
+			.ib_execute = &cayman_ring_ib_execute,
+			.ib_parse = &evergreen_ib_parse,
+			.emit_fence = &cayman_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		},
+		[CAYMAN_RING_TYPE_CP2_INDEX] = {
+			.ib_execute = &cayman_ring_ib_execute,
+			.ib_parse = &evergreen_ib_parse,
+			.emit_fence = &cayman_fence_ring_emit,
+			.emit_semaphore = &r600_semaphore_ring_emit,
+		}
+	},
 	.irq_set = &evergreen_irq_set,
 	.irq_process = &evergreen_irq_process,
 	.get_vblank_counter = &evergreen_get_vblank_counter,
-	.fence_ring_emit = &r600_fence_ring_emit,
 	.cs_parse = &evergreen_cs_parse,
 	.copy_blit = &r600_copy_blit,
 	.copy_dma = NULL,
@@ -945,6 +1036,9 @@ int radeon_asic_init(struct radeon_device *rdev)
 	else
 		rdev->num_crtc = 2;
 
+	/* set the ring used for bo copies */
+	rdev->copy_ring = RADEON_RING_TYPE_GFX_INDEX;
+
 	switch (rdev->family) {
 	case CHIP_R100:
 	case CHIP_RV100:
@@ -1050,6 +1144,7 @@ int radeon_asic_init(struct radeon_device *rdev)
 		rdev->asic = &cayman_asic;
 		/* set num crtcs */
 		rdev->num_crtc = 6;
+		rdev->vm_manager.funcs = &cayman_vm_funcs;
 		break;
 	default:
 		/* FIXME: not supported yet */
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index 59914842a729..6304aef0d9b2 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -58,17 +58,20 @@ void r100_fini(struct radeon_device *rdev);
 int r100_suspend(struct radeon_device *rdev);
 int r100_resume(struct radeon_device *rdev);
 void r100_vga_set_state(struct radeon_device *rdev, bool state);
-bool r100_gpu_is_lockup(struct radeon_device *rdev);
+bool r100_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 int r100_asic_reset(struct radeon_device *rdev);
 u32 r100_get_vblank_counter(struct radeon_device *rdev, int crtc);
 void r100_pci_gart_tlb_flush(struct radeon_device *rdev);
 int r100_pci_gart_set_page(struct radeon_device *rdev, int i, uint64_t addr);
-void r100_cp_commit(struct radeon_device *rdev);
 void r100_ring_start(struct radeon_device *rdev);
 int r100_irq_set(struct radeon_device *rdev);
 int r100_irq_process(struct radeon_device *rdev);
 void r100_fence_ring_emit(struct radeon_device *rdev,
 			  struct radeon_fence *fence);
+void r100_semaphore_ring_emit(struct radeon_device *rdev,
+			      struct radeon_ring *cp,
+			      struct radeon_semaphore *semaphore,
+			      bool emit_wait);
 int r100_cs_parse(struct radeon_cs_parser *p);
 void r100_pll_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v);
 uint32_t r100_pll_rreg(struct radeon_device *rdev, uint32_t reg);
@@ -83,7 +86,7 @@ int r100_set_surface_reg(struct radeon_device *rdev, int reg,
 void r100_clear_surface_reg(struct radeon_device *rdev, int reg);
 void r100_bandwidth_update(struct radeon_device *rdev);
 void r100_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
-int r100_ring_test(struct radeon_device *rdev);
+int r100_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
 void r100_hpd_init(struct radeon_device *rdev);
 void r100_hpd_fini(struct radeon_device *rdev);
 bool r100_hpd_sense(struct radeon_device *rdev, enum radeon_hpd_id hpd);
@@ -101,12 +104,12 @@ void r100_pci_gart_disable(struct radeon_device *rdev);
 int r100_debugfs_mc_info_init(struct radeon_device *rdev);
 int r100_gui_wait_for_idle(struct radeon_device *rdev);
 void r100_gpu_lockup_update(struct r100_gpu_lockup *lockup,
-			    struct radeon_cp *cp);
+			    struct radeon_ring *cp);
 bool r100_gpu_cp_is_lockup(struct radeon_device *rdev,
 			   struct r100_gpu_lockup *lockup,
-			   struct radeon_cp *cp);
+			   struct radeon_ring *cp);
 void r100_ib_fini(struct radeon_device *rdev);
-int r100_ib_init(struct radeon_device *rdev);
+int r100_ib_test(struct radeon_device *rdev);
 void r100_irq_disable(struct radeon_device *rdev);
 void r100_mc_stop(struct radeon_device *rdev, struct r100_mc_save *save);
 void r100_mc_resume(struct radeon_device *rdev, struct r100_mc_save *save);
@@ -154,7 +157,7 @@ extern int r300_init(struct radeon_device *rdev);
 extern void r300_fini(struct radeon_device *rdev);
 extern int r300_suspend(struct radeon_device *rdev);
 extern int r300_resume(struct radeon_device *rdev);
-extern bool r300_gpu_is_lockup(struct radeon_device *rdev);
+extern bool r300_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 extern int r300_asic_reset(struct radeon_device *rdev);
 extern void r300_ring_start(struct radeon_device *rdev);
 extern void r300_fence_ring_emit(struct radeon_device *rdev,
@@ -293,22 +296,25 @@ int r600_resume(struct radeon_device *rdev);
 void r600_vga_set_state(struct radeon_device *rdev, bool state);
 int r600_wb_init(struct radeon_device *rdev);
 void r600_wb_fini(struct radeon_device *rdev);
-void r600_cp_commit(struct radeon_device *rdev);
 void r600_pcie_gart_tlb_flush(struct radeon_device *rdev);
 uint32_t r600_pciep_rreg(struct radeon_device *rdev, uint32_t reg);
 void r600_pciep_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v);
 int r600_cs_parse(struct radeon_cs_parser *p);
 void r600_fence_ring_emit(struct radeon_device *rdev,
 			  struct radeon_fence *fence);
-bool r600_gpu_is_lockup(struct radeon_device *rdev);
+void r600_semaphore_ring_emit(struct radeon_device *rdev,
+			      struct radeon_ring *cp,
+			      struct radeon_semaphore *semaphore,
+			      bool emit_wait);
+bool r600_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 int r600_asic_reset(struct radeon_device *rdev);
 int r600_set_surface_reg(struct radeon_device *rdev, int reg,
 			 uint32_t tiling_flags, uint32_t pitch,
 			 uint32_t offset, uint32_t obj_size);
 void r600_clear_surface_reg(struct radeon_device *rdev, int reg);
-int r600_ib_test(struct radeon_device *rdev);
+int r600_ib_test(struct radeon_device *rdev, int ring);
 void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
-int r600_ring_test(struct radeon_device *rdev);
+int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
 int r600_copy_blit(struct radeon_device *rdev,
 		   uint64_t src_offset, uint64_t dst_offset,
 		   unsigned num_gpu_pages, struct radeon_fence *fence);
@@ -328,7 +334,7 @@ extern int r600_get_pcie_lanes(struct radeon_device *rdev);
 bool r600_card_posted(struct radeon_device *rdev);
 void r600_cp_stop(struct radeon_device *rdev);
 int r600_cp_start(struct radeon_device *rdev);
-void r600_ring_init(struct radeon_device *rdev, unsigned ring_size);
+void r600_ring_init(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ring_size);
 int r600_cp_resume(struct radeon_device *rdev);
 void r600_cp_fini(struct radeon_device *rdev);
 int r600_count_pipe_bits(uint32_t val);
@@ -397,7 +403,7 @@ int evergreen_init(struct radeon_device *rdev);
 void evergreen_fini(struct radeon_device *rdev);
 int evergreen_suspend(struct radeon_device *rdev);
 int evergreen_resume(struct radeon_device *rdev);
-bool evergreen_gpu_is_lockup(struct radeon_device *rdev);
+bool evergreen_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 int evergreen_asic_reset(struct radeon_device *rdev);
 void evergreen_bandwidth_update(struct radeon_device *rdev);
 void evergreen_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
@@ -423,12 +429,26 @@ int evergreen_blit_init(struct radeon_device *rdev);
 /*
  * cayman
  */
+void cayman_fence_ring_emit(struct radeon_device *rdev,
+			    struct radeon_fence *fence);
 void cayman_pcie_gart_tlb_flush(struct radeon_device *rdev);
 int cayman_init(struct radeon_device *rdev);
 void cayman_fini(struct radeon_device *rdev);
 int cayman_suspend(struct radeon_device *rdev);
 int cayman_resume(struct radeon_device *rdev);
-bool cayman_gpu_is_lockup(struct radeon_device *rdev);
+bool cayman_gpu_is_lockup(struct radeon_device *rdev, struct radeon_ring *cp);
 int cayman_asic_reset(struct radeon_device *rdev);
+void cayman_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
+int cayman_vm_init(struct radeon_device *rdev);
+void cayman_vm_fini(struct radeon_device *rdev);
+int cayman_vm_bind(struct radeon_device *rdev, struct radeon_vm *vm, int id);
+void cayman_vm_unbind(struct radeon_device *rdev, struct radeon_vm *vm);
+void cayman_vm_tlb_flush(struct radeon_device *rdev, struct radeon_vm *vm);
+uint32_t cayman_vm_page_flags(struct radeon_device *rdev,
+			      struct radeon_vm *vm,
+			      uint32_t flags);
+void cayman_vm_set_page(struct radeon_device *rdev, struct radeon_vm *vm,
+			unsigned pfn, uint64_t addr, uint32_t flags);
+int evergreen_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib);
 
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_benchmark.c b/drivers/gpu/drm/radeon/radeon_benchmark.c
index 17e1a9b2d8fb..815f2341ab94 100644
--- a/drivers/gpu/drm/radeon/radeon_benchmark.c
+++ b/drivers/gpu/drm/radeon/radeon_benchmark.c
@@ -43,7 +43,7 @@ static int radeon_benchmark_do_move(struct radeon_device *rdev, unsigned size,
 
 	start_jiffies = jiffies;
 	for (i = 0; i < n; i++) {
-		r = radeon_fence_create(rdev, &fence);
+		r = radeon_fence_create(rdev, &fence, RADEON_RING_TYPE_GFX_INDEX);
 		if (r)
 			return r;
 
@@ -229,21 +229,21 @@ void radeon_benchmark(struct radeon_device *rdev, int test_number)
 		break;
 	case 6:
 		/* GTT to VRAM, buffer size sweep, common modes */
-		for (i = 1; i < RADEON_BENCHMARK_COMMON_MODES_N; i++)
+		for (i = 0; i < RADEON_BENCHMARK_COMMON_MODES_N; i++)
 			radeon_benchmark_move(rdev, common_modes[i],
 					      RADEON_GEM_DOMAIN_GTT,
 					      RADEON_GEM_DOMAIN_VRAM);
 		break;
 	case 7:
 		/* VRAM to GTT, buffer size sweep, common modes */
-		for (i = 1; i < RADEON_BENCHMARK_COMMON_MODES_N; i++)
+		for (i = 0; i < RADEON_BENCHMARK_COMMON_MODES_N; i++)
 			radeon_benchmark_move(rdev, common_modes[i],
 					      RADEON_GEM_DOMAIN_VRAM,
 					      RADEON_GEM_DOMAIN_GTT);
 		break;
 	case 8:
 		/* VRAM to VRAM, buffer size sweep, common modes */
-		for (i = 1; i < RADEON_BENCHMARK_COMMON_MODES_N; i++)
+		for (i = 0; i < RADEON_BENCHMARK_COMMON_MODES_N; i++)
 			radeon_benchmark_move(rdev, common_modes[i],
 					      RADEON_GEM_DOMAIN_VRAM,
 					      RADEON_GEM_DOMAIN_VRAM);
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 29afd71e0840..435a3d970ab8 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -58,7 +58,7 @@ int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 
 		duplicate = false;
 		r = (struct drm_radeon_cs_reloc *)&chunk->kdata[i*4];
-		for (j = 0; j < p->nrelocs; j++) {
+		for (j = 0; j < i; j++) {
 			if (r->handle == p->relocs[j].handle) {
 				p->relocs_ptr[i] = &p->relocs[j];
 				duplicate = true;
@@ -84,16 +84,75 @@ int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 			p->relocs[i].flags = r->flags;
 			radeon_bo_list_add_object(&p->relocs[i].lobj,
 						  &p->validated);
-		}
+
+			if (p->relocs[i].robj->tbo.sync_obj && !(r->flags & RADEON_RELOC_DONT_SYNC)) {
+				struct radeon_fence *fence = p->relocs[i].robj->tbo.sync_obj;
+				if (!radeon_fence_signaled(fence)) {
+					p->sync_to_ring[fence->ring] = true;
+				}
+			}
+		} else
+			p->relocs[i].handle = 0;
 	}
 	return radeon_bo_list_validate(&p->validated);
 }
 
+static int radeon_cs_get_ring(struct radeon_cs_parser *p, u32 ring, s32 priority)
+{
+	p->priority = priority;
+
+	switch (ring) {
+	default:
+		DRM_ERROR("unknown ring id: %d\n", ring);
+		return -EINVAL;
+	case RADEON_CS_RING_GFX:
+		p->ring = RADEON_RING_TYPE_GFX_INDEX;
+		break;
+	case RADEON_CS_RING_COMPUTE:
+		/* for now */
+		p->ring = RADEON_RING_TYPE_GFX_INDEX;
+		break;
+	}
+	return 0;
+}
+
+static int radeon_cs_sync_rings(struct radeon_cs_parser *p)
+{
+	int i, r;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		/* no need to sync to our own or unused rings */
+		if (i == p->ring || !p->sync_to_ring[i] || !p->rdev->ring[i].ready)
+			continue;
+
+		if (!p->ib->fence->semaphore) {
+			r = radeon_semaphore_create(p->rdev, &p->ib->fence->semaphore);
+			if (r)
+				return r;
+		}
+
+		r = radeon_ring_lock(p->rdev, &p->rdev->ring[i], 3);
+		if (r)
+			return r;
+		radeon_semaphore_emit_signal(p->rdev, i, p->ib->fence->semaphore);
+		radeon_ring_unlock_commit(p->rdev, &p->rdev->ring[i]);
+
+		r = radeon_ring_lock(p->rdev, &p->rdev->ring[p->ring], 3);
+		if (r)
+			return r;
+		radeon_semaphore_emit_wait(p->rdev, p->ring, p->ib->fence->semaphore);
+		radeon_ring_unlock_commit(p->rdev, &p->rdev->ring[p->ring]);
+	}
+	return 0;
+}
+
 int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 {
 	struct drm_radeon_cs *cs = data;
 	uint64_t *chunk_array_ptr;
-	unsigned size, i, flags = 0;
+	unsigned size, i;
+	u32 ring = RADEON_CS_RING_GFX;
+	s32 priority = 0;
 
 	if (!cs->num_chunks) {
 		return 0;
@@ -103,6 +162,7 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 	p->idx = 0;
 	p->chunk_ib_idx = -1;
 	p->chunk_relocs_idx = -1;
+	p->chunk_flags_idx = -1;
 	p->chunks_array = kcalloc(cs->num_chunks, sizeof(uint64_t), GFP_KERNEL);
 	if (p->chunks_array == NULL) {
 		return -ENOMEM;
@@ -112,6 +172,7 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 			       sizeof(uint64_t)*cs->num_chunks)) {
 		return -EFAULT;
 	}
+	p->cs_flags = 0;
 	p->nchunks = cs->num_chunks;
 	p->chunks = kcalloc(p->nchunks, sizeof(struct radeon_cs_chunk), GFP_KERNEL);
 	if (p->chunks == NULL) {
@@ -140,16 +201,19 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 			if (p->chunks[i].length_dw == 0)
 				return -EINVAL;
 		}
-		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_FLAGS &&
-		    !p->chunks[i].length_dw) {
-			return -EINVAL;
+		if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_FLAGS) {
+			p->chunk_flags_idx = i;
+			/* zero length flags aren't useful */
+			if (p->chunks[i].length_dw == 0)
+				return -EINVAL;
 		}
 
 		p->chunks[i].length_dw = user_chunk.length_dw;
 		p->chunks[i].user_ptr = (void __user *)(unsigned long)user_chunk.chunk_data;
 
 		cdata = (uint32_t *)(unsigned long)user_chunk.chunk_data;
-		if (p->chunks[i].chunk_id != RADEON_CHUNK_ID_IB) {
+		if ((p->chunks[i].chunk_id == RADEON_CHUNK_ID_RELOCS) ||
+		    (p->chunks[i].chunk_id == RADEON_CHUNK_ID_FLAGS)) {
 			size = p->chunks[i].length_dw * sizeof(uint32_t);
 			p->chunks[i].kdata = kmalloc(size, GFP_KERNEL);
 			if (p->chunks[i].kdata == NULL) {
@@ -160,29 +224,58 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 				return -EFAULT;
 			}
 			if (p->chunks[i].chunk_id == RADEON_CHUNK_ID_FLAGS) {
-				flags = p->chunks[i].kdata[0];
+				p->cs_flags = p->chunks[i].kdata[0];
+				if (p->chunks[i].length_dw > 1)
+					ring = p->chunks[i].kdata[1];
+				if (p->chunks[i].length_dw > 2)
+					priority = (s32)p->chunks[i].kdata[2];
 			}
-		} else {
-			p->chunks[i].kpage[0] = kmalloc(PAGE_SIZE, GFP_KERNEL);
-			p->chunks[i].kpage[1] = kmalloc(PAGE_SIZE, GFP_KERNEL);
-			if (p->chunks[i].kpage[0] == NULL || p->chunks[i].kpage[1] == NULL) {
-				kfree(p->chunks[i].kpage[0]);
-				kfree(p->chunks[i].kpage[1]);
-				return -ENOMEM;
-			}
-			p->chunks[i].kpage_idx[0] = -1;
-			p->chunks[i].kpage_idx[1] = -1;
-			p->chunks[i].last_copied_page = -1;
-			p->chunks[i].last_page_index = ((p->chunks[i].length_dw * 4) - 1) / PAGE_SIZE;
 		}
 	}
-	if (p->chunks[p->chunk_ib_idx].length_dw > (16 * 1024)) {
-		DRM_ERROR("cs IB too big: %d\n",
-			  p->chunks[p->chunk_ib_idx].length_dw);
+
+	if ((p->cs_flags & RADEON_CS_USE_VM) &&
+	    !p->rdev->vm_manager.enabled) {
+		DRM_ERROR("VM not active on asic!\n");
+		if (p->chunk_relocs_idx != -1)
+			kfree(p->chunks[p->chunk_relocs_idx].kdata);
+		if (p->chunk_flags_idx != -1)
+			kfree(p->chunks[p->chunk_flags_idx].kdata);
 		return -EINVAL;
 	}
 
-	p->keep_tiling_flags = (flags & RADEON_CS_KEEP_TILING_FLAGS) != 0;
+	if (radeon_cs_get_ring(p, ring, priority)) {
+		if (p->chunk_relocs_idx != -1)
+			kfree(p->chunks[p->chunk_relocs_idx].kdata);
+		if (p->chunk_flags_idx != -1)
+			kfree(p->chunks[p->chunk_flags_idx].kdata);
+		return -EINVAL;
+	}
+
+
+	/* deal with non-vm */
+	if ((p->chunk_ib_idx != -1) &&
+	    ((p->cs_flags & RADEON_CS_USE_VM) == 0) &&
+	    (p->chunks[p->chunk_ib_idx].chunk_id == RADEON_CHUNK_ID_IB)) {
+		if (p->chunks[p->chunk_ib_idx].length_dw > (16 * 1024)) {
+			DRM_ERROR("cs IB too big: %d\n",
+				  p->chunks[p->chunk_ib_idx].length_dw);
+			return -EINVAL;
+		}
+		p->chunks[p->chunk_ib_idx].kpage[0] = kmalloc(PAGE_SIZE, GFP_KERNEL);
+		p->chunks[p->chunk_ib_idx].kpage[1] = kmalloc(PAGE_SIZE, GFP_KERNEL);
+		if (p->chunks[p->chunk_ib_idx].kpage[0] == NULL ||
+		    p->chunks[p->chunk_ib_idx].kpage[1] == NULL) {
+			kfree(p->chunks[p->chunk_ib_idx].kpage[0]);
+			kfree(p->chunks[p->chunk_ib_idx].kpage[1]);
+			return -ENOMEM;
+		}
+		p->chunks[p->chunk_ib_idx].kpage_idx[0] = -1;
+		p->chunks[p->chunk_ib_idx].kpage_idx[1] = -1;
+		p->chunks[p->chunk_ib_idx].last_copied_page = -1;
+		p->chunks[p->chunk_ib_idx].last_page_index =
+			((p->chunks[p->chunk_ib_idx].length_dw * 4) - 1) / PAGE_SIZE;
+	}
+
 	return 0;
 }
 
@@ -224,11 +317,139 @@ static void radeon_cs_parser_fini(struct radeon_cs_parser *parser, int error)
 	radeon_ib_free(parser->rdev, &parser->ib);
 }
 
+static int radeon_cs_ib_chunk(struct radeon_device *rdev,
+			      struct radeon_cs_parser *parser)
+{
+	struct radeon_cs_chunk *ib_chunk;
+	int r;
+
+	if (parser->chunk_ib_idx == -1)
+		return 0;
+
+	if (parser->cs_flags & RADEON_CS_USE_VM)
+		return 0;
+
+	ib_chunk = &parser->chunks[parser->chunk_ib_idx];
+	/* Copy the packet into the IB, the parser will read from the
+	 * input memory (cached) and write to the IB (which can be
+	 * uncached).
+	 */
+	r =  radeon_ib_get(rdev, parser->ring, &parser->ib,
+			   ib_chunk->length_dw * 4);
+	if (r) {
+		DRM_ERROR("Failed to get ib !\n");
+		return r;
+	}
+	parser->ib->length_dw = ib_chunk->length_dw;
+	r = radeon_cs_parse(parser);
+	if (r || parser->parser_error) {
+		DRM_ERROR("Invalid command stream !\n");
+		return r;
+	}
+	r = radeon_cs_finish_pages(parser);
+	if (r) {
+		DRM_ERROR("Invalid command stream !\n");
+		return r;
+	}
+	r = radeon_cs_sync_rings(parser);
+	if (r) {
+		DRM_ERROR("Failed to synchronize rings !\n");
+	}
+	parser->ib->vm_id = 0;
+	r = radeon_ib_schedule(rdev, parser->ib);
+	if (r) {
+		DRM_ERROR("Failed to schedule IB !\n");
+	}
+	return 0;
+}
+
+static int radeon_bo_vm_update_pte(struct radeon_cs_parser *parser,
+				   struct radeon_vm *vm)
+{
+	struct radeon_bo_list *lobj;
+	struct radeon_bo *bo;
+	int r;
+
+	list_for_each_entry(lobj, &parser->validated, tv.head) {
+		bo = lobj->bo;
+		r = radeon_vm_bo_update_pte(parser->rdev, vm, bo, &bo->tbo.mem);
+		if (r) {
+			return r;
+		}
+	}
+	return 0;
+}
+
+static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
+				 struct radeon_cs_parser *parser)
+{
+	struct radeon_cs_chunk *ib_chunk;
+	struct radeon_fpriv *fpriv = parser->filp->driver_priv;
+	struct radeon_vm *vm = &fpriv->vm;
+	int r;
+
+	if (parser->chunk_ib_idx == -1)
+		return 0;
+
+	if ((parser->cs_flags & RADEON_CS_USE_VM) == 0)
+		return 0;
+
+	ib_chunk = &parser->chunks[parser->chunk_ib_idx];
+	if (ib_chunk->length_dw > RADEON_IB_VM_MAX_SIZE) {
+		DRM_ERROR("cs IB too big: %d\n", ib_chunk->length_dw);
+		return -EINVAL;
+	}
+	r =  radeon_ib_get(rdev, parser->ring, &parser->ib,
+			   ib_chunk->length_dw * 4);
+	if (r) {
+		DRM_ERROR("Failed to get ib !\n");
+		return r;
+	}
+	parser->ib->length_dw = ib_chunk->length_dw;
+	/* Copy the packet into the IB */
+	if (DRM_COPY_FROM_USER(parser->ib->ptr, ib_chunk->user_ptr,
+			       ib_chunk->length_dw * 4)) {
+		return -EFAULT;
+	}
+	r = radeon_ring_ib_parse(rdev, parser->ring, parser->ib);
+	if (r) {
+		return r;
+	}
+
+	mutex_lock(&vm->mutex);
+	r = radeon_vm_bind(rdev, vm);
+	if (r) {
+		goto out;
+	}
+	r = radeon_bo_vm_update_pte(parser, vm);
+	if (r) {
+		goto out;
+	}
+	r = radeon_cs_sync_rings(parser);
+	if (r) {
+		DRM_ERROR("Failed to synchronize rings !\n");
+	}
+	parser->ib->vm_id = vm->id;
+	/* ib pool is bind at 0 in virtual address space to gpu_addr is the
+	 * offset inside the pool bo
+	 */
+	parser->ib->gpu_addr = parser->ib->sa_bo.offset;
+	r = radeon_ib_schedule(rdev, parser->ib);
+out:
+	if (!r) {
+		if (vm->fence) {
+			radeon_fence_unref(&vm->fence);
+		}
+		vm->fence = radeon_fence_ref(parser->ib->fence);
+	}
+	mutex_unlock(&fpriv->vm.mutex);
+	return r;
+}
+
 int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 {
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_cs_parser parser;
-	struct radeon_cs_chunk *ib_chunk;
 	int r;
 
 	radeon_mutex_lock(&rdev->cs_mutex);
@@ -245,13 +466,6 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		radeon_mutex_unlock(&rdev->cs_mutex);
 		return r;
 	}
-	r =  radeon_ib_get(rdev, &parser.ib);
-	if (r) {
-		DRM_ERROR("Failed to get ib !\n");
-		radeon_cs_parser_fini(&parser, r);
-		radeon_mutex_unlock(&rdev->cs_mutex);
-		return r;
-	}
 	r = radeon_cs_parser_relocs(&parser);
 	if (r) {
 		if (r != -ERESTARTSYS)
@@ -260,29 +474,15 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		radeon_mutex_unlock(&rdev->cs_mutex);
 		return r;
 	}
-	/* Copy the packet into the IB, the parser will read from the
-	 * input memory (cached) and write to the IB (which can be
-	 * uncached). */
-	ib_chunk = &parser.chunks[parser.chunk_ib_idx];
-	parser.ib->length_dw = ib_chunk->length_dw;
-	r = radeon_cs_parse(&parser);
-	if (r || parser.parser_error) {
-		DRM_ERROR("Invalid command stream !\n");
-		radeon_cs_parser_fini(&parser, r);
-		radeon_mutex_unlock(&rdev->cs_mutex);
-		return r;
-	}
-	r = radeon_cs_finish_pages(&parser);
+	r = radeon_cs_ib_chunk(rdev, &parser);
 	if (r) {
-		DRM_ERROR("Invalid command stream !\n");
-		radeon_cs_parser_fini(&parser, r);
-		radeon_mutex_unlock(&rdev->cs_mutex);
-		return r;
+		goto out;
 	}
-	r = radeon_ib_schedule(rdev, parser.ib);
+	r = radeon_cs_ib_vm_chunk(rdev, &parser);
 	if (r) {
-		DRM_ERROR("Failed to schedule IB !\n");
+		goto out;
 	}
+out:
 	radeon_cs_parser_fini(&parser, r);
 	radeon_mutex_unlock(&rdev->cs_mutex);
 	return r;
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index c4d00a171411..0afb13bd8dca 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -224,8 +224,11 @@ int radeon_wb_init(struct radeon_device *rdev)
 	if (radeon_no_wb == 1)
 		rdev->wb.enabled = false;
 	else {
-		/* often unreliable on AGP */
 		if (rdev->flags & RADEON_IS_AGP) {
+			/* often unreliable on AGP */
+			rdev->wb.enabled = false;
+		} else if (rdev->family < CHIP_R300) {
+			/* often unreliable on pre-r300 */
 			rdev->wb.enabled = false;
 		} else {
 			rdev->wb.enabled = true;
@@ -718,17 +721,24 @@ int radeon_device_init(struct radeon_device *rdev,
 	 * can recall function without having locking issues */
 	radeon_mutex_init(&rdev->cs_mutex);
 	mutex_init(&rdev->ib_pool.mutex);
-	mutex_init(&rdev->cp.mutex);
+	for (i = 0; i < RADEON_NUM_RINGS; ++i)
+		mutex_init(&rdev->ring[i].mutex);
 	mutex_init(&rdev->dc_hw_i2c_mutex);
 	if (rdev->family >= CHIP_R600)
 		spin_lock_init(&rdev->ih.lock);
 	mutex_init(&rdev->gem.mutex);
 	mutex_init(&rdev->pm.mutex);
 	mutex_init(&rdev->vram_mutex);
-	rwlock_init(&rdev->fence_drv.lock);
+	rwlock_init(&rdev->fence_lock);
+	rwlock_init(&rdev->semaphore_drv.lock);
 	INIT_LIST_HEAD(&rdev->gem.objects);
 	init_waitqueue_head(&rdev->irq.vblank_queue);
 	init_waitqueue_head(&rdev->irq.idle_queue);
+	INIT_LIST_HEAD(&rdev->semaphore_drv.bo);
+	/* initialize vm here */
+	rdev->vm_manager.use_bitmap = 1;
+	rdev->vm_manager.max_pfn = 1 << 20;
+	INIT_LIST_HEAD(&rdev->vm_manager.lru_vm);
 
 	/* Set asic functions */
 	r = radeon_asic_init(rdev);
@@ -765,8 +775,14 @@ int radeon_device_init(struct radeon_device *rdev,
 	r = pci_set_dma_mask(rdev->pdev, DMA_BIT_MASK(dma_bits));
 	if (r) {
 		rdev->need_dma32 = true;
+		dma_bits = 32;
 		printk(KERN_WARNING "radeon: No suitable DMA available.\n");
 	}
+	r = pci_set_consistent_dma_mask(rdev->pdev, DMA_BIT_MASK(dma_bits));
+	if (r) {
+		pci_set_consistent_dma_mask(rdev->pdev, DMA_BIT_MASK(32));
+		printk(KERN_WARNING "radeon: No coherent DMA available.\n");
+	}
 
 	/* Registers mapping */
 	/* TODO: block userspace mapping of io register */
@@ -814,15 +830,20 @@ int radeon_device_init(struct radeon_device *rdev,
 		if (r)
 			return r;
 	}
-	if (radeon_testing) {
+	if ((radeon_testing & 1)) {
 		radeon_test_moves(rdev);
 	}
+	if ((radeon_testing & 2)) {
+		radeon_test_syncing(rdev);
+	}
 	if (radeon_benchmarking) {
 		radeon_benchmark(rdev, radeon_benchmarking);
 	}
 	return 0;
 }
 
+static void radeon_debugfs_remove_files(struct radeon_device *rdev);
+
 void radeon_device_fini(struct radeon_device *rdev)
 {
 	DRM_INFO("radeon: finishing device.\n");
@@ -837,6 +858,7 @@ void radeon_device_fini(struct radeon_device *rdev)
 	rdev->rio_mem = NULL;
 	iounmap(rdev->rmmio);
 	rdev->rmmio = NULL;
+	radeon_debugfs_remove_files(rdev);
 }
 
 
@@ -848,7 +870,7 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 	struct radeon_device *rdev;
 	struct drm_crtc *crtc;
 	struct drm_connector *connector;
-	int r;
+	int i, r;
 
 	if (dev == NULL || dev->dev_private == NULL) {
 		return -ENODEV;
@@ -887,7 +909,8 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 	/* evict vram memory */
 	radeon_bo_evict_vram(rdev);
 	/* wait for gpu to finish processing current batch */
-	radeon_fence_wait_last(rdev);
+	for (i = 0; i < RADEON_NUM_RINGS; i++)
+		radeon_fence_wait_last(rdev, i);
 
 	radeon_save_bios_scratch_regs(rdev);
 
@@ -986,36 +1009,29 @@ int radeon_gpu_reset(struct radeon_device *rdev)
 /*
  * Debugfs
  */
-struct radeon_debugfs {
-	struct drm_info_list	*files;
-	unsigned		num_files;
-};
-static struct radeon_debugfs _radeon_debugfs[RADEON_DEBUGFS_MAX_COMPONENTS];
-static unsigned _radeon_debugfs_count = 0;
-
 int radeon_debugfs_add_files(struct radeon_device *rdev,
 			     struct drm_info_list *files,
 			     unsigned nfiles)
 {
 	unsigned i;
 
-	for (i = 0; i < _radeon_debugfs_count; i++) {
-		if (_radeon_debugfs[i].files == files) {
+	for (i = 0; i < rdev->debugfs_count; i++) {
+		if (rdev->debugfs[i].files == files) {
 			/* Already registered */
 			return 0;
 		}
 	}
 
-	i = _radeon_debugfs_count + 1;
+	i = rdev->debugfs_count + 1;
 	if (i > RADEON_DEBUGFS_MAX_COMPONENTS) {
 		DRM_ERROR("Reached maximum number of debugfs components.\n");
 		DRM_ERROR("Report so we increase "
 		          "RADEON_DEBUGFS_MAX_COMPONENTS.\n");
 		return -EINVAL;
 	}
-	_radeon_debugfs[_radeon_debugfs_count].files = files;
-	_radeon_debugfs[_radeon_debugfs_count].num_files = nfiles;
-	_radeon_debugfs_count = i;
+	rdev->debugfs[rdev->debugfs_count].files = files;
+	rdev->debugfs[rdev->debugfs_count].num_files = nfiles;
+	rdev->debugfs_count = i;
 #if defined(CONFIG_DEBUG_FS)
 	drm_debugfs_create_files(files, nfiles,
 				 rdev->ddev->control->debugfs_root,
@@ -1027,6 +1043,22 @@ int radeon_debugfs_add_files(struct radeon_device *rdev,
 	return 0;
 }
 
+static void radeon_debugfs_remove_files(struct radeon_device *rdev)
+{
+#if defined(CONFIG_DEBUG_FS)
+	unsigned i;
+
+	for (i = 0; i < rdev->debugfs_count; i++) {
+		drm_debugfs_remove_files(rdev->debugfs[i].files,
+					 rdev->debugfs[i].num_files,
+					 rdev->ddev->control);
+		drm_debugfs_remove_files(rdev->debugfs[i].files,
+					 rdev->debugfs[i].num_files,
+					 rdev->ddev->primary);
+	}
+#endif
+}
+
 #if defined(CONFIG_DEBUG_FS)
 int radeon_debugfs_init(struct drm_minor *minor)
 {
@@ -1035,11 +1067,5 @@ int radeon_debugfs_init(struct drm_minor *minor)
 
 void radeon_debugfs_cleanup(struct drm_minor *minor)
 {
-	unsigned i;
-
-	for (i = 0; i < _radeon_debugfs_count; i++) {
-		drm_debugfs_remove_files(_radeon_debugfs[i].files,
-					 _radeon_debugfs[i].num_files, minor);
-	}
 }
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index a22d6e6a49a2..d3ffc18774a6 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -406,7 +406,7 @@ static int radeon_crtc_page_flip(struct drm_crtc *crtc,
 	if (!ASIC_IS_AVIVO(rdev)) {
 		/* crtc offset is from display base addr not FB location */
 		base -= radeon_crtc->legacy_display_base_addr;
-		pitch_pixels = fb->pitch / (fb->bits_per_pixel / 8);
+		pitch_pixels = fb->pitches[0] / (fb->bits_per_pixel / 8);
 
 		if (tiling_flags & RADEON_TILING_MACRO) {
 			if (ASIC_IS_R300(rdev)) {
@@ -1081,7 +1081,7 @@ static const struct drm_framebuffer_funcs radeon_fb_funcs = {
 void
 radeon_framebuffer_init(struct drm_device *dev,
 			struct radeon_framebuffer *rfb,
-			struct drm_mode_fb_cmd *mode_cmd,
+			struct drm_mode_fb_cmd2 *mode_cmd,
 			struct drm_gem_object *obj)
 {
 	rfb->obj = obj;
@@ -1092,15 +1092,15 @@ radeon_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 radeon_user_framebuffer_create(struct drm_device *dev,
 			       struct drm_file *file_priv,
-			       struct drm_mode_fb_cmd *mode_cmd)
+			       struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj;
 	struct radeon_framebuffer *radeon_fb;
 
-	obj = drm_gem_object_lookup(dev, file_priv, mode_cmd->handle);
+	obj = drm_gem_object_lookup(dev, file_priv, mode_cmd->handles[0]);
 	if (obj ==  NULL) {
 		dev_err(&dev->pdev->dev, "No GEM object associated to handle 0x%08X, "
-			"can't create framebuffer\n", mode_cmd->handle);
+			"can't create framebuffer\n", mode_cmd->handles[0]);
 		return ERR_PTR(-ENOENT);
 	}
 
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index 71499fc3daf5..31da622eef63 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -54,9 +54,10 @@
  *   2.10.0 - fusion 2D tiling
  *   2.11.0 - backend map, initial compute support for the CS checker
  *   2.12.0 - RADEON_CS_KEEP_TILING_FLAGS
+ *   2.13.0 - virtual memory support
  */
 #define KMS_DRIVER_MAJOR	2
-#define KMS_DRIVER_MINOR	12
+#define KMS_DRIVER_MINOR	13
 #define KMS_DRIVER_PATCHLEVEL	0
 int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags);
 int radeon_driver_unload_kms(struct drm_device *dev);
@@ -84,6 +85,10 @@ int radeon_dma_ioctl_kms(struct drm_device *dev, void *data,
 			 struct drm_file *file_priv);
 int radeon_gem_object_init(struct drm_gem_object *obj);
 void radeon_gem_object_free(struct drm_gem_object *obj);
+int radeon_gem_object_open(struct drm_gem_object *obj,
+				struct drm_file *file_priv);
+void radeon_gem_object_close(struct drm_gem_object *obj,
+				struct drm_file *file_priv);
 extern int radeon_get_crtc_scanoutpos(struct drm_device *dev, int crtc,
 				      int *vpos, int *hpos);
 extern struct drm_ioctl_desc radeon_ioctls_kms[];
@@ -206,6 +211,21 @@ static struct pci_device_id pciidlist[] = {
 MODULE_DEVICE_TABLE(pci, pciidlist);
 #endif
 
+static const struct file_operations radeon_driver_old_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = radeon_compat_ioctl,
+#endif
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver_old = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_USE_MTRR | DRIVER_PCI_DMA | DRIVER_SG |
@@ -232,21 +252,7 @@ static struct drm_driver driver_old = {
 	.reclaim_buffers = drm_core_reclaim_buffers,
 	.ioctls = radeon_ioctls,
 	.dma_ioctl = radeon_cp_buffers,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = drm_ioctl,
-		 .mmap = drm_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .read = drm_read,
-#ifdef CONFIG_COMPAT
-		 .compat_ioctl = radeon_compat_ioctl,
-#endif
-		 .llseek = noop_llseek,
-	},
-
+	.fops = &radeon_driver_old_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
@@ -304,6 +310,20 @@ radeon_pci_resume(struct pci_dev *pdev)
 	return radeon_resume_kms(dev);
 }
 
+static const struct file_operations radeon_driver_kms_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = radeon_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = radeon_kms_compat_ioctl,
+#endif
+};
+
 static struct drm_driver kms_driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_USE_MTRR | DRIVER_PCI_DMA | DRIVER_SG |
@@ -335,24 +355,13 @@ static struct drm_driver kms_driver = {
 	.ioctls = radeon_ioctls_kms,
 	.gem_init_object = radeon_gem_object_init,
 	.gem_free_object = radeon_gem_object_free,
+	.gem_open_object = radeon_gem_object_open,
+	.gem_close_object = radeon_gem_object_close,
 	.dma_ioctl = radeon_dma_ioctl_kms,
 	.dumb_create = radeon_mode_dumb_create,
 	.dumb_map_offset = radeon_mode_dumb_mmap,
 	.dumb_destroy = radeon_mode_dumb_destroy,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = drm_ioctl,
-		 .mmap = radeon_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .read = drm_read,
-#ifdef CONFIG_COMPAT
-		 .compat_ioctl = radeon_kms_compat_ioctl,
-#endif
-	},
-
+	.fops = &radeon_driver_kms_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/radeon/radeon_fb.c b/drivers/gpu/drm/radeon/radeon_fb.c
index 0b7b486c97e8..cf2bf35b56b8 100644
--- a/drivers/gpu/drm/radeon/radeon_fb.c
+++ b/drivers/gpu/drm/radeon/radeon_fb.c
@@ -103,7 +103,7 @@ static void radeonfb_destroy_pinned_object(struct drm_gem_object *gobj)
 }
 
 static int radeonfb_create_pinned_object(struct radeon_fbdev *rfbdev,
-					 struct drm_mode_fb_cmd *mode_cmd,
+					 struct drm_mode_fb_cmd2 *mode_cmd,
 					 struct drm_gem_object **gobj_p)
 {
 	struct radeon_device *rdev = rfbdev->rdev;
@@ -114,13 +114,17 @@ static int radeonfb_create_pinned_object(struct radeon_fbdev *rfbdev,
 	int ret;
 	int aligned_size, size;
 	int height = mode_cmd->height;
+	u32 bpp, depth;
+
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
 
 	/* need to align pitch with crtc limits */
-	mode_cmd->pitch = radeon_align_pitch(rdev, mode_cmd->width, mode_cmd->bpp, fb_tiled) * ((mode_cmd->bpp + 1) / 8);
+	mode_cmd->pitches[0] = radeon_align_pitch(rdev, mode_cmd->width, bpp,
+						  fb_tiled) * ((bpp + 1) / 8);
 
 	if (rdev->family >= CHIP_R600)
 		height = ALIGN(mode_cmd->height, 8);
-	size = mode_cmd->pitch * height;
+	size = mode_cmd->pitches[0] * height;
 	aligned_size = ALIGN(size, PAGE_SIZE);
 	ret = radeon_gem_object_create(rdev, aligned_size, 0,
 				       RADEON_GEM_DOMAIN_VRAM,
@@ -137,7 +141,7 @@ static int radeonfb_create_pinned_object(struct radeon_fbdev *rfbdev,
 		tiling_flags = RADEON_TILING_MACRO;
 
 #ifdef __BIG_ENDIAN
-	switch (mode_cmd->bpp) {
+	switch (bpp) {
 	case 32:
 		tiling_flags |= RADEON_TILING_SWAP_32BIT;
 		break;
@@ -151,7 +155,7 @@ static int radeonfb_create_pinned_object(struct radeon_fbdev *rfbdev,
 	if (tiling_flags) {
 		ret = radeon_bo_set_tiling_flags(rbo,
 						 tiling_flags | RADEON_TILING_SURFACE,
-						 mode_cmd->pitch);
+						 mode_cmd->pitches[0]);
 		if (ret)
 			dev_err(rdev->dev, "FB failed to set tiling flags\n");
 	}
@@ -187,7 +191,7 @@ static int radeonfb_create(struct radeon_fbdev *rfbdev,
 	struct radeon_device *rdev = rfbdev->rdev;
 	struct fb_info *info;
 	struct drm_framebuffer *fb = NULL;
-	struct drm_mode_fb_cmd mode_cmd;
+	struct drm_mode_fb_cmd2 mode_cmd;
 	struct drm_gem_object *gobj = NULL;
 	struct radeon_bo *rbo = NULL;
 	struct device *device = &rdev->pdev->dev;
@@ -201,8 +205,8 @@ static int radeonfb_create(struct radeon_fbdev *rfbdev,
 	if ((sizes->surface_bpp == 24) && ASIC_IS_AVIVO(rdev))
 		sizes->surface_bpp = 32;
 
-	mode_cmd.bpp = sizes->surface_bpp;
-	mode_cmd.depth = sizes->surface_depth;
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
+							  sizes->surface_depth);
 
 	ret = radeonfb_create_pinned_object(rfbdev, &mode_cmd, &gobj);
 	rbo = gem_to_radeon_bo(gobj);
@@ -228,7 +232,7 @@ static int radeonfb_create(struct radeon_fbdev *rfbdev,
 
 	strcpy(info->fix.id, "radeondrmfb");
 
-	drm_fb_helper_fill_fix(info, fb->pitch, fb->depth);
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
 
 	info->flags = FBINFO_DEFAULT | FBINFO_CAN_FORCE_OUTPUT;
 	info->fbops = &radeonfb_ops;
@@ -271,7 +275,7 @@ static int radeonfb_create(struct radeon_fbdev *rfbdev,
 	DRM_INFO("vram apper at 0x%lX\n",  (unsigned long)rdev->mc.aper_base);
 	DRM_INFO("size %lu\n", (unsigned long)radeon_bo_size(rbo));
 	DRM_INFO("fb depth is %d\n", fb->depth);
-	DRM_INFO("   pitch is %d\n", fb->pitch);
+	DRM_INFO("   pitch is %d\n", fb->pitches[0]);
 
 	vga_switcheroo_client_fb_set(rdev->ddev->pdev, info);
 	return 0;
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index 76ec0e9ed8ae..64ea3dd9e6ff 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -40,32 +40,24 @@
 #include "radeon.h"
 #include "radeon_trace.h"
 
-static void radeon_fence_write(struct radeon_device *rdev, u32 seq)
+static void radeon_fence_write(struct radeon_device *rdev, u32 seq, int ring)
 {
 	if (rdev->wb.enabled) {
-		u32 scratch_index;
-		if (rdev->wb.use_event)
-			scratch_index = R600_WB_EVENT_OFFSET + rdev->fence_drv.scratch_reg - rdev->scratch.reg_base;
-		else
-			scratch_index = RADEON_WB_SCRATCH_OFFSET + rdev->fence_drv.scratch_reg - rdev->scratch.reg_base;
-		rdev->wb.wb[scratch_index/4] = cpu_to_le32(seq);
-	} else
-		WREG32(rdev->fence_drv.scratch_reg, seq);
+		*rdev->fence_drv[ring].cpu_addr = cpu_to_le32(seq);
+	} else {
+		WREG32(rdev->fence_drv[ring].scratch_reg, seq);
+	}
 }
 
-static u32 radeon_fence_read(struct radeon_device *rdev)
+static u32 radeon_fence_read(struct radeon_device *rdev, int ring)
 {
-	u32 seq;
+	u32 seq = 0;
 
 	if (rdev->wb.enabled) {
-		u32 scratch_index;
-		if (rdev->wb.use_event)
-			scratch_index = R600_WB_EVENT_OFFSET + rdev->fence_drv.scratch_reg - rdev->scratch.reg_base;
-		else
-			scratch_index = RADEON_WB_SCRATCH_OFFSET + rdev->fence_drv.scratch_reg - rdev->scratch.reg_base;
-		seq = le32_to_cpu(rdev->wb.wb[scratch_index/4]);
-	} else
-		seq = RREG32(rdev->fence_drv.scratch_reg);
+		seq = le32_to_cpu(*rdev->fence_drv[ring].cpu_addr);
+	} else {
+		seq = RREG32(rdev->fence_drv[ring].scratch_reg);
+	}
 	return seq;
 }
 
@@ -73,28 +65,28 @@ int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence *fence)
 {
 	unsigned long irq_flags;
 
-	write_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-	if (fence->emited) {
-		write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	if (fence->emitted) {
+		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 		return 0;
 	}
-	fence->seq = atomic_add_return(1, &rdev->fence_drv.seq);
-	if (!rdev->cp.ready)
+	fence->seq = atomic_add_return(1, &rdev->fence_drv[fence->ring].seq);
+	if (!rdev->ring[fence->ring].ready)
 		/* FIXME: cp is not running assume everythings is done right
 		 * away
 		 */
-		radeon_fence_write(rdev, fence->seq);
+		radeon_fence_write(rdev, fence->seq, fence->ring);
 	else
-		radeon_fence_ring_emit(rdev, fence);
+		radeon_fence_ring_emit(rdev, fence->ring, fence);
 
 	trace_radeon_fence_emit(rdev->ddev, fence->seq);
-	fence->emited = true;
-	list_move_tail(&fence->list, &rdev->fence_drv.emited);
-	write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	fence->emitted = true;
+	list_move_tail(&fence->list, &rdev->fence_drv[fence->ring].emitted);
+	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	return 0;
 }
 
-static bool radeon_fence_poll_locked(struct radeon_device *rdev)
+static bool radeon_fence_poll_locked(struct radeon_device *rdev, int ring)
 {
 	struct radeon_fence *fence;
 	struct list_head *i, *n;
@@ -102,34 +94,34 @@ static bool radeon_fence_poll_locked(struct radeon_device *rdev)
 	bool wake = false;
 	unsigned long cjiffies;
 
-	seq = radeon_fence_read(rdev);
-	if (seq != rdev->fence_drv.last_seq) {
-		rdev->fence_drv.last_seq = seq;
-		rdev->fence_drv.last_jiffies = jiffies;
-		rdev->fence_drv.last_timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
+	seq = radeon_fence_read(rdev, ring);
+	if (seq != rdev->fence_drv[ring].last_seq) {
+		rdev->fence_drv[ring].last_seq = seq;
+		rdev->fence_drv[ring].last_jiffies = jiffies;
+		rdev->fence_drv[ring].last_timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
 	} else {
 		cjiffies = jiffies;
-		if (time_after(cjiffies, rdev->fence_drv.last_jiffies)) {
-			cjiffies -= rdev->fence_drv.last_jiffies;
-			if (time_after(rdev->fence_drv.last_timeout, cjiffies)) {
+		if (time_after(cjiffies, rdev->fence_drv[ring].last_jiffies)) {
+			cjiffies -= rdev->fence_drv[ring].last_jiffies;
+			if (time_after(rdev->fence_drv[ring].last_timeout, cjiffies)) {
 				/* update the timeout */
-				rdev->fence_drv.last_timeout -= cjiffies;
+				rdev->fence_drv[ring].last_timeout -= cjiffies;
 			} else {
 				/* the 500ms timeout is elapsed we should test
 				 * for GPU lockup
 				 */
-				rdev->fence_drv.last_timeout = 1;
+				rdev->fence_drv[ring].last_timeout = 1;
 			}
 		} else {
 			/* wrap around update last jiffies, we will just wait
 			 * a little longer
 			 */
-			rdev->fence_drv.last_jiffies = cjiffies;
+			rdev->fence_drv[ring].last_jiffies = cjiffies;
 		}
 		return false;
 	}
 	n = NULL;
-	list_for_each(i, &rdev->fence_drv.emited) {
+	list_for_each(i, &rdev->fence_drv[ring].emitted) {
 		fence = list_entry(i, struct radeon_fence, list);
 		if (fence->seq == seq) {
 			n = i;
@@ -141,11 +133,11 @@ static bool radeon_fence_poll_locked(struct radeon_device *rdev)
 		i = n;
 		do {
 			n = i->prev;
-			list_move_tail(i, &rdev->fence_drv.signaled);
+			list_move_tail(i, &rdev->fence_drv[ring].signaled);
 			fence = list_entry(i, struct radeon_fence, list);
 			fence->signaled = true;
 			i = n;
-		} while (i != &rdev->fence_drv.emited);
+		} while (i != &rdev->fence_drv[ring].emitted);
 		wake = true;
 	}
 	return wake;
@@ -157,14 +149,18 @@ static void radeon_fence_destroy(struct kref *kref)
         struct radeon_fence *fence;
 
 	fence = container_of(kref, struct radeon_fence, kref);
-	write_lock_irqsave(&fence->rdev->fence_drv.lock, irq_flags);
+	write_lock_irqsave(&fence->rdev->fence_lock, irq_flags);
 	list_del(&fence->list);
-	fence->emited = false;
-	write_unlock_irqrestore(&fence->rdev->fence_drv.lock, irq_flags);
+	fence->emitted = false;
+	write_unlock_irqrestore(&fence->rdev->fence_lock, irq_flags);
+	if (fence->semaphore)
+		radeon_semaphore_free(fence->rdev, fence->semaphore);
 	kfree(fence);
 }
 
-int radeon_fence_create(struct radeon_device *rdev, struct radeon_fence **fence)
+int radeon_fence_create(struct radeon_device *rdev,
+			struct radeon_fence **fence,
+			int ring)
 {
 	unsigned long irq_flags;
 
@@ -174,18 +170,19 @@ int radeon_fence_create(struct radeon_device *rdev, struct radeon_fence **fence)
 	}
 	kref_init(&((*fence)->kref));
 	(*fence)->rdev = rdev;
-	(*fence)->emited = false;
+	(*fence)->emitted = false;
 	(*fence)->signaled = false;
 	(*fence)->seq = 0;
+	(*fence)->ring = ring;
+	(*fence)->semaphore = NULL;
 	INIT_LIST_HEAD(&(*fence)->list);
 
-	write_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-	list_add_tail(&(*fence)->list, &rdev->fence_drv.created);
-	write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	list_add_tail(&(*fence)->list, &rdev->fence_drv[ring].created);
+	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	return 0;
 }
 
-
 bool radeon_fence_signaled(struct radeon_fence *fence)
 {
 	unsigned long irq_flags;
@@ -197,21 +194,21 @@ bool radeon_fence_signaled(struct radeon_fence *fence)
 	if (fence->rdev->gpu_lockup)
 		return true;
 
-	write_lock_irqsave(&fence->rdev->fence_drv.lock, irq_flags);
+	write_lock_irqsave(&fence->rdev->fence_lock, irq_flags);
 	signaled = fence->signaled;
 	/* if we are shuting down report all fence as signaled */
 	if (fence->rdev->shutdown) {
 		signaled = true;
 	}
-	if (!fence->emited) {
-		WARN(1, "Querying an unemited fence : %p !\n", fence);
+	if (!fence->emitted) {
+		WARN(1, "Querying an unemitted fence : %p !\n", fence);
 		signaled = true;
 	}
 	if (!signaled) {
-		radeon_fence_poll_locked(fence->rdev);
+		radeon_fence_poll_locked(fence->rdev, fence->ring);
 		signaled = fence->signaled;
 	}
-	write_unlock_irqrestore(&fence->rdev->fence_drv.lock, irq_flags);
+	write_unlock_irqrestore(&fence->rdev->fence_lock, irq_flags);
 	return signaled;
 }
 
@@ -230,24 +227,24 @@ int radeon_fence_wait(struct radeon_fence *fence, bool intr)
 	if (radeon_fence_signaled(fence)) {
 		return 0;
 	}
-	timeout = rdev->fence_drv.last_timeout;
+	timeout = rdev->fence_drv[fence->ring].last_timeout;
 retry:
 	/* save current sequence used to check for GPU lockup */
-	seq = rdev->fence_drv.last_seq;
+	seq = rdev->fence_drv[fence->ring].last_seq;
 	trace_radeon_fence_wait_begin(rdev->ddev, seq);
 	if (intr) {
-		radeon_irq_kms_sw_irq_get(rdev);
-		r = wait_event_interruptible_timeout(rdev->fence_drv.queue,
+		radeon_irq_kms_sw_irq_get(rdev, fence->ring);
+		r = wait_event_interruptible_timeout(rdev->fence_drv[fence->ring].queue,
 				radeon_fence_signaled(fence), timeout);
-		radeon_irq_kms_sw_irq_put(rdev);
+		radeon_irq_kms_sw_irq_put(rdev, fence->ring);
 		if (unlikely(r < 0)) {
 			return r;
 		}
 	} else {
-		radeon_irq_kms_sw_irq_get(rdev);
-		r = wait_event_timeout(rdev->fence_drv.queue,
+		radeon_irq_kms_sw_irq_get(rdev, fence->ring);
+		r = wait_event_timeout(rdev->fence_drv[fence->ring].queue,
 			 radeon_fence_signaled(fence), timeout);
-		radeon_irq_kms_sw_irq_put(rdev);
+		radeon_irq_kms_sw_irq_put(rdev, fence->ring);
 	}
 	trace_radeon_fence_wait_end(rdev->ddev, seq);
 	if (unlikely(!radeon_fence_signaled(fence))) {
@@ -258,10 +255,11 @@ retry:
 			timeout = r;
 			goto retry;
 		}
-		/* don't protect read access to rdev->fence_drv.last_seq
+		/* don't protect read access to rdev->fence_drv[t].last_seq
 		 * if we experiencing a lockup the value doesn't change
 		 */
-		if (seq == rdev->fence_drv.last_seq && radeon_gpu_is_lockup(rdev)) {
+		if (seq == rdev->fence_drv[fence->ring].last_seq &&
+		    radeon_gpu_is_lockup(rdev, &rdev->ring[fence->ring])) {
 			/* good news we believe it's a lockup */
 			printk(KERN_WARNING "GPU lockup (waiting for 0x%08X last fence id 0x%08X)\n",
 			     fence->seq, seq);
@@ -272,20 +270,20 @@ retry:
 			r = radeon_gpu_reset(rdev);
 			if (r)
 				return r;
-			radeon_fence_write(rdev, fence->seq);
+			radeon_fence_write(rdev, fence->seq, fence->ring);
 			rdev->gpu_lockup = false;
 		}
 		timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
-		write_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-		rdev->fence_drv.last_timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
-		rdev->fence_drv.last_jiffies = jiffies;
-		write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+		write_lock_irqsave(&rdev->fence_lock, irq_flags);
+		rdev->fence_drv[fence->ring].last_timeout = RADEON_FENCE_JIFFIES_TIMEOUT;
+		rdev->fence_drv[fence->ring].last_jiffies = jiffies;
+		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 		goto retry;
 	}
 	return 0;
 }
 
-int radeon_fence_wait_next(struct radeon_device *rdev)
+int radeon_fence_wait_next(struct radeon_device *rdev, int ring)
 {
 	unsigned long irq_flags;
 	struct radeon_fence *fence;
@@ -294,21 +292,21 @@ int radeon_fence_wait_next(struct radeon_device *rdev)
 	if (rdev->gpu_lockup) {
 		return 0;
 	}
-	write_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-	if (list_empty(&rdev->fence_drv.emited)) {
-		write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	if (list_empty(&rdev->fence_drv[ring].emitted)) {
+		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 		return 0;
 	}
-	fence = list_entry(rdev->fence_drv.emited.next,
+	fence = list_entry(rdev->fence_drv[ring].emitted.next,
 			   struct radeon_fence, list);
 	radeon_fence_ref(fence);
-	write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	r = radeon_fence_wait(fence, false);
 	radeon_fence_unref(&fence);
 	return r;
 }
 
-int radeon_fence_wait_last(struct radeon_device *rdev)
+int radeon_fence_wait_last(struct radeon_device *rdev, int ring)
 {
 	unsigned long irq_flags;
 	struct radeon_fence *fence;
@@ -317,15 +315,15 @@ int radeon_fence_wait_last(struct radeon_device *rdev)
 	if (rdev->gpu_lockup) {
 		return 0;
 	}
-	write_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-	if (list_empty(&rdev->fence_drv.emited)) {
-		write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	if (list_empty(&rdev->fence_drv[ring].emitted)) {
+		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 		return 0;
 	}
-	fence = list_entry(rdev->fence_drv.emited.prev,
+	fence = list_entry(rdev->fence_drv[ring].emitted.prev,
 			   struct radeon_fence, list);
 	radeon_fence_ref(fence);
-	write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	r = radeon_fence_wait(fence, false);
 	radeon_fence_unref(&fence);
 	return r;
@@ -347,39 +345,95 @@ void radeon_fence_unref(struct radeon_fence **fence)
 	}
 }
 
-void radeon_fence_process(struct radeon_device *rdev)
+void radeon_fence_process(struct radeon_device *rdev, int ring)
 {
 	unsigned long irq_flags;
 	bool wake;
 
-	write_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-	wake = radeon_fence_poll_locked(rdev);
-	write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	wake = radeon_fence_poll_locked(rdev, ring);
+	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	if (wake) {
-		wake_up_all(&rdev->fence_drv.queue);
+		wake_up_all(&rdev->fence_drv[ring].queue);
 	}
 }
 
-int radeon_fence_driver_init(struct radeon_device *rdev)
+int radeon_fence_count_emitted(struct radeon_device *rdev, int ring)
+{
+	unsigned long irq_flags;
+	int not_processed = 0;
+
+	read_lock_irqsave(&rdev->fence_lock, irq_flags);
+	if (!rdev->fence_drv[ring].initialized)
+		return 0;
+
+	if (!list_empty(&rdev->fence_drv[ring].emitted)) {
+		struct list_head *ptr;
+		list_for_each(ptr, &rdev->fence_drv[ring].emitted) {
+			/* count up to 3, that's enought info */
+			if (++not_processed >= 3)
+				break;
+		}
+	}
+	read_unlock_irqrestore(&rdev->fence_lock, irq_flags);
+	return not_processed;
+}
+
+int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring)
 {
 	unsigned long irq_flags;
+	uint64_t index;
 	int r;
 
-	write_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-	r = radeon_scratch_get(rdev, &rdev->fence_drv.scratch_reg);
-	if (r) {
-		dev_err(rdev->dev, "fence failed to get scratch register\n");
-		write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
-		return r;
+	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	radeon_scratch_free(rdev, rdev->fence_drv[ring].scratch_reg);
+	if (rdev->wb.use_event) {
+		rdev->fence_drv[ring].scratch_reg = 0;
+		index = R600_WB_EVENT_OFFSET + ring * 4;
+	} else {
+		r = radeon_scratch_get(rdev, &rdev->fence_drv[ring].scratch_reg);
+		if (r) {
+			dev_err(rdev->dev, "fence failed to get scratch register\n");
+			write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
+			return r;
+		}
+		index = RADEON_WB_SCRATCH_OFFSET +
+			rdev->fence_drv[ring].scratch_reg -
+			rdev->scratch.reg_base;
 	}
-	radeon_fence_write(rdev, 0);
-	atomic_set(&rdev->fence_drv.seq, 0);
-	INIT_LIST_HEAD(&rdev->fence_drv.created);
-	INIT_LIST_HEAD(&rdev->fence_drv.emited);
-	INIT_LIST_HEAD(&rdev->fence_drv.signaled);
-	init_waitqueue_head(&rdev->fence_drv.queue);
-	rdev->fence_drv.initialized = true;
-	write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
+	rdev->fence_drv[ring].cpu_addr = &rdev->wb.wb[index/4];
+	rdev->fence_drv[ring].gpu_addr = rdev->wb.gpu_addr + index;
+	radeon_fence_write(rdev, atomic_read(&rdev->fence_drv[ring].seq), ring);
+	rdev->fence_drv[ring].initialized = true;
+	DRM_INFO("fence driver on ring %d use gpu addr 0x%08Lx and cpu addr 0x%p\n",
+		 ring, rdev->fence_drv[ring].gpu_addr, rdev->fence_drv[ring].cpu_addr);
+	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
+	return 0;
+}
+
+static void radeon_fence_driver_init_ring(struct radeon_device *rdev, int ring)
+{
+	rdev->fence_drv[ring].scratch_reg = -1;
+	rdev->fence_drv[ring].cpu_addr = NULL;
+	rdev->fence_drv[ring].gpu_addr = 0;
+	atomic_set(&rdev->fence_drv[ring].seq, 0);
+	INIT_LIST_HEAD(&rdev->fence_drv[ring].created);
+	INIT_LIST_HEAD(&rdev->fence_drv[ring].emitted);
+	INIT_LIST_HEAD(&rdev->fence_drv[ring].signaled);
+	init_waitqueue_head(&rdev->fence_drv[ring].queue);
+	rdev->fence_drv[ring].initialized = false;
+}
+
+int radeon_fence_driver_init(struct radeon_device *rdev)
+{
+	unsigned long irq_flags;
+	int ring;
+
+	write_lock_irqsave(&rdev->fence_lock, irq_flags);
+	for (ring = 0; ring < RADEON_NUM_RINGS; ring++) {
+		radeon_fence_driver_init_ring(rdev, ring);
+	}
+	write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
 	if (radeon_debugfs_fence_init(rdev)) {
 		dev_err(rdev->dev, "fence debugfs file creation failed\n");
 	}
@@ -389,14 +443,18 @@ int radeon_fence_driver_init(struct radeon_device *rdev)
 void radeon_fence_driver_fini(struct radeon_device *rdev)
 {
 	unsigned long irq_flags;
-
-	if (!rdev->fence_drv.initialized)
-		return;
-	wake_up_all(&rdev->fence_drv.queue);
-	write_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-	radeon_scratch_free(rdev, rdev->fence_drv.scratch_reg);
-	write_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
-	rdev->fence_drv.initialized = false;
+	int ring;
+
+	for (ring = 0; ring < RADEON_NUM_RINGS; ring++) {
+		if (!rdev->fence_drv[ring].initialized)
+			continue;
+		radeon_fence_wait_last(rdev, ring);
+		wake_up_all(&rdev->fence_drv[ring].queue);
+		write_lock_irqsave(&rdev->fence_lock, irq_flags);
+		radeon_scratch_free(rdev, rdev->fence_drv[ring].scratch_reg);
+		write_unlock_irqrestore(&rdev->fence_lock, irq_flags);
+		rdev->fence_drv[ring].initialized = false;
+	}
 }
 
 
@@ -410,14 +468,21 @@ static int radeon_debugfs_fence_info(struct seq_file *m, void *data)
 	struct drm_device *dev = node->minor->dev;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_fence *fence;
-
-	seq_printf(m, "Last signaled fence 0x%08X\n",
-		   radeon_fence_read(rdev));
-	if (!list_empty(&rdev->fence_drv.emited)) {
-		   fence = list_entry(rdev->fence_drv.emited.prev,
-				      struct radeon_fence, list);
-		   seq_printf(m, "Last emited fence %p with 0x%08X\n",
-			      fence,  fence->seq);
+	int i;
+
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (!rdev->fence_drv[i].initialized)
+			continue;
+
+		seq_printf(m, "--- ring %d ---\n", i);
+		seq_printf(m, "Last signaled fence 0x%08X\n",
+			   radeon_fence_read(rdev, i));
+		if (!list_empty(&rdev->fence_drv[i].emitted)) {
+			fence = list_entry(rdev->fence_drv[i].emitted.prev,
+					   struct radeon_fence, list);
+			seq_printf(m, "Last emitted fence %p with 0x%08X\n",
+				   fence,  fence->seq);
+		}
 	}
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_gart.c b/drivers/gpu/drm/radeon/radeon_gart.c
index ba7ab79e12c1..010dad8b66ae 100644
--- a/drivers/gpu/drm/radeon/radeon_gart.c
+++ b/drivers/gpu/drm/radeon/radeon_gart.c
@@ -157,9 +157,6 @@ void radeon_gart_unbind(struct radeon_device *rdev, unsigned offset,
 	p = t / (PAGE_SIZE / RADEON_GPU_PAGE_SIZE);
 	for (i = 0; i < pages; i++, p++) {
 		if (rdev->gart.pages[p]) {
-			if (!rdev->gart.ttm_alloced[p])
-				pci_unmap_page(rdev->pdev, rdev->gart.pages_addr[p],
-						PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
 			rdev->gart.pages[p] = NULL;
 			rdev->gart.pages_addr[p] = rdev->dummy_page.addr;
 			page_base = rdev->gart.pages_addr[p];
@@ -191,23 +188,7 @@ int radeon_gart_bind(struct radeon_device *rdev, unsigned offset,
 	p = t / (PAGE_SIZE / RADEON_GPU_PAGE_SIZE);
 
 	for (i = 0; i < pages; i++, p++) {
-		/* we reverted the patch using dma_addr in TTM for now but this
-		 * code stops building on alpha so just comment it out for now */
-		if (0) { /*dma_addr[i] != DMA_ERROR_CODE) */
-			rdev->gart.ttm_alloced[p] = true;
-			rdev->gart.pages_addr[p] = dma_addr[i];
-		} else {
-			/* we need to support large memory configurations */
-			/* assume that unbind have already been call on the range */
-			rdev->gart.pages_addr[p] = pci_map_page(rdev->pdev, pagelist[i],
-							0, PAGE_SIZE,
-							PCI_DMA_BIDIRECTIONAL);
-			if (pci_dma_mapping_error(rdev->pdev, rdev->gart.pages_addr[p])) {
-				/* FIXME: failed to map page (return -ENOMEM?) */
-				radeon_gart_unbind(rdev, offset, pages);
-				return -ENOMEM;
-			}
-		}
+		rdev->gart.pages_addr[p] = dma_addr[i];
 		rdev->gart.pages[p] = pagelist[i];
 		if (rdev->gart.ptr) {
 			page_base = rdev->gart.pages_addr[p];
@@ -274,12 +255,6 @@ int radeon_gart_init(struct radeon_device *rdev)
 		radeon_gart_fini(rdev);
 		return -ENOMEM;
 	}
-	rdev->gart.ttm_alloced = kzalloc(sizeof(bool) *
-					 rdev->gart.num_cpu_pages, GFP_KERNEL);
-	if (rdev->gart.ttm_alloced == NULL) {
-		radeon_gart_fini(rdev);
-		return -ENOMEM;
-	}
 	/* set GART entry to point to the dummy page by default */
 	for (i = 0; i < rdev->gart.num_cpu_pages; i++) {
 		rdev->gart.pages_addr[i] = rdev->dummy_page.addr;
@@ -296,10 +271,404 @@ void radeon_gart_fini(struct radeon_device *rdev)
 	rdev->gart.ready = false;
 	kfree(rdev->gart.pages);
 	kfree(rdev->gart.pages_addr);
-	kfree(rdev->gart.ttm_alloced);
 	rdev->gart.pages = NULL;
 	rdev->gart.pages_addr = NULL;
-	rdev->gart.ttm_alloced = NULL;
 
 	radeon_dummy_page_fini(rdev);
 }
+
+/*
+ * vm helpers
+ *
+ * TODO bind a default page at vm initialization for default address
+ */
+int radeon_vm_manager_init(struct radeon_device *rdev)
+{
+	int r;
+
+	rdev->vm_manager.enabled = false;
+
+	/* mark first vm as always in use, it's the system one */
+	r = radeon_sa_bo_manager_init(rdev, &rdev->vm_manager.sa_manager,
+				      rdev->vm_manager.max_pfn * 8,
+				      RADEON_GEM_DOMAIN_VRAM);
+	if (r) {
+		dev_err(rdev->dev, "failed to allocate vm bo (%dKB)\n",
+			(rdev->vm_manager.max_pfn * 8) >> 10);
+		return r;
+	}
+
+	r = rdev->vm_manager.funcs->init(rdev);
+	if (r == 0)
+		rdev->vm_manager.enabled = true;
+
+	return r;
+}
+
+/* cs mutex must be lock */
+static void radeon_vm_unbind_locked(struct radeon_device *rdev,
+				    struct radeon_vm *vm)
+{
+	struct radeon_bo_va *bo_va;
+
+	if (vm->id == -1) {
+		return;
+	}
+
+	/* wait for vm use to end */
+	if (vm->fence) {
+		radeon_fence_wait(vm->fence, false);
+		radeon_fence_unref(&vm->fence);
+	}
+
+	/* hw unbind */
+	rdev->vm_manager.funcs->unbind(rdev, vm);
+	rdev->vm_manager.use_bitmap &= ~(1 << vm->id);
+	list_del_init(&vm->list);
+	vm->id = -1;
+	radeon_sa_bo_free(rdev, &vm->sa_bo);
+	vm->pt = NULL;
+
+	list_for_each_entry(bo_va, &vm->va, vm_list) {
+		bo_va->valid = false;
+	}
+}
+
+void radeon_vm_manager_fini(struct radeon_device *rdev)
+{
+	if (rdev->vm_manager.sa_manager.bo == NULL)
+		return;
+	radeon_vm_manager_suspend(rdev);
+	rdev->vm_manager.funcs->fini(rdev);
+	radeon_sa_bo_manager_fini(rdev, &rdev->vm_manager.sa_manager);
+	rdev->vm_manager.enabled = false;
+}
+
+int radeon_vm_manager_start(struct radeon_device *rdev)
+{
+	if (rdev->vm_manager.sa_manager.bo == NULL) {
+		return -EINVAL;
+	}
+	return radeon_sa_bo_manager_start(rdev, &rdev->vm_manager.sa_manager);
+}
+
+int radeon_vm_manager_suspend(struct radeon_device *rdev)
+{
+	struct radeon_vm *vm, *tmp;
+
+	radeon_mutex_lock(&rdev->cs_mutex);
+	/* unbind all active vm */
+	list_for_each_entry_safe(vm, tmp, &rdev->vm_manager.lru_vm, list) {
+		radeon_vm_unbind_locked(rdev, vm);
+	}
+	rdev->vm_manager.funcs->fini(rdev);
+	radeon_mutex_unlock(&rdev->cs_mutex);
+	return radeon_sa_bo_manager_suspend(rdev, &rdev->vm_manager.sa_manager);
+}
+
+/* cs mutex must be lock */
+void radeon_vm_unbind(struct radeon_device *rdev, struct radeon_vm *vm)
+{
+	mutex_lock(&vm->mutex);
+	radeon_vm_unbind_locked(rdev, vm);
+	mutex_unlock(&vm->mutex);
+}
+
+/* cs mutex must be lock & vm mutex must be lock */
+int radeon_vm_bind(struct radeon_device *rdev, struct radeon_vm *vm)
+{
+	struct radeon_vm *vm_evict;
+	unsigned i;
+	int id = -1, r;
+
+	if (vm == NULL) {
+		return -EINVAL;
+	}
+
+	if (vm->id != -1) {
+		/* update lru */
+		list_del_init(&vm->list);
+		list_add_tail(&vm->list, &rdev->vm_manager.lru_vm);
+		return 0;
+	}
+
+retry:
+	r = radeon_sa_bo_new(rdev, &rdev->vm_manager.sa_manager, &vm->sa_bo,
+			     RADEON_GPU_PAGE_ALIGN(vm->last_pfn * 8),
+			     RADEON_GPU_PAGE_SIZE);
+	if (r) {
+		if (list_empty(&rdev->vm_manager.lru_vm)) {
+			return r;
+		}
+		vm_evict = list_first_entry(&rdev->vm_manager.lru_vm, struct radeon_vm, list);
+		radeon_vm_unbind(rdev, vm_evict);
+		goto retry;
+	}
+	vm->pt = rdev->vm_manager.sa_manager.cpu_ptr;
+	vm->pt += (vm->sa_bo.offset >> 3);
+	vm->pt_gpu_addr = rdev->vm_manager.sa_manager.gpu_addr;
+	vm->pt_gpu_addr += vm->sa_bo.offset;
+	memset(vm->pt, 0, RADEON_GPU_PAGE_ALIGN(vm->last_pfn * 8));
+
+retry_id:
+	/* search for free vm */
+	for (i = 0; i < rdev->vm_manager.nvm; i++) {
+		if (!(rdev->vm_manager.use_bitmap & (1 << i))) {
+			id = i;
+			break;
+		}
+	}
+	/* evict vm if necessary */
+	if (id == -1) {
+		vm_evict = list_first_entry(&rdev->vm_manager.lru_vm, struct radeon_vm, list);
+		radeon_vm_unbind(rdev, vm_evict);
+		goto retry_id;
+	}
+
+	/* do hw bind */
+	r = rdev->vm_manager.funcs->bind(rdev, vm, id);
+	if (r) {
+		radeon_sa_bo_free(rdev, &vm->sa_bo);
+		return r;
+	}
+	rdev->vm_manager.use_bitmap |= 1 << id;
+	vm->id = id;
+	list_add_tail(&vm->list, &rdev->vm_manager.lru_vm);
+	return radeon_vm_bo_update_pte(rdev, vm, rdev->ib_pool.sa_manager.bo,
+				       &rdev->ib_pool.sa_manager.bo->tbo.mem);
+}
+
+/* object have to be reserved */
+int radeon_vm_bo_add(struct radeon_device *rdev,
+		     struct radeon_vm *vm,
+		     struct radeon_bo *bo,
+		     uint64_t offset,
+		     uint32_t flags)
+{
+	struct radeon_bo_va *bo_va, *tmp;
+	struct list_head *head;
+	uint64_t size = radeon_bo_size(bo), last_offset = 0;
+	unsigned last_pfn;
+
+	bo_va = kzalloc(sizeof(struct radeon_bo_va), GFP_KERNEL);
+	if (bo_va == NULL) {
+		return -ENOMEM;
+	}
+	bo_va->vm = vm;
+	bo_va->bo = bo;
+	bo_va->soffset = offset;
+	bo_va->eoffset = offset + size;
+	bo_va->flags = flags;
+	bo_va->valid = false;
+	INIT_LIST_HEAD(&bo_va->bo_list);
+	INIT_LIST_HEAD(&bo_va->vm_list);
+	/* make sure object fit at this offset */
+	if (bo_va->soffset >= bo_va->eoffset) {
+		kfree(bo_va);
+		return -EINVAL;
+	}
+
+	last_pfn = bo_va->eoffset / RADEON_GPU_PAGE_SIZE;
+	if (last_pfn > rdev->vm_manager.max_pfn) {
+		kfree(bo_va);
+		dev_err(rdev->dev, "va above limit (0x%08X > 0x%08X)\n",
+			last_pfn, rdev->vm_manager.max_pfn);
+		return -EINVAL;
+	}
+
+	mutex_lock(&vm->mutex);
+	if (last_pfn > vm->last_pfn) {
+		/* grow va space 32M by 32M */
+		unsigned align = ((32 << 20) >> 12) - 1;
+		radeon_mutex_lock(&rdev->cs_mutex);
+		radeon_vm_unbind_locked(rdev, vm);
+		radeon_mutex_unlock(&rdev->cs_mutex);
+		vm->last_pfn = (last_pfn + align) & ~align;
+	}
+	head = &vm->va;
+	last_offset = 0;
+	list_for_each_entry(tmp, &vm->va, vm_list) {
+		if (bo_va->soffset >= last_offset && bo_va->eoffset < tmp->soffset) {
+			/* bo can be added before this one */
+			break;
+		}
+		if (bo_va->soffset >= tmp->soffset && bo_va->soffset < tmp->eoffset) {
+			/* bo and tmp overlap, invalid offset */
+			dev_err(rdev->dev, "bo %p va 0x%08X conflict with (bo %p 0x%08X 0x%08X)\n",
+				bo, (unsigned)bo_va->soffset, tmp->bo,
+				(unsigned)tmp->soffset, (unsigned)tmp->eoffset);
+			kfree(bo_va);
+			mutex_unlock(&vm->mutex);
+			return -EINVAL;
+		}
+		last_offset = tmp->eoffset;
+		head = &tmp->vm_list;
+	}
+	list_add(&bo_va->vm_list, head);
+	list_add_tail(&bo_va->bo_list, &bo->va);
+	mutex_unlock(&vm->mutex);
+	return 0;
+}
+
+static u64 radeon_vm_get_addr(struct radeon_device *rdev,
+			      struct ttm_mem_reg *mem,
+			      unsigned pfn)
+{
+	u64 addr = 0;
+
+	switch (mem->mem_type) {
+	case TTM_PL_VRAM:
+		addr = (mem->start << PAGE_SHIFT);
+		addr += pfn * RADEON_GPU_PAGE_SIZE;
+		addr += rdev->vm_manager.vram_base_offset;
+		break;
+	case TTM_PL_TT:
+		/* offset inside page table */
+		addr = mem->start << PAGE_SHIFT;
+		addr += pfn * RADEON_GPU_PAGE_SIZE;
+		addr = addr >> PAGE_SHIFT;
+		/* page table offset */
+		addr = rdev->gart.pages_addr[addr];
+		/* in case cpu page size != gpu page size*/
+		addr += (pfn * RADEON_GPU_PAGE_SIZE) & (~PAGE_MASK);
+		break;
+	default:
+		break;
+	}
+	return addr;
+}
+
+/* object have to be reserved & cs mutex took & vm mutex took */
+int radeon_vm_bo_update_pte(struct radeon_device *rdev,
+			    struct radeon_vm *vm,
+			    struct radeon_bo *bo,
+			    struct ttm_mem_reg *mem)
+{
+	struct radeon_bo_va *bo_va;
+	unsigned ngpu_pages, i;
+	uint64_t addr = 0, pfn;
+	uint32_t flags;
+
+	/* nothing to do if vm isn't bound */
+	if (vm->id == -1)
+		return 0;;
+
+	bo_va = radeon_bo_va(bo, vm);
+	if (bo_va == NULL) {
+		dev_err(rdev->dev, "bo %p not in vm %p\n", bo, vm);
+		return -EINVAL;
+	}
+
+	if (bo_va->valid)
+		return 0;
+
+	ngpu_pages = radeon_bo_ngpu_pages(bo);
+	bo_va->flags &= ~RADEON_VM_PAGE_VALID;
+	bo_va->flags &= ~RADEON_VM_PAGE_SYSTEM;
+	if (mem) {
+		if (mem->mem_type != TTM_PL_SYSTEM) {
+			bo_va->flags |= RADEON_VM_PAGE_VALID;
+			bo_va->valid = true;
+		}
+		if (mem->mem_type == TTM_PL_TT) {
+			bo_va->flags |= RADEON_VM_PAGE_SYSTEM;
+		}
+	}
+	pfn = bo_va->soffset / RADEON_GPU_PAGE_SIZE;
+	flags = rdev->vm_manager.funcs->page_flags(rdev, bo_va->vm, bo_va->flags);
+	for (i = 0, addr = 0; i < ngpu_pages; i++) {
+		if (mem && bo_va->valid) {
+			addr = radeon_vm_get_addr(rdev, mem, i);
+		}
+		rdev->vm_manager.funcs->set_page(rdev, bo_va->vm, i + pfn, addr, flags);
+	}
+	rdev->vm_manager.funcs->tlb_flush(rdev, bo_va->vm);
+	return 0;
+}
+
+/* object have to be reserved */
+int radeon_vm_bo_rmv(struct radeon_device *rdev,
+		     struct radeon_vm *vm,
+		     struct radeon_bo *bo)
+{
+	struct radeon_bo_va *bo_va;
+
+	bo_va = radeon_bo_va(bo, vm);
+	if (bo_va == NULL)
+		return 0;
+
+	list_del(&bo_va->bo_list);
+	mutex_lock(&vm->mutex);
+	radeon_mutex_lock(&rdev->cs_mutex);
+	radeon_vm_bo_update_pte(rdev, vm, bo, NULL);
+	radeon_mutex_unlock(&rdev->cs_mutex);
+	list_del(&bo_va->vm_list);
+	mutex_unlock(&vm->mutex);
+
+	kfree(bo_va);
+	return 0;
+}
+
+void radeon_vm_bo_invalidate(struct radeon_device *rdev,
+			     struct radeon_bo *bo)
+{
+	struct radeon_bo_va *bo_va;
+
+	BUG_ON(!atomic_read(&bo->tbo.reserved));
+	list_for_each_entry(bo_va, &bo->va, bo_list) {
+		bo_va->valid = false;
+	}
+}
+
+int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
+{
+	int r;
+
+	vm->id = -1;
+	vm->fence = NULL;
+	mutex_init(&vm->mutex);
+	INIT_LIST_HEAD(&vm->list);
+	INIT_LIST_HEAD(&vm->va);
+	vm->last_pfn = 0;
+	/* map the ib pool buffer at 0 in virtual address space, set
+	 * read only
+	 */
+	r = radeon_vm_bo_add(rdev, vm, rdev->ib_pool.sa_manager.bo, 0,
+			     RADEON_VM_PAGE_READABLE | RADEON_VM_PAGE_SNOOPED);
+	return r;
+}
+
+void radeon_vm_fini(struct radeon_device *rdev, struct radeon_vm *vm)
+{
+	struct radeon_bo_va *bo_va, *tmp;
+	int r;
+
+	mutex_lock(&vm->mutex);
+
+	radeon_mutex_lock(&rdev->cs_mutex);
+	radeon_vm_unbind_locked(rdev, vm);
+	radeon_mutex_unlock(&rdev->cs_mutex);
+
+	/* remove all bo */
+	r = radeon_bo_reserve(rdev->ib_pool.sa_manager.bo, false);
+	if (!r) {
+		bo_va = radeon_bo_va(rdev->ib_pool.sa_manager.bo, vm);
+		list_del_init(&bo_va->bo_list);
+		list_del_init(&bo_va->vm_list);
+		radeon_bo_unreserve(rdev->ib_pool.sa_manager.bo);
+		kfree(bo_va);
+	}
+	if (!list_empty(&vm->va)) {
+		dev_err(rdev->dev, "still active bo inside vm\n");
+	}
+	list_for_each_entry_safe(bo_va, tmp, &vm->va, vm_list) {
+		list_del_init(&bo_va->vm_list);
+		r = radeon_bo_reserve(bo_va->bo, false);
+		if (!r) {
+			list_del_init(&bo_va->bo_list);
+			radeon_bo_unreserve(bo_va->bo);
+			kfree(bo_va);
+		}
+	}
+	mutex_unlock(&vm->mutex);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index aa1ca2dea42f..7337850af2fa 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -142,6 +142,44 @@ void radeon_gem_fini(struct radeon_device *rdev)
 	radeon_bo_force_delete(rdev);
 }
 
+/*
+ * Call from drm_gem_handle_create which appear in both new and open ioctl
+ * case.
+ */
+int radeon_gem_object_open(struct drm_gem_object *obj, struct drm_file *file_priv)
+{
+	return 0;
+}
+
+void radeon_gem_object_close(struct drm_gem_object *obj,
+			     struct drm_file *file_priv)
+{
+	struct radeon_bo *rbo = gem_to_radeon_bo(obj);
+	struct radeon_device *rdev = rbo->rdev;
+	struct radeon_fpriv *fpriv = file_priv->driver_priv;
+	struct radeon_vm *vm = &fpriv->vm;
+	struct radeon_bo_va *bo_va, *tmp;
+
+	if (rdev->family < CHIP_CAYMAN) {
+		return;
+	}
+
+	if (radeon_bo_reserve(rbo, false)) {
+		return;
+	}
+	list_for_each_entry_safe(bo_va, tmp, &rbo->va, bo_list) {
+		if (bo_va->vm == vm) {
+			/* remove from this vm address space */
+			mutex_lock(&vm->mutex);
+			list_del(&bo_va->vm_list);
+			mutex_unlock(&vm->mutex);
+			list_del(&bo_va->bo_list);
+			kfree(bo_va);
+		}
+	}
+	radeon_bo_unreserve(rbo);
+}
+
 
 /*
  * GEM ioctls.
@@ -152,6 +190,7 @@ int radeon_gem_info_ioctl(struct drm_device *dev, void *data,
 	struct radeon_device *rdev = dev->dev_private;
 	struct drm_radeon_gem_info *args = data;
 	struct ttm_mem_type_manager *man;
+	unsigned i;
 
 	man = &rdev->mman.bdev.man[TTM_PL_VRAM];
 
@@ -160,8 +199,9 @@ int radeon_gem_info_ioctl(struct drm_device *dev, void *data,
 	if (rdev->stollen_vga_memory)
 		args->vram_visible -= radeon_bo_size(rdev->stollen_vga_memory);
 	args->vram_visible -= radeon_fbdev_total_size(rdev);
-	args->gart_size = rdev->mc.gtt_size - rdev->cp.ring_size - 4096 -
-		RADEON_IB_POOL_SIZE*64*1024;
+	args->gart_size = rdev->mc.gtt_size - 4096 - RADEON_IB_POOL_SIZE*64*1024;
+	for(i = 0; i < RADEON_NUM_RINGS; ++i)
+		args->gart_size -= rdev->ring[i].ring_size;
 	return 0;
 }
 
@@ -352,6 +392,109 @@ out:
 	return r;
 }
 
+int radeon_gem_va_ioctl(struct drm_device *dev, void *data,
+			  struct drm_file *filp)
+{
+	struct drm_radeon_gem_va *args = data;
+	struct drm_gem_object *gobj;
+	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_fpriv *fpriv = filp->driver_priv;
+	struct radeon_bo *rbo;
+	struct radeon_bo_va *bo_va;
+	u32 invalid_flags;
+	int r = 0;
+
+	if (!rdev->vm_manager.enabled) {
+		args->operation = RADEON_VA_RESULT_ERROR;
+		return -ENOTTY;
+	}
+
+	/* !! DONT REMOVE !!
+	 * We don't support vm_id yet, to be sure we don't have have broken
+	 * userspace, reject anyone trying to use non 0 value thus moving
+	 * forward we can use those fields without breaking existant userspace
+	 */
+	if (args->vm_id) {
+		args->operation = RADEON_VA_RESULT_ERROR;
+		return -EINVAL;
+	}
+
+	if (args->offset < RADEON_VA_RESERVED_SIZE) {
+		dev_err(&dev->pdev->dev,
+			"offset 0x%lX is in reserved area 0x%X\n",
+			(unsigned long)args->offset,
+			RADEON_VA_RESERVED_SIZE);
+		args->operation = RADEON_VA_RESULT_ERROR;
+		return -EINVAL;
+	}
+
+	/* don't remove, we need to enforce userspace to set the snooped flag
+	 * otherwise we will endup with broken userspace and we won't be able
+	 * to enable this feature without adding new interface
+	 */
+	invalid_flags = RADEON_VM_PAGE_VALID | RADEON_VM_PAGE_SYSTEM;
+	if ((args->flags & invalid_flags)) {
+		dev_err(&dev->pdev->dev, "invalid flags 0x%08X vs 0x%08X\n",
+			args->flags, invalid_flags);
+		args->operation = RADEON_VA_RESULT_ERROR;
+		return -EINVAL;
+	}
+	if (!(args->flags & RADEON_VM_PAGE_SNOOPED)) {
+		dev_err(&dev->pdev->dev, "only supported snooped mapping for now\n");
+		args->operation = RADEON_VA_RESULT_ERROR;
+		return -EINVAL;
+	}
+
+	switch (args->operation) {
+	case RADEON_VA_MAP:
+	case RADEON_VA_UNMAP:
+		break;
+	default:
+		dev_err(&dev->pdev->dev, "unsupported operation %d\n",
+			args->operation);
+		args->operation = RADEON_VA_RESULT_ERROR;
+		return -EINVAL;
+	}
+
+	gobj = drm_gem_object_lookup(dev, filp, args->handle);
+	if (gobj == NULL) {
+		args->operation = RADEON_VA_RESULT_ERROR;
+		return -ENOENT;
+	}
+	rbo = gem_to_radeon_bo(gobj);
+	r = radeon_bo_reserve(rbo, false);
+	if (r) {
+		args->operation = RADEON_VA_RESULT_ERROR;
+		drm_gem_object_unreference_unlocked(gobj);
+		return r;
+	}
+	switch (args->operation) {
+	case RADEON_VA_MAP:
+		bo_va = radeon_bo_va(rbo, &fpriv->vm);
+		if (bo_va) {
+			args->operation = RADEON_VA_RESULT_VA_EXIST;
+			args->offset = bo_va->soffset;
+			goto out;
+		}
+		r = radeon_vm_bo_add(rdev, &fpriv->vm, rbo,
+				     args->offset, args->flags);
+		break;
+	case RADEON_VA_UNMAP:
+		r = radeon_vm_bo_rmv(rdev, &fpriv->vm, rbo);
+		break;
+	default:
+		break;
+	}
+	args->operation = RADEON_VA_RESULT_OK;
+	if (r) {
+		args->operation = RADEON_VA_RESULT_ERROR;
+	}
+out:
+	radeon_bo_unreserve(rbo);
+	drm_gem_object_unreference_unlocked(gobj);
+	return r;
+}
+
 int radeon_mode_dumb_create(struct drm_file *file_priv,
 			    struct drm_device *dev,
 			    struct drm_mode_create_dumb *args)
diff --git a/drivers/gpu/drm/radeon/radeon_irq_kms.c b/drivers/gpu/drm/radeon/radeon_irq_kms.c
index 8f86aeb26693..be38921bf761 100644
--- a/drivers/gpu/drm/radeon/radeon_irq_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_irq_kms.c
@@ -65,7 +65,8 @@ void radeon_driver_irq_preinstall_kms(struct drm_device *dev)
 	unsigned i;
 
 	/* Disable *all* interrupts */
-	rdev->irq.sw_int = false;
+	for (i = 0; i < RADEON_NUM_RINGS; i++)
+		rdev->irq.sw_int[i] = false;
 	rdev->irq.gui_idle = false;
 	for (i = 0; i < RADEON_MAX_HPD_PINS; i++)
 		rdev->irq.hpd[i] = false;
@@ -81,9 +82,11 @@ void radeon_driver_irq_preinstall_kms(struct drm_device *dev)
 int radeon_driver_irq_postinstall_kms(struct drm_device *dev)
 {
 	struct radeon_device *rdev = dev->dev_private;
+	unsigned i;
 
 	dev->max_vblank_count = 0x001fffff;
-	rdev->irq.sw_int = true;
+	for (i = 0; i < RADEON_NUM_RINGS; i++)
+		rdev->irq.sw_int[i] = true;
 	radeon_irq_set(rdev);
 	return 0;
 }
@@ -97,7 +100,8 @@ void radeon_driver_irq_uninstall_kms(struct drm_device *dev)
 		return;
 	}
 	/* Disable *all* interrupts */
-	rdev->irq.sw_int = false;
+	for (i = 0; i < RADEON_NUM_RINGS; i++)
+		rdev->irq.sw_int[i] = false;
 	rdev->irq.gui_idle = false;
 	for (i = 0; i < RADEON_MAX_HPD_PINS; i++)
 		rdev->irq.hpd[i] = false;
@@ -194,26 +198,26 @@ void radeon_irq_kms_fini(struct radeon_device *rdev)
 	flush_work_sync(&rdev->hotplug_work);
 }
 
-void radeon_irq_kms_sw_irq_get(struct radeon_device *rdev)
+void radeon_irq_kms_sw_irq_get(struct radeon_device *rdev, int ring)
 {
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&rdev->irq.sw_lock, irqflags);
-	if (rdev->ddev->irq_enabled && (++rdev->irq.sw_refcount == 1)) {
-		rdev->irq.sw_int = true;
+	if (rdev->ddev->irq_enabled && (++rdev->irq.sw_refcount[ring] == 1)) {
+		rdev->irq.sw_int[ring] = true;
 		radeon_irq_set(rdev);
 	}
 	spin_unlock_irqrestore(&rdev->irq.sw_lock, irqflags);
 }
 
-void radeon_irq_kms_sw_irq_put(struct radeon_device *rdev)
+void radeon_irq_kms_sw_irq_put(struct radeon_device *rdev, int ring)
 {
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&rdev->irq.sw_lock, irqflags);
-	BUG_ON(rdev->ddev->irq_enabled && rdev->irq.sw_refcount <= 0);
-	if (rdev->ddev->irq_enabled && (--rdev->irq.sw_refcount == 0)) {
-		rdev->irq.sw_int = false;
+	BUG_ON(rdev->ddev->irq_enabled && rdev->irq.sw_refcount[ring] <= 0);
+	if (rdev->ddev->irq_enabled && (--rdev->irq.sw_refcount[ring] == 0)) {
+		rdev->irq.sw_int[ring] = false;
 		radeon_irq_set(rdev);
 	}
 	spin_unlock_irqrestore(&rdev->irq.sw_lock, irqflags);
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index be2c1224e68a..d3352889a870 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -250,6 +250,18 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 			return -EINVAL;
 		}
 		break;
+	case RADEON_INFO_VA_START:
+		/* this is where we report if vm is supported or not */
+		if (rdev->family < CHIP_CAYMAN)
+			return -EINVAL;
+		value = RADEON_VA_RESERVED_SIZE;
+		break;
+	case RADEON_INFO_IB_VM_MAX_SIZE:
+		/* this is where we report if vm is supported or not */
+		if (rdev->family < CHIP_CAYMAN)
+			return -EINVAL;
+		value = RADEON_IB_VM_MAX_SIZE;
+		break;
 	default:
 		DRM_DEBUG_KMS("Invalid request %d\n", info->request);
 		return -EINVAL;
@@ -270,7 +282,6 @@ int radeon_driver_firstopen_kms(struct drm_device *dev)
 	return 0;
 }
 
-
 void radeon_driver_lastclose_kms(struct drm_device *dev)
 {
 	vga_switcheroo_process_delayed_switch();
@@ -278,12 +289,45 @@ void radeon_driver_lastclose_kms(struct drm_device *dev)
 
 int radeon_driver_open_kms(struct drm_device *dev, struct drm_file *file_priv)
 {
+	struct radeon_device *rdev = dev->dev_private;
+
+	file_priv->driver_priv = NULL;
+
+	/* new gpu have virtual address space support */
+	if (rdev->family >= CHIP_CAYMAN) {
+		struct radeon_fpriv *fpriv;
+		int r;
+
+		fpriv = kzalloc(sizeof(*fpriv), GFP_KERNEL);
+		if (unlikely(!fpriv)) {
+			return -ENOMEM;
+		}
+
+		r = radeon_vm_init(rdev, &fpriv->vm);
+		if (r) {
+			radeon_vm_fini(rdev, &fpriv->vm);
+			kfree(fpriv);
+			return r;
+		}
+
+		file_priv->driver_priv = fpriv;
+	}
 	return 0;
 }
 
 void radeon_driver_postclose_kms(struct drm_device *dev,
 				 struct drm_file *file_priv)
 {
+	struct radeon_device *rdev = dev->dev_private;
+
+	/* new gpu have virtual address space support */
+	if (rdev->family >= CHIP_CAYMAN && file_priv->driver_priv) {
+		struct radeon_fpriv *fpriv = file_priv->driver_priv;
+
+		radeon_vm_fini(rdev, &fpriv->vm);
+		kfree(fpriv);
+		file_priv->driver_priv = NULL;
+	}
 }
 
 void radeon_driver_preclose_kms(struct drm_device *dev,
@@ -451,5 +495,6 @@ struct drm_ioctl_desc radeon_ioctls_kms[] = {
 	DRM_IOCTL_DEF_DRV(RADEON_GEM_SET_TILING, radeon_gem_set_tiling_ioctl, DRM_AUTH|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(RADEON_GEM_GET_TILING, radeon_gem_get_tiling_ioctl, DRM_AUTH|DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(RADEON_GEM_BUSY, radeon_gem_busy_ioctl, DRM_AUTH|DRM_UNLOCKED),
+	DRM_IOCTL_DEF_DRV(RADEON_GEM_VA, radeon_gem_va_ioctl, DRM_AUTH|DRM_UNLOCKED),
 };
 int radeon_max_kms_ioctl = DRM_ARRAY_SIZE(radeon_ioctls_kms);
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
index daadf2111040..25a19c483075 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
@@ -437,7 +437,7 @@ int radeon_crtc_do_set_base(struct drm_crtc *crtc,
 
 	crtc_offset_cntl = 0;
 
-	pitch_pixels = target_fb->pitch / (target_fb->bits_per_pixel / 8);
+	pitch_pixels = target_fb->pitches[0] / (target_fb->bits_per_pixel / 8);
 	crtc_pitch  = (((pitch_pixels * target_fb->bits_per_pixel) +
 			((target_fb->bits_per_pixel * 8) - 1)) /
 		       (target_fb->bits_per_pixel * 8));
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index 2c2e75ef8a37..08ff857c8fd6 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -643,7 +643,7 @@ extern void radeon_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green
 				     u16 *blue, int regno);
 void radeon_framebuffer_init(struct drm_device *dev,
 			     struct radeon_framebuffer *rfb,
-			     struct drm_mode_fb_cmd *mode_cmd,
+			     struct drm_mode_fb_cmd2 *mode_cmd,
 			     struct drm_gem_object *obj);
 
 int radeonfb_remove(struct drm_device *dev, struct drm_framebuffer *fb);
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 1c851521f458..d45df1763598 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -46,6 +46,20 @@ static void radeon_bo_clear_surface_reg(struct radeon_bo *bo);
  * function are calling it.
  */
 
+void radeon_bo_clear_va(struct radeon_bo *bo)
+{
+	struct radeon_bo_va *bo_va, *tmp;
+
+	list_for_each_entry_safe(bo_va, tmp, &bo->va, bo_list) {
+		/* remove from all vm address space */
+		mutex_lock(&bo_va->vm->mutex);
+		list_del(&bo_va->vm_list);
+		mutex_unlock(&bo_va->vm->mutex);
+		list_del(&bo_va->bo_list);
+		kfree(bo_va);
+	}
+}
+
 static void radeon_ttm_bo_destroy(struct ttm_buffer_object *tbo)
 {
 	struct radeon_bo *bo;
@@ -55,6 +69,7 @@ static void radeon_ttm_bo_destroy(struct ttm_buffer_object *tbo)
 	list_del_init(&bo->list);
 	mutex_unlock(&bo->rdev->gem.mutex);
 	radeon_bo_clear_surface_reg(bo);
+	radeon_bo_clear_va(bo);
 	drm_gem_object_release(&bo->gem_base);
 	kfree(bo);
 }
@@ -95,6 +110,7 @@ int radeon_bo_create(struct radeon_device *rdev,
 	enum ttm_bo_type type;
 	unsigned long page_align = roundup(byte_align, PAGE_SIZE) >> PAGE_SHIFT;
 	unsigned long max_size = 0;
+	size_t acc_size;
 	int r;
 
 	size = ALIGN(size, PAGE_SIZE);
@@ -117,6 +133,9 @@ int radeon_bo_create(struct radeon_device *rdev,
 		return -ENOMEM;
 	}
 
+	acc_size = ttm_bo_dma_acc_size(&rdev->mman.bdev, size,
+				       sizeof(struct radeon_bo));
+
 retry:
 	bo = kzalloc(sizeof(struct radeon_bo), GFP_KERNEL);
 	if (bo == NULL)
@@ -130,12 +149,13 @@ retry:
 	bo->gem_base.driver_private = NULL;
 	bo->surface_reg = -1;
 	INIT_LIST_HEAD(&bo->list);
+	INIT_LIST_HEAD(&bo->va);
 	radeon_ttm_placement_from_domain(bo, domain);
 	/* Kernel allocation are uninterruptible */
 	mutex_lock(&rdev->vram_mutex);
 	r = ttm_bo_init(&rdev->mman.bdev, &bo->tbo, size, type,
-			&bo->placement, page_align, 0, !kernel, NULL, size,
-			&radeon_ttm_bo_destroy);
+			&bo->placement, page_align, 0, !kernel, NULL,
+			acc_size, &radeon_ttm_bo_destroy);
 	mutex_unlock(&rdev->vram_mutex);
 	if (unlikely(r != 0)) {
 		if (r != -ERESTARTSYS) {
@@ -483,6 +503,7 @@ void radeon_bo_move_notify(struct ttm_buffer_object *bo,
 		return;
 	rbo = container_of(bo, struct radeon_bo, tbo);
 	radeon_bo_check_tiling(rbo, 0, 1);
+	radeon_vm_bo_invalidate(rbo->rdev, rbo);
 }
 
 int radeon_bo_fault_reserve_notify(struct ttm_buffer_object *bo)
@@ -556,3 +577,16 @@ int radeon_bo_reserve(struct radeon_bo *bo, bool no_wait)
 	}
 	return 0;
 }
+
+/* object have to be reserved */
+struct radeon_bo_va *radeon_bo_va(struct radeon_bo *rbo, struct radeon_vm *vm)
+{
+	struct radeon_bo_va *bo_va;
+
+	list_for_each_entry(bo_va, &rbo->va, bo_list) {
+		if (bo_va->vm == vm) {
+			return bo_va;
+		}
+	}
+	return NULL;
+}
diff --git a/drivers/gpu/drm/radeon/radeon_object.h b/drivers/gpu/drm/radeon/radeon_object.h
index b07f0f9b8627..cde430308870 100644
--- a/drivers/gpu/drm/radeon/radeon_object.h
+++ b/drivers/gpu/drm/radeon/radeon_object.h
@@ -83,6 +83,16 @@ static inline bool radeon_bo_is_reserved(struct radeon_bo *bo)
 	return !!atomic_read(&bo->tbo.reserved);
 }
 
+static inline unsigned radeon_bo_ngpu_pages(struct radeon_bo *bo)
+{
+	return (bo->tbo.num_pages << PAGE_SHIFT) / RADEON_GPU_PAGE_SIZE;
+}
+
+static inline unsigned radeon_bo_gpu_page_alignment(struct radeon_bo *bo)
+{
+	return (bo->tbo.mem.page_alignment << PAGE_SHIFT) / RADEON_GPU_PAGE_SIZE;
+}
+
 /**
  * radeon_bo_mmap_offset - return mmap offset of bo
  * @bo:	radeon object for which we query the offset
@@ -128,4 +138,26 @@ extern void radeon_bo_move_notify(struct ttm_buffer_object *bo,
 					struct ttm_mem_reg *mem);
 extern int radeon_bo_fault_reserve_notify(struct ttm_buffer_object *bo);
 extern int radeon_bo_get_surface_reg(struct radeon_bo *bo);
+extern struct radeon_bo_va *radeon_bo_va(struct radeon_bo *rbo,
+					 struct radeon_vm *vm);
+
+/*
+ * sub allocation
+ */
+extern int radeon_sa_bo_manager_init(struct radeon_device *rdev,
+				     struct radeon_sa_manager *sa_manager,
+				     unsigned size, u32 domain);
+extern void radeon_sa_bo_manager_fini(struct radeon_device *rdev,
+				      struct radeon_sa_manager *sa_manager);
+extern int radeon_sa_bo_manager_start(struct radeon_device *rdev,
+				      struct radeon_sa_manager *sa_manager);
+extern int radeon_sa_bo_manager_suspend(struct radeon_device *rdev,
+					struct radeon_sa_manager *sa_manager);
+extern int radeon_sa_bo_new(struct radeon_device *rdev,
+			    struct radeon_sa_manager *sa_manager,
+			    struct radeon_sa_bo *sa_bo,
+			    unsigned size, unsigned align);
+extern void radeon_sa_bo_free(struct radeon_device *rdev,
+			      struct radeon_sa_bo *sa_bo);
+
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index 78a665bd9519..095148e29a1f 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -252,7 +252,10 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 
 	mutex_lock(&rdev->ddev->struct_mutex);
 	mutex_lock(&rdev->vram_mutex);
-	mutex_lock(&rdev->cp.mutex);
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (rdev->ring[i].ring_obj)
+			mutex_lock(&rdev->ring[i].mutex);
+	}
 
 	/* gui idle int has issues on older chips it seems */
 	if (rdev->family >= CHIP_R600) {
@@ -268,12 +271,13 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 			radeon_irq_set(rdev);
 		}
 	} else {
-		if (rdev->cp.ready) {
+		struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+		if (ring->ready) {
 			struct radeon_fence *fence;
-			radeon_ring_alloc(rdev, 64);
-			radeon_fence_create(rdev, &fence);
+			radeon_ring_alloc(rdev, ring, 64);
+			radeon_fence_create(rdev, &fence, radeon_ring_index(rdev, ring));
 			radeon_fence_emit(rdev, fence);
-			radeon_ring_commit(rdev);
+			radeon_ring_commit(rdev, ring);
 			radeon_fence_wait(fence, false);
 			radeon_fence_unref(&fence);
 		}
@@ -307,7 +311,10 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 
 	rdev->pm.dynpm_planned_action = DYNPM_ACTION_NONE;
 
-	mutex_unlock(&rdev->cp.mutex);
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (rdev->ring[i].ring_obj)
+			mutex_unlock(&rdev->ring[i].mutex);
+	}
 	mutex_unlock(&rdev->vram_mutex);
 	mutex_unlock(&rdev->ddev->struct_mutex);
 }
@@ -795,19 +802,14 @@ static void radeon_dynpm_idle_work_handler(struct work_struct *work)
 	resched = ttm_bo_lock_delayed_workqueue(&rdev->mman.bdev);
 	mutex_lock(&rdev->pm.mutex);
 	if (rdev->pm.dynpm_state == DYNPM_STATE_ACTIVE) {
-		unsigned long irq_flags;
 		int not_processed = 0;
+		int i;
 
-		read_lock_irqsave(&rdev->fence_drv.lock, irq_flags);
-		if (!list_empty(&rdev->fence_drv.emited)) {
-			struct list_head *ptr;
-			list_for_each(ptr, &rdev->fence_drv.emited) {
-				/* count up to 3, that's enought info */
-				if (++not_processed >= 3)
-					break;
-			}
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			not_processed += radeon_fence_count_emitted(rdev, i);
+			if (not_processed >= 3)
+				break;
 		}
-		read_unlock_irqrestore(&rdev->fence_drv.lock, irq_flags);
 
 		if (not_processed >= 3) { /* should upclock */
 			if (rdev->pm.dynpm_planned_action == DYNPM_ACTION_DOWNCLOCK) {
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index 49d58202202c..e8bc70933d1b 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -34,6 +34,7 @@
 #include "atom.h"
 
 int radeon_debugfs_ib_init(struct radeon_device *rdev);
+int radeon_debugfs_ring_init(struct radeon_device *rdev);
 
 u32 radeon_get_ib_value(struct radeon_cs_parser *p, int idx)
 {
@@ -60,105 +61,106 @@ u32 radeon_get_ib_value(struct radeon_cs_parser *p, int idx)
 	return idx_value;
 }
 
-void radeon_ring_write(struct radeon_device *rdev, uint32_t v)
+void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
 {
 #if DRM_DEBUG_CODE
-	if (rdev->cp.count_dw <= 0) {
+	if (ring->count_dw <= 0) {
 		DRM_ERROR("radeon: writting more dword to ring than expected !\n");
 	}
 #endif
-	rdev->cp.ring[rdev->cp.wptr++] = v;
-	rdev->cp.wptr &= rdev->cp.ptr_mask;
-	rdev->cp.count_dw--;
-	rdev->cp.ring_free_dw--;
+	ring->ring[ring->wptr++] = v;
+	ring->wptr &= ring->ptr_mask;
+	ring->count_dw--;
+	ring->ring_free_dw--;
 }
 
-void radeon_ib_bogus_cleanup(struct radeon_device *rdev)
-{
-	struct radeon_ib *ib, *n;
-
-	list_for_each_entry_safe(ib, n, &rdev->ib_pool.bogus_ib, list) {
-		list_del(&ib->list);
-		vfree(ib->ptr);
-		kfree(ib);
-	}
-}
-
-void radeon_ib_bogus_add(struct radeon_device *rdev, struct radeon_ib *ib)
+/*
+ * IB.
+ */
+bool radeon_ib_try_free(struct radeon_device *rdev, struct radeon_ib *ib)
 {
-	struct radeon_ib *bib;
-
-	bib = kmalloc(sizeof(*bib), GFP_KERNEL);
-	if (bib == NULL)
-		return;
-	bib->ptr = vmalloc(ib->length_dw * 4);
-	if (bib->ptr == NULL) {
-		kfree(bib);
-		return;
+	bool done = false;
+
+	/* only free ib which have been emited */
+	if (ib->fence && ib->fence->emitted) {
+		if (radeon_fence_signaled(ib->fence)) {
+			radeon_fence_unref(&ib->fence);
+			radeon_sa_bo_free(rdev, &ib->sa_bo);
+			done = true;
+		}
 	}
-	memcpy(bib->ptr, ib->ptr, ib->length_dw * 4);
-	bib->length_dw = ib->length_dw;
-	mutex_lock(&rdev->ib_pool.mutex);
-	list_add_tail(&bib->list, &rdev->ib_pool.bogus_ib);
-	mutex_unlock(&rdev->ib_pool.mutex);
+	return done;
 }
 
-/*
- * IB.
- */
-int radeon_ib_get(struct radeon_device *rdev, struct radeon_ib **ib)
+int radeon_ib_get(struct radeon_device *rdev, int ring,
+		  struct radeon_ib **ib, unsigned size)
 {
 	struct radeon_fence *fence;
-	struct radeon_ib *nib;
-	int r = 0, i, c;
+	unsigned cretry = 0;
+	int r = 0, i, idx;
 
 	*ib = NULL;
-	r = radeon_fence_create(rdev, &fence);
+	/* align size on 256 bytes */
+	size = ALIGN(size, 256);
+
+	r = radeon_fence_create(rdev, &fence, ring);
 	if (r) {
 		dev_err(rdev->dev, "failed to create fence for new IB\n");
 		return r;
 	}
+
 	mutex_lock(&rdev->ib_pool.mutex);
-	for (i = rdev->ib_pool.head_id, c = 0, nib = NULL; c < RADEON_IB_POOL_SIZE; c++, i++) {
-		i &= (RADEON_IB_POOL_SIZE - 1);
-		if (rdev->ib_pool.ibs[i].free) {
-			nib = &rdev->ib_pool.ibs[i];
-			break;
-		}
-	}
-	if (nib == NULL) {
-		/* This should never happen, it means we allocated all
-		 * IB and haven't scheduled one yet, return EBUSY to
-		 * userspace hoping that on ioctl recall we get better
-		 * luck
-		 */
-		dev_err(rdev->dev, "no free indirect buffer !\n");
+	idx = rdev->ib_pool.head_id;
+retry:
+	if (cretry > 5) {
+		dev_err(rdev->dev, "failed to get an ib after 5 retry\n");
 		mutex_unlock(&rdev->ib_pool.mutex);
 		radeon_fence_unref(&fence);
-		return -EBUSY;
+		return -ENOMEM;
 	}
-	rdev->ib_pool.head_id = (nib->idx + 1) & (RADEON_IB_POOL_SIZE - 1);
-	nib->free = false;
-	if (nib->fence) {
-		mutex_unlock(&rdev->ib_pool.mutex);
-		r = radeon_fence_wait(nib->fence, false);
-		if (r) {
-			dev_err(rdev->dev, "error waiting fence of IB(%u:0x%016lX:%u)\n",
-				nib->idx, (unsigned long)nib->gpu_addr, nib->length_dw);
-			mutex_lock(&rdev->ib_pool.mutex);
-			nib->free = true;
-			mutex_unlock(&rdev->ib_pool.mutex);
-			radeon_fence_unref(&fence);
-			return r;
+	cretry++;
+	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
+		radeon_ib_try_free(rdev, &rdev->ib_pool.ibs[idx]);
+		if (rdev->ib_pool.ibs[idx].fence == NULL) {
+			r = radeon_sa_bo_new(rdev, &rdev->ib_pool.sa_manager,
+					     &rdev->ib_pool.ibs[idx].sa_bo,
+					     size, 256);
+			if (!r) {
+				*ib = &rdev->ib_pool.ibs[idx];
+				(*ib)->ptr = rdev->ib_pool.sa_manager.cpu_ptr;
+				(*ib)->ptr += ((*ib)->sa_bo.offset >> 2);
+				(*ib)->gpu_addr = rdev->ib_pool.sa_manager.gpu_addr;
+				(*ib)->gpu_addr += (*ib)->sa_bo.offset;
+				(*ib)->fence = fence;
+				(*ib)->vm_id = 0;
+				/* ib are most likely to be allocated in a ring fashion
+				 * thus rdev->ib_pool.head_id should be the id of the
+				 * oldest ib
+				 */
+				rdev->ib_pool.head_id = (1 + idx);
+				rdev->ib_pool.head_id &= (RADEON_IB_POOL_SIZE - 1);
+				mutex_unlock(&rdev->ib_pool.mutex);
+				return 0;
+			}
 		}
-		mutex_lock(&rdev->ib_pool.mutex);
+		idx = (idx + 1) & (RADEON_IB_POOL_SIZE - 1);
+	}
+	/* this should be rare event, ie all ib scheduled none signaled yet.
+	 */
+	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
+		if (rdev->ib_pool.ibs[idx].fence && rdev->ib_pool.ibs[idx].fence->emitted) {
+			r = radeon_fence_wait(rdev->ib_pool.ibs[idx].fence, false);
+			if (!r) {
+				goto retry;
+			}
+			/* an error happened */
+			break;
+		}
+		idx = (idx + 1) & (RADEON_IB_POOL_SIZE - 1);
 	}
-	radeon_fence_unref(&nib->fence);
-	nib->fence = fence;
-	nib->length_dw = 0;
 	mutex_unlock(&rdev->ib_pool.mutex);
-	*ib = nib;
-	return 0;
+	radeon_fence_unref(&fence);
+	return r;
 }
 
 void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib **ib)
@@ -169,247 +171,255 @@ void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib **ib)
 	if (tmp == NULL) {
 		return;
 	}
-	if (!tmp->fence->emited)
-		radeon_fence_unref(&tmp->fence);
 	mutex_lock(&rdev->ib_pool.mutex);
-	tmp->free = true;
+	if (tmp->fence && !tmp->fence->emitted) {
+		radeon_sa_bo_free(rdev, &tmp->sa_bo);
+		radeon_fence_unref(&tmp->fence);
+	}
 	mutex_unlock(&rdev->ib_pool.mutex);
 }
 
 int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib)
 {
+	struct radeon_ring *ring = &rdev->ring[ib->fence->ring];
 	int r = 0;
 
-	if (!ib->length_dw || !rdev->cp.ready) {
+	if (!ib->length_dw || !ring->ready) {
 		/* TODO: Nothings in the ib we should report. */
 		DRM_ERROR("radeon: couldn't schedule IB(%u).\n", ib->idx);
 		return -EINVAL;
 	}
 
 	/* 64 dwords should be enough for fence too */
-	r = radeon_ring_lock(rdev, 64);
+	r = radeon_ring_lock(rdev, ring, 64);
 	if (r) {
 		DRM_ERROR("radeon: scheduling IB failed (%d).\n", r);
 		return r;
 	}
-	radeon_ring_ib_execute(rdev, ib);
+	radeon_ring_ib_execute(rdev, ib->fence->ring, ib);
 	radeon_fence_emit(rdev, ib->fence);
-	mutex_lock(&rdev->ib_pool.mutex);
-	/* once scheduled IB is considered free and protected by the fence */
-	ib->free = true;
-	mutex_unlock(&rdev->ib_pool.mutex);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_unlock_commit(rdev, ring);
 	return 0;
 }
 
 int radeon_ib_pool_init(struct radeon_device *rdev)
 {
-	void *ptr;
-	uint64_t gpu_addr;
-	int i;
-	int r = 0;
+	int i, r;
 
-	if (rdev->ib_pool.robj)
+	mutex_lock(&rdev->ib_pool.mutex);
+	if (rdev->ib_pool.ready) {
+		mutex_unlock(&rdev->ib_pool.mutex);
 		return 0;
-	INIT_LIST_HEAD(&rdev->ib_pool.bogus_ib);
-	/* Allocate 1M object buffer */
-	r = radeon_bo_create(rdev, RADEON_IB_POOL_SIZE*64*1024,
-			     PAGE_SIZE, true, RADEON_GEM_DOMAIN_GTT,
-			     &rdev->ib_pool.robj);
-	if (r) {
-		DRM_ERROR("radeon: failed to ib pool (%d).\n", r);
-		return r;
 	}
-	r = radeon_bo_reserve(rdev->ib_pool.robj, false);
-	if (unlikely(r != 0))
-		return r;
-	r = radeon_bo_pin(rdev->ib_pool.robj, RADEON_GEM_DOMAIN_GTT, &gpu_addr);
-	if (r) {
-		radeon_bo_unreserve(rdev->ib_pool.robj);
-		DRM_ERROR("radeon: failed to pin ib pool (%d).\n", r);
-		return r;
-	}
-	r = radeon_bo_kmap(rdev->ib_pool.robj, &ptr);
-	radeon_bo_unreserve(rdev->ib_pool.robj);
+
+	r = radeon_sa_bo_manager_init(rdev, &rdev->ib_pool.sa_manager,
+				      RADEON_IB_POOL_SIZE*64*1024,
+				      RADEON_GEM_DOMAIN_GTT);
 	if (r) {
-		DRM_ERROR("radeon: failed to map ib pool (%d).\n", r);
+		mutex_unlock(&rdev->ib_pool.mutex);
 		return r;
 	}
-	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
-		unsigned offset;
 
-		offset = i * 64 * 1024;
-		rdev->ib_pool.ibs[i].gpu_addr = gpu_addr + offset;
-		rdev->ib_pool.ibs[i].ptr = ptr + offset;
+	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
+		rdev->ib_pool.ibs[i].fence = NULL;
 		rdev->ib_pool.ibs[i].idx = i;
 		rdev->ib_pool.ibs[i].length_dw = 0;
-		rdev->ib_pool.ibs[i].free = true;
+		INIT_LIST_HEAD(&rdev->ib_pool.ibs[i].sa_bo.list);
 	}
 	rdev->ib_pool.head_id = 0;
 	rdev->ib_pool.ready = true;
 	DRM_INFO("radeon: ib pool ready.\n");
+
 	if (radeon_debugfs_ib_init(rdev)) {
 		DRM_ERROR("Failed to register debugfs file for IB !\n");
 	}
-	return r;
+	if (radeon_debugfs_ring_init(rdev)) {
+		DRM_ERROR("Failed to register debugfs file for rings !\n");
+	}
+	mutex_unlock(&rdev->ib_pool.mutex);
+	return 0;
 }
 
 void radeon_ib_pool_fini(struct radeon_device *rdev)
 {
-	int r;
-	struct radeon_bo *robj;
+	unsigned i;
 
-	if (!rdev->ib_pool.ready) {
-		return;
-	}
 	mutex_lock(&rdev->ib_pool.mutex);
-	radeon_ib_bogus_cleanup(rdev);
-	robj = rdev->ib_pool.robj;
-	rdev->ib_pool.robj = NULL;
-	mutex_unlock(&rdev->ib_pool.mutex);
-
-	if (robj) {
-		r = radeon_bo_reserve(robj, false);
-		if (likely(r == 0)) {
-			radeon_bo_kunmap(robj);
-			radeon_bo_unpin(robj);
-			radeon_bo_unreserve(robj);
+	if (rdev->ib_pool.ready) {
+		for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
+			radeon_sa_bo_free(rdev, &rdev->ib_pool.ibs[i].sa_bo);
+			radeon_fence_unref(&rdev->ib_pool.ibs[i].fence);
 		}
-		radeon_bo_unref(&robj);
+		radeon_sa_bo_manager_fini(rdev, &rdev->ib_pool.sa_manager);
+		rdev->ib_pool.ready = false;
 	}
+	mutex_unlock(&rdev->ib_pool.mutex);
 }
 
+int radeon_ib_pool_start(struct radeon_device *rdev)
+{
+	return radeon_sa_bo_manager_start(rdev, &rdev->ib_pool.sa_manager);
+}
+
+int radeon_ib_pool_suspend(struct radeon_device *rdev)
+{
+	return radeon_sa_bo_manager_suspend(rdev, &rdev->ib_pool.sa_manager);
+}
 
 /*
  * Ring.
  */
-void radeon_ring_free_size(struct radeon_device *rdev)
+int radeon_ring_index(struct radeon_device *rdev, struct radeon_ring *ring)
 {
-	if (rdev->wb.enabled)
-		rdev->cp.rptr = le32_to_cpu(rdev->wb.wb[RADEON_WB_CP_RPTR_OFFSET/4]);
-	else {
-		if (rdev->family >= CHIP_R600)
-			rdev->cp.rptr = RREG32(R600_CP_RB_RPTR);
-		else
-			rdev->cp.rptr = RREG32(RADEON_CP_RB_RPTR);
+	/* r1xx-r5xx only has CP ring */
+	if (rdev->family < CHIP_R600)
+		return RADEON_RING_TYPE_GFX_INDEX;
+
+	if (rdev->family >= CHIP_CAYMAN) {
+		if (ring == &rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX])
+			return CAYMAN_RING_TYPE_CP1_INDEX;
+		else if (ring == &rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX])
+			return CAYMAN_RING_TYPE_CP2_INDEX;
 	}
+	return RADEON_RING_TYPE_GFX_INDEX;
+}
+
+void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	u32 rptr;
+
+	if (rdev->wb.enabled)
+		rptr = le32_to_cpu(rdev->wb.wb[ring->rptr_offs/4]);
+	else
+		rptr = RREG32(ring->rptr_reg);
+	ring->rptr = (rptr & ring->ptr_reg_mask) >> ring->ptr_reg_shift;
 	/* This works because ring_size is a power of 2 */
-	rdev->cp.ring_free_dw = (rdev->cp.rptr + (rdev->cp.ring_size / 4));
-	rdev->cp.ring_free_dw -= rdev->cp.wptr;
-	rdev->cp.ring_free_dw &= rdev->cp.ptr_mask;
-	if (!rdev->cp.ring_free_dw) {
-		rdev->cp.ring_free_dw = rdev->cp.ring_size / 4;
+	ring->ring_free_dw = (ring->rptr + (ring->ring_size / 4));
+	ring->ring_free_dw -= ring->wptr;
+	ring->ring_free_dw &= ring->ptr_mask;
+	if (!ring->ring_free_dw) {
+		ring->ring_free_dw = ring->ring_size / 4;
 	}
 }
 
-int radeon_ring_alloc(struct radeon_device *rdev, unsigned ndw)
+
+int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ndw)
 {
 	int r;
 
 	/* Align requested size with padding so unlock_commit can
 	 * pad safely */
-	ndw = (ndw + rdev->cp.align_mask) & ~rdev->cp.align_mask;
-	while (ndw > (rdev->cp.ring_free_dw - 1)) {
-		radeon_ring_free_size(rdev);
-		if (ndw < rdev->cp.ring_free_dw) {
+	ndw = (ndw + ring->align_mask) & ~ring->align_mask;
+	while (ndw > (ring->ring_free_dw - 1)) {
+		radeon_ring_free_size(rdev, ring);
+		if (ndw < ring->ring_free_dw) {
 			break;
 		}
-		r = radeon_fence_wait_next(rdev);
+		r = radeon_fence_wait_next(rdev, radeon_ring_index(rdev, ring));
 		if (r)
 			return r;
 	}
-	rdev->cp.count_dw = ndw;
-	rdev->cp.wptr_old = rdev->cp.wptr;
+	ring->count_dw = ndw;
+	ring->wptr_old = ring->wptr;
 	return 0;
 }
 
-int radeon_ring_lock(struct radeon_device *rdev, unsigned ndw)
+int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ndw)
 {
 	int r;
 
-	mutex_lock(&rdev->cp.mutex);
-	r = radeon_ring_alloc(rdev, ndw);
+	mutex_lock(&ring->mutex);
+	r = radeon_ring_alloc(rdev, ring, ndw);
 	if (r) {
-		mutex_unlock(&rdev->cp.mutex);
+		mutex_unlock(&ring->mutex);
 		return r;
 	}
 	return 0;
 }
 
-void radeon_ring_commit(struct radeon_device *rdev)
+void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	unsigned count_dw_pad;
 	unsigned i;
 
 	/* We pad to match fetch size */
-	count_dw_pad = (rdev->cp.align_mask + 1) -
-		       (rdev->cp.wptr & rdev->cp.align_mask);
+	count_dw_pad = (ring->align_mask + 1) -
+		       (ring->wptr & ring->align_mask);
 	for (i = 0; i < count_dw_pad; i++) {
-		radeon_ring_write(rdev, 2 << 30);
+		radeon_ring_write(ring, ring->nop);
 	}
 	DRM_MEMORYBARRIER();
-	radeon_cp_commit(rdev);
+	WREG32(ring->wptr_reg, (ring->wptr << ring->ptr_reg_shift) & ring->ptr_reg_mask);
+	(void)RREG32(ring->wptr_reg);
 }
 
-void radeon_ring_unlock_commit(struct radeon_device *rdev)
+void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 {
-	radeon_ring_commit(rdev);
-	mutex_unlock(&rdev->cp.mutex);
+	radeon_ring_commit(rdev, ring);
+	mutex_unlock(&ring->mutex);
 }
 
-void radeon_ring_unlock_undo(struct radeon_device *rdev)
+void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *ring)
 {
-	rdev->cp.wptr = rdev->cp.wptr_old;
-	mutex_unlock(&rdev->cp.mutex);
+	ring->wptr = ring->wptr_old;
+	mutex_unlock(&ring->mutex);
 }
 
-int radeon_ring_init(struct radeon_device *rdev, unsigned ring_size)
+int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsigned ring_size,
+		     unsigned rptr_offs, unsigned rptr_reg, unsigned wptr_reg,
+		     u32 ptr_reg_shift, u32 ptr_reg_mask, u32 nop)
 {
 	int r;
 
-	rdev->cp.ring_size = ring_size;
+	ring->ring_size = ring_size;
+	ring->rptr_offs = rptr_offs;
+	ring->rptr_reg = rptr_reg;
+	ring->wptr_reg = wptr_reg;
+	ring->ptr_reg_shift = ptr_reg_shift;
+	ring->ptr_reg_mask = ptr_reg_mask;
+	ring->nop = nop;
 	/* Allocate ring buffer */
-	if (rdev->cp.ring_obj == NULL) {
-		r = radeon_bo_create(rdev, rdev->cp.ring_size, PAGE_SIZE, true,
+	if (ring->ring_obj == NULL) {
+		r = radeon_bo_create(rdev, ring->ring_size, PAGE_SIZE, true,
 					RADEON_GEM_DOMAIN_GTT,
-					&rdev->cp.ring_obj);
+					&ring->ring_obj);
 		if (r) {
 			dev_err(rdev->dev, "(%d) ring create failed\n", r);
 			return r;
 		}
-		r = radeon_bo_reserve(rdev->cp.ring_obj, false);
+		r = radeon_bo_reserve(ring->ring_obj, false);
 		if (unlikely(r != 0))
 			return r;
-		r = radeon_bo_pin(rdev->cp.ring_obj, RADEON_GEM_DOMAIN_GTT,
-					&rdev->cp.gpu_addr);
+		r = radeon_bo_pin(ring->ring_obj, RADEON_GEM_DOMAIN_GTT,
+					&ring->gpu_addr);
 		if (r) {
-			radeon_bo_unreserve(rdev->cp.ring_obj);
+			radeon_bo_unreserve(ring->ring_obj);
 			dev_err(rdev->dev, "(%d) ring pin failed\n", r);
 			return r;
 		}
-		r = radeon_bo_kmap(rdev->cp.ring_obj,
-				       (void **)&rdev->cp.ring);
-		radeon_bo_unreserve(rdev->cp.ring_obj);
+		r = radeon_bo_kmap(ring->ring_obj,
+				       (void **)&ring->ring);
+		radeon_bo_unreserve(ring->ring_obj);
 		if (r) {
 			dev_err(rdev->dev, "(%d) ring map failed\n", r);
 			return r;
 		}
 	}
-	rdev->cp.ptr_mask = (rdev->cp.ring_size / 4) - 1;
-	rdev->cp.ring_free_dw = rdev->cp.ring_size / 4;
+	ring->ptr_mask = (ring->ring_size / 4) - 1;
+	ring->ring_free_dw = ring->ring_size / 4;
 	return 0;
 }
 
-void radeon_ring_fini(struct radeon_device *rdev)
+void radeon_ring_fini(struct radeon_device *rdev, struct radeon_ring *ring)
 {
 	int r;
 	struct radeon_bo *ring_obj;
 
-	mutex_lock(&rdev->cp.mutex);
-	ring_obj = rdev->cp.ring_obj;
-	rdev->cp.ring = NULL;
-	rdev->cp.ring_obj = NULL;
-	mutex_unlock(&rdev->cp.mutex);
+	mutex_lock(&ring->mutex);
+	ring_obj = ring->ring_obj;
+	ring->ring = NULL;
+	ring->ring_obj = NULL;
+	mutex_unlock(&ring->mutex);
 
 	if (ring_obj) {
 		r = radeon_bo_reserve(ring_obj, false);
@@ -422,72 +432,83 @@ void radeon_ring_fini(struct radeon_device *rdev)
 	}
 }
 
-
 /*
  * Debugfs info
  */
 #if defined(CONFIG_DEBUG_FS)
-static int radeon_debugfs_ib_info(struct seq_file *m, void *data)
+
+static int radeon_debugfs_ring_info(struct seq_file *m, void *data)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct radeon_ib *ib = node->info_ent->data;
-	unsigned i;
-
-	if (ib == NULL) {
-		return 0;
-	}
-	seq_printf(m, "IB %04u\n", ib->idx);
-	seq_printf(m, "IB fence %p\n", ib->fence);
-	seq_printf(m, "IB size %05u dwords\n", ib->length_dw);
-	for (i = 0; i < ib->length_dw; i++) {
-		seq_printf(m, "[%05u]=0x%08X\n", i, ib->ptr[i]);
+	struct drm_device *dev = node->minor->dev;
+	struct radeon_device *rdev = dev->dev_private;
+	int ridx = *(int*)node->info_ent->data;
+	struct radeon_ring *ring = &rdev->ring[ridx];
+	unsigned count, i, j;
+
+	radeon_ring_free_size(rdev, ring);
+	count = (ring->ring_size / 4) - ring->ring_free_dw;
+	seq_printf(m, "wptr(0x%04x): 0x%08x\n", ring->wptr_reg, RREG32(ring->wptr_reg));
+	seq_printf(m, "rptr(0x%04x): 0x%08x\n", ring->rptr_reg, RREG32(ring->rptr_reg));
+	seq_printf(m, "driver's copy of the wptr: 0x%08x\n", ring->wptr);
+	seq_printf(m, "driver's copy of the rptr: 0x%08x\n", ring->rptr);
+	seq_printf(m, "%u free dwords in ring\n", ring->ring_free_dw);
+	seq_printf(m, "%u dwords in ring\n", count);
+	i = ring->rptr;
+	for (j = 0; j <= count; j++) {
+		seq_printf(m, "r[%04d]=0x%08x\n", i, ring->ring[i]);
+		i = (i + 1) & ring->ptr_mask;
 	}
 	return 0;
 }
 
-static int radeon_debugfs_ib_bogus_info(struct seq_file *m, void *data)
+static int radeon_ring_type_gfx_index = RADEON_RING_TYPE_GFX_INDEX;
+static int cayman_ring_type_cp1_index = CAYMAN_RING_TYPE_CP1_INDEX;
+static int cayman_ring_type_cp2_index = CAYMAN_RING_TYPE_CP2_INDEX;
+
+static struct drm_info_list radeon_debugfs_ring_info_list[] = {
+	{"radeon_ring_gfx", radeon_debugfs_ring_info, 0, &radeon_ring_type_gfx_index},
+	{"radeon_ring_cp1", radeon_debugfs_ring_info, 0, &cayman_ring_type_cp1_index},
+	{"radeon_ring_cp2", radeon_debugfs_ring_info, 0, &cayman_ring_type_cp2_index},
+};
+
+static int radeon_debugfs_ib_info(struct seq_file *m, void *data)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct radeon_device *rdev = node->info_ent->data;
-	struct radeon_ib *ib;
+	struct radeon_ib *ib = node->info_ent->data;
 	unsigned i;
 
-	mutex_lock(&rdev->ib_pool.mutex);
-	if (list_empty(&rdev->ib_pool.bogus_ib)) {
-		mutex_unlock(&rdev->ib_pool.mutex);
-		seq_printf(m, "no bogus IB recorded\n");
+	if (ib == NULL) {
 		return 0;
 	}
-	ib = list_first_entry(&rdev->ib_pool.bogus_ib, struct radeon_ib, list);
-	list_del_init(&ib->list);
-	mutex_unlock(&rdev->ib_pool.mutex);
+	seq_printf(m, "IB %04u\n", ib->idx);
+	seq_printf(m, "IB fence %p\n", ib->fence);
 	seq_printf(m, "IB size %05u dwords\n", ib->length_dw);
 	for (i = 0; i < ib->length_dw; i++) {
 		seq_printf(m, "[%05u]=0x%08X\n", i, ib->ptr[i]);
 	}
-	vfree(ib->ptr);
-	kfree(ib);
 	return 0;
 }
 
 static struct drm_info_list radeon_debugfs_ib_list[RADEON_IB_POOL_SIZE];
 static char radeon_debugfs_ib_names[RADEON_IB_POOL_SIZE][32];
+#endif
 
-static struct drm_info_list radeon_debugfs_ib_bogus_info_list[] = {
-	{"radeon_ib_bogus", radeon_debugfs_ib_bogus_info, 0, NULL},
-};
+int radeon_debugfs_ring_init(struct radeon_device *rdev)
+{
+#if defined(CONFIG_DEBUG_FS)
+	return radeon_debugfs_add_files(rdev, radeon_debugfs_ring_info_list,
+					ARRAY_SIZE(radeon_debugfs_ring_info_list));
+#else
+	return 0;
 #endif
+}
 
 int radeon_debugfs_ib_init(struct radeon_device *rdev)
 {
 #if defined(CONFIG_DEBUG_FS)
 	unsigned i;
-	int r;
 
-	radeon_debugfs_ib_bogus_info_list[0].data = rdev;
-	r = radeon_debugfs_add_files(rdev, radeon_debugfs_ib_bogus_info_list, 1);
-	if (r)
-		return r;
 	for (i = 0; i < RADEON_IB_POOL_SIZE; i++) {
 		sprintf(radeon_debugfs_ib_names[i], "radeon_ib_%04u", i);
 		radeon_debugfs_ib_list[i].name = radeon_debugfs_ib_names[i];
diff --git a/drivers/gpu/drm/radeon/radeon_sa.c b/drivers/gpu/drm/radeon/radeon_sa.c
new file mode 100644
index 000000000000..4cce47e7dc0d
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_sa.c
@@ -0,0 +1,189 @@
+/*
+ * Copyright 2011 Red Hat Inc.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors:
+ *    Jerome Glisse <glisse@freedesktop.org>
+ */
+#include "drmP.h"
+#include "drm.h"
+#include "radeon.h"
+
+int radeon_sa_bo_manager_init(struct radeon_device *rdev,
+			      struct radeon_sa_manager *sa_manager,
+			      unsigned size, u32 domain)
+{
+	int r;
+
+	sa_manager->bo = NULL;
+	sa_manager->size = size;
+	sa_manager->domain = domain;
+	INIT_LIST_HEAD(&sa_manager->sa_bo);
+
+	r = radeon_bo_create(rdev, size, RADEON_GPU_PAGE_SIZE, true,
+			     RADEON_GEM_DOMAIN_CPU, &sa_manager->bo);
+	if (r) {
+		dev_err(rdev->dev, "(%d) failed to allocate bo for manager\n", r);
+		return r;
+	}
+
+	return r;
+}
+
+void radeon_sa_bo_manager_fini(struct radeon_device *rdev,
+			       struct radeon_sa_manager *sa_manager)
+{
+	struct radeon_sa_bo *sa_bo, *tmp;
+
+	if (!list_empty(&sa_manager->sa_bo)) {
+		dev_err(rdev->dev, "sa_manager is not empty, clearing anyway\n");
+	}
+	list_for_each_entry_safe(sa_bo, tmp, &sa_manager->sa_bo, list) {
+		list_del_init(&sa_bo->list);
+	}
+	radeon_bo_unref(&sa_manager->bo);
+	sa_manager->size = 0;
+}
+
+int radeon_sa_bo_manager_start(struct radeon_device *rdev,
+			       struct radeon_sa_manager *sa_manager)
+{
+	int r;
+
+	if (sa_manager->bo == NULL) {
+		dev_err(rdev->dev, "no bo for sa manager\n");
+		return -EINVAL;
+	}
+
+	/* map the buffer */
+	r = radeon_bo_reserve(sa_manager->bo, false);
+	if (r) {
+		dev_err(rdev->dev, "(%d) failed to reserve manager bo\n", r);
+		return r;
+	}
+	r = radeon_bo_pin(sa_manager->bo, sa_manager->domain, &sa_manager->gpu_addr);
+	if (r) {
+		radeon_bo_unreserve(sa_manager->bo);
+		dev_err(rdev->dev, "(%d) failed to pin manager bo\n", r);
+		return r;
+	}
+	r = radeon_bo_kmap(sa_manager->bo, &sa_manager->cpu_ptr);
+	radeon_bo_unreserve(sa_manager->bo);
+	return r;
+}
+
+int radeon_sa_bo_manager_suspend(struct radeon_device *rdev,
+				 struct radeon_sa_manager *sa_manager)
+{
+	int r;
+
+	if (sa_manager->bo == NULL) {
+		dev_err(rdev->dev, "no bo for sa manager\n");
+		return -EINVAL;
+	}
+
+	r = radeon_bo_reserve(sa_manager->bo, false);
+	if (!r) {
+		radeon_bo_kunmap(sa_manager->bo);
+		radeon_bo_unpin(sa_manager->bo);
+		radeon_bo_unreserve(sa_manager->bo);
+	}
+	return r;
+}
+
+/*
+ * Principe is simple, we keep a list of sub allocation in offset
+ * order (first entry has offset == 0, last entry has the highest
+ * offset).
+ *
+ * When allocating new object we first check if there is room at
+ * the end total_size - (last_object_offset + last_object_size) >=
+ * alloc_size. If so we allocate new object there.
+ *
+ * When there is not enough room at the end, we start waiting for
+ * each sub object until we reach object_offset+object_size >=
+ * alloc_size, this object then become the sub object we return.
+ *
+ * Alignment can't be bigger than page size
+ */
+int radeon_sa_bo_new(struct radeon_device *rdev,
+		     struct radeon_sa_manager *sa_manager,
+		     struct radeon_sa_bo *sa_bo,
+		     unsigned size, unsigned align)
+{
+	struct radeon_sa_bo *tmp;
+	struct list_head *head;
+	unsigned offset = 0, wasted = 0;
+
+	BUG_ON(align > RADEON_GPU_PAGE_SIZE);
+	BUG_ON(size > sa_manager->size);
+
+	/* no one ? */
+	head = sa_manager->sa_bo.prev;
+	if (list_empty(&sa_manager->sa_bo)) {
+		goto out;
+	}
+
+	/* look for a hole big enough */
+	offset = 0;
+	list_for_each_entry(tmp, &sa_manager->sa_bo, list) {
+		/* room before this object ? */
+		if ((tmp->offset - offset) >= size) {
+			head = tmp->list.prev;
+			goto out;
+		}
+		offset = tmp->offset + tmp->size;
+		wasted = offset % align;
+		if (wasted) {
+			wasted = align - wasted;
+		}
+		offset += wasted;
+	}
+	/* room at the end ? */
+	head = sa_manager->sa_bo.prev;
+	tmp = list_entry(head, struct radeon_sa_bo, list);
+	offset = tmp->offset + tmp->size;
+	wasted = offset % align;
+	if (wasted) {
+		wasted = align - wasted;
+	}
+	offset += wasted;
+	if ((sa_manager->size - offset) < size) {
+		/* failed to find somethings big enough */
+		return -ENOMEM;
+	}
+
+out:
+	sa_bo->manager = sa_manager;
+	sa_bo->offset = offset;
+	sa_bo->size = size;
+	list_add(&sa_bo->list, head);
+	return 0;
+}
+
+void radeon_sa_bo_free(struct radeon_device *rdev, struct radeon_sa_bo *sa_bo)
+{
+	list_del_init(&sa_bo->list);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_semaphore.c b/drivers/gpu/drm/radeon/radeon_semaphore.c
new file mode 100644
index 000000000000..61dd4e3c9209
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_semaphore.c
@@ -0,0 +1,178 @@
+/*
+ * Copyright 2011 Christian König.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors:
+ *    Christian König <deathsimple@vodafone.de>
+ */
+#include "drmP.h"
+#include "drm.h"
+#include "radeon.h"
+
+static int radeon_semaphore_add_bo(struct radeon_device *rdev)
+{
+	struct radeon_semaphore_bo *bo;
+	unsigned long irq_flags;
+	uint64_t gpu_addr;
+	uint32_t *cpu_ptr;
+	int r, i;
+
+
+	bo = kmalloc(sizeof(struct radeon_semaphore_bo), GFP_KERNEL);
+	if (bo == NULL) {
+		return -ENOMEM;
+	}
+	INIT_LIST_HEAD(&bo->free);
+	INIT_LIST_HEAD(&bo->list);
+	bo->nused = 0;
+
+	r = radeon_ib_get(rdev, 0, &bo->ib, RADEON_SEMAPHORE_BO_SIZE);
+	if (r) {
+		dev_err(rdev->dev, "failed to get a bo after 5 retry\n");
+		kfree(bo);
+		return r;
+	}
+	gpu_addr = rdev->ib_pool.sa_manager.gpu_addr;
+	gpu_addr += bo->ib->sa_bo.offset;
+	cpu_ptr = rdev->ib_pool.sa_manager.cpu_ptr;
+	cpu_ptr += (bo->ib->sa_bo.offset >> 2);
+	for (i = 0; i < (RADEON_SEMAPHORE_BO_SIZE/8); i++) {
+		bo->semaphores[i].gpu_addr = gpu_addr;
+		bo->semaphores[i].cpu_ptr = cpu_ptr;
+		bo->semaphores[i].bo = bo;
+		list_add_tail(&bo->semaphores[i].list, &bo->free);
+		gpu_addr += 8;
+		cpu_ptr += 2;
+	}
+	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
+	list_add_tail(&bo->list, &rdev->semaphore_drv.bo);
+	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
+	return 0;
+}
+
+static void radeon_semaphore_del_bo_locked(struct radeon_device *rdev,
+					   struct radeon_semaphore_bo *bo)
+{
+	radeon_sa_bo_free(rdev, &bo->ib->sa_bo);
+	radeon_fence_unref(&bo->ib->fence);
+	list_del(&bo->list);
+	kfree(bo);
+}
+
+void radeon_semaphore_shrink_locked(struct radeon_device *rdev)
+{
+	struct radeon_semaphore_bo *bo, *n;
+
+	if (list_empty(&rdev->semaphore_drv.bo)) {
+		return;
+	}
+	/* only shrink if first bo has free semaphore */
+	bo = list_first_entry(&rdev->semaphore_drv.bo, struct radeon_semaphore_bo, list);
+	if (list_empty(&bo->free)) {
+		return;
+	}
+	list_for_each_entry_safe_continue(bo, n, &rdev->semaphore_drv.bo, list) {
+		if (bo->nused)
+			continue;
+		radeon_semaphore_del_bo_locked(rdev, bo);
+	}
+}
+
+int radeon_semaphore_create(struct radeon_device *rdev,
+			    struct radeon_semaphore **semaphore)
+{
+	struct radeon_semaphore_bo *bo;
+	unsigned long irq_flags;
+	bool do_retry = true;
+	int r;
+
+retry:
+	*semaphore = NULL;
+	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
+	list_for_each_entry(bo, &rdev->semaphore_drv.bo, list) {
+		if (list_empty(&bo->free))
+			continue;
+		*semaphore = list_first_entry(&bo->free, struct radeon_semaphore, list);
+		(*semaphore)->cpu_ptr[0] = 0;
+		(*semaphore)->cpu_ptr[1] = 0;
+		list_del(&(*semaphore)->list);
+		bo->nused++;
+		break;
+	}
+	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
+
+	if (*semaphore == NULL) {
+		if (do_retry) {
+			do_retry = false;
+			r = radeon_semaphore_add_bo(rdev);
+			if (r)
+				return r;
+			goto retry;
+		}
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void radeon_semaphore_emit_signal(struct radeon_device *rdev, int ring,
+			          struct radeon_semaphore *semaphore)
+{
+	radeon_semaphore_ring_emit(rdev, ring, &rdev->ring[ring], semaphore, false);
+}
+
+void radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
+			        struct radeon_semaphore *semaphore)
+{
+	radeon_semaphore_ring_emit(rdev, ring, &rdev->ring[ring], semaphore, true);
+}
+
+void radeon_semaphore_free(struct radeon_device *rdev,
+			   struct radeon_semaphore *semaphore)
+{
+	unsigned long irq_flags;
+
+	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
+	semaphore->bo->nused--;
+	list_add_tail(&semaphore->list, &semaphore->bo->free);
+	radeon_semaphore_shrink_locked(rdev);
+	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
+}
+
+void radeon_semaphore_driver_fini(struct radeon_device *rdev)
+{
+	struct radeon_semaphore_bo *bo, *n;
+	unsigned long irq_flags;
+
+	write_lock_irqsave(&rdev->semaphore_drv.lock, irq_flags);
+	/* we force to free everything */
+	list_for_each_entry_safe(bo, n, &rdev->semaphore_drv.bo, list) {
+		if (!list_empty(&bo->free)) {
+			dev_err(rdev->dev, "still in use semaphore\n");
+		}
+		radeon_semaphore_del_bo_locked(rdev, bo);
+	}
+	write_unlock_irqrestore(&rdev->semaphore_drv.lock, irq_flags);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_test.c b/drivers/gpu/drm/radeon/radeon_test.c
index 602fa3541c45..dc5dcf483aa3 100644
--- a/drivers/gpu/drm/radeon/radeon_test.c
+++ b/drivers/gpu/drm/radeon/radeon_test.c
@@ -42,7 +42,9 @@ void radeon_test_moves(struct radeon_device *rdev)
 	/* Number of tests =
 	 * (Total GTT - IB pool - writeback page - ring buffers) / test size
 	 */
-	n = rdev->mc.gtt_size - RADEON_IB_POOL_SIZE*64*1024 - rdev->cp.ring_size;
+	n = rdev->mc.gtt_size - RADEON_IB_POOL_SIZE*64*1024;
+	for (i = 0; i < RADEON_NUM_RINGS; ++i)
+		n -= rdev->ring[i].ring_size;
 	if (rdev->wb.wb_obj)
 		n -= RADEON_GPU_PAGE_SIZE;
 	if (rdev->ih.ring_obj)
@@ -104,7 +106,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 
 		radeon_bo_kunmap(gtt_obj[i]);
 
-		r = radeon_fence_create(rdev, &fence);
+		r = radeon_fence_create(rdev, &fence, RADEON_RING_TYPE_GFX_INDEX);
 		if (r) {
 			DRM_ERROR("Failed to create GTT->VRAM fence %d\n", i);
 			goto out_cleanup;
@@ -153,7 +155,7 @@ void radeon_test_moves(struct radeon_device *rdev)
 
 		radeon_bo_kunmap(vram_obj);
 
-		r = radeon_fence_create(rdev, &fence);
+		r = radeon_fence_create(rdev, &fence, RADEON_RING_TYPE_GFX_INDEX);
 		if (r) {
 			DRM_ERROR("Failed to create VRAM->GTT fence %d\n", i);
 			goto out_cleanup;
@@ -232,3 +234,264 @@ out_cleanup:
 		printk(KERN_WARNING "Error while testing BO move.\n");
 	}
 }
+
+void radeon_test_ring_sync(struct radeon_device *rdev,
+			   struct radeon_ring *ringA,
+			   struct radeon_ring *ringB)
+{
+	struct radeon_fence *fence1 = NULL, *fence2 = NULL;
+	struct radeon_semaphore *semaphore = NULL;
+	int ridxA = radeon_ring_index(rdev, ringA);
+	int ridxB = radeon_ring_index(rdev, ringB);
+	int r;
+
+	r = radeon_fence_create(rdev, &fence1, ridxA);
+	if (r) {
+		DRM_ERROR("Failed to create sync fence 1\n");
+		goto out_cleanup;
+	}
+	r = radeon_fence_create(rdev, &fence2, ridxA);
+	if (r) {
+		DRM_ERROR("Failed to create sync fence 2\n");
+		goto out_cleanup;
+	}
+
+	r = radeon_semaphore_create(rdev, &semaphore);
+	if (r) {
+		DRM_ERROR("Failed to create semaphore\n");
+		goto out_cleanup;
+	}
+
+	r = radeon_ring_lock(rdev, ringA, 64);
+	if (r) {
+		DRM_ERROR("Failed to lock ring A %d\n", ridxA);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_wait(rdev, ridxA, semaphore);
+	radeon_fence_emit(rdev, fence1);
+	radeon_semaphore_emit_wait(rdev, ridxA, semaphore);
+	radeon_fence_emit(rdev, fence2);
+	radeon_ring_unlock_commit(rdev, ringA);
+
+	mdelay(1000);
+
+	if (radeon_fence_signaled(fence1)) {
+		DRM_ERROR("Fence 1 signaled without waiting for semaphore.\n");
+		goto out_cleanup;
+	}
+
+	r = radeon_ring_lock(rdev, ringB, 64);
+	if (r) {
+		DRM_ERROR("Failed to lock ring B %p\n", ringB);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_signal(rdev, ridxB, semaphore);
+	radeon_ring_unlock_commit(rdev, ringB);
+
+	r = radeon_fence_wait(fence1, false);
+	if (r) {
+		DRM_ERROR("Failed to wait for sync fence 1\n");
+		goto out_cleanup;
+	}
+
+	mdelay(1000);
+
+	if (radeon_fence_signaled(fence2)) {
+		DRM_ERROR("Fence 2 signaled without waiting for semaphore.\n");
+		goto out_cleanup;
+	}
+
+	r = radeon_ring_lock(rdev, ringB, 64);
+	if (r) {
+		DRM_ERROR("Failed to lock ring B %p\n", ringB);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_signal(rdev, ridxB, semaphore);
+	radeon_ring_unlock_commit(rdev, ringB);
+
+	r = radeon_fence_wait(fence2, false);
+	if (r) {
+		DRM_ERROR("Failed to wait for sync fence 1\n");
+		goto out_cleanup;
+	}
+
+out_cleanup:
+	if (semaphore)
+		radeon_semaphore_free(rdev, semaphore);
+
+	if (fence1)
+		radeon_fence_unref(&fence1);
+
+	if (fence2)
+		radeon_fence_unref(&fence2);
+
+	if (r)
+		printk(KERN_WARNING "Error while testing ring sync (%d).\n", r);
+}
+
+void radeon_test_ring_sync2(struct radeon_device *rdev,
+			    struct radeon_ring *ringA,
+			    struct radeon_ring *ringB,
+			    struct radeon_ring *ringC)
+{
+	struct radeon_fence *fenceA = NULL, *fenceB = NULL;
+	struct radeon_semaphore *semaphore = NULL;
+	int ridxA = radeon_ring_index(rdev, ringA);
+	int ridxB = radeon_ring_index(rdev, ringB);
+	int ridxC = radeon_ring_index(rdev, ringC);
+	bool sigA, sigB;
+	int i, r;
+
+	r = radeon_fence_create(rdev, &fenceA, ridxA);
+	if (r) {
+		DRM_ERROR("Failed to create sync fence 1\n");
+		goto out_cleanup;
+	}
+	r = radeon_fence_create(rdev, &fenceB, ridxB);
+	if (r) {
+		DRM_ERROR("Failed to create sync fence 2\n");
+		goto out_cleanup;
+	}
+
+	r = radeon_semaphore_create(rdev, &semaphore);
+	if (r) {
+		DRM_ERROR("Failed to create semaphore\n");
+		goto out_cleanup;
+	}
+
+	r = radeon_ring_lock(rdev, ringA, 64);
+	if (r) {
+		DRM_ERROR("Failed to lock ring A %d\n", ridxA);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_wait(rdev, ridxA, semaphore);
+	radeon_fence_emit(rdev, fenceA);
+	radeon_ring_unlock_commit(rdev, ringA);
+
+	r = radeon_ring_lock(rdev, ringB, 64);
+	if (r) {
+		DRM_ERROR("Failed to lock ring B %d\n", ridxB);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_wait(rdev, ridxB, semaphore);
+	radeon_fence_emit(rdev, fenceB);
+	radeon_ring_unlock_commit(rdev, ringB);
+
+	mdelay(1000);
+
+	if (radeon_fence_signaled(fenceA)) {
+		DRM_ERROR("Fence A signaled without waiting for semaphore.\n");
+		goto out_cleanup;
+	}
+	if (radeon_fence_signaled(fenceB)) {
+		DRM_ERROR("Fence A signaled without waiting for semaphore.\n");
+		goto out_cleanup;
+	}
+
+	r = radeon_ring_lock(rdev, ringC, 64);
+	if (r) {
+		DRM_ERROR("Failed to lock ring B %p\n", ringC);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_signal(rdev, ridxC, semaphore);
+	radeon_ring_unlock_commit(rdev, ringC);
+
+	for (i = 0; i < 30; ++i) {
+		mdelay(100);
+		sigA = radeon_fence_signaled(fenceA);
+		sigB = radeon_fence_signaled(fenceB);
+		if (sigA || sigB)
+			break;
+	}
+
+	if (!sigA && !sigB) {
+		DRM_ERROR("Neither fence A nor B has been signaled\n");
+		goto out_cleanup;
+	} else if (sigA && sigB) {
+		DRM_ERROR("Both fence A and B has been signaled\n");
+		goto out_cleanup;
+	}
+
+	DRM_INFO("Fence %c was first signaled\n", sigA ? 'A' : 'B');
+
+	r = radeon_ring_lock(rdev, ringC, 64);
+	if (r) {
+		DRM_ERROR("Failed to lock ring B %p\n", ringC);
+		goto out_cleanup;
+	}
+	radeon_semaphore_emit_signal(rdev, ridxC, semaphore);
+	radeon_ring_unlock_commit(rdev, ringC);
+
+	mdelay(1000);
+
+	r = radeon_fence_wait(fenceA, false);
+	if (r) {
+		DRM_ERROR("Failed to wait for sync fence A\n");
+		goto out_cleanup;
+	}
+	r = radeon_fence_wait(fenceB, false);
+	if (r) {
+		DRM_ERROR("Failed to wait for sync fence B\n");
+		goto out_cleanup;
+	}
+
+out_cleanup:
+	if (semaphore)
+		radeon_semaphore_free(rdev, semaphore);
+
+	if (fenceA)
+		radeon_fence_unref(&fenceA);
+
+	if (fenceB)
+		radeon_fence_unref(&fenceB);
+
+	if (r)
+		printk(KERN_WARNING "Error while testing ring sync (%d).\n", r);
+}
+
+void radeon_test_syncing(struct radeon_device *rdev)
+{
+	int i, j, k;
+
+	for (i = 1; i < RADEON_NUM_RINGS; ++i) {
+		struct radeon_ring *ringA = &rdev->ring[i];
+		if (!ringA->ready)
+			continue;
+
+		for (j = 0; j < i; ++j) {
+			struct radeon_ring *ringB = &rdev->ring[j];
+			if (!ringB->ready)
+				continue;
+
+			DRM_INFO("Testing syncing between rings %d and %d...\n", i, j);
+			radeon_test_ring_sync(rdev, ringA, ringB);
+
+			DRM_INFO("Testing syncing between rings %d and %d...\n", j, i);
+			radeon_test_ring_sync(rdev, ringB, ringA);
+
+			for (k = 0; k < j; ++k) {
+				struct radeon_ring *ringC = &rdev->ring[k];
+				if (!ringC->ready)
+					continue;
+
+				DRM_INFO("Testing syncing between rings %d, %d and %d...\n", i, j, k);
+				radeon_test_ring_sync2(rdev, ringA, ringB, ringC);
+
+				DRM_INFO("Testing syncing between rings %d, %d and %d...\n", i, k, j);
+				radeon_test_ring_sync2(rdev, ringA, ringC, ringB);
+
+				DRM_INFO("Testing syncing between rings %d, %d and %d...\n", j, i, k);
+				radeon_test_ring_sync2(rdev, ringB, ringA, ringC);
+
+				DRM_INFO("Testing syncing between rings %d, %d and %d...\n", j, k, i);
+				radeon_test_ring_sync2(rdev, ringB, ringC, ringA);
+
+				DRM_INFO("Testing syncing between rings %d, %d and %d...\n", k, i, j);
+				radeon_test_ring_sync2(rdev, ringC, ringA, ringB);
+
+				DRM_INFO("Testing syncing between rings %d, %d and %d...\n", k, j, i);
+				radeon_test_ring_sync2(rdev, ringC, ringB, ringA);
+			}
+		}
+	}
+}
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index 0b5468bfaf54..c421e77ace71 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -114,24 +114,6 @@ static void radeon_ttm_global_fini(struct radeon_device *rdev)
 	}
 }
 
-struct ttm_backend *radeon_ttm_backend_create(struct radeon_device *rdev);
-
-static struct ttm_backend*
-radeon_create_ttm_backend_entry(struct ttm_bo_device *bdev)
-{
-	struct radeon_device *rdev;
-
-	rdev = radeon_get_rdev(bdev);
-#if __OS_HAS_AGP
-	if (rdev->flags & RADEON_IS_AGP) {
-		return ttm_agp_backend_init(bdev, rdev->ddev->agp->bridge);
-	} else
-#endif
-	{
-		return radeon_ttm_backend_create(rdev);
-	}
-}
-
 static int radeon_invalidate_caches(struct ttm_bo_device *bdev, uint32_t flags)
 {
 	return 0;
@@ -206,7 +188,7 @@ static void radeon_evict_flags(struct ttm_buffer_object *bo,
 	rbo = container_of(bo, struct radeon_bo, tbo);
 	switch (bo->mem.mem_type) {
 	case TTM_PL_VRAM:
-		if (rbo->rdev->cp.ready == false)
+		if (rbo->rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready == false)
 			radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_CPU);
 		else
 			radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_GTT);
@@ -241,10 +223,10 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 	struct radeon_device *rdev;
 	uint64_t old_start, new_start;
 	struct radeon_fence *fence;
-	int r;
+	int r, i;
 
 	rdev = radeon_get_rdev(bo->bdev);
-	r = radeon_fence_create(rdev, &fence);
+	r = radeon_fence_create(rdev, &fence, rdev->copy_ring);
 	if (unlikely(r)) {
 		return r;
 	}
@@ -273,13 +255,43 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 		DRM_ERROR("Unknown placement %d\n", old_mem->mem_type);
 		return -EINVAL;
 	}
-	if (!rdev->cp.ready) {
-		DRM_ERROR("Trying to move memory with CP turned off.\n");
+	if (!rdev->ring[rdev->copy_ring].ready) {
+		DRM_ERROR("Trying to move memory with ring turned off.\n");
 		return -EINVAL;
 	}
 
 	BUILD_BUG_ON((PAGE_SIZE % RADEON_GPU_PAGE_SIZE) != 0);
 
+	/* sync other rings */
+	if (rdev->family >= CHIP_R600) {
+		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+			/* no need to sync to our own or unused rings */
+			if (i == rdev->copy_ring || !rdev->ring[i].ready)
+				continue;
+
+			if (!fence->semaphore) {
+				r = radeon_semaphore_create(rdev, &fence->semaphore);
+				/* FIXME: handle semaphore error */
+				if (r)
+					continue;
+			}
+
+			r = radeon_ring_lock(rdev, &rdev->ring[i], 3);
+			/* FIXME: handle ring lock error */
+			if (r)
+				continue;
+			radeon_semaphore_emit_signal(rdev, i, fence->semaphore);
+			radeon_ring_unlock_commit(rdev, &rdev->ring[i]);
+
+			r = radeon_ring_lock(rdev, &rdev->ring[rdev->copy_ring], 3);
+			/* FIXME: handle ring lock error */
+			if (r)
+				continue;
+			radeon_semaphore_emit_wait(rdev, rdev->copy_ring, fence->semaphore);
+			radeon_ring_unlock_commit(rdev, &rdev->ring[rdev->copy_ring]);
+		}
+	}
+
 	r = radeon_copy(rdev, old_start, new_start,
 			new_mem->num_pages * (PAGE_SIZE / RADEON_GPU_PAGE_SIZE), /* GPU pages */
 			fence);
@@ -398,7 +410,7 @@ static int radeon_bo_move(struct ttm_buffer_object *bo,
 		radeon_move_null(bo, new_mem);
 		return 0;
 	}
-	if (!rdev->cp.ready || rdev->asic->copy == NULL) {
+	if (!rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready || rdev->asic->copy == NULL) {
 		/* use memcpy */
 		goto memcpy;
 	}
@@ -515,8 +527,166 @@ static bool radeon_sync_obj_signaled(void *sync_obj, void *sync_arg)
 	return radeon_fence_signaled((struct radeon_fence *)sync_obj);
 }
 
+/*
+ * TTM backend functions.
+ */
+struct radeon_ttm_tt {
+	struct ttm_dma_tt		ttm;
+	struct radeon_device		*rdev;
+	u64				offset;
+};
+
+static int radeon_ttm_backend_bind(struct ttm_tt *ttm,
+				   struct ttm_mem_reg *bo_mem)
+{
+	struct radeon_ttm_tt *gtt = (void*)ttm;
+	int r;
+
+	gtt->offset = (unsigned long)(bo_mem->start << PAGE_SHIFT);
+	if (!ttm->num_pages) {
+		WARN(1, "nothing to bind %lu pages for mreg %p back %p!\n",
+		     ttm->num_pages, bo_mem, ttm);
+	}
+	r = radeon_gart_bind(gtt->rdev, gtt->offset,
+			     ttm->num_pages, ttm->pages, gtt->ttm.dma_address);
+	if (r) {
+		DRM_ERROR("failed to bind %lu pages at 0x%08X\n",
+			  ttm->num_pages, (unsigned)gtt->offset);
+		return r;
+	}
+	return 0;
+}
+
+static int radeon_ttm_backend_unbind(struct ttm_tt *ttm)
+{
+	struct radeon_ttm_tt *gtt = (void *)ttm;
+
+	radeon_gart_unbind(gtt->rdev, gtt->offset, ttm->num_pages);
+	return 0;
+}
+
+static void radeon_ttm_backend_destroy(struct ttm_tt *ttm)
+{
+	struct radeon_ttm_tt *gtt = (void *)ttm;
+
+	ttm_dma_tt_fini(&gtt->ttm);
+	kfree(gtt);
+}
+
+static struct ttm_backend_func radeon_backend_func = {
+	.bind = &radeon_ttm_backend_bind,
+	.unbind = &radeon_ttm_backend_unbind,
+	.destroy = &radeon_ttm_backend_destroy,
+};
+
+struct ttm_tt *radeon_ttm_tt_create(struct ttm_bo_device *bdev,
+				    unsigned long size, uint32_t page_flags,
+				    struct page *dummy_read_page)
+{
+	struct radeon_device *rdev;
+	struct radeon_ttm_tt *gtt;
+
+	rdev = radeon_get_rdev(bdev);
+#if __OS_HAS_AGP
+	if (rdev->flags & RADEON_IS_AGP) {
+		return ttm_agp_tt_create(bdev, rdev->ddev->agp->bridge,
+					 size, page_flags, dummy_read_page);
+	}
+#endif
+
+	gtt = kzalloc(sizeof(struct radeon_ttm_tt), GFP_KERNEL);
+	if (gtt == NULL) {
+		return NULL;
+	}
+	gtt->ttm.ttm.func = &radeon_backend_func;
+	gtt->rdev = rdev;
+	if (ttm_dma_tt_init(&gtt->ttm, bdev, size, page_flags, dummy_read_page)) {
+		kfree(gtt);
+		return NULL;
+	}
+	return &gtt->ttm.ttm;
+}
+
+static int radeon_ttm_tt_populate(struct ttm_tt *ttm)
+{
+	struct radeon_device *rdev;
+	struct radeon_ttm_tt *gtt = (void *)ttm;
+	unsigned i;
+	int r;
+
+	if (ttm->state != tt_unpopulated)
+		return 0;
+
+	rdev = radeon_get_rdev(ttm->bdev);
+#if __OS_HAS_AGP
+	if (rdev->flags & RADEON_IS_AGP) {
+		return ttm_agp_tt_populate(ttm);
+	}
+#endif
+
+#ifdef CONFIG_SWIOTLB
+	if (swiotlb_nr_tbl()) {
+		return ttm_dma_populate(&gtt->ttm, rdev->dev);
+	}
+#endif
+
+	r = ttm_pool_populate(ttm);
+	if (r) {
+		return r;
+	}
+
+	for (i = 0; i < ttm->num_pages; i++) {
+		gtt->ttm.dma_address[i] = pci_map_page(rdev->pdev, ttm->pages[i],
+						       0, PAGE_SIZE,
+						       PCI_DMA_BIDIRECTIONAL);
+		if (pci_dma_mapping_error(rdev->pdev, gtt->ttm.dma_address[i])) {
+			while (--i) {
+				pci_unmap_page(rdev->pdev, gtt->ttm.dma_address[i],
+					       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
+				gtt->ttm.dma_address[i] = 0;
+			}
+			ttm_pool_unpopulate(ttm);
+			return -EFAULT;
+		}
+	}
+	return 0;
+}
+
+static void radeon_ttm_tt_unpopulate(struct ttm_tt *ttm)
+{
+	struct radeon_device *rdev;
+	struct radeon_ttm_tt *gtt = (void *)ttm;
+	unsigned i;
+
+	rdev = radeon_get_rdev(ttm->bdev);
+#if __OS_HAS_AGP
+	if (rdev->flags & RADEON_IS_AGP) {
+		ttm_agp_tt_unpopulate(ttm);
+		return;
+	}
+#endif
+
+#ifdef CONFIG_SWIOTLB
+	if (swiotlb_nr_tbl()) {
+		ttm_dma_unpopulate(&gtt->ttm, rdev->dev);
+		return;
+	}
+#endif
+
+	for (i = 0; i < ttm->num_pages; i++) {
+		if (gtt->ttm.dma_address[i]) {
+			pci_unmap_page(rdev->pdev, gtt->ttm.dma_address[i],
+				       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
+		}
+	}
+
+	ttm_pool_unpopulate(ttm);
+}
+
 static struct ttm_bo_driver radeon_bo_driver = {
-	.create_ttm_backend_entry = &radeon_create_ttm_backend_entry,
+	.ttm_tt_create = &radeon_ttm_tt_create,
+	.ttm_tt_populate = &radeon_ttm_tt_populate,
+	.ttm_tt_unpopulate = &radeon_ttm_tt_unpopulate,
 	.invalidate_caches = &radeon_invalidate_caches,
 	.init_mem_type = &radeon_init_mem_type,
 	.evict_flags = &radeon_evict_flags,
@@ -680,124 +850,6 @@ int radeon_mmap(struct file *filp, struct vm_area_struct *vma)
 }
 
 
-/*
- * TTM backend functions.
- */
-struct radeon_ttm_backend {
-	struct ttm_backend		backend;
-	struct radeon_device		*rdev;
-	unsigned long			num_pages;
-	struct page			**pages;
-	struct page			*dummy_read_page;
-	dma_addr_t			*dma_addrs;
-	bool				populated;
-	bool				bound;
-	unsigned			offset;
-};
-
-static int radeon_ttm_backend_populate(struct ttm_backend *backend,
-				       unsigned long num_pages,
-				       struct page **pages,
-				       struct page *dummy_read_page,
-				       dma_addr_t *dma_addrs)
-{
-	struct radeon_ttm_backend *gtt;
-
-	gtt = container_of(backend, struct radeon_ttm_backend, backend);
-	gtt->pages = pages;
-	gtt->dma_addrs = dma_addrs;
-	gtt->num_pages = num_pages;
-	gtt->dummy_read_page = dummy_read_page;
-	gtt->populated = true;
-	return 0;
-}
-
-static void radeon_ttm_backend_clear(struct ttm_backend *backend)
-{
-	struct radeon_ttm_backend *gtt;
-
-	gtt = container_of(backend, struct radeon_ttm_backend, backend);
-	gtt->pages = NULL;
-	gtt->dma_addrs = NULL;
-	gtt->num_pages = 0;
-	gtt->dummy_read_page = NULL;
-	gtt->populated = false;
-	gtt->bound = false;
-}
-
-
-static int radeon_ttm_backend_bind(struct ttm_backend *backend,
-				   struct ttm_mem_reg *bo_mem)
-{
-	struct radeon_ttm_backend *gtt;
-	int r;
-
-	gtt = container_of(backend, struct radeon_ttm_backend, backend);
-	gtt->offset = bo_mem->start << PAGE_SHIFT;
-	if (!gtt->num_pages) {
-		WARN(1, "nothing to bind %lu pages for mreg %p back %p!\n",
-		     gtt->num_pages, bo_mem, backend);
-	}
-	r = radeon_gart_bind(gtt->rdev, gtt->offset,
-			     gtt->num_pages, gtt->pages, gtt->dma_addrs);
-	if (r) {
-		DRM_ERROR("failed to bind %lu pages at 0x%08X\n",
-			  gtt->num_pages, gtt->offset);
-		return r;
-	}
-	gtt->bound = true;
-	return 0;
-}
-
-static int radeon_ttm_backend_unbind(struct ttm_backend *backend)
-{
-	struct radeon_ttm_backend *gtt;
-
-	gtt = container_of(backend, struct radeon_ttm_backend, backend);
-	radeon_gart_unbind(gtt->rdev, gtt->offset, gtt->num_pages);
-	gtt->bound = false;
-	return 0;
-}
-
-static void radeon_ttm_backend_destroy(struct ttm_backend *backend)
-{
-	struct radeon_ttm_backend *gtt;
-
-	gtt = container_of(backend, struct radeon_ttm_backend, backend);
-	if (gtt->bound) {
-		radeon_ttm_backend_unbind(backend);
-	}
-	kfree(gtt);
-}
-
-static struct ttm_backend_func radeon_backend_func = {
-	.populate = &radeon_ttm_backend_populate,
-	.clear = &radeon_ttm_backend_clear,
-	.bind = &radeon_ttm_backend_bind,
-	.unbind = &radeon_ttm_backend_unbind,
-	.destroy = &radeon_ttm_backend_destroy,
-};
-
-struct ttm_backend *radeon_ttm_backend_create(struct radeon_device *rdev)
-{
-	struct radeon_ttm_backend *gtt;
-
-	gtt = kzalloc(sizeof(struct radeon_ttm_backend), GFP_KERNEL);
-	if (gtt == NULL) {
-		return NULL;
-	}
-	gtt->backend.bdev = &rdev->mman.bdev;
-	gtt->backend.flags = 0;
-	gtt->backend.func = &radeon_backend_func;
-	gtt->rdev = rdev;
-	gtt->pages = NULL;
-	gtt->num_pages = 0;
-	gtt->dummy_read_page = NULL;
-	gtt->populated = false;
-	gtt->bound = false;
-	return &gtt->backend;
-}
-
 #define RADEON_DEBUGFS_MEM_TYPES 2
 
 #if defined(CONFIG_DEBUG_FS)
@@ -820,8 +872,8 @@ static int radeon_mm_dump_table(struct seq_file *m, void *data)
 static int radeon_ttm_debugfs_init(struct radeon_device *rdev)
 {
 #if defined(CONFIG_DEBUG_FS)
-	static struct drm_info_list radeon_mem_types_list[RADEON_DEBUGFS_MEM_TYPES+1];
-	static char radeon_mem_types_names[RADEON_DEBUGFS_MEM_TYPES+1][32];
+	static struct drm_info_list radeon_mem_types_list[RADEON_DEBUGFS_MEM_TYPES+2];
+	static char radeon_mem_types_names[RADEON_DEBUGFS_MEM_TYPES+2][32];
 	unsigned i;
 
 	for (i = 0; i < RADEON_DEBUGFS_MEM_TYPES; i++) {
@@ -843,8 +895,17 @@ static int radeon_ttm_debugfs_init(struct radeon_device *rdev)
 	radeon_mem_types_list[i].name = radeon_mem_types_names[i];
 	radeon_mem_types_list[i].show = &ttm_page_alloc_debugfs;
 	radeon_mem_types_list[i].driver_features = 0;
-	radeon_mem_types_list[i].data = NULL;
-	return radeon_debugfs_add_files(rdev, radeon_mem_types_list, RADEON_DEBUGFS_MEM_TYPES+1);
+	radeon_mem_types_list[i++].data = NULL;
+#ifdef CONFIG_SWIOTLB
+	if (swiotlb_nr_tbl()) {
+		sprintf(radeon_mem_types_names[i], "ttm_dma_page_pool");
+		radeon_mem_types_list[i].name = radeon_mem_types_names[i];
+		radeon_mem_types_list[i].show = &ttm_dma_page_alloc_debugfs;
+		radeon_mem_types_list[i].driver_features = 0;
+		radeon_mem_types_list[i++].data = NULL;
+	}
+#endif
+	return radeon_debugfs_add_files(rdev, radeon_mem_types_list, i);
 
 #endif
 	return 0;
diff --git a/drivers/gpu/drm/radeon/rs400.c b/drivers/gpu/drm/radeon/rs400.c
index 06b90c87f8f3..b0ce84a20a68 100644
--- a/drivers/gpu/drm/radeon/rs400.c
+++ b/drivers/gpu/drm/radeon/rs400.c
@@ -410,6 +410,12 @@ static int rs400_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	r100_irq_set(rdev);
 	rdev->config.r300.hdp_cntl = RREG32(RADEON_HOST_PATH_CNTL);
@@ -419,11 +425,18 @@ static int rs400_startup(struct radeon_device *rdev)
 		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
-	r = r100_ib_init(rdev);
+
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
 		return r;
 	}
+
 	return 0;
 }
 
@@ -447,11 +460,14 @@ int rs400_resume(struct radeon_device *rdev)
 	r300_clock_startup(rdev);
 	/* Initialize surface registers */
 	radeon_surface_init(rdev);
+
+	rdev->accel_working = true;
 	return rs400_startup(rdev);
 }
 
 int rs400_suspend(struct radeon_device *rdev)
 {
+	radeon_ib_pool_suspend(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
 	r100_irq_disable(rdev);
@@ -530,7 +546,14 @@ int rs400_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 	r300_set_reg_safe(rdev);
+
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = rs400_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index b1053d640423..803e0d3c1773 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -324,10 +324,10 @@ void rs600_hpd_fini(struct radeon_device *rdev)
 
 void rs600_bm_disable(struct radeon_device *rdev)
 {
-	u32 tmp;
+	u16 tmp;
 
 	/* disable bus mastering */
-	pci_read_config_word(rdev->pdev, 0x4, (u16*)&tmp);
+	pci_read_config_word(rdev->pdev, 0x4, &tmp);
 	pci_write_config_word(rdev->pdev, 0x4, tmp & 0xFFFB);
 	mdelay(1);
 }
@@ -549,7 +549,7 @@ int rs600_irq_set(struct radeon_device *rdev)
 		WREG32(R_000040_GEN_INT_CNTL, 0);
 		return -EINVAL;
 	}
-	if (rdev->irq.sw_int) {
+	if (rdev->irq.sw_int[RADEON_RING_TYPE_GFX_INDEX]) {
 		tmp |= S_000040_SW_INT_EN(1);
 	}
 	if (rdev->irq.gui_idle) {
@@ -642,7 +642,7 @@ int rs600_irq_process(struct radeon_device *rdev)
 	while (status || rdev->irq.stat_regs.r500.disp_int) {
 		/* SW interrupt */
 		if (G_000044_SW_INT(status)) {
-			radeon_fence_process(rdev);
+			radeon_fence_process(rdev, RADEON_RING_TYPE_GFX_INDEX);
 		}
 		/* GUI idle */
 		if (G_000040_GUI_IDLE(status)) {
@@ -849,6 +849,12 @@ static int rs600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	rs600_irq_set(rdev);
 	rdev->config.r300.hdp_cntl = RREG32(RADEON_HOST_PATH_CNTL);
@@ -858,15 +864,21 @@ static int rs600_startup(struct radeon_device *rdev)
 		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
-	r = r100_ib_init(rdev);
+
+	r = r600_audio_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing audio\n");
 		return r;
 	}
 
-	r = r600_audio_init(rdev);
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing audio\n");
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
 		return r;
 	}
 
@@ -891,11 +903,14 @@ int rs600_resume(struct radeon_device *rdev)
 	rv515_clock_startup(rdev);
 	/* Initialize surface registers */
 	radeon_surface_init(rdev);
+
+	rdev->accel_working = true;
 	return rs600_startup(rdev);
 }
 
 int rs600_suspend(struct radeon_device *rdev)
 {
+	radeon_ib_pool_suspend(rdev);
 	r600_audio_fini(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
@@ -976,7 +991,14 @@ int rs600_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 	rs600_set_safe_registers(rdev);
+
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = rs600_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
diff --git a/drivers/gpu/drm/radeon/rs690.c b/drivers/gpu/drm/radeon/rs690.c
index a9049ed1a519..4f24a0fa8c82 100644
--- a/drivers/gpu/drm/radeon/rs690.c
+++ b/drivers/gpu/drm/radeon/rs690.c
@@ -621,6 +621,12 @@ static int rs690_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	rs600_irq_set(rdev);
 	rdev->config.r300.hdp_cntl = RREG32(RADEON_HOST_PATH_CNTL);
@@ -630,15 +636,21 @@ static int rs690_startup(struct radeon_device *rdev)
 		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
-	r = r100_ib_init(rdev);
+
+	r = r600_audio_init(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed initializing audio\n");
 		return r;
 	}
 
-	r = r600_audio_init(rdev);
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing audio\n");
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
 		return r;
 	}
 
@@ -663,11 +675,14 @@ int rs690_resume(struct radeon_device *rdev)
 	rv515_clock_startup(rdev);
 	/* Initialize surface registers */
 	radeon_surface_init(rdev);
+
+	rdev->accel_working = true;
 	return rs690_startup(rdev);
 }
 
 int rs690_suspend(struct radeon_device *rdev)
 {
+	radeon_ib_pool_suspend(rdev);
 	r600_audio_fini(rdev);
 	r100_cp_disable(rdev);
 	radeon_wb_disable(rdev);
@@ -749,7 +764,14 @@ int rs690_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 	rs600_set_safe_registers(rdev);
+
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = rs690_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
diff --git a/drivers/gpu/drm/radeon/rv515.c b/drivers/gpu/drm/radeon/rv515.c
index 6613ee9ecca3..880637fd1946 100644
--- a/drivers/gpu/drm/radeon/rv515.c
+++ b/drivers/gpu/drm/radeon/rv515.c
@@ -55,44 +55,45 @@ void rv515_debugfs(struct radeon_device *rdev)
 
 void rv515_ring_start(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
-	r = radeon_ring_lock(rdev, 64);
+	r = radeon_ring_lock(rdev, ring, 64);
 	if (r) {
 		return;
 	}
-	radeon_ring_write(rdev, PACKET0(ISYNC_CNTL, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(ISYNC_CNTL, 0));
+	radeon_ring_write(ring,
 			  ISYNC_ANY2D_IDLE3D |
 			  ISYNC_ANY3D_IDLE2D |
 			  ISYNC_WAIT_IDLEGUI |
 			  ISYNC_CPSCRATCH_IDLEGUI);
-	radeon_ring_write(rdev, PACKET0(WAIT_UNTIL, 0));
-	radeon_ring_write(rdev, WAIT_2D_IDLECLEAN | WAIT_3D_IDLECLEAN);
-	radeon_ring_write(rdev, PACKET0(R300_DST_PIPE_CONFIG, 0));
-	radeon_ring_write(rdev, R300_PIPE_AUTO_CONFIG);
-	radeon_ring_write(rdev, PACKET0(GB_SELECT, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, PACKET0(GB_ENABLE, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, PACKET0(R500_SU_REG_DEST, 0));
-	radeon_ring_write(rdev, (1 << rdev->num_gb_pipes) - 1);
-	radeon_ring_write(rdev, PACKET0(VAP_INDEX_OFFSET, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, PACKET0(RB3D_DSTCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, RB3D_DC_FLUSH | RB3D_DC_FREE);
-	radeon_ring_write(rdev, PACKET0(ZB_ZCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, ZC_FLUSH | ZC_FREE);
-	radeon_ring_write(rdev, PACKET0(WAIT_UNTIL, 0));
-	radeon_ring_write(rdev, WAIT_2D_IDLECLEAN | WAIT_3D_IDLECLEAN);
-	radeon_ring_write(rdev, PACKET0(GB_AA_CONFIG, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_write(rdev, PACKET0(RB3D_DSTCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, RB3D_DC_FLUSH | RB3D_DC_FREE);
-	radeon_ring_write(rdev, PACKET0(ZB_ZCACHE_CTLSTAT, 0));
-	radeon_ring_write(rdev, ZC_FLUSH | ZC_FREE);
-	radeon_ring_write(rdev, PACKET0(GB_MSPOS0, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(WAIT_UNTIL, 0));
+	radeon_ring_write(ring, WAIT_2D_IDLECLEAN | WAIT_3D_IDLECLEAN);
+	radeon_ring_write(ring, PACKET0(R300_DST_PIPE_CONFIG, 0));
+	radeon_ring_write(ring, R300_PIPE_AUTO_CONFIG);
+	radeon_ring_write(ring, PACKET0(GB_SELECT, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(GB_ENABLE, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(R500_SU_REG_DEST, 0));
+	radeon_ring_write(ring, (1 << rdev->num_gb_pipes) - 1);
+	radeon_ring_write(ring, PACKET0(VAP_INDEX_OFFSET, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(RB3D_DSTCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, RB3D_DC_FLUSH | RB3D_DC_FREE);
+	radeon_ring_write(ring, PACKET0(ZB_ZCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, ZC_FLUSH | ZC_FREE);
+	radeon_ring_write(ring, PACKET0(WAIT_UNTIL, 0));
+	radeon_ring_write(ring, WAIT_2D_IDLECLEAN | WAIT_3D_IDLECLEAN);
+	radeon_ring_write(ring, PACKET0(GB_AA_CONFIG, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(RB3D_DSTCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, RB3D_DC_FLUSH | RB3D_DC_FREE);
+	radeon_ring_write(ring, PACKET0(ZB_ZCACHE_CTLSTAT, 0));
+	radeon_ring_write(ring, ZC_FLUSH | ZC_FREE);
+	radeon_ring_write(ring, PACKET0(GB_MSPOS0, 0));
+	radeon_ring_write(ring,
 			  ((6 << MS_X0_SHIFT) |
 			   (6 << MS_Y0_SHIFT) |
 			   (6 << MS_X1_SHIFT) |
@@ -101,8 +102,8 @@ void rv515_ring_start(struct radeon_device *rdev)
 			   (6 << MS_Y2_SHIFT) |
 			   (6 << MSBD0_Y_SHIFT) |
 			   (6 << MSBD0_X_SHIFT)));
-	radeon_ring_write(rdev, PACKET0(GB_MSPOS1, 0));
-	radeon_ring_write(rdev,
+	radeon_ring_write(ring, PACKET0(GB_MSPOS1, 0));
+	radeon_ring_write(ring,
 			  ((6 << MS_X3_SHIFT) |
 			   (6 << MS_Y3_SHIFT) |
 			   (6 << MS_X4_SHIFT) |
@@ -110,15 +111,15 @@ void rv515_ring_start(struct radeon_device *rdev)
 			   (6 << MS_X5_SHIFT) |
 			   (6 << MS_Y5_SHIFT) |
 			   (6 << MSBD1_SHIFT)));
-	radeon_ring_write(rdev, PACKET0(GA_ENHANCE, 0));
-	radeon_ring_write(rdev, GA_DEADLOCK_CNTL | GA_FASTSYNC_CNTL);
-	radeon_ring_write(rdev, PACKET0(GA_POLY_MODE, 0));
-	radeon_ring_write(rdev, FRONT_PTYPE_TRIANGE | BACK_PTYPE_TRIANGE);
-	radeon_ring_write(rdev, PACKET0(GA_ROUND_MODE, 0));
-	radeon_ring_write(rdev, GEOMETRY_ROUND_NEAREST | COLOR_ROUND_NEAREST);
-	radeon_ring_write(rdev, PACKET0(0x20C8, 0));
-	radeon_ring_write(rdev, 0);
-	radeon_ring_unlock_commit(rdev);
+	radeon_ring_write(ring, PACKET0(GA_ENHANCE, 0));
+	radeon_ring_write(ring, GA_DEADLOCK_CNTL | GA_FASTSYNC_CNTL);
+	radeon_ring_write(ring, PACKET0(GA_POLY_MODE, 0));
+	radeon_ring_write(ring, FRONT_PTYPE_TRIANGE | BACK_PTYPE_TRIANGE);
+	radeon_ring_write(ring, PACKET0(GA_ROUND_MODE, 0));
+	radeon_ring_write(ring, GEOMETRY_ROUND_NEAREST | COLOR_ROUND_NEAREST);
+	radeon_ring_write(ring, PACKET0(0x20C8, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_unlock_commit(rdev, ring);
 }
 
 int rv515_mc_wait_for_idle(struct radeon_device *rdev)
@@ -392,6 +393,12 @@ static int rv515_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	rs600_irq_set(rdev);
 	rdev->config.r300.hdp_cntl = RREG32(RADEON_HOST_PATH_CNTL);
@@ -401,9 +408,15 @@ static int rv515_startup(struct radeon_device *rdev)
 		dev_err(rdev->dev, "failed initializing CP (%d).\n", r);
 		return r;
 	}
-	r = r100_ib_init(rdev);
+
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r100_ib_test(rdev);
 	if (r) {
-		dev_err(rdev->dev, "failed initializing IB (%d).\n", r);
+		dev_err(rdev->dev, "failed testing IB (%d).\n", r);
+		rdev->accel_working = false;
 		return r;
 	}
 	return 0;
@@ -428,6 +441,8 @@ int rv515_resume(struct radeon_device *rdev)
 	rv515_clock_startup(rdev);
 	/* Initialize surface registers */
 	radeon_surface_init(rdev);
+
+	rdev->accel_working = true;
 	return rv515_startup(rdev);
 }
 
@@ -524,7 +539,14 @@ int rv515_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 	rv515_set_safe_registers(rdev);
+
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = rv515_startup(rdev);
 	if (r) {
 		/* Somethings want wront with the accel init stop accel */
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index 23ae1c60ab3d..a1668b659ddd 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -357,7 +357,7 @@ static int rv770_cp_load_microcode(struct radeon_device *rdev)
 void r700_cp_fini(struct radeon_device *rdev)
 {
 	r700_cp_stop(rdev);
-	radeon_ring_fini(rdev);
+	radeon_ring_fini(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX]);
 }
 
 /*
@@ -1043,6 +1043,7 @@ int rv770_mc_init(struct radeon_device *rdev)
 
 static int rv770_startup(struct radeon_device *rdev)
 {
+	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
 	int r;
 
 	/* enable pcie gen2 link */
@@ -1082,6 +1083,12 @@ static int rv770_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_fence_driver_start_ring(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "failed initializing CP fences (%d).\n", r);
+		return r;
+	}
+
 	/* Enable IRQ */
 	r = r600_irq_init(rdev);
 	if (r) {
@@ -1091,7 +1098,9 @@ static int rv770_startup(struct radeon_device *rdev)
 	}
 	r600_irq_set(rdev);
 
-	r = radeon_ring_init(rdev, rdev->cp.ring_size);
+	r = radeon_ring_init(rdev, ring, ring->ring_size, RADEON_WB_CP_RPTR_OFFSET,
+			     R600_CP_RB_RPTR, R600_CP_RB_WPTR,
+			     0, 0xfffff, RADEON_CP_PACKET2);
 	if (r)
 		return r;
 	r = rv770_cp_load_microcode(rdev);
@@ -1101,6 +1110,17 @@ static int rv770_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_ib_pool_start(rdev);
+	if (r)
+		return r;
+
+	r = r600_ib_test(rdev, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		dev_err(rdev->dev, "IB test failed (%d).\n", r);
+		rdev->accel_working = false;
+		return r;
+	}
+
 	return 0;
 }
 
@@ -1115,18 +1135,13 @@ int rv770_resume(struct radeon_device *rdev)
 	/* post card */
 	atom_asic_init(rdev->mode_info.atom_context);
 
+	rdev->accel_working = true;
 	r = rv770_startup(rdev);
 	if (r) {
 		DRM_ERROR("r600 startup failed on resume\n");
 		return r;
 	}
 
-	r = r600_ib_test(rdev);
-	if (r) {
-		DRM_ERROR("radeon: failed testing IB (%d).\n", r);
-		return r;
-	}
-
 	r = r600_audio_init(rdev);
 	if (r) {
 		dev_err(rdev->dev, "radeon: audio init failed\n");
@@ -1140,13 +1155,14 @@ int rv770_resume(struct radeon_device *rdev)
 int rv770_suspend(struct radeon_device *rdev)
 {
 	r600_audio_fini(rdev);
+	radeon_ib_pool_suspend(rdev);
+	r600_blit_suspend(rdev);
 	/* FIXME: we should wait for ring to be empty */
 	r700_cp_stop(rdev);
-	rdev->cp.ready = false;
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ready = false;
 	r600_irq_suspend(rdev);
 	radeon_wb_disable(rdev);
 	rv770_pcie_gart_disable(rdev);
-	r600_blit_suspend(rdev);
 
 	return 0;
 }
@@ -1215,8 +1231,8 @@ int rv770_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
-	rdev->cp.ring_obj = NULL;
-	r600_ring_init(rdev, 1024 * 1024);
+	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ring_obj = NULL;
+	r600_ring_init(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX], 1024 * 1024);
 
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
@@ -1225,30 +1241,24 @@ int rv770_init(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	r = radeon_ib_pool_init(rdev);
 	rdev->accel_working = true;
+	if (r) {
+		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
+		rdev->accel_working = false;
+	}
+
 	r = rv770_startup(rdev);
 	if (r) {
 		dev_err(rdev->dev, "disabling GPU acceleration\n");
 		r700_cp_fini(rdev);
 		r600_irq_fini(rdev);
 		radeon_wb_fini(rdev);
+		r100_ib_fini(rdev);
 		radeon_irq_kms_fini(rdev);
 		rv770_pcie_gart_fini(rdev);
 		rdev->accel_working = false;
 	}
-	if (rdev->accel_working) {
-		r = radeon_ib_pool_init(rdev);
-		if (r) {
-			dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
-			rdev->accel_working = false;
-		} else {
-			r = r600_ib_test(rdev);
-			if (r) {
-				dev_err(rdev->dev, "IB test failed (%d).\n", r);
-				rdev->accel_working = false;
-			}
-		}
-	}
 
 	r = r600_audio_init(rdev);
 	if (r) {
@@ -1265,11 +1275,12 @@ void rv770_fini(struct radeon_device *rdev)
 	r700_cp_fini(rdev);
 	r600_irq_fini(rdev);
 	radeon_wb_fini(rdev);
-	radeon_ib_pool_fini(rdev);
+	r100_ib_fini(rdev);
 	radeon_irq_kms_fini(rdev);
 	rv770_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
+	radeon_semaphore_driver_fini(rdev);
 	radeon_fence_driver_fini(rdev);
 	radeon_agp_fini(rdev);
 	radeon_bo_fini(rdev);
diff --git a/drivers/gpu/drm/savage/savage_drv.c b/drivers/gpu/drm/savage/savage_drv.c
index 5468d1cd3296..89afe0b83643 100644
--- a/drivers/gpu/drm/savage/savage_drv.c
+++ b/drivers/gpu/drm/savage/savage_drv.c
@@ -35,6 +35,17 @@ static struct pci_device_id pciidlist[] = {
 	savage_PCI_IDS
 };
 
+static const struct file_operations savage_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_USE_MTRR | DRIVER_HAVE_DMA | DRIVER_PCI_DMA,
@@ -46,17 +57,7 @@ static struct drm_driver driver = {
 	.reclaim_buffers = savage_reclaim_buffers,
 	.ioctls = savage_ioctls,
 	.dma_ioctl = savage_bci_buffers,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = drm_ioctl,
-		 .mmap = drm_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .llseek = noop_llseek,
-	},
-
+	.fops = &savage_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/sis/sis_drv.c b/drivers/gpu/drm/sis/sis_drv.c
index a9c5716bea4e..06da063ece2e 100644
--- a/drivers/gpu/drm/sis/sis_drv.c
+++ b/drivers/gpu/drm/sis/sis_drv.c
@@ -48,9 +48,7 @@ static int sis_driver_load(struct drm_device *dev, unsigned long chipset)
 
 	dev->dev_private = (void *)dev_priv;
 	dev_priv->chipset = chipset;
-	ret = drm_sman_init(&dev_priv->sman, 2, 12, 8);
-	if (ret)
-		kfree(dev_priv);
+	idr_init(&dev->object_name_idr);
 
 	return ret;
 }
@@ -59,32 +57,60 @@ static int sis_driver_unload(struct drm_device *dev)
 {
 	drm_sis_private_t *dev_priv = dev->dev_private;
 
-	drm_sman_takedown(&dev_priv->sman);
+	idr_remove_all(&dev_priv->object_idr);
+	idr_destroy(&dev_priv->object_idr);
+
 	kfree(dev_priv);
 
 	return 0;
 }
 
+static const struct file_operations sis_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.llseek = noop_llseek,
+};
+
+static int sis_driver_open(struct drm_device *dev, struct drm_file *file)
+{
+	struct sis_file_private *file_priv;
+
+	DRM_DEBUG_DRIVER("\n");
+	file_priv = kmalloc(sizeof(*file_priv), GFP_KERNEL);
+	if (!file_priv)
+		return -ENOMEM;
+
+	file->driver_priv = file_priv;
+
+	INIT_LIST_HEAD(&file_priv->obj_list);
+
+	return 0;
+}
+
+void sis_driver_postclose(struct drm_device *dev, struct drm_file *file)
+{
+	struct sis_file_private *file_priv = file->driver_priv;
+
+	kfree(file_priv);
+}
+
 static struct drm_driver driver = {
 	.driver_features = DRIVER_USE_AGP | DRIVER_USE_MTRR,
 	.load = sis_driver_load,
 	.unload = sis_driver_unload,
+	.open = sis_driver_open,
+	.postclose = sis_driver_postclose,
 	.dma_quiescent = sis_idle,
 	.reclaim_buffers = NULL,
 	.reclaim_buffers_idlelocked = sis_reclaim_buffers_locked,
 	.lastclose = sis_lastclose,
 	.ioctls = sis_ioctls,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = drm_ioctl,
-		 .mmap = drm_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .llseek = noop_llseek,
-	},
-
+	.fops = &sis_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/sis/sis_drv.h b/drivers/gpu/drm/sis/sis_drv.h
index 194303c177ad..573758b2d2d6 100644
--- a/drivers/gpu/drm/sis/sis_drv.h
+++ b/drivers/gpu/drm/sis/sis_drv.h
@@ -44,7 +44,7 @@ enum sis_family {
 	SIS_CHIP_315 = 1,
 };
 
-#include "drm_sman.h"
+#include "drm_mm.h"
 
 
 #define SIS_BASE (dev_priv->mmio)
@@ -54,12 +54,15 @@ enum sis_family {
 typedef struct drm_sis_private {
 	drm_local_map_t *mmio;
 	unsigned int idle_fault;
-	struct drm_sman sman;
 	unsigned int chipset;
 	int vram_initialized;
 	int agp_initialized;
 	unsigned long vram_offset;
 	unsigned long agp_offset;
+	struct drm_mm vram_mm;
+	struct drm_mm agp_mm;
+	/** Mapping of userspace keys to mm objects */
+	struct idr object_idr;
 } drm_sis_private_t;
 
 extern int sis_idle(struct drm_device *dev);
diff --git a/drivers/gpu/drm/sis/sis_mm.c b/drivers/gpu/drm/sis/sis_mm.c
index 7fe2b63412ce..dd4a316c3d74 100644
--- a/drivers/gpu/drm/sis/sis_mm.c
+++ b/drivers/gpu/drm/sis/sis_mm.c
@@ -41,40 +41,18 @@
 #define AGP_TYPE 1
 
 
+struct sis_memblock {
+	struct drm_mm_node mm_node;
+	struct sis_memreq req;
+	struct list_head owner_list;
+};
+
 #if defined(CONFIG_FB_SIS) || defined(CONFIG_FB_SIS_MODULE)
 /* fb management via fb device */
 
 #define SIS_MM_ALIGN_SHIFT 0
 #define SIS_MM_ALIGN_MASK 0
 
-static void *sis_sman_mm_allocate(void *private, unsigned long size,
-				  unsigned alignment)
-{
-	struct sis_memreq req;
-
-	req.size = size;
-	sis_malloc(&req);
-	if (req.size == 0)
-		return NULL;
-	else
-		return (void *)(unsigned long)~req.offset;
-}
-
-static void sis_sman_mm_free(void *private, void *ref)
-{
-	sis_free(~((unsigned long)ref));
-}
-
-static void sis_sman_mm_destroy(void *private)
-{
-	;
-}
-
-static unsigned long sis_sman_mm_offset(void *private, void *ref)
-{
-	return ~((unsigned long)ref);
-}
-
 #else /* CONFIG_FB_SIS[_MODULE] */
 
 #define SIS_MM_ALIGN_SHIFT 4
@@ -86,30 +64,11 @@ static int sis_fb_init(struct drm_device *dev, void *data, struct drm_file *file
 {
 	drm_sis_private_t *dev_priv = dev->dev_private;
 	drm_sis_fb_t *fb = data;
-	int ret;
 
 	mutex_lock(&dev->struct_mutex);
-#if defined(CONFIG_FB_SIS) || defined(CONFIG_FB_SIS_MODULE)
-	{
-		struct drm_sman_mm sman_mm;
-		sman_mm.private = (void *)0xFFFFFFFF;
-		sman_mm.allocate = sis_sman_mm_allocate;
-		sman_mm.free = sis_sman_mm_free;
-		sman_mm.destroy = sis_sman_mm_destroy;
-		sman_mm.offset = sis_sman_mm_offset;
-		ret =
-		    drm_sman_set_manager(&dev_priv->sman, VIDEO_TYPE, &sman_mm);
-	}
-#else
-	ret = drm_sman_set_range(&dev_priv->sman, VIDEO_TYPE, 0,
-				 fb->size >> SIS_MM_ALIGN_SHIFT);
-#endif
-
-	if (ret) {
-		DRM_ERROR("VRAM memory manager initialisation error\n");
-		mutex_unlock(&dev->struct_mutex);
-		return ret;
-	}
+	/* Unconditionally init the drm_mm, even though we don't use it when the
+	 * fb sis driver is available - make cleanup easier. */
+	drm_mm_init(&dev_priv->vram_mm, 0, fb->size >> SIS_MM_ALIGN_SHIFT);
 
 	dev_priv->vram_initialized = 1;
 	dev_priv->vram_offset = fb->offset;
@@ -120,13 +79,15 @@ static int sis_fb_init(struct drm_device *dev, void *data, struct drm_file *file
 	return 0;
 }
 
-static int sis_drm_alloc(struct drm_device *dev, struct drm_file *file_priv,
+static int sis_drm_alloc(struct drm_device *dev, struct drm_file *file,
 			 void *data, int pool)
 {
 	drm_sis_private_t *dev_priv = dev->dev_private;
 	drm_sis_mem_t *mem = data;
-	int retval = 0;
-	struct drm_memblock_item *item;
+	int retval = 0, user_key;
+	struct sis_memblock *item;
+	struct sis_file_private *file_priv = file->driver_priv;
+	unsigned long offset;
 
 	mutex_lock(&dev->struct_mutex);
 
@@ -138,25 +99,68 @@ static int sis_drm_alloc(struct drm_device *dev, struct drm_file *file_priv,
 		return -EINVAL;
 	}
 
-	mem->size = (mem->size + SIS_MM_ALIGN_MASK) >> SIS_MM_ALIGN_SHIFT;
-	item = drm_sman_alloc(&dev_priv->sman, pool, mem->size, 0,
-			      (unsigned long)file_priv);
+	item = kzalloc(sizeof(*item), GFP_KERNEL);
+	if (!item) {
+		retval = -ENOMEM;
+		goto fail_alloc;
+	}
 
-	mutex_unlock(&dev->struct_mutex);
-	if (item) {
-		mem->offset = ((pool == 0) ?
-			      dev_priv->vram_offset : dev_priv->agp_offset) +
-		    (item->mm->
-		     offset(item->mm, item->mm_info) << SIS_MM_ALIGN_SHIFT);
-		mem->free = item->user_hash.key;
-		mem->size = mem->size << SIS_MM_ALIGN_SHIFT;
+	mem->size = (mem->size + SIS_MM_ALIGN_MASK) >> SIS_MM_ALIGN_SHIFT;
+	if (pool == AGP_TYPE) {
+		retval = drm_mm_insert_node(&dev_priv->agp_mm,
+					    &item->mm_node,
+					    mem->size, 0);
+		offset = item->mm_node.start;
 	} else {
-		mem->offset = 0;
-		mem->size = 0;
-		mem->free = 0;
+#if defined(CONFIG_FB_SIS) || defined(CONFIG_FB_SIS_MODULE)
+		item->req.size = mem->size;
+		sis_malloc(&item->req);
+		if (item->req.size == 0)
+			retval = -ENOMEM;
+		offset = item->req.offset;
+#else
+		retval = drm_mm_insert_node(&dev_priv->vram_mm,
+					    &item->mm_node,
+					    mem->size, 0);
+		offset = item->mm_node.start;
+#endif
+	}
+	if (retval)
+		goto fail_alloc;
+
+again:
+	if (idr_pre_get(&dev_priv->object_idr, GFP_KERNEL) == 0) {
 		retval = -ENOMEM;
+		goto fail_idr;
 	}
 
+	retval = idr_get_new_above(&dev_priv->object_idr, item, 1, &user_key);
+	if (retval == -EAGAIN)
+		goto again;
+	if (retval)
+		goto fail_idr;
+
+	list_add(&item->owner_list, &file_priv->obj_list);
+	mutex_unlock(&dev->struct_mutex);
+
+	mem->offset = ((pool == 0) ?
+		      dev_priv->vram_offset : dev_priv->agp_offset) +
+	    (offset << SIS_MM_ALIGN_SHIFT);
+	mem->free = user_key;
+	mem->size = mem->size << SIS_MM_ALIGN_SHIFT;
+
+	return 0;
+
+fail_idr:
+	drm_mm_remove_node(&item->mm_node);
+fail_alloc:
+	kfree(item);
+	mutex_unlock(&dev->struct_mutex);
+
+	mem->offset = 0;
+	mem->size = 0;
+	mem->free = 0;
+
 	DRM_DEBUG("alloc %d, size = %d, offset = %d\n", pool, mem->size,
 		  mem->offset);
 
@@ -167,14 +171,28 @@ static int sis_drm_free(struct drm_device *dev, void *data, struct drm_file *fil
 {
 	drm_sis_private_t *dev_priv = dev->dev_private;
 	drm_sis_mem_t *mem = data;
-	int ret;
+	struct sis_memblock *obj;
 
 	mutex_lock(&dev->struct_mutex);
-	ret = drm_sman_free_key(&dev_priv->sman, mem->free);
+	obj = idr_find(&dev_priv->object_idr, mem->free);
+	if (obj == NULL) {
+		mutex_unlock(&dev->struct_mutex);
+		return -EINVAL;
+	}
+
+	idr_remove(&dev_priv->object_idr, mem->free);
+	list_del(&obj->owner_list);
+	if (drm_mm_node_allocated(&obj->mm_node))
+		drm_mm_remove_node(&obj->mm_node);
+#if defined(CONFIG_FB_SIS) || defined(CONFIG_FB_SIS_MODULE)
+	else
+		sis_free(obj->req.offset);
+#endif
+	kfree(obj);
 	mutex_unlock(&dev->struct_mutex);
 	DRM_DEBUG("free = 0x%lx\n", mem->free);
 
-	return ret;
+	return 0;
 }
 
 static int sis_fb_alloc(struct drm_device *dev, void *data,
@@ -188,18 +206,10 @@ static int sis_ioctl_agp_init(struct drm_device *dev, void *data,
 {
 	drm_sis_private_t *dev_priv = dev->dev_private;
 	drm_sis_agp_t *agp = data;
-	int ret;
 	dev_priv = dev->dev_private;
 
 	mutex_lock(&dev->struct_mutex);
-	ret = drm_sman_set_range(&dev_priv->sman, AGP_TYPE, 0,
-				 agp->size >> SIS_MM_ALIGN_SHIFT);
-
-	if (ret) {
-		DRM_ERROR("AGP memory manager initialisation error\n");
-		mutex_unlock(&dev->struct_mutex);
-		return ret;
-	}
+	drm_mm_init(&dev_priv->agp_mm, 0, agp->size >> SIS_MM_ALIGN_SHIFT);
 
 	dev_priv->agp_initialized = 1;
 	dev_priv->agp_offset = agp->offset;
@@ -293,20 +303,26 @@ void sis_lastclose(struct drm_device *dev)
 		return;
 
 	mutex_lock(&dev->struct_mutex);
-	drm_sman_cleanup(&dev_priv->sman);
-	dev_priv->vram_initialized = 0;
-	dev_priv->agp_initialized = 0;
+	if (dev_priv->vram_initialized) {
+		drm_mm_takedown(&dev_priv->vram_mm);
+		dev_priv->vram_initialized = 0;
+	}
+	if (dev_priv->agp_initialized) {
+		drm_mm_takedown(&dev_priv->agp_mm);
+		dev_priv->agp_initialized = 0;
+	}
 	dev_priv->mmio = NULL;
 	mutex_unlock(&dev->struct_mutex);
 }
 
 void sis_reclaim_buffers_locked(struct drm_device *dev,
-				struct drm_file *file_priv)
+				struct drm_file *file)
 {
-	drm_sis_private_t *dev_priv = dev->dev_private;
+	struct sis_file_private *file_priv = file->driver_priv;
+	struct sis_memblock *entry, *next;
 
 	mutex_lock(&dev->struct_mutex);
-	if (drm_sman_owner_clean(&dev_priv->sman, (unsigned long)file_priv)) {
+	if (list_empty(&file_priv->obj_list)) {
 		mutex_unlock(&dev->struct_mutex);
 		return;
 	}
@@ -314,7 +330,18 @@ void sis_reclaim_buffers_locked(struct drm_device *dev,
 	if (dev->driver->dma_quiescent)
 		dev->driver->dma_quiescent(dev);
 
-	drm_sman_owner_cleanup(&dev_priv->sman, (unsigned long)file_priv);
+
+	list_for_each_entry_safe(entry, next, &file_priv->obj_list,
+				 owner_list) {
+		list_del(&entry->owner_list);
+		if (drm_mm_node_allocated(&entry->mm_node))
+			drm_mm_remove_node(&entry->mm_node);
+#if defined(CONFIG_FB_SIS) || defined(CONFIG_FB_SIS_MODULE)
+		else
+			sis_free(entry->req.offset);
+#endif
+		kfree(entry);
+	}
 	mutex_unlock(&dev->struct_mutex);
 	return;
 }
diff --git a/drivers/gpu/drm/tdfx/tdfx_drv.c b/drivers/gpu/drm/tdfx/tdfx_drv.c
index cda29911e332..1613c78544c0 100644
--- a/drivers/gpu/drm/tdfx/tdfx_drv.c
+++ b/drivers/gpu/drm/tdfx/tdfx_drv.c
@@ -41,20 +41,21 @@ static struct pci_device_id pciidlist[] = {
 	tdfx_PCI_IDS
 };
 
+static const struct file_operations tdfx_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	.driver_features = DRIVER_USE_MTRR,
 	.reclaim_buffers = drm_core_reclaim_buffers,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = drm_ioctl,
-		 .mmap = drm_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .llseek = noop_llseek,
-	},
-
+	.fops = &tdfx_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/ttm/Makefile b/drivers/gpu/drm/ttm/Makefile
index f3cf6f02c997..b2b33dde2afb 100644
--- a/drivers/gpu/drm/ttm/Makefile
+++ b/drivers/gpu/drm/ttm/Makefile
@@ -7,4 +7,8 @@ ttm-y := ttm_agp_backend.o ttm_memory.o ttm_tt.o ttm_bo.o \
 	ttm_object.o ttm_lock.o ttm_execbuf_util.o ttm_page_alloc.o \
 	ttm_bo_manager.o
 
+ifeq ($(CONFIG_SWIOTLB),y)
+ttm-y += ttm_page_alloc_dma.o
+endif
+
 obj-$(CONFIG_DRM_TTM) += ttm.o
diff --git a/drivers/gpu/drm/ttm/ttm_agp_backend.c b/drivers/gpu/drm/ttm/ttm_agp_backend.c
index 1c4a72f681c1..747c1413fc95 100644
--- a/drivers/gpu/drm/ttm/ttm_agp_backend.c
+++ b/drivers/gpu/drm/ttm/ttm_agp_backend.c
@@ -31,6 +31,7 @@
 
 #include "ttm/ttm_module.h"
 #include "ttm/ttm_bo_driver.h"
+#include "ttm/ttm_page_alloc.h"
 #ifdef TTM_HAS_AGP
 #include "ttm/ttm_placement.h"
 #include <linux/agp_backend.h>
@@ -40,45 +41,33 @@
 #include <asm/agp.h>
 
 struct ttm_agp_backend {
-	struct ttm_backend backend;
+	struct ttm_tt ttm;
 	struct agp_memory *mem;
 	struct agp_bridge_data *bridge;
 };
 
-static int ttm_agp_populate(struct ttm_backend *backend,
-			    unsigned long num_pages, struct page **pages,
-			    struct page *dummy_read_page,
-			    dma_addr_t *dma_addrs)
+static int ttm_agp_bind(struct ttm_tt *ttm, struct ttm_mem_reg *bo_mem)
 {
-	struct ttm_agp_backend *agp_be =
-	    container_of(backend, struct ttm_agp_backend, backend);
-	struct page **cur_page, **last_page = pages + num_pages;
+	struct ttm_agp_backend *agp_be = container_of(ttm, struct ttm_agp_backend, ttm);
+	struct drm_mm_node *node = bo_mem->mm_node;
 	struct agp_memory *mem;
+	int ret, cached = (bo_mem->placement & TTM_PL_FLAG_CACHED);
+	unsigned i;
 
-	mem = agp_allocate_memory(agp_be->bridge, num_pages, AGP_USER_MEMORY);
+	mem = agp_allocate_memory(agp_be->bridge, ttm->num_pages, AGP_USER_MEMORY);
 	if (unlikely(mem == NULL))
 		return -ENOMEM;
 
 	mem->page_count = 0;
-	for (cur_page = pages; cur_page < last_page; ++cur_page) {
-		struct page *page = *cur_page;
+	for (i = 0; i < ttm->num_pages; i++) {
+		struct page *page = ttm->pages[i];
+
 		if (!page)
-			page = dummy_read_page;
+			page = ttm->dummy_read_page;
 
 		mem->pages[mem->page_count++] = page;
 	}
 	agp_be->mem = mem;
-	return 0;
-}
-
-static int ttm_agp_bind(struct ttm_backend *backend, struct ttm_mem_reg *bo_mem)
-{
-	struct ttm_agp_backend *agp_be =
-	    container_of(backend, struct ttm_agp_backend, backend);
-	struct drm_mm_node *node = bo_mem->mm_node;
-	struct agp_memory *mem = agp_be->mem;
-	int cached = (bo_mem->placement & TTM_PL_FLAG_CACHED);
-	int ret;
 
 	mem->is_flushed = 1;
 	mem->type = (cached) ? AGP_USER_CACHED_MEMORY : AGP_USER_MEMORY;
@@ -90,50 +79,39 @@ static int ttm_agp_bind(struct ttm_backend *backend, struct ttm_mem_reg *bo_mem)
 	return ret;
 }
 
-static int ttm_agp_unbind(struct ttm_backend *backend)
+static int ttm_agp_unbind(struct ttm_tt *ttm)
 {
-	struct ttm_agp_backend *agp_be =
-	    container_of(backend, struct ttm_agp_backend, backend);
+	struct ttm_agp_backend *agp_be = container_of(ttm, struct ttm_agp_backend, ttm);
 
-	if (agp_be->mem->is_bound)
-		return agp_unbind_memory(agp_be->mem);
-	else
-		return 0;
-}
-
-static void ttm_agp_clear(struct ttm_backend *backend)
-{
-	struct ttm_agp_backend *agp_be =
-	    container_of(backend, struct ttm_agp_backend, backend);
-	struct agp_memory *mem = agp_be->mem;
-
-	if (mem) {
-		ttm_agp_unbind(backend);
-		agp_free_memory(mem);
+	if (agp_be->mem) {
+		if (agp_be->mem->is_bound)
+			return agp_unbind_memory(agp_be->mem);
+		agp_free_memory(agp_be->mem);
+		agp_be->mem = NULL;
 	}
-	agp_be->mem = NULL;
+	return 0;
 }
 
-static void ttm_agp_destroy(struct ttm_backend *backend)
+static void ttm_agp_destroy(struct ttm_tt *ttm)
 {
-	struct ttm_agp_backend *agp_be =
-	    container_of(backend, struct ttm_agp_backend, backend);
+	struct ttm_agp_backend *agp_be = container_of(ttm, struct ttm_agp_backend, ttm);
 
 	if (agp_be->mem)
-		ttm_agp_clear(backend);
+		ttm_agp_unbind(ttm);
+	ttm_tt_fini(ttm);
 	kfree(agp_be);
 }
 
 static struct ttm_backend_func ttm_agp_func = {
-	.populate = ttm_agp_populate,
-	.clear = ttm_agp_clear,
 	.bind = ttm_agp_bind,
 	.unbind = ttm_agp_unbind,
 	.destroy = ttm_agp_destroy,
 };
 
-struct ttm_backend *ttm_agp_backend_init(struct ttm_bo_device *bdev,
-					 struct agp_bridge_data *bridge)
+struct ttm_tt *ttm_agp_tt_create(struct ttm_bo_device *bdev,
+				 struct agp_bridge_data *bridge,
+				 unsigned long size, uint32_t page_flags,
+				 struct page *dummy_read_page)
 {
 	struct ttm_agp_backend *agp_be;
 
@@ -143,10 +121,29 @@ struct ttm_backend *ttm_agp_backend_init(struct ttm_bo_device *bdev,
 
 	agp_be->mem = NULL;
 	agp_be->bridge = bridge;
-	agp_be->backend.func = &ttm_agp_func;
-	agp_be->backend.bdev = bdev;
-	return &agp_be->backend;
+	agp_be->ttm.func = &ttm_agp_func;
+
+	if (ttm_tt_init(&agp_be->ttm, bdev, size, page_flags, dummy_read_page)) {
+		return NULL;
+	}
+
+	return &agp_be->ttm;
+}
+EXPORT_SYMBOL(ttm_agp_tt_create);
+
+int ttm_agp_tt_populate(struct ttm_tt *ttm)
+{
+	if (ttm->state != tt_unpopulated)
+		return 0;
+
+	return ttm_pool_populate(ttm);
+}
+EXPORT_SYMBOL(ttm_agp_tt_populate);
+
+void ttm_agp_tt_unpopulate(struct ttm_tt *ttm)
+{
+	ttm_pool_unpopulate(ttm);
 }
-EXPORT_SYMBOL(ttm_agp_backend_init);
+EXPORT_SYMBOL(ttm_agp_tt_unpopulate);
 
 #endif
diff --git a/drivers/gpu/drm/ttm/ttm_bo.c b/drivers/gpu/drm/ttm/ttm_bo.c
index 0bb0f5f713e6..2f0eab66ece6 100644
--- a/drivers/gpu/drm/ttm/ttm_bo.c
+++ b/drivers/gpu/drm/ttm/ttm_bo.c
@@ -137,6 +137,7 @@ static void ttm_bo_release_list(struct kref *list_kref)
 	struct ttm_buffer_object *bo =
 	    container_of(list_kref, struct ttm_buffer_object, list_kref);
 	struct ttm_bo_device *bdev = bo->bdev;
+	size_t acc_size = bo->acc_size;
 
 	BUG_ON(atomic_read(&bo->list_kref.refcount));
 	BUG_ON(atomic_read(&bo->kref.refcount));
@@ -152,9 +153,9 @@ static void ttm_bo_release_list(struct kref *list_kref)
 	if (bo->destroy)
 		bo->destroy(bo);
 	else {
-		ttm_mem_global_free(bdev->glob->mem_glob, bo->acc_size);
 		kfree(bo);
 	}
+	ttm_mem_global_free(bdev->glob->mem_glob, acc_size);
 }
 
 int ttm_bo_wait_unreserved(struct ttm_buffer_object *bo, bool interruptible)
@@ -337,27 +338,11 @@ static int ttm_bo_add_ttm(struct ttm_buffer_object *bo, bool zero_alloc)
 		if (zero_alloc)
 			page_flags |= TTM_PAGE_FLAG_ZERO_ALLOC;
 	case ttm_bo_type_kernel:
-		bo->ttm = ttm_tt_create(bdev, bo->num_pages << PAGE_SHIFT,
-					page_flags, glob->dummy_read_page);
+		bo->ttm = bdev->driver->ttm_tt_create(bdev, bo->num_pages << PAGE_SHIFT,
+						      page_flags, glob->dummy_read_page);
 		if (unlikely(bo->ttm == NULL))
 			ret = -ENOMEM;
 		break;
-	case ttm_bo_type_user:
-		bo->ttm = ttm_tt_create(bdev, bo->num_pages << PAGE_SHIFT,
-					page_flags | TTM_PAGE_FLAG_USER,
-					glob->dummy_read_page);
-		if (unlikely(bo->ttm == NULL)) {
-			ret = -ENOMEM;
-			break;
-		}
-
-		ret = ttm_tt_set_user(bo->ttm, current,
-				      bo->buffer_start, bo->num_pages);
-		if (unlikely(ret != 0)) {
-			ttm_tt_destroy(bo->ttm);
-			bo->ttm = NULL;
-		}
-		break;
 	default:
 		printk(KERN_ERR TTM_PFX "Illegal buffer object type\n");
 		ret = -EINVAL;
@@ -419,9 +404,6 @@ static int ttm_bo_handle_move_mem(struct ttm_buffer_object *bo,
 		}
 	}
 
-	if (bdev->driver->move_notify)
-		bdev->driver->move_notify(bo, mem);
-
 	if (!(old_man->flags & TTM_MEMTYPE_FLAG_FIXED) &&
 	    !(new_man->flags & TTM_MEMTYPE_FLAG_FIXED))
 		ret = ttm_bo_move_ttm(bo, evict, no_wait_reserve, no_wait_gpu, mem);
@@ -434,6 +416,9 @@ static int ttm_bo_handle_move_mem(struct ttm_buffer_object *bo,
 	if (ret)
 		goto out_err;
 
+	if (bdev->driver->move_notify)
+		bdev->driver->move_notify(bo, mem);
+
 moved:
 	if (bo->evicted) {
 		ret = bdev->driver->invalidate_caches(bdev, bo->mem.placement);
@@ -472,6 +457,9 @@ out_err:
 
 static void ttm_bo_cleanup_memtype_use(struct ttm_buffer_object *bo)
 {
+	if (bo->bdev->driver->move_notify)
+		bo->bdev->driver->move_notify(bo, NULL);
+
 	if (bo->ttm) {
 		ttm_tt_unbind(bo->ttm);
 		ttm_tt_destroy(bo->ttm);
@@ -913,16 +901,12 @@ static uint32_t ttm_bo_select_caching(struct ttm_mem_type_manager *man,
 }
 
 static bool ttm_bo_mt_compatible(struct ttm_mem_type_manager *man,
-				 bool disallow_fixed,
 				 uint32_t mem_type,
 				 uint32_t proposed_placement,
 				 uint32_t *masked_placement)
 {
 	uint32_t cur_flags = ttm_bo_type_flags(mem_type);
 
-	if ((man->flags & TTM_MEMTYPE_FLAG_FIXED) && disallow_fixed)
-		return false;
-
 	if ((cur_flags & proposed_placement & TTM_PL_MASK_MEM) == 0)
 		return false;
 
@@ -967,7 +951,6 @@ int ttm_bo_mem_space(struct ttm_buffer_object *bo,
 		man = &bdev->man[mem_type];
 
 		type_ok = ttm_bo_mt_compatible(man,
-						bo->type == ttm_bo_type_user,
 						mem_type,
 						placement->placement[i],
 						&cur_flags);
@@ -1015,7 +998,6 @@ int ttm_bo_mem_space(struct ttm_buffer_object *bo,
 		if (!man->has_type)
 			continue;
 		if (!ttm_bo_mt_compatible(man,
-						bo->type == ttm_bo_type_user,
 						mem_type,
 						placement->busy_placement[i],
 						&cur_flags))
@@ -1185,6 +1167,17 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 {
 	int ret = 0;
 	unsigned long num_pages;
+	struct ttm_mem_global *mem_glob = bdev->glob->mem_glob;
+
+	ret = ttm_mem_global_alloc(mem_glob, acc_size, false, false);
+	if (ret) {
+		printk(KERN_ERR TTM_PFX "Out of kernel memory.\n");
+		if (destroy)
+			(*destroy)(bo);
+		else
+			kfree(bo);
+		return -ENOMEM;
+	}
 
 	size += buffer_start & ~PAGE_MASK;
 	num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
@@ -1255,14 +1248,34 @@ out_err:
 }
 EXPORT_SYMBOL(ttm_bo_init);
 
-static inline size_t ttm_bo_size(struct ttm_bo_global *glob,
-				 unsigned long num_pages)
+size_t ttm_bo_acc_size(struct ttm_bo_device *bdev,
+		       unsigned long bo_size,
+		       unsigned struct_size)
 {
-	size_t page_array_size = (num_pages * sizeof(void *) + PAGE_SIZE - 1) &
-	    PAGE_MASK;
+	unsigned npages = (PAGE_ALIGN(bo_size)) >> PAGE_SHIFT;
+	size_t size = 0;
 
-	return glob->ttm_bo_size + 2 * page_array_size;
+	size += ttm_round_pot(struct_size);
+	size += PAGE_ALIGN(npages * sizeof(void *));
+	size += ttm_round_pot(sizeof(struct ttm_tt));
+	return size;
 }
+EXPORT_SYMBOL(ttm_bo_acc_size);
+
+size_t ttm_bo_dma_acc_size(struct ttm_bo_device *bdev,
+			   unsigned long bo_size,
+			   unsigned struct_size)
+{
+	unsigned npages = (PAGE_ALIGN(bo_size)) >> PAGE_SHIFT;
+	size_t size = 0;
+
+	size += ttm_round_pot(struct_size);
+	size += PAGE_ALIGN(npages * sizeof(void *));
+	size += PAGE_ALIGN(npages * sizeof(dma_addr_t));
+	size += ttm_round_pot(sizeof(struct ttm_dma_tt));
+	return size;
+}
+EXPORT_SYMBOL(ttm_bo_dma_acc_size);
 
 int ttm_bo_create(struct ttm_bo_device *bdev,
 			unsigned long size,
@@ -1276,10 +1289,10 @@ int ttm_bo_create(struct ttm_bo_device *bdev,
 {
 	struct ttm_buffer_object *bo;
 	struct ttm_mem_global *mem_glob = bdev->glob->mem_glob;
+	size_t acc_size;
 	int ret;
 
-	size_t acc_size =
-	    ttm_bo_size(bdev->glob, (size + PAGE_SIZE - 1) >> PAGE_SHIFT);
+	acc_size = ttm_bo_acc_size(bdev, size, sizeof(struct ttm_buffer_object));
 	ret = ttm_mem_global_alloc(mem_glob, acc_size, false, false);
 	if (unlikely(ret != 0))
 		return ret;
@@ -1465,13 +1478,6 @@ int ttm_bo_global_init(struct drm_global_reference *ref)
 		goto out_no_shrink;
 	}
 
-	glob->ttm_bo_extra_size =
-		ttm_round_pot(sizeof(struct ttm_tt)) +
-		ttm_round_pot(sizeof(struct ttm_backend));
-
-	glob->ttm_bo_size = glob->ttm_bo_extra_size +
-		ttm_round_pot(sizeof(struct ttm_buffer_object));
-
 	atomic_set(&glob->bo_count, 0);
 
 	ret = kobject_init_and_add(
diff --git a/drivers/gpu/drm/ttm/ttm_bo_util.c b/drivers/gpu/drm/ttm/ttm_bo_util.c
index 082fcaea583f..f8187ead7b37 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_util.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_util.c
@@ -244,7 +244,7 @@ static int ttm_copy_io_ttm_page(struct ttm_tt *ttm, void *src,
 				unsigned long page,
 				pgprot_t prot)
 {
-	struct page *d = ttm_tt_get_page(ttm, page);
+	struct page *d = ttm->pages[page];
 	void *dst;
 
 	if (!d)
@@ -281,7 +281,7 @@ static int ttm_copy_ttm_io_page(struct ttm_tt *ttm, void *dst,
 				unsigned long page,
 				pgprot_t prot)
 {
-	struct page *s = ttm_tt_get_page(ttm, page);
+	struct page *s = ttm->pages[page];
 	void *src;
 
 	if (!s)
@@ -342,6 +342,12 @@ int ttm_bo_move_memcpy(struct ttm_buffer_object *bo,
 	if (old_iomap == NULL && ttm == NULL)
 		goto out2;
 
+	if (ttm->state == tt_unpopulated) {
+		ret = ttm->bdev->driver->ttm_tt_populate(ttm);
+		if (ret)
+			goto out1;
+	}
+
 	add = 0;
 	dir = 1;
 
@@ -439,6 +445,7 @@ static int ttm_buffer_object_transfer(struct ttm_buffer_object *bo,
 	kref_init(&fbo->list_kref);
 	kref_init(&fbo->kref);
 	fbo->destroy = &ttm_transfered_destroy;
+	fbo->acc_size = 0;
 
 	*new_obj = fbo;
 	return 0;
@@ -502,10 +509,16 @@ static int ttm_bo_kmap_ttm(struct ttm_buffer_object *bo,
 {
 	struct ttm_mem_reg *mem = &bo->mem; pgprot_t prot;
 	struct ttm_tt *ttm = bo->ttm;
-	struct page *d;
-	int i;
+	int ret;
 
 	BUG_ON(!ttm);
+
+	if (ttm->state == tt_unpopulated) {
+		ret = ttm->bdev->driver->ttm_tt_populate(ttm);
+		if (ret)
+			return ret;
+	}
+
 	if (num_pages == 1 && (mem->placement & TTM_PL_FLAG_CACHED)) {
 		/*
 		 * We're mapping a single page, and the desired
@@ -513,18 +526,9 @@ static int ttm_bo_kmap_ttm(struct ttm_buffer_object *bo,
 		 */
 
 		map->bo_kmap_type = ttm_bo_map_kmap;
-		map->page = ttm_tt_get_page(ttm, start_page);
+		map->page = ttm->pages[start_page];
 		map->virtual = kmap(map->page);
 	} else {
-	    /*
-	     * Populate the part we're mapping;
-	     */
-		for (i = start_page; i < start_page + num_pages; ++i) {
-			d = ttm_tt_get_page(ttm, i);
-			if (!d)
-				return -ENOMEM;
-		}
-
 		/*
 		 * We need to use vmap to get the desired page protection
 		 * or to make the buffer object look contiguous.
diff --git a/drivers/gpu/drm/ttm/ttm_bo_vm.c b/drivers/gpu/drm/ttm/ttm_bo_vm.c
index 221b924acebe..54412848de88 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_vm.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_vm.c
@@ -174,18 +174,23 @@ static int ttm_bo_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 		vma->vm_page_prot = (bo->mem.placement & TTM_PL_FLAG_CACHED) ?
 		    vm_get_page_prot(vma->vm_flags) :
 		    ttm_io_prot(bo->mem.placement, vma->vm_page_prot);
+
+		/* Allocate all page at once, most common usage */
+		if (ttm->bdev->driver->ttm_tt_populate(ttm)) {
+			retval = VM_FAULT_OOM;
+			goto out_io_unlock;
+		}
 	}
 
 	/*
 	 * Speculatively prefault a number of pages. Only error on
 	 * first page.
 	 */
-
 	for (i = 0; i < TTM_BO_VM_NUM_PREFAULT; ++i) {
 		if (bo->mem.bus.is_iomem)
 			pfn = ((bo->mem.bus.base + bo->mem.bus.offset) >> PAGE_SHIFT) + page_offset;
 		else {
-			page = ttm_tt_get_page(ttm, page_offset);
+			page = ttm->pages[page_offset];
 			if (unlikely(!page && i == 0)) {
 				retval = VM_FAULT_OOM;
 				goto out_io_unlock;
diff --git a/drivers/gpu/drm/ttm/ttm_memory.c b/drivers/gpu/drm/ttm/ttm_memory.c
index e70ddd82dc02..9eba8e9a4e9c 100644
--- a/drivers/gpu/drm/ttm/ttm_memory.c
+++ b/drivers/gpu/drm/ttm/ttm_memory.c
@@ -395,6 +395,7 @@ int ttm_mem_global_init(struct ttm_mem_global *glob)
 		       zone->name, (unsigned long long) zone->max_mem >> 10);
 	}
 	ttm_page_alloc_init(glob, glob->zone_kernel->max_mem/(2*PAGE_SIZE));
+	ttm_dma_page_alloc_init(glob, glob->zone_kernel->max_mem/(2*PAGE_SIZE));
 	return 0;
 out_no_zone:
 	ttm_mem_global_release(glob);
@@ -409,6 +410,7 @@ void ttm_mem_global_release(struct ttm_mem_global *glob)
 
 	/* let the page allocator first stop the shrink work. */
 	ttm_page_alloc_fini();
+	ttm_dma_page_alloc_fini();
 
 	flush_workqueue(glob->swap_queue);
 	destroy_workqueue(glob->swap_queue);
diff --git a/drivers/gpu/drm/ttm/ttm_page_alloc.c b/drivers/gpu/drm/ttm/ttm_page_alloc.c
index 727e93daac3b..499debda791e 100644
--- a/drivers/gpu/drm/ttm/ttm_page_alloc.c
+++ b/drivers/gpu/drm/ttm/ttm_page_alloc.c
@@ -619,8 +619,10 @@ static void ttm_page_pool_fill_locked(struct ttm_page_pool *pool,
  * @return count of pages still required to fulfill the request.
  */
 static unsigned ttm_page_pool_get_pages(struct ttm_page_pool *pool,
-		struct list_head *pages, int ttm_flags,
-		enum ttm_caching_state cstate, unsigned count)
+					struct list_head *pages,
+					int ttm_flags,
+					enum ttm_caching_state cstate,
+					unsigned count)
 {
 	unsigned long irq_flags;
 	struct list_head *p;
@@ -660,17 +662,67 @@ out:
 	return count;
 }
 
+/* Put all pages in pages list to correct pool to wait for reuse */
+static void ttm_put_pages(struct page **pages, unsigned npages, int flags,
+			  enum ttm_caching_state cstate)
+{
+	unsigned long irq_flags;
+	struct ttm_page_pool *pool = ttm_get_pool(flags, cstate);
+	unsigned i;
+
+	if (pool == NULL) {
+		/* No pool for this memory type so free the pages */
+		for (i = 0; i < npages; i++) {
+			if (pages[i]) {
+				if (page_count(pages[i]) != 1)
+					printk(KERN_ERR TTM_PFX
+					       "Erroneous page count. "
+					       "Leaking pages.\n");
+				__free_page(pages[i]);
+				pages[i] = NULL;
+			}
+		}
+		return;
+	}
+
+	spin_lock_irqsave(&pool->lock, irq_flags);
+	for (i = 0; i < npages; i++) {
+		if (pages[i]) {
+			if (page_count(pages[i]) != 1)
+				printk(KERN_ERR TTM_PFX
+				       "Erroneous page count. "
+				       "Leaking pages.\n");
+			list_add_tail(&pages[i]->lru, &pool->list);
+			pages[i] = NULL;
+			pool->npages++;
+		}
+	}
+	/* Check that we don't go over the pool limit */
+	npages = 0;
+	if (pool->npages > _manager->options.max_size) {
+		npages = pool->npages - _manager->options.max_size;
+		/* free at least NUM_PAGES_TO_ALLOC number of pages
+		 * to reduce calls to set_memory_wb */
+		if (npages < NUM_PAGES_TO_ALLOC)
+			npages = NUM_PAGES_TO_ALLOC;
+	}
+	spin_unlock_irqrestore(&pool->lock, irq_flags);
+	if (npages)
+		ttm_page_pool_free(pool, npages);
+}
+
 /*
  * On success pages list will hold count number of correctly
  * cached pages.
  */
-int ttm_get_pages(struct list_head *pages, int flags,
-		  enum ttm_caching_state cstate, unsigned count,
-		  dma_addr_t *dma_address)
+static int ttm_get_pages(struct page **pages, unsigned npages, int flags,
+			 enum ttm_caching_state cstate)
 {
 	struct ttm_page_pool *pool = ttm_get_pool(flags, cstate);
+	struct list_head plist;
 	struct page *p = NULL;
 	gfp_t gfp_flags = GFP_USER;
+	unsigned count;
 	int r;
 
 	/* set zero flag for page allocation if required */
@@ -684,7 +736,7 @@ int ttm_get_pages(struct list_head *pages, int flags,
 		else
 			gfp_flags |= GFP_HIGHUSER;
 
-		for (r = 0; r < count; ++r) {
+		for (r = 0; r < npages; ++r) {
 			p = alloc_page(gfp_flags);
 			if (!p) {
 
@@ -693,87 +745,53 @@ int ttm_get_pages(struct list_head *pages, int flags,
 				return -ENOMEM;
 			}
 
-			list_add(&p->lru, pages);
+			pages[r] = p;
 		}
 		return 0;
 	}
 
-
 	/* combine zero flag to pool flags */
 	gfp_flags |= pool->gfp_flags;
 
 	/* First we take pages from the pool */
-	count = ttm_page_pool_get_pages(pool, pages, flags, cstate, count);
+	INIT_LIST_HEAD(&plist);
+	npages = ttm_page_pool_get_pages(pool, &plist, flags, cstate, npages);
+	count = 0;
+	list_for_each_entry(p, &plist, lru) {
+		pages[count++] = p;
+	}
 
 	/* clear the pages coming from the pool if requested */
 	if (flags & TTM_PAGE_FLAG_ZERO_ALLOC) {
-		list_for_each_entry(p, pages, lru) {
+		list_for_each_entry(p, &plist, lru) {
 			clear_page(page_address(p));
 		}
 	}
 
 	/* If pool didn't have enough pages allocate new one. */
-	if (count > 0) {
+	if (npages > 0) {
 		/* ttm_alloc_new_pages doesn't reference pool so we can run
 		 * multiple requests in parallel.
 		 **/
-		r = ttm_alloc_new_pages(pages, gfp_flags, flags, cstate, count);
+		INIT_LIST_HEAD(&plist);
+		r = ttm_alloc_new_pages(&plist, gfp_flags, flags, cstate, npages);
+		list_for_each_entry(p, &plist, lru) {
+			pages[count++] = p;
+		}
 		if (r) {
 			/* If there is any pages in the list put them back to
 			 * the pool. */
 			printk(KERN_ERR TTM_PFX
 			       "Failed to allocate extra pages "
 			       "for large request.");
-			ttm_put_pages(pages, 0, flags, cstate, NULL);
+			ttm_put_pages(pages, count, flags, cstate);
 			return r;
 		}
 	}
 
-
 	return 0;
 }
 
-/* Put all pages in pages list to correct pool to wait for reuse */
-void ttm_put_pages(struct list_head *pages, unsigned page_count, int flags,
-		   enum ttm_caching_state cstate, dma_addr_t *dma_address)
-{
-	unsigned long irq_flags;
-	struct ttm_page_pool *pool = ttm_get_pool(flags, cstate);
-	struct page *p, *tmp;
-
-	if (pool == NULL) {
-		/* No pool for this memory type so free the pages */
-
-		list_for_each_entry_safe(p, tmp, pages, lru) {
-			__free_page(p);
-		}
-		/* Make the pages list empty */
-		INIT_LIST_HEAD(pages);
-		return;
-	}
-	if (page_count == 0) {
-		list_for_each_entry_safe(p, tmp, pages, lru) {
-			++page_count;
-		}
-	}
-
-	spin_lock_irqsave(&pool->lock, irq_flags);
-	list_splice_init(pages, &pool->list);
-	pool->npages += page_count;
-	/* Check that we don't go over the pool limit */
-	page_count = 0;
-	if (pool->npages > _manager->options.max_size) {
-		page_count = pool->npages - _manager->options.max_size;
-		/* free at least NUM_PAGES_TO_ALLOC number of pages
-		 * to reduce calls to set_memory_wb */
-		if (page_count < NUM_PAGES_TO_ALLOC)
-			page_count = NUM_PAGES_TO_ALLOC;
-	}
-	spin_unlock_irqrestore(&pool->lock, irq_flags);
-	if (page_count)
-		ttm_page_pool_free(pool, page_count);
-}
-
 static void ttm_page_pool_init_locked(struct ttm_page_pool *pool, int flags,
 		char *name)
 {
@@ -836,6 +854,62 @@ void ttm_page_alloc_fini(void)
 	_manager = NULL;
 }
 
+int ttm_pool_populate(struct ttm_tt *ttm)
+{
+	struct ttm_mem_global *mem_glob = ttm->glob->mem_glob;
+	unsigned i;
+	int ret;
+
+	if (ttm->state != tt_unpopulated)
+		return 0;
+
+	for (i = 0; i < ttm->num_pages; ++i) {
+		ret = ttm_get_pages(&ttm->pages[i], 1,
+				    ttm->page_flags,
+				    ttm->caching_state);
+		if (ret != 0) {
+			ttm_pool_unpopulate(ttm);
+			return -ENOMEM;
+		}
+
+		ret = ttm_mem_global_alloc_page(mem_glob, ttm->pages[i],
+						false, false);
+		if (unlikely(ret != 0)) {
+			ttm_pool_unpopulate(ttm);
+			return -ENOMEM;
+		}
+	}
+
+	if (unlikely(ttm->page_flags & TTM_PAGE_FLAG_SWAPPED)) {
+		ret = ttm_tt_swapin(ttm);
+		if (unlikely(ret != 0)) {
+			ttm_pool_unpopulate(ttm);
+			return ret;
+		}
+	}
+
+	ttm->state = tt_unbound;
+	return 0;
+}
+EXPORT_SYMBOL(ttm_pool_populate);
+
+void ttm_pool_unpopulate(struct ttm_tt *ttm)
+{
+	unsigned i;
+
+	for (i = 0; i < ttm->num_pages; ++i) {
+		if (ttm->pages[i]) {
+			ttm_mem_global_free_page(ttm->glob->mem_glob,
+						 ttm->pages[i]);
+			ttm_put_pages(&ttm->pages[i], 1,
+				      ttm->page_flags,
+				      ttm->caching_state);
+		}
+	}
+	ttm->state = tt_unpopulated;
+}
+EXPORT_SYMBOL(ttm_pool_unpopulate);
+
 int ttm_page_alloc_debugfs(struct seq_file *m, void *data)
 {
 	struct ttm_page_pool *p;
diff --git a/drivers/gpu/drm/ttm/ttm_page_alloc_dma.c b/drivers/gpu/drm/ttm/ttm_page_alloc_dma.c
new file mode 100644
index 000000000000..37ead6995c87
--- /dev/null
+++ b/drivers/gpu/drm/ttm/ttm_page_alloc_dma.c
@@ -0,0 +1,1143 @@
+/*
+ * Copyright 2011 (c) Oracle Corp.
+
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sub license,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Author: Konrad Rzeszutek Wilk <konrad.wilk@oracle.com>
+ */
+
+/*
+ * A simple DMA pool losely based on dmapool.c. It has certain advantages
+ * over the DMA pools:
+ * - Pool collects resently freed pages for reuse (and hooks up to
+ *   the shrinker).
+ * - Tracks currently in use pages
+ * - Tracks whether the page is UC, WB or cached (and reverts to WB
+ *   when freed).
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/list.h>
+#include <linux/seq_file.h> /* for seq_printf */
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/highmem.h>
+#include <linux/mm_types.h>
+#include <linux/module.h>
+#include <linux/mm.h>
+#include <linux/atomic.h>
+#include <linux/device.h>
+#include <linux/kthread.h>
+#include "ttm/ttm_bo_driver.h"
+#include "ttm/ttm_page_alloc.h"
+#ifdef TTM_HAS_AGP
+#include <asm/agp.h>
+#endif
+
+#define NUM_PAGES_TO_ALLOC		(PAGE_SIZE/sizeof(struct page *))
+#define SMALL_ALLOCATION		4
+#define FREE_ALL_PAGES			(~0U)
+/* times are in msecs */
+#define IS_UNDEFINED			(0)
+#define IS_WC				(1<<1)
+#define IS_UC				(1<<2)
+#define IS_CACHED			(1<<3)
+#define IS_DMA32			(1<<4)
+
+enum pool_type {
+	POOL_IS_UNDEFINED,
+	POOL_IS_WC = IS_WC,
+	POOL_IS_UC = IS_UC,
+	POOL_IS_CACHED = IS_CACHED,
+	POOL_IS_WC_DMA32 = IS_WC | IS_DMA32,
+	POOL_IS_UC_DMA32 = IS_UC | IS_DMA32,
+	POOL_IS_CACHED_DMA32 = IS_CACHED | IS_DMA32,
+};
+/*
+ * The pool structure. There are usually six pools:
+ *  - generic (not restricted to DMA32):
+ *      - write combined, uncached, cached.
+ *  - dma32 (up to 2^32 - so up 4GB):
+ *      - write combined, uncached, cached.
+ * for each 'struct device'. The 'cached' is for pages that are actively used.
+ * The other ones can be shrunk by the shrinker API if neccessary.
+ * @pools: The 'struct device->dma_pools' link.
+ * @type: Type of the pool
+ * @lock: Protects the inuse_list and free_list from concurrnet access. Must be
+ * used with irqsave/irqrestore variants because pool allocator maybe called
+ * from delayed work.
+ * @inuse_list: Pool of pages that are in use. The order is very important and
+ *   it is in the order that the TTM pages that are put back are in.
+ * @free_list: Pool of pages that are free to be used. No order requirements.
+ * @dev: The device that is associated with these pools.
+ * @size: Size used during DMA allocation.
+ * @npages_free: Count of available pages for re-use.
+ * @npages_in_use: Count of pages that are in use.
+ * @nfrees: Stats when pool is shrinking.
+ * @nrefills: Stats when the pool is grown.
+ * @gfp_flags: Flags to pass for alloc_page.
+ * @name: Name of the pool.
+ * @dev_name: Name derieved from dev - similar to how dev_info works.
+ *   Used during shutdown as the dev_info during release is unavailable.
+ */
+struct dma_pool {
+	struct list_head pools; /* The 'struct device->dma_pools link */
+	enum pool_type type;
+	spinlock_t lock;
+	struct list_head inuse_list;
+	struct list_head free_list;
+	struct device *dev;
+	unsigned size;
+	unsigned npages_free;
+	unsigned npages_in_use;
+	unsigned long nfrees; /* Stats when shrunk. */
+	unsigned long nrefills; /* Stats when grown. */
+	gfp_t gfp_flags;
+	char name[13]; /* "cached dma32" */
+	char dev_name[64]; /* Constructed from dev */
+};
+
+/*
+ * The accounting page keeping track of the allocated page along with
+ * the DMA address.
+ * @page_list: The link to the 'page_list' in 'struct dma_pool'.
+ * @vaddr: The virtual address of the page
+ * @dma: The bus address of the page. If the page is not allocated
+ *   via the DMA API, it will be -1.
+ */
+struct dma_page {
+	struct list_head page_list;
+	void *vaddr;
+	struct page *p;
+	dma_addr_t dma;
+};
+
+/*
+ * Limits for the pool. They are handled without locks because only place where
+ * they may change is in sysfs store. They won't have immediate effect anyway
+ * so forcing serialization to access them is pointless.
+ */
+
+struct ttm_pool_opts {
+	unsigned	alloc_size;
+	unsigned	max_size;
+	unsigned	small;
+};
+
+/*
+ * Contains the list of all of the 'struct device' and their corresponding
+ * DMA pools. Guarded by _mutex->lock.
+ * @pools: The link to 'struct ttm_pool_manager->pools'
+ * @dev: The 'struct device' associated with the 'pool'
+ * @pool: The 'struct dma_pool' associated with the 'dev'
+ */
+struct device_pools {
+	struct list_head pools;
+	struct device *dev;
+	struct dma_pool *pool;
+};
+
+/*
+ * struct ttm_pool_manager - Holds memory pools for fast allocation
+ *
+ * @lock: Lock used when adding/removing from pools
+ * @pools: List of 'struct device' and 'struct dma_pool' tuples.
+ * @options: Limits for the pool.
+ * @npools: Total amount of pools in existence.
+ * @shrinker: The structure used by [un|]register_shrinker
+ */
+struct ttm_pool_manager {
+	struct mutex		lock;
+	struct list_head	pools;
+	struct ttm_pool_opts	options;
+	unsigned		npools;
+	struct shrinker		mm_shrink;
+	struct kobject		kobj;
+};
+
+static struct ttm_pool_manager *_manager;
+
+static struct attribute ttm_page_pool_max = {
+	.name = "pool_max_size",
+	.mode = S_IRUGO | S_IWUSR
+};
+static struct attribute ttm_page_pool_small = {
+	.name = "pool_small_allocation",
+	.mode = S_IRUGO | S_IWUSR
+};
+static struct attribute ttm_page_pool_alloc_size = {
+	.name = "pool_allocation_size",
+	.mode = S_IRUGO | S_IWUSR
+};
+
+static struct attribute *ttm_pool_attrs[] = {
+	&ttm_page_pool_max,
+	&ttm_page_pool_small,
+	&ttm_page_pool_alloc_size,
+	NULL
+};
+
+static void ttm_pool_kobj_release(struct kobject *kobj)
+{
+	struct ttm_pool_manager *m =
+		container_of(kobj, struct ttm_pool_manager, kobj);
+	kfree(m);
+}
+
+static ssize_t ttm_pool_store(struct kobject *kobj, struct attribute *attr,
+			      const char *buffer, size_t size)
+{
+	struct ttm_pool_manager *m =
+		container_of(kobj, struct ttm_pool_manager, kobj);
+	int chars;
+	unsigned val;
+	chars = sscanf(buffer, "%u", &val);
+	if (chars == 0)
+		return size;
+
+	/* Convert kb to number of pages */
+	val = val / (PAGE_SIZE >> 10);
+
+	if (attr == &ttm_page_pool_max)
+		m->options.max_size = val;
+	else if (attr == &ttm_page_pool_small)
+		m->options.small = val;
+	else if (attr == &ttm_page_pool_alloc_size) {
+		if (val > NUM_PAGES_TO_ALLOC*8) {
+			printk(KERN_ERR TTM_PFX
+			       "Setting allocation size to %lu "
+			       "is not allowed. Recommended size is "
+			       "%lu\n",
+			       NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 7),
+			       NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 10));
+			return size;
+		} else if (val > NUM_PAGES_TO_ALLOC) {
+			printk(KERN_WARNING TTM_PFX
+			       "Setting allocation size to "
+			       "larger than %lu is not recommended.\n",
+			       NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 10));
+		}
+		m->options.alloc_size = val;
+	}
+
+	return size;
+}
+
+static ssize_t ttm_pool_show(struct kobject *kobj, struct attribute *attr,
+			     char *buffer)
+{
+	struct ttm_pool_manager *m =
+		container_of(kobj, struct ttm_pool_manager, kobj);
+	unsigned val = 0;
+
+	if (attr == &ttm_page_pool_max)
+		val = m->options.max_size;
+	else if (attr == &ttm_page_pool_small)
+		val = m->options.small;
+	else if (attr == &ttm_page_pool_alloc_size)
+		val = m->options.alloc_size;
+
+	val = val * (PAGE_SIZE >> 10);
+
+	return snprintf(buffer, PAGE_SIZE, "%u\n", val);
+}
+
+static const struct sysfs_ops ttm_pool_sysfs_ops = {
+	.show = &ttm_pool_show,
+	.store = &ttm_pool_store,
+};
+
+static struct kobj_type ttm_pool_kobj_type = {
+	.release = &ttm_pool_kobj_release,
+	.sysfs_ops = &ttm_pool_sysfs_ops,
+	.default_attrs = ttm_pool_attrs,
+};
+
+#ifndef CONFIG_X86
+static int set_pages_array_wb(struct page **pages, int addrinarray)
+{
+#ifdef TTM_HAS_AGP
+	int i;
+
+	for (i = 0; i < addrinarray; i++)
+		unmap_page_from_agp(pages[i]);
+#endif
+	return 0;
+}
+
+static int set_pages_array_wc(struct page **pages, int addrinarray)
+{
+#ifdef TTM_HAS_AGP
+	int i;
+
+	for (i = 0; i < addrinarray; i++)
+		map_page_into_agp(pages[i]);
+#endif
+	return 0;
+}
+
+static int set_pages_array_uc(struct page **pages, int addrinarray)
+{
+#ifdef TTM_HAS_AGP
+	int i;
+
+	for (i = 0; i < addrinarray; i++)
+		map_page_into_agp(pages[i]);
+#endif
+	return 0;
+}
+#endif /* for !CONFIG_X86 */
+
+static int ttm_set_pages_caching(struct dma_pool *pool,
+				 struct page **pages, unsigned cpages)
+{
+	int r = 0;
+	/* Set page caching */
+	if (pool->type & IS_UC) {
+		r = set_pages_array_uc(pages, cpages);
+		if (r)
+			pr_err(TTM_PFX
+			       "%s: Failed to set %d pages to uc!\n",
+			       pool->dev_name, cpages);
+	}
+	if (pool->type & IS_WC) {
+		r = set_pages_array_wc(pages, cpages);
+		if (r)
+			pr_err(TTM_PFX
+			       "%s: Failed to set %d pages to wc!\n",
+			       pool->dev_name, cpages);
+	}
+	return r;
+}
+
+static void __ttm_dma_free_page(struct dma_pool *pool, struct dma_page *d_page)
+{
+	dma_addr_t dma = d_page->dma;
+	dma_free_coherent(pool->dev, pool->size, d_page->vaddr, dma);
+
+	kfree(d_page);
+	d_page = NULL;
+}
+static struct dma_page *__ttm_dma_alloc_page(struct dma_pool *pool)
+{
+	struct dma_page *d_page;
+
+	d_page = kmalloc(sizeof(struct dma_page), GFP_KERNEL);
+	if (!d_page)
+		return NULL;
+
+	d_page->vaddr = dma_alloc_coherent(pool->dev, pool->size,
+					   &d_page->dma,
+					   pool->gfp_flags);
+	if (d_page->vaddr)
+		d_page->p = virt_to_page(d_page->vaddr);
+	else {
+		kfree(d_page);
+		d_page = NULL;
+	}
+	return d_page;
+}
+static enum pool_type ttm_to_type(int flags, enum ttm_caching_state cstate)
+{
+	enum pool_type type = IS_UNDEFINED;
+
+	if (flags & TTM_PAGE_FLAG_DMA32)
+		type |= IS_DMA32;
+	if (cstate == tt_cached)
+		type |= IS_CACHED;
+	else if (cstate == tt_uncached)
+		type |= IS_UC;
+	else
+		type |= IS_WC;
+
+	return type;
+}
+
+static void ttm_pool_update_free_locked(struct dma_pool *pool,
+					unsigned freed_pages)
+{
+	pool->npages_free -= freed_pages;
+	pool->nfrees += freed_pages;
+
+}
+
+/* set memory back to wb and free the pages. */
+static void ttm_dma_pages_put(struct dma_pool *pool, struct list_head *d_pages,
+			      struct page *pages[], unsigned npages)
+{
+	struct dma_page *d_page, *tmp;
+
+	/* Don't set WB on WB page pool. */
+	if (npages && !(pool->type & IS_CACHED) &&
+	    set_pages_array_wb(pages, npages))
+		pr_err(TTM_PFX "%s: Failed to set %d pages to wb!\n",
+			pool->dev_name, npages);
+
+	list_for_each_entry_safe(d_page, tmp, d_pages, page_list) {
+		list_del(&d_page->page_list);
+		__ttm_dma_free_page(pool, d_page);
+	}
+}
+
+static void ttm_dma_page_put(struct dma_pool *pool, struct dma_page *d_page)
+{
+	/* Don't set WB on WB page pool. */
+	if (!(pool->type & IS_CACHED) && set_pages_array_wb(&d_page->p, 1))
+		pr_err(TTM_PFX "%s: Failed to set %d pages to wb!\n",
+			pool->dev_name, 1);
+
+	list_del(&d_page->page_list);
+	__ttm_dma_free_page(pool, d_page);
+}
+
+/*
+ * Free pages from pool.
+ *
+ * To prevent hogging the ttm_swap process we only free NUM_PAGES_TO_ALLOC
+ * number of pages in one go.
+ *
+ * @pool: to free the pages from
+ * @nr_free: If set to true will free all pages in pool
+ **/
+static unsigned ttm_dma_page_pool_free(struct dma_pool *pool, unsigned nr_free)
+{
+	unsigned long irq_flags;
+	struct dma_page *dma_p, *tmp;
+	struct page **pages_to_free;
+	struct list_head d_pages;
+	unsigned freed_pages = 0,
+		 npages_to_free = nr_free;
+
+	if (NUM_PAGES_TO_ALLOC < nr_free)
+		npages_to_free = NUM_PAGES_TO_ALLOC;
+#if 0
+	if (nr_free > 1) {
+		pr_debug("%s: (%s:%d) Attempting to free %d (%d) pages\n",
+			pool->dev_name, pool->name, current->pid,
+			npages_to_free, nr_free);
+	}
+#endif
+	pages_to_free = kmalloc(npages_to_free * sizeof(struct page *),
+			GFP_KERNEL);
+
+	if (!pages_to_free) {
+		pr_err(TTM_PFX
+		       "%s: Failed to allocate memory for pool free operation.\n",
+			pool->dev_name);
+		return 0;
+	}
+	INIT_LIST_HEAD(&d_pages);
+restart:
+	spin_lock_irqsave(&pool->lock, irq_flags);
+
+	/* We picking the oldest ones off the list */
+	list_for_each_entry_safe_reverse(dma_p, tmp, &pool->free_list,
+					 page_list) {
+		if (freed_pages >= npages_to_free)
+			break;
+
+		/* Move the dma_page from one list to another. */
+		list_move(&dma_p->page_list, &d_pages);
+
+		pages_to_free[freed_pages++] = dma_p->p;
+		/* We can only remove NUM_PAGES_TO_ALLOC at a time. */
+		if (freed_pages >= NUM_PAGES_TO_ALLOC) {
+
+			ttm_pool_update_free_locked(pool, freed_pages);
+			/**
+			 * Because changing page caching is costly
+			 * we unlock the pool to prevent stalling.
+			 */
+			spin_unlock_irqrestore(&pool->lock, irq_flags);
+
+			ttm_dma_pages_put(pool, &d_pages, pages_to_free,
+					  freed_pages);
+
+			INIT_LIST_HEAD(&d_pages);
+
+			if (likely(nr_free != FREE_ALL_PAGES))
+				nr_free -= freed_pages;
+
+			if (NUM_PAGES_TO_ALLOC >= nr_free)
+				npages_to_free = nr_free;
+			else
+				npages_to_free = NUM_PAGES_TO_ALLOC;
+
+			freed_pages = 0;
+
+			/* free all so restart the processing */
+			if (nr_free)
+				goto restart;
+
+			/* Not allowed to fall through or break because
+			 * following context is inside spinlock while we are
+			 * outside here.
+			 */
+			goto out;
+
+		}
+	}
+
+	/* remove range of pages from the pool */
+	if (freed_pages) {
+		ttm_pool_update_free_locked(pool, freed_pages);
+		nr_free -= freed_pages;
+	}
+
+	spin_unlock_irqrestore(&pool->lock, irq_flags);
+
+	if (freed_pages)
+		ttm_dma_pages_put(pool, &d_pages, pages_to_free, freed_pages);
+out:
+	kfree(pages_to_free);
+	return nr_free;
+}
+
+static void ttm_dma_free_pool(struct device *dev, enum pool_type type)
+{
+	struct device_pools *p;
+	struct dma_pool *pool;
+
+	if (!dev)
+		return;
+
+	mutex_lock(&_manager->lock);
+	list_for_each_entry_reverse(p, &_manager->pools, pools) {
+		if (p->dev != dev)
+			continue;
+		pool = p->pool;
+		if (pool->type != type)
+			continue;
+
+		list_del(&p->pools);
+		kfree(p);
+		_manager->npools--;
+		break;
+	}
+	list_for_each_entry_reverse(pool, &dev->dma_pools, pools) {
+		if (pool->type != type)
+			continue;
+		/* Takes a spinlock.. */
+		ttm_dma_page_pool_free(pool, FREE_ALL_PAGES);
+		WARN_ON(((pool->npages_in_use + pool->npages_free) != 0));
+		/* This code path is called after _all_ references to the
+		 * struct device has been dropped - so nobody should be
+		 * touching it. In case somebody is trying to _add_ we are
+		 * guarded by the mutex. */
+		list_del(&pool->pools);
+		kfree(pool);
+		break;
+	}
+	mutex_unlock(&_manager->lock);
+}
+
+/*
+ * On free-ing of the 'struct device' this deconstructor is run.
+ * Albeit the pool might have already been freed earlier.
+ */
+static void ttm_dma_pool_release(struct device *dev, void *res)
+{
+	struct dma_pool *pool = *(struct dma_pool **)res;
+
+	if (pool)
+		ttm_dma_free_pool(dev, pool->type);
+}
+
+static int ttm_dma_pool_match(struct device *dev, void *res, void *match_data)
+{
+	return *(struct dma_pool **)res == match_data;
+}
+
+static struct dma_pool *ttm_dma_pool_init(struct device *dev, gfp_t flags,
+					  enum pool_type type)
+{
+	char *n[] = {"wc", "uc", "cached", " dma32", "unknown",};
+	enum pool_type t[] = {IS_WC, IS_UC, IS_CACHED, IS_DMA32, IS_UNDEFINED};
+	struct device_pools *sec_pool = NULL;
+	struct dma_pool *pool = NULL, **ptr;
+	unsigned i;
+	int ret = -ENODEV;
+	char *p;
+
+	if (!dev)
+		return NULL;
+
+	ptr = devres_alloc(ttm_dma_pool_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return NULL;
+
+	ret = -ENOMEM;
+
+	pool = kmalloc_node(sizeof(struct dma_pool), GFP_KERNEL,
+			    dev_to_node(dev));
+	if (!pool)
+		goto err_mem;
+
+	sec_pool = kmalloc_node(sizeof(struct device_pools), GFP_KERNEL,
+				dev_to_node(dev));
+	if (!sec_pool)
+		goto err_mem;
+
+	INIT_LIST_HEAD(&sec_pool->pools);
+	sec_pool->dev = dev;
+	sec_pool->pool =  pool;
+
+	INIT_LIST_HEAD(&pool->free_list);
+	INIT_LIST_HEAD(&pool->inuse_list);
+	INIT_LIST_HEAD(&pool->pools);
+	spin_lock_init(&pool->lock);
+	pool->dev = dev;
+	pool->npages_free = pool->npages_in_use = 0;
+	pool->nfrees = 0;
+	pool->gfp_flags = flags;
+	pool->size = PAGE_SIZE;
+	pool->type = type;
+	pool->nrefills = 0;
+	p = pool->name;
+	for (i = 0; i < 5; i++) {
+		if (type & t[i]) {
+			p += snprintf(p, sizeof(pool->name) - (p - pool->name),
+				      "%s", n[i]);
+		}
+	}
+	*p = 0;
+	/* We copy the name for pr_ calls b/c when dma_pool_destroy is called
+	 * - the kobj->name has already been deallocated.*/
+	snprintf(pool->dev_name, sizeof(pool->dev_name), "%s %s",
+		 dev_driver_string(dev), dev_name(dev));
+	mutex_lock(&_manager->lock);
+	/* You can get the dma_pool from either the global: */
+	list_add(&sec_pool->pools, &_manager->pools);
+	_manager->npools++;
+	/* or from 'struct device': */
+	list_add(&pool->pools, &dev->dma_pools);
+	mutex_unlock(&_manager->lock);
+
+	*ptr = pool;
+	devres_add(dev, ptr);
+
+	return pool;
+err_mem:
+	devres_free(ptr);
+	kfree(sec_pool);
+	kfree(pool);
+	return ERR_PTR(ret);
+}
+
+static struct dma_pool *ttm_dma_find_pool(struct device *dev,
+					  enum pool_type type)
+{
+	struct dma_pool *pool, *tmp, *found = NULL;
+
+	if (type == IS_UNDEFINED)
+		return found;
+
+	/* NB: We iterate on the 'struct dev' which has no spinlock, but
+	 * it does have a kref which we have taken. The kref is taken during
+	 * graphic driver loading - in the drm_pci_init it calls either
+	 * pci_dev_get or pci_register_driver which both end up taking a kref
+	 * on 'struct device'.
+	 *
+	 * On teardown, the graphic drivers end up quiescing the TTM (put_pages)
+	 * and calls the dev_res deconstructors: ttm_dma_pool_release. The nice
+	 * thing is at that point of time there are no pages associated with the
+	 * driver so this function will not be called.
+	 */
+	list_for_each_entry_safe(pool, tmp, &dev->dma_pools, pools) {
+		if (pool->type != type)
+			continue;
+		found = pool;
+		break;
+	}
+	return found;
+}
+
+/*
+ * Free pages the pages that failed to change the caching state. If there
+ * are pages that have changed their caching state already put them to the
+ * pool.
+ */
+static void ttm_dma_handle_caching_state_failure(struct dma_pool *pool,
+						 struct list_head *d_pages,
+						 struct page **failed_pages,
+						 unsigned cpages)
+{
+	struct dma_page *d_page, *tmp;
+	struct page *p;
+	unsigned i = 0;
+
+	p = failed_pages[0];
+	if (!p)
+		return;
+	/* Find the failed page. */
+	list_for_each_entry_safe(d_page, tmp, d_pages, page_list) {
+		if (d_page->p != p)
+			continue;
+		/* .. and then progress over the full list. */
+		list_del(&d_page->page_list);
+		__ttm_dma_free_page(pool, d_page);
+		if (++i < cpages)
+			p = failed_pages[i];
+		else
+			break;
+	}
+
+}
+
+/*
+ * Allocate 'count' pages, and put 'need' number of them on the
+ * 'pages' and as well on the 'dma_address' starting at 'dma_offset' offset.
+ * The full list of pages should also be on 'd_pages'.
+ * We return zero for success, and negative numbers as errors.
+ */
+static int ttm_dma_pool_alloc_new_pages(struct dma_pool *pool,
+					struct list_head *d_pages,
+					unsigned count)
+{
+	struct page **caching_array;
+	struct dma_page *dma_p;
+	struct page *p;
+	int r = 0;
+	unsigned i, cpages;
+	unsigned max_cpages = min(count,
+			(unsigned)(PAGE_SIZE/sizeof(struct page *)));
+
+	/* allocate array for page caching change */
+	caching_array = kmalloc(max_cpages*sizeof(struct page *), GFP_KERNEL);
+
+	if (!caching_array) {
+		pr_err(TTM_PFX
+		       "%s: Unable to allocate table for new pages.",
+			pool->dev_name);
+		return -ENOMEM;
+	}
+
+	if (count > 1) {
+		pr_debug("%s: (%s:%d) Getting %d pages\n",
+			pool->dev_name, pool->name, current->pid,
+			count);
+	}
+
+	for (i = 0, cpages = 0; i < count; ++i) {
+		dma_p = __ttm_dma_alloc_page(pool);
+		if (!dma_p) {
+			pr_err(TTM_PFX "%s: Unable to get page %u.\n",
+				pool->dev_name, i);
+
+			/* store already allocated pages in the pool after
+			 * setting the caching state */
+			if (cpages) {
+				r = ttm_set_pages_caching(pool, caching_array,
+							  cpages);
+				if (r)
+					ttm_dma_handle_caching_state_failure(
+						pool, d_pages, caching_array,
+						cpages);
+			}
+			r = -ENOMEM;
+			goto out;
+		}
+		p = dma_p->p;
+#ifdef CONFIG_HIGHMEM
+		/* gfp flags of highmem page should never be dma32 so we
+		 * we should be fine in such case
+		 */
+		if (!PageHighMem(p))
+#endif
+		{
+			caching_array[cpages++] = p;
+			if (cpages == max_cpages) {
+				/* Note: Cannot hold the spinlock */
+				r = ttm_set_pages_caching(pool, caching_array,
+						 cpages);
+				if (r) {
+					ttm_dma_handle_caching_state_failure(
+						pool, d_pages, caching_array,
+						cpages);
+					goto out;
+				}
+				cpages = 0;
+			}
+		}
+		list_add(&dma_p->page_list, d_pages);
+	}
+
+	if (cpages) {
+		r = ttm_set_pages_caching(pool, caching_array, cpages);
+		if (r)
+			ttm_dma_handle_caching_state_failure(pool, d_pages,
+					caching_array, cpages);
+	}
+out:
+	kfree(caching_array);
+	return r;
+}
+
+/*
+ * @return count of pages still required to fulfill the request.
+ */
+static int ttm_dma_page_pool_fill_locked(struct dma_pool *pool,
+					 unsigned long *irq_flags)
+{
+	unsigned count = _manager->options.small;
+	int r = pool->npages_free;
+
+	if (count > pool->npages_free) {
+		struct list_head d_pages;
+
+		INIT_LIST_HEAD(&d_pages);
+
+		spin_unlock_irqrestore(&pool->lock, *irq_flags);
+
+		/* Returns how many more are neccessary to fulfill the
+		 * request. */
+		r = ttm_dma_pool_alloc_new_pages(pool, &d_pages, count);
+
+		spin_lock_irqsave(&pool->lock, *irq_flags);
+		if (!r) {
+			/* Add the fresh to the end.. */
+			list_splice(&d_pages, &pool->free_list);
+			++pool->nrefills;
+			pool->npages_free += count;
+			r = count;
+		} else {
+			struct dma_page *d_page;
+			unsigned cpages = 0;
+
+			pr_err(TTM_PFX "%s: Failed to fill %s pool (r:%d)!\n",
+				pool->dev_name, pool->name, r);
+
+			list_for_each_entry(d_page, &d_pages, page_list) {
+				cpages++;
+			}
+			list_splice_tail(&d_pages, &pool->free_list);
+			pool->npages_free += cpages;
+			r = cpages;
+		}
+	}
+	return r;
+}
+
+/*
+ * @return count of pages still required to fulfill the request.
+ * The populate list is actually a stack (not that is matters as TTM
+ * allocates one page at a time.
+ */
+static int ttm_dma_pool_get_pages(struct dma_pool *pool,
+				  struct ttm_dma_tt *ttm_dma,
+				  unsigned index)
+{
+	struct dma_page *d_page;
+	struct ttm_tt *ttm = &ttm_dma->ttm;
+	unsigned long irq_flags;
+	int count, r = -ENOMEM;
+
+	spin_lock_irqsave(&pool->lock, irq_flags);
+	count = ttm_dma_page_pool_fill_locked(pool, &irq_flags);
+	if (count) {
+		d_page = list_first_entry(&pool->free_list, struct dma_page, page_list);
+		ttm->pages[index] = d_page->p;
+		ttm_dma->dma_address[index] = d_page->dma;
+		list_move_tail(&d_page->page_list, &ttm_dma->pages_list);
+		r = 0;
+		pool->npages_in_use += 1;
+		pool->npages_free -= 1;
+	}
+	spin_unlock_irqrestore(&pool->lock, irq_flags);
+	return r;
+}
+
+/*
+ * On success pages list will hold count number of correctly
+ * cached pages. On failure will hold the negative return value (-ENOMEM, etc).
+ */
+int ttm_dma_populate(struct ttm_dma_tt *ttm_dma, struct device *dev)
+{
+	struct ttm_tt *ttm = &ttm_dma->ttm;
+	struct ttm_mem_global *mem_glob = ttm->glob->mem_glob;
+	struct dma_pool *pool;
+	enum pool_type type;
+	unsigned i;
+	gfp_t gfp_flags;
+	int ret;
+
+	if (ttm->state != tt_unpopulated)
+		return 0;
+
+	type = ttm_to_type(ttm->page_flags, ttm->caching_state);
+	if (ttm->page_flags & TTM_PAGE_FLAG_DMA32)
+		gfp_flags = GFP_USER | GFP_DMA32;
+	else
+		gfp_flags = GFP_HIGHUSER;
+	if (ttm->page_flags & TTM_PAGE_FLAG_ZERO_ALLOC)
+		gfp_flags |= __GFP_ZERO;
+
+	pool = ttm_dma_find_pool(dev, type);
+	if (!pool) {
+		pool = ttm_dma_pool_init(dev, gfp_flags, type);
+		if (IS_ERR_OR_NULL(pool)) {
+			return -ENOMEM;
+		}
+	}
+
+	INIT_LIST_HEAD(&ttm_dma->pages_list);
+	for (i = 0; i < ttm->num_pages; ++i) {
+		ret = ttm_dma_pool_get_pages(pool, ttm_dma, i);
+		if (ret != 0) {
+			ttm_dma_unpopulate(ttm_dma, dev);
+			return -ENOMEM;
+		}
+
+		ret = ttm_mem_global_alloc_page(mem_glob, ttm->pages[i],
+						false, false);
+		if (unlikely(ret != 0)) {
+			ttm_dma_unpopulate(ttm_dma, dev);
+			return -ENOMEM;
+		}
+	}
+
+	if (unlikely(ttm->page_flags & TTM_PAGE_FLAG_SWAPPED)) {
+		ret = ttm_tt_swapin(ttm);
+		if (unlikely(ret != 0)) {
+			ttm_dma_unpopulate(ttm_dma, dev);
+			return ret;
+		}
+	}
+
+	ttm->state = tt_unbound;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ttm_dma_populate);
+
+/* Get good estimation how many pages are free in pools */
+static int ttm_dma_pool_get_num_unused_pages(void)
+{
+	struct device_pools *p;
+	unsigned total = 0;
+
+	mutex_lock(&_manager->lock);
+	list_for_each_entry(p, &_manager->pools, pools)
+		total += p->pool->npages_free;
+	mutex_unlock(&_manager->lock);
+	return total;
+}
+
+/* Put all pages in pages list to correct pool to wait for reuse */
+void ttm_dma_unpopulate(struct ttm_dma_tt *ttm_dma, struct device *dev)
+{
+	struct ttm_tt *ttm = &ttm_dma->ttm;
+	struct dma_pool *pool;
+	struct dma_page *d_page, *next;
+	enum pool_type type;
+	bool is_cached = false;
+	unsigned count = 0, i, npages = 0;
+	unsigned long irq_flags;
+
+	type = ttm_to_type(ttm->page_flags, ttm->caching_state);
+	pool = ttm_dma_find_pool(dev, type);
+	if (!pool) {
+		WARN_ON(!pool);
+		return;
+	}
+	is_cached = (ttm_dma_find_pool(pool->dev,
+		     ttm_to_type(ttm->page_flags, tt_cached)) == pool);
+
+	/* make sure pages array match list and count number of pages */
+	list_for_each_entry(d_page, &ttm_dma->pages_list, page_list) {
+		ttm->pages[count] = d_page->p;
+		count++;
+	}
+
+	spin_lock_irqsave(&pool->lock, irq_flags);
+	pool->npages_in_use -= count;
+	if (is_cached) {
+		pool->nfrees += count;
+	} else {
+		pool->npages_free += count;
+		list_splice(&ttm_dma->pages_list, &pool->free_list);
+		npages = count;
+		if (pool->npages_free > _manager->options.max_size) {
+			npages = pool->npages_free - _manager->options.max_size;
+			/* free at least NUM_PAGES_TO_ALLOC number of pages
+			 * to reduce calls to set_memory_wb */
+			if (npages < NUM_PAGES_TO_ALLOC)
+				npages = NUM_PAGES_TO_ALLOC;
+		}
+	}
+	spin_unlock_irqrestore(&pool->lock, irq_flags);
+
+	if (is_cached) {
+		list_for_each_entry_safe(d_page, next, &ttm_dma->pages_list, page_list) {
+			ttm_mem_global_free_page(ttm->glob->mem_glob,
+						 d_page->p);
+			ttm_dma_page_put(pool, d_page);
+		}
+	} else {
+		for (i = 0; i < count; i++) {
+			ttm_mem_global_free_page(ttm->glob->mem_glob,
+						 ttm->pages[i]);
+		}
+	}
+
+	INIT_LIST_HEAD(&ttm_dma->pages_list);
+	for (i = 0; i < ttm->num_pages; i++) {
+		ttm->pages[i] = NULL;
+		ttm_dma->dma_address[i] = 0;
+	}
+
+	/* shrink pool if necessary (only on !is_cached pools)*/
+	if (npages)
+		ttm_dma_page_pool_free(pool, npages);
+	ttm->state = tt_unpopulated;
+}
+EXPORT_SYMBOL_GPL(ttm_dma_unpopulate);
+
+/**
+ * Callback for mm to request pool to reduce number of page held.
+ */
+static int ttm_dma_pool_mm_shrink(struct shrinker *shrink,
+				  struct shrink_control *sc)
+{
+	static atomic_t start_pool = ATOMIC_INIT(0);
+	unsigned idx = 0;
+	unsigned pool_offset = atomic_add_return(1, &start_pool);
+	unsigned shrink_pages = sc->nr_to_scan;
+	struct device_pools *p;
+
+	if (list_empty(&_manager->pools))
+		return 0;
+
+	mutex_lock(&_manager->lock);
+	pool_offset = pool_offset % _manager->npools;
+	list_for_each_entry(p, &_manager->pools, pools) {
+		unsigned nr_free;
+
+		if (!p->dev)
+			continue;
+		if (shrink_pages == 0)
+			break;
+		/* Do it in round-robin fashion. */
+		if (++idx < pool_offset)
+			continue;
+		nr_free = shrink_pages;
+		shrink_pages = ttm_dma_page_pool_free(p->pool, nr_free);
+		pr_debug("%s: (%s:%d) Asked to shrink %d, have %d more to go\n",
+			p->pool->dev_name, p->pool->name, current->pid, nr_free,
+			shrink_pages);
+	}
+	mutex_unlock(&_manager->lock);
+	/* return estimated number of unused pages in pool */
+	return ttm_dma_pool_get_num_unused_pages();
+}
+
+static void ttm_dma_pool_mm_shrink_init(struct ttm_pool_manager *manager)
+{
+	manager->mm_shrink.shrink = &ttm_dma_pool_mm_shrink;
+	manager->mm_shrink.seeks = 1;
+	register_shrinker(&manager->mm_shrink);
+}
+
+static void ttm_dma_pool_mm_shrink_fini(struct ttm_pool_manager *manager)
+{
+	unregister_shrinker(&manager->mm_shrink);
+}
+
+int ttm_dma_page_alloc_init(struct ttm_mem_global *glob, unsigned max_pages)
+{
+	int ret = -ENOMEM;
+
+	WARN_ON(_manager);
+
+	printk(KERN_INFO TTM_PFX "Initializing DMA pool allocator.\n");
+
+	_manager = kzalloc(sizeof(*_manager), GFP_KERNEL);
+	if (!_manager)
+		goto err_manager;
+
+	mutex_init(&_manager->lock);
+	INIT_LIST_HEAD(&_manager->pools);
+
+	_manager->options.max_size = max_pages;
+	_manager->options.small = SMALL_ALLOCATION;
+	_manager->options.alloc_size = NUM_PAGES_TO_ALLOC;
+
+	/* This takes care of auto-freeing the _manager */
+	ret = kobject_init_and_add(&_manager->kobj, &ttm_pool_kobj_type,
+				   &glob->kobj, "dma_pool");
+	if (unlikely(ret != 0)) {
+		kobject_put(&_manager->kobj);
+		goto err;
+	}
+	ttm_dma_pool_mm_shrink_init(_manager);
+	return 0;
+err_manager:
+	kfree(_manager);
+	_manager = NULL;
+err:
+	return ret;
+}
+
+void ttm_dma_page_alloc_fini(void)
+{
+	struct device_pools *p, *t;
+
+	printk(KERN_INFO TTM_PFX "Finalizing DMA pool allocator.\n");
+	ttm_dma_pool_mm_shrink_fini(_manager);
+
+	list_for_each_entry_safe_reverse(p, t, &_manager->pools, pools) {
+		dev_dbg(p->dev, "(%s:%d) Freeing.\n", p->pool->name,
+			current->pid);
+		WARN_ON(devres_destroy(p->dev, ttm_dma_pool_release,
+			ttm_dma_pool_match, p->pool));
+		ttm_dma_free_pool(p->dev, p->pool->type);
+	}
+	kobject_put(&_manager->kobj);
+	_manager = NULL;
+}
+
+int ttm_dma_page_alloc_debugfs(struct seq_file *m, void *data)
+{
+	struct device_pools *p;
+	struct dma_pool *pool = NULL;
+	char *h[] = {"pool", "refills", "pages freed", "inuse", "available",
+		     "name", "virt", "busaddr"};
+
+	if (!_manager) {
+		seq_printf(m, "No pool allocator running.\n");
+		return 0;
+	}
+	seq_printf(m, "%13s %12s %13s %8s %8s %8s\n",
+		   h[0], h[1], h[2], h[3], h[4], h[5]);
+	mutex_lock(&_manager->lock);
+	list_for_each_entry(p, &_manager->pools, pools) {
+		struct device *dev = p->dev;
+		if (!dev)
+			continue;
+		pool = p->pool;
+		seq_printf(m, "%13s %12ld %13ld %8d %8d %8s\n",
+				pool->name, pool->nrefills,
+				pool->nfrees, pool->npages_in_use,
+				pool->npages_free,
+				pool->dev_name);
+	}
+	mutex_unlock(&_manager->lock);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ttm_dma_page_alloc_debugfs);
diff --git a/drivers/gpu/drm/ttm/ttm_tt.c b/drivers/gpu/drm/ttm/ttm_tt.c
index f9cc548d6d98..2f75d203a2bf 100644
--- a/drivers/gpu/drm/ttm/ttm_tt.c
+++ b/drivers/gpu/drm/ttm/ttm_tt.c
@@ -43,139 +43,20 @@
 #include "ttm/ttm_placement.h"
 #include "ttm/ttm_page_alloc.h"
 
-static int ttm_tt_swapin(struct ttm_tt *ttm);
-
 /**
  * Allocates storage for pointers to the pages that back the ttm.
  */
 static void ttm_tt_alloc_page_directory(struct ttm_tt *ttm)
 {
-	ttm->pages = drm_calloc_large(ttm->num_pages, sizeof(*ttm->pages));
-	ttm->dma_address = drm_calloc_large(ttm->num_pages,
-					    sizeof(*ttm->dma_address));
-}
-
-static void ttm_tt_free_page_directory(struct ttm_tt *ttm)
-{
-	drm_free_large(ttm->pages);
-	ttm->pages = NULL;
-	drm_free_large(ttm->dma_address);
-	ttm->dma_address = NULL;
-}
-
-static void ttm_tt_free_user_pages(struct ttm_tt *ttm)
-{
-	int write;
-	int dirty;
-	struct page *page;
-	int i;
-	struct ttm_backend *be = ttm->be;
-
-	BUG_ON(!(ttm->page_flags & TTM_PAGE_FLAG_USER));
-	write = ((ttm->page_flags & TTM_PAGE_FLAG_WRITE) != 0);
-	dirty = ((ttm->page_flags & TTM_PAGE_FLAG_USER_DIRTY) != 0);
-
-	if (be)
-		be->func->clear(be);
-
-	for (i = 0; i < ttm->num_pages; ++i) {
-		page = ttm->pages[i];
-		if (page == NULL)
-			continue;
-
-		if (page == ttm->dummy_read_page) {
-			BUG_ON(write);
-			continue;
-		}
-
-		if (write && dirty && !PageReserved(page))
-			set_page_dirty_lock(page);
-
-		ttm->pages[i] = NULL;
-		ttm_mem_global_free(ttm->glob->mem_glob, PAGE_SIZE);
-		put_page(page);
-	}
-	ttm->state = tt_unpopulated;
-	ttm->first_himem_page = ttm->num_pages;
-	ttm->last_lomem_page = -1;
+	ttm->pages = drm_calloc_large(ttm->num_pages, sizeof(void*));
 }
 
-static struct page *__ttm_tt_get_page(struct ttm_tt *ttm, int index)
+static void ttm_dma_tt_alloc_page_directory(struct ttm_dma_tt *ttm)
 {
-	struct page *p;
-	struct list_head h;
-	struct ttm_mem_global *mem_glob = ttm->glob->mem_glob;
-	int ret;
-
-	while (NULL == (p = ttm->pages[index])) {
-
-		INIT_LIST_HEAD(&h);
-
-		ret = ttm_get_pages(&h, ttm->page_flags, ttm->caching_state, 1,
-				    &ttm->dma_address[index]);
-
-		if (ret != 0)
-			return NULL;
-
-		p = list_first_entry(&h, struct page, lru);
-
-		ret = ttm_mem_global_alloc_page(mem_glob, p, false, false);
-		if (unlikely(ret != 0))
-			goto out_err;
-
-		if (PageHighMem(p))
-			ttm->pages[--ttm->first_himem_page] = p;
-		else
-			ttm->pages[++ttm->last_lomem_page] = p;
-	}
-	return p;
-out_err:
-	put_page(p);
-	return NULL;
-}
-
-struct page *ttm_tt_get_page(struct ttm_tt *ttm, int index)
-{
-	int ret;
-
-	if (unlikely(ttm->page_flags & TTM_PAGE_FLAG_SWAPPED)) {
-		ret = ttm_tt_swapin(ttm);
-		if (unlikely(ret != 0))
-			return NULL;
-	}
-	return __ttm_tt_get_page(ttm, index);
-}
-
-int ttm_tt_populate(struct ttm_tt *ttm)
-{
-	struct page *page;
-	unsigned long i;
-	struct ttm_backend *be;
-	int ret;
-
-	if (ttm->state != tt_unpopulated)
-		return 0;
-
-	if (unlikely(ttm->page_flags & TTM_PAGE_FLAG_SWAPPED)) {
-		ret = ttm_tt_swapin(ttm);
-		if (unlikely(ret != 0))
-			return ret;
-	}
-
-	be = ttm->be;
-
-	for (i = 0; i < ttm->num_pages; ++i) {
-		page = __ttm_tt_get_page(ttm, i);
-		if (!page)
-			return -ENOMEM;
-	}
-
-	be->func->populate(be, ttm->num_pages, ttm->pages,
-			   ttm->dummy_read_page, ttm->dma_address);
-	ttm->state = tt_unbound;
-	return 0;
+	ttm->ttm.pages = drm_calloc_large(ttm->ttm.num_pages, sizeof(void*));
+	ttm->dma_address = drm_calloc_large(ttm->ttm.num_pages,
+					    sizeof(*ttm->dma_address));
 }
-EXPORT_SYMBOL(ttm_tt_populate);
 
 #ifdef CONFIG_X86
 static inline int ttm_tt_set_page_caching(struct page *p,
@@ -278,153 +159,100 @@ int ttm_tt_set_placement_caching(struct ttm_tt *ttm, uint32_t placement)
 }
 EXPORT_SYMBOL(ttm_tt_set_placement_caching);
 
-static void ttm_tt_free_alloced_pages(struct ttm_tt *ttm)
-{
-	int i;
-	unsigned count = 0;
-	struct list_head h;
-	struct page *cur_page;
-	struct ttm_backend *be = ttm->be;
-
-	INIT_LIST_HEAD(&h);
-
-	if (be)
-		be->func->clear(be);
-	for (i = 0; i < ttm->num_pages; ++i) {
-
-		cur_page = ttm->pages[i];
-		ttm->pages[i] = NULL;
-		if (cur_page) {
-			if (page_count(cur_page) != 1)
-				printk(KERN_ERR TTM_PFX
-				       "Erroneous page count. "
-				       "Leaking pages.\n");
-			ttm_mem_global_free_page(ttm->glob->mem_glob,
-						 cur_page);
-			list_add(&cur_page->lru, &h);
-			count++;
-		}
-	}
-	ttm_put_pages(&h, count, ttm->page_flags, ttm->caching_state,
-		      ttm->dma_address);
-	ttm->state = tt_unpopulated;
-	ttm->first_himem_page = ttm->num_pages;
-	ttm->last_lomem_page = -1;
-}
-
 void ttm_tt_destroy(struct ttm_tt *ttm)
 {
-	struct ttm_backend *be;
-
 	if (unlikely(ttm == NULL))
 		return;
 
-	be = ttm->be;
-	if (likely(be != NULL)) {
-		be->func->destroy(be);
-		ttm->be = NULL;
+	if (ttm->state == tt_bound) {
+		ttm_tt_unbind(ttm);
 	}
 
 	if (likely(ttm->pages != NULL)) {
-		if (ttm->page_flags & TTM_PAGE_FLAG_USER)
-			ttm_tt_free_user_pages(ttm);
-		else
-			ttm_tt_free_alloced_pages(ttm);
-
-		ttm_tt_free_page_directory(ttm);
+		ttm->bdev->driver->ttm_tt_unpopulate(ttm);
 	}
 
 	if (!(ttm->page_flags & TTM_PAGE_FLAG_PERSISTENT_SWAP) &&
 	    ttm->swap_storage)
 		fput(ttm->swap_storage);
 
-	kfree(ttm);
+	ttm->swap_storage = NULL;
+	ttm->func->destroy(ttm);
 }
 
-int ttm_tt_set_user(struct ttm_tt *ttm,
-		    struct task_struct *tsk,
-		    unsigned long start, unsigned long num_pages)
+int ttm_tt_init(struct ttm_tt *ttm, struct ttm_bo_device *bdev,
+		unsigned long size, uint32_t page_flags,
+		struct page *dummy_read_page)
 {
-	struct mm_struct *mm = tsk->mm;
-	int ret;
-	int write = (ttm->page_flags & TTM_PAGE_FLAG_WRITE) != 0;
-	struct ttm_mem_global *mem_glob = ttm->glob->mem_glob;
-
-	BUG_ON(num_pages != ttm->num_pages);
-	BUG_ON((ttm->page_flags & TTM_PAGE_FLAG_USER) == 0);
-
-	/**
-	 * Account user pages as lowmem pages for now.
-	 */
-
-	ret = ttm_mem_global_alloc(mem_glob, num_pages * PAGE_SIZE,
-				   false, false);
-	if (unlikely(ret != 0))
-		return ret;
-
-	down_read(&mm->mmap_sem);
-	ret = get_user_pages(tsk, mm, start, num_pages,
-			     write, 0, ttm->pages, NULL);
-	up_read(&mm->mmap_sem);
+	ttm->bdev = bdev;
+	ttm->glob = bdev->glob;
+	ttm->num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
+	ttm->caching_state = tt_cached;
+	ttm->page_flags = page_flags;
+	ttm->dummy_read_page = dummy_read_page;
+	ttm->state = tt_unpopulated;
+	ttm->swap_storage = NULL;
 
-	if (ret != num_pages && write) {
-		ttm_tt_free_user_pages(ttm);
-		ttm_mem_global_free(mem_glob, num_pages * PAGE_SIZE);
+	ttm_tt_alloc_page_directory(ttm);
+	if (!ttm->pages) {
+		ttm_tt_destroy(ttm);
+		printk(KERN_ERR TTM_PFX "Failed allocating page table\n");
 		return -ENOMEM;
 	}
-
-	ttm->tsk = tsk;
-	ttm->start = start;
-	ttm->state = tt_unbound;
-
 	return 0;
 }
+EXPORT_SYMBOL(ttm_tt_init);
 
-struct ttm_tt *ttm_tt_create(struct ttm_bo_device *bdev, unsigned long size,
-			     uint32_t page_flags, struct page *dummy_read_page)
+void ttm_tt_fini(struct ttm_tt *ttm)
 {
-	struct ttm_bo_driver *bo_driver = bdev->driver;
-	struct ttm_tt *ttm;
-
-	if (!bo_driver)
-		return NULL;
+	drm_free_large(ttm->pages);
+	ttm->pages = NULL;
+}
+EXPORT_SYMBOL(ttm_tt_fini);
 
-	ttm = kzalloc(sizeof(*ttm), GFP_KERNEL);
-	if (!ttm)
-		return NULL;
+int ttm_dma_tt_init(struct ttm_dma_tt *ttm_dma, struct ttm_bo_device *bdev,
+		unsigned long size, uint32_t page_flags,
+		struct page *dummy_read_page)
+{
+	struct ttm_tt *ttm = &ttm_dma->ttm;
 
+	ttm->bdev = bdev;
 	ttm->glob = bdev->glob;
 	ttm->num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
-	ttm->first_himem_page = ttm->num_pages;
-	ttm->last_lomem_page = -1;
 	ttm->caching_state = tt_cached;
 	ttm->page_flags = page_flags;
-
 	ttm->dummy_read_page = dummy_read_page;
+	ttm->state = tt_unpopulated;
+	ttm->swap_storage = NULL;
 
-	ttm_tt_alloc_page_directory(ttm);
-	if (!ttm->pages) {
+	INIT_LIST_HEAD(&ttm_dma->pages_list);
+	ttm_dma_tt_alloc_page_directory(ttm_dma);
+	if (!ttm->pages || !ttm_dma->dma_address) {
 		ttm_tt_destroy(ttm);
 		printk(KERN_ERR TTM_PFX "Failed allocating page table\n");
-		return NULL;
-	}
-	ttm->be = bo_driver->create_ttm_backend_entry(bdev);
-	if (!ttm->be) {
-		ttm_tt_destroy(ttm);
-		printk(KERN_ERR TTM_PFX "Failed creating ttm backend entry\n");
-		return NULL;
+		return -ENOMEM;
 	}
-	ttm->state = tt_unpopulated;
-	return ttm;
+	return 0;
 }
+EXPORT_SYMBOL(ttm_dma_tt_init);
+
+void ttm_dma_tt_fini(struct ttm_dma_tt *ttm_dma)
+{
+	struct ttm_tt *ttm = &ttm_dma->ttm;
+
+	drm_free_large(ttm->pages);
+	ttm->pages = NULL;
+	drm_free_large(ttm_dma->dma_address);
+	ttm_dma->dma_address = NULL;
+}
+EXPORT_SYMBOL(ttm_dma_tt_fini);
 
 void ttm_tt_unbind(struct ttm_tt *ttm)
 {
 	int ret;
-	struct ttm_backend *be = ttm->be;
 
 	if (ttm->state == tt_bound) {
-		ret = be->func->unbind(be);
+		ret = ttm->func->unbind(ttm);
 		BUG_ON(ret);
 		ttm->state = tt_unbound;
 	}
@@ -433,7 +261,6 @@ void ttm_tt_unbind(struct ttm_tt *ttm)
 int ttm_tt_bind(struct ttm_tt *ttm, struct ttm_mem_reg *bo_mem)
 {
 	int ret = 0;
-	struct ttm_backend *be;
 
 	if (!ttm)
 		return -EINVAL;
@@ -441,25 +268,21 @@ int ttm_tt_bind(struct ttm_tt *ttm, struct ttm_mem_reg *bo_mem)
 	if (ttm->state == tt_bound)
 		return 0;
 
-	be = ttm->be;
-
-	ret = ttm_tt_populate(ttm);
+	ret = ttm->bdev->driver->ttm_tt_populate(ttm);
 	if (ret)
 		return ret;
 
-	ret = be->func->bind(be, bo_mem);
+	ret = ttm->func->bind(ttm, bo_mem);
 	if (unlikely(ret != 0))
 		return ret;
 
 	ttm->state = tt_bound;
 
-	if (ttm->page_flags & TTM_PAGE_FLAG_USER)
-		ttm->page_flags |= TTM_PAGE_FLAG_USER_DIRTY;
 	return 0;
 }
 EXPORT_SYMBOL(ttm_tt_bind);
 
-static int ttm_tt_swapin(struct ttm_tt *ttm)
+int ttm_tt_swapin(struct ttm_tt *ttm)
 {
 	struct address_space *swap_space;
 	struct file *swap_storage;
@@ -470,16 +293,6 @@ static int ttm_tt_swapin(struct ttm_tt *ttm)
 	int i;
 	int ret = -ENOMEM;
 
-	if (ttm->page_flags & TTM_PAGE_FLAG_USER) {
-		ret = ttm_tt_set_user(ttm, ttm->tsk, ttm->start,
-				      ttm->num_pages);
-		if (unlikely(ret != 0))
-			return ret;
-
-		ttm->page_flags &= ~TTM_PAGE_FLAG_SWAPPED;
-		return 0;
-	}
-
 	swap_storage = ttm->swap_storage;
 	BUG_ON(swap_storage == NULL);
 
@@ -491,7 +304,7 @@ static int ttm_tt_swapin(struct ttm_tt *ttm)
 			ret = PTR_ERR(from_page);
 			goto out_err;
 		}
-		to_page = __ttm_tt_get_page(ttm, i);
+		to_page = ttm->pages[i];
 		if (unlikely(to_page == NULL))
 			goto out_err;
 
@@ -512,7 +325,6 @@ static int ttm_tt_swapin(struct ttm_tt *ttm)
 
 	return 0;
 out_err:
-	ttm_tt_free_alloced_pages(ttm);
 	return ret;
 }
 
@@ -530,18 +342,6 @@ int ttm_tt_swapout(struct ttm_tt *ttm, struct file *persistent_swap_storage)
 	BUG_ON(ttm->state != tt_unbound && ttm->state != tt_unpopulated);
 	BUG_ON(ttm->caching_state != tt_cached);
 
-	/*
-	 * For user buffers, just unpin the pages, as there should be
-	 * vma references.
-	 */
-
-	if (ttm->page_flags & TTM_PAGE_FLAG_USER) {
-		ttm_tt_free_user_pages(ttm);
-		ttm->page_flags |= TTM_PAGE_FLAG_SWAPPED;
-		ttm->swap_storage = NULL;
-		return 0;
-	}
-
 	if (!persistent_swap_storage) {
 		swap_storage = shmem_file_setup("ttm swap",
 						ttm->num_pages << PAGE_SHIFT,
@@ -576,7 +376,7 @@ int ttm_tt_swapout(struct ttm_tt *ttm, struct file *persistent_swap_storage)
 		page_cache_release(to_page);
 	}
 
-	ttm_tt_free_alloced_pages(ttm);
+	ttm->bdev->driver->ttm_tt_unpopulate(ttm);
 	ttm->swap_storage = swap_storage;
 	ttm->page_flags |= TTM_PAGE_FLAG_SWAPPED;
 	if (persistent_swap_storage)
diff --git a/drivers/gpu/drm/via/via_drv.c b/drivers/gpu/drm/via/via_drv.c
index a83e86d3956c..02661f35f7a0 100644
--- a/drivers/gpu/drm/via/via_drv.c
+++ b/drivers/gpu/drm/via/via_drv.c
@@ -30,16 +30,52 @@
 
 #include "drm_pciids.h"
 
+static int via_driver_open(struct drm_device *dev, struct drm_file *file)
+{
+	struct via_file_private *file_priv;
+
+	DRM_DEBUG_DRIVER("\n");
+	file_priv = kmalloc(sizeof(*file_priv), GFP_KERNEL);
+	if (!file_priv)
+		return -ENOMEM;
+
+	file->driver_priv = file_priv;
+
+	INIT_LIST_HEAD(&file_priv->obj_list);
+
+	return 0;
+}
+
+void via_driver_postclose(struct drm_device *dev, struct drm_file *file)
+{
+	struct via_file_private *file_priv = file->driver_priv;
+
+	kfree(file_priv);
+}
+
 static struct pci_device_id pciidlist[] = {
 	viadrv_PCI_IDS
 };
 
+static const struct file_operations via_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	.driver_features =
 	    DRIVER_USE_AGP | DRIVER_USE_MTRR | DRIVER_HAVE_IRQ |
 	    DRIVER_IRQ_SHARED,
 	.load = via_driver_load,
 	.unload = via_driver_unload,
+	.open = via_driver_open,
+	.postclose = via_driver_postclose,
 	.context_dtor = via_final_context,
 	.get_vblank_counter = via_get_vblank_counter,
 	.enable_vblank = via_enable_vblank,
@@ -54,17 +90,7 @@ static struct drm_driver driver = {
 	.reclaim_buffers_idlelocked = via_reclaim_buffers_locked,
 	.lastclose = via_lastclose,
 	.ioctls = via_ioctls,
-	.fops = {
-		.owner = THIS_MODULE,
-		.open = drm_open,
-		.release = drm_release,
-		.unlocked_ioctl = drm_ioctl,
-		.mmap = drm_mmap,
-		.poll = drm_poll,
-		.fasync = drm_fasync,
-		.llseek = noop_llseek,
-		},
-
+	.fops = &via_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = DRIVER_DATE,
diff --git a/drivers/gpu/drm/via/via_drv.h b/drivers/gpu/drm/via/via_drv.h
index 9cf87d912325..88edacc93006 100644
--- a/drivers/gpu/drm/via/via_drv.h
+++ b/drivers/gpu/drm/via/via_drv.h
@@ -24,7 +24,7 @@
 #ifndef _VIA_DRV_H_
 #define _VIA_DRV_H_
 
-#include "drm_sman.h"
+#include "drm_mm.h"
 #define DRIVER_AUTHOR	"Various"
 
 #define DRIVER_NAME		"via"
@@ -88,9 +88,12 @@ typedef struct drm_via_private {
 	uint32_t irq_pending_mask;
 	int *irq_map;
 	unsigned int idle_fault;
-	struct drm_sman sman;
 	int vram_initialized;
+	struct drm_mm vram_mm;
 	int agp_initialized;
+	struct drm_mm agp_mm;
+	/** Mapping of userspace keys to mm objects */
+	struct idr object_idr;
 	unsigned long vram_offset;
 	unsigned long agp_offset;
 	drm_via_blitq_t blit_queues[VIA_NUM_BLIT_ENGINES];
diff --git a/drivers/gpu/drm/via/via_map.c b/drivers/gpu/drm/via/via_map.c
index 6cca9a709f7a..a2ab34365151 100644
--- a/drivers/gpu/drm/via/via_map.c
+++ b/drivers/gpu/drm/via/via_map.c
@@ -104,15 +104,10 @@ int via_driver_load(struct drm_device *dev, unsigned long chipset)
 
 	dev_priv->chipset = chipset;
 
-	ret = drm_sman_init(&dev_priv->sman, 2, 12, 8);
-	if (ret) {
-		kfree(dev_priv);
-		return ret;
-	}
+	idr_init(&dev->object_name_idr);
 
 	ret = drm_vblank_init(dev, 1);
 	if (ret) {
-		drm_sman_takedown(&dev_priv->sman);
 		kfree(dev_priv);
 		return ret;
 	}
@@ -124,7 +119,8 @@ int via_driver_unload(struct drm_device *dev)
 {
 	drm_via_private_t *dev_priv = dev->dev_private;
 
-	drm_sman_takedown(&dev_priv->sman);
+	idr_remove_all(&dev_priv->object_idr);
+	idr_destroy(&dev_priv->object_idr);
 
 	kfree(dev_priv);
 
diff --git a/drivers/gpu/drm/via/via_mm.c b/drivers/gpu/drm/via/via_mm.c
index 6cc2dadae3ef..a3574d09a07d 100644
--- a/drivers/gpu/drm/via/via_mm.c
+++ b/drivers/gpu/drm/via/via_mm.c
@@ -28,26 +28,22 @@
 #include "drmP.h"
 #include "via_drm.h"
 #include "via_drv.h"
-#include "drm_sman.h"
 
 #define VIA_MM_ALIGN_SHIFT 4
 #define VIA_MM_ALIGN_MASK ((1 << VIA_MM_ALIGN_SHIFT) - 1)
 
+struct via_memblock {
+	struct drm_mm_node mm_node;
+	struct list_head owner_list;
+};
+
 int via_agp_init(struct drm_device *dev, void *data, struct drm_file *file_priv)
 {
 	drm_via_agp_t *agp = data;
 	drm_via_private_t *dev_priv = (drm_via_private_t *) dev->dev_private;
-	int ret;
 
 	mutex_lock(&dev->struct_mutex);
-	ret = drm_sman_set_range(&dev_priv->sman, VIA_MEM_AGP, 0,
-				 agp->size >> VIA_MM_ALIGN_SHIFT);
-
-	if (ret) {
-		DRM_ERROR("AGP memory manager initialisation error\n");
-		mutex_unlock(&dev->struct_mutex);
-		return ret;
-	}
+	drm_mm_init(&dev_priv->agp_mm, 0, agp->size >> VIA_MM_ALIGN_SHIFT);
 
 	dev_priv->agp_initialized = 1;
 	dev_priv->agp_offset = agp->offset;
@@ -61,17 +57,9 @@ int via_fb_init(struct drm_device *dev, void *data, struct drm_file *file_priv)
 {
 	drm_via_fb_t *fb = data;
 	drm_via_private_t *dev_priv = (drm_via_private_t *) dev->dev_private;
-	int ret;
 
 	mutex_lock(&dev->struct_mutex);
-	ret = drm_sman_set_range(&dev_priv->sman, VIA_MEM_VIDEO, 0,
-				 fb->size >> VIA_MM_ALIGN_SHIFT);
-
-	if (ret) {
-		DRM_ERROR("VRAM memory manager initialisation error\n");
-		mutex_unlock(&dev->struct_mutex);
-		return ret;
-	}
+	drm_mm_init(&dev_priv->vram_mm, 0, fb->size >> VIA_MM_ALIGN_SHIFT);
 
 	dev_priv->vram_initialized = 1;
 	dev_priv->vram_offset = fb->offset;
@@ -108,19 +96,25 @@ void via_lastclose(struct drm_device *dev)
 		return;
 
 	mutex_lock(&dev->struct_mutex);
-	drm_sman_cleanup(&dev_priv->sman);
-	dev_priv->vram_initialized = 0;
-	dev_priv->agp_initialized = 0;
+	if (dev_priv->vram_initialized) {
+		drm_mm_takedown(&dev_priv->vram_mm);
+		dev_priv->vram_initialized = 0;
+	}
+	if (dev_priv->agp_initialized) {
+		drm_mm_takedown(&dev_priv->agp_mm);
+		dev_priv->agp_initialized = 0;
+	}
 	mutex_unlock(&dev->struct_mutex);
 }
 
 int via_mem_alloc(struct drm_device *dev, void *data,
-		  struct drm_file *file_priv)
+		  struct drm_file *file)
 {
 	drm_via_mem_t *mem = data;
-	int retval = 0;
-	struct drm_memblock_item *item;
+	int retval = 0, user_key;
+	struct via_memblock *item;
 	drm_via_private_t *dev_priv = (drm_via_private_t *) dev->dev_private;
+	struct via_file_private *file_priv = file->driver_priv;
 	unsigned long tmpSize;
 
 	if (mem->type > VIA_MEM_AGP) {
@@ -136,24 +130,57 @@ int via_mem_alloc(struct drm_device *dev, void *data,
 		return -EINVAL;
 	}
 
+	item = kzalloc(sizeof(*item), GFP_KERNEL);
+	if (!item) {
+		retval = -ENOMEM;
+		goto fail_alloc;
+	}
+
 	tmpSize = (mem->size + VIA_MM_ALIGN_MASK) >> VIA_MM_ALIGN_SHIFT;
-	item = drm_sman_alloc(&dev_priv->sman, mem->type, tmpSize, 0,
-			      (unsigned long)file_priv);
-	mutex_unlock(&dev->struct_mutex);
-	if (item) {
-		mem->offset = ((mem->type == VIA_MEM_VIDEO) ?
-			      dev_priv->vram_offset : dev_priv->agp_offset) +
-		    (item->mm->
-		     offset(item->mm, item->mm_info) << VIA_MM_ALIGN_SHIFT);
-		mem->index = item->user_hash.key;
-	} else {
-		mem->offset = 0;
-		mem->size = 0;
-		mem->index = 0;
-		DRM_DEBUG("Video memory allocation failed\n");
+	if (mem->type == VIA_MEM_AGP)
+		retval = drm_mm_insert_node(&dev_priv->agp_mm,
+					    &item->mm_node,
+					    tmpSize, 0);
+	else
+		retval = drm_mm_insert_node(&dev_priv->vram_mm,
+					    &item->mm_node,
+					    tmpSize, 0);
+	if (retval)
+		goto fail_alloc;
+
+again:
+	if (idr_pre_get(&dev_priv->object_idr, GFP_KERNEL) == 0) {
 		retval = -ENOMEM;
+		goto fail_idr;
 	}
 
+	retval = idr_get_new_above(&dev_priv->object_idr, item, 1, &user_key);
+	if (retval == -EAGAIN)
+		goto again;
+	if (retval)
+		goto fail_idr;
+
+	list_add(&item->owner_list, &file_priv->obj_list);
+	mutex_unlock(&dev->struct_mutex);
+
+	mem->offset = ((mem->type == VIA_MEM_VIDEO) ?
+		      dev_priv->vram_offset : dev_priv->agp_offset) +
+	    ((item->mm_node.start) << VIA_MM_ALIGN_SHIFT);
+	mem->index = user_key;
+
+	return 0;
+
+fail_idr:
+	drm_mm_remove_node(&item->mm_node);
+fail_alloc:
+	kfree(item);
+	mutex_unlock(&dev->struct_mutex);
+
+	mem->offset = 0;
+	mem->size = 0;
+	mem->index = 0;
+	DRM_DEBUG("Video memory allocation failed\n");
+
 	return retval;
 }
 
@@ -161,24 +188,35 @@ int via_mem_free(struct drm_device *dev, void *data, struct drm_file *file_priv)
 {
 	drm_via_private_t *dev_priv = dev->dev_private;
 	drm_via_mem_t *mem = data;
-	int ret;
+	struct via_memblock *obj;
 
 	mutex_lock(&dev->struct_mutex);
-	ret = drm_sman_free_key(&dev_priv->sman, mem->index);
+	obj = idr_find(&dev_priv->object_idr, mem->index);
+	if (obj == NULL) {
+		mutex_unlock(&dev->struct_mutex);
+		return -EINVAL;
+	}
+
+	idr_remove(&dev_priv->object_idr, mem->index);
+	list_del(&obj->owner_list);
+	drm_mm_remove_node(&obj->mm_node);
+	kfree(obj);
 	mutex_unlock(&dev->struct_mutex);
+
 	DRM_DEBUG("free = 0x%lx\n", mem->index);
 
-	return ret;
+	return 0;
 }
 
 
 void via_reclaim_buffers_locked(struct drm_device *dev,
-				struct drm_file *file_priv)
+				struct drm_file *file)
 {
-	drm_via_private_t *dev_priv = dev->dev_private;
+	struct via_file_private *file_priv = file->driver_priv;
+	struct via_memblock *entry, *next;
 
 	mutex_lock(&dev->struct_mutex);
-	if (drm_sman_owner_clean(&dev_priv->sman, (unsigned long)file_priv)) {
+	if (list_empty(&file_priv->obj_list)) {
 		mutex_unlock(&dev->struct_mutex);
 		return;
 	}
@@ -186,7 +224,12 @@ void via_reclaim_buffers_locked(struct drm_device *dev,
 	if (dev->driver->dma_quiescent)
 		dev->driver->dma_quiescent(dev);
 
-	drm_sman_owner_cleanup(&dev_priv->sman, (unsigned long)file_priv);
+	list_for_each_entry_safe(entry, next, &file_priv->obj_list,
+				 owner_list) {
+		list_del(&entry->owner_list);
+		drm_mm_remove_node(&entry->mm_node);
+		kfree(entry);
+	}
 	mutex_unlock(&dev->struct_mutex);
 	return;
 }
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c b/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
index 5a72ed908232..1e2c0fb7f786 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
@@ -28,6 +28,7 @@
 #include "vmwgfx_drv.h"
 #include "ttm/ttm_bo_driver.h"
 #include "ttm/ttm_placement.h"
+#include "ttm/ttm_page_alloc.h"
 
 static uint32_t vram_placement_flags = TTM_PL_FLAG_VRAM |
 	TTM_PL_FLAG_CACHED;
@@ -139,85 +140,63 @@ struct ttm_placement vmw_srf_placement = {
 	.busy_placement = gmr_vram_placement_flags
 };
 
-struct vmw_ttm_backend {
-	struct ttm_backend backend;
-	struct page **pages;
-	unsigned long num_pages;
+struct vmw_ttm_tt {
+	struct ttm_tt ttm;
 	struct vmw_private *dev_priv;
 	int gmr_id;
 };
 
-static int vmw_ttm_populate(struct ttm_backend *backend,
-			    unsigned long num_pages, struct page **pages,
-			    struct page *dummy_read_page,
-			    dma_addr_t *dma_addrs)
+static int vmw_ttm_bind(struct ttm_tt *ttm, struct ttm_mem_reg *bo_mem)
 {
-	struct vmw_ttm_backend *vmw_be =
-	    container_of(backend, struct vmw_ttm_backend, backend);
-
-	vmw_be->pages = pages;
-	vmw_be->num_pages = num_pages;
-
-	return 0;
-}
-
-static int vmw_ttm_bind(struct ttm_backend *backend, struct ttm_mem_reg *bo_mem)
-{
-	struct vmw_ttm_backend *vmw_be =
-	    container_of(backend, struct vmw_ttm_backend, backend);
+	struct vmw_ttm_tt *vmw_be = container_of(ttm, struct vmw_ttm_tt, ttm);
 
 	vmw_be->gmr_id = bo_mem->start;
 
-	return vmw_gmr_bind(vmw_be->dev_priv, vmw_be->pages,
-			    vmw_be->num_pages, vmw_be->gmr_id);
+	return vmw_gmr_bind(vmw_be->dev_priv, ttm->pages,
+			    ttm->num_pages, vmw_be->gmr_id);
 }
 
-static int vmw_ttm_unbind(struct ttm_backend *backend)
+static int vmw_ttm_unbind(struct ttm_tt *ttm)
 {
-	struct vmw_ttm_backend *vmw_be =
-	    container_of(backend, struct vmw_ttm_backend, backend);
+	struct vmw_ttm_tt *vmw_be = container_of(ttm, struct vmw_ttm_tt, ttm);
 
 	vmw_gmr_unbind(vmw_be->dev_priv, vmw_be->gmr_id);
 	return 0;
 }
 
-static void vmw_ttm_clear(struct ttm_backend *backend)
+static void vmw_ttm_destroy(struct ttm_tt *ttm)
 {
-	struct vmw_ttm_backend *vmw_be =
-		container_of(backend, struct vmw_ttm_backend, backend);
-
-	vmw_be->pages = NULL;
-	vmw_be->num_pages = 0;
-}
-
-static void vmw_ttm_destroy(struct ttm_backend *backend)
-{
-	struct vmw_ttm_backend *vmw_be =
-	    container_of(backend, struct vmw_ttm_backend, backend);
+	struct vmw_ttm_tt *vmw_be = container_of(ttm, struct vmw_ttm_tt, ttm);
 
+	ttm_tt_fini(ttm);
 	kfree(vmw_be);
 }
 
 static struct ttm_backend_func vmw_ttm_func = {
-	.populate = vmw_ttm_populate,
-	.clear = vmw_ttm_clear,
 	.bind = vmw_ttm_bind,
 	.unbind = vmw_ttm_unbind,
 	.destroy = vmw_ttm_destroy,
 };
 
-struct ttm_backend *vmw_ttm_backend_init(struct ttm_bo_device *bdev)
+struct ttm_tt *vmw_ttm_tt_create(struct ttm_bo_device *bdev,
+				 unsigned long size, uint32_t page_flags,
+				 struct page *dummy_read_page)
 {
-	struct vmw_ttm_backend *vmw_be;
+	struct vmw_ttm_tt *vmw_be;
 
 	vmw_be = kmalloc(sizeof(*vmw_be), GFP_KERNEL);
 	if (!vmw_be)
 		return NULL;
 
-	vmw_be->backend.func = &vmw_ttm_func;
+	vmw_be->ttm.func = &vmw_ttm_func;
 	vmw_be->dev_priv = container_of(bdev, struct vmw_private, bdev);
 
-	return &vmw_be->backend;
+	if (ttm_tt_init(&vmw_be->ttm, bdev, size, page_flags, dummy_read_page)) {
+		kfree(vmw_be);
+		return NULL;
+	}
+
+	return &vmw_be->ttm;
 }
 
 int vmw_invalidate_caches(struct ttm_bo_device *bdev, uint32_t flags)
@@ -357,7 +336,9 @@ static int vmw_sync_obj_wait(void *sync_obj, void *sync_arg,
 }
 
 struct ttm_bo_driver vmw_bo_driver = {
-	.create_ttm_backend_entry = vmw_ttm_backend_init,
+	.ttm_tt_create = &vmw_ttm_tt_create,
+	.ttm_tt_populate = &ttm_pool_populate,
+	.ttm_tt_unpopulate = &ttm_pool_unpopulate,
 	.invalidate_caches = vmw_invalidate_caches,
 	.init_mem_type = vmw_init_mem_type,
 	.evict_flags = vmw_evict_flags,
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index dff8fc767152..f390f5f9cb68 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -1064,6 +1064,21 @@ static const struct dev_pm_ops vmw_pm_ops = {
 	.resume = vmw_pm_resume,
 };
 
+static const struct file_operations vmwgfx_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = vmw_unlocked_ioctl,
+	.mmap = vmw_mmap,
+	.poll = vmw_fops_poll,
+	.read = vmw_fops_read,
+	.fasync = drm_fasync,
+#if defined(CONFIG_COMPAT)
+	.compat_ioctl = drm_compat_ioctl,
+#endif
+	.llseek = noop_llseek,
+};
+
 static struct drm_driver driver = {
 	.driver_features = DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED |
 	DRIVER_MODESET,
@@ -1088,20 +1103,7 @@ static struct drm_driver driver = {
 	.master_drop = vmw_master_drop,
 	.open = vmw_driver_open,
 	.postclose = vmw_postclose,
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = vmw_unlocked_ioctl,
-		 .mmap = vmw_mmap,
-		 .poll = vmw_fops_poll,
-		 .read = vmw_fops_read,
-		 .fasync = drm_fasync,
-#if defined(CONFIG_COMPAT)
-		 .compat_ioctl = drm_compat_ioctl,
-#endif
-		 .llseek = noop_llseek,
-	},
+	.fops = &vmwgfx_driver_fops,
 	.name = VMWGFX_DRIVER_NAME,
 	.desc = VMWGFX_DRIVER_DESC,
 	.date = VMWGFX_DRIVER_DATE,
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
index f94b33ae2215..0af6ebdf205d 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
@@ -690,7 +690,7 @@ static int vmw_kms_new_framebuffer_surface(struct vmw_private *dev_priv,
 
 	/* XXX get the first 3 from the surface info */
 	vfbs->base.base.bits_per_pixel = mode_cmd->bpp;
-	vfbs->base.base.pitch = mode_cmd->pitch;
+	vfbs->base.base.pitches[0] = mode_cmd->pitch;
 	vfbs->base.base.depth = mode_cmd->depth;
 	vfbs->base.base.width = mode_cmd->width;
 	vfbs->base.base.height = mode_cmd->height;
@@ -804,7 +804,7 @@ static int do_dmabuf_define_gmrfb(struct drm_file *file_priv,
 	cmd->body.format.bitsPerPixel = framebuffer->base.bits_per_pixel;
 	cmd->body.format.colorDepth = depth;
 	cmd->body.format.reserved = 0;
-	cmd->body.bytesPerLine = framebuffer->base.pitch;
+	cmd->body.bytesPerLine = framebuffer->base.pitches[0];
 	cmd->body.ptr.gmrId = framebuffer->user_handle;
 	cmd->body.ptr.offset = 0;
 
@@ -1056,7 +1056,7 @@ static int vmw_kms_new_framebuffer_dmabuf(struct vmw_private *dev_priv,
 	}
 
 	vfbd->base.base.bits_per_pixel = mode_cmd->bpp;
-	vfbd->base.base.pitch = mode_cmd->pitch;
+	vfbd->base.base.pitches[0] = mode_cmd->pitch;
 	vfbd->base.base.depth = mode_cmd->depth;
 	vfbd->base.base.width = mode_cmd->width;
 	vfbd->base.base.height = mode_cmd->height;
@@ -1085,7 +1085,7 @@ out_err1:
 
 static struct drm_framebuffer *vmw_kms_fb_create(struct drm_device *dev,
 						 struct drm_file *file_priv,
-						 struct drm_mode_fb_cmd *mode_cmd)
+						 struct drm_mode_fb_cmd2 *mode_cmd2)
 {
 	struct vmw_private *dev_priv = vmw_priv(dev);
 	struct ttm_object_file *tfile = vmw_fpriv(file_priv)->tfile;
@@ -1093,8 +1093,16 @@ static struct drm_framebuffer *vmw_kms_fb_create(struct drm_device *dev,
 	struct vmw_surface *surface = NULL;
 	struct vmw_dma_buffer *bo = NULL;
 	struct ttm_base_object *user_obj;
+	struct drm_mode_fb_cmd mode_cmd;
 	int ret;
 
+	mode_cmd.width = mode_cmd2->width;
+	mode_cmd.height = mode_cmd2->height;
+	mode_cmd.pitch = mode_cmd2->pitches[0];
+	mode_cmd.handle = mode_cmd2->handles[0];
+	drm_fb_get_bpp_depth(mode_cmd2->pixel_format, &mode_cmd.depth,
+				    &mode_cmd.bpp);
+
 	/**
 	 * This code should be conditioned on Screen Objects not being used.
 	 * If screen objects are used, we can allocate a GMR to hold the
@@ -1102,8 +1110,8 @@ static struct drm_framebuffer *vmw_kms_fb_create(struct drm_device *dev,
 	 */
 
 	if (!vmw_kms_validate_mode_vram(dev_priv,
-					mode_cmd->pitch,
-					mode_cmd->height)) {
+					mode_cmd.pitch,
+					mode_cmd.height)) {
 		DRM_ERROR("VRAM size is too small for requested mode.\n");
 		return ERR_PTR(-ENOMEM);
 	}
@@ -1117,15 +1125,19 @@ static struct drm_framebuffer *vmw_kms_fb_create(struct drm_device *dev,
 	 * command stream using user-space handles.
 	 */
 
-	user_obj = ttm_base_object_lookup(tfile, mode_cmd->handle);
+	user_obj = ttm_base_object_lookup(tfile, mode_cmd.handle);
 	if (unlikely(user_obj == NULL)) {
 		DRM_ERROR("Could not locate requested kms frame buffer.\n");
 		return ERR_PTR(-ENOENT);
 	}
 
+	/**
+	 * End conditioned code.
+	 */
+
 	/* returns either a dmabuf or surface */
 	ret = vmw_user_lookup_handle(dev_priv, tfile,
-				     mode_cmd->handle,
+				     mode_cmd.handle,
 				     &surface, &bo);
 	if (ret)
 		goto err_out;
@@ -1133,10 +1145,10 @@ static struct drm_framebuffer *vmw_kms_fb_create(struct drm_device *dev,
 	/* Create the new framebuffer depending one what we got back */
 	if (bo)
 		ret = vmw_kms_new_framebuffer_dmabuf(dev_priv, bo, &vfb,
-						     mode_cmd);
+						     &mode_cmd);
 	else if (surface)
 		ret = vmw_kms_new_framebuffer_surface(dev_priv, file_priv,
-						      surface, &vfb, mode_cmd);
+						      surface, &vfb, &mode_cmd);
 	else
 		BUG();
 
@@ -1344,7 +1356,7 @@ int vmw_kms_readback(struct vmw_private *dev_priv,
 	cmd->body.format.bitsPerPixel = vfb->base.bits_per_pixel;
 	cmd->body.format.colorDepth = vfb->base.depth;
 	cmd->body.format.reserved = 0;
-	cmd->body.bytesPerLine = vfb->base.pitch;
+	cmd->body.bytesPerLine = vfb->base.pitches[0];
 	cmd->body.ptr.gmrId = vfb->user_handle;
 	cmd->body.ptr.offset = 0;
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.h b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.h
index e1cb8556355f..a4f7f034996a 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.h
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.h
@@ -29,6 +29,7 @@
 #define VMWGFX_KMS_H_
 
 #include "drmP.h"
+#include "drm_crtc_helper.h"
 #include "vmwgfx_drv.h"
 
 #define VMWGFX_NUM_DISPLAY_UNITS 8
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
index 8f8dbd43c33d..f77b184be807 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
@@ -95,7 +95,7 @@ static int vmw_ldu_commit_list(struct vmw_private *dev_priv)
 			return 0;
 		fb = entry->base.crtc.fb;
 
-		return vmw_kms_write_svga(dev_priv, w, h, fb->pitch,
+		return vmw_kms_write_svga(dev_priv, w, h, fb->pitches[0],
 					  fb->bits_per_pixel, fb->depth);
 	}
 
@@ -103,7 +103,7 @@ static int vmw_ldu_commit_list(struct vmw_private *dev_priv)
 		entry = list_entry(lds->active.next, typeof(*entry), active);
 		fb = entry->base.crtc.fb;
 
-		vmw_kms_write_svga(dev_priv, fb->width, fb->height, fb->pitch,
+		vmw_kms_write_svga(dev_priv, fb->width, fb->height, fb->pitches[0],
 				   fb->bits_per_pixel, fb->depth);
 	}
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
index 1c7f09e26819..a37abb581cbb 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
@@ -1540,29 +1540,10 @@ out_bad_surface:
 /**
  * Buffer management.
  */
-
-static size_t vmw_dmabuf_acc_size(struct ttm_bo_global *glob,
-				  unsigned long num_pages)
-{
-	static size_t bo_user_size = ~0;
-
-	size_t page_array_size =
-	    (num_pages * sizeof(void *) + PAGE_SIZE - 1) & PAGE_MASK;
-
-	if (unlikely(bo_user_size == ~0)) {
-		bo_user_size = glob->ttm_bo_extra_size +
-		    ttm_round_pot(sizeof(struct vmw_dma_buffer));
-	}
-
-	return bo_user_size + page_array_size;
-}
-
 void vmw_dmabuf_bo_free(struct ttm_buffer_object *bo)
 {
 	struct vmw_dma_buffer *vmw_bo = vmw_dma_buffer(bo);
-	struct ttm_bo_global *glob = bo->glob;
 
-	ttm_mem_global_free(glob->mem_glob, bo->acc_size);
 	kfree(vmw_bo);
 }
 
@@ -1573,24 +1554,12 @@ int vmw_dmabuf_init(struct vmw_private *dev_priv,
 		    void (*bo_free) (struct ttm_buffer_object *bo))
 {
 	struct ttm_bo_device *bdev = &dev_priv->bdev;
-	struct ttm_mem_global *mem_glob = bdev->glob->mem_glob;
 	size_t acc_size;
 	int ret;
 
 	BUG_ON(!bo_free);
 
-	acc_size =
-	    vmw_dmabuf_acc_size(bdev->glob,
-				(size + PAGE_SIZE - 1) >> PAGE_SHIFT);
-
-	ret = ttm_mem_global_alloc(mem_glob, acc_size, false, false);
-	if (unlikely(ret != 0)) {
-		/* we must free the bo here as
-		 * ttm_buffer_object_init does so as well */
-		bo_free(&vmw_bo->base);
-		return ret;
-	}
-
+	acc_size = ttm_bo_acc_size(bdev, size, sizeof(struct vmw_dma_buffer));
 	memset(vmw_bo, 0, sizeof(*vmw_bo));
 
 	INIT_LIST_HEAD(&vmw_bo->validate_list);
@@ -1605,9 +1574,7 @@ int vmw_dmabuf_init(struct vmw_private *dev_priv,
 static void vmw_user_dmabuf_destroy(struct ttm_buffer_object *bo)
 {
 	struct vmw_user_dma_buffer *vmw_user_bo = vmw_user_dma_buffer(bo);
-	struct ttm_bo_global *glob = bo->glob;
 
-	ttm_mem_global_free(glob->mem_glob, bo->acc_size);
 	kfree(vmw_user_bo);
 }
 
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 22a4a051f221..a421abdd1ab7 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -31,6 +31,11 @@ config HID
 
 	  If unsure, say Y.
 
+config HID_BATTERY_STRENGTH
+	bool
+	depends on HID && POWER_SUPPLY && HID = POWER_SUPPLY
+	default y
+
 config HIDRAW
 	bool "/dev/hidraw raw HID device support"
 	depends on HID
@@ -335,6 +340,7 @@ config HID_MULTITOUCH
 	  Say Y here if you have one of the following devices:
 	  - 3M PCT touch screens
 	  - ActionStar dual touch panels
+	  - Atmel panels
 	  - Cando dual touch panels
 	  - Chunghwa panels
 	  - CVTouch panels
@@ -349,12 +355,15 @@ config HID_MULTITOUCH
 	  - Lumio CrystalTouch panels
 	  - MosArt dual-touch panels
 	  - PenMount dual touch panels
+	  - PixArt optical touch screen
 	  - Pixcir dual touch panels
+	  - Quanta panels
 	  - eGalax dual-touch panels, including the Joojoo and Wetab tablets
 	  - Stantum multitouch panels
 	  - Touch International Panels
 	  - Unitec Panels
 	  - XAT optical touch panels
+	  - Xiroku optical touch panels
 
 	  If unsure, say N.
 
@@ -466,12 +475,6 @@ config HID_PRIMAX
 	Support for Primax devices that are not fully compliant with the
 	HID standard.
 
-config HID_QUANTA
-	tristate "Quanta Optical Touch panels"
-	depends on USB_HID
-	---help---
-	Support for Quanta Optical Touch dual-touch panels.
-
 config HID_ROCCAT
 	tristate "Roccat special event support"
 	depends on USB_HID
@@ -492,6 +495,13 @@ config HID_ROCCAT_ARVO
 	---help---
 	Support for Roccat Arvo keyboard.
 
+config HID_ROCCAT_ISKU
+	tristate "Roccat Isku keyboard support"
+	depends on USB_HID
+	depends on HID_ROCCAT
+	---help---
+	Support for Roccat Isku keyboard.
+
 config HID_ROCCAT_KONE
 	tristate "Roccat Kone Mouse support"
 	depends on USB_HID
@@ -560,6 +570,12 @@ config GREENASIA_FF
 	(like MANTA Warrior MM816 and SpeedLink Strike2 SL-6635) or adapter
 	and want to enable force feedback support for it.
 
+config HID_HYPERV_MOUSE
+	tristate "Microsoft Hyper-V mouse driver"
+	depends on HYPERV
+	---help---
+	Select this option to enable the Hyper-V mouse driver.
+
 config HID_SMARTJOYPLUS
 	tristate "SmartJoy PLUS PS2/USB adapter support"
 	depends on USB_HID
@@ -620,9 +636,19 @@ config HID_WIIMOTE
 	depends on BT_HIDP
 	depends on LEDS_CLASS
 	select POWER_SUPPLY
+	select INPUT_FF_MEMLESS
 	---help---
 	Support for the Nintendo Wii Remote bluetooth device.
 
+config HID_WIIMOTE_EXT
+	bool "Nintendo Wii Remote Extension support"
+	depends on HID_WIIMOTE
+	default HID_WIIMOTE
+	---help---
+	Support for extension controllers of the Nintendo Wii Remote. Say yes
+	here if you want to use the Nintendo Motion+, Nunchuck or Classic
+	extension controllers with your Wii Remote.
+
 config HID_ZEROPLUS
 	tristate "Zeroplus based game controller support"
 	depends on USB_HID
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index 1e0d2a638b28..8aefdc963cce 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -25,6 +25,14 @@ ifdef CONFIG_LOGIWHEELS_FF
 	hid-logitech-y	+= hid-lg4ff.o
 endif
 
+hid-wiimote-y		:= hid-wiimote-core.o
+ifdef CONFIG_HID_WIIMOTE_EXT
+	hid-wiimote-y	+= hid-wiimote-ext.o
+endif
+ifdef CONFIG_DEBUG_FS
+	hid-wiimote-y	+= hid-wiimote-debug.o
+endif
+
 obj-$(CONFIG_HID_A4TECH)	+= hid-a4tech.o
 obj-$(CONFIG_HID_ACRUX)		+= hid-axff.o
 obj-$(CONFIG_HID_APPLE)		+= hid-apple.o
@@ -38,6 +46,7 @@ obj-$(CONFIG_HID_ELECOM)	+= hid-elecom.o
 obj-$(CONFIG_HID_EZKEY)		+= hid-ezkey.o
 obj-$(CONFIG_HID_GYRATION)	+= hid-gyration.o
 obj-$(CONFIG_HID_HOLTEK)	+= hid-holtekff.o
+obj-$(CONFIG_HID_HYPERV_MOUSE)	+= hid-hyperv.o
 obj-$(CONFIG_HID_KENSINGTON)	+= hid-kensington.o
 obj-$(CONFIG_HID_KEYTOUCH)	+= hid-keytouch.o
 obj-$(CONFIG_HID_KYE)		+= hid-kye.o
@@ -51,7 +60,6 @@ obj-$(CONFIG_HID_MULTITOUCH)	+= hid-multitouch.o
 obj-$(CONFIG_HID_NTRIG)		+= hid-ntrig.o
 obj-$(CONFIG_HID_ORTEK)		+= hid-ortek.o
 obj-$(CONFIG_HID_PRODIKEYS)	+= hid-prodikeys.o
-obj-$(CONFIG_HID_QUANTA)	+= hid-quanta.o
 obj-$(CONFIG_HID_PANTHERLORD)	+= hid-pl.o
 obj-$(CONFIG_HID_PETALYNX)	+= hid-petalynx.o
 obj-$(CONFIG_HID_PICOLCD)	+= hid-picolcd.o
@@ -59,6 +67,7 @@ obj-$(CONFIG_HID_PRIMAX)	+= hid-primax.o
 obj-$(CONFIG_HID_ROCCAT)	+= hid-roccat.o
 obj-$(CONFIG_HID_ROCCAT_COMMON)	+= hid-roccat-common.o
 obj-$(CONFIG_HID_ROCCAT_ARVO)	+= hid-roccat-arvo.o
+obj-$(CONFIG_HID_ROCCAT_ISKU)	+= hid-roccat-isku.o
 obj-$(CONFIG_HID_ROCCAT_KONE)	+= hid-roccat-kone.o
 obj-$(CONFIG_HID_ROCCAT_KONEPLUS)	+= hid-roccat-koneplus.o
 obj-$(CONFIG_HID_ROCCAT_KOVAPLUS)	+= hid-roccat-kovaplus.o
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index af353842f75f..af08ce7207d9 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -90,7 +90,7 @@ static struct hid_field *hid_register_field(struct hid_report *report, unsigned
 	struct hid_field *field;
 
 	if (report->maxfield == HID_MAX_FIELDS) {
-		dbg_hid("too many fields in report\n");
+		hid_err(report->device, "too many fields in report\n");
 		return NULL;
 	}
 
@@ -121,7 +121,7 @@ static int open_collection(struct hid_parser *parser, unsigned type)
 	usage = parser->local.usage[0];
 
 	if (parser->collection_stack_ptr == HID_COLLECTION_STACK_SIZE) {
-		dbg_hid("collection stack overflow\n");
+		hid_err(parser->device, "collection stack overflow\n");
 		return -1;
 	}
 
@@ -129,7 +129,7 @@ static int open_collection(struct hid_parser *parser, unsigned type)
 		collection = kmalloc(sizeof(struct hid_collection) *
 				parser->device->collection_size * 2, GFP_KERNEL);
 		if (collection == NULL) {
-			dbg_hid("failed to reallocate collection array\n");
+			hid_err(parser->device, "failed to reallocate collection array\n");
 			return -1;
 		}
 		memcpy(collection, parser->device->collection,
@@ -165,7 +165,7 @@ static int open_collection(struct hid_parser *parser, unsigned type)
 static int close_collection(struct hid_parser *parser)
 {
 	if (!parser->collection_stack_ptr) {
-		dbg_hid("collection stack underflow\n");
+		hid_err(parser->device, "collection stack underflow\n");
 		return -1;
 	}
 	parser->collection_stack_ptr--;
@@ -197,7 +197,7 @@ static unsigned hid_lookup_collection(struct hid_parser *parser, unsigned type)
 static int hid_add_usage(struct hid_parser *parser, unsigned usage)
 {
 	if (parser->local.usage_index >= HID_MAX_USAGES) {
-		dbg_hid("usage index exceeded\n");
+		hid_err(parser->device, "usage index exceeded\n");
 		return -1;
 	}
 	parser->local.usage[parser->local.usage_index] = usage;
@@ -222,12 +222,13 @@ static int hid_add_field(struct hid_parser *parser, unsigned report_type, unsign
 
 	report = hid_register_report(parser->device, report_type, parser->global.report_id);
 	if (!report) {
-		dbg_hid("hid_register_report failed\n");
+		hid_err(parser->device, "hid_register_report failed\n");
 		return -1;
 	}
 
 	if (parser->global.logical_maximum < parser->global.logical_minimum) {
-		dbg_hid("logical range invalid %d %d\n", parser->global.logical_minimum, parser->global.logical_maximum);
+		hid_err(parser->device, "logical range invalid %d %d\n",
+				parser->global.logical_minimum, parser->global.logical_maximum);
 		return -1;
 	}
 
@@ -307,7 +308,7 @@ static int hid_parser_global(struct hid_parser *parser, struct hid_item *item)
 	case HID_GLOBAL_ITEM_TAG_PUSH:
 
 		if (parser->global_stack_ptr == HID_GLOBAL_STACK_SIZE) {
-			dbg_hid("global environment stack overflow\n");
+			hid_err(parser->device, "global environment stack overflow\n");
 			return -1;
 		}
 
@@ -318,7 +319,7 @@ static int hid_parser_global(struct hid_parser *parser, struct hid_item *item)
 	case HID_GLOBAL_ITEM_TAG_POP:
 
 		if (!parser->global_stack_ptr) {
-			dbg_hid("global environment stack underflow\n");
+			hid_err(parser->device, "global environment stack underflow\n");
 			return -1;
 		}
 
@@ -362,8 +363,8 @@ static int hid_parser_global(struct hid_parser *parser, struct hid_item *item)
 
 	case HID_GLOBAL_ITEM_TAG_REPORT_SIZE:
 		parser->global.report_size = item_udata(item);
-		if (parser->global.report_size > 32) {
-			dbg_hid("invalid report_size %d\n",
+		if (parser->global.report_size > 96) {
+			hid_err(parser->device, "invalid report_size %d\n",
 					parser->global.report_size);
 			return -1;
 		}
@@ -372,7 +373,7 @@ static int hid_parser_global(struct hid_parser *parser, struct hid_item *item)
 	case HID_GLOBAL_ITEM_TAG_REPORT_COUNT:
 		parser->global.report_count = item_udata(item);
 		if (parser->global.report_count > HID_MAX_USAGES) {
-			dbg_hid("invalid report_count %d\n",
+			hid_err(parser->device, "invalid report_count %d\n",
 					parser->global.report_count);
 			return -1;
 		}
@@ -381,13 +382,13 @@ static int hid_parser_global(struct hid_parser *parser, struct hid_item *item)
 	case HID_GLOBAL_ITEM_TAG_REPORT_ID:
 		parser->global.report_id = item_udata(item);
 		if (parser->global.report_id == 0) {
-			dbg_hid("report_id 0 is invalid\n");
+			hid_err(parser->device, "report_id 0 is invalid\n");
 			return -1;
 		}
 		return 0;
 
 	default:
-		dbg_hid("unknown global tag 0x%x\n", item->tag);
+		hid_err(parser->device, "unknown global tag 0x%x\n", item->tag);
 		return -1;
 	}
 }
@@ -414,14 +415,14 @@ static int hid_parser_local(struct hid_parser *parser, struct hid_item *item)
 			 * items and the first delimiter set.
 			 */
 			if (parser->local.delimiter_depth != 0) {
-				dbg_hid("nested delimiters\n");
+				hid_err(parser->device, "nested delimiters\n");
 				return -1;
 			}
 			parser->local.delimiter_depth++;
 			parser->local.delimiter_branch++;
 		} else {
 			if (parser->local.delimiter_depth < 1) {
-				dbg_hid("bogus close delimiter\n");
+				hid_err(parser->device, "bogus close delimiter\n");
 				return -1;
 			}
 			parser->local.delimiter_depth--;
@@ -506,7 +507,7 @@ static int hid_parser_main(struct hid_parser *parser, struct hid_item *item)
 		ret = hid_add_field(parser, HID_FEATURE_REPORT, data);
 		break;
 	default:
-		dbg_hid("unknown main item tag 0x%x\n", item->tag);
+		hid_err(parser->device, "unknown main item tag 0x%x\n", item->tag);
 		ret = 0;
 	}
 
@@ -678,12 +679,12 @@ int hid_parse_report(struct hid_device *device, __u8 *start,
 	while ((start = fetch_item(start, end, &item)) != NULL) {
 
 		if (item.format != HID_ITEM_FORMAT_SHORT) {
-			dbg_hid("unexpected long global item\n");
+			hid_err(device, "unexpected long global item\n");
 			goto err;
 		}
 
 		if (dispatch_type[item.type](parser, &item)) {
-			dbg_hid("item %u %u %u %u parsing failed\n",
+			hid_err(device, "item %u %u %u %u parsing failed\n",
 				item.format, (unsigned)item.size,
 				(unsigned)item.type, (unsigned)item.tag);
 			goto err;
@@ -691,11 +692,11 @@ int hid_parse_report(struct hid_device *device, __u8 *start,
 
 		if (start == end) {
 			if (parser->collection_stack_ptr) {
-				dbg_hid("unbalanced collection at end of report description\n");
+				hid_err(device, "unbalanced collection at end of report description\n");
 				goto err;
 			}
 			if (parser->local.delimiter_depth) {
-				dbg_hid("unbalanced delimiter at end of report description\n");
+				hid_err(device, "unbalanced delimiter at end of report description\n");
 				goto err;
 			}
 			vfree(parser);
@@ -703,7 +704,7 @@ int hid_parse_report(struct hid_device *device, __u8 *start,
 		}
 	}
 
-	dbg_hid("item fetching failed at offset %d\n", (int)(end - start));
+	hid_err(device, "item fetching failed at offset %d\n", (int)(end - start));
 err:
 	vfree(parser);
 	return ret;
@@ -873,7 +874,7 @@ static void hid_process_event(struct hid_device *hid, struct hid_field *field,
 		ret = hdrv->event(hid, field, usage, value);
 		if (ret != 0) {
 			if (ret < 0)
-				dbg_hid("%s's event failed with %d\n",
+				hid_err(hid, "%s's event failed with %d\n",
 						hdrv->name, ret);
 			return;
 		}
@@ -995,12 +996,13 @@ int hid_set_field(struct hid_field *field, unsigned offset, __s32 value)
 	hid_dump_input(field->report->device, field->usage + offset, value);
 
 	if (offset >= field->report_count) {
-		dbg_hid("offset (%d) exceeds report_count (%d)\n", offset, field->report_count);
+		hid_err(field->report->device, "offset (%d) exceeds report_count (%d)\n",
+				offset, field->report_count);
 		return -1;
 	}
 	if (field->logical_minimum < 0) {
 		if (value != snto32(s32ton(value, size), size)) {
-			dbg_hid("value %d is out of range\n", value);
+			hid_err(field->report->device, "value %d is out of range\n", value);
 			return -1;
 		}
 	}
@@ -1157,7 +1159,7 @@ static bool hid_match_one_id(struct hid_device *hdev,
 		(id->product == HID_ANY_ID || id->product == hdev->product);
 }
 
-static const struct hid_device_id *hid_match_id(struct hid_device *hdev,
+const struct hid_device_id *hid_match_id(struct hid_device *hdev,
 		const struct hid_device_id *id)
 {
 	for (; id->bus; id++)
@@ -1404,11 +1406,13 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_CYPRESS, USB_DEVICE_ID_CYPRESS_TRUETOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, 0x0006) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DRAGONRISE, 0x0011) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH1) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH3) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH4) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480D) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480E) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_720C) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_726B) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72A1) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7302) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DWAV, USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_ELECOM, USB_DEVICE_ID_ELECOM_BM084) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_TS2515) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_EMS, USB_DEVICE_ID_EMS_TRIO_LINKER_PLUS_II) },
@@ -1423,6 +1427,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_3) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HANVON, USB_DEVICE_ID_HANVON_MULTITOUCH) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_HANVON_ALT, USB_DEVICE_ID_HANVON_ALT_MULTITOUCH) },
  	{ HID_USB_DEVICE(USB_VENDOR_ID_IDEACOM, USB_DEVICE_ID_IDEACOM_IDC6650) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK, USB_DEVICE_ID_HOLTEK_ON_LINE_GRIP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ILITEK, USB_DEVICE_ID_ILITEK_MULTITOUCH) },
@@ -1498,11 +1503,15 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_WKB2000) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PENMOUNT, USB_DEVICE_ID_PENMOUNT_PCI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PETALYNX, USB_DEVICE_ID_PETALYNX_MAXTER_REMOTE) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PRIMAX, USB_DEVICE_ID_PRIMAX_KEYBOARD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ARVO) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ISKU) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KONEPLUS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_KOVAPLUS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_PYRA_WIRED) },
@@ -1544,11 +1553,21 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_DUAL_BOX_PRO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_JOY_BOX_5_PRO) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_WACOM, USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_WACOM, USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SLIM_TABLET_5_8_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SLIM_TABLET_12_1_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_10_6_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_MEDIA_TABLET_14_1_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_XAT, USB_DEVICE_ID_XAT_CSR) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_SPX) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_MPX) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_CSR) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_SPX1) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_MPX1) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_CSR1) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_SPX2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_MPX2) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_XIROKU, USB_DEVICE_ID_XIROKU_CSR2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_X_TENSIONS, USB_DEVICE_ID_SPEEDLINK_VAD_CEZANNE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ZEROPLUS, 0x0005) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ZEROPLUS, 0x0030) },
@@ -1768,6 +1787,7 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DELORME, USB_DEVICE_ID_DELORME_EARTHMATE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DELORME, USB_DEVICE_ID_DELORME_EM_LT20) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_DREAM_CHEEKY, 0x0004) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_DREAM_CHEEKY, 0x000a) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ESSENTIAL_REALITY, USB_DEVICE_ID_ESSENTIAL_REALITY_P5) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ETT, USB_DEVICE_ID_TC5UH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ETT, USB_DEVICE_ID_TC4UM) },
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index ee80d733801d..01dd9a7daf7a 100644
--- a/drivers/hid/hid-debug.c
+++ b/drivers/hid/hid-debug.c
@@ -114,6 +114,14 @@ static const struct hid_usage_entry hid_usage_table[] = {
       {0, 0xbd, "FlareRelease"},
       {0, 0xbe, "LandingGear"},
       {0, 0xbf, "ToeBrake"},
+  {  6, 0, "GenericDeviceControls" },
+      {0, 0x20, "BatteryStrength" },
+      {0, 0x21, "WirelessChannel" },
+      {0, 0x22, "WirelessID" },
+      {0, 0x23, "DiscoverWirelessControl" },
+      {0, 0x24, "SecurityCodeCharacterEntered" },
+      {0, 0x25, "SecurityCodeCharactedErased" },
+      {0, 0x26, "SecurityCodeCleared" },
   {  7, 0, "Keyboard" },
   {  8, 0, "LED" },
       {0, 0x01, "NumLock"},
diff --git a/drivers/hid/hid-emsff.c b/drivers/hid/hid-emsff.c
index 9bdde867a02f..2630d483d262 100644
--- a/drivers/hid/hid-emsff.c
+++ b/drivers/hid/hid-emsff.c
@@ -140,7 +140,7 @@ err:
 }
 
 static const struct hid_device_id ems_devices[] = {
-	{ HID_USB_DEVICE(USB_VENDOR_ID_EMS, 0x118) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_EMS, USB_DEVICE_ID_EMS_TRIO_LINKER_PLUS_II) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, ems_devices);
diff --git a/drivers/staging/hv/hv_mouse.c b/drivers/hid/hid-hyperv.c
index ccd39c70c527..0c33ae9cf0f0 100644
--- a/drivers/staging/hv/hv_mouse.c
+++ b/drivers/hid/hid-hyperv.c
@@ -14,11 +14,8 @@
  */
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/delay.h>
 #include <linux/device.h>
-#include <linux/workqueue.h>
-#include <linux/sched.h>
-#include <linux/wait.h>
+#include <linux/completion.h>
 #include <linux/input.h>
 #include <linux/hid.h>
 #include <linux/hiddev.h>
@@ -118,7 +115,6 @@ struct synthhid_input_report {
 #define INPUTVSC_SEND_RING_BUFFER_SIZE		(10*PAGE_SIZE)
 #define INPUTVSC_RECV_RING_BUFFER_SIZE		(10*PAGE_SIZE)
 
-#define NBITS(x) (((x)/BITS_PER_LONG)+1)
 
 enum pipe_prot_msg_type {
 	PIPE_MESSAGE_INVALID,
@@ -148,7 +144,8 @@ struct  mousevsc_prt_msg {
  */
 struct mousevsc_dev {
 	struct hv_device	*device;
-	unsigned char		init_complete;
+	bool			init_complete;
+	bool			connected;
 	struct mousevsc_prt_msg	protocol_req;
 	struct mousevsc_prt_msg	protocol_resp;
 	/* Synchronize the request/response if needed */
@@ -159,12 +156,11 @@ struct mousevsc_dev {
 	unsigned char		*report_desc;
 	u32			report_desc_size;
 	struct hv_input_dev_info hid_dev_info;
-	int			connected;
 	struct hid_device       *hid_device;
 };
 
 
-static struct mousevsc_dev *alloc_input_device(struct hv_device *device)
+static struct mousevsc_dev *mousevsc_alloc_device(struct hv_device *device)
 {
 	struct mousevsc_dev *input_dev;
 
@@ -176,11 +172,12 @@ static struct mousevsc_dev *alloc_input_device(struct hv_device *device)
 	input_dev->device = device;
 	hv_set_drvdata(device, input_dev);
 	init_completion(&input_dev->wait_event);
+	input_dev->init_complete = false;
 
 	return input_dev;
 }
 
-static void free_input_device(struct mousevsc_dev *device)
+static void mousevsc_free_device(struct mousevsc_dev *device)
 {
 	kfree(device->hid_desc);
 	kfree(device->report_desc);
@@ -188,7 +185,6 @@ static void free_input_device(struct mousevsc_dev *device)
 	kfree(device);
 }
 
-
 static void mousevsc_on_receive_device_info(struct mousevsc_dev *input_device,
 				struct synthhid_device_info *device_info)
 {
@@ -196,14 +192,12 @@ static void mousevsc_on_receive_device_info(struct mousevsc_dev *input_device,
 	struct hid_descriptor *desc;
 	struct mousevsc_prt_msg ack;
 
-	/* Assume success for now */
-	input_device->dev_info_status = 0;
-
-	memcpy(&input_device->hid_dev_info, &device_info->hid_dev_info,
-		sizeof(struct hv_input_dev_info));
+	input_device->dev_info_status = -ENOMEM;
 
+	input_device->hid_dev_info = device_info->hid_dev_info;
 	desc = &device_info->hid_descriptor;
-	WARN_ON(desc->bLength == 0);
+	if (desc->bLength == 0)
+		goto cleanup;
 
 	input_device->hid_desc = kzalloc(desc->bLength, GFP_ATOMIC);
 
@@ -213,13 +207,18 @@ static void mousevsc_on_receive_device_info(struct mousevsc_dev *input_device,
 	memcpy(input_device->hid_desc, desc, desc->bLength);
 
 	input_device->report_desc_size = desc->desc[0].wDescriptorLength;
-	if (input_device->report_desc_size == 0)
+	if (input_device->report_desc_size == 0) {
+		input_device->dev_info_status = -EINVAL;
 		goto cleanup;
+	}
+
 	input_device->report_desc = kzalloc(input_device->report_desc_size,
 					  GFP_ATOMIC);
 
-	if (!input_device->report_desc)
+	if (!input_device->report_desc) {
+		input_device->dev_info_status = -ENOMEM;
 		goto cleanup;
+	}
 
 	memcpy(input_device->report_desc,
 	       ((unsigned char *)desc) + desc->bLength,
@@ -242,22 +241,14 @@ static void mousevsc_on_receive_device_info(struct mousevsc_dev *input_device,
 			(unsigned long)&ack,
 			VM_PKT_DATA_INBAND,
 			VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
-	if (ret != 0)
-		goto cleanup;
-
-	complete(&input_device->wait_event);
 
-	return;
+	if (!ret)
+		input_device->dev_info_status = 0;
 
 cleanup:
-	kfree(input_device->hid_desc);
-	input_device->hid_desc = NULL;
-
-	kfree(input_device->report_desc);
-	input_device->report_desc = NULL;
-
-	input_device->dev_info_status = -1;
 	complete(&input_device->wait_event);
+
+	return;
 }
 
 static void mousevsc_on_receive(struct hv_device *device,
@@ -274,10 +265,22 @@ static void mousevsc_on_receive(struct hv_device *device,
 	if (pipe_msg->type != PIPE_MESSAGE_DATA)
 		return;
 
-	hid_msg = (struct synthhid_msg *)&pipe_msg->data[0];
+	hid_msg = (struct synthhid_msg *)pipe_msg->data;
 
 	switch (hid_msg->header.type) {
 	case SYNTH_HID_PROTOCOL_RESPONSE:
+		/*
+		 * While it will be impossible for us to protect against
+		 * malicious/buggy hypervisor/host, add a check here to
+		 * ensure we don't corrupt memory.
+		 */
+		if ((pipe_msg->size + sizeof(struct pipe_prt_msg)
+			- sizeof(unsigned char))
+			> sizeof(struct mousevsc_prt_msg)) {
+			WARN_ON(1);
+			break;
+		}
+
 		memcpy(&input_dev->protocol_resp, pipe_msg,
 		       pipe_msg->size + sizeof(struct pipe_prt_msg) -
 		       sizeof(unsigned char));
@@ -292,11 +295,11 @@ static void mousevsc_on_receive(struct hv_device *device,
 		 * hid desc and report desc
 		 */
 		mousevsc_on_receive_device_info(input_dev,
-			(struct synthhid_device_info *)&pipe_msg->data[0]);
+			(struct synthhid_device_info *)pipe_msg->data);
 		break;
 	case SYNTH_HID_INPUT_REPORT:
 		input_report =
-			(struct synthhid_input_report *)&pipe_msg->data[0];
+			(struct synthhid_input_report *)pipe_msg->data;
 		if (!input_dev->init_complete)
 			break;
 		hid_input_report(input_dev->hid_device,
@@ -313,73 +316,60 @@ static void mousevsc_on_receive(struct hv_device *device,
 
 static void mousevsc_on_channel_callback(void *context)
 {
-	const int packetSize = 0x100;
-	int ret = 0;
-	struct hv_device *device = (struct hv_device *)context;
-
+	const int packet_size = 0x100;
+	int ret;
+	struct hv_device *device = context;
 	u32 bytes_recvd;
 	u64 req_id;
-	unsigned char packet[0x100];
 	struct vmpacket_descriptor *desc;
-	unsigned char	*buffer = packet;
-	int	bufferlen = packetSize;
+	unsigned char	*buffer;
+	int	bufferlen = packet_size;
 
+	buffer = kmalloc(bufferlen, GFP_ATOMIC);
+	if (!buffer)
+		return;
 
 	do {
 		ret = vmbus_recvpacket_raw(device->channel, buffer,
 					bufferlen, &bytes_recvd, &req_id);
 
-		if (ret == 0) {
-			if (bytes_recvd > 0) {
-				desc = (struct vmpacket_descriptor *)buffer;
-
-				switch (desc->type) {
-				case VM_PKT_COMP:
-					break;
-
-				case VM_PKT_DATA_INBAND:
-					mousevsc_on_receive(
-						device, desc);
-					break;
-
-				default:
-					pr_err("unhandled packet type %d, tid %llx len %d\n",
-						   desc->type,
-						   req_id,
-						   bytes_recvd);
-					break;
-				}
-
-				/* reset */
-				if (bufferlen > packetSize) {
-					kfree(buffer);
-
-					buffer = packet;
-					bufferlen = packetSize;
-				}
-			} else {
-				if (bufferlen > packetSize) {
-					kfree(buffer);
-
-					buffer = packet;
-					bufferlen = packetSize;
-				}
+		switch (ret) {
+		case 0:
+			if (bytes_recvd <= 0) {
+				kfree(buffer);
+				return;
+			}
+			desc = (struct vmpacket_descriptor *)buffer;
+
+			switch (desc->type) {
+			case VM_PKT_COMP:
+				break;
+
+			case VM_PKT_DATA_INBAND:
+				mousevsc_on_receive(device, desc);
+				break;
+
+			default:
+				pr_err("unhandled packet type %d, tid %llx len %d\n",
+					desc->type, req_id, bytes_recvd);
 				break;
 			}
-		} else if (ret == -ENOBUFS) {
+
+			break;
+
+		case -ENOBUFS:
+			kfree(buffer);
 			/* Handle large packet */
 			bufferlen = bytes_recvd;
-			buffer = kzalloc(bytes_recvd, GFP_ATOMIC);
+			buffer = kmalloc(bytes_recvd, GFP_ATOMIC);
 
-			if (buffer == NULL) {
-				buffer = packet;
-				bufferlen = packetSize;
-				break;
-			}
+			if (!buffer)
+				return;
+
+			break;
 		}
 	} while (1);
 
-	return;
 }
 
 static int mousevsc_connect_to_vsp(struct hv_device *device)
@@ -390,19 +380,15 @@ static int mousevsc_connect_to_vsp(struct hv_device *device)
 	struct mousevsc_prt_msg *request;
 	struct mousevsc_prt_msg *response;
 
-
 	request = &input_dev->protocol_req;
-
 	memset(request, 0, sizeof(struct mousevsc_prt_msg));
 
 	request->type = PIPE_MESSAGE_DATA;
 	request->size = sizeof(struct synthhid_protocol_request);
-
 	request->request.header.type = SYNTH_HID_PROTOCOL_REQUEST;
 	request->request.header.size = sizeof(unsigned int);
 	request->request.version_requested.version = SYNTHHID_INPUT_VERSION;
 
-
 	ret = vmbus_sendpacket(device->channel, request,
 				sizeof(struct pipe_prt_msg) -
 				sizeof(unsigned char) +
@@ -410,11 +396,11 @@ static int mousevsc_connect_to_vsp(struct hv_device *device)
 				(unsigned long)request,
 				VM_PKT_DATA_INBAND,
 				VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
-	if (ret != 0)
+	if (ret)
 		goto cleanup;
 
 	t = wait_for_completion_timeout(&input_dev->wait_event, 5*HZ);
-	if (t == 0) {
+	if (!t) {
 		ret = -ETIMEDOUT;
 		goto cleanup;
 	}
@@ -422,14 +408,14 @@ static int mousevsc_connect_to_vsp(struct hv_device *device)
 	response = &input_dev->protocol_resp;
 
 	if (!response->response.approved) {
-		pr_err("synthhid protocol request failed (version %d)",
+		pr_err("synthhid protocol request failed (version %d)\n",
 		       SYNTHHID_INPUT_VERSION);
 		ret = -ENODEV;
 		goto cleanup;
 	}
 
 	t = wait_for_completion_timeout(&input_dev->wait_event, 5*HZ);
-	if (t == 0) {
+	if (!t) {
 		ret = -ETIMEDOUT;
 		goto cleanup;
 	}
@@ -438,11 +424,9 @@ static int mousevsc_connect_to_vsp(struct hv_device *device)
 	 * We should have gotten the device attr, hid desc and report
 	 * desc at this point
 	 */
-	if (input_dev->dev_info_status)
-		ret = -ENOMEM;
+	ret = input_dev->dev_info_status;
 
 cleanup:
-
 	return ret;
 }
 
@@ -451,61 +435,40 @@ static int mousevsc_hid_open(struct hid_device *hid)
 	return 0;
 }
 
+static int mousevsc_hid_start(struct hid_device *hid)
+{
+	return 0;
+}
+
 static void mousevsc_hid_close(struct hid_device *hid)
 {
 }
 
+static void mousevsc_hid_stop(struct hid_device *hid)
+{
+}
+
 static struct hid_ll_driver mousevsc_ll_driver = {
 	.open = mousevsc_hid_open,
 	.close = mousevsc_hid_close,
+	.start = mousevsc_hid_start,
+	.stop = mousevsc_hid_stop,
 };
 
 static struct hid_driver mousevsc_hid_driver;
 
-static void reportdesc_callback(struct hv_device *dev, void *packet, u32 len)
-{
-	struct hid_device *hid_dev;
-	struct mousevsc_dev *input_device = hv_get_drvdata(dev);
-
-	hid_dev = hid_allocate_device();
-	if (IS_ERR(hid_dev))
-		return;
-
-	hid_dev->ll_driver = &mousevsc_ll_driver;
-	hid_dev->driver = &mousevsc_hid_driver;
-
-	if (hid_parse_report(hid_dev, packet, len))
-		return;
-
-	hid_dev->bus = BUS_VIRTUAL;
-	hid_dev->vendor = input_device->hid_dev_info.vendor;
-	hid_dev->product = input_device->hid_dev_info.product;
-	hid_dev->version = input_device->hid_dev_info.version;
-
-	sprintf(hid_dev->name, "%s", "Microsoft Vmbus HID-compliant Mouse");
-
-	if (!hidinput_connect(hid_dev, 0)) {
-		hid_dev->claimed |= HID_CLAIMED_INPUT;
-
-		input_device->connected = 1;
-
-	}
-
-	input_device->hid_device = hid_dev;
-}
-
-static int mousevsc_on_device_add(struct hv_device *device)
+static int mousevsc_probe(struct hv_device *device,
+			const struct hv_vmbus_device_id *dev_id)
 {
-	int ret = 0;
+	int ret;
 	struct mousevsc_dev *input_dev;
+	struct hid_device *hid_dev;
 
-	input_dev = alloc_input_device(device);
+	input_dev = mousevsc_alloc_device(device);
 
 	if (!input_dev)
 		return -ENOMEM;
 
-	input_dev->init_complete = false;
-
 	ret = vmbus_open(device->channel,
 		INPUTVSC_SEND_RING_BUFFER_SIZE,
 		INPUTVSC_RECV_RING_BUFFER_SIZE,
@@ -515,54 +478,78 @@ static int mousevsc_on_device_add(struct hv_device *device)
 		device
 		);
 
-	if (ret != 0) {
-		free_input_device(input_dev);
-		return ret;
-	}
-
+	if (ret)
+		goto probe_err0;
 
 	ret = mousevsc_connect_to_vsp(device);
 
-	if (ret != 0) {
-		vmbus_close(device->channel);
-		free_input_device(input_dev);
-		return ret;
-	}
-
+	if (ret)
+		goto probe_err1;
 
 	/* workaround SA-167 */
 	if (input_dev->report_desc[14] == 0x25)
 		input_dev->report_desc[14] = 0x29;
 
-	reportdesc_callback(device, input_dev->report_desc,
-			    input_dev->report_desc_size);
+	hid_dev = hid_allocate_device();
+	if (IS_ERR(hid_dev)) {
+		ret = PTR_ERR(hid_dev);
+		goto probe_err1;
+	}
+
+	hid_dev->ll_driver = &mousevsc_ll_driver;
+	hid_dev->driver = &mousevsc_hid_driver;
+	hid_dev->bus = BUS_VIRTUAL;
+	hid_dev->vendor = input_dev->hid_dev_info.vendor;
+	hid_dev->product = input_dev->hid_dev_info.product;
+	hid_dev->version = input_dev->hid_dev_info.version;
+	input_dev->hid_device = hid_dev;
+
+	sprintf(hid_dev->name, "%s", "Microsoft Vmbus HID-compliant Mouse");
+
+	ret = hid_add_device(hid_dev);
+	if (ret)
+		goto probe_err1;
+
+	ret = hid_parse_report(hid_dev, input_dev->report_desc,
+				input_dev->report_desc_size);
+
+	if (ret) {
+		hid_err(hid_dev, "parse failed\n");
+		goto probe_err2;
+	}
 
+	ret = hid_hw_start(hid_dev, HID_CONNECT_HIDINPUT | HID_CONNECT_HIDDEV);
+
+	if (ret) {
+		hid_err(hid_dev, "hw start failed\n");
+		goto probe_err2;
+	}
+
+	input_dev->connected = true;
 	input_dev->init_complete = true;
 
 	return ret;
-}
 
-static int mousevsc_probe(struct hv_device *dev,
-			const struct hv_vmbus_device_id *dev_id)
-{
+probe_err2:
+	hid_destroy_device(hid_dev);
 
-	return mousevsc_on_device_add(dev);
+probe_err1:
+	vmbus_close(device->channel);
 
+probe_err0:
+	mousevsc_free_device(input_dev);
+
+	return ret;
 }
 
+
 static int mousevsc_remove(struct hv_device *dev)
 {
 	struct mousevsc_dev *input_dev = hv_get_drvdata(dev);
 
 	vmbus_close(dev->channel);
-
-	if (input_dev->connected) {
-		hidinput_disconnect(input_dev->hid_device);
-		input_dev->connected = 0;
-		hid_destroy_device(input_dev->hid_device);
-	}
-
-	free_input_device(input_dev);
+	hid_destroy_device(input_dev->hid_device);
+	mousevsc_free_device(input_dev);
 
 	return 0;
 }
@@ -577,7 +564,7 @@ static const struct hv_vmbus_device_id id_table[] = {
 MODULE_DEVICE_TABLE(vmbus, id_table);
 
 static struct  hv_driver mousevsc_drv = {
-	.name = "mousevsc",
+	.name = KBUILD_MODNAME,
 	.id_table = id_table,
 	.probe = mousevsc_probe,
 	.remove = mousevsc_remove,
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 4a441a6f9967..b8574cddd953 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -21,6 +21,7 @@
 #define USB_VENDOR_ID_3M		0x0596
 #define USB_DEVICE_ID_3M1968		0x0500
 #define USB_DEVICE_ID_3M2256		0x0502
+#define USB_DEVICE_ID_3M3266		0x0506
 
 #define USB_VENDOR_ID_A4TECH		0x09da
 #define USB_DEVICE_ID_A4TECH_WCP32PU	0x0006
@@ -124,6 +125,7 @@
 #define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ANSI  0x0239
 #define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ISO   0x023a
 #define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_JIS   0x023b
+#define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ANSI  0x0255
 #define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ISO   0x0256
 #define USB_DEVICE_ID_APPLE_FOUNTAIN_TP_ONLY	0x030a
 #define USB_DEVICE_ID_APPLE_GEYSER1_TP_ONLY	0x030b
@@ -145,6 +147,9 @@
 #define USB_DEVICE_ID_ATEN_4PORTKVM	0x2205
 #define USB_DEVICE_ID_ATEN_4PORTKVMC	0x2208
 
+#define USB_VENDOR_ID_ATMEL		0x03eb
+#define USB_DEVICE_ID_ATMEL_MULTITOUCH	0x211c
+
 #define USB_VENDOR_ID_AVERMEDIA		0x07ca
 #define USB_DEVICE_ID_AVER_FM_MR800	0xb800
 
@@ -230,11 +235,14 @@
 
 #define USB_VENDOR_ID_DWAV		0x0eef
 #define USB_DEVICE_ID_EGALAX_TOUCHCONTROLLER	0x0001
-#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH	0x480d
-#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH1	0x720c
-#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH2	0x72a1
-#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH3	0x480e
-#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH4	0x726b
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480D	0x480d
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480E	0x480e
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_720C	0x720c
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_726B	0x726b
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72A1	0x72a1
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72FA	0x72fa
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7302	0x7302
+#define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001	0xa001
 
 #define USB_VENDOR_ID_ELECOM		0x056e
 #define USB_DEVICE_ID_ELECOM_BM084	0x0061
@@ -356,6 +364,9 @@
 #define USB_VENDOR_ID_HANVON		0x20b3
 #define USB_DEVICE_ID_HANVON_MULTITOUCH	0x0a18
 
+#define USB_VENDOR_ID_HANVON_ALT	0x22ed
+#define USB_DEVICE_ID_HANVON_ALT_MULTITOUCH	0x1010
+
 #define USB_VENDOR_ID_HAPP		0x078b
 #define USB_DEVICE_ID_UGCI_DRIVING	0x0010
 #define USB_DEVICE_ID_UGCI_FLYING	0x0020
@@ -571,6 +582,11 @@
 #define USB_VENDOR_ID_PI_ENGINEERING	0x05f3
 #define USB_DEVICE_ID_PI_ENGINEERING_VEC_USB_FOOTPEDAL	0xff
 
+#define USB_VENDOR_ID_PIXART				0x093a
+#define USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN	0x8001
+#define USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1	0x8002
+#define USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2	0x8003
+
 #define USB_VENDOR_ID_PLAYDOTCOM	0x0b43
 #define USB_DEVICE_ID_PLAYDOTCOM_EMS_USBII	0x0003
 
@@ -581,11 +597,14 @@
 #define USB_DEVICE_ID_PRODIGE_CORDLESS	0x3062
 
 #define USB_VENDOR_ID_QUANTA		0x0408
-#define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH	0x3000
+#define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH		0x3000
+#define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3001		0x3001
+#define USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3008		0x3008
 #define USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN	0x3001
 
 #define USB_VENDOR_ID_ROCCAT		0x1e7d
 #define USB_DEVICE_ID_ROCCAT_ARVO	0x30d4
+#define USB_DEVICE_ID_ROCCAT_ISKU	0x319c
 #define USB_DEVICE_ID_ROCCAT_KONE	0x2ced
 #define USB_DEVICE_ID_ROCCAT_KONEPLUS	0x2d51
 #define USB_DEVICE_ID_ROCCAT_KOVAPLUS	0x2d50
@@ -679,6 +698,7 @@
 
 #define USB_VENDOR_ID_WACOM		0x056a
 #define USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH	0x81
+#define USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH   0x00BD
 
 #define USB_VENDOR_ID_WALTOP				0x172f
 #define USB_DEVICE_ID_WALTOP_SLIM_TABLET_5_8_INCH	0x0032
@@ -707,6 +727,17 @@
 #define USB_VENDOR_ID_XAT	0x2505
 #define USB_DEVICE_ID_XAT_CSR	0x0220
 
+#define USB_VENDOR_ID_XIROKU		0x1477
+#define USB_DEVICE_ID_XIROKU_SPX	0x1006
+#define USB_DEVICE_ID_XIROKU_MPX	0x1007
+#define USB_DEVICE_ID_XIROKU_CSR	0x100e
+#define USB_DEVICE_ID_XIROKU_SPX1	0x1021
+#define USB_DEVICE_ID_XIROKU_CSR1	0x1022
+#define USB_DEVICE_ID_XIROKU_MPX1	0x1023
+#define USB_DEVICE_ID_XIROKU_SPX2	0x1024
+#define USB_DEVICE_ID_XIROKU_CSR2	0x1025
+#define USB_DEVICE_ID_XIROKU_MPX2	0x1026
+
 #define USB_VENDOR_ID_YEALINK		0x6993
 #define USB_DEVICE_ID_YEALINK_P1K_P4K_B2K	0xb001
 
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index f333139d1a48..9333d692a786 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -32,6 +32,8 @@
 #include <linux/hid.h>
 #include <linux/hid-debug.h>
 
+#include "hid-ids.h"
+
 #define unk	KEY_UNKNOWN
 
 static const unsigned char hid_keyboard[256] = {
@@ -271,6 +273,161 @@ static __s32 hidinput_calc_abs_res(const struct hid_field *field, __u16 code)
 	return logical_extents / physical_extents;
 }
 
+#ifdef CONFIG_HID_BATTERY_STRENGTH
+static enum power_supply_property hidinput_battery_props[] = {
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_MODEL_NAME,
+	POWER_SUPPLY_PROP_STATUS
+};
+
+#define HID_BATTERY_QUIRK_PERCENT	(1 << 0) /* always reports percent */
+#define HID_BATTERY_QUIRK_FEATURE	(1 << 1) /* ask for feature report */
+
+static const struct hid_device_id hid_battery_quirks[] = {
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE,
+			       USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ANSI),
+	  HID_BATTERY_QUIRK_PERCENT | HID_BATTERY_QUIRK_FEATURE },
+	{}
+};
+
+static unsigned find_battery_quirk(struct hid_device *hdev)
+{
+	unsigned quirks = 0;
+	const struct hid_device_id *match;
+
+	match = hid_match_id(hdev, hid_battery_quirks);
+	if (match != NULL)
+		quirks = match->driver_data;
+
+	return quirks;
+}
+
+static int hidinput_get_battery_property(struct power_supply *psy,
+					 enum power_supply_property prop,
+					 union power_supply_propval *val)
+{
+	struct hid_device *dev = container_of(psy, struct hid_device, battery);
+	int ret = 0;
+	__u8 buf[2] = {};
+
+	switch (prop) {
+	case POWER_SUPPLY_PROP_PRESENT:
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = 1;
+		break;
+
+	case POWER_SUPPLY_PROP_CAPACITY:
+		ret = dev->hid_get_raw_report(dev, dev->battery_report_id,
+					      buf, sizeof(buf),
+					      dev->battery_report_type);
+
+		if (ret != 2) {
+			if (ret >= 0)
+				ret = -EINVAL;
+			break;
+		}
+
+		if (dev->battery_min < dev->battery_max &&
+		    buf[1] >= dev->battery_min &&
+		    buf[1] <= dev->battery_max)
+			val->intval = (100 * (buf[1] - dev->battery_min)) /
+				(dev->battery_max - dev->battery_min);
+		break;
+
+	case POWER_SUPPLY_PROP_MODEL_NAME:
+		val->strval = dev->name;
+		break;
+
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static bool hidinput_setup_battery(struct hid_device *dev, unsigned report_type, struct hid_field *field)
+{
+	struct power_supply *battery = &dev->battery;
+	int ret;
+	unsigned quirks;
+	s32 min, max;
+
+	if (field->usage->hid != HID_DC_BATTERYSTRENGTH)
+		return false;	/* no match */
+
+	if (battery->name != NULL)
+		goto out;	/* already initialized? */
+
+	battery->name = kasprintf(GFP_KERNEL, "hid-%s-battery", dev->uniq);
+	if (battery->name == NULL)
+		goto out;
+
+	battery->type = POWER_SUPPLY_TYPE_BATTERY;
+	battery->properties = hidinput_battery_props;
+	battery->num_properties = ARRAY_SIZE(hidinput_battery_props);
+	battery->use_for_apm = 0;
+	battery->get_property = hidinput_get_battery_property;
+
+	quirks = find_battery_quirk(dev);
+
+	hid_dbg(dev, "device %x:%x:%x %d quirks %d\n",
+		dev->bus, dev->vendor, dev->product, dev->version, quirks);
+
+	min = field->logical_minimum;
+	max = field->logical_maximum;
+
+	if (quirks & HID_BATTERY_QUIRK_PERCENT) {
+		min = 0;
+		max = 100;
+	}
+
+	if (quirks & HID_BATTERY_QUIRK_FEATURE)
+		report_type = HID_FEATURE_REPORT;
+
+	dev->battery_min = min;
+	dev->battery_max = max;
+	dev->battery_report_type = report_type;
+	dev->battery_report_id = field->report->id;
+
+	ret = power_supply_register(&dev->dev, battery);
+	if (ret != 0) {
+		hid_warn(dev, "can't register power supply: %d\n", ret);
+		kfree(battery->name);
+		battery->name = NULL;
+	}
+
+out:
+	return true;
+}
+
+static void hidinput_cleanup_battery(struct hid_device *dev)
+{
+	if (!dev->battery.name)
+		return;
+
+	power_supply_unregister(&dev->battery);
+	kfree(dev->battery.name);
+	dev->battery.name = NULL;
+}
+#else  /* !CONFIG_HID_BATTERY_STRENGTH */
+static bool hidinput_setup_battery(struct hid_device *dev, unsigned report_type,
+				   struct hid_field *field)
+{
+	return false;
+}
+
+static void hidinput_cleanup_battery(struct hid_device *dev)
+{
+}
+#endif	/* CONFIG_HID_BATTERY_STRENGTH */
+
 static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_field *field,
 				     struct hid_usage *usage)
 {
@@ -629,6 +786,13 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		}
 		break;
 
+	case HID_UP_GENDEVCTRLS:
+		if (hidinput_setup_battery(device, HID_INPUT_REPORT, field))
+			goto ignore;
+		else
+			goto unknown;
+		break;
+
 	case HID_UP_HPVENDOR:	/* Reported on a Dutch layout HP5308 */
 		set_bit(EV_REP, input->evbit);
 		switch (usage->hid & HID_USAGE) {
@@ -822,6 +986,12 @@ void hidinput_hid_event(struct hid_device *hid, struct hid_field *field, struct
 		return;
 	}
 
+	/* Ignore out-of-range values as per HID specification, section 5.10 */
+	if (value < field->logical_minimum || value > field->logical_maximum) {
+		dbg_hid("Ignoring out-of-range value %x\n", value);
+		return;
+	}
+
 	/* report the usage code as scancode if the key status has changed */
 	if (usage->type == EV_KEY && !!test_bit(usage->code, input->key) != value)
 		input_event(input, EV_MSC, MSC_SCAN, usage->hid);
@@ -861,6 +1031,48 @@ int hidinput_find_field(struct hid_device *hid, unsigned int type, unsigned int
 }
 EXPORT_SYMBOL_GPL(hidinput_find_field);
 
+struct hid_field *hidinput_get_led_field(struct hid_device *hid)
+{
+	struct hid_report *report;
+	struct hid_field *field;
+	int i, j;
+
+	list_for_each_entry(report,
+			    &hid->report_enum[HID_OUTPUT_REPORT].report_list,
+			    list) {
+		for (i = 0; i < report->maxfield; i++) {
+			field = report->field[i];
+			for (j = 0; j < field->maxusage; j++)
+				if (field->usage[j].type == EV_LED)
+					return field;
+		}
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(hidinput_get_led_field);
+
+unsigned int hidinput_count_leds(struct hid_device *hid)
+{
+	struct hid_report *report;
+	struct hid_field *field;
+	int i, j;
+	unsigned int count = 0;
+
+	list_for_each_entry(report,
+			    &hid->report_enum[HID_OUTPUT_REPORT].report_list,
+			    list) {
+		for (i = 0; i < report->maxfield; i++) {
+			field = report->field[i];
+			for (j = 0; j < field->maxusage; j++)
+				if (field->usage[j].type == EV_LED &&
+				    field->value[j])
+					count += 1;
+		}
+	}
+	return count;
+}
+EXPORT_SYMBOL_GPL(hidinput_count_leds);
+
 static int hidinput_open(struct input_dev *dev)
 {
 	struct hid_device *hid = input_get_drvdata(dev);
@@ -882,15 +1094,17 @@ static void report_features(struct hid_device *hid)
 	struct hid_report *rep;
 	int i, j;
 
-	if (!drv->feature_mapping)
-		return;
-
 	rep_enum = &hid->report_enum[HID_FEATURE_REPORT];
 	list_for_each_entry(rep, &rep_enum->report_list, list)
 		for (i = 0; i < rep->maxfield; i++)
-			for (j = 0; j < rep->field[i]->maxusage; j++)
-				drv->feature_mapping(hid, rep->field[i],
-						     rep->field[i]->usage + j);
+			for (j = 0; j < rep->field[i]->maxusage; j++) {
+				/* Verify if Battery Strength feature is available */
+				hidinput_setup_battery(hid, HID_FEATURE_REPORT, rep->field[i]);
+
+				if (drv->feature_mapping)
+					drv->feature_mapping(hid, rep->field[i],
+							     rep->field[i]->usage + j);
+			}
 }
 
 /*
@@ -1010,6 +1224,8 @@ void hidinput_disconnect(struct hid_device *hid)
 {
 	struct hid_input *hidinput, *next;
 
+	hidinput_cleanup_battery(hid);
+
 	list_for_each_entry_safe(hidinput, next, &hid->inputs, list) {
 		list_del(&hidinput->list);
 		input_unregister_device(hidinput->input);
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index 103f30d93f76..6ecc9e220440 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -430,7 +430,7 @@ int lg4ff_init(struct hid_device *hid)
 	}
 
 	/* Add the device to device_list */
-	entry = (struct lg4ff_device_entry *)kzalloc(sizeof(struct lg4ff_device_entry), GFP_KERNEL);
+	entry = kzalloc(sizeof(struct lg4ff_device_entry), GFP_KERNEL);
 	if (!entry) {
 		hid_err(hid, "Cannot add device, insufficient memory.\n");
 		return -ENOMEM;
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index f1c909f1b239..24fc4423b937 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -50,7 +50,6 @@ MODULE_LICENSE("GPL");
 #define MT_QUIRK_ALWAYS_VALID		(1 << 4)
 #define MT_QUIRK_VALID_IS_INRANGE	(1 << 5)
 #define MT_QUIRK_VALID_IS_CONFIDENCE	(1 << 6)
-#define MT_QUIRK_EGALAX_XYZ_FIXUP	(1 << 7)
 #define MT_QUIRK_SLOT_IS_CONTACTID_MINUS_ONE	(1 << 8)
 
 struct mt_slot {
@@ -60,9 +59,19 @@ struct mt_slot {
 	bool seen_in_this_frame;/* has this slot been updated */
 };
 
+struct mt_class {
+	__s32 name;	/* MT_CLS */
+	__s32 quirks;
+	__s32 sn_move;	/* Signal/noise ratio for move events */
+	__s32 sn_width;	/* Signal/noise ratio for width events */
+	__s32 sn_height;	/* Signal/noise ratio for height events */
+	__s32 sn_pressure;	/* Signal/noise ratio for pressure events */
+	__u8 maxcontacts;
+};
+
 struct mt_device {
 	struct mt_slot curdata;	/* placeholder of incoming data */
-	struct mt_class *mtclass;	/* our mt device class */
+	struct mt_class mtclass;	/* our mt device class */
 	unsigned last_field_index;	/* last field index of the report */
 	unsigned last_slot_field;	/* the last field of a slot */
 	int last_mt_collection;	/* last known mt-related collection */
@@ -74,30 +83,23 @@ struct mt_device {
 	struct mt_slot *slots;
 };
 
-struct mt_class {
-	__s32 name;	/* MT_CLS */
-	__s32 quirks;
-	__s32 sn_move;	/* Signal/noise ratio for move events */
-	__s32 sn_width;	/* Signal/noise ratio for width events */
-	__s32 sn_height;	/* Signal/noise ratio for height events */
-	__s32 sn_pressure;	/* Signal/noise ratio for pressure events */
-	__u8 maxcontacts;
-};
-
 /* classes of device behavior */
 #define MT_CLS_DEFAULT				0x0001
 
 #define MT_CLS_SERIAL				0x0002
 #define MT_CLS_CONFIDENCE			0x0003
-#define MT_CLS_CONFIDENCE_MINUS_ONE		0x0004
-#define MT_CLS_DUAL_INRANGE_CONTACTID		0x0005
-#define MT_CLS_DUAL_INRANGE_CONTACTNUMBER	0x0006
-#define MT_CLS_DUAL_NSMU_CONTACTID		0x0007
+#define MT_CLS_CONFIDENCE_CONTACT_ID		0x0004
+#define MT_CLS_CONFIDENCE_MINUS_ONE		0x0005
+#define MT_CLS_DUAL_INRANGE_CONTACTID		0x0006
+#define MT_CLS_DUAL_INRANGE_CONTACTNUMBER	0x0007
+#define MT_CLS_DUAL_NSMU_CONTACTID		0x0008
+#define MT_CLS_INRANGE_CONTACTNUMBER		0x0009
 
 /* vendor specific classes */
 #define MT_CLS_3M				0x0101
 #define MT_CLS_CYPRESS				0x0102
 #define MT_CLS_EGALAX				0x0103
+#define MT_CLS_EGALAX_SERIAL			0x0104
 
 #define MT_DEFAULT_MAXCONTACT	10
 
@@ -133,13 +135,16 @@ static int find_slot_from_contactid(struct mt_device *td)
 	return -1;
 }
 
-struct mt_class mt_classes[] = {
+static struct mt_class mt_classes[] = {
 	{ .name = MT_CLS_DEFAULT,
 		.quirks = MT_QUIRK_NOT_SEEN_MEANS_UP },
 	{ .name = MT_CLS_SERIAL,
 		.quirks = MT_QUIRK_ALWAYS_VALID},
 	{ .name = MT_CLS_CONFIDENCE,
 		.quirks = MT_QUIRK_VALID_IS_CONFIDENCE },
+	{ .name = MT_CLS_CONFIDENCE_CONTACT_ID,
+		.quirks = MT_QUIRK_VALID_IS_CONFIDENCE |
+			MT_QUIRK_SLOT_IS_CONTACTID },
 	{ .name = MT_CLS_CONFIDENCE_MINUS_ONE,
 		.quirks = MT_QUIRK_VALID_IS_CONFIDENCE |
 			MT_QUIRK_SLOT_IS_CONTACTID_MINUS_ONE },
@@ -155,6 +160,9 @@ struct mt_class mt_classes[] = {
 		.quirks = MT_QUIRK_NOT_SEEN_MEANS_UP |
 			MT_QUIRK_SLOT_IS_CONTACTID,
 		.maxcontacts = 2 },
+	{ .name = MT_CLS_INRANGE_CONTACTNUMBER,
+		.quirks = MT_QUIRK_VALID_IS_INRANGE |
+			MT_QUIRK_SLOT_IS_CONTACTNUMBER },
 
 	/*
 	 * vendor specific classes
@@ -171,9 +179,13 @@ struct mt_class mt_classes[] = {
 		.maxcontacts = 10 },
 	{ .name = MT_CLS_EGALAX,
 		.quirks =  MT_QUIRK_SLOT_IS_CONTACTID |
-			MT_QUIRK_VALID_IS_INRANGE |
-			MT_QUIRK_EGALAX_XYZ_FIXUP,
-		.maxcontacts = 2,
+			MT_QUIRK_VALID_IS_INRANGE,
+		.sn_move = 4096,
+		.sn_pressure = 32,
+	},
+	{ .name = MT_CLS_EGALAX_SERIAL,
+		.quirks =  MT_QUIRK_SLOT_IS_CONTACTID |
+			MT_QUIRK_ALWAYS_VALID,
 		.sn_move = 4096,
 		.sn_pressure = 32,
 	},
@@ -181,6 +193,44 @@ struct mt_class mt_classes[] = {
 	{ }
 };
 
+static ssize_t mt_show_quirks(struct device *dev,
+			   struct device_attribute *attr,
+			   char *buf)
+{
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct mt_device *td = hid_get_drvdata(hdev);
+
+	return sprintf(buf, "%u\n", td->mtclass.quirks);
+}
+
+static ssize_t mt_set_quirks(struct device *dev,
+			  struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct mt_device *td = hid_get_drvdata(hdev);
+
+	unsigned long val;
+
+	if (kstrtoul(buf, 0, &val))
+		return -EINVAL;
+
+	td->mtclass.quirks = val;
+
+	return count;
+}
+
+static DEVICE_ATTR(quirks, S_IWUSR | S_IRUGO, mt_show_quirks, mt_set_quirks);
+
+static struct attribute *sysfs_attrs[] = {
+	&dev_attr_quirks.attr,
+	NULL
+};
+
+static struct attribute_group mt_attribute_group = {
+	.attrs = sysfs_attrs
+};
+
 static void mt_feature_mapping(struct hid_device *hdev,
 		struct hid_field *field, struct hid_usage *usage)
 {
@@ -192,9 +242,9 @@ static void mt_feature_mapping(struct hid_device *hdev,
 		break;
 	case HID_DG_CONTACTMAX:
 		td->maxcontacts = field->value[0];
-		if (td->mtclass->maxcontacts)
+		if (td->mtclass.maxcontacts)
 			/* check if the maxcontacts is given by the class */
-			td->maxcontacts = td->mtclass->maxcontacts;
+			td->maxcontacts = td->mtclass.maxcontacts;
 
 		break;
 	}
@@ -214,8 +264,7 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 		unsigned long **bit, int *max)
 {
 	struct mt_device *td = hid_get_drvdata(hdev);
-	struct mt_class *cls = td->mtclass;
-	__s32 quirks = cls->quirks;
+	struct mt_class *cls = &td->mtclass;
 
 	/* Only map fields from TouchScreen or TouchPad collections.
          * We need to ignore fields that belong to other collections
@@ -227,13 +276,17 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	else
 		return 0;
 
+	/* eGalax devices provide a Digitizer.Stylus input which overrides
+	 * the correct Digitizers.Finger X/Y ranges.
+	 * Let's just ignore this input. */
+	if (field->physical == HID_DG_STYLUS)
+		return -1;
+
 	switch (usage->hid & HID_USAGE_PAGE) {
 
 	case HID_UP_GENDESK:
 		switch (usage->hid) {
 		case HID_GD_X:
-			if (quirks & MT_QUIRK_EGALAX_XYZ_FIXUP)
-				field->logical_maximum = 32760;
 			hid_map_usage(hi, usage, bit, max,
 					EV_ABS, ABS_MT_POSITION_X);
 			set_abs(hi->input, ABS_MT_POSITION_X, field,
@@ -246,8 +299,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			}
 			return 1;
 		case HID_GD_Y:
-			if (quirks & MT_QUIRK_EGALAX_XYZ_FIXUP)
-				field->logical_maximum = 32760;
 			hid_map_usage(hi, usage, bit, max,
 					EV_ABS, ABS_MT_POSITION_Y);
 			set_abs(hi->input, ABS_MT_POSITION_Y, field,
@@ -315,8 +366,6 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			}
 			return 1;
 		case HID_DG_TIPPRESSURE:
-			if (quirks & MT_QUIRK_EGALAX_XYZ_FIXUP)
-				field->logical_minimum = 0;
 			hid_map_usage(hi, usage, bit, max,
 					EV_ABS, ABS_MT_PRESSURE);
 			set_abs(hi->input, ABS_MT_PRESSURE, field,
@@ -363,7 +412,7 @@ static int mt_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 
 static int mt_compute_slot(struct mt_device *td)
 {
-	__s32 quirks = td->mtclass->quirks;
+	__s32 quirks = td->mtclass.quirks;
 
 	if (quirks & MT_QUIRK_SLOT_IS_CONTACTID)
 		return td->curdata.contactid;
@@ -407,7 +456,7 @@ static void mt_emit_event(struct mt_device *td, struct input_dev *input)
 
 	for (i = 0; i < td->maxcontacts; ++i) {
 		struct mt_slot *s = &(td->slots[i]);
-		if ((td->mtclass->quirks & MT_QUIRK_NOT_SEEN_MEANS_UP) &&
+		if ((td->mtclass.quirks & MT_QUIRK_NOT_SEEN_MEANS_UP) &&
 			!s->seen_in_this_frame) {
 			s->touch_state = false;
 		}
@@ -444,7 +493,7 @@ static int mt_event(struct hid_device *hid, struct hid_field *field,
 				struct hid_usage *usage, __s32 value)
 {
 	struct mt_device *td = hid_get_drvdata(hid);
-	__s32 quirks = td->mtclass->quirks;
+	__s32 quirks = td->mtclass.quirks;
 
 	if (hid->claimed & HID_CLAIMED_INPUT && td->slots) {
 		switch (usage->hid) {
@@ -552,7 +601,7 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		dev_err(&hdev->dev, "cannot allocate multitouch data\n");
 		return -ENOMEM;
 	}
-	td->mtclass = mtclass;
+	td->mtclass = *mtclass;
 	td->inputmode = -1;
 	td->last_mt_collection = -1;
 	hid_set_drvdata(hdev, td);
@@ -574,6 +623,8 @@ static int mt_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		goto fail;
 	}
 
+	ret = sysfs_create_group(&hdev->dev.kobj, &mt_attribute_group);
+
 	mt_set_input_mode(hdev);
 
 	return 0;
@@ -594,6 +645,7 @@ static int mt_reset_resume(struct hid_device *hdev)
 static void mt_remove(struct hid_device *hdev)
 {
 	struct mt_device *td = hid_get_drvdata(hdev);
+	sysfs_remove_group(&hdev->dev.kobj, &mt_attribute_group);
 	hid_hw_stop(hdev);
 	kfree(td->slots);
 	kfree(td);
@@ -609,12 +661,20 @@ static const struct hid_device_id mt_devices[] = {
 	{ .driver_data = MT_CLS_3M,
 		HID_USB_DEVICE(USB_VENDOR_ID_3M,
 			USB_DEVICE_ID_3M2256) },
+	{ .driver_data = MT_CLS_3M,
+		HID_USB_DEVICE(USB_VENDOR_ID_3M,
+			USB_DEVICE_ID_3M3266) },
 
 	/* ActionStar panels */
 	{ .driver_data = MT_CLS_DEFAULT,
 		HID_USB_DEVICE(USB_VENDOR_ID_ACTIONSTAR,
 			USB_DEVICE_ID_ACTIONSTAR_1011) },
 
+	/* Atmel panels */
+	{ .driver_data = MT_CLS_SERIAL,
+		HID_USB_DEVICE(USB_VENDOR_ID_ATMEL,
+			USB_DEVICE_ID_ATMEL_MULTITOUCH) },
+
 	/* Cando panels */
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTNUMBER,
 		HID_USB_DEVICE(USB_VENDOR_ID_CANDO,
@@ -645,23 +705,32 @@ static const struct hid_device_id mt_devices[] = {
 			USB_DEVICE_ID_CYPRESS_TRUETOUCH) },
 
 	/* eGalax devices (resistive) */
-	{  .driver_data = MT_CLS_EGALAX,
+	{ .driver_data = MT_CLS_EGALAX,
 		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
-			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH) },
-	{  .driver_data = MT_CLS_EGALAX,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480D) },
+	{ .driver_data = MT_CLS_EGALAX,
 		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
-			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH3) },
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_480E) },
 
 	/* eGalax devices (capacitive) */
-	{  .driver_data = MT_CLS_EGALAX,
+	{ .driver_data = MT_CLS_EGALAX,
 		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
-			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH1) },
-	{  .driver_data = MT_CLS_EGALAX,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_720C) },
+	{ .driver_data = MT_CLS_EGALAX,
 		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
-			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH2) },
-	{  .driver_data = MT_CLS_EGALAX,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_726B) },
+	{ .driver_data = MT_CLS_EGALAX,
 		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
-			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH4) },
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72A1) },
+	{ .driver_data = MT_CLS_EGALAX,
+		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_72FA) },
+	{ .driver_data = MT_CLS_EGALAX,
+		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_7302) },
+	{ .driver_data = MT_CLS_EGALAX_SERIAL,
+		HID_USB_DEVICE(USB_VENDOR_ID_DWAV,
+			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001) },
 
 	/* Elo TouchSystems IntelliTouch Plus panel */
 	{ .driver_data = MT_CLS_DUAL_NSMU_CONTACTID,
@@ -678,6 +747,11 @@ static const struct hid_device_id mt_devices[] = {
 		HID_USB_DEVICE(USB_VENDOR_ID_GOODTOUCH,
 			USB_DEVICE_ID_GOODTOUCH_000f) },
 
+	/* Hanvon panels */
+	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
+		HID_USB_DEVICE(USB_VENDOR_ID_HANVON_ALT,
+			USB_DEVICE_ID_HANVON_ALT_MULTITOUCH) },
+
 	/* Ideacom panel */
 	{ .driver_data = MT_CLS_SERIAL,
 		HID_USB_DEVICE(USB_VENDOR_ID_IDEACOM,
@@ -722,6 +796,17 @@ static const struct hid_device_id mt_devices[] = {
 		HID_USB_DEVICE(USB_VENDOR_ID_PENMOUNT,
 			USB_DEVICE_ID_PENMOUNT_PCI) },
 
+	/* PixArt optical touch screen */
+	{ .driver_data = MT_CLS_INRANGE_CONTACTNUMBER,
+		HID_USB_DEVICE(USB_VENDOR_ID_PIXART,
+			USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN) },
+	{ .driver_data = MT_CLS_INRANGE_CONTACTNUMBER,
+		HID_USB_DEVICE(USB_VENDOR_ID_PIXART,
+			USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1) },
+	{ .driver_data = MT_CLS_INRANGE_CONTACTNUMBER,
+		HID_USB_DEVICE(USB_VENDOR_ID_PIXART,
+			USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2) },
+
 	/* PixCir-based panels */
 	{ .driver_data = MT_CLS_DUAL_INRANGE_CONTACTID,
 		HID_USB_DEVICE(USB_VENDOR_ID_HANVON,
@@ -730,6 +815,17 @@ static const struct hid_device_id mt_devices[] = {
 		HID_USB_DEVICE(USB_VENDOR_ID_CANDO,
 			USB_DEVICE_ID_CANDO_PIXCIR_MULTI_TOUCH) },
 
+	/* Quanta-based panels */
+	{ .driver_data = MT_CLS_CONFIDENCE_CONTACT_ID,
+		HID_USB_DEVICE(USB_VENDOR_ID_QUANTA,
+			USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH) },
+	{ .driver_data = MT_CLS_CONFIDENCE_CONTACT_ID,
+		HID_USB_DEVICE(USB_VENDOR_ID_QUANTA,
+			USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3001) },
+	{ .driver_data = MT_CLS_CONFIDENCE_CONTACT_ID,
+		HID_USB_DEVICE(USB_VENDOR_ID_QUANTA,
+			USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH_3008) },
+
 	/* Stantum panels */
 	{ .driver_data = MT_CLS_CONFIDENCE,
 		HID_USB_DEVICE(USB_VENDOR_ID_STANTUM,
@@ -758,6 +854,35 @@ static const struct hid_device_id mt_devices[] = {
 		HID_USB_DEVICE(USB_VENDOR_ID_XAT,
 			USB_DEVICE_ID_XAT_CSR) },
 
+	/* Xiroku */
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_SPX) },
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_MPX) },
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_CSR) },
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_SPX1) },
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_MPX1) },
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_CSR1) },
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_SPX2) },
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_MPX2) },
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_XIROKU,
+			USB_DEVICE_ID_XIROKU_CSR2) },
+
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, mt_devices);
diff --git a/drivers/hid/hid-picolcd.c b/drivers/hid/hid-picolcd.c
index 01e7d2cd7c26..12f9777c385d 100644
--- a/drivers/hid/hid-picolcd.c
+++ b/drivers/hid/hid-picolcd.c
@@ -633,7 +633,7 @@ struct picolcd_fb_cleanup_item {
 	struct picolcd_fb_cleanup_item *next;
 };
 static struct picolcd_fb_cleanup_item *fb_pending;
-DEFINE_SPINLOCK(fb_pending_lock);
+static DEFINE_SPINLOCK(fb_pending_lock);
 
 static void picolcd_fb_do_cleanup(struct work_struct *data)
 {
@@ -658,7 +658,7 @@ static void picolcd_fb_do_cleanup(struct work_struct *data)
 	} while (item);
 }
 
-DECLARE_WORK(picolcd_fb_cleanup, picolcd_fb_do_cleanup);
+static DECLARE_WORK(picolcd_fb_cleanup, picolcd_fb_do_cleanup);
 
 static int picolcd_fb_open(struct fb_info *info, int u)
 {
diff --git a/drivers/hid/hid-quanta.c b/drivers/hid/hid-quanta.c
deleted file mode 100644
index 87a54df4d4ac..000000000000
--- a/drivers/hid/hid-quanta.c
+++ /dev/null
@@ -1,261 +0,0 @@
-/*
- *  HID driver for Quanta Optical Touch dual-touch panels
- *
- *  Copyright (c) 2009-2010 Stephane Chatty <chatty@enac.fr>
- *
- */
-
-/*
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the Free
- * Software Foundation; either version 2 of the License, or (at your option)
- * any later version.
- */
-
-#include <linux/device.h>
-#include <linux/hid.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-
-MODULE_AUTHOR("Stephane Chatty <chatty@enac.fr>");
-MODULE_DESCRIPTION("Quanta dual-touch panel");
-MODULE_LICENSE("GPL");
-
-#include "hid-ids.h"
-
-struct quanta_data {
-	__u16 x, y;
-	__u8 id;
-	bool valid;		/* valid finger data, or just placeholder? */
-	bool first;		/* is this the first finger in this frame? */
-	bool activity_now;	/* at least one active finger in this frame? */
-	bool activity;		/* at least one active finger previously? */
-};
-
-static int quanta_input_mapping(struct hid_device *hdev, struct hid_input *hi,
-		struct hid_field *field, struct hid_usage *usage,
-		unsigned long **bit, int *max)
-{
-	switch (usage->hid & HID_USAGE_PAGE) {
-
-	case HID_UP_GENDESK:
-		switch (usage->hid) {
-		case HID_GD_X:
-			hid_map_usage(hi, usage, bit, max,
-					EV_ABS, ABS_MT_POSITION_X);
-			/* touchscreen emulation */
-			input_set_abs_params(hi->input, ABS_X,
-						field->logical_minimum,
-						field->logical_maximum, 0, 0);
-			return 1;
-		case HID_GD_Y:
-			hid_map_usage(hi, usage, bit, max,
-					EV_ABS, ABS_MT_POSITION_Y);
-			/* touchscreen emulation */
-			input_set_abs_params(hi->input, ABS_Y,
-						field->logical_minimum,
-						field->logical_maximum, 0, 0);
-			return 1;
-		}
-		return 0;
-
-	case HID_UP_DIGITIZER:
-		switch (usage->hid) {
-		case HID_DG_CONFIDENCE:
-		case HID_DG_TIPSWITCH:
-		case HID_DG_INPUTMODE:
-		case HID_DG_DEVICEINDEX:
-		case HID_DG_CONTACTCOUNT:
-		case HID_DG_CONTACTMAX:
-		case HID_DG_TIPPRESSURE:
-		case HID_DG_WIDTH:
-		case HID_DG_HEIGHT:
-			return -1;
-		case HID_DG_INRANGE:
-			/* touchscreen emulation */
-			hid_map_usage(hi, usage, bit, max, EV_KEY, BTN_TOUCH);
-			return 1;
-		case HID_DG_CONTACTID:
-			hid_map_usage(hi, usage, bit, max,
-					EV_ABS, ABS_MT_TRACKING_ID);
-			return 1;
-		}
-		return 0;
-
-	case 0xff000000:
-		/* ignore vendor-specific features */
-		return -1;
-	}
-
-	return 0;
-}
-
-static int quanta_input_mapped(struct hid_device *hdev, struct hid_input *hi,
-		struct hid_field *field, struct hid_usage *usage,
-		unsigned long **bit, int *max)
-{
-	if (usage->type == EV_KEY || usage->type == EV_ABS)
-		clear_bit(usage->code, *bit);
-
-	return 0;
-}
-
-/*
- * this function is called when a whole finger has been parsed,
- * so that it can decide what to send to the input layer.
- */
-static void quanta_filter_event(struct quanta_data *td, struct input_dev *input)
-{
-	
-	td->first = !td->first; /* touchscreen emulation */
-
-	if (!td->valid) {
-		/*
-		 * touchscreen emulation: if no finger in this frame is valid
-		 * and there previously was finger activity, this is a release
-		 */ 
-		if (!td->first && !td->activity_now && td->activity) {
-			input_event(input, EV_KEY, BTN_TOUCH, 0);
-			td->activity = false;
-		}
-		return;
-	}
-
-	input_event(input, EV_ABS, ABS_MT_TRACKING_ID, td->id);
-	input_event(input, EV_ABS, ABS_MT_POSITION_X, td->x);
-	input_event(input, EV_ABS, ABS_MT_POSITION_Y, td->y);
-
-	input_mt_sync(input);
-	td->valid = false;
-
-	/* touchscreen emulation: if first active finger in this frame... */
-	if (!td->activity_now) {
-		/* if there was no previous activity, emit touch event */
-		if (!td->activity) {
-			input_event(input, EV_KEY, BTN_TOUCH, 1);
-			td->activity = true;
-		}
-		td->activity_now = true;
-		/* and in any case this is our preferred finger */
-		input_event(input, EV_ABS, ABS_X, td->x);
-		input_event(input, EV_ABS, ABS_Y, td->y);
-	}
-}
-
-
-static int quanta_event(struct hid_device *hid, struct hid_field *field,
-				struct hid_usage *usage, __s32 value)
-{
-	struct quanta_data *td = hid_get_drvdata(hid);
-
-	if (hid->claimed & HID_CLAIMED_INPUT) {
-		struct input_dev *input = field->hidinput->input;
-
-		switch (usage->hid) {
-		case HID_DG_INRANGE:
-			td->valid = !!value;
-			break;
-		case HID_GD_X:
-			td->x = value;
-			break;
-		case HID_GD_Y:
-			td->y = value;
-			quanta_filter_event(td, input);
-			break;
-		case HID_DG_CONTACTID:
-			td->id = value;
-			break;
-		case HID_DG_CONTACTCOUNT:
-			/* touch emulation: this is the last field in a frame */
-			td->first = false;
-			td->activity_now = false;
-			break;
-		case HID_DG_CONFIDENCE:
-		case HID_DG_TIPSWITCH:
-			/* avoid interference from generic hidinput handling */
-			break;
-
-		default:
-			/* fallback to the generic hidinput handling */
-			return 0;
-		}
-	}
-
-	/* we have handled the hidinput part, now remains hiddev */
-	if (hid->claimed & HID_CLAIMED_HIDDEV && hid->hiddev_hid_event)
-		hid->hiddev_hid_event(hid, field, usage, value);
-
-	return 1;
-}
-
-static int quanta_probe(struct hid_device *hdev, const struct hid_device_id *id)
-{
-	int ret;
-	struct quanta_data *td;
-
-	td = kmalloc(sizeof(struct quanta_data), GFP_KERNEL);
-	if (!td) {
-		hid_err(hdev, "cannot allocate Quanta Touch data\n");
-		return -ENOMEM;
-	}
-	td->valid = false;
-	td->activity = false;
-	td->activity_now = false;
-	td->first = false;
-	hid_set_drvdata(hdev, td);
-
-	ret = hid_parse(hdev);
-	if (!ret)
-		ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
-
-	if (ret)
-		kfree(td);
-
-	return ret;
-}
-
-static void quanta_remove(struct hid_device *hdev)
-{
-	hid_hw_stop(hdev);
-	kfree(hid_get_drvdata(hdev));
-	hid_set_drvdata(hdev, NULL);
-}
-
-static const struct hid_device_id quanta_devices[] = {
-	{ HID_USB_DEVICE(USB_VENDOR_ID_QUANTA,
-			USB_DEVICE_ID_QUANTA_OPTICAL_TOUCH) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_QUANTA,
-			USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN) },
-	{ }
-};
-MODULE_DEVICE_TABLE(hid, quanta_devices);
-
-static const struct hid_usage_id quanta_grabbed_usages[] = {
-	{ HID_ANY_ID, HID_ANY_ID, HID_ANY_ID },
-	{ HID_ANY_ID - 1, HID_ANY_ID - 1, HID_ANY_ID - 1}
-};
-
-static struct hid_driver quanta_driver = {
-	.name = "quanta-touch",
-	.id_table = quanta_devices,
-	.probe = quanta_probe,
-	.remove = quanta_remove,
-	.input_mapping = quanta_input_mapping,
-	.input_mapped = quanta_input_mapped,
-	.usage_table = quanta_grabbed_usages,
-	.event = quanta_event,
-};
-
-static int __init quanta_init(void)
-{
-	return hid_register_driver(&quanta_driver);
-}
-
-static void __exit quanta_exit(void)
-{
-	hid_unregister_driver(&quanta_driver);
-}
-
-module_init(quanta_init);
-module_exit(quanta_exit);
-
diff --git a/drivers/hid/hid-roccat-common.c b/drivers/hid/hid-roccat-common.c
index b07e7f96a358..a6d93992c75a 100644
--- a/drivers/hid/hid-roccat-common.c
+++ b/drivers/hid/hid-roccat-common.c
@@ -49,12 +49,10 @@ int roccat_common_send(struct usb_device *usb_dev, uint report_id,
 	char *buf;
 	int len;
 
-	buf = kmalloc(size, GFP_KERNEL);
+	buf = kmemdup(data, size, GFP_KERNEL);
 	if (buf == NULL)
 		return -ENOMEM;
 
-	memcpy(buf, data, size);
-
 	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
 			HID_REQ_SET_REPORT,
 			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
diff --git a/drivers/hid/hid-roccat-isku.c b/drivers/hid/hid-roccat-isku.c
new file mode 100644
index 000000000000..0e4a0ab47142
--- /dev/null
+++ b/drivers/hid/hid-roccat-isku.c
@@ -0,0 +1,487 @@
+/*
+ * Roccat Isku driver for Linux
+ *
+ * Copyright (c) 2011 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+/*
+ * Roccat Isku is a gamer keyboard with macro keys that can be configured in
+ * 5 profiles.
+ */
+
+#include <linux/device.h>
+#include <linux/input.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/hid-roccat.h>
+#include "hid-ids.h"
+#include "hid-roccat-common.h"
+#include "hid-roccat-isku.h"
+
+static struct class *isku_class;
+
+static void isku_profile_activated(struct isku_device *isku, uint new_profile)
+{
+	isku->actual_profile = new_profile;
+}
+
+static int isku_receive(struct usb_device *usb_dev, uint command,
+		void *buf, uint size)
+{
+	return roccat_common_receive(usb_dev, command, buf, size);
+}
+
+static int isku_receive_control_status(struct usb_device *usb_dev)
+{
+	int retval;
+	struct isku_control control;
+
+	do {
+		msleep(50);
+		retval = isku_receive(usb_dev, ISKU_COMMAND_CONTROL,
+				&control, sizeof(struct isku_control));
+
+		if (retval)
+			return retval;
+
+		switch (control.value) {
+		case ISKU_CONTROL_VALUE_STATUS_OK:
+			return 0;
+		case ISKU_CONTROL_VALUE_STATUS_WAIT:
+			continue;
+		case ISKU_CONTROL_VALUE_STATUS_INVALID:
+		/* seems to be critical - replug necessary */
+		case ISKU_CONTROL_VALUE_STATUS_OVERLOAD:
+			return -EINVAL;
+		default:
+			hid_err(usb_dev, "isku_receive_control_status: "
+					"unknown response value 0x%x\n",
+					control.value);
+			return -EINVAL;
+		}
+
+	} while (1);
+}
+
+static int isku_send(struct usb_device *usb_dev, uint command,
+		void const *buf, uint size)
+{
+	int retval;
+
+	retval = roccat_common_send(usb_dev, command, buf, size);
+	if (retval)
+		return retval;
+
+	return isku_receive_control_status(usb_dev);
+}
+
+static int isku_get_actual_profile(struct usb_device *usb_dev)
+{
+	struct isku_actual_profile buf;
+	int retval;
+
+	retval = isku_receive(usb_dev, ISKU_COMMAND_ACTUAL_PROFILE,
+			&buf, sizeof(struct isku_actual_profile));
+	return retval ? retval : buf.actual_profile;
+}
+
+static int isku_set_actual_profile(struct usb_device *usb_dev, int new_profile)
+{
+	struct isku_actual_profile buf;
+
+	buf.command = ISKU_COMMAND_ACTUAL_PROFILE;
+	buf.size = sizeof(struct isku_actual_profile);
+	buf.actual_profile = new_profile;
+	return isku_send(usb_dev, ISKU_COMMAND_ACTUAL_PROFILE, &buf,
+			sizeof(struct isku_actual_profile));
+}
+
+static ssize_t isku_sysfs_show_actual_profile(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct isku_device *isku =
+			hid_get_drvdata(dev_get_drvdata(dev->parent->parent));
+	return snprintf(buf, PAGE_SIZE, "%d\n", isku->actual_profile);
+}
+
+static ssize_t isku_sysfs_set_actual_profile(struct device *dev,
+		struct device_attribute *attr, char const *buf, size_t size)
+{
+	struct isku_device *isku;
+	struct usb_device *usb_dev;
+	unsigned long profile;
+	int retval;
+	struct isku_roccat_report roccat_report;
+
+	dev = dev->parent->parent;
+	isku = hid_get_drvdata(dev_get_drvdata(dev));
+	usb_dev = interface_to_usbdev(to_usb_interface(dev));
+
+	retval = strict_strtoul(buf, 10, &profile);
+	if (retval)
+		return retval;
+
+	if (profile > 4)
+		return -EINVAL;
+
+	mutex_lock(&isku->isku_lock);
+
+	retval = isku_set_actual_profile(usb_dev, profile);
+	if (retval) {
+		mutex_unlock(&isku->isku_lock);
+		return retval;
+	}
+
+	isku_profile_activated(isku, profile);
+
+	roccat_report.event = ISKU_REPORT_BUTTON_EVENT_PROFILE;
+	roccat_report.data1 = profile + 1;
+	roccat_report.data2 = 0;
+	roccat_report.profile = profile + 1;
+	roccat_report_event(isku->chrdev_minor, (uint8_t const *)&roccat_report);
+
+	mutex_unlock(&isku->isku_lock);
+
+	return size;
+}
+
+static struct device_attribute isku_attributes[] = {
+	__ATTR(actual_profile, 0660,
+			isku_sysfs_show_actual_profile,
+			isku_sysfs_set_actual_profile),
+	__ATTR_NULL
+};
+
+static ssize_t isku_sysfs_read(struct file *fp, struct kobject *kobj,
+		char *buf, loff_t off, size_t count,
+		size_t real_size, uint command)
+{
+	struct device *dev =
+			container_of(kobj, struct device, kobj)->parent->parent;
+	struct isku_device *isku = hid_get_drvdata(dev_get_drvdata(dev));
+	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
+	int retval;
+
+	if (off >= real_size)
+		return 0;
+
+	if (off != 0 || count != real_size)
+		return -EINVAL;
+
+	mutex_lock(&isku->isku_lock);
+	retval = isku_receive(usb_dev, command, buf, real_size);
+	mutex_unlock(&isku->isku_lock);
+
+	return retval ? retval : real_size;
+}
+
+static ssize_t isku_sysfs_write(struct file *fp, struct kobject *kobj,
+		void const *buf, loff_t off, size_t count,
+		size_t real_size, uint command)
+{
+	struct device *dev =
+			container_of(kobj, struct device, kobj)->parent->parent;
+	struct isku_device *isku = hid_get_drvdata(dev_get_drvdata(dev));
+	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
+	int retval;
+
+	if (off != 0 || count != real_size)
+		return -EINVAL;
+
+	mutex_lock(&isku->isku_lock);
+	retval = isku_send(usb_dev, command, (void *)buf, real_size);
+	mutex_unlock(&isku->isku_lock);
+
+	return retval ? retval : real_size;
+}
+
+#define ISKU_SYSFS_W(thingy, THINGY) \
+static ssize_t isku_sysfs_write_ ## thingy(struct file *fp, struct kobject *kobj, \
+		struct bin_attribute *attr, char *buf, \
+		loff_t off, size_t count) \
+{ \
+	return isku_sysfs_write(fp, kobj, buf, off, count, \
+			sizeof(struct isku_ ## thingy), ISKU_COMMAND_ ## THINGY); \
+}
+
+#define ISKU_SYSFS_R(thingy, THINGY) \
+static ssize_t isku_sysfs_read_ ## thingy(struct file *fp, struct kobject *kobj, \
+		struct bin_attribute *attr, char *buf, \
+		loff_t off, size_t count) \
+{ \
+	return isku_sysfs_read(fp, kobj, buf, off, count, \
+			sizeof(struct isku_ ## thingy), ISKU_COMMAND_ ## THINGY); \
+}
+
+#define ISKU_SYSFS_RW(thingy, THINGY) \
+ISKU_SYSFS_R(thingy, THINGY) \
+ISKU_SYSFS_W(thingy, THINGY)
+
+#define ISKU_BIN_ATTR_RW(thingy) \
+{ \
+	.attr = { .name = #thingy, .mode = 0660 }, \
+	.size = sizeof(struct isku_ ## thingy), \
+	.read = isku_sysfs_read_ ## thingy, \
+	.write = isku_sysfs_write_ ## thingy \
+}
+
+#define ISKU_BIN_ATTR_R(thingy) \
+{ \
+	.attr = { .name = #thingy, .mode = 0440 }, \
+	.size = sizeof(struct isku_ ## thingy), \
+	.read = isku_sysfs_read_ ## thingy, \
+}
+
+#define ISKU_BIN_ATTR_W(thingy) \
+{ \
+	.attr = { .name = #thingy, .mode = 0220 }, \
+	.size = sizeof(struct isku_ ## thingy), \
+	.write = isku_sysfs_write_ ## thingy \
+}
+
+ISKU_SYSFS_RW(macro, MACRO)
+ISKU_SYSFS_RW(keys_function, KEYS_FUNCTION)
+ISKU_SYSFS_RW(keys_easyzone, KEYS_EASYZONE)
+ISKU_SYSFS_RW(keys_media, KEYS_MEDIA)
+ISKU_SYSFS_RW(keys_thumbster, KEYS_THUMBSTER)
+ISKU_SYSFS_RW(keys_macro, KEYS_MACRO)
+ISKU_SYSFS_RW(keys_capslock, KEYS_CAPSLOCK)
+ISKU_SYSFS_RW(light, LIGHT)
+ISKU_SYSFS_RW(key_mask, KEY_MASK)
+ISKU_SYSFS_RW(last_set, LAST_SET)
+ISKU_SYSFS_W(talk, TALK)
+ISKU_SYSFS_R(info, INFO)
+ISKU_SYSFS_W(control, CONTROL)
+
+static struct bin_attribute isku_bin_attributes[] = {
+	ISKU_BIN_ATTR_RW(macro),
+	ISKU_BIN_ATTR_RW(keys_function),
+	ISKU_BIN_ATTR_RW(keys_easyzone),
+	ISKU_BIN_ATTR_RW(keys_media),
+	ISKU_BIN_ATTR_RW(keys_thumbster),
+	ISKU_BIN_ATTR_RW(keys_macro),
+	ISKU_BIN_ATTR_RW(keys_capslock),
+	ISKU_BIN_ATTR_RW(light),
+	ISKU_BIN_ATTR_RW(key_mask),
+	ISKU_BIN_ATTR_RW(last_set),
+	ISKU_BIN_ATTR_W(talk),
+	ISKU_BIN_ATTR_R(info),
+	ISKU_BIN_ATTR_W(control),
+	__ATTR_NULL
+};
+
+static int isku_init_isku_device_struct(struct usb_device *usb_dev,
+		struct isku_device *isku)
+{
+	int retval;
+
+	mutex_init(&isku->isku_lock);
+
+	retval = isku_get_actual_profile(usb_dev);
+	if (retval < 0)
+		return retval;
+	isku_profile_activated(isku, retval);
+
+	return 0;
+}
+
+static int isku_init_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct usb_device *usb_dev = interface_to_usbdev(intf);
+	struct isku_device *isku;
+	int retval;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= ISKU_USB_INTERFACE_PROTOCOL) {
+		hid_set_drvdata(hdev, NULL);
+		return 0;
+	}
+
+	isku = kzalloc(sizeof(*isku), GFP_KERNEL);
+	if (!isku) {
+		hid_err(hdev, "can't alloc device descriptor\n");
+		return -ENOMEM;
+	}
+	hid_set_drvdata(hdev, isku);
+
+	retval = isku_init_isku_device_struct(usb_dev, isku);
+	if (retval) {
+		hid_err(hdev, "couldn't init struct isku_device\n");
+		goto exit_free;
+	}
+
+	retval = roccat_connect(isku_class, hdev,
+			sizeof(struct isku_roccat_report));
+	if (retval < 0) {
+		hid_err(hdev, "couldn't init char dev\n");
+	} else {
+		isku->chrdev_minor = retval;
+		isku->roccat_claimed = 1;
+	}
+
+	return 0;
+exit_free:
+	kfree(isku);
+	return retval;
+}
+
+static void isku_remove_specials(struct hid_device *hdev)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct isku_device *isku;
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= ISKU_USB_INTERFACE_PROTOCOL)
+		return;
+
+	isku = hid_get_drvdata(hdev);
+	if (isku->roccat_claimed)
+		roccat_disconnect(isku->chrdev_minor);
+	kfree(isku);
+}
+
+static int isku_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	int retval;
+
+	retval = hid_parse(hdev);
+	if (retval) {
+		hid_err(hdev, "parse failed\n");
+		goto exit;
+	}
+
+	retval = hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+	if (retval) {
+		hid_err(hdev, "hw start failed\n");
+		goto exit;
+	}
+
+	retval = isku_init_specials(hdev);
+	if (retval) {
+		hid_err(hdev, "couldn't install keyboard\n");
+		goto exit_stop;
+	}
+
+	return 0;
+
+exit_stop:
+	hid_hw_stop(hdev);
+exit:
+	return retval;
+}
+
+static void isku_remove(struct hid_device *hdev)
+{
+	isku_remove_specials(hdev);
+	hid_hw_stop(hdev);
+}
+
+static void isku_keep_values_up_to_date(struct isku_device *isku,
+		u8 const *data)
+{
+	struct isku_report_button const *button_report;
+
+	switch (data[0]) {
+	case ISKU_REPORT_NUMBER_BUTTON:
+		button_report = (struct isku_report_button const *)data;
+		switch (button_report->event) {
+		case ISKU_REPORT_BUTTON_EVENT_PROFILE:
+			isku_profile_activated(isku, button_report->data1 - 1);
+			break;
+		}
+		break;
+	}
+}
+
+static void isku_report_to_chrdev(struct isku_device const *isku,
+		u8 const *data)
+{
+	struct isku_roccat_report roccat_report;
+	struct isku_report_button const *button_report;
+
+	if (data[0] != ISKU_REPORT_NUMBER_BUTTON)
+		return;
+
+	button_report = (struct isku_report_button const *)data;
+
+	roccat_report.event = button_report->event;
+	roccat_report.data1 = button_report->data1;
+	roccat_report.data2 = button_report->data2;
+	roccat_report.profile = isku->actual_profile + 1;
+	roccat_report_event(isku->chrdev_minor,
+			(uint8_t const *)&roccat_report);
+}
+
+static int isku_raw_event(struct hid_device *hdev,
+		struct hid_report *report, u8 *data, int size)
+{
+	struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+	struct isku_device *isku = hid_get_drvdata(hdev);
+
+	if (intf->cur_altsetting->desc.bInterfaceProtocol
+			!= ISKU_USB_INTERFACE_PROTOCOL)
+		return 0;
+
+	if (isku == NULL)
+		return 0;
+
+	isku_keep_values_up_to_date(isku, data);
+
+	if (isku->roccat_claimed)
+		isku_report_to_chrdev(isku, data);
+
+	return 0;
+}
+
+static const struct hid_device_id isku_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ROCCAT, USB_DEVICE_ID_ROCCAT_ISKU) },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(hid, isku_devices);
+
+static struct hid_driver isku_driver = {
+		.name = "isku",
+		.id_table = isku_devices,
+		.probe = isku_probe,
+		.remove = isku_remove,
+		.raw_event = isku_raw_event
+};
+
+static int __init isku_init(void)
+{
+	int retval;
+	isku_class = class_create(THIS_MODULE, "isku");
+	if (IS_ERR(isku_class))
+		return PTR_ERR(isku_class);
+	isku_class->dev_attrs = isku_attributes;
+	isku_class->dev_bin_attrs = isku_bin_attributes;
+
+	retval = hid_register_driver(&isku_driver);
+	if (retval)
+		class_destroy(isku_class);
+	return retval;
+}
+
+static void __exit isku_exit(void)
+{
+	hid_unregister_driver(&isku_driver);
+	class_destroy(isku_class);
+}
+
+module_init(isku_init);
+module_exit(isku_exit);
+
+MODULE_AUTHOR("Stefan Achatz");
+MODULE_DESCRIPTION("USB Roccat Isku driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-roccat-isku.h b/drivers/hid/hid-roccat-isku.h
new file mode 100644
index 000000000000..075f6efaec58
--- /dev/null
+++ b/drivers/hid/hid-roccat-isku.h
@@ -0,0 +1,147 @@
+#ifndef __HID_ROCCAT_ISKU_H
+#define __HID_ROCCAT_ISKU_H
+
+/*
+ * Copyright (c) 2011 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/types.h>
+
+enum {
+	ISKU_PROFILE_NUM = 5,
+	ISKU_USB_INTERFACE_PROTOCOL = 0,
+};
+
+struct isku_control {
+	uint8_t command; /* ISKU_COMMAND_CONTROL */
+	uint8_t value;
+	uint8_t request;
+} __packed;
+
+enum isku_control_values {
+	ISKU_CONTROL_VALUE_STATUS_OVERLOAD = 0,
+	ISKU_CONTROL_VALUE_STATUS_OK = 1,
+	ISKU_CONTROL_VALUE_STATUS_INVALID = 2,
+	ISKU_CONTROL_VALUE_STATUS_WAIT = 3,
+};
+
+struct isku_actual_profile {
+	uint8_t command; /* ISKU_COMMAND_ACTUAL_PROFILE */
+	uint8_t size; /* always 3 */
+	uint8_t actual_profile;
+} __packed;
+
+struct isku_key_mask {
+	uint8_t command; /* ISKU_COMMAND_KEY_MASK */
+	uint8_t size; /* 6 */
+	uint8_t profile_number; /* 0-4 */
+	uint8_t mask;
+	uint16_t checksum;
+} __packed;
+
+struct isku_keys_function {
+	uint8_t data[0x29];
+} __packed;
+
+struct isku_keys_easyzone {
+	uint8_t data[0x41];
+} __packed;
+
+struct isku_keys_media {
+	uint8_t data[0x1d];
+} __packed;
+
+struct isku_keys_thumbster {
+	uint8_t data[0x17];
+} __packed;
+
+struct isku_keys_macro {
+	uint8_t data[0x23];
+} __packed;
+
+struct isku_keys_capslock {
+	uint8_t data[0x6];
+} __packed;
+
+struct isku_macro {
+	uint8_t data[0x823];
+} __packed;
+
+struct isku_light {
+	uint8_t data[0xa];
+} __packed;
+
+struct isku_info {
+	uint8_t data[2];
+	uint8_t firmware_version;
+	uint8_t unknown[3];
+} __packed;
+
+struct isku_talk {
+	uint8_t data[0x10];
+} __packed;
+
+struct isku_last_set {
+	uint8_t data[0x14];
+} __packed;
+
+enum isku_commands {
+	ISKU_COMMAND_CONTROL = 0x4,
+	ISKU_COMMAND_ACTUAL_PROFILE = 0x5,
+	ISKU_COMMAND_KEY_MASK = 0x7,
+	ISKU_COMMAND_KEYS_FUNCTION = 0x8,
+	ISKU_COMMAND_KEYS_EASYZONE = 0x9,
+	ISKU_COMMAND_KEYS_MEDIA = 0xa,
+	ISKU_COMMAND_KEYS_THUMBSTER = 0xb,
+	ISKU_COMMAND_KEYS_MACRO = 0xd,
+	ISKU_COMMAND_MACRO = 0xe,
+	ISKU_COMMAND_INFO = 0xf,
+	ISKU_COMMAND_LIGHT = 0x10,
+	ISKU_COMMAND_KEYS_CAPSLOCK = 0x13,
+	ISKU_COMMAND_LAST_SET = 0x14,
+	ISKU_COMMAND_15 = 0x15,
+	ISKU_COMMAND_TALK = 0x16,
+	ISKU_COMMAND_FIRMWARE_WRITE = 0x1b,
+	ISKU_COMMAND_FIRMWARE_WRITE_CONTROL = 0x1c,
+};
+
+struct isku_report_button {
+	uint8_t number; /* ISKU_REPORT_NUMBER_BUTTON */
+	uint8_t zero;
+	uint8_t event;
+	uint8_t data1;
+	uint8_t data2;
+};
+
+enum isku_report_numbers {
+	ISKU_REPORT_NUMBER_BUTTON = 3,
+};
+
+enum isku_report_button_events {
+	ISKU_REPORT_BUTTON_EVENT_PROFILE = 0x2,
+};
+
+struct isku_roccat_report {
+	uint8_t event;
+	uint8_t data1;
+	uint8_t data2;
+	uint8_t profile;
+} __packed;
+
+struct isku_device {
+	int roccat_claimed;
+	int chrdev_minor;
+
+	struct mutex isku_lock;
+
+	int actual_profile;
+};
+
+#endif
diff --git a/drivers/hid/hid-roccat-kone.c b/drivers/hid/hid-roccat-kone.c
index e2072afb34bb..40090d602158 100644
--- a/drivers/hid/hid-roccat-kone.c
+++ b/drivers/hid/hid-roccat-kone.c
@@ -78,12 +78,10 @@ static int kone_send(struct usb_device *usb_dev, uint usb_command,
 	char *buf;
 	int len;
 
-	buf = kmalloc(size, GFP_KERNEL);
+	buf = kmemdup(data, size, GFP_KERNEL);
 	if (buf == NULL)
 		return -ENOMEM;
 
-	memcpy(buf, data, size);
-
 	len = usb_control_msg(usb_dev, usb_sndctrlpipe(usb_dev, 0),
 			HID_REQ_SET_REPORT,
 			USB_TYPE_CLASS | USB_RECIP_INTERFACE | USB_DIR_OUT,
diff --git a/drivers/hid/hid-wacom.c b/drivers/hid/hid-wacom.c
index 17bb88f782b6..f2183486a9b6 100644
--- a/drivers/hid/hid-wacom.c
+++ b/drivers/hid/hid-wacom.c
@@ -9,6 +9,7 @@
  *  Copyright (c) 2008 Jiri Slaby <jirislaby@gmail.com>
  *  Copyright (c) 2006 Andrew Zabolotny <zap@homelink.ru>
  *  Copyright (c) 2009 Bastien Nocera <hadess@hadess.net>
+ *  Copyright (c) 2011 Przemysław Firszt <przemo@firszt.eu>
  */
 
 /*
@@ -33,6 +34,7 @@
 struct wacom_data {
 	__u16 tool;
 	unsigned char butstate;
+	__u8 features;
 	unsigned char high_speed;
 #ifdef CONFIG_HID_WACOM_POWER_SUPPLY
 	int battery_capacity;
@@ -107,6 +109,19 @@ static int wacom_ac_get_property(struct power_supply *psy,
 }
 #endif
 
+static void wacom_set_features(struct hid_device *hdev)
+{
+	int ret;
+	__u8 rep_data[2];
+
+	/*set high speed, tablet mode*/
+	rep_data[0] = 0x03;
+	rep_data[1] = 0x20;
+	ret = hdev->hid_output_raw_report(hdev, rep_data, 2,
+				HID_FEATURE_REPORT);
+	return;
+}
+
 static void wacom_poke(struct hid_device *hdev, u8 speed)
 {
 	struct wacom_data *wdata = hid_get_drvdata(hdev);
@@ -177,26 +192,13 @@ static ssize_t wacom_store_speed(struct device *dev,
 static DEVICE_ATTR(speed, S_IRUGO | S_IWUSR | S_IWGRP,
 		wacom_show_speed, wacom_store_speed);
 
-static int wacom_raw_event(struct hid_device *hdev, struct hid_report *report,
-		u8 *raw_data, int size)
+static int wacom_gr_parse_report(struct hid_device *hdev,
+			struct wacom_data *wdata,
+			struct input_dev *input, unsigned char *data)
 {
-	struct wacom_data *wdata = hid_get_drvdata(hdev);
-	struct hid_input *hidinput;
-	struct input_dev *input;
-	unsigned char *data = (unsigned char *) raw_data;
 	int tool, x, y, rw;
 
-	if (!(hdev->claimed & HID_CLAIMED_INPUT))
-		return 0;
-
 	tool = 0;
-	hidinput = list_entry(hdev->inputs.next, struct hid_input, list);
-	input = hidinput->input;
-
-	/* Check if this is a tablet report */
-	if (data[0] != 0x03)
-		return 0;
-
 	/* Get X & Y positions */
 	x = le16_to_cpu(*(__le16 *) &data[2]);
 	y = le16_to_cpu(*(__le16 *) &data[4]);
@@ -304,6 +306,121 @@ static int wacom_raw_event(struct hid_device *hdev, struct hid_report *report,
 	return 1;
 }
 
+static void wacom_i4_parse_pen_report(struct wacom_data *wdata,
+			struct input_dev *input, unsigned char *data)
+{
+	__u16 x, y, pressure;
+	__u32 id;
+
+	switch (data[1]) {
+	case 0x80: /* Out of proximity report */
+		wdata->tool = 0;
+		input_report_key(input, BTN_TOUCH, 0);
+		input_report_abs(input, ABS_PRESSURE, 0);
+		input_report_key(input, wdata->tool, 0);
+		input_sync(input);
+		break;
+	case 0xC2: /* Tool report */
+		id = ((data[2] << 4) | (data[3] >> 4) |
+			((data[7] & 0x0f) << 20) |
+			((data[8] & 0xf0) << 12)) & 0xfffff;
+
+		switch (id) {
+		case 0x802:
+			wdata->tool = BTN_TOOL_PEN;
+			break;
+		case 0x80A:
+			wdata->tool = BTN_TOOL_RUBBER;
+			break;
+		}
+		break;
+	default: /* Position/pressure report */
+		x = data[2] << 9 | data[3] << 1 | ((data[9] & 0x02) >> 1);
+		y = data[4] << 9 | data[5] << 1 | (data[9] & 0x01);
+		pressure = (data[6] << 3) | ((data[7] & 0xC0) >> 5)
+			| (data[1] & 0x01);
+
+		input_report_key(input, BTN_TOUCH, pressure > 1);
+
+		input_report_key(input, BTN_STYLUS, data[1] & 0x02);
+		input_report_key(input, BTN_STYLUS2, data[1] & 0x04);
+		input_report_key(input, wdata->tool, 1);
+		input_report_abs(input, ABS_X, x);
+		input_report_abs(input, ABS_Y, y);
+		input_report_abs(input, ABS_PRESSURE, pressure);
+		input_sync(input);
+		break;
+	}
+
+	return;
+}
+
+static void wacom_i4_parse_report(struct hid_device *hdev,
+			struct wacom_data *wdata,
+			struct input_dev *input, unsigned char *data)
+{
+	switch (data[0]) {
+	case 0x00: /* Empty report */
+		break;
+	case 0x02: /* Pen report */
+		wacom_i4_parse_pen_report(wdata, input, data);
+		break;
+	case 0x03: /* Features Report */
+		wdata->features = data[2];
+		break;
+	case 0x0C: /* Button report */
+		break;
+	default:
+		hid_err(hdev, "Unknown report: %d,%d\n", data[0], data[1]);
+		break;
+	}
+}
+
+static int wacom_raw_event(struct hid_device *hdev, struct hid_report *report,
+		u8 *raw_data, int size)
+{
+	struct wacom_data *wdata = hid_get_drvdata(hdev);
+	struct hid_input *hidinput;
+	struct input_dev *input;
+	unsigned char *data = (unsigned char *) raw_data;
+	int i;
+
+	if (!(hdev->claimed & HID_CLAIMED_INPUT))
+		return 0;
+
+	hidinput = list_entry(hdev->inputs.next, struct hid_input, list);
+	input = hidinput->input;
+
+	/* Check if this is a tablet report */
+	if (data[0] != 0x03)
+		return 0;
+
+	switch (hdev->product) {
+	case USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH:
+		return wacom_gr_parse_report(hdev, wdata, input, data);
+		break;
+	case USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH:
+		i = 1;
+
+		switch (data[0]) {
+		case 0x04:
+			wacom_i4_parse_report(hdev, wdata, input, data + i);
+			i += 10;
+			/* fall through */
+		case 0x03:
+			wacom_i4_parse_report(hdev, wdata, input, data + i);
+			i += 10;
+			wacom_i4_parse_report(hdev, wdata, input, data + i);
+			break;
+		default:
+			hid_err(hdev, "Unknown report: %d,%d size:%d\n",
+					data[0], data[1], size);
+			return 0;
+		}
+	}
+	return 1;
+}
+
 static int wacom_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 	struct hid_field *field, struct hid_usage *usage, unsigned long **bit,
 								int *max)
@@ -338,10 +455,19 @@ static int wacom_input_mapped(struct hid_device *hdev, struct hid_input *hi,
 	__set_bit(BTN_TOOL_RUBBER, input->keybit);
 	__set_bit(BTN_TOOL_MOUSE, input->keybit);
 
-	input_set_abs_params(input, ABS_X, 0, 16704, 4, 0);
-	input_set_abs_params(input, ABS_Y, 0, 12064, 4, 0);
-	input_set_abs_params(input, ABS_PRESSURE, 0, 511, 0, 0);
-	input_set_abs_params(input, ABS_DISTANCE, 0, 32, 0, 0);
+	switch (hdev->product) {
+	case USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH:
+		input_set_abs_params(input, ABS_X, 0, 16704, 4, 0);
+		input_set_abs_params(input, ABS_Y, 0, 12064, 4, 0);
+		input_set_abs_params(input, ABS_PRESSURE, 0, 511, 0, 0);
+		input_set_abs_params(input, ABS_DISTANCE, 0, 32, 0, 0);
+		break;
+	case USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH:
+		input_set_abs_params(input, ABS_X, 0, 40640, 4, 0);
+		input_set_abs_params(input, ABS_Y, 0, 25400, 4, 0);
+		input_set_abs_params(input, ABS_PRESSURE, 0, 2047, 0, 0);
+		break;
+	}
 
 	return 0;
 }
@@ -378,8 +504,16 @@ static int wacom_probe(struct hid_device *hdev,
 		hid_warn(hdev,
 			 "can't create sysfs speed attribute err: %d\n", ret);
 
-	/* Set Wacom mode 2 with high reporting speed */
-	wacom_poke(hdev, 1);
+	switch (hdev->product) {
+	case USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH:
+		/* Set Wacom mode 2 with high reporting speed */
+		wacom_poke(hdev, 1);
+		break;
+	case USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH:
+		wdata->features = 0;
+		wacom_set_features(hdev);
+		break;
+	}
 
 #ifdef CONFIG_HID_WACOM_POWER_SUPPLY
 	wdata->battery.properties = wacom_battery_props;
@@ -441,6 +575,7 @@ static void wacom_remove(struct hid_device *hdev)
 
 static const struct hid_device_id wacom_devices[] = {
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_WACOM, USB_DEVICE_ID_WACOM_GRAPHIRE_BLUETOOTH) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_WACOM, USB_DEVICE_ID_WACOM_INTUOS4_BLUETOOTH) },
 
 	{ }
 };
diff --git a/drivers/hid/hid-wiimote.c b/drivers/hid/hid-wiimote-core.c
index 76739c07fa3c..61881b35c670 100644
--- a/drivers/hid/hid-wiimote.c
+++ b/drivers/hid/hid-wiimote-core.c
@@ -20,91 +20,7 @@
 #include <linux/power_supply.h>
 #include <linux/spinlock.h>
 #include "hid-ids.h"
-
-#define WIIMOTE_VERSION "0.2"
-#define WIIMOTE_NAME "Nintendo Wii Remote"
-#define WIIMOTE_BUFSIZE 32
-
-struct wiimote_buf {
-	__u8 data[HID_MAX_BUFFER_SIZE];
-	size_t size;
-};
-
-struct wiimote_state {
-	spinlock_t lock;
-	__u8 flags;
-	__u8 accel_split[2];
-
-	/* synchronous cmd requests */
-	struct mutex sync;
-	struct completion ready;
-	int cmd;
-	__u32 opt;
-
-	/* results of synchronous requests */
-	__u8 cmd_battery;
-	__u8 cmd_err;
-};
-
-struct wiimote_data {
-	struct hid_device *hdev;
-	struct input_dev *input;
-	struct led_classdev *leds[4];
-	struct input_dev *accel;
-	struct input_dev *ir;
-	struct power_supply battery;
-
-	spinlock_t qlock;
-	__u8 head;
-	__u8 tail;
-	struct wiimote_buf outq[WIIMOTE_BUFSIZE];
-	struct work_struct worker;
-
-	struct wiimote_state state;
-};
-
-#define WIIPROTO_FLAG_LED1		0x01
-#define WIIPROTO_FLAG_LED2		0x02
-#define WIIPROTO_FLAG_LED3		0x04
-#define WIIPROTO_FLAG_LED4		0x08
-#define WIIPROTO_FLAG_RUMBLE		0x10
-#define WIIPROTO_FLAG_ACCEL		0x20
-#define WIIPROTO_FLAG_IR_BASIC		0x40
-#define WIIPROTO_FLAG_IR_EXT		0x80
-#define WIIPROTO_FLAG_IR_FULL		0xc0 /* IR_BASIC | IR_EXT */
-#define WIIPROTO_FLAGS_LEDS (WIIPROTO_FLAG_LED1 | WIIPROTO_FLAG_LED2 | \
-					WIIPROTO_FLAG_LED3 | WIIPROTO_FLAG_LED4)
-#define WIIPROTO_FLAGS_IR (WIIPROTO_FLAG_IR_BASIC | WIIPROTO_FLAG_IR_EXT | \
-							WIIPROTO_FLAG_IR_FULL)
-
-/* return flag for led \num */
-#define WIIPROTO_FLAG_LED(num) (WIIPROTO_FLAG_LED1 << (num - 1))
-
-enum wiiproto_reqs {
-	WIIPROTO_REQ_NULL = 0x0,
-	WIIPROTO_REQ_RUMBLE = 0x10,
-	WIIPROTO_REQ_LED = 0x11,
-	WIIPROTO_REQ_DRM = 0x12,
-	WIIPROTO_REQ_IR1 = 0x13,
-	WIIPROTO_REQ_SREQ = 0x15,
-	WIIPROTO_REQ_WMEM = 0x16,
-	WIIPROTO_REQ_RMEM = 0x17,
-	WIIPROTO_REQ_IR2 = 0x1a,
-	WIIPROTO_REQ_STATUS = 0x20,
-	WIIPROTO_REQ_DATA = 0x21,
-	WIIPROTO_REQ_RETURN = 0x22,
-	WIIPROTO_REQ_DRM_K = 0x30,
-	WIIPROTO_REQ_DRM_KA = 0x31,
-	WIIPROTO_REQ_DRM_KE = 0x32,
-	WIIPROTO_REQ_DRM_KAI = 0x33,
-	WIIPROTO_REQ_DRM_KEE = 0x34,
-	WIIPROTO_REQ_DRM_KAE = 0x35,
-	WIIPROTO_REQ_DRM_KIE = 0x36,
-	WIIPROTO_REQ_DRM_KAIE = 0x37,
-	WIIPROTO_REQ_DRM_E = 0x3d,
-	WIIPROTO_REQ_DRM_SKAI1 = 0x3e,
-	WIIPROTO_REQ_DRM_SKAI2 = 0x3f,
-};
+#include "hid-wiimote.h"
 
 enum wiiproto_keys {
 	WIIPROTO_KEY_LEFT,
@@ -139,52 +55,6 @@ static enum power_supply_property wiimote_battery_props[] = {
 	POWER_SUPPLY_PROP_CAPACITY
 };
 
-/* requires the state.lock spinlock to be held */
-static inline bool wiimote_cmd_pending(struct wiimote_data *wdata, int cmd,
-								__u32 opt)
-{
-	return wdata->state.cmd == cmd && wdata->state.opt == opt;
-}
-
-/* requires the state.lock spinlock to be held */
-static inline void wiimote_cmd_complete(struct wiimote_data *wdata)
-{
-	wdata->state.cmd = WIIPROTO_REQ_NULL;
-	complete(&wdata->state.ready);
-}
-
-static inline int wiimote_cmd_acquire(struct wiimote_data *wdata)
-{
-	return mutex_lock_interruptible(&wdata->state.sync) ? -ERESTARTSYS : 0;
-}
-
-/* requires the state.lock spinlock to be held */
-static inline void wiimote_cmd_set(struct wiimote_data *wdata, int cmd,
-								__u32 opt)
-{
-	INIT_COMPLETION(wdata->state.ready);
-	wdata->state.cmd = cmd;
-	wdata->state.opt = opt;
-}
-
-static inline void wiimote_cmd_release(struct wiimote_data *wdata)
-{
-	mutex_unlock(&wdata->state.sync);
-}
-
-static inline int wiimote_cmd_wait(struct wiimote_data *wdata)
-{
-	int ret;
-
-	ret = wait_for_completion_interruptible_timeout(&wdata->state.ready, HZ);
-	if (ret < 0)
-		return -ERESTARTSYS;
-	else if (ret == 0)
-		return -EIO;
-	else
-		return 0;
-}
-
 static ssize_t wiimote_hid_send(struct hid_device *hdev, __u8 *buffer,
 								size_t count)
 {
@@ -329,6 +199,7 @@ static void wiiproto_req_leds(struct wiimote_data *wdata, int leds)
 static __u8 select_drm(struct wiimote_data *wdata)
 {
 	__u8 ir = wdata->state.flags & WIIPROTO_FLAGS_IR;
+	bool ext = wiiext_active(wdata);
 
 	if (ir == WIIPROTO_FLAG_IR_BASIC) {
 		if (wdata->state.flags & WIIPROTO_FLAG_ACCEL)
@@ -340,14 +211,21 @@ static __u8 select_drm(struct wiimote_data *wdata)
 	} else if (ir == WIIPROTO_FLAG_IR_FULL) {
 		return WIIPROTO_REQ_DRM_SKAI1;
 	} else {
-		if (wdata->state.flags & WIIPROTO_FLAG_ACCEL)
-			return WIIPROTO_REQ_DRM_KA;
-		else
-			return WIIPROTO_REQ_DRM_K;
+		if (wdata->state.flags & WIIPROTO_FLAG_ACCEL) {
+			if (ext)
+				return WIIPROTO_REQ_DRM_KAE;
+			else
+				return WIIPROTO_REQ_DRM_KA;
+		} else {
+			if (ext)
+				return WIIPROTO_REQ_DRM_KE;
+			else
+				return WIIPROTO_REQ_DRM_K;
+		}
 	}
 }
 
-static void wiiproto_req_drm(struct wiimote_data *wdata, __u8 drm)
+void wiiproto_req_drm(struct wiimote_data *wdata, __u8 drm)
 {
 	__u8 cmd[3];
 
@@ -358,6 +236,7 @@ static void wiiproto_req_drm(struct wiimote_data *wdata, __u8 drm)
 	cmd[1] = 0;
 	cmd[2] = drm;
 
+	wdata->state.drm = drm;
 	wiiproto_keep_rumble(wdata, &cmd[1]);
 	wiimote_queue(wdata, cmd, sizeof(cmd));
 }
@@ -440,8 +319,33 @@ static void wiiproto_req_wmem(struct wiimote_data *wdata, bool eeprom,
 	wiimote_queue(wdata, cmd, sizeof(cmd));
 }
 
+void wiiproto_req_rmem(struct wiimote_data *wdata, bool eeprom, __u32 offset,
+								__u16 size)
+{
+	__u8 cmd[7];
+
+	if (size == 0) {
+		hid_warn(wdata->hdev, "Invalid length %d rmem request\n", size);
+		return;
+	}
+
+	cmd[0] = WIIPROTO_REQ_RMEM;
+	cmd[1] = 0;
+	cmd[2] = (offset >> 16) & 0xff;
+	cmd[3] = (offset >> 8) & 0xff;
+	cmd[4] = offset & 0xff;
+	cmd[5] = (size >> 8) & 0xff;
+	cmd[6] = size & 0xff;
+
+	if (!eeprom)
+		cmd[1] |= 0x04;
+
+	wiiproto_keep_rumble(wdata, &cmd[1]);
+	wiimote_queue(wdata, cmd, sizeof(cmd));
+}
+
 /* requries the cmd-mutex to be held */
-static int wiimote_cmd_write(struct wiimote_data *wdata, __u32 offset,
+int wiimote_cmd_write(struct wiimote_data *wdata, __u32 offset,
 						const __u8 *wmem, __u8 size)
 {
 	unsigned long flags;
@@ -459,6 +363,36 @@ static int wiimote_cmd_write(struct wiimote_data *wdata, __u32 offset,
 	return ret;
 }
 
+/* requries the cmd-mutex to be held */
+ssize_t wiimote_cmd_read(struct wiimote_data *wdata, __u32 offset, __u8 *rmem,
+								__u8 size)
+{
+	unsigned long flags;
+	ssize_t ret;
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	wdata->state.cmd_read_size = size;
+	wdata->state.cmd_read_buf = rmem;
+	wiimote_cmd_set(wdata, WIIPROTO_REQ_RMEM, offset & 0xffff);
+	wiiproto_req_rreg(wdata, offset, size);
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	ret = wiimote_cmd_wait(wdata);
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	wdata->state.cmd_read_buf = NULL;
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	if (!ret) {
+		if (wdata->state.cmd_read_size == 0)
+			ret = -EIO;
+		else
+			ret = wdata->state.cmd_read_size;
+	}
+
+	return ret;
+}
+
 static int wiimote_battery_get_property(struct power_supply *psy,
 						enum power_supply_property psp,
 						union power_supply_propval *val)
@@ -862,6 +796,8 @@ static void handler_status(struct wiimote_data *wdata, const __u8 *payload)
 	/* on status reports the drm is reset so we need to resend the drm */
 	wiiproto_req_drm(wdata, WIIPROTO_REQ_NULL);
 
+	wiiext_event(wdata, payload[2] & 0x02);
+
 	if (wiimote_cmd_pending(wdata, WIIPROTO_REQ_SREQ, 0)) {
 		wdata->state.cmd_battery = payload[5];
 		wiimote_cmd_complete(wdata);
@@ -870,7 +806,23 @@ static void handler_status(struct wiimote_data *wdata, const __u8 *payload)
 
 static void handler_data(struct wiimote_data *wdata, const __u8 *payload)
 {
+	__u16 offset = payload[3] << 8 | payload[4];
+	__u8 size = (payload[2] >> 4) + 1;
+	__u8 err = payload[2] & 0x0f;
+
 	handler_keys(wdata, payload);
+
+	if (wiimote_cmd_pending(wdata, WIIPROTO_REQ_RMEM, offset)) {
+		if (err)
+			size = 0;
+		else if (size > wdata->state.cmd_read_size)
+			size = wdata->state.cmd_read_size;
+
+		wdata->state.cmd_read_size = size;
+		if (wdata->state.cmd_read_buf)
+			memcpy(wdata->state.cmd_read_buf, &payload[5], size);
+		wiimote_cmd_complete(wdata);
+	}
 }
 
 static void handler_return(struct wiimote_data *wdata, const __u8 *payload)
@@ -898,6 +850,7 @@ static void handler_drm_KA(struct wiimote_data *wdata, const __u8 *payload)
 static void handler_drm_KE(struct wiimote_data *wdata, const __u8 *payload)
 {
 	handler_keys(wdata, payload);
+	wiiext_handle(wdata, &payload[2]);
 }
 
 static void handler_drm_KAI(struct wiimote_data *wdata, const __u8 *payload)
@@ -914,6 +867,7 @@ static void handler_drm_KAI(struct wiimote_data *wdata, const __u8 *payload)
 static void handler_drm_KEE(struct wiimote_data *wdata, const __u8 *payload)
 {
 	handler_keys(wdata, payload);
+	wiiext_handle(wdata, &payload[2]);
 }
 
 static void handler_drm_KIE(struct wiimote_data *wdata, const __u8 *payload)
@@ -924,12 +878,14 @@ static void handler_drm_KIE(struct wiimote_data *wdata, const __u8 *payload)
 	ir_to_input2(wdata, &payload[7], false);
 	ir_to_input3(wdata, &payload[9], true);
 	input_sync(wdata->ir);
+	wiiext_handle(wdata, &payload[12]);
 }
 
 static void handler_drm_KAE(struct wiimote_data *wdata, const __u8 *payload)
 {
 	handler_keys(wdata, payload);
 	handler_accel(wdata, payload);
+	wiiext_handle(wdata, &payload[5]);
 }
 
 static void handler_drm_KAIE(struct wiimote_data *wdata, const __u8 *payload)
@@ -941,10 +897,12 @@ static void handler_drm_KAIE(struct wiimote_data *wdata, const __u8 *payload)
 	ir_to_input2(wdata, &payload[10], false);
 	ir_to_input3(wdata, &payload[12], true);
 	input_sync(wdata->ir);
+	wiiext_handle(wdata, &payload[15]);
 }
 
 static void handler_drm_E(struct wiimote_data *wdata, const __u8 *payload)
 {
+	wiiext_handle(wdata, payload);
 }
 
 static void handler_drm_SKAI1(struct wiimote_data *wdata, const __u8 *payload)
@@ -1182,6 +1140,7 @@ static struct wiimote_data *wiimote_create(struct hid_device *hdev)
 	spin_lock_init(&wdata->state.lock);
 	init_completion(&wdata->state.ready);
 	mutex_init(&wdata->state.sync);
+	wdata->state.drm = WIIPROTO_REQ_DRM_K;
 
 	return wdata;
 
@@ -1196,6 +1155,8 @@ err:
 
 static void wiimote_destroy(struct wiimote_data *wdata)
 {
+	wiidebug_deinit(wdata);
+	wiiext_deinit(wdata);
 	wiimote_leds_destroy(wdata);
 
 	power_supply_unregister(&wdata->battery);
@@ -1214,6 +1175,8 @@ static int wiimote_hid_probe(struct hid_device *hdev,
 	struct wiimote_data *wdata;
 	int ret;
 
+	hdev->quirks |= HID_QUIRK_NO_INIT_REPORTS;
+
 	wdata = wiimote_create(hdev);
 	if (!wdata) {
 		hid_err(hdev, "Can't alloc device\n");
@@ -1267,6 +1230,14 @@ static int wiimote_hid_probe(struct hid_device *hdev,
 	if (ret)
 		goto err_free;
 
+	ret = wiiext_init(wdata);
+	if (ret)
+		goto err_free;
+
+	ret = wiidebug_init(wdata);
+	if (ret)
+		goto err_free;
+
 	hid_info(hdev, "New device registered\n");
 
 	/* by default set led1 after device initialization */
@@ -1343,4 +1314,3 @@ module_exit(wiimote_exit);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("David Herrmann <dh.herrmann@gmail.com>");
 MODULE_DESCRIPTION(WIIMOTE_NAME " Device Driver");
-MODULE_VERSION(WIIMOTE_VERSION);
diff --git a/drivers/hid/hid-wiimote-debug.c b/drivers/hid/hid-wiimote-debug.c
new file mode 100644
index 000000000000..17dabc1f339e
--- /dev/null
+++ b/drivers/hid/hid-wiimote-debug.c
@@ -0,0 +1,227 @@
+/*
+ * Debug support for HID Nintendo Wiimote devices
+ * Copyright (c) 2011 David Herrmann
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/module.h>
+#include <linux/seq_file.h>
+#include <linux/spinlock.h>
+#include <linux/uaccess.h>
+#include "hid-wiimote.h"
+
+struct wiimote_debug {
+	struct wiimote_data *wdata;
+	struct dentry *eeprom;
+	struct dentry *drm;
+};
+
+static int wiidebug_eeprom_open(struct inode *i, struct file *f)
+{
+	f->private_data = i->i_private;
+	return 0;
+}
+
+static ssize_t wiidebug_eeprom_read(struct file *f, char __user *u, size_t s,
+								loff_t *off)
+{
+	struct wiimote_debug *dbg = f->private_data;
+	struct wiimote_data *wdata = dbg->wdata;
+	unsigned long flags;
+	ssize_t ret;
+	char buf[16];
+	__u16 size;
+
+	if (s == 0)
+		return -EINVAL;
+	if (*off > 0xffffff)
+		return 0;
+	if (s > 16)
+		s = 16;
+
+	ret = wiimote_cmd_acquire(wdata);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	wdata->state.cmd_read_size = s;
+	wdata->state.cmd_read_buf = buf;
+	wiimote_cmd_set(wdata, WIIPROTO_REQ_RMEM, *off & 0xffff);
+	wiiproto_req_reeprom(wdata, *off, s);
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	ret = wiimote_cmd_wait(wdata);
+	if (!ret)
+		size = wdata->state.cmd_read_size;
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	wdata->state.cmd_read_buf = NULL;
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	wiimote_cmd_release(wdata);
+
+	if (ret)
+		return ret;
+	else if (size == 0)
+		return -EIO;
+
+	if (copy_to_user(u, buf, size))
+		return -EFAULT;
+
+	*off += size;
+	ret = size;
+
+	return ret;
+}
+
+static const struct file_operations wiidebug_eeprom_fops = {
+	.owner = THIS_MODULE,
+	.open = wiidebug_eeprom_open,
+	.read = wiidebug_eeprom_read,
+	.llseek = generic_file_llseek,
+};
+
+static const char *wiidebug_drmmap[] = {
+	[WIIPROTO_REQ_NULL] = "NULL",
+	[WIIPROTO_REQ_DRM_K] = "K",
+	[WIIPROTO_REQ_DRM_KA] = "KA",
+	[WIIPROTO_REQ_DRM_KE] = "KE",
+	[WIIPROTO_REQ_DRM_KAI] = "KAI",
+	[WIIPROTO_REQ_DRM_KEE] = "KEE",
+	[WIIPROTO_REQ_DRM_KAE] = "KAE",
+	[WIIPROTO_REQ_DRM_KIE] = "KIE",
+	[WIIPROTO_REQ_DRM_KAIE] = "KAIE",
+	[WIIPROTO_REQ_DRM_E] = "E",
+	[WIIPROTO_REQ_DRM_SKAI1] = "SKAI1",
+	[WIIPROTO_REQ_DRM_SKAI2] = "SKAI2",
+	[WIIPROTO_REQ_MAX] = NULL
+};
+
+static int wiidebug_drm_show(struct seq_file *f, void *p)
+{
+	struct wiimote_debug *dbg = f->private;
+	const char *str = NULL;
+	unsigned long flags;
+	__u8 drm;
+
+	spin_lock_irqsave(&dbg->wdata->state.lock, flags);
+	drm = dbg->wdata->state.drm;
+	spin_unlock_irqrestore(&dbg->wdata->state.lock, flags);
+
+	if (drm < WIIPROTO_REQ_MAX)
+		str = wiidebug_drmmap[drm];
+	if (!str)
+		str = "unknown";
+
+	seq_printf(f, "%s\n", str);
+
+	return 0;
+}
+
+static int wiidebug_drm_open(struct inode *i, struct file *f)
+{
+	return single_open(f, wiidebug_drm_show, i->i_private);
+}
+
+static ssize_t wiidebug_drm_write(struct file *f, const char __user *u,
+							size_t s, loff_t *off)
+{
+	struct wiimote_debug *dbg = f->private_data;
+	unsigned long flags;
+	char buf[16];
+	ssize_t len;
+	int i;
+
+	if (s == 0)
+		return -EINVAL;
+
+	len = min((size_t) 15, s);
+	if (copy_from_user(buf, u, len))
+		return -EFAULT;
+
+	buf[15] = 0;
+
+	for (i = 0; i < WIIPROTO_REQ_MAX; ++i) {
+		if (!wiidebug_drmmap[i])
+			continue;
+		if (!strcasecmp(buf, wiidebug_drmmap[i]))
+			break;
+	}
+
+	if (i == WIIPROTO_REQ_MAX)
+		i = simple_strtoul(buf, NULL, 10);
+
+	spin_lock_irqsave(&dbg->wdata->state.lock, flags);
+	wiiproto_req_drm(dbg->wdata, (__u8) i);
+	spin_unlock_irqrestore(&dbg->wdata->state.lock, flags);
+
+	return len;
+}
+
+static const struct file_operations wiidebug_drm_fops = {
+	.owner = THIS_MODULE,
+	.open = wiidebug_drm_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.write = wiidebug_drm_write,
+	.release = single_release,
+};
+
+int wiidebug_init(struct wiimote_data *wdata)
+{
+	struct wiimote_debug *dbg;
+	unsigned long flags;
+	int ret = -ENOMEM;
+
+	dbg = kzalloc(sizeof(*dbg), GFP_KERNEL);
+	if (!dbg)
+		return -ENOMEM;
+
+	dbg->wdata = wdata;
+
+	dbg->eeprom = debugfs_create_file("eeprom", S_IRUSR,
+		dbg->wdata->hdev->debug_dir, dbg, &wiidebug_eeprom_fops);
+	if (!dbg->eeprom)
+		goto err;
+
+	dbg->drm = debugfs_create_file("drm", S_IRUSR,
+			dbg->wdata->hdev->debug_dir, dbg, &wiidebug_drm_fops);
+	if (!dbg->drm)
+		goto err_drm;
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	wdata->debug = dbg;
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	return 0;
+
+err_drm:
+	debugfs_remove(dbg->eeprom);
+err:
+	kfree(dbg);
+	return ret;
+}
+
+void wiidebug_deinit(struct wiimote_data *wdata)
+{
+	struct wiimote_debug *dbg = wdata->debug;
+	unsigned long flags;
+
+	if (!dbg)
+		return;
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	wdata->debug = NULL;
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	debugfs_remove(dbg->drm);
+	debugfs_remove(dbg->eeprom);
+	kfree(dbg);
+}
diff --git a/drivers/hid/hid-wiimote-ext.c b/drivers/hid/hid-wiimote-ext.c
new file mode 100644
index 000000000000..aa958706c0e5
--- /dev/null
+++ b/drivers/hid/hid-wiimote-ext.c
@@ -0,0 +1,752 @@
+/*
+ * HID driver for Nintendo Wiimote extension devices
+ * Copyright (c) 2011 David Herrmann
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/atomic.h>
+#include <linux/module.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include "hid-wiimote.h"
+
+struct wiimote_ext {
+	struct wiimote_data *wdata;
+	struct work_struct worker;
+	struct input_dev *input;
+	struct input_dev *mp_input;
+
+	atomic_t opened;
+	atomic_t mp_opened;
+	bool plugged;
+	bool mp_plugged;
+	bool motionp;
+	__u8 ext_type;
+};
+
+enum wiiext_type {
+	WIIEXT_NONE,		/* placeholder */
+	WIIEXT_CLASSIC,		/* Nintendo classic controller */
+	WIIEXT_NUNCHUCK,	/* Nintendo nunchuck controller */
+};
+
+enum wiiext_keys {
+	WIIEXT_KEY_C,
+	WIIEXT_KEY_Z,
+	WIIEXT_KEY_A,
+	WIIEXT_KEY_B,
+	WIIEXT_KEY_X,
+	WIIEXT_KEY_Y,
+	WIIEXT_KEY_ZL,
+	WIIEXT_KEY_ZR,
+	WIIEXT_KEY_PLUS,
+	WIIEXT_KEY_MINUS,
+	WIIEXT_KEY_HOME,
+	WIIEXT_KEY_LEFT,
+	WIIEXT_KEY_RIGHT,
+	WIIEXT_KEY_UP,
+	WIIEXT_KEY_DOWN,
+	WIIEXT_KEY_LT,
+	WIIEXT_KEY_RT,
+	WIIEXT_KEY_COUNT
+};
+
+static __u16 wiiext_keymap[] = {
+	BTN_C,		/* WIIEXT_KEY_C */
+	BTN_Z,		/* WIIEXT_KEY_Z */
+	BTN_A,		/* WIIEXT_KEY_A */
+	BTN_B,		/* WIIEXT_KEY_B */
+	BTN_X,		/* WIIEXT_KEY_X */
+	BTN_Y,		/* WIIEXT_KEY_Y */
+	BTN_TL2,	/* WIIEXT_KEY_ZL */
+	BTN_TR2,	/* WIIEXT_KEY_ZR */
+	KEY_NEXT,	/* WIIEXT_KEY_PLUS */
+	KEY_PREVIOUS,	/* WIIEXT_KEY_MINUS */
+	BTN_MODE,	/* WIIEXT_KEY_HOME */
+	KEY_LEFT,	/* WIIEXT_KEY_LEFT */
+	KEY_RIGHT,	/* WIIEXT_KEY_RIGHT */
+	KEY_UP,		/* WIIEXT_KEY_UP */
+	KEY_DOWN,	/* WIIEXT_KEY_DOWN */
+	BTN_TL,		/* WIIEXT_KEY_LT */
+	BTN_TR,		/* WIIEXT_KEY_RT */
+};
+
+/* diable all extensions */
+static void ext_disable(struct wiimote_ext *ext)
+{
+	unsigned long flags;
+	__u8 wmem = 0x55;
+
+	if (!wiimote_cmd_acquire(ext->wdata)) {
+		wiimote_cmd_write(ext->wdata, 0xa400f0, &wmem, sizeof(wmem));
+		wiimote_cmd_release(ext->wdata);
+	}
+
+	spin_lock_irqsave(&ext->wdata->state.lock, flags);
+	ext->motionp = false;
+	ext->ext_type = WIIEXT_NONE;
+	wiiproto_req_drm(ext->wdata, WIIPROTO_REQ_NULL);
+	spin_unlock_irqrestore(&ext->wdata->state.lock, flags);
+}
+
+static bool motionp_read(struct wiimote_ext *ext)
+{
+	__u8 rmem[2], wmem;
+	ssize_t ret;
+	bool avail = false;
+
+	if (!atomic_read(&ext->mp_opened))
+		return false;
+
+	if (wiimote_cmd_acquire(ext->wdata))
+		return false;
+
+	/* initialize motion plus */
+	wmem = 0x55;
+	ret = wiimote_cmd_write(ext->wdata, 0xa600f0, &wmem, sizeof(wmem));
+	if (ret)
+		goto error;
+
+	/* read motion plus ID */
+	ret = wiimote_cmd_read(ext->wdata, 0xa600fe, rmem, 2);
+	if (ret == 2 || rmem[1] == 0x5)
+		avail = true;
+
+error:
+	wiimote_cmd_release(ext->wdata);
+	return avail;
+}
+
+static __u8 ext_read(struct wiimote_ext *ext)
+{
+	ssize_t ret;
+	__u8 rmem[2], wmem;
+	__u8 type = WIIEXT_NONE;
+
+	if (!ext->plugged || !atomic_read(&ext->opened))
+		return WIIEXT_NONE;
+
+	if (wiimote_cmd_acquire(ext->wdata))
+		return WIIEXT_NONE;
+
+	/* initialize extension */
+	wmem = 0x55;
+	ret = wiimote_cmd_write(ext->wdata, 0xa400f0, &wmem, sizeof(wmem));
+	if (!ret) {
+		/* disable encryption */
+		wmem = 0x0;
+		wiimote_cmd_write(ext->wdata, 0xa400fb, &wmem, sizeof(wmem));
+	}
+
+	/* read extension ID */
+	ret = wiimote_cmd_read(ext->wdata, 0xa400fe, rmem, 2);
+	if (ret == 2) {
+		if (rmem[0] == 0 && rmem[1] == 0)
+			type = WIIEXT_NUNCHUCK;
+		else if (rmem[0] == 0x01 && rmem[1] == 0x01)
+			type = WIIEXT_CLASSIC;
+	}
+
+	wiimote_cmd_release(ext->wdata);
+
+	return type;
+}
+
+static void ext_enable(struct wiimote_ext *ext, bool motionp, __u8 ext_type)
+{
+	unsigned long flags;
+	__u8 wmem;
+	int ret;
+
+	if (motionp) {
+		if (wiimote_cmd_acquire(ext->wdata))
+			return;
+
+		if (ext_type == WIIEXT_CLASSIC)
+			wmem = 0x07;
+		else if (ext_type == WIIEXT_NUNCHUCK)
+			wmem = 0x05;
+		else
+			wmem = 0x04;
+
+		ret = wiimote_cmd_write(ext->wdata, 0xa600fe, &wmem, sizeof(wmem));
+		wiimote_cmd_release(ext->wdata);
+		if (ret)
+			return;
+	}
+
+	spin_lock_irqsave(&ext->wdata->state.lock, flags);
+	ext->motionp = motionp;
+	ext->ext_type = ext_type;
+	wiiproto_req_drm(ext->wdata, WIIPROTO_REQ_NULL);
+	spin_unlock_irqrestore(&ext->wdata->state.lock, flags);
+}
+
+static void wiiext_worker(struct work_struct *work)
+{
+	struct wiimote_ext *ext = container_of(work, struct wiimote_ext,
+									worker);
+	bool motionp;
+	__u8 ext_type;
+
+	ext_disable(ext);
+	motionp = motionp_read(ext);
+	ext_type = ext_read(ext);
+	ext_enable(ext, motionp, ext_type);
+}
+
+/* schedule work only once, otherwise mark for reschedule */
+static void wiiext_schedule(struct wiimote_ext *ext)
+{
+	queue_work(system_nrt_wq, &ext->worker);
+}
+
+/*
+ * Reacts on extension port events
+ * Whenever the driver gets an event from the wiimote that an extension has been
+ * plugged or unplugged, this funtion shall be called. It checks what extensions
+ * are connected and initializes and activates them.
+ * This can be called in atomic context. The initialization is done in a
+ * separate worker thread. The state.lock spinlock must be held by the caller.
+ */
+void wiiext_event(struct wiimote_data *wdata, bool plugged)
+{
+	if (!wdata->ext)
+		return;
+
+	if (wdata->ext->plugged == plugged)
+		return;
+
+	wdata->ext->plugged = plugged;
+
+	if (!plugged)
+		wdata->ext->mp_plugged = false;
+
+	/*
+	 * We need to call wiiext_schedule(wdata->ext) here, however, the
+	 * extension initialization logic is not fully understood and so
+	 * automatic initialization is not supported, yet.
+	 */
+}
+
+/*
+ * Returns true if the current DRM mode should contain extension data and false
+ * if there is no interest in extension data.
+ * All supported extensions send 6 byte extension data so any DRM that contains
+ * extension bytes is fine.
+ * The caller must hold the state.lock spinlock.
+ */
+bool wiiext_active(struct wiimote_data *wdata)
+{
+	if (!wdata->ext)
+		return false;
+
+	return wdata->ext->motionp || wdata->ext->ext_type;
+}
+
+static void handler_motionp(struct wiimote_ext *ext, const __u8 *payload)
+{
+	__s32 x, y, z;
+	bool plugged;
+
+	/*        |   8    7    6    5    4    3 |  2  |  1  |
+	 *   -----+------------------------------+-----+-----+
+	 *    1   |               Yaw Speed <7:0>            |
+	 *    2   |              Roll Speed <7:0>            |
+	 *    3   |             Pitch Speed <7:0>            |
+	 *   -----+------------------------------+-----+-----+
+	 *    4   |       Yaw Speed <13:8>       | Yaw |Pitch|
+	 *   -----+------------------------------+-----+-----+
+	 *    5   |      Roll Speed <13:8>       |Roll | Ext |
+	 *   -----+------------------------------+-----+-----+
+	 *    6   |     Pitch Speed <13:8>       |  1  |  0  |
+	 *   -----+------------------------------+-----+-----+
+	 * The single bits Yaw, Roll, Pitch in the lower right corner specify
+	 * whether the wiimote is rotating fast (0) or slow (1). Speed for slow
+	 * roation is 440 deg/s and for fast rotation 2000 deg/s. To get a
+	 * linear scale we multiply by 2000/440 = ~4.5454 which is 18 for fast
+	 * and 9 for slow.
+	 * If the wiimote is not rotating the sensor reports 2^13 = 8192.
+	 * Ext specifies whether an extension is connected to the motionp.
+	 */
+
+	x = payload[0];
+	y = payload[1];
+	z = payload[2];
+
+	x |= (((__u16)payload[3]) << 6) & 0xff00;
+	y |= (((__u16)payload[4]) << 6) & 0xff00;
+	z |= (((__u16)payload[5]) << 6) & 0xff00;
+
+	x -= 8192;
+	y -= 8192;
+	z -= 8192;
+
+	if (!(payload[3] & 0x02))
+		x *= 18;
+	else
+		x *= 9;
+	if (!(payload[4] & 0x02))
+		y *= 18;
+	else
+		y *= 9;
+	if (!(payload[3] & 0x01))
+		z *= 18;
+	else
+		z *= 9;
+
+	input_report_abs(ext->mp_input, ABS_RX, x);
+	input_report_abs(ext->mp_input, ABS_RY, y);
+	input_report_abs(ext->mp_input, ABS_RZ, z);
+	input_sync(ext->mp_input);
+
+	plugged = payload[5] & 0x01;
+	if (plugged != ext->mp_plugged)
+		ext->mp_plugged = plugged;
+}
+
+static void handler_nunchuck(struct wiimote_ext *ext, const __u8 *payload)
+{
+	__s16 x, y, z, bx, by;
+
+	/*   Byte |   8    7 |  6    5 |  4    3 |  2 |  1  |
+	 *   -----+----------+---------+---------+----+-----+
+	 *    1   |              Button X <7:0>             |
+	 *    2   |              Button Y <7:0>             |
+	 *   -----+----------+---------+---------+----+-----+
+	 *    3   |               Speed X <9:2>             |
+	 *    4   |               Speed Y <9:2>             |
+	 *    5   |               Speed Z <9:2>             |
+	 *   -----+----------+---------+---------+----+-----+
+	 *    6   | Z <1:0>  | Y <1:0> | X <1:0> | BC | BZ  |
+	 *   -----+----------+---------+---------+----+-----+
+	 * Button X/Y is the analog stick. Speed X, Y and Z are the
+	 * accelerometer data in the same format as the wiimote's accelerometer.
+	 * The 6th byte contains the LSBs of the accelerometer data.
+	 * BC and BZ are the C and Z buttons: 0 means pressed
+	 *
+	 * If reported interleaved with motionp, then the layout changes. The
+	 * 5th and 6th byte changes to:
+	 *   -----+-----------------------------------+-----+
+	 *    5   |            Speed Z <9:3>          | EXT |
+	 *   -----+--------+-----+-----+----+----+----+-----+
+	 *    6   |Z <2:1> |Y <1>|X <1>| BC | BZ | 0  |  0  |
+	 *   -----+--------+-----+-----+----+----+----+-----+
+	 * All three accelerometer values lose their LSB. The other data is
+	 * still available but slightly moved.
+	 *
+	 * Center data for button values is 128. Center value for accelerometer
+	 * values it 512 / 0x200
+	 */
+
+	bx = payload[0];
+	by = payload[1];
+	bx -= 128;
+	by -= 128;
+
+	x = payload[2] << 2;
+	y = payload[3] << 2;
+	z = payload[4] << 2;
+
+	if (ext->motionp) {
+		x |= (payload[5] >> 3) & 0x02;
+		y |= (payload[5] >> 4) & 0x02;
+		z &= ~0x4;
+		z |= (payload[5] >> 5) & 0x06;
+	} else {
+		x |= (payload[5] >> 2) & 0x03;
+		y |= (payload[5] >> 4) & 0x03;
+		z |= (payload[5] >> 6) & 0x03;
+	}
+
+	x -= 0x200;
+	y -= 0x200;
+	z -= 0x200;
+
+	input_report_abs(ext->input, ABS_HAT0X, bx);
+	input_report_abs(ext->input, ABS_HAT0Y, by);
+
+	input_report_abs(ext->input, ABS_RX, x);
+	input_report_abs(ext->input, ABS_RY, y);
+	input_report_abs(ext->input, ABS_RZ, z);
+
+	if (ext->motionp) {
+		input_report_key(ext->input,
+			wiiext_keymap[WIIEXT_KEY_Z], !!(payload[5] & 0x04));
+		input_report_key(ext->input,
+			wiiext_keymap[WIIEXT_KEY_C], !!(payload[5] & 0x08));
+	} else {
+		input_report_key(ext->input,
+			wiiext_keymap[WIIEXT_KEY_Z], !!(payload[5] & 0x01));
+		input_report_key(ext->input,
+			wiiext_keymap[WIIEXT_KEY_C], !!(payload[5] & 0x02));
+	}
+
+	input_sync(ext->input);
+}
+
+static void handler_classic(struct wiimote_ext *ext, const __u8 *payload)
+{
+	__s8 rx, ry, lx, ly, lt, rt;
+
+	/*   Byte |  8  |  7  |  6  |  5  |  4  |  3  |  2  |  1  |
+	 *   -----+-----+-----+-----+-----+-----+-----+-----+-----+
+	 *    1   | RX <5:4>  |              LX <5:0>             |
+	 *    2   | RX <3:2>  |              LY <5:0>             |
+	 *   -----+-----+-----+-----+-----------------------------+
+	 *    3   |RX<1>| LT <5:4>  |         RY <5:1>            |
+	 *   -----+-----+-----------+-----------------------------+
+	 *    4   |     LT <3:1>    |         RT <5:1>            |
+	 *   -----+-----+-----+-----+-----+-----+-----+-----+-----+
+	 *    5   | BDR | BDD | BLT | B-  | BH  | B+  | BRT |  1  |
+	 *   -----+-----+-----+-----+-----+-----+-----+-----+-----+
+	 *    6   | BZL | BB  | BY  | BA  | BX  | BZR | BDL | BDU |
+	 *   -----+-----+-----+-----+-----+-----+-----+-----+-----+
+	 * All buttons are 0 if pressed
+	 * RX and RY are right analog stick
+	 * LX and LY are left analog stick
+	 * LT is left trigger, RT is right trigger
+	 * BLT is 0 if left trigger is fully pressed
+	 * BRT is 0 if right trigger is fully pressed
+	 * BDR, BDD, BDL, BDU form the D-Pad with right, down, left, up buttons
+	 * BZL is left Z button and BZR is right Z button
+	 * B-, BH, B+ are +, HOME and - buttons
+	 * BB, BY, BA, BX are A, B, X, Y buttons
+	 * LSB of RX, RY, LT, and RT are not transmitted and always 0.
+	 *
+	 * With motionp enabled it changes slightly to this:
+	 *   Byte |  8  |  7  |  6  |  5  |  4  |  3  |  2  |  1  |
+	 *   -----+-----+-----+-----+-----+-----+-----+-----+-----+
+	 *    1   | RX <4:3>  |          LX <5:1>           | BDU |
+	 *    2   | RX <2:1>  |          LY <5:1>           | BDL |
+	 *   -----+-----+-----+-----+-----------------------+-----+
+	 *    3   |RX<0>| LT <4:3>  |         RY <4:0>            |
+	 *   -----+-----+-----------+-----------------------------+
+	 *    4   |     LT <2:0>    |         RT <4:0>            |
+	 *   -----+-----+-----+-----+-----+-----+-----+-----+-----+
+	 *    5   | BDR | BDD | BLT | B-  | BH  | B+  | BRT | EXT |
+	 *   -----+-----+-----+-----+-----+-----+-----+-----+-----+
+	 *    6   | BZL | BB  | BY  | BA  | BX  | BZR |  0  |  0  |
+	 *   -----+-----+-----+-----+-----+-----+-----+-----+-----+
+	 * Only the LSBs of LX and LY are lost. BDU and BDL are moved, the rest
+	 * is the same as before.
+	 */
+
+	if (ext->motionp) {
+		lx = payload[0] & 0x3e;
+		ly = payload[0] & 0x3e;
+	} else {
+		lx = payload[0] & 0x3f;
+		ly = payload[0] & 0x3f;
+	}
+
+	rx = (payload[0] >> 3) & 0x14;
+	rx |= (payload[1] >> 5) & 0x06;
+	rx |= (payload[2] >> 7) & 0x01;
+	ry = payload[2] & 0x1f;
+
+	rt = payload[3] & 0x1f;
+	lt = (payload[2] >> 2) & 0x18;
+	lt |= (payload[3] >> 5) & 0x07;
+
+	rx <<= 1;
+	ry <<= 1;
+	rt <<= 1;
+	lt <<= 1;
+
+	input_report_abs(ext->input, ABS_HAT1X, lx - 0x20);
+	input_report_abs(ext->input, ABS_HAT1Y, ly - 0x20);
+	input_report_abs(ext->input, ABS_HAT2X, rx - 0x20);
+	input_report_abs(ext->input, ABS_HAT2Y, ry - 0x20);
+	input_report_abs(ext->input, ABS_HAT3X, rt - 0x20);
+	input_report_abs(ext->input, ABS_HAT3Y, lt - 0x20);
+
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_RIGHT],
+							!!(payload[4] & 0x80));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_DOWN],
+							!!(payload[4] & 0x40));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_LT],
+							!!(payload[4] & 0x20));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_MINUS],
+							!!(payload[4] & 0x10));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_HOME],
+							!!(payload[4] & 0x08));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_PLUS],
+							!!(payload[4] & 0x04));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_RT],
+							!!(payload[4] & 0x02));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_ZL],
+							!!(payload[5] & 0x80));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_B],
+							!!(payload[5] & 0x40));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_Y],
+							!!(payload[5] & 0x20));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_A],
+							!!(payload[5] & 0x10));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_X],
+							!!(payload[5] & 0x08));
+	input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_ZR],
+							!!(payload[5] & 0x04));
+
+	if (ext->motionp) {
+		input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_UP],
+							!!(payload[0] & 0x01));
+		input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_LEFT],
+							!!(payload[1] & 0x01));
+	} else {
+		input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_UP],
+							!!(payload[5] & 0x01));
+		input_report_key(ext->input, wiiext_keymap[WIIEXT_KEY_LEFT],
+							!!(payload[5] & 0x02));
+	}
+
+	input_sync(ext->input);
+}
+
+/* call this with state.lock spinlock held */
+void wiiext_handle(struct wiimote_data *wdata, const __u8 *payload)
+{
+	struct wiimote_ext *ext = wdata->ext;
+
+	if (!ext)
+		return;
+
+	if (ext->motionp && (payload[5] & 0x02)) {
+		handler_motionp(ext, payload);
+	} else if (ext->ext_type == WIIEXT_NUNCHUCK) {
+		handler_nunchuck(ext, payload);
+	} else if (ext->ext_type == WIIEXT_CLASSIC) {
+		handler_classic(ext, payload);
+	}
+}
+
+static ssize_t wiiext_show(struct device *dev, struct device_attribute *attr,
+								char *buf)
+{
+	struct wiimote_data *wdata = dev_to_wii(dev);
+	__u8 type = WIIEXT_NONE;
+	bool motionp = false;
+	unsigned long flags;
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	if (wdata->ext) {
+		motionp = wdata->ext->motionp;
+		type = wdata->ext->ext_type;
+	}
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	if (type == WIIEXT_NUNCHUCK) {
+		if (motionp)
+			return sprintf(buf, "motionp+nunchuck\n");
+		else
+			return sprintf(buf, "nunchuck\n");
+	} else if (type == WIIEXT_CLASSIC) {
+		if (motionp)
+			return sprintf(buf, "motionp+classic\n");
+		else
+			return sprintf(buf, "classic\n");
+	} else {
+		if (motionp)
+			return sprintf(buf, "motionp\n");
+		else
+			return sprintf(buf, "none\n");
+	}
+}
+
+static DEVICE_ATTR(extension, S_IRUGO, wiiext_show, NULL);
+
+static int wiiext_input_open(struct input_dev *dev)
+{
+	struct wiimote_ext *ext = input_get_drvdata(dev);
+	int ret;
+
+	ret = hid_hw_open(ext->wdata->hdev);
+	if (ret)
+		return ret;
+
+	atomic_inc(&ext->opened);
+	wiiext_schedule(ext);
+
+	return 0;
+}
+
+static void wiiext_input_close(struct input_dev *dev)
+{
+	struct wiimote_ext *ext = input_get_drvdata(dev);
+
+	atomic_dec(&ext->opened);
+	wiiext_schedule(ext);
+	hid_hw_close(ext->wdata->hdev);
+}
+
+static int wiiext_mp_open(struct input_dev *dev)
+{
+	struct wiimote_ext *ext = input_get_drvdata(dev);
+	int ret;
+
+	ret = hid_hw_open(ext->wdata->hdev);
+	if (ret)
+		return ret;
+
+	atomic_inc(&ext->mp_opened);
+	wiiext_schedule(ext);
+
+	return 0;
+}
+
+static void wiiext_mp_close(struct input_dev *dev)
+{
+	struct wiimote_ext *ext = input_get_drvdata(dev);
+
+	atomic_dec(&ext->mp_opened);
+	wiiext_schedule(ext);
+	hid_hw_close(ext->wdata->hdev);
+}
+
+/* Initializes the extension driver of a wiimote */
+int wiiext_init(struct wiimote_data *wdata)
+{
+	struct wiimote_ext *ext;
+	unsigned long flags;
+	int ret, i;
+
+	ext = kzalloc(sizeof(*ext), GFP_KERNEL);
+	if (!ext)
+		return -ENOMEM;
+
+	ext->wdata = wdata;
+	INIT_WORK(&ext->worker, wiiext_worker);
+
+	ext->input = input_allocate_device();
+	if (!ext->input) {
+		ret = -ENOMEM;
+		goto err_input;
+	}
+
+	input_set_drvdata(ext->input, ext);
+	ext->input->open = wiiext_input_open;
+	ext->input->close = wiiext_input_close;
+	ext->input->dev.parent = &wdata->hdev->dev;
+	ext->input->id.bustype = wdata->hdev->bus;
+	ext->input->id.vendor = wdata->hdev->vendor;
+	ext->input->id.product = wdata->hdev->product;
+	ext->input->id.version = wdata->hdev->version;
+	ext->input->name = WIIMOTE_NAME " Extension";
+
+	set_bit(EV_KEY, ext->input->evbit);
+	for (i = 0; i < WIIEXT_KEY_COUNT; ++i)
+		set_bit(wiiext_keymap[i], ext->input->keybit);
+
+	set_bit(EV_ABS, ext->input->evbit);
+	set_bit(ABS_HAT0X, ext->input->absbit);
+	set_bit(ABS_HAT0Y, ext->input->absbit);
+	set_bit(ABS_HAT1X, ext->input->absbit);
+	set_bit(ABS_HAT1Y, ext->input->absbit);
+	set_bit(ABS_HAT2X, ext->input->absbit);
+	set_bit(ABS_HAT2Y, ext->input->absbit);
+	set_bit(ABS_HAT3X, ext->input->absbit);
+	set_bit(ABS_HAT3Y, ext->input->absbit);
+	input_set_abs_params(ext->input, ABS_HAT0X, -120, 120, 2, 4);
+	input_set_abs_params(ext->input, ABS_HAT0Y, -120, 120, 2, 4);
+	input_set_abs_params(ext->input, ABS_HAT1X, -30, 30, 1, 1);
+	input_set_abs_params(ext->input, ABS_HAT1Y, -30, 30, 1, 1);
+	input_set_abs_params(ext->input, ABS_HAT2X, -30, 30, 1, 1);
+	input_set_abs_params(ext->input, ABS_HAT2Y, -30, 30, 1, 1);
+	input_set_abs_params(ext->input, ABS_HAT3X, -30, 30, 1, 1);
+	input_set_abs_params(ext->input, ABS_HAT3Y, -30, 30, 1, 1);
+	set_bit(ABS_RX, ext->input->absbit);
+	set_bit(ABS_RY, ext->input->absbit);
+	set_bit(ABS_RZ, ext->input->absbit);
+	input_set_abs_params(ext->input, ABS_RX, -500, 500, 2, 4);
+	input_set_abs_params(ext->input, ABS_RY, -500, 500, 2, 4);
+	input_set_abs_params(ext->input, ABS_RZ, -500, 500, 2, 4);
+
+	ret = input_register_device(ext->input);
+	if (ret) {
+		input_free_device(ext->input);
+		goto err_input;
+	}
+
+	ext->mp_input = input_allocate_device();
+	if (!ext->mp_input) {
+		ret = -ENOMEM;
+		goto err_mp;
+	}
+
+	input_set_drvdata(ext->mp_input, ext);
+	ext->mp_input->open = wiiext_mp_open;
+	ext->mp_input->close = wiiext_mp_close;
+	ext->mp_input->dev.parent = &wdata->hdev->dev;
+	ext->mp_input->id.bustype = wdata->hdev->bus;
+	ext->mp_input->id.vendor = wdata->hdev->vendor;
+	ext->mp_input->id.product = wdata->hdev->product;
+	ext->mp_input->id.version = wdata->hdev->version;
+	ext->mp_input->name = WIIMOTE_NAME " Motion+";
+
+	set_bit(EV_ABS, ext->mp_input->evbit);
+	set_bit(ABS_RX, ext->mp_input->absbit);
+	set_bit(ABS_RY, ext->mp_input->absbit);
+	set_bit(ABS_RZ, ext->mp_input->absbit);
+	input_set_abs_params(ext->mp_input, ABS_RX, -160000, 160000, 4, 8);
+	input_set_abs_params(ext->mp_input, ABS_RY, -160000, 160000, 4, 8);
+	input_set_abs_params(ext->mp_input, ABS_RZ, -160000, 160000, 4, 8);
+
+	ret = input_register_device(ext->mp_input);
+	if (ret) {
+		input_free_device(ext->mp_input);
+		goto err_mp;
+	}
+
+	ret = device_create_file(&wdata->hdev->dev, &dev_attr_extension);
+	if (ret)
+		goto err_dev;
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	wdata->ext = ext;
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	return 0;
+
+err_dev:
+	input_unregister_device(ext->mp_input);
+err_mp:
+	input_unregister_device(ext->input);
+err_input:
+	kfree(ext);
+	return ret;
+}
+
+/* Deinitializes the extension driver of a wiimote */
+void wiiext_deinit(struct wiimote_data *wdata)
+{
+	struct wiimote_ext *ext = wdata->ext;
+	unsigned long flags;
+
+	if (!ext)
+		return;
+
+	/*
+	 * We first unset wdata->ext to avoid further input from the wiimote
+	 * core. The worker thread does not access this pointer so it is not
+	 * affected by this.
+	 * We kill the worker after this so it does not get respawned during
+	 * deinitialization.
+	 */
+
+	spin_lock_irqsave(&wdata->state.lock, flags);
+	wdata->ext = NULL;
+	spin_unlock_irqrestore(&wdata->state.lock, flags);
+
+	device_remove_file(&wdata->hdev->dev, &dev_attr_extension);
+	input_unregister_device(ext->mp_input);
+	input_unregister_device(ext->input);
+
+	cancel_work_sync(&ext->worker);
+	kfree(ext);
+}
diff --git a/drivers/hid/hid-wiimote.h b/drivers/hid/hid-wiimote.h
new file mode 100644
index 000000000000..c81dbeb086c5
--- /dev/null
+++ b/drivers/hid/hid-wiimote.h
@@ -0,0 +1,208 @@
+#ifndef __HID_WIIMOTE_H
+#define __HID_WIIMOTE_H
+
+/*
+ * HID driver for Nintendo Wiimote devices
+ * Copyright (c) 2011 David Herrmann
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/completion.h>
+#include <linux/device.h>
+#include <linux/hid.h>
+#include <linux/input.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/power_supply.h>
+#include <linux/spinlock.h>
+
+#define WIIMOTE_NAME "Nintendo Wii Remote"
+#define WIIMOTE_BUFSIZE 32
+
+#define WIIPROTO_FLAG_LED1		0x01
+#define WIIPROTO_FLAG_LED2		0x02
+#define WIIPROTO_FLAG_LED3		0x04
+#define WIIPROTO_FLAG_LED4		0x08
+#define WIIPROTO_FLAG_RUMBLE		0x10
+#define WIIPROTO_FLAG_ACCEL		0x20
+#define WIIPROTO_FLAG_IR_BASIC		0x40
+#define WIIPROTO_FLAG_IR_EXT		0x80
+#define WIIPROTO_FLAG_IR_FULL		0xc0 /* IR_BASIC | IR_EXT */
+#define WIIPROTO_FLAGS_LEDS (WIIPROTO_FLAG_LED1 | WIIPROTO_FLAG_LED2 | \
+					WIIPROTO_FLAG_LED3 | WIIPROTO_FLAG_LED4)
+#define WIIPROTO_FLAGS_IR (WIIPROTO_FLAG_IR_BASIC | WIIPROTO_FLAG_IR_EXT | \
+							WIIPROTO_FLAG_IR_FULL)
+
+/* return flag for led \num */
+#define WIIPROTO_FLAG_LED(num) (WIIPROTO_FLAG_LED1 << (num - 1))
+
+struct wiimote_buf {
+	__u8 data[HID_MAX_BUFFER_SIZE];
+	size_t size;
+};
+
+struct wiimote_state {
+	spinlock_t lock;
+	__u8 flags;
+	__u8 accel_split[2];
+	__u8 drm;
+
+	/* synchronous cmd requests */
+	struct mutex sync;
+	struct completion ready;
+	int cmd;
+	__u32 opt;
+
+	/* results of synchronous requests */
+	__u8 cmd_battery;
+	__u8 cmd_err;
+	__u8 *cmd_read_buf;
+	__u8 cmd_read_size;
+};
+
+struct wiimote_data {
+	struct hid_device *hdev;
+	struct input_dev *input;
+	struct led_classdev *leds[4];
+	struct input_dev *accel;
+	struct input_dev *ir;
+	struct power_supply battery;
+	struct wiimote_ext *ext;
+	struct wiimote_debug *debug;
+
+	spinlock_t qlock;
+	__u8 head;
+	__u8 tail;
+	struct wiimote_buf outq[WIIMOTE_BUFSIZE];
+	struct work_struct worker;
+
+	struct wiimote_state state;
+};
+
+enum wiiproto_reqs {
+	WIIPROTO_REQ_NULL = 0x0,
+	WIIPROTO_REQ_RUMBLE = 0x10,
+	WIIPROTO_REQ_LED = 0x11,
+	WIIPROTO_REQ_DRM = 0x12,
+	WIIPROTO_REQ_IR1 = 0x13,
+	WIIPROTO_REQ_SREQ = 0x15,
+	WIIPROTO_REQ_WMEM = 0x16,
+	WIIPROTO_REQ_RMEM = 0x17,
+	WIIPROTO_REQ_IR2 = 0x1a,
+	WIIPROTO_REQ_STATUS = 0x20,
+	WIIPROTO_REQ_DATA = 0x21,
+	WIIPROTO_REQ_RETURN = 0x22,
+	WIIPROTO_REQ_DRM_K = 0x30,
+	WIIPROTO_REQ_DRM_KA = 0x31,
+	WIIPROTO_REQ_DRM_KE = 0x32,
+	WIIPROTO_REQ_DRM_KAI = 0x33,
+	WIIPROTO_REQ_DRM_KEE = 0x34,
+	WIIPROTO_REQ_DRM_KAE = 0x35,
+	WIIPROTO_REQ_DRM_KIE = 0x36,
+	WIIPROTO_REQ_DRM_KAIE = 0x37,
+	WIIPROTO_REQ_DRM_E = 0x3d,
+	WIIPROTO_REQ_DRM_SKAI1 = 0x3e,
+	WIIPROTO_REQ_DRM_SKAI2 = 0x3f,
+	WIIPROTO_REQ_MAX
+};
+
+#define dev_to_wii(pdev) hid_get_drvdata(container_of(pdev, struct hid_device, \
+									dev))
+
+extern void wiiproto_req_drm(struct wiimote_data *wdata, __u8 drm);
+extern int wiimote_cmd_write(struct wiimote_data *wdata, __u32 offset,
+						const __u8 *wmem, __u8 size);
+extern ssize_t wiimote_cmd_read(struct wiimote_data *wdata, __u32 offset,
+							__u8 *rmem, __u8 size);
+
+#define wiiproto_req_rreg(wdata, os, sz) \
+				wiiproto_req_rmem((wdata), false, (os), (sz))
+#define wiiproto_req_reeprom(wdata, os, sz) \
+				wiiproto_req_rmem((wdata), true, (os), (sz))
+extern void wiiproto_req_rmem(struct wiimote_data *wdata, bool eeprom,
+						__u32 offset, __u16 size);
+
+#ifdef CONFIG_HID_WIIMOTE_EXT
+
+extern int wiiext_init(struct wiimote_data *wdata);
+extern void wiiext_deinit(struct wiimote_data *wdata);
+extern void wiiext_event(struct wiimote_data *wdata, bool plugged);
+extern bool wiiext_active(struct wiimote_data *wdata);
+extern void wiiext_handle(struct wiimote_data *wdata, const __u8 *payload);
+
+#else
+
+static inline int wiiext_init(void *u) { return 0; }
+static inline void wiiext_deinit(void *u) { }
+static inline void wiiext_event(void *u, bool p) { }
+static inline bool wiiext_active(void *u) { return false; }
+static inline void wiiext_handle(void *u, const __u8 *p) { }
+
+#endif
+
+#ifdef CONFIG_DEBUG_FS
+
+extern int wiidebug_init(struct wiimote_data *wdata);
+extern void wiidebug_deinit(struct wiimote_data *wdata);
+
+#else
+
+static inline int wiidebug_init(void *u) { return 0; }
+static inline void wiidebug_deinit(void *u) { }
+
+#endif
+
+/* requires the state.lock spinlock to be held */
+static inline bool wiimote_cmd_pending(struct wiimote_data *wdata, int cmd,
+								__u32 opt)
+{
+	return wdata->state.cmd == cmd && wdata->state.opt == opt;
+}
+
+/* requires the state.lock spinlock to be held */
+static inline void wiimote_cmd_complete(struct wiimote_data *wdata)
+{
+	wdata->state.cmd = WIIPROTO_REQ_NULL;
+	complete(&wdata->state.ready);
+}
+
+static inline int wiimote_cmd_acquire(struct wiimote_data *wdata)
+{
+	return mutex_lock_interruptible(&wdata->state.sync) ? -ERESTARTSYS : 0;
+}
+
+/* requires the state.lock spinlock to be held */
+static inline void wiimote_cmd_set(struct wiimote_data *wdata, int cmd,
+								__u32 opt)
+{
+	INIT_COMPLETION(wdata->state.ready);
+	wdata->state.cmd = cmd;
+	wdata->state.opt = opt;
+}
+
+static inline void wiimote_cmd_release(struct wiimote_data *wdata)
+{
+	mutex_unlock(&wdata->state.sync);
+}
+
+static inline int wiimote_cmd_wait(struct wiimote_data *wdata)
+{
+	int ret;
+
+	ret = wait_for_completion_interruptible_timeout(&wdata->state.ready, HZ);
+	if (ret < 0)
+		return -ERESTARTSYS;
+	else if (ret == 0)
+		return -EIO;
+	else
+		return 0;
+}
+
+#endif
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index b403fcef0b86..5bf91dbad59d 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -197,16 +197,24 @@ static int usbhid_restart_out_queue(struct usbhid_device *usbhid)
 {
 	struct hid_device *hid = usb_get_intfdata(usbhid->intf);
 	int kicked;
+	int r;
 
 	if (!hid)
 		return 0;
 
 	if ((kicked = (usbhid->outhead != usbhid->outtail))) {
 		dbg("Kicking head %d tail %d", usbhid->outhead, usbhid->outtail);
+
+		r = usb_autopm_get_interface_async(usbhid->intf);
+		if (r < 0)
+			return r;
+		/* Asynchronously flush queue. */
+		set_bit(HID_OUT_RUNNING, &usbhid->iofl);
 		if (hid_submit_out(hid)) {
 			clear_bit(HID_OUT_RUNNING, &usbhid->iofl);
-			wake_up(&usbhid->wait);
+			usb_autopm_put_interface_async(usbhid->intf);
 		}
+		wake_up(&usbhid->wait);
 	}
 	return kicked;
 }
@@ -215,6 +223,7 @@ static int usbhid_restart_ctrl_queue(struct usbhid_device *usbhid)
 {
 	struct hid_device *hid = usb_get_intfdata(usbhid->intf);
 	int kicked;
+	int r;
 
 	WARN_ON(hid == NULL);
 	if (!hid)
@@ -222,10 +231,17 @@ static int usbhid_restart_ctrl_queue(struct usbhid_device *usbhid)
 
 	if ((kicked = (usbhid->ctrlhead != usbhid->ctrltail))) {
 		dbg("Kicking head %d tail %d", usbhid->ctrlhead, usbhid->ctrltail);
+
+		r = usb_autopm_get_interface_async(usbhid->intf);
+		if (r < 0)
+			return r;
+		/* Asynchronously flush queue. */
+		set_bit(HID_CTRL_RUNNING, &usbhid->iofl);
 		if (hid_submit_ctrl(hid)) {
 			clear_bit(HID_CTRL_RUNNING, &usbhid->iofl);
-			wake_up(&usbhid->wait);
+			usb_autopm_put_interface_async(usbhid->intf);
 		}
+		wake_up(&usbhid->wait);
 	}
 	return kicked;
 }
@@ -304,30 +320,21 @@ static int hid_submit_out(struct hid_device *hid)
 	report = usbhid->out[usbhid->outtail].report;
 	raw_report = usbhid->out[usbhid->outtail].raw_report;
 
-	r = usb_autopm_get_interface_async(usbhid->intf);
-	if (r < 0)
-		return -1;
-
-	/*
-	 * if the device hasn't been woken, we leave the output
-	 * to resume()
-	 */
-	if (!test_bit(HID_REPORTED_IDLE, &usbhid->iofl)) {
-		usbhid->urbout->transfer_buffer_length = ((report->size - 1) >> 3) + 1 + (report->id > 0);
-		usbhid->urbout->dev = hid_to_usb_dev(hid);
-		memcpy(usbhid->outbuf, raw_report, usbhid->urbout->transfer_buffer_length);
-		kfree(raw_report);
+	usbhid->urbout->transfer_buffer_length = ((report->size - 1) >> 3) +
+						 1 + (report->id > 0);
+	usbhid->urbout->dev = hid_to_usb_dev(hid);
+	memcpy(usbhid->outbuf, raw_report,
+	       usbhid->urbout->transfer_buffer_length);
+	kfree(raw_report);
 
-		dbg_hid("submitting out urb\n");
+	dbg_hid("submitting out urb\n");
 
-		if (usb_submit_urb(usbhid->urbout, GFP_ATOMIC)) {
-			hid_err(hid, "usb_submit_urb(out) failed\n");
-			usb_autopm_put_interface_async(usbhid->intf);
-			return -1;
-		}
-		usbhid->last_out = jiffies;
+	r = usb_submit_urb(usbhid->urbout, GFP_ATOMIC);
+	if (r < 0) {
+		hid_err(hid, "usb_submit_urb(out) failed: %d\n", r);
+		return r;
 	}
-
+	usbhid->last_out = jiffies;
 	return 0;
 }
 
@@ -343,50 +350,48 @@ static int hid_submit_ctrl(struct hid_device *hid)
 	raw_report = usbhid->ctrl[usbhid->ctrltail].raw_report;
 	dir = usbhid->ctrl[usbhid->ctrltail].dir;
 
-	r = usb_autopm_get_interface_async(usbhid->intf);
-	if (r < 0)
-		return -1;
-	if (!test_bit(HID_REPORTED_IDLE, &usbhid->iofl)) {
-		len = ((report->size - 1) >> 3) + 1 + (report->id > 0);
-		if (dir == USB_DIR_OUT) {
-			usbhid->urbctrl->pipe = usb_sndctrlpipe(hid_to_usb_dev(hid), 0);
-			usbhid->urbctrl->transfer_buffer_length = len;
-			memcpy(usbhid->ctrlbuf, raw_report, len);
-			kfree(raw_report);
-		} else {
-			int maxpacket, padlen;
-
-			usbhid->urbctrl->pipe = usb_rcvctrlpipe(hid_to_usb_dev(hid), 0);
-			maxpacket = usb_maxpacket(hid_to_usb_dev(hid), usbhid->urbctrl->pipe, 0);
-			if (maxpacket > 0) {
-				padlen = DIV_ROUND_UP(len, maxpacket);
-				padlen *= maxpacket;
-				if (padlen > usbhid->bufsize)
-					padlen = usbhid->bufsize;
-			} else
-				padlen = 0;
-			usbhid->urbctrl->transfer_buffer_length = padlen;
-		}
-		usbhid->urbctrl->dev = hid_to_usb_dev(hid);
-
-		usbhid->cr->bRequestType = USB_TYPE_CLASS | USB_RECIP_INTERFACE | dir;
-		usbhid->cr->bRequest = (dir == USB_DIR_OUT) ? HID_REQ_SET_REPORT : HID_REQ_GET_REPORT;
-		usbhid->cr->wValue = cpu_to_le16(((report->type + 1) << 8) | report->id);
-		usbhid->cr->wIndex = cpu_to_le16(usbhid->ifnum);
-		usbhid->cr->wLength = cpu_to_le16(len);
-
-		dbg_hid("submitting ctrl urb: %s wValue=0x%04x wIndex=0x%04x wLength=%u\n",
-			usbhid->cr->bRequest == HID_REQ_SET_REPORT ? "Set_Report" : "Get_Report",
-			usbhid->cr->wValue, usbhid->cr->wIndex, usbhid->cr->wLength);
-
-		if (usb_submit_urb(usbhid->urbctrl, GFP_ATOMIC)) {
-			usb_autopm_put_interface_async(usbhid->intf);
-			hid_err(hid, "usb_submit_urb(ctrl) failed\n");
-			return -1;
-		}
-		usbhid->last_ctrl = jiffies;
+	len = ((report->size - 1) >> 3) + 1 + (report->id > 0);
+	if (dir == USB_DIR_OUT) {
+		usbhid->urbctrl->pipe = usb_sndctrlpipe(hid_to_usb_dev(hid), 0);
+		usbhid->urbctrl->transfer_buffer_length = len;
+		memcpy(usbhid->ctrlbuf, raw_report, len);
+		kfree(raw_report);
+	} else {
+		int maxpacket, padlen;
+
+		usbhid->urbctrl->pipe = usb_rcvctrlpipe(hid_to_usb_dev(hid), 0);
+		maxpacket = usb_maxpacket(hid_to_usb_dev(hid),
+					  usbhid->urbctrl->pipe, 0);
+		if (maxpacket > 0) {
+			padlen = DIV_ROUND_UP(len, maxpacket);
+			padlen *= maxpacket;
+			if (padlen > usbhid->bufsize)
+				padlen = usbhid->bufsize;
+		} else
+			padlen = 0;
+		usbhid->urbctrl->transfer_buffer_length = padlen;
 	}
-
+	usbhid->urbctrl->dev = hid_to_usb_dev(hid);
+
+	usbhid->cr->bRequestType = USB_TYPE_CLASS | USB_RECIP_INTERFACE | dir;
+	usbhid->cr->bRequest = (dir == USB_DIR_OUT) ? HID_REQ_SET_REPORT :
+						      HID_REQ_GET_REPORT;
+	usbhid->cr->wValue = cpu_to_le16(((report->type + 1) << 8) |
+					 report->id);
+	usbhid->cr->wIndex = cpu_to_le16(usbhid->ifnum);
+	usbhid->cr->wLength = cpu_to_le16(len);
+
+	dbg_hid("submitting ctrl urb: %s wValue=0x%04x wIndex=0x%04x wLength=%u\n",
+		usbhid->cr->bRequest == HID_REQ_SET_REPORT ? "Set_Report" :
+							     "Get_Report",
+		usbhid->cr->wValue, usbhid->cr->wIndex, usbhid->cr->wLength);
+
+	r = usb_submit_urb(usbhid->urbctrl, GFP_ATOMIC);
+	if (r < 0) {
+		hid_err(hid, "usb_submit_urb(ctrl) failed: %d\n", r);
+		return r;
+	}
+	usbhid->last_ctrl = jiffies;
 	return 0;
 }
 
@@ -423,11 +428,8 @@ static void hid_irq_out(struct urb *urb)
 	else
 		usbhid->outtail = (usbhid->outtail + 1) & (HID_OUTPUT_FIFO_SIZE - 1);
 
-	if (usbhid->outhead != usbhid->outtail) {
-		if (hid_submit_out(hid)) {
-			clear_bit(HID_OUT_RUNNING, &usbhid->iofl);
-			wake_up(&usbhid->wait);
-		}
+	if (usbhid->outhead != usbhid->outtail && !hid_submit_out(hid)) {
+		/* Successfully submitted next urb in queue */
 		spin_unlock_irqrestore(&usbhid->lock, flags);
 		return;
 	}
@@ -474,13 +476,9 @@ static void hid_ctrl(struct urb *urb)
 	else
 		usbhid->ctrltail = (usbhid->ctrltail + 1) & (HID_CONTROL_FIFO_SIZE - 1);
 
-	if (usbhid->ctrlhead != usbhid->ctrltail) {
-		if (hid_submit_ctrl(hid)) {
-			clear_bit(HID_CTRL_RUNNING, &usbhid->iofl);
-			wake_up(&usbhid->wait);
-		}
+	if (usbhid->ctrlhead != usbhid->ctrltail && !hid_submit_ctrl(hid)) {
+		/* Successfully submitted next urb in queue */
 		spin_unlock(&usbhid->lock);
-		usb_autopm_put_interface_async(usbhid->intf);
 		return;
 	}
 
@@ -515,9 +513,23 @@ static void __usbhid_submit_report(struct hid_device *hid, struct hid_report *re
 		usbhid->out[usbhid->outhead].report = report;
 		usbhid->outhead = head;
 
+		/* Try to awake from autosuspend... */
+		if (usb_autopm_get_interface_async(usbhid->intf) < 0)
+			return;
+
+		/*
+		 * But if still suspended, leave urb enqueued, don't submit.
+		 * Submission will occur if/when resume() drains the queue.
+		 */
+		if (test_bit(HID_REPORTED_IDLE, &usbhid->iofl))
+			return;
+
 		if (!test_and_set_bit(HID_OUT_RUNNING, &usbhid->iofl)) {
-			if (hid_submit_out(hid))
+			if (hid_submit_out(hid)) {
 				clear_bit(HID_OUT_RUNNING, &usbhid->iofl);
+				usb_autopm_put_interface_async(usbhid->intf);
+			}
+			wake_up(&usbhid->wait);
 		} else {
 			/*
 			 * the queue is known to run
@@ -549,9 +561,23 @@ static void __usbhid_submit_report(struct hid_device *hid, struct hid_report *re
 	usbhid->ctrl[usbhid->ctrlhead].dir = dir;
 	usbhid->ctrlhead = head;
 
+	/* Try to awake from autosuspend... */
+	if (usb_autopm_get_interface_async(usbhid->intf) < 0)
+		return;
+
+	/*
+	 * If already suspended, leave urb enqueued, but don't submit.
+	 * Submission will occur if/when resume() drains the queue.
+	 */
+	if (test_bit(HID_REPORTED_IDLE, &usbhid->iofl))
+		return;
+
 	if (!test_and_set_bit(HID_CTRL_RUNNING, &usbhid->iofl)) {
-		if (hid_submit_ctrl(hid))
+		if (hid_submit_ctrl(hid)) {
 			clear_bit(HID_CTRL_RUNNING, &usbhid->iofl);
+			usb_autopm_put_interface_async(usbhid->intf);
+		}
+		wake_up(&usbhid->wait);
 	} else {
 		/*
 		 * the queue is known to run
@@ -576,6 +602,30 @@ void usbhid_submit_report(struct hid_device *hid, struct hid_report *report, uns
 }
 EXPORT_SYMBOL_GPL(usbhid_submit_report);
 
+/* Workqueue routine to send requests to change LEDs */
+static void hid_led(struct work_struct *work)
+{
+	struct usbhid_device *usbhid =
+		container_of(work, struct usbhid_device, led_work);
+	struct hid_device *hid = usbhid->hid;
+	struct hid_field *field;
+	unsigned long flags;
+
+	field = hidinput_get_led_field(hid);
+	if (!field) {
+		hid_warn(hid, "LED event field not found\n");
+		return;
+	}
+
+	spin_lock_irqsave(&usbhid->lock, flags);
+	if (!test_bit(HID_DISCONNECTED, &usbhid->iofl)) {
+		usbhid->ledcount = hidinput_count_leds(hid);
+		hid_dbg(usbhid->hid, "New ledcount = %u\n", usbhid->ledcount);
+		__usbhid_submit_report(hid, field->report, USB_DIR_OUT);
+	}
+	spin_unlock_irqrestore(&usbhid->lock, flags);
+}
+
 static int usb_hidinput_input_event(struct input_dev *dev, unsigned int type, unsigned int code, int value)
 {
 	struct hid_device *hid = input_get_drvdata(dev);
@@ -595,17 +645,15 @@ static int usb_hidinput_input_event(struct input_dev *dev, unsigned int type, un
 		return -1;
 	}
 
+	spin_lock_irqsave(&usbhid->lock, flags);
 	hid_set_field(field, offset, value);
-	if (value) {
-		spin_lock_irqsave(&usbhid->lock, flags);
-		usbhid->ledcount++;
-		spin_unlock_irqrestore(&usbhid->lock, flags);
-	} else {
-		spin_lock_irqsave(&usbhid->lock, flags);
-		usbhid->ledcount--;
-		spin_unlock_irqrestore(&usbhid->lock, flags);
-	}
-	usbhid_submit_report(hid, field->report, USB_DIR_OUT);
+	spin_unlock_irqrestore(&usbhid->lock, flags);
+
+	/*
+	 * Defer performing requested LED action.
+	 * This is more likely gather all LED changes into a single URB.
+	 */
+	schedule_work(&usbhid->led_work);
 
 	return 0;
 }
@@ -1100,7 +1148,7 @@ static void usbhid_stop(struct hid_device *hid)
 		return;
 
 	clear_bit(HID_STARTED, &usbhid->iofl);
-	spin_lock_irq(&usbhid->lock);	/* Sync with error handler */
+	spin_lock_irq(&usbhid->lock);	/* Sync with error and led handlers */
 	set_bit(HID_DISCONNECTED, &usbhid->iofl);
 	spin_unlock_irq(&usbhid->lock);
 	usb_kill_urb(usbhid->urbin);
@@ -1234,6 +1282,8 @@ static int usbhid_probe(struct usb_interface *intf, const struct usb_device_id *
 	setup_timer(&usbhid->io_retry, hid_retry_timeout, (unsigned long) hid);
 	spin_lock_init(&usbhid->lock);
 
+	INIT_WORK(&usbhid->led_work, hid_led);
+
 	ret = hid_add_device(hid);
 	if (ret) {
 		if (ret != -ENODEV)
@@ -1266,6 +1316,7 @@ static void hid_cancel_delayed_stuff(struct usbhid_device *usbhid)
 {
 	del_timer_sync(&usbhid->io_retry);
 	cancel_work_sync(&usbhid->reset_work);
+	cancel_work_sync(&usbhid->led_work);
 }
 
 static void hid_cease_io(struct usbhid_device *usbhid)
@@ -1367,16 +1418,6 @@ static int hid_suspend(struct usb_interface *intf, pm_message_t message)
 			return -EIO;
 	}
 
-	if (!ignoreled && PMSG_IS_AUTO(message)) {
-		spin_lock_irq(&usbhid->lock);
-		if (test_bit(HID_LED_ON, &usbhid->iofl)) {
-			spin_unlock_irq(&usbhid->lock);
-			usbhid_mark_busy(usbhid);
-			return -EBUSY;
-		}
-		spin_unlock_irq(&usbhid->lock);
-	}
-
 	hid_cancel_delayed_stuff(usbhid);
 	hid_cease_io(usbhid);
 
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 5028d60a22a1..c831af937481 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -47,6 +47,7 @@ static const struct hid_blacklist {
 
 	{ USB_VENDOR_ID_AFATECH, USB_DEVICE_ID_AFATECH_AF9016, HID_QUIRK_FULLSPEED_INTERVAL },
 
+	{ USB_VENDOR_ID_EMS, USB_DEVICE_ID_EMS_TRIO_LINKER_PLUS_II, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_ETURBOTOUCH, USB_DEVICE_ID_ETURBOTOUCH, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_GREENASIA, USB_DEVICE_ID_GREENASIA_DUAL_USB_JOYPAD, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_PANTHERLORD, USB_DEVICE_ID_PANTHERLORD_TWIN_USB_JOYSTICK, HID_QUIRK_MULTI_INPUT | HID_QUIRK_SKIP_OUTPUT_REPORTS },
@@ -67,6 +68,9 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_CH, USB_DEVICE_ID_CH_AXIS_295, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_DMI, USB_DEVICE_ID_DMI_ENC, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_TS2700, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_PRODIGE, USB_DEVICE_ID_PRODIGE_CORDLESS, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_QUANTA, USB_DEVICE_ID_PIXART_IMAGING_INC_OPTICAL_TOUCH_SCREEN, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_SUN, USB_DEVICE_ID_RARITAN_KVM_DONGLE, HID_QUIRK_NOGET },
diff --git a/drivers/hid/usbhid/usbhid.h b/drivers/hid/usbhid/usbhid.h
index 1673cac93d77..cb8f703efde5 100644
--- a/drivers/hid/usbhid/usbhid.h
+++ b/drivers/hid/usbhid/usbhid.h
@@ -55,7 +55,6 @@ struct usb_interface *usbhid_find_interface(int minor);
 #define HID_STARTED		8
 #define HID_REPORTED_IDLE	9
 #define HID_KEYS_PRESSED	10
-#define HID_LED_ON		11
 
 /*
  * USB-specific HID struct, to be pointed to
@@ -97,6 +96,8 @@ struct usbhid_device {
 	struct work_struct reset_work;                                  /* Task context for resets */
 	wait_queue_head_t wait;						/* For sleeping */
 	int ledcount;							/* counting the number of active leds */
+
+	struct work_struct led_work;					/* Task context for setting LEDs */
 };
 
 #define	hid_to_usb_dev(hid_dev) \
diff --git a/drivers/hid/usbhid/usbkbd.c b/drivers/hid/usbhid/usbkbd.c
index bc445d7e3bf5..796086980f4a 100644
--- a/drivers/hid/usbhid/usbkbd.c
+++ b/drivers/hid/usbhid/usbkbd.c
@@ -64,6 +64,32 @@ static const unsigned char usb_kbd_keycode[256] = {
 	150,158,159,128,136,177,178,176,142,152,173,140
 };
 
+
+/**
+ * struct usb_kbd - state of each attached keyboard
+ * @dev:	input device associated with this keyboard
+ * @usbdev:	usb device associated with this keyboard
+ * @old:	data received in the past from the @irq URB representing which
+ *		keys were pressed. By comparing with the current list of keys
+ *		that are pressed, we are able to see key releases.
+ * @irq:	URB for receiving a list of keys that are pressed when a
+ *		new key is pressed or a key that was pressed is released.
+ * @led:	URB for sending LEDs (e.g. numlock, ...)
+ * @newleds:	data that will be sent with the @led URB representing which LEDs
+ 		should be on
+ * @name:	Name of the keyboard. @dev's name field points to this buffer
+ * @phys:	Physical path of the keyboard. @dev's phys field points to this
+ *		buffer
+ * @new:	Buffer for the @irq URB
+ * @cr:		Control request for @led URB
+ * @leds:	Buffer for the @led URB
+ * @new_dma:	DMA address for @irq URB
+ * @leds_dma:	DMA address for @led URB
+ * @leds_lock:	spinlock that protects @leds, @newleds, and @led_urb_submitted
+ * @led_urb_submitted: indicates whether @led is in progress, i.e. it has been
+ *		submitted and its completion handler has not returned yet
+ *		without	resubmitting @led
+ */
 struct usb_kbd {
 	struct input_dev *dev;
 	struct usb_device *usbdev;
@@ -78,6 +104,10 @@ struct usb_kbd {
 	unsigned char *leds;
 	dma_addr_t new_dma;
 	dma_addr_t leds_dma;
+	
+	spinlock_t leds_lock;
+	bool led_urb_submitted;
+
 };
 
 static void usb_kbd_irq(struct urb *urb)
@@ -136,44 +166,66 @@ resubmit:
 static int usb_kbd_event(struct input_dev *dev, unsigned int type,
 			 unsigned int code, int value)
 {
+	unsigned long flags;
 	struct usb_kbd *kbd = input_get_drvdata(dev);
 
 	if (type != EV_LED)
 		return -1;
 
+	spin_lock_irqsave(&kbd->leds_lock, flags);
 	kbd->newleds = (!!test_bit(LED_KANA,    dev->led) << 3) | (!!test_bit(LED_COMPOSE, dev->led) << 3) |
 		       (!!test_bit(LED_SCROLLL, dev->led) << 2) | (!!test_bit(LED_CAPSL,   dev->led) << 1) |
 		       (!!test_bit(LED_NUML,    dev->led));
 
-	if (kbd->led->status == -EINPROGRESS)
+	if (kbd->led_urb_submitted){
+		spin_unlock_irqrestore(&kbd->leds_lock, flags);
 		return 0;
+	}
 
-	if (*(kbd->leds) == kbd->newleds)
+	if (*(kbd->leds) == kbd->newleds){
+		spin_unlock_irqrestore(&kbd->leds_lock, flags);
 		return 0;
+	}
 
 	*(kbd->leds) = kbd->newleds;
+	
 	kbd->led->dev = kbd->usbdev;
 	if (usb_submit_urb(kbd->led, GFP_ATOMIC))
 		pr_err("usb_submit_urb(leds) failed\n");
-
+	else
+		kbd->led_urb_submitted = true;
+	
+	spin_unlock_irqrestore(&kbd->leds_lock, flags);
+	
 	return 0;
 }
 
 static void usb_kbd_led(struct urb *urb)
 {
+	unsigned long flags;
 	struct usb_kbd *kbd = urb->context;
 
 	if (urb->status)
 		hid_warn(urb->dev, "led urb status %d received\n",
 			 urb->status);
 
-	if (*(kbd->leds) == kbd->newleds)
+	spin_lock_irqsave(&kbd->leds_lock, flags);
+
+	if (*(kbd->leds) == kbd->newleds){
+		kbd->led_urb_submitted = false;
+		spin_unlock_irqrestore(&kbd->leds_lock, flags);
 		return;
+	}
 
 	*(kbd->leds) = kbd->newleds;
+	
 	kbd->led->dev = kbd->usbdev;
-	if (usb_submit_urb(kbd->led, GFP_ATOMIC))
+	if (usb_submit_urb(kbd->led, GFP_ATOMIC)){
 		hid_err(urb->dev, "usb_submit_urb(leds) failed\n");
+		kbd->led_urb_submitted = false;
+	}
+	spin_unlock_irqrestore(&kbd->leds_lock, flags);
+	
 }
 
 static int usb_kbd_open(struct input_dev *dev)
@@ -252,6 +304,7 @@ static int usb_kbd_probe(struct usb_interface *iface,
 
 	kbd->usbdev = dev;
 	kbd->dev = input_dev;
+	spin_lock_init(&kbd->leds_lock);
 
 	if (dev->manufacturer)
 		strlcpy(kbd->name, dev->manufacturer, sizeof(kbd->name));
@@ -334,6 +387,7 @@ static void usb_kbd_disconnect(struct usb_interface *intf)
 	if (kbd) {
 		usb_kill_urb(kbd->irq);
 		input_unregister_device(kbd->dev);
+		usb_kill_urb(kbd->led);
 		usb_kbd_free_mem(interface_to_usbdev(intf), kbd);
 		kfree(kbd);
 	}
diff --git a/drivers/hv/hv_kvp.c b/drivers/hv/hv_kvp.c
index 89f52440fcf4..0e8343f585bb 100644
--- a/drivers/hv/hv_kvp.c
+++ b/drivers/hv/hv_kvp.c
@@ -212,11 +212,13 @@ kvp_respond_to_host(char *key, char *value, int error)
 	 * The windows host expects the key/value pair to be encoded
 	 * in utf16.
 	 */
-	keylen = utf8s_to_utf16s(key_name, strlen(key_name),
-				(wchar_t *)kvp_data->data.key);
+	keylen = utf8s_to_utf16s(key_name, strlen(key_name), UTF16_HOST_ENDIAN,
+				(wchar_t *) kvp_data->data.key,
+				HV_KVP_EXCHANGE_MAX_KEY_SIZE / 2);
 	kvp_data->data.key_size = 2*(keylen + 1); /* utf16 encoding */
-	valuelen = utf8s_to_utf16s(value, strlen(value),
-				(wchar_t *)kvp_data->data.value);
+	valuelen = utf8s_to_utf16s(value, strlen(value), UTF16_HOST_ENDIAN,
+				(wchar_t *) kvp_data->data.value,
+				HV_KVP_EXCHANGE_MAX_VALUE_SIZE / 2);
 	kvp_data->data.value_size = 2*(valuelen + 1); /* utf16 encoding */
 
 	kvp_data->data.value_type = REG_SZ; /* all our values are strings */
diff --git a/drivers/i2c/busses/i2c-puv3.c b/drivers/i2c/busses/i2c-puv3.c
index fac673940849..93709fbe30eb 100644
--- a/drivers/i2c/busses/i2c-puv3.c
+++ b/drivers/i2c/busses/i2c-puv3.c
@@ -276,8 +276,6 @@ static int puv3_i2c_resume(struct platform_device *dev)
 #define puv3_i2c_resume NULL
 #endif
 
-MODULE_ALIAS("platform:puv3_i2c");
-
 static struct platform_driver puv3_i2c_driver = {
 	.probe		= puv3_i2c_probe,
 	.remove		= __devexit_p(puv3_i2c_remove),
@@ -289,18 +287,8 @@ static struct platform_driver puv3_i2c_driver = {
 	}
 };
 
-static int __init puv3_i2c_init(void)
-{
-	return platform_driver_register(&puv3_i2c_driver);
-}
-
-static void __exit puv3_i2c_exit(void)
-{
-	platform_driver_unregister(&puv3_i2c_driver);
-}
-
-module_init(puv3_i2c_init);
-module_exit(puv3_i2c_exit);
+module_platform_driver(puv3_i2c_driver);
 
 MODULE_DESCRIPTION("PKUnity v3 I2C driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:puv3_i2c");
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index 46b6500c5478..6381604696d3 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -558,7 +558,7 @@ static const struct i2c_algorithm tegra_i2c_algo = {
 	.functionality	= tegra_i2c_func,
 };
 
-static int tegra_i2c_probe(struct platform_device *pdev)
+static int __devinit tegra_i2c_probe(struct platform_device *pdev)
 {
 	struct tegra_i2c_dev *i2c_dev;
 	struct tegra_i2c_platform_data *pdata = pdev->dev.platform_data;
@@ -636,7 +636,10 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 			i2c_dev->bus_clk_rate = be32_to_cpup(prop);
 	}
 
-	if (pdev->id == 3)
+	if (pdev->dev.of_node)
+		i2c_dev->is_dvc = of_device_is_compatible(pdev->dev.of_node,
+						"nvidia,tegra20-i2c-dvc");
+	else if (pdev->id == 3)
 		i2c_dev->is_dvc = 1;
 	init_completion(&i2c_dev->msg_complete);
 
@@ -690,7 +693,7 @@ err_iounmap:
 	return ret;
 }
 
-static int tegra_i2c_remove(struct platform_device *pdev)
+static int __devexit tegra_i2c_remove(struct platform_device *pdev)
 {
 	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
 	i2c_del_adapter(&i2c_dev->adapter);
@@ -742,6 +745,7 @@ static int tegra_i2c_resume(struct platform_device *pdev)
 /* Match table for of_platform binding */
 static const struct of_device_id tegra_i2c_of_match[] __devinitconst = {
 	{ .compatible = "nvidia,tegra20-i2c", },
+	{ .compatible = "nvidia,tegra20-i2c-dvc", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, tegra_i2c_of_match);
diff --git a/drivers/ide/at91_ide.c b/drivers/ide/at91_ide.c
index 6dede8f366c5..41d415529479 100644
--- a/drivers/ide/at91_ide.c
+++ b/drivers/ide/at91_ide.c
@@ -314,7 +314,7 @@ static int __init at91_ide_probe(struct platform_device *pdev)
 	apply_timings(board->chipselect, 0, ide_timing_find_mode(XFER_PIO_0), 0);
 
 	/* with GPIO interrupt we have to do quirks in handler */
-	if (board->irq_pin >= PIN_BASE)
+	if (gpio_is_valid(board->irq_pin))
 		host->irq_handler = at91_irq_handler;
 
 	host->ports[0]->select_data = board->chipselect;
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 4cf25347b015..76457d50bc34 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -369,7 +369,7 @@ static int evdev_fetch_next_event(struct evdev_client *client,
 
 	spin_lock_irq(&client->buffer_lock);
 
-	have_event = client->head != client->tail;
+	have_event = client->packet_head != client->tail;
 	if (have_event) {
 		*event = client->buffer[client->tail++];
 		client->tail &= client->bufsize - 1;
@@ -391,14 +391,13 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 	if (count < input_event_size())
 		return -EINVAL;
 
-	if (client->packet_head == client->tail && evdev->exist &&
-	    (file->f_flags & O_NONBLOCK))
-		return -EAGAIN;
-
-	retval = wait_event_interruptible(evdev->wait,
-		client->packet_head != client->tail || !evdev->exist);
-	if (retval)
-		return retval;
+	if (!(file->f_flags & O_NONBLOCK)) {
+		retval = wait_event_interruptible(evdev->wait,
+				client->packet_head != client->tail ||
+				!evdev->exist);
+		if (retval)
+			return retval;
+	}
 
 	if (!evdev->exist)
 		return -ENODEV;
@@ -412,6 +411,9 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 		retval += input_event_size();
 	}
 
+	if (retval == 0 && (file->f_flags & O_NONBLOCK))
+		return -EAGAIN;
+
 	return retval;
 }
 
diff --git a/drivers/input/input-polldev.c b/drivers/input/input-polldev.c
index 7dfe1009fae0..7f161d93203c 100644
--- a/drivers/input/input-polldev.c
+++ b/drivers/input/input-polldev.c
@@ -84,10 +84,12 @@ static ssize_t input_polldev_set_poll(struct device *dev,
 {
 	struct input_polled_dev *polldev = dev_get_drvdata(dev);
 	struct input_dev *input = polldev->input;
-	unsigned long interval;
+	unsigned int interval;
+	int err;
 
-	if (strict_strtoul(buf, 0, &interval))
-		return -EINVAL;
+	err = kstrtouint(buf, 0, &interval);
+	if (err)
+		return err;
 
 	if (interval < polldev->poll_interval_min)
 		return -EINVAL;
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index 615c21f2a553..cdc385b2cf7d 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -221,6 +221,22 @@ config KEYBOARD_TCA6416
 	  To compile this driver as a module, choose M here: the
 	  module will be called tca6416_keypad.
 
+config KEYBOARD_TCA8418
+	tristate "TCA8418 Keypad Support"
+	depends on I2C
+	help
+	  This driver implements basic keypad functionality
+	  for keys connected through TCA8418 keypad decoder.
+
+	  Say Y here if your device has keys connected to
+	  TCA8418 keypad decoder.
+
+	  If enabled the complete TCA8418 device will be managed through
+	  this driver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tca8418_keypad.
+
 config KEYBOARD_MATRIX
 	tristate "GPIO driven matrix keypad support"
 	depends on GENERIC_GPIO
@@ -425,9 +441,10 @@ config KEYBOARD_PMIC8XXX
 
 config KEYBOARD_SAMSUNG
 	tristate "Samsung keypad support"
-	depends on SAMSUNG_DEV_KEYPAD
+	depends on HAVE_CLK
 	help
-	  Say Y here if you want to use the Samsung keypad.
+	  Say Y here if you want to use the keypad on your Samsung mobile
+	  device.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called samsung-keypad.
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index ddde0fd476f7..df7061f12918 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_KEYBOARD_EP93XX)		+= ep93xx_keypad.o
 obj-$(CONFIG_KEYBOARD_GPIO)		+= gpio_keys.o
 obj-$(CONFIG_KEYBOARD_GPIO_POLLED)	+= gpio_keys_polled.o
 obj-$(CONFIG_KEYBOARD_TCA6416)		+= tca6416-keypad.o
+obj-$(CONFIG_KEYBOARD_TCA8418)		+= tca8418_keypad.o
 obj-$(CONFIG_KEYBOARD_HIL)		+= hil_kbd.o
 obj-$(CONFIG_KEYBOARD_HIL_OLD)		+= hilkbd.o
 obj-$(CONFIG_KEYBOARD_IMX)		+= imx_keypad.o
diff --git a/drivers/input/keyboard/adp5520-keys.c b/drivers/input/keyboard/adp5520-keys.c
index 3db8006dac3a..e9e8674dfda1 100644
--- a/drivers/input/keyboard/adp5520-keys.c
+++ b/drivers/input/keyboard/adp5520-keys.c
@@ -202,18 +202,7 @@ static struct platform_driver adp5520_keys_driver = {
 	.probe		= adp5520_keys_probe,
 	.remove		= __devexit_p(adp5520_keys_remove),
 };
-
-static int __init adp5520_keys_init(void)
-{
-	return platform_driver_register(&adp5520_keys_driver);
-}
-module_init(adp5520_keys_init);
-
-static void __exit adp5520_keys_exit(void)
-{
-	platform_driver_unregister(&adp5520_keys_driver);
-}
-module_exit(adp5520_keys_exit);
+module_platform_driver(adp5520_keys_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Keys ADP5520 Driver");
diff --git a/drivers/input/keyboard/amikbd.c b/drivers/input/keyboard/amikbd.c
index 79172af164f2..6df5f6aa7908 100644
--- a/drivers/input/keyboard/amikbd.c
+++ b/drivers/input/keyboard/amikbd.c
@@ -259,19 +259,6 @@ static struct platform_driver amikbd_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init amikbd_init(void)
-{
-	return platform_driver_probe(&amikbd_driver, amikbd_probe);
-}
-
-module_init(amikbd_init);
-
-static void __exit amikbd_exit(void)
-{
-	platform_driver_unregister(&amikbd_driver);
-}
-
-module_exit(amikbd_exit);
+module_platform_driver(amikbd_driver);
 
 MODULE_ALIAS("platform:amiga-keyboard");
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index 19cfc0cf558c..e05a2e7073c6 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -1305,7 +1305,7 @@ static ssize_t atkbd_show_extra(struct atkbd *atkbd, char *buf)
 static ssize_t atkbd_set_extra(struct atkbd *atkbd, const char *buf, size_t count)
 {
 	struct input_dev *old_dev, *new_dev;
-	unsigned long value;
+	unsigned int value;
 	int err;
 	bool old_extra;
 	unsigned char old_set;
@@ -1313,7 +1313,11 @@ static ssize_t atkbd_set_extra(struct atkbd *atkbd, const char *buf, size_t coun
 	if (!atkbd->write)
 		return -EIO;
 
-	if (strict_strtoul(buf, 10, &value) || value > 1)
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value > 1)
 		return -EINVAL;
 
 	if (atkbd->extra != value) {
@@ -1389,11 +1393,15 @@ static ssize_t atkbd_show_scroll(struct atkbd *atkbd, char *buf)
 static ssize_t atkbd_set_scroll(struct atkbd *atkbd, const char *buf, size_t count)
 {
 	struct input_dev *old_dev, *new_dev;
-	unsigned long value;
+	unsigned int value;
 	int err;
 	bool old_scroll;
 
-	if (strict_strtoul(buf, 10, &value) || value > 1)
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value > 1)
 		return -EINVAL;
 
 	if (atkbd->scroll != value) {
@@ -1433,7 +1441,7 @@ static ssize_t atkbd_show_set(struct atkbd *atkbd, char *buf)
 static ssize_t atkbd_set_set(struct atkbd *atkbd, const char *buf, size_t count)
 {
 	struct input_dev *old_dev, *new_dev;
-	unsigned long value;
+	unsigned int value;
 	int err;
 	unsigned char old_set;
 	bool old_extra;
@@ -1441,7 +1449,11 @@ static ssize_t atkbd_set_set(struct atkbd *atkbd, const char *buf, size_t count)
 	if (!atkbd->write)
 		return -EIO;
 
-	if (strict_strtoul(buf, 10, &value) || (value != 2 && value != 3))
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value != 2 && value != 3)
 		return -EINVAL;
 
 	if (atkbd->set != value) {
@@ -1484,14 +1496,18 @@ static ssize_t atkbd_show_softrepeat(struct atkbd *atkbd, char *buf)
 static ssize_t atkbd_set_softrepeat(struct atkbd *atkbd, const char *buf, size_t count)
 {
 	struct input_dev *old_dev, *new_dev;
-	unsigned long value;
+	unsigned int value;
 	int err;
 	bool old_softrepeat, old_softraw;
 
 	if (!atkbd->write)
 		return -EIO;
 
-	if (strict_strtoul(buf, 10, &value) || value > 1)
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value > 1)
 		return -EINVAL;
 
 	if (atkbd->softrepeat != value) {
@@ -1534,11 +1550,15 @@ static ssize_t atkbd_show_softraw(struct atkbd *atkbd, char *buf)
 static ssize_t atkbd_set_softraw(struct atkbd *atkbd, const char *buf, size_t count)
 {
 	struct input_dev *old_dev, *new_dev;
-	unsigned long value;
+	unsigned int value;
 	int err;
 	bool old_softraw;
 
-	if (strict_strtoul(buf, 10, &value) || value > 1)
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value > 1)
 		return -EINVAL;
 
 	if (atkbd->softraw != value) {
diff --git a/drivers/input/keyboard/bf54x-keys.c b/drivers/input/keyboard/bf54x-keys.c
index 7d989603f875..8eb9116e0a5f 100644
--- a/drivers/input/keyboard/bf54x-keys.c
+++ b/drivers/input/keyboard/bf54x-keys.c
@@ -384,7 +384,7 @@ static int bfin_kpad_resume(struct platform_device *pdev)
 # define bfin_kpad_resume  NULL
 #endif
 
-struct platform_driver bfin_kpad_device_driver = {
+static struct platform_driver bfin_kpad_device_driver = {
 	.driver		= {
 		.name	= DRV_NAME,
 		.owner	= THIS_MODULE,
@@ -394,19 +394,7 @@ struct platform_driver bfin_kpad_device_driver = {
 	.suspend	= bfin_kpad_suspend,
 	.resume		= bfin_kpad_resume,
 };
-
-static int __init bfin_kpad_init(void)
-{
-	return platform_driver_register(&bfin_kpad_device_driver);
-}
-
-static void __exit bfin_kpad_exit(void)
-{
-	platform_driver_unregister(&bfin_kpad_device_driver);
-}
-
-module_init(bfin_kpad_init);
-module_exit(bfin_kpad_exit);
+module_platform_driver(bfin_kpad_device_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
diff --git a/drivers/input/keyboard/davinci_keyscan.c b/drivers/input/keyboard/davinci_keyscan.c
index 9d82b3aeff5e..469825247552 100644
--- a/drivers/input/keyboard/davinci_keyscan.c
+++ b/drivers/input/keyboard/davinci_keyscan.c
@@ -328,18 +328,7 @@ static struct platform_driver davinci_ks_driver = {
 	},
 	.remove	= __devexit_p(davinci_ks_remove),
 };
-
-static int __init davinci_ks_init(void)
-{
-	return platform_driver_probe(&davinci_ks_driver, davinci_ks_probe);
-}
-module_init(davinci_ks_init);
-
-static void __exit davinci_ks_exit(void)
-{
-	platform_driver_unregister(&davinci_ks_driver);
-}
-module_exit(davinci_ks_exit);
+module_platform_driver(davinci_ks_driver);
 
 MODULE_AUTHOR("Miguel Aguilar");
 MODULE_DESCRIPTION("Texas Instruments DaVinci Key Scan Driver");
diff --git a/drivers/input/keyboard/ep93xx_keypad.c b/drivers/input/keyboard/ep93xx_keypad.c
index 4662c5da8018..0ba69f3fcb52 100644
--- a/drivers/input/keyboard/ep93xx_keypad.c
+++ b/drivers/input/keyboard/ep93xx_keypad.c
@@ -390,19 +390,7 @@ static struct platform_driver ep93xx_keypad_driver = {
 	.suspend	= ep93xx_keypad_suspend,
 	.resume		= ep93xx_keypad_resume,
 };
-
-static int __init ep93xx_keypad_init(void)
-{
-	return platform_driver_register(&ep93xx_keypad_driver);
-}
-
-static void __exit ep93xx_keypad_exit(void)
-{
-	platform_driver_unregister(&ep93xx_keypad_driver);
-}
-
-module_init(ep93xx_keypad_init);
-module_exit(ep93xx_keypad_exit);
+module_platform_driver(ep93xx_keypad_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("H Hartley Sweeten <hsweeten@visionengravers.com>");
diff --git a/drivers/input/keyboard/gpio_keys_polled.c b/drivers/input/keyboard/gpio_keys_polled.c
index 4c17aff20657..20c8ab172214 100644
--- a/drivers/input/keyboard/gpio_keys_polled.c
+++ b/drivers/input/keyboard/gpio_keys_polled.c
@@ -241,19 +241,7 @@ static struct platform_driver gpio_keys_polled_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init gpio_keys_polled_init(void)
-{
-	return platform_driver_register(&gpio_keys_polled_driver);
-}
-
-static void __exit gpio_keys_polled_exit(void)
-{
-	platform_driver_unregister(&gpio_keys_polled_driver);
-}
-
-module_init(gpio_keys_polled_init);
-module_exit(gpio_keys_polled_exit);
+module_platform_driver(gpio_keys_polled_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Gabor Juhos <juhosg@openwrt.org>");
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index ccebd2d09151..fb87b3bcadb9 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -619,19 +619,7 @@ static struct platform_driver imx_keypad_driver = {
 	.probe		= imx_keypad_probe,
 	.remove		= __devexit_p(imx_keypad_remove),
 };
-
-static int __init imx_keypad_init(void)
-{
-	return platform_driver_register(&imx_keypad_driver);
-}
-
-static void __exit imx_keypad_exit(void)
-{
-	platform_driver_unregister(&imx_keypad_driver);
-}
-
-module_init(imx_keypad_init);
-module_exit(imx_keypad_exit);
+module_platform_driver(imx_keypad_driver);
 
 MODULE_AUTHOR("Alberto Panizzo <maramaopercheseimorto@gmail.com>");
 MODULE_DESCRIPTION("IMX Keypad Port Driver");
diff --git a/drivers/input/keyboard/jornada680_kbd.c b/drivers/input/keyboard/jornada680_kbd.c
index 7197c5698747..24f3ea01c4d5 100644
--- a/drivers/input/keyboard/jornada680_kbd.c
+++ b/drivers/input/keyboard/jornada680_kbd.c
@@ -260,19 +260,7 @@ static struct platform_driver jornada680kbd_driver = {
 	.probe	= jornada680kbd_probe,
 	.remove	= __devexit_p(jornada680kbd_remove),
 };
-
-static int __init jornada680kbd_init(void)
-{
-	return platform_driver_register(&jornada680kbd_driver);
-}
-
-static void __exit jornada680kbd_exit(void)
-{
-	platform_driver_unregister(&jornada680kbd_driver);
-}
-
-module_init(jornada680kbd_init);
-module_exit(jornada680kbd_exit);
+module_platform_driver(jornada680kbd_driver);
 
 MODULE_AUTHOR("Kristoffer Ericson <kristoffer.ericson@gmail.com>");
 MODULE_DESCRIPTION("HP Jornada 620/660/680/690 Keyboard Driver");
diff --git a/drivers/input/keyboard/jornada720_kbd.c b/drivers/input/keyboard/jornada720_kbd.c
index 0aa6740e60d0..eeafc30b207b 100644
--- a/drivers/input/keyboard/jornada720_kbd.c
+++ b/drivers/input/keyboard/jornada720_kbd.c
@@ -174,16 +174,4 @@ static struct platform_driver jornada720_kbd_driver = {
 	.probe   = jornada720_kbd_probe,
 	.remove  = __devexit_p(jornada720_kbd_remove),
 };
-
-static int __init jornada720_kbd_init(void)
-{
-	return platform_driver_register(&jornada720_kbd_driver);
-}
-
-static void __exit jornada720_kbd_exit(void)
-{
-	platform_driver_unregister(&jornada720_kbd_driver);
-}
-
-module_init(jornada720_kbd_init);
-module_exit(jornada720_kbd_exit);
+module_platform_driver(jornada720_kbd_driver);
diff --git a/drivers/input/keyboard/lm8323.c b/drivers/input/keyboard/lm8323.c
index 82d1dc8badd5..21823bfc7911 100644
--- a/drivers/input/keyboard/lm8323.c
+++ b/drivers/input/keyboard/lm8323.c
@@ -545,13 +545,12 @@ static ssize_t lm8323_pwm_store_time(struct device *dev,
 {
 	struct led_classdev *led_cdev = dev_get_drvdata(dev);
 	struct lm8323_pwm *pwm = cdev_to_pwm(led_cdev);
-	int ret;
-	unsigned long time;
+	int ret, time;
 
-	ret = strict_strtoul(buf, 10, &time);
+	ret = kstrtoint(buf, 10, &time);
 	/* Numbers only, please. */
 	if (ret)
-		return -EINVAL;
+		return ret;
 
 	pwm->fade_time = time;
 
@@ -613,9 +612,9 @@ static ssize_t lm8323_set_disable(struct device *dev,
 {
 	struct lm8323_chip *lm = dev_get_drvdata(dev);
 	int ret;
-	unsigned long i;
+	unsigned int i;
 
-	ret = strict_strtoul(buf, 10, &i);
+	ret = kstrtouint(buf, 10, &i);
 
 	mutex_lock(&lm->lock);
 	lm->kp_enabled = !i;
diff --git a/drivers/input/keyboard/matrix_keypad.c b/drivers/input/keyboard/matrix_keypad.c
index e2ae657717ea..9b223d73de32 100644
--- a/drivers/input/keyboard/matrix_keypad.c
+++ b/drivers/input/keyboard/matrix_keypad.c
@@ -496,19 +496,7 @@ static struct platform_driver matrix_keypad_driver = {
 #endif
 	},
 };
-
-static int __init matrix_keypad_init(void)
-{
-	return platform_driver_register(&matrix_keypad_driver);
-}
-
-static void __exit matrix_keypad_exit(void)
-{
-	platform_driver_unregister(&matrix_keypad_driver);
-}
-
-module_init(matrix_keypad_init);
-module_exit(matrix_keypad_exit);
+module_platform_driver(matrix_keypad_driver);
 
 MODULE_AUTHOR("Marek Vasut <marek.vasut@gmail.com>");
 MODULE_DESCRIPTION("GPIO Driven Matrix Keypad Driver");
diff --git a/drivers/input/keyboard/nomadik-ske-keypad.c b/drivers/input/keyboard/nomadik-ske-keypad.c
index fcdec5e2b297..5a71e55c9c54 100644
--- a/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -379,7 +379,7 @@ static const struct dev_pm_ops ske_keypad_dev_pm_ops = {
 };
 #endif
 
-struct platform_driver ske_keypad_driver = {
+static struct platform_driver ske_keypad_driver = {
 	.driver = {
 		.name = "nmk-ske-keypad",
 		.owner  = THIS_MODULE,
@@ -390,18 +390,7 @@ struct platform_driver ske_keypad_driver = {
 	.probe = ske_keypad_probe,
 	.remove = __devexit_p(ske_keypad_remove),
 };
-
-static int __init ske_keypad_init(void)
-{
-	return platform_driver_probe(&ske_keypad_driver, ske_keypad_probe);
-}
-module_init(ske_keypad_init);
-
-static void __exit ske_keypad_exit(void)
-{
-	platform_driver_unregister(&ske_keypad_driver);
-}
-module_exit(ske_keypad_exit);
+module_platform_driver(ske_keypad_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Naveen Kumar <naveen.gaddipati@stericsson.com> / Sundar Iyer <sundar.iyer@stericsson.com>");
diff --git a/drivers/input/keyboard/omap-keypad.c b/drivers/input/keyboard/omap-keypad.c
index 323bcdfff248..6b630d9d3dff 100644
--- a/drivers/input/keyboard/omap-keypad.c
+++ b/drivers/input/keyboard/omap-keypad.c
@@ -473,20 +473,7 @@ static struct platform_driver omap_kp_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init omap_kp_init(void)
-{
-	printk(KERN_INFO "OMAP Keypad Driver\n");
-	return platform_driver_register(&omap_kp_driver);
-}
-
-static void __exit omap_kp_exit(void)
-{
-	platform_driver_unregister(&omap_kp_driver);
-}
-
-module_init(omap_kp_init);
-module_exit(omap_kp_exit);
+module_platform_driver(omap_kp_driver);
 
 MODULE_AUTHOR("Timo Teräs");
 MODULE_DESCRIPTION("OMAP Keypad Driver");
diff --git a/drivers/input/keyboard/omap4-keypad.c b/drivers/input/keyboard/omap4-keypad.c
index c51a3c4a7feb..d5c5d77f4b82 100644
--- a/drivers/input/keyboard/omap4-keypad.c
+++ b/drivers/input/keyboard/omap4-keypad.c
@@ -335,18 +335,7 @@ static struct platform_driver omap4_keypad_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init omap4_keypad_init(void)
-{
-	return platform_driver_register(&omap4_keypad_driver);
-}
-module_init(omap4_keypad_init);
-
-static void __exit omap4_keypad_exit(void)
-{
-	platform_driver_unregister(&omap4_keypad_driver);
-}
-module_exit(omap4_keypad_exit);
+module_platform_driver(omap4_keypad_driver);
 
 MODULE_AUTHOR("Texas Instruments");
 MODULE_DESCRIPTION("OMAP4 Keypad Driver");
diff --git a/drivers/input/keyboard/opencores-kbd.c b/drivers/input/keyboard/opencores-kbd.c
index 1f1a5563f60a..abe728c7b88e 100644
--- a/drivers/input/keyboard/opencores-kbd.c
+++ b/drivers/input/keyboard/opencores-kbd.c
@@ -163,18 +163,7 @@ static struct platform_driver opencores_kbd_device_driver = {
 		.name = "opencores-kbd",
 	},
 };
-
-static int __init opencores_kbd_init(void)
-{
-	return platform_driver_register(&opencores_kbd_device_driver);
-}
-module_init(opencores_kbd_init);
-
-static void __exit opencores_kbd_exit(void)
-{
-	platform_driver_unregister(&opencores_kbd_device_driver);
-}
-module_exit(opencores_kbd_exit);
+module_platform_driver(opencores_kbd_device_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Javier Herrero <jherrero@hvsistemas.es>");
diff --git a/drivers/input/keyboard/pmic8xxx-keypad.c b/drivers/input/keyboard/pmic8xxx-keypad.c
index e7cc51d0fb34..01a1c9f8a383 100644
--- a/drivers/input/keyboard/pmic8xxx-keypad.c
+++ b/drivers/input/keyboard/pmic8xxx-keypad.c
@@ -780,18 +780,7 @@ static struct platform_driver pmic8xxx_kp_driver = {
 		.pm = &pm8xxx_kp_pm_ops,
 	},
 };
-
-static int __init pmic8xxx_kp_init(void)
-{
-	return platform_driver_register(&pmic8xxx_kp_driver);
-}
-module_init(pmic8xxx_kp_init);
-
-static void __exit pmic8xxx_kp_exit(void)
-{
-	platform_driver_unregister(&pmic8xxx_kp_driver);
-}
-module_exit(pmic8xxx_kp_exit);
+module_platform_driver(pmic8xxx_kp_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("PMIC8XXX keypad driver");
diff --git a/drivers/input/keyboard/pxa27x_keypad.c b/drivers/input/keyboard/pxa27x_keypad.c
index eca6ae63de14..29fe1b2be1c1 100644
--- a/drivers/input/keyboard/pxa27x_keypad.c
+++ b/drivers/input/keyboard/pxa27x_keypad.c
@@ -602,19 +602,7 @@ static struct platform_driver pxa27x_keypad_driver = {
 #endif
 	},
 };
-
-static int __init pxa27x_keypad_init(void)
-{
-	return platform_driver_register(&pxa27x_keypad_driver);
-}
-
-static void __exit pxa27x_keypad_exit(void)
-{
-	platform_driver_unregister(&pxa27x_keypad_driver);
-}
-
-module_init(pxa27x_keypad_init);
-module_exit(pxa27x_keypad_exit);
+module_platform_driver(pxa27x_keypad_driver);
 
 MODULE_DESCRIPTION("PXA27x Keypad Controller Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/input/keyboard/pxa930_rotary.c b/drivers/input/keyboard/pxa930_rotary.c
index 35451bf780c7..d7f1134b789e 100644
--- a/drivers/input/keyboard/pxa930_rotary.c
+++ b/drivers/input/keyboard/pxa930_rotary.c
@@ -195,18 +195,7 @@ static struct platform_driver pxa930_rotary_driver = {
 	.probe		= pxa930_rotary_probe,
 	.remove		= __devexit_p(pxa930_rotary_remove),
 };
-
-static int __init pxa930_rotary_init(void)
-{
-	return platform_driver_register(&pxa930_rotary_driver);
-}
-module_init(pxa930_rotary_init);
-
-static void __exit pxa930_rotary_exit(void)
-{
-	platform_driver_unregister(&pxa930_rotary_driver);
-}
-module_exit(pxa930_rotary_exit);
+module_platform_driver(pxa930_rotary_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Driver for PXA93x Enhanced Rotary Controller");
diff --git a/drivers/input/keyboard/samsung-keypad.c b/drivers/input/keyboard/samsung-keypad.c
index f689f49e3109..17ba7f9f80f3 100644
--- a/drivers/input/keyboard/samsung-keypad.c
+++ b/drivers/input/keyboard/samsung-keypad.c
@@ -20,9 +20,13 @@
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
 #include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 #include <linux/sched.h>
-#include <plat/keypad.h>
+#include <linux/input/samsung-keypad.h>
 
 #define SAMSUNG_KEYIFCON			0x00
 #define SAMSUNG_KEYIFSTSCLR			0x04
@@ -63,36 +67,33 @@ enum samsung_keypad_type {
 
 struct samsung_keypad {
 	struct input_dev *input_dev;
+	struct platform_device *pdev;
 	struct clk *clk;
 	void __iomem *base;
 	wait_queue_head_t wait;
 	bool stopped;
+	bool wake_enabled;
 	int irq;
+	enum samsung_keypad_type type;
 	unsigned int row_shift;
 	unsigned int rows;
 	unsigned int cols;
 	unsigned int row_state[SAMSUNG_MAX_COLS];
+#ifdef CONFIG_OF
+	int row_gpios[SAMSUNG_MAX_ROWS];
+	int col_gpios[SAMSUNG_MAX_COLS];
+#endif
 	unsigned short keycodes[];
 };
 
-static int samsung_keypad_is_s5pv210(struct device *dev)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	enum samsung_keypad_type type =
-		platform_get_device_id(pdev)->driver_data;
-
-	return type == KEYPAD_TYPE_S5PV210;
-}
-
 static void samsung_keypad_scan(struct samsung_keypad *keypad,
 				unsigned int *row_state)
 {
-	struct device *dev = keypad->input_dev->dev.parent;
 	unsigned int col;
 	unsigned int val;
 
 	for (col = 0; col < keypad->cols; col++) {
-		if (samsung_keypad_is_s5pv210(dev)) {
+		if (keypad->type == KEYPAD_TYPE_S5PV210) {
 			val = S5PV210_KEYIFCOLEN_MASK;
 			val &= ~(1 << col) << 8;
 		} else {
@@ -158,6 +159,8 @@ static irqreturn_t samsung_keypad_irq(int irq, void *dev_id)
 	unsigned int val;
 	bool key_down;
 
+	pm_runtime_get_sync(&keypad->pdev->dev);
+
 	do {
 		val = readl(keypad->base + SAMSUNG_KEYIFSTSCLR);
 		/* Clear interrupt. */
@@ -172,6 +175,8 @@ static irqreturn_t samsung_keypad_irq(int irq, void *dev_id)
 
 	} while (key_down && !keypad->stopped);
 
+	pm_runtime_put_sync(&keypad->pdev->dev);
+
 	return IRQ_HANDLED;
 }
 
@@ -179,6 +184,8 @@ static void samsung_keypad_start(struct samsung_keypad *keypad)
 {
 	unsigned int val;
 
+	pm_runtime_get_sync(&keypad->pdev->dev);
+
 	/* Tell IRQ thread that it may poll the device. */
 	keypad->stopped = false;
 
@@ -191,12 +198,16 @@ static void samsung_keypad_start(struct samsung_keypad *keypad)
 
 	/* KEYIFCOL reg clear. */
 	writel(0, keypad->base + SAMSUNG_KEYIFCOL);
+
+	pm_runtime_put_sync(&keypad->pdev->dev);
 }
 
 static void samsung_keypad_stop(struct samsung_keypad *keypad)
 {
 	unsigned int val;
 
+	pm_runtime_get_sync(&keypad->pdev->dev);
+
 	/* Signal IRQ thread to stop polling and disable the handler. */
 	keypad->stopped = true;
 	wake_up(&keypad->wait);
@@ -217,6 +228,8 @@ static void samsung_keypad_stop(struct samsung_keypad *keypad)
 	 * re-enable the handler.
 	 */
 	enable_irq(keypad->irq);
+
+	pm_runtime_put_sync(&keypad->pdev->dev);
 }
 
 static int samsung_keypad_open(struct input_dev *input_dev)
@@ -235,6 +248,126 @@ static void samsung_keypad_close(struct input_dev *input_dev)
 	samsung_keypad_stop(keypad);
 }
 
+#ifdef CONFIG_OF
+static struct samsung_keypad_platdata *samsung_keypad_parse_dt(
+				struct device *dev)
+{
+	struct samsung_keypad_platdata *pdata;
+	struct matrix_keymap_data *keymap_data;
+	uint32_t *keymap, num_rows = 0, num_cols = 0;
+	struct device_node *np = dev->of_node, *key_np;
+	unsigned int key_count = 0;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(dev, "could not allocate memory for platform data\n");
+		return NULL;
+	}
+
+	of_property_read_u32(np, "samsung,keypad-num-rows", &num_rows);
+	of_property_read_u32(np, "samsung,keypad-num-columns", &num_cols);
+	if (!num_rows || !num_cols) {
+		dev_err(dev, "number of keypad rows/columns not specified\n");
+		return NULL;
+	}
+	pdata->rows = num_rows;
+	pdata->cols = num_cols;
+
+	keymap_data = devm_kzalloc(dev, sizeof(*keymap_data), GFP_KERNEL);
+	if (!keymap_data) {
+		dev_err(dev, "could not allocate memory for keymap data\n");
+		return NULL;
+	}
+	pdata->keymap_data = keymap_data;
+
+	for_each_child_of_node(np, key_np)
+		key_count++;
+
+	keymap_data->keymap_size = key_count;
+	keymap = devm_kzalloc(dev, sizeof(uint32_t) * key_count, GFP_KERNEL);
+	if (!keymap) {
+		dev_err(dev, "could not allocate memory for keymap\n");
+		return NULL;
+	}
+	keymap_data->keymap = keymap;
+
+	for_each_child_of_node(np, key_np) {
+		u32 row, col, key_code;
+		of_property_read_u32(key_np, "keypad,row", &row);
+		of_property_read_u32(key_np, "keypad,column", &col);
+		of_property_read_u32(key_np, "linux,code", &key_code);
+		*keymap++ = KEY(row, col, key_code);
+	}
+
+	if (of_get_property(np, "linux,input-no-autorepeat", NULL))
+		pdata->no_autorepeat = true;
+	if (of_get_property(np, "linux,input-wakeup", NULL))
+		pdata->wakeup = true;
+
+	return pdata;
+}
+
+static void samsung_keypad_parse_dt_gpio(struct device *dev,
+				struct samsung_keypad *keypad)
+{
+	struct device_node *np = dev->of_node;
+	int gpio, ret, row, col;
+
+	for (row = 0; row < keypad->rows; row++) {
+		gpio = of_get_named_gpio(np, "row-gpios", row);
+		keypad->row_gpios[row] = gpio;
+		if (!gpio_is_valid(gpio)) {
+			dev_err(dev, "keypad row[%d]: invalid gpio %d\n",
+					row, gpio);
+			continue;
+		}
+
+		ret = gpio_request(gpio, "keypad-row");
+		if (ret)
+			dev_err(dev, "keypad row[%d] gpio request failed\n",
+					row);
+	}
+
+	for (col = 0; col < keypad->cols; col++) {
+		gpio = of_get_named_gpio(np, "col-gpios", col);
+		keypad->col_gpios[col] = gpio;
+		if (!gpio_is_valid(gpio)) {
+			dev_err(dev, "keypad column[%d]: invalid gpio %d\n",
+					col, gpio);
+			continue;
+		}
+
+		ret = gpio_request(gpio, "keypad-col");
+		if (ret)
+			dev_err(dev, "keypad column[%d] gpio request failed\n",
+					col);
+	}
+}
+
+static void samsung_keypad_dt_gpio_free(struct samsung_keypad *keypad)
+{
+	int cnt;
+
+	for (cnt = 0; cnt < keypad->rows; cnt++)
+		if (gpio_is_valid(keypad->row_gpios[cnt]))
+			gpio_free(keypad->row_gpios[cnt]);
+
+	for (cnt = 0; cnt < keypad->cols; cnt++)
+		if (gpio_is_valid(keypad->col_gpios[cnt]))
+			gpio_free(keypad->col_gpios[cnt]);
+}
+#else
+static
+struct samsung_keypad_platdata *samsung_keypad_parse_dt(struct device *dev)
+{
+	return NULL;
+}
+
+static void samsung_keypad_dt_gpio_free(struct samsung_keypad *keypad)
+{
+}
+#endif
+
 static int __devinit samsung_keypad_probe(struct platform_device *pdev)
 {
 	const struct samsung_keypad_platdata *pdata;
@@ -246,7 +379,10 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
 	unsigned int keymap_size;
 	int error;
 
-	pdata = pdev->dev.platform_data;
+	if (pdev->dev.of_node)
+		pdata = samsung_keypad_parse_dt(&pdev->dev);
+	else
+		pdata = pdev->dev.platform_data;
 	if (!pdata) {
 		dev_err(&pdev->dev, "no platform data defined\n");
 		return -EINVAL;
@@ -298,11 +434,23 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
 	}
 
 	keypad->input_dev = input_dev;
+	keypad->pdev = pdev;
 	keypad->row_shift = row_shift;
 	keypad->rows = pdata->rows;
 	keypad->cols = pdata->cols;
+	keypad->stopped = true;
 	init_waitqueue_head(&keypad->wait);
 
+	if (pdev->dev.of_node) {
+#ifdef CONFIG_OF
+		samsung_keypad_parse_dt_gpio(&pdev->dev, keypad);
+		keypad->type = of_device_is_compatible(pdev->dev.of_node,
+					"samsung,s5pv210-keypad");
+#endif
+	} else {
+		keypad->type = platform_get_device_id(pdev)->driver_data;
+	}
+
 	input_dev->name = pdev->name;
 	input_dev->id.bustype = BUS_HOST;
 	input_dev->dev.parent = &pdev->dev;
@@ -337,18 +485,29 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
 		goto err_put_clk;
 	}
 
+	device_init_wakeup(&pdev->dev, pdata->wakeup);
+	platform_set_drvdata(pdev, keypad);
+	pm_runtime_enable(&pdev->dev);
+
 	error = input_register_device(keypad->input_dev);
 	if (error)
 		goto err_free_irq;
 
-	device_init_wakeup(&pdev->dev, pdata->wakeup);
-	platform_set_drvdata(pdev, keypad);
+	if (pdev->dev.of_node) {
+		devm_kfree(&pdev->dev, (void *)pdata->keymap_data->keymap);
+		devm_kfree(&pdev->dev, (void *)pdata->keymap_data);
+		devm_kfree(&pdev->dev, (void *)pdata);
+	}
 	return 0;
 
 err_free_irq:
 	free_irq(keypad->irq, keypad);
+	pm_runtime_disable(&pdev->dev);
+	device_init_wakeup(&pdev->dev, 0);
+	platform_set_drvdata(pdev, NULL);
 err_put_clk:
 	clk_put(keypad->clk);
+	samsung_keypad_dt_gpio_free(keypad);
 err_unmap_base:
 	iounmap(keypad->base);
 err_free_mem:
@@ -362,6 +521,7 @@ static int __devexit samsung_keypad_remove(struct platform_device *pdev)
 {
 	struct samsung_keypad *keypad = platform_get_drvdata(pdev);
 
+	pm_runtime_disable(&pdev->dev);
 	device_init_wakeup(&pdev->dev, 0);
 	platform_set_drvdata(pdev, NULL);
 
@@ -374,6 +534,7 @@ static int __devexit samsung_keypad_remove(struct platform_device *pdev)
 	free_irq(keypad->irq, keypad);
 
 	clk_put(keypad->clk);
+	samsung_keypad_dt_gpio_free(keypad);
 
 	iounmap(keypad->base);
 	kfree(keypad);
@@ -381,11 +542,57 @@ static int __devexit samsung_keypad_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_RUNTIME
+static int samsung_keypad_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct samsung_keypad *keypad = platform_get_drvdata(pdev);
+	unsigned int val;
+	int error;
+
+	if (keypad->stopped)
+		return 0;
+
+	/* This may fail on some SoCs due to lack of controller support */
+	error = enable_irq_wake(keypad->irq);
+	if (!error)
+		keypad->wake_enabled = true;
+
+	val = readl(keypad->base + SAMSUNG_KEYIFCON);
+	val |= SAMSUNG_KEYIFCON_WAKEUPEN;
+	writel(val, keypad->base + SAMSUNG_KEYIFCON);
+
+	clk_disable(keypad->clk);
+
+	return 0;
+}
+
+static int samsung_keypad_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct samsung_keypad *keypad = platform_get_drvdata(pdev);
+	unsigned int val;
+
+	if (keypad->stopped)
+		return 0;
+
+	clk_enable(keypad->clk);
+
+	val = readl(keypad->base + SAMSUNG_KEYIFCON);
+	val &= ~SAMSUNG_KEYIFCON_WAKEUPEN;
+	writel(val, keypad->base + SAMSUNG_KEYIFCON);
+
+	if (keypad->wake_enabled)
+		disable_irq_wake(keypad->irq);
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_PM_SLEEP
 static void samsung_keypad_toggle_wakeup(struct samsung_keypad *keypad,
 					 bool enable)
 {
-	struct device *dev = keypad->input_dev->dev.parent;
 	unsigned int val;
 
 	clk_enable(keypad->clk);
@@ -393,11 +600,11 @@ static void samsung_keypad_toggle_wakeup(struct samsung_keypad *keypad,
 	val = readl(keypad->base + SAMSUNG_KEYIFCON);
 	if (enable) {
 		val |= SAMSUNG_KEYIFCON_WAKEUPEN;
-		if (device_may_wakeup(dev))
+		if (device_may_wakeup(&keypad->pdev->dev))
 			enable_irq_wake(keypad->irq);
 	} else {
 		val &= ~SAMSUNG_KEYIFCON_WAKEUPEN;
-		if (device_may_wakeup(dev))
+		if (device_may_wakeup(&keypad->pdev->dev))
 			disable_irq_wake(keypad->irq);
 	}
 	writel(val, keypad->base + SAMSUNG_KEYIFCON);
@@ -440,11 +647,23 @@ static int samsung_keypad_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static const struct dev_pm_ops samsung_keypad_pm_ops = {
-	.suspend	= samsung_keypad_suspend,
-	.resume		= samsung_keypad_resume,
+	SET_SYSTEM_SLEEP_PM_OPS(samsung_keypad_suspend, samsung_keypad_resume)
+	SET_RUNTIME_PM_OPS(samsung_keypad_runtime_suspend,
+			   samsung_keypad_runtime_resume, NULL)
 };
+
+#ifdef CONFIG_OF
+static const struct of_device_id samsung_keypad_dt_match[] = {
+	{ .compatible = "samsung,s3c6410-keypad" },
+	{ .compatible = "samsung,s5pv210-keypad" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, samsung_keypad_dt_match);
+#else
+#define samsung_keypad_dt_match NULL
 #endif
 
 static struct platform_device_id samsung_keypad_driver_ids[] = {
@@ -465,27 +684,14 @@ static struct platform_driver samsung_keypad_driver = {
 	.driver		= {
 		.name	= "samsung-keypad",
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
+		.of_match_table = samsung_keypad_dt_match,
 		.pm	= &samsung_keypad_pm_ops,
-#endif
 	},
 	.id_table	= samsung_keypad_driver_ids,
 };
-
-static int __init samsung_keypad_init(void)
-{
-	return platform_driver_register(&samsung_keypad_driver);
-}
-module_init(samsung_keypad_init);
-
-static void __exit samsung_keypad_exit(void)
-{
-	platform_driver_unregister(&samsung_keypad_driver);
-}
-module_exit(samsung_keypad_exit);
+module_platform_driver(samsung_keypad_driver);
 
 MODULE_DESCRIPTION("Samsung keypad driver");
 MODULE_AUTHOR("Joonyoung Shim <jy0922.shim@samsung.com>");
 MODULE_AUTHOR("Donghwa Lee <dh09.lee@samsung.com>");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:samsung-keypad");
diff --git a/drivers/input/keyboard/sh_keysc.c b/drivers/input/keyboard/sh_keysc.c
index 934aeb583b30..da54ad5db154 100644
--- a/drivers/input/keyboard/sh_keysc.c
+++ b/drivers/input/keyboard/sh_keysc.c
@@ -337,19 +337,7 @@ static struct platform_driver sh_keysc_device_driver = {
 		.pm	= &sh_keysc_dev_pm_ops,
 	}
 };
-
-static int __init sh_keysc_init(void)
-{
-	return platform_driver_register(&sh_keysc_device_driver);
-}
-
-static void __exit sh_keysc_exit(void)
-{
-	platform_driver_unregister(&sh_keysc_device_driver);
-}
-
-module_init(sh_keysc_init);
-module_exit(sh_keysc_exit);
+module_platform_driver(sh_keysc_device_driver);
 
 MODULE_AUTHOR("Magnus Damm");
 MODULE_DESCRIPTION("SuperH KEYSC Keypad Driver");
diff --git a/drivers/input/keyboard/spear-keyboard.c b/drivers/input/keyboard/spear-keyboard.c
index d712dffd2157..c88bd63dc9cc 100644
--- a/drivers/input/keyboard/spear-keyboard.c
+++ b/drivers/input/keyboard/spear-keyboard.c
@@ -326,18 +326,7 @@ static struct platform_driver spear_kbd_driver = {
 #endif
 	},
 };
-
-static int __init spear_kbd_init(void)
-{
-	return platform_driver_register(&spear_kbd_driver);
-}
-module_init(spear_kbd_init);
-
-static void __exit spear_kbd_exit(void)
-{
-	platform_driver_unregister(&spear_kbd_driver);
-}
-module_exit(spear_kbd_exit);
+module_platform_driver(spear_kbd_driver);
 
 MODULE_AUTHOR("Rajeev Kumar");
 MODULE_DESCRIPTION("SPEAr Keyboard Driver");
diff --git a/drivers/input/keyboard/stmpe-keypad.c b/drivers/input/keyboard/stmpe-keypad.c
index ab7610ca10eb..9397cf9c625c 100644
--- a/drivers/input/keyboard/stmpe-keypad.c
+++ b/drivers/input/keyboard/stmpe-keypad.c
@@ -368,18 +368,7 @@ static struct platform_driver stmpe_keypad_driver = {
 	.probe		= stmpe_keypad_probe,
 	.remove		= __devexit_p(stmpe_keypad_remove),
 };
-
-static int __init stmpe_keypad_init(void)
-{
-	return platform_driver_register(&stmpe_keypad_driver);
-}
-module_init(stmpe_keypad_init);
-
-static void __exit stmpe_keypad_exit(void)
-{
-	platform_driver_unregister(&stmpe_keypad_driver);
-}
-module_exit(stmpe_keypad_exit);
+module_platform_driver(stmpe_keypad_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("STMPExxxx keypad driver");
diff --git a/drivers/input/keyboard/tc3589x-keypad.c b/drivers/input/keyboard/tc3589x-keypad.c
index f60c9e82f204..2dee3e4e7c6f 100644
--- a/drivers/input/keyboard/tc3589x-keypad.c
+++ b/drivers/input/keyboard/tc3589x-keypad.c
@@ -74,11 +74,13 @@
 
 /**
  * struct tc_keypad - data structure used by keypad driver
+ * @tc3589x:    pointer to tc35893
  * @input:      pointer to input device object
  * @board:      keypad platform device
  * @krow:	number of rows
  * @kcol:	number of coloumns
  * @keymap:     matrix scan code table for keycodes
+ * @keypad_stopped: holds keypad status
  */
 struct tc_keypad {
 	struct tc3589x *tc3589x;
@@ -453,18 +455,7 @@ static struct platform_driver tc3589x_keypad_driver = {
 	.probe	= tc3589x_keypad_probe,
 	.remove	= __devexit_p(tc3589x_keypad_remove),
 };
-
-static int __init tc3589x_keypad_init(void)
-{
-	return platform_driver_register(&tc3589x_keypad_driver);
-}
-module_init(tc3589x_keypad_init);
-
-static void __exit tc3589x_keypad_exit(void)
-{
-	return platform_driver_unregister(&tc3589x_keypad_driver);
-}
-module_exit(tc3589x_keypad_exit);
+module_platform_driver(tc3589x_keypad_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Jayeeta Banerjee/Sundar Iyer");
diff --git a/drivers/input/keyboard/tca8418_keypad.c b/drivers/input/keyboard/tca8418_keypad.c
new file mode 100644
index 000000000000..958ec107bfbc
--- /dev/null
+++ b/drivers/input/keyboard/tca8418_keypad.c
@@ -0,0 +1,430 @@
+/*
+ * Driver for TCA8418 I2C keyboard
+ *
+ * Copyright (C) 2011 Fuel7, Inc.  All rights reserved.
+ *
+ * Author: Kyle Manna <kyle.manna@fuel7.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public
+ * License v2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this program; if not, write to the
+ * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+ * Boston, MA 021110-1307, USA.
+ *
+ * If you can't comply with GPLv2, alternative licensing terms may be
+ * arranged. Please contact Fuel7, Inc. (http://fuel7.com/) for proprietary
+ * alternative licensing inquiries.
+ */
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/input/tca8418_keypad.h>
+
+/* TCA8418 hardware limits */
+#define TCA8418_MAX_ROWS	8
+#define TCA8418_MAX_COLS	10
+
+/* TCA8418 register offsets */
+#define REG_CFG			0x01
+#define REG_INT_STAT		0x02
+#define REG_KEY_LCK_EC		0x03
+#define REG_KEY_EVENT_A		0x04
+#define REG_KEY_EVENT_B		0x05
+#define REG_KEY_EVENT_C		0x06
+#define REG_KEY_EVENT_D		0x07
+#define REG_KEY_EVENT_E		0x08
+#define REG_KEY_EVENT_F		0x09
+#define REG_KEY_EVENT_G		0x0A
+#define REG_KEY_EVENT_H		0x0B
+#define REG_KEY_EVENT_I		0x0C
+#define REG_KEY_EVENT_J		0x0D
+#define REG_KP_LCK_TIMER	0x0E
+#define REG_UNLOCK1		0x0F
+#define REG_UNLOCK2		0x10
+#define REG_GPIO_INT_STAT1	0x11
+#define REG_GPIO_INT_STAT2	0x12
+#define REG_GPIO_INT_STAT3	0x13
+#define REG_GPIO_DAT_STAT1	0x14
+#define REG_GPIO_DAT_STAT2	0x15
+#define REG_GPIO_DAT_STAT3	0x16
+#define REG_GPIO_DAT_OUT1	0x17
+#define REG_GPIO_DAT_OUT2	0x18
+#define REG_GPIO_DAT_OUT3	0x19
+#define REG_GPIO_INT_EN1	0x1A
+#define REG_GPIO_INT_EN2	0x1B
+#define REG_GPIO_INT_EN3	0x1C
+#define REG_KP_GPIO1		0x1D
+#define REG_KP_GPIO2		0x1E
+#define REG_KP_GPIO3		0x1F
+#define REG_GPI_EM1		0x20
+#define REG_GPI_EM2		0x21
+#define REG_GPI_EM3		0x22
+#define REG_GPIO_DIR1		0x23
+#define REG_GPIO_DIR2		0x24
+#define REG_GPIO_DIR3		0x25
+#define REG_GPIO_INT_LVL1	0x26
+#define REG_GPIO_INT_LVL2	0x27
+#define REG_GPIO_INT_LVL3	0x28
+#define REG_DEBOUNCE_DIS1	0x29
+#define REG_DEBOUNCE_DIS2	0x2A
+#define REG_DEBOUNCE_DIS3	0x2B
+#define REG_GPIO_PULL1		0x2C
+#define REG_GPIO_PULL2		0x2D
+#define REG_GPIO_PULL3		0x2E
+
+/* TCA8418 bit definitions */
+#define CFG_AI			BIT(7)
+#define CFG_GPI_E_CFG		BIT(6)
+#define CFG_OVR_FLOW_M		BIT(5)
+#define CFG_INT_CFG		BIT(4)
+#define CFG_OVR_FLOW_IEN	BIT(3)
+#define CFG_K_LCK_IEN		BIT(2)
+#define CFG_GPI_IEN		BIT(1)
+#define CFG_KE_IEN		BIT(0)
+
+#define INT_STAT_CAD_INT	BIT(4)
+#define INT_STAT_OVR_FLOW_INT	BIT(3)
+#define INT_STAT_K_LCK_INT	BIT(2)
+#define INT_STAT_GPI_INT	BIT(1)
+#define INT_STAT_K_INT		BIT(0)
+
+/* TCA8418 register masks */
+#define KEY_LCK_EC_KEC		0x7
+#define KEY_EVENT_CODE		0x7f
+#define KEY_EVENT_VALUE		0x80
+
+
+static const struct i2c_device_id tca8418_id[] = {
+	{ TCA8418_NAME, 8418, },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tca8418_id);
+
+struct tca8418_keypad {
+	unsigned int rows;
+	unsigned int cols;
+	unsigned int keypad_mask; /* Mask for keypad col/rol regs */
+	unsigned int irq;
+	unsigned int row_shift;
+
+	struct i2c_client *client;
+	struct input_dev *input;
+
+	/* Flexible array member, must be at end of struct */
+	unsigned short keymap[];
+};
+
+/*
+ * Write a byte to the TCA8418
+ */
+static int tca8418_write_byte(struct tca8418_keypad *keypad_data,
+			      int reg, u8 val)
+{
+	int error;
+
+	error = i2c_smbus_write_byte_data(keypad_data->client, reg, val);
+	if (error < 0) {
+		dev_err(&keypad_data->client->dev,
+			"%s failed, reg: %d, val: %d, error: %d\n",
+			__func__, reg, val, error);
+		return error;
+	}
+
+	return 0;
+}
+
+/*
+ * Read a byte from the TCA8418
+ */
+static int tca8418_read_byte(struct tca8418_keypad *keypad_data,
+			     int reg, u8 *val)
+{
+	int error;
+
+	error = i2c_smbus_read_byte_data(keypad_data->client, reg);
+	if (error < 0) {
+		dev_err(&keypad_data->client->dev,
+				"%s failed, reg: %d, error: %d\n",
+				__func__, reg, error);
+		return error;
+	}
+
+	*val = (u8)error;
+
+	return 0;
+}
+
+static void tca8418_read_keypad(struct tca8418_keypad *keypad_data)
+{
+	int error, col, row;
+	u8 reg, state, code;
+
+	/* Initial read of the key event FIFO */
+	error = tca8418_read_byte(keypad_data, REG_KEY_EVENT_A, &reg);
+
+	/* Assume that key code 0 signifies empty FIFO */
+	while (error >= 0 && reg > 0) {
+		state = reg & KEY_EVENT_VALUE;
+		code  = reg & KEY_EVENT_CODE;
+
+		row = code / TCA8418_MAX_COLS;
+		col = code % TCA8418_MAX_COLS;
+
+		row = (col) ? row : row - 1;
+		col = (col) ? col - 1 : TCA8418_MAX_COLS - 1;
+
+		code = MATRIX_SCAN_CODE(row, col, keypad_data->row_shift);
+		input_event(keypad_data->input, EV_MSC, MSC_SCAN, code);
+		input_report_key(keypad_data->input,
+				keypad_data->keymap[code], state);
+
+		/* Read for next loop */
+		error = tca8418_read_byte(keypad_data, REG_KEY_EVENT_A, &reg);
+	}
+
+	if (error < 0)
+		dev_err(&keypad_data->client->dev,
+			"unable to read REG_KEY_EVENT_A\n");
+
+	input_sync(keypad_data->input);
+}
+
+/*
+ * Threaded IRQ handler and this can (and will) sleep.
+ */
+static irqreturn_t tca8418_irq_handler(int irq, void *dev_id)
+{
+	struct tca8418_keypad *keypad_data = dev_id;
+	u8 reg;
+	int error;
+
+	error = tca8418_read_byte(keypad_data, REG_INT_STAT, &reg);
+	if (error) {
+		dev_err(&keypad_data->client->dev,
+			"unable to read REG_INT_STAT\n");
+		goto exit;
+	}
+
+	if (reg & INT_STAT_OVR_FLOW_INT)
+		dev_warn(&keypad_data->client->dev, "overflow occurred\n");
+
+	if (reg & INT_STAT_K_INT)
+		tca8418_read_keypad(keypad_data);
+
+exit:
+	/* Clear all interrupts, even IRQs we didn't check (GPI, CAD, LCK) */
+	reg = 0xff;
+	error = tca8418_write_byte(keypad_data, REG_INT_STAT, reg);
+	if (error)
+		dev_err(&keypad_data->client->dev,
+			"unable to clear REG_INT_STAT\n");
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Configure the TCA8418 for keypad operation
+ */
+static int __devinit tca8418_configure(struct tca8418_keypad *keypad_data)
+{
+	int reg, error;
+
+	/* Write config register, if this fails assume device not present */
+	error = tca8418_write_byte(keypad_data, REG_CFG,
+				CFG_INT_CFG | CFG_OVR_FLOW_IEN | CFG_KE_IEN);
+	if (error < 0)
+		return -ENODEV;
+
+
+	/* Assemble a mask for row and column registers */
+	reg  =  ~(~0 << keypad_data->rows);
+	reg += (~(~0 << keypad_data->cols)) << 8;
+	keypad_data->keypad_mask = reg;
+
+	/* Set registers to keypad mode */
+	error |= tca8418_write_byte(keypad_data, REG_KP_GPIO1, reg);
+	error |= tca8418_write_byte(keypad_data, REG_KP_GPIO2, reg >> 8);
+	error |= tca8418_write_byte(keypad_data, REG_KP_GPIO3, reg >> 16);
+
+	/* Enable column debouncing */
+	error |= tca8418_write_byte(keypad_data, REG_DEBOUNCE_DIS1, reg);
+	error |= tca8418_write_byte(keypad_data, REG_DEBOUNCE_DIS2, reg >> 8);
+	error |= tca8418_write_byte(keypad_data, REG_DEBOUNCE_DIS3, reg >> 16);
+
+	return error;
+}
+
+static int __devinit tca8418_keypad_probe(struct i2c_client *client,
+					  const struct i2c_device_id *id)
+{
+	const struct tca8418_keypad_platform_data *pdata =
+						client->dev.platform_data;
+	struct tca8418_keypad *keypad_data;
+	struct input_dev *input;
+	int error, row_shift, max_keys;
+
+	/* Copy the platform data */
+	if (!pdata) {
+		dev_dbg(&client->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	if (!pdata->keymap_data) {
+		dev_err(&client->dev, "no keymap data defined\n");
+		return -EINVAL;
+	}
+
+	if (!pdata->rows || pdata->rows > TCA8418_MAX_ROWS) {
+		dev_err(&client->dev, "invalid rows\n");
+		return -EINVAL;
+	}
+
+	if (!pdata->cols || pdata->cols > TCA8418_MAX_COLS) {
+		dev_err(&client->dev, "invalid columns\n");
+		return -EINVAL;
+	}
+
+	/* Check i2c driver capabilities */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE)) {
+		dev_err(&client->dev, "%s adapter not supported\n",
+			dev_driver_string(&client->adapter->dev));
+		return -ENODEV;
+	}
+
+	row_shift = get_count_order(pdata->cols);
+	max_keys = pdata->rows << row_shift;
+
+	/* Allocate memory for keypad_data, keymap and input device */
+	keypad_data = kzalloc(sizeof(*keypad_data) +
+			max_keys * sizeof(keypad_data->keymap[0]), GFP_KERNEL);
+	if (!keypad_data)
+		return -ENOMEM;
+
+	keypad_data->rows = pdata->rows;
+	keypad_data->cols = pdata->cols;
+	keypad_data->client = client;
+	keypad_data->row_shift = row_shift;
+
+	/* Initialize the chip or fail if chip isn't present */
+	error = tca8418_configure(keypad_data);
+	if (error < 0)
+		goto fail1;
+
+	/* Configure input device */
+	input = input_allocate_device();
+	if (!input) {
+		error = -ENOMEM;
+		goto fail1;
+	}
+	keypad_data->input = input;
+
+	input->name = client->name;
+	input->dev.parent = &client->dev;
+
+	input->id.bustype = BUS_I2C;
+	input->id.vendor  = 0x0001;
+	input->id.product = 0x001;
+	input->id.version = 0x0001;
+
+	input->keycode     = keypad_data->keymap;
+	input->keycodesize = sizeof(keypad_data->keymap[0]);
+	input->keycodemax  = max_keys;
+
+	__set_bit(EV_KEY, input->evbit);
+	if (pdata->rep)
+		__set_bit(EV_REP, input->evbit);
+
+	input_set_capability(input, EV_MSC, MSC_SCAN);
+
+	input_set_drvdata(input, keypad_data);
+
+	matrix_keypad_build_keymap(pdata->keymap_data, row_shift,
+			input->keycode, input->keybit);
+
+	if (pdata->irq_is_gpio)
+		client->irq = gpio_to_irq(client->irq);
+
+	error = request_threaded_irq(client->irq, NULL, tca8418_irq_handler,
+				     IRQF_TRIGGER_FALLING,
+				     client->name, keypad_data);
+	if (error) {
+		dev_dbg(&client->dev,
+			"Unable to claim irq %d; error %d\n",
+			client->irq, error);
+		goto fail2;
+	}
+
+	error = input_register_device(input);
+	if (error) {
+		dev_dbg(&client->dev,
+			"Unable to register input device, error: %d\n", error);
+		goto fail3;
+	}
+
+	i2c_set_clientdata(client, keypad_data);
+	return 0;
+
+fail3:
+	free_irq(client->irq, keypad_data);
+fail2:
+	input_free_device(input);
+fail1:
+	kfree(keypad_data);
+	return error;
+}
+
+static int __devexit tca8418_keypad_remove(struct i2c_client *client)
+{
+	struct tca8418_keypad *keypad_data = i2c_get_clientdata(client);
+
+	free_irq(keypad_data->client->irq, keypad_data);
+
+	input_unregister_device(keypad_data->input);
+
+	kfree(keypad_data);
+
+	return 0;
+}
+
+
+static struct i2c_driver tca8418_keypad_driver = {
+	.driver = {
+		.name	= TCA8418_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tca8418_keypad_probe,
+	.remove		= __devexit_p(tca8418_keypad_remove),
+	.id_table	= tca8418_id,
+};
+
+static int __init tca8418_keypad_init(void)
+{
+	return i2c_add_driver(&tca8418_keypad_driver);
+}
+subsys_initcall(tca8418_keypad_init);
+
+static void __exit tca8418_keypad_exit(void)
+{
+	i2c_del_driver(&tca8418_keypad_driver);
+}
+module_exit(tca8418_keypad_exit);
+
+MODULE_AUTHOR("Kyle Manna <kyle.manna@fuel7.com>");
+MODULE_DESCRIPTION("Keypad driver for TCA8418");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/keyboard/tegra-kbc.c b/drivers/input/keyboard/tegra-kbc.c
index cf3228b0ab90..a136e2e832be 100644
--- a/drivers/input/keyboard/tegra-kbc.c
+++ b/drivers/input/keyboard/tegra-kbc.c
@@ -26,6 +26,7 @@
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/interrupt.h>
+#include <linux/of.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
 #include <mach/clk.h>
@@ -52,6 +53,7 @@
 /* KBC Interrupt Register */
 #define KBC_INT_0	0x4
 #define KBC_INT_FIFO_CNT_INT_STATUS	(1 << 2)
+#define KBC_INT_KEYPRESS_INT_STATUS	(1 << 0)
 
 #define KBC_ROW_CFG0_0	0x8
 #define KBC_COL_CFG0_0	0x18
@@ -74,15 +76,17 @@ struct tegra_kbc {
 	unsigned int cp_to_wkup_dly;
 	bool use_fn_map;
 	bool use_ghost_filter;
+	bool keypress_caused_wake;
 	const struct tegra_kbc_platform_data *pdata;
 	unsigned short keycode[KBC_MAX_KEY * 2];
 	unsigned short current_keys[KBC_MAX_KPENT];
 	unsigned int num_pressed_keys;
+	u32 wakeup_key;
 	struct timer_list timer;
 	struct clk *clk;
 };
 
-static const u32 tegra_kbc_default_keymap[] = {
+static const u32 tegra_kbc_default_keymap[] __devinitdata = {
 	KEY(0, 2, KEY_W),
 	KEY(0, 3, KEY_S),
 	KEY(0, 4, KEY_A),
@@ -217,7 +221,8 @@ static const u32 tegra_kbc_default_keymap[] = {
 	KEY(31, 4, KEY_HELP),
 };
 
-static const struct matrix_keymap_data tegra_kbc_default_keymap_data = {
+static const
+struct matrix_keymap_data tegra_kbc_default_keymap_data __devinitdata = {
 	.keymap		= tegra_kbc_default_keymap,
 	.keymap_size	= ARRAY_SIZE(tegra_kbc_default_keymap),
 };
@@ -409,6 +414,9 @@ static irqreturn_t tegra_kbc_isr(int irq, void *args)
 		 */
 		tegra_kbc_set_fifo_interrupt(kbc, false);
 		mod_timer(&kbc->timer, jiffies + kbc->cp_dly_jiffies);
+	} else if (val & KBC_INT_KEYPRESS_INT_STATUS) {
+		/* We can be here only through system resume path */
+		kbc->keypress_caused_wake = true;
 	}
 
 	spin_unlock_irqrestore(&kbc->lock, flags);
@@ -576,6 +584,56 @@ tegra_kbc_check_pin_cfg(const struct tegra_kbc_platform_data *pdata,
 	return true;
 }
 
+#ifdef CONFIG_OF
+static struct tegra_kbc_platform_data * __devinit
+tegra_kbc_dt_parse_pdata(struct platform_device *pdev)
+{
+	struct tegra_kbc_platform_data *pdata;
+	struct device_node *np = pdev->dev.of_node;
+
+	if (!np)
+		return NULL;
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	if (!of_property_read_u32(np, "debounce-delay", &prop))
+		pdata->debounce_cnt = prop;
+
+	if (!of_property_read_u32(np, "repeat-delay", &prop))
+		pdata->repeat_cnt = prop;
+
+	if (of_find_property(np, "needs-ghost-filter", NULL))
+		pdata->use_ghost_filter = true;
+
+	if (of_find_property(np, "wakeup-source", NULL))
+		pdata->wakeup = true;
+
+	/*
+	 * All currently known keymaps with device tree support use the same
+	 * pin_cfg, so set it up here.
+	 */
+	for (i = 0; i < KBC_MAX_ROW; i++) {
+		pdata->pin_cfg[i].num = i;
+		pdata->pin_cfg[i].is_row = true;
+	}
+
+	for (i = 0; i < KBC_MAX_COL; i++) {
+		pdata->pin_cfg[KBC_MAX_ROW + i].num = i;
+		pdata->pin_cfg[KBC_MAX_ROW + i].is_row = false;
+	}
+
+	return pdata;
+}
+#else
+static inline struct tegra_kbc_platform_data *tegra_kbc_dt_parse_pdata(
+	struct platform_device *pdev)
+{
+	return NULL;
+}
+#endif
+
 static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 {
 	const struct tegra_kbc_platform_data *pdata = pdev->dev.platform_data;
@@ -590,21 +648,28 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 	unsigned int scan_time_rows;
 
 	if (!pdata)
-		return -EINVAL;
+		pdata = tegra_kbc_dt_parse_pdata(pdev);
 
-	if (!tegra_kbc_check_pin_cfg(pdata, &pdev->dev, &num_rows))
+	if (!pdata)
 		return -EINVAL;
 
+	if (!tegra_kbc_check_pin_cfg(pdata, &pdev->dev, &num_rows)) {
+		err = -EINVAL;
+		goto err_free_pdata;
+	}
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "failed to get I/O memory\n");
-		return -ENXIO;
+		err = -ENXIO;
+		goto err_free_pdata;
 	}
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		dev_err(&pdev->dev, "failed to get keyboard IRQ\n");
-		return -ENXIO;
+		err = -ENXIO;
+		goto err_free_pdata;
 	}
 
 	kbc = kzalloc(sizeof(*kbc), GFP_KERNEL);
@@ -674,9 +739,10 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 	keymap_data = pdata->keymap_data ?: &tegra_kbc_default_keymap_data;
 	matrix_keypad_build_keymap(keymap_data, KBC_ROW_SHIFT,
 				   input_dev->keycode, input_dev->keybit);
+	kbc->wakeup_key = pdata->wakeup_key;
 
-	err = request_irq(kbc->irq, tegra_kbc_isr, IRQF_TRIGGER_HIGH,
-			  pdev->name, kbc);
+	err = request_irq(kbc->irq, tegra_kbc_isr,
+			  IRQF_NO_SUSPEND | IRQF_TRIGGER_HIGH, pdev->name, kbc);
 	if (err) {
 		dev_err(&pdev->dev, "failed to request keyboard IRQ\n");
 		goto err_put_clk;
@@ -706,6 +772,9 @@ err_free_mem_region:
 err_free_mem:
 	input_free_device(input_dev);
 	kfree(kbc);
+err_free_pdata:
+	if (!pdev->dev.platform_data)
+		kfree(pdata);
 
 	return err;
 }
@@ -715,6 +784,8 @@ static int __devexit tegra_kbc_remove(struct platform_device *pdev)
 	struct tegra_kbc *kbc = platform_get_drvdata(pdev);
 	struct resource *res;
 
+	platform_set_drvdata(pdev, NULL);
+
 	free_irq(kbc->irq, pdev);
 	clk_put(kbc->clk);
 
@@ -723,9 +794,14 @@ static int __devexit tegra_kbc_remove(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(res->start, resource_size(res));
 
-	kfree(kbc);
+	/*
+	 * If we do not have platform data attached to the device we
+	 * allocated it ourselves and thus need to free it.
+	 */
+	if (!pdev->dev.platform_data)
+		kfree(kbc->pdata);
 
-	platform_set_drvdata(pdev, NULL);
+	kfree(kbc);
 
 	return 0;
 }
@@ -754,6 +830,8 @@ static int tegra_kbc_suspend(struct device *dev)
 		tegra_kbc_setup_wakekeys(kbc, true);
 		msleep(30);
 
+		kbc->keypress_caused_wake = false;
+		enable_irq(kbc->irq);
 		enable_irq_wake(kbc->irq);
 	} else {
 		if (kbc->idev->users)
@@ -780,7 +858,19 @@ static int tegra_kbc_resume(struct device *dev)
 
 		tegra_kbc_set_fifo_interrupt(kbc, true);
 
-		enable_irq(kbc->irq);
+		if (kbc->keypress_caused_wake && kbc->wakeup_key) {
+			/*
+			 * We can't report events directly from the ISR
+			 * because timekeeping is stopped when processing
+			 * wakeup request and we get a nasty warning when
+			 * we try to call do_gettimeofday() in evdev
+			 * handler.
+			 */
+			input_report_key(kbc->idev, kbc->wakeup_key, 1);
+			input_sync(kbc->idev);
+			input_report_key(kbc->idev, kbc->wakeup_key, 0);
+			input_sync(kbc->idev);
+		}
 	} else {
 		if (kbc->idev->users)
 			err = tegra_kbc_start(kbc);
@@ -793,6 +883,12 @@ static int tegra_kbc_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(tegra_kbc_pm_ops, tegra_kbc_suspend, tegra_kbc_resume);
 
+static const struct of_device_id tegra_kbc_of_match[] = {
+	{ .compatible = "nvidia,tegra20-kbc", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tegra_kbc_of_match);
+
 static struct platform_driver tegra_kbc_driver = {
 	.probe		= tegra_kbc_probe,
 	.remove		= __devexit_p(tegra_kbc_remove),
@@ -800,20 +896,10 @@ static struct platform_driver tegra_kbc_driver = {
 		.name	= "tegra-kbc",
 		.owner  = THIS_MODULE,
 		.pm	= &tegra_kbc_pm_ops,
+		.of_match_table = tegra_kbc_of_match,
 	},
 };
-
-static void __exit tegra_kbc_exit(void)
-{
-	platform_driver_unregister(&tegra_kbc_driver);
-}
-module_exit(tegra_kbc_exit);
-
-static int __init tegra_kbc_init(void)
-{
-	return platform_driver_register(&tegra_kbc_driver);
-}
-module_init(tegra_kbc_init);
+module_platform_driver(tegra_kbc_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Rakesh Iyer <riyer@nvidia.com>");
diff --git a/drivers/input/keyboard/tnetv107x-keypad.c b/drivers/input/keyboard/tnetv107x-keypad.c
index 66e55e5cfdd6..fb39c94b6fdd 100644
--- a/drivers/input/keyboard/tnetv107x-keypad.c
+++ b/drivers/input/keyboard/tnetv107x-keypad.c
@@ -322,19 +322,7 @@ static struct platform_driver keypad_driver = {
 	.driver.name	= "tnetv107x-keypad",
 	.driver.owner	= THIS_MODULE,
 };
-
-static int __init keypad_init(void)
-{
-	return platform_driver_register(&keypad_driver);
-}
-
-static void __exit keypad_exit(void)
-{
-	platform_driver_unregister(&keypad_driver);
-}
-
-module_init(keypad_init);
-module_exit(keypad_exit);
+module_platform_driver(keypad_driver);
 
 MODULE_AUTHOR("Cyril Chemparathy");
 MODULE_DESCRIPTION("TNETV107X Keypad Driver");
diff --git a/drivers/input/keyboard/twl4030_keypad.c b/drivers/input/keyboard/twl4030_keypad.c
index a26922cf0e84..a588578037eb 100644
--- a/drivers/input/keyboard/twl4030_keypad.c
+++ b/drivers/input/keyboard/twl4030_keypad.c
@@ -460,18 +460,7 @@ static struct platform_driver twl4030_kp_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init twl4030_kp_init(void)
-{
-	return platform_driver_register(&twl4030_kp_driver);
-}
-module_init(twl4030_kp_init);
-
-static void __exit twl4030_kp_exit(void)
-{
-	platform_driver_unregister(&twl4030_kp_driver);
-}
-module_exit(twl4030_kp_exit);
+module_platform_driver(twl4030_kp_driver);
 
 MODULE_AUTHOR("Texas Instruments");
 MODULE_DESCRIPTION("TWL4030 Keypad Driver");
diff --git a/drivers/input/keyboard/w90p910_keypad.c b/drivers/input/keyboard/w90p910_keypad.c
index 318586dadacf..99bbb7e775ae 100644
--- a/drivers/input/keyboard/w90p910_keypad.c
+++ b/drivers/input/keyboard/w90p910_keypad.c
@@ -262,19 +262,7 @@ static struct platform_driver w90p910_keypad_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init w90p910_keypad_init(void)
-{
-	return platform_driver_register(&w90p910_keypad_driver);
-}
-
-static void __exit w90p910_keypad_exit(void)
-{
-	platform_driver_unregister(&w90p910_keypad_driver);
-}
-
-module_init(w90p910_keypad_init);
-module_exit(w90p910_keypad_exit);
+module_platform_driver(w90p910_keypad_driver);
 
 MODULE_AUTHOR("Wan ZongShun <mcuos.com@gmail.com>");
 MODULE_DESCRIPTION("w90p910 keypad driver");
diff --git a/drivers/input/misc/88pm860x_onkey.c b/drivers/input/misc/88pm860x_onkey.c
index 3dca3c14510e..f2e0cbc5ab64 100644
--- a/drivers/input/misc/88pm860x_onkey.c
+++ b/drivers/input/misc/88pm860x_onkey.c
@@ -137,18 +137,7 @@ static struct platform_driver pm860x_onkey_driver = {
 	.probe		= pm860x_onkey_probe,
 	.remove		= __devexit_p(pm860x_onkey_remove),
 };
-
-static int __init pm860x_onkey_init(void)
-{
-	return platform_driver_register(&pm860x_onkey_driver);
-}
-module_init(pm860x_onkey_init);
-
-static void __exit pm860x_onkey_exit(void)
-{
-	platform_driver_unregister(&pm860x_onkey_driver);
-}
-module_exit(pm860x_onkey_exit);
+module_platform_driver(pm860x_onkey_driver);
 
 MODULE_DESCRIPTION("Marvell 88PM860x ONKEY driver");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 22d875fde53a..7b46781c30c9 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -179,6 +179,31 @@ config INPUT_APANEL
 	 To compile this driver as a module, choose M here: the module will
 	 be called apanel.
 
+config INPUT_GP2A
+	tristate "Sharp GP2AP002A00F I2C Proximity/Opto sensor driver"
+	depends on I2C
+	depends on GENERIC_GPIO
+	help
+	  Say Y here if you have a Sharp GP2AP002A00F proximity/als combo-chip
+	  hooked to an I2C bus.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called gp2ap002a00f.
+
+config INPUT_GPIO_TILT_POLLED
+	tristate "Polled GPIO tilt switch"
+	depends on GENERIC_GPIO
+	select INPUT_POLLDEV
+	help
+	  This driver implements support for tilt switches connected
+	  to GPIO pins that are not capable of generating interrupts.
+
+	  The list of gpios to use and the mapping of their states
+	  to specific angles is done via platform data.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called gpio_tilt_polled.
+
 config INPUT_IXP4XX_BEEPER
 	tristate "IXP4XX Beeper support"
 	depends on ARCH_IXP4XX
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index a244fc6a781c..46671a875b91 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -22,6 +22,8 @@ obj-$(CONFIG_INPUT_CMA3000)		+= cma3000_d0x.o
 obj-$(CONFIG_INPUT_CMA3000_I2C)		+= cma3000_d0x_i2c.o
 obj-$(CONFIG_INPUT_COBALT_BTNS)		+= cobalt_btns.o
 obj-$(CONFIG_INPUT_DM355EVM)		+= dm355evm_keys.o
+obj-$(CONFIG_INPUT_GP2A)		+= gp2ap002a00f.o
+obj-$(CONFIG_INPUT_GPIO_TILT_POLLED)	+= gpio_tilt_polled.o
 obj-$(CONFIG_HP_SDC_RTC)		+= hp_sdc_rtc.o
 obj-$(CONFIG_INPUT_IXP4XX_BEEPER)	+= ixp4xx-beeper.o
 obj-$(CONFIG_INPUT_KEYSPAN_REMOTE)	+= keyspan_remote.o
diff --git a/drivers/input/misc/ab8500-ponkey.c b/drivers/input/misc/ab8500-ponkey.c
index 3d3288a78fdc..79d901633635 100644
--- a/drivers/input/misc/ab8500-ponkey.c
+++ b/drivers/input/misc/ab8500-ponkey.c
@@ -139,18 +139,7 @@ static struct platform_driver ab8500_ponkey_driver = {
 	.probe		= ab8500_ponkey_probe,
 	.remove		= __devexit_p(ab8500_ponkey_remove),
 };
-
-static int __init ab8500_ponkey_init(void)
-{
-	return platform_driver_register(&ab8500_ponkey_driver);
-}
-module_init(ab8500_ponkey_init);
-
-static void __exit ab8500_ponkey_exit(void)
-{
-	platform_driver_unregister(&ab8500_ponkey_driver);
-}
-module_exit(ab8500_ponkey_exit);
+module_platform_driver(ab8500_ponkey_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Sundar Iyer <sundar.iyer@stericsson.com>");
diff --git a/drivers/input/misc/adxl34x-spi.c b/drivers/input/misc/adxl34x-spi.c
index f29de22fdda0..34d401efd4a1 100644
--- a/drivers/input/misc/adxl34x-spi.c
+++ b/drivers/input/misc/adxl34x-spi.c
@@ -122,7 +122,6 @@ static SIMPLE_DEV_PM_OPS(adxl34x_spi_pm, adxl34x_spi_suspend,
 static struct spi_driver adxl34x_driver = {
 	.driver = {
 		.name = "adxl34x",
-		.bus = &spi_bus_type,
 		.owner = THIS_MODULE,
 		.pm = &adxl34x_spi_pm,
 	},
diff --git a/drivers/input/misc/adxl34x.c b/drivers/input/misc/adxl34x.c
index 09244804fb97..1cf72fe513e6 100644
--- a/drivers/input/misc/adxl34x.c
+++ b/drivers/input/misc/adxl34x.c
@@ -452,10 +452,10 @@ static ssize_t adxl34x_disable_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct adxl34x *ac = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned int val;
 	int error;
 
-	error = strict_strtoul(buf, 10, &val);
+	error = kstrtouint(buf, 10, &val);
 	if (error)
 		return error;
 
@@ -541,10 +541,10 @@ static ssize_t adxl34x_rate_store(struct device *dev,
 				  const char *buf, size_t count)
 {
 	struct adxl34x *ac = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned char val;
 	int error;
 
-	error = strict_strtoul(buf, 10, &val);
+	error = kstrtou8(buf, 10, &val);
 	if (error)
 		return error;
 
@@ -576,10 +576,10 @@ static ssize_t adxl34x_autosleep_store(struct device *dev,
 				  const char *buf, size_t count)
 {
 	struct adxl34x *ac = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned int val;
 	int error;
 
-	error = strict_strtoul(buf, 10, &val);
+	error = kstrtouint(buf, 10, &val);
 	if (error)
 		return error;
 
@@ -623,13 +623,13 @@ static ssize_t adxl34x_write_store(struct device *dev,
 				   const char *buf, size_t count)
 {
 	struct adxl34x *ac = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned int val;
 	int error;
 
 	/*
 	 * This allows basic ADXL register write access for debug purposes.
 	 */
-	error = strict_strtoul(buf, 16, &val);
+	error = kstrtouint(buf, 16, &val);
 	if (error)
 		return error;
 
diff --git a/drivers/input/misc/ati_remote2.c b/drivers/input/misc/ati_remote2.c
index 874a51c2fbb2..f63341f20b91 100644
--- a/drivers/input/misc/ati_remote2.c
+++ b/drivers/input/misc/ati_remote2.c
@@ -42,13 +42,13 @@ static int ati_remote2_set_mask(const char *val,
 				const struct kernel_param *kp,
 				unsigned int max)
 {
-	unsigned long mask;
+	unsigned int mask;
 	int ret;
 
 	if (!val)
 		return -EINVAL;
 
-	ret = strict_strtoul(val, 0, &mask);
+	ret = kstrtouint(val, 0, &mask);
 	if (ret)
 		return ret;
 
@@ -720,11 +720,12 @@ static ssize_t ati_remote2_store_channel_mask(struct device *dev,
 	struct usb_device *udev = to_usb_device(dev);
 	struct usb_interface *intf = usb_ifnum_to_if(udev, 0);
 	struct ati_remote2 *ar2 = usb_get_intfdata(intf);
-	unsigned long mask;
+	unsigned int mask;
 	int r;
 
-	if (strict_strtoul(buf, 0, &mask))
-		return -EINVAL;
+	r = kstrtouint(buf, 0, &mask);
+	if (r)
+		return r;
 
 	if (mask & ~ATI_REMOTE2_MAX_CHANNEL_MASK)
 		return -EINVAL;
@@ -769,10 +770,12 @@ static ssize_t ati_remote2_store_mode_mask(struct device *dev,
 	struct usb_device *udev = to_usb_device(dev);
 	struct usb_interface *intf = usb_ifnum_to_if(udev, 0);
 	struct ati_remote2 *ar2 = usb_get_intfdata(intf);
-	unsigned long mask;
+	unsigned int mask;
+	int err;
 
-	if (strict_strtoul(buf, 0, &mask))
-		return -EINVAL;
+	err = kstrtouint(buf, 0, &mask);
+	if (err)
+		return err;
 
 	if (mask & ~ATI_REMOTE2_MAX_MODE_MASK)
 		return -EINVAL;
diff --git a/drivers/input/misc/bfin_rotary.c b/drivers/input/misc/bfin_rotary.c
index d00edc9f39d1..1c4146fccfdf 100644
--- a/drivers/input/misc/bfin_rotary.c
+++ b/drivers/input/misc/bfin_rotary.c
@@ -264,18 +264,7 @@ static struct platform_driver bfin_rotary_device_driver = {
 #endif
 	},
 };
-
-static int __init bfin_rotary_init(void)
-{
-	return platform_driver_register(&bfin_rotary_device_driver);
-}
-module_init(bfin_rotary_init);
-
-static void __exit bfin_rotary_exit(void)
-{
-	platform_driver_unregister(&bfin_rotary_device_driver);
-}
-module_exit(bfin_rotary_exit);
+module_platform_driver(bfin_rotary_device_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
diff --git a/drivers/input/misc/cobalt_btns.c b/drivers/input/misc/cobalt_btns.c
index fd8407a29631..53e43d295148 100644
--- a/drivers/input/misc/cobalt_btns.c
+++ b/drivers/input/misc/cobalt_btns.c
@@ -163,16 +163,4 @@ static struct platform_driver cobalt_buttons_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init cobalt_buttons_init(void)
-{
-	return platform_driver_register(&cobalt_buttons_driver);
-}
-
-static void __exit cobalt_buttons_exit(void)
-{
-	platform_driver_unregister(&cobalt_buttons_driver);
-}
-
-module_init(cobalt_buttons_init);
-module_exit(cobalt_buttons_exit);
+module_platform_driver(cobalt_buttons_driver);
diff --git a/drivers/input/misc/dm355evm_keys.c b/drivers/input/misc/dm355evm_keys.c
index 7283dd2a1ad3..35083c6836c3 100644
--- a/drivers/input/misc/dm355evm_keys.c
+++ b/drivers/input/misc/dm355evm_keys.c
@@ -267,17 +267,6 @@ static struct platform_driver dm355evm_keys_driver = {
 		.name	= "dm355evm_keys",
 	},
 };
-
-static int __init dm355evm_keys_init(void)
-{
-	return platform_driver_register(&dm355evm_keys_driver);
-}
-module_init(dm355evm_keys_init);
-
-static void __exit dm355evm_keys_exit(void)
-{
-	platform_driver_unregister(&dm355evm_keys_driver);
-}
-module_exit(dm355evm_keys_exit);
+module_platform_driver(dm355evm_keys_driver);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/gp2ap002a00f.c b/drivers/input/misc/gp2ap002a00f.c
new file mode 100644
index 000000000000..71fba8c2fc66
--- /dev/null
+++ b/drivers/input/misc/gp2ap002a00f.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2011 Sony Ericsson Mobile Communications Inc.
+ *
+ * Author: Courtney Cavin <courtney.cavin@sonyericsson.com>
+ * Prepared for up-stream by: Oskar Andero <oskar.andero@sonyericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/input/gp2ap002a00f.h>
+
+struct gp2a_data {
+	struct input_dev *input;
+	const struct gp2a_platform_data *pdata;
+	struct i2c_client *i2c_client;
+};
+
+enum gp2a_addr {
+	GP2A_ADDR_PROX	= 0x0,
+	GP2A_ADDR_GAIN	= 0x1,
+	GP2A_ADDR_HYS	= 0x2,
+	GP2A_ADDR_CYCLE	= 0x3,
+	GP2A_ADDR_OPMOD	= 0x4,
+	GP2A_ADDR_CON	= 0x6
+};
+
+enum gp2a_controls {
+	/* Software Shutdown control: 0 = shutdown, 1 = normal operation */
+	GP2A_CTRL_SSD	= 0x01
+};
+
+static int gp2a_report(struct gp2a_data *dt)
+{
+	int vo = gpio_get_value(dt->pdata->vout_gpio);
+
+	input_report_switch(dt->input, SW_FRONT_PROXIMITY, !vo);
+	input_sync(dt->input);
+
+	return 0;
+}
+
+static irqreturn_t gp2a_irq(int irq, void *handle)
+{
+	struct gp2a_data *dt = handle;
+
+	gp2a_report(dt);
+
+	return IRQ_HANDLED;
+}
+
+static int gp2a_enable(struct gp2a_data *dt)
+{
+	return i2c_smbus_write_byte_data(dt->i2c_client, GP2A_ADDR_OPMOD,
+					 GP2A_CTRL_SSD);
+}
+
+static int gp2a_disable(struct gp2a_data *dt)
+{
+	return i2c_smbus_write_byte_data(dt->i2c_client, GP2A_ADDR_OPMOD,
+					 0x00);
+}
+
+static int gp2a_device_open(struct input_dev *dev)
+{
+	struct gp2a_data *dt = input_get_drvdata(dev);
+	int error;
+
+	error = gp2a_enable(dt);
+	if (error < 0) {
+		dev_err(&dt->i2c_client->dev,
+			"unable to activate, err %d\n", error);
+		return error;
+	}
+
+	gp2a_report(dt);
+
+	return 0;
+}
+
+static void gp2a_device_close(struct input_dev *dev)
+{
+	struct gp2a_data *dt = input_get_drvdata(dev);
+	int error;
+
+	error = gp2a_disable(dt);
+	if (error < 0)
+		dev_err(&dt->i2c_client->dev,
+			"unable to deactivate, err %d\n", error);
+}
+
+static int __devinit gp2a_initialize(struct gp2a_data *dt)
+{
+	int error;
+
+	error = i2c_smbus_write_byte_data(dt->i2c_client, GP2A_ADDR_GAIN,
+					  0x08);
+	if (error < 0)
+		return error;
+
+	error = i2c_smbus_write_byte_data(dt->i2c_client, GP2A_ADDR_HYS,
+					  0xc2);
+	if (error < 0)
+		return error;
+
+	error = i2c_smbus_write_byte_data(dt->i2c_client, GP2A_ADDR_CYCLE,
+					  0x04);
+	if (error < 0)
+		return error;
+
+	error = gp2a_disable(dt);
+
+	return error;
+}
+
+static int __devinit gp2a_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	const struct gp2a_platform_data *pdata = client->dev.platform_data;
+	struct gp2a_data *dt;
+	int error;
+
+	if (!pdata)
+		return -EINVAL;
+
+	if (pdata->hw_setup) {
+		error = pdata->hw_setup(client);
+		if (error < 0)
+			return error;
+	}
+
+	error = gpio_request_one(pdata->vout_gpio, GPIOF_IN, GP2A_I2C_NAME);
+	if (error)
+		goto err_hw_shutdown;
+
+	dt = kzalloc(sizeof(struct gp2a_data), GFP_KERNEL);
+	if (!dt) {
+		error = -ENOMEM;
+		goto err_free_gpio;
+	}
+
+	dt->pdata = pdata;
+	dt->i2c_client = client;
+
+	error = gp2a_initialize(dt);
+	if (error < 0)
+		goto err_free_mem;
+
+	dt->input = input_allocate_device();
+	if (!dt->input) {
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	input_set_drvdata(dt->input, dt);
+
+	dt->input->open = gp2a_device_open;
+	dt->input->close = gp2a_device_close;
+	dt->input->name = GP2A_I2C_NAME;
+	dt->input->id.bustype = BUS_I2C;
+	dt->input->dev.parent = &client->dev;
+
+	input_set_capability(dt->input, EV_SW, SW_FRONT_PROXIMITY);
+
+	error = request_threaded_irq(client->irq, NULL, gp2a_irq,
+			IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING |
+				IRQF_ONESHOT,
+			GP2A_I2C_NAME, dt);
+	if (error) {
+		dev_err(&client->dev, "irq request failed\n");
+		goto err_free_input_dev;
+	}
+
+	error = input_register_device(dt->input);
+	if (error) {
+		dev_err(&client->dev, "device registration failed\n");
+		goto err_free_irq;
+	}
+
+	device_init_wakeup(&client->dev, pdata->wakeup);
+	i2c_set_clientdata(client, dt);
+
+	return 0;
+
+err_free_irq:
+	free_irq(client->irq, dt);
+err_free_input_dev:
+	input_free_device(dt->input);
+err_free_mem:
+	kfree(dt);
+err_free_gpio:
+	gpio_free(pdata->vout_gpio);
+err_hw_shutdown:
+	if (pdata->hw_shutdown)
+		pdata->hw_shutdown(client);
+	return error;
+}
+
+static int __devexit gp2a_remove(struct i2c_client *client)
+{
+	struct gp2a_data *dt = i2c_get_clientdata(client);
+	const struct gp2a_platform_data *pdata = dt->pdata;
+
+	device_init_wakeup(&client->dev, false);
+
+	free_irq(client->irq, dt);
+
+	input_unregister_device(dt->input);
+	kfree(dt);
+
+	gpio_free(pdata->vout_gpio);
+
+	if (pdata->hw_shutdown)
+		pdata->hw_shutdown(client);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int gp2a_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct gp2a_data *dt = i2c_get_clientdata(client);
+	int retval = 0;
+
+	if (device_may_wakeup(&client->dev)) {
+		enable_irq_wake(client->irq);
+	} else {
+		mutex_lock(&dt->input->mutex);
+		if (dt->input->users)
+			retval = gp2a_disable(dt);
+		mutex_unlock(&dt->input->mutex);
+	}
+
+	return retval;
+}
+
+static int gp2a_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct gp2a_data *dt = i2c_get_clientdata(client);
+	int retval = 0;
+
+	if (device_may_wakeup(&client->dev)) {
+		disable_irq_wake(client->irq);
+	} else {
+		mutex_lock(&dt->input->mutex);
+		if (dt->input->users)
+			retval = gp2a_enable(dt);
+		mutex_unlock(&dt->input->mutex);
+	}
+
+	return retval;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(gp2a_pm, gp2a_suspend, gp2a_resume);
+
+static const struct i2c_device_id gp2a_i2c_id[] = {
+	{ GP2A_I2C_NAME, 0 },
+	{ }
+};
+
+static struct i2c_driver gp2a_i2c_driver = {
+	.driver = {
+		.name	= GP2A_I2C_NAME,
+		.owner	= THIS_MODULE,
+		.pm	= &gp2a_pm,
+	},
+	.probe		= gp2a_probe,
+	.remove		= __devexit_p(gp2a_remove),
+	.id_table	= gp2a_i2c_id,
+};
+
+static int __init gp2a_init(void)
+{
+	return i2c_add_driver(&gp2a_i2c_driver);
+}
+
+static void __exit gp2a_exit(void)
+{
+	i2c_del_driver(&gp2a_i2c_driver);
+}
+
+module_init(gp2a_init);
+module_exit(gp2a_exit);
+
+MODULE_AUTHOR("Courtney Cavin <courtney.cavin@sonyericsson.com>");
+MODULE_DESCRIPTION("Sharp GP2AP002A00F I2C Proximity/Opto sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/misc/gpio_tilt_polled.c b/drivers/input/misc/gpio_tilt_polled.c
new file mode 100644
index 000000000000..277a0574c199
--- /dev/null
+++ b/drivers/input/misc/gpio_tilt_polled.c
@@ -0,0 +1,213 @@
+/*
+ *  Driver for tilt switches connected via GPIO lines
+ *  not capable of generating interrupts
+ *
+ *  Copyright (C) 2011 Heiko Stuebner <heiko@sntech.de>
+ *
+ *  based on: drivers/input/keyboard/gpio_keys_polled.c
+ *
+ *  Copyright (C) 2007-2010 Gabor Juhos <juhosg@openwrt.org>
+ *  Copyright (C) 2010 Nuno Goncalves <nunojpg@gmail.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/input-polldev.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/input/gpio_tilt.h>
+
+#define DRV_NAME	"gpio-tilt-polled"
+
+struct gpio_tilt_polled_dev {
+	struct input_polled_dev *poll_dev;
+	struct device *dev;
+	const struct gpio_tilt_platform_data *pdata;
+
+	int last_state;
+
+	int threshold;
+	int count;
+};
+
+static void gpio_tilt_polled_poll(struct input_polled_dev *dev)
+{
+	struct gpio_tilt_polled_dev *tdev = dev->private;
+	const struct gpio_tilt_platform_data *pdata = tdev->pdata;
+	struct input_dev *input = dev->input;
+	struct gpio_tilt_state *tilt_state = NULL;
+	int state, i;
+
+	if (tdev->count < tdev->threshold) {
+		tdev->count++;
+	} else {
+		state = 0;
+		for (i = 0; i < pdata->nr_gpios; i++)
+			state |= (!!gpio_get_value(pdata->gpios[i].gpio) << i);
+
+		if (state != tdev->last_state) {
+			for (i = 0; i < pdata->nr_states; i++)
+				if (pdata->states[i].gpios == state)
+					tilt_state = &pdata->states[i];
+
+			if (tilt_state) {
+				for (i = 0; i < pdata->nr_axes; i++)
+					input_report_abs(input,
+							 pdata->axes[i].axis,
+							 tilt_state->axes[i]);
+
+				input_sync(input);
+			}
+
+			tdev->count = 0;
+			tdev->last_state = state;
+		}
+	}
+}
+
+static void gpio_tilt_polled_open(struct input_polled_dev *dev)
+{
+	struct gpio_tilt_polled_dev *tdev = dev->private;
+	const struct gpio_tilt_platform_data *pdata = tdev->pdata;
+
+	if (pdata->enable)
+		pdata->enable(tdev->dev);
+
+	/* report initial state of the axes */
+	tdev->last_state = -1;
+	tdev->count = tdev->threshold;
+	gpio_tilt_polled_poll(tdev->poll_dev);
+}
+
+static void gpio_tilt_polled_close(struct input_polled_dev *dev)
+{
+	struct gpio_tilt_polled_dev *tdev = dev->private;
+	const struct gpio_tilt_platform_data *pdata = tdev->pdata;
+
+	if (pdata->disable)
+		pdata->disable(tdev->dev);
+}
+
+static int __devinit gpio_tilt_polled_probe(struct platform_device *pdev)
+{
+	const struct gpio_tilt_platform_data *pdata = pdev->dev.platform_data;
+	struct device *dev = &pdev->dev;
+	struct gpio_tilt_polled_dev *tdev;
+	struct input_polled_dev *poll_dev;
+	struct input_dev *input;
+	int error, i;
+
+	if (!pdata || !pdata->poll_interval)
+		return -EINVAL;
+
+	tdev = kzalloc(sizeof(struct gpio_tilt_polled_dev), GFP_KERNEL);
+	if (!tdev) {
+		dev_err(dev, "no memory for private data\n");
+		return -ENOMEM;
+	}
+
+	error = gpio_request_array(pdata->gpios, pdata->nr_gpios);
+	if (error) {
+		dev_err(dev,
+			"Could not request tilt GPIOs: %d\n", error);
+		goto err_free_tdev;
+	}
+
+	poll_dev = input_allocate_polled_device();
+	if (!poll_dev) {
+		dev_err(dev, "no memory for polled device\n");
+		error = -ENOMEM;
+		goto err_free_gpios;
+	}
+
+	poll_dev->private = tdev;
+	poll_dev->poll = gpio_tilt_polled_poll;
+	poll_dev->poll_interval = pdata->poll_interval;
+	poll_dev->open = gpio_tilt_polled_open;
+	poll_dev->close = gpio_tilt_polled_close;
+
+	input = poll_dev->input;
+
+	input->name = pdev->name;
+	input->phys = DRV_NAME"/input0";
+	input->dev.parent = &pdev->dev;
+
+	input->id.bustype = BUS_HOST;
+	input->id.vendor = 0x0001;
+	input->id.product = 0x0001;
+	input->id.version = 0x0100;
+
+	__set_bit(EV_ABS, input->evbit);
+	for (i = 0; i < pdata->nr_axes; i++)
+		input_set_abs_params(input, pdata->axes[i].axis,
+				     pdata->axes[i].min, pdata->axes[i].max,
+				     pdata->axes[i].fuzz, pdata->axes[i].flat);
+
+	tdev->threshold = DIV_ROUND_UP(pdata->debounce_interval,
+				       pdata->poll_interval);
+
+	tdev->poll_dev = poll_dev;
+	tdev->dev = dev;
+	tdev->pdata = pdata;
+
+	error = input_register_polled_device(poll_dev);
+	if (error) {
+		dev_err(dev, "unable to register polled device, err=%d\n",
+			error);
+		goto err_free_polldev;
+	}
+
+	platform_set_drvdata(pdev, tdev);
+
+	return 0;
+
+err_free_polldev:
+	input_free_polled_device(poll_dev);
+err_free_gpios:
+	gpio_free_array(pdata->gpios, pdata->nr_gpios);
+err_free_tdev:
+	kfree(tdev);
+
+	return error;
+}
+
+static int __devexit gpio_tilt_polled_remove(struct platform_device *pdev)
+{
+	struct gpio_tilt_polled_dev *tdev = platform_get_drvdata(pdev);
+	const struct gpio_tilt_platform_data *pdata = tdev->pdata;
+
+	platform_set_drvdata(pdev, NULL);
+
+	input_unregister_polled_device(tdev->poll_dev);
+	input_free_polled_device(tdev->poll_dev);
+
+	gpio_free_array(pdata->gpios, pdata->nr_gpios);
+
+	kfree(tdev);
+
+	return 0;
+}
+
+static struct platform_driver gpio_tilt_polled_driver = {
+	.probe	= gpio_tilt_polled_probe,
+	.remove	= __devexit_p(gpio_tilt_polled_remove),
+	.driver	= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_platform_driver(gpio_tilt_polled_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Heiko Stuebner <heiko@sntech.de>");
+MODULE_DESCRIPTION("Polled GPIO tilt driver");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/input/misc/ixp4xx-beeper.c b/drivers/input/misc/ixp4xx-beeper.c
index 302ab46ce752..50e283068301 100644
--- a/drivers/input/misc/ixp4xx-beeper.c
+++ b/drivers/input/misc/ixp4xx-beeper.c
@@ -168,16 +168,5 @@ static struct platform_driver ixp4xx_spkr_platform_driver = {
 	.remove		= __devexit_p(ixp4xx_spkr_remove),
 	.shutdown	= ixp4xx_spkr_shutdown,
 };
+module_platform_driver(ixp4xx_spkr_platform_driver);
 
-static int __init ixp4xx_spkr_init(void)
-{
-	return platform_driver_register(&ixp4xx_spkr_platform_driver);
-}
-
-static void __exit ixp4xx_spkr_exit(void)
-{
-	platform_driver_unregister(&ixp4xx_spkr_platform_driver);
-}
-
-module_init(ixp4xx_spkr_init);
-module_exit(ixp4xx_spkr_exit);
diff --git a/drivers/input/misc/max8925_onkey.c b/drivers/input/misc/max8925_onkey.c
index 7de0ded4ccc3..23cf08271049 100644
--- a/drivers/input/misc/max8925_onkey.c
+++ b/drivers/input/misc/max8925_onkey.c
@@ -166,18 +166,7 @@ static struct platform_driver max8925_onkey_driver = {
 	.probe		= max8925_onkey_probe,
 	.remove		= __devexit_p(max8925_onkey_remove),
 };
-
-static int __init max8925_onkey_init(void)
-{
-	return platform_driver_register(&max8925_onkey_driver);
-}
-module_init(max8925_onkey_init);
-
-static void __exit max8925_onkey_exit(void)
-{
-	platform_driver_unregister(&max8925_onkey_driver);
-}
-module_exit(max8925_onkey_exit);
+module_platform_driver(max8925_onkey_driver);
 
 MODULE_DESCRIPTION("Maxim MAX8925 ONKEY driver");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
diff --git a/drivers/input/misc/mc13783-pwrbutton.c b/drivers/input/misc/mc13783-pwrbutton.c
index 09b052288657..8428f1e8e83e 100644
--- a/drivers/input/misc/mc13783-pwrbutton.c
+++ b/drivers/input/misc/mc13783-pwrbutton.c
@@ -255,7 +255,7 @@ static int __devexit mc13783_pwrbutton_remove(struct platform_device *pdev)
 	return 0;
 }
 
-struct platform_driver mc13783_pwrbutton_driver = {
+static struct platform_driver mc13783_pwrbutton_driver = {
 	.probe		= mc13783_pwrbutton_probe,
 	.remove		= __devexit_p(mc13783_pwrbutton_remove),
 	.driver		= {
@@ -264,17 +264,7 @@ struct platform_driver mc13783_pwrbutton_driver = {
 	},
 };
 
-static int __init mc13783_pwrbutton_init(void)
-{
-	return platform_driver_register(&mc13783_pwrbutton_driver);
-}
-module_init(mc13783_pwrbutton_init);
-
-static void __exit mc13783_pwrbutton_exit(void)
-{
-	platform_driver_unregister(&mc13783_pwrbutton_driver);
-}
-module_exit(mc13783_pwrbutton_exit);
+module_platform_driver(mc13783_pwrbutton_driver);
 
 MODULE_ALIAS("platform:mc13783-pwrbutton");
 MODULE_DESCRIPTION("MC13783 Power Button");
diff --git a/drivers/input/misc/mpu3050.c b/drivers/input/misc/mpu3050.c
index f71dc728da58..208d1a1cc7f3 100644
--- a/drivers/input/misc/mpu3050.c
+++ b/drivers/input/misc/mpu3050.c
@@ -41,18 +41,67 @@
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
 
-#define MPU3050_CHIP_ID_REG	0x00
 #define MPU3050_CHIP_ID		0x69
-#define MPU3050_XOUT_H		0x1D
-#define MPU3050_PWR_MGM		0x3E
-#define MPU3050_PWR_MGM_POS	6
-#define MPU3050_PWR_MGM_MASK	0x40
 
 #define MPU3050_AUTO_DELAY	1000
 
 #define MPU3050_MIN_VALUE	-32768
 #define MPU3050_MAX_VALUE	32767
 
+#define MPU3050_DEFAULT_POLL_INTERVAL	200
+#define MPU3050_DEFAULT_FS_RANGE	3
+
+/* Register map */
+#define MPU3050_CHIP_ID_REG	0x00
+#define MPU3050_SMPLRT_DIV	0x15
+#define MPU3050_DLPF_FS_SYNC	0x16
+#define MPU3050_INT_CFG		0x17
+#define MPU3050_XOUT_H		0x1D
+#define MPU3050_PWR_MGM		0x3E
+#define MPU3050_PWR_MGM_POS	6
+
+/* Register bits */
+
+/* DLPF_FS_SYNC */
+#define MPU3050_EXT_SYNC_NONE		0x00
+#define MPU3050_EXT_SYNC_TEMP		0x20
+#define MPU3050_EXT_SYNC_GYROX		0x40
+#define MPU3050_EXT_SYNC_GYROY		0x60
+#define MPU3050_EXT_SYNC_GYROZ		0x80
+#define MPU3050_EXT_SYNC_ACCELX	0xA0
+#define MPU3050_EXT_SYNC_ACCELY	0xC0
+#define MPU3050_EXT_SYNC_ACCELZ	0xE0
+#define MPU3050_EXT_SYNC_MASK		0xE0
+#define MPU3050_FS_250DPS		0x00
+#define MPU3050_FS_500DPS		0x08
+#define MPU3050_FS_1000DPS		0x10
+#define MPU3050_FS_2000DPS		0x18
+#define MPU3050_FS_MASK		0x18
+#define MPU3050_DLPF_CFG_256HZ_NOLPF2	0x00
+#define MPU3050_DLPF_CFG_188HZ		0x01
+#define MPU3050_DLPF_CFG_98HZ		0x02
+#define MPU3050_DLPF_CFG_42HZ		0x03
+#define MPU3050_DLPF_CFG_20HZ		0x04
+#define MPU3050_DLPF_CFG_10HZ		0x05
+#define MPU3050_DLPF_CFG_5HZ		0x06
+#define MPU3050_DLPF_CFG_2100HZ_NOLPF	0x07
+#define MPU3050_DLPF_CFG_MASK		0x07
+/* INT_CFG */
+#define MPU3050_RAW_RDY_EN		0x01
+#define MPU3050_MPU_RDY_EN		0x02
+#define MPU3050_LATCH_INT_EN		0x04
+/* PWR_MGM */
+#define MPU3050_PWR_MGM_PLL_X		0x01
+#define MPU3050_PWR_MGM_PLL_Y		0x02
+#define MPU3050_PWR_MGM_PLL_Z		0x03
+#define MPU3050_PWR_MGM_CLKSEL		0x07
+#define MPU3050_PWR_MGM_STBY_ZG	0x08
+#define MPU3050_PWR_MGM_STBY_YG	0x10
+#define MPU3050_PWR_MGM_STBY_XG	0x20
+#define MPU3050_PWR_MGM_SLEEP		0x40
+#define MPU3050_PWR_MGM_RESET		0x80
+#define MPU3050_PWR_MGM_MASK		0x40
+
 struct axis_data {
 	s16 x;
 	s16 y;
@@ -148,9 +197,20 @@ static void mpu3050_set_power_mode(struct i2c_client *client, u8 val)
 static int mpu3050_input_open(struct input_dev *input)
 {
 	struct mpu3050_sensor *sensor = input_get_drvdata(input);
+	int error;
 
 	pm_runtime_get(sensor->dev);
 
+	/* Enable interrupts */
+	error = i2c_smbus_write_byte_data(sensor->client, MPU3050_INT_CFG,
+					  MPU3050_LATCH_INT_EN |
+					  MPU3050_RAW_RDY_EN |
+					  MPU3050_MPU_RDY_EN);
+	if (error < 0) {
+		pm_runtime_put(sensor->dev);
+		return error;
+	}
+
 	return 0;
 }
 
@@ -192,6 +252,51 @@ static irqreturn_t mpu3050_interrupt_thread(int irq, void *data)
 }
 
 /**
+ *	mpu3050_hw_init	-	initialize hardware
+ *	@sensor: the sensor
+ *
+ *	Called during device probe; configures the sampling method.
+ */
+static int __devinit mpu3050_hw_init(struct mpu3050_sensor *sensor)
+{
+	struct i2c_client *client = sensor->client;
+	int ret;
+	u8 reg;
+
+	/* Reset */
+	ret = i2c_smbus_write_byte_data(client, MPU3050_PWR_MGM,
+					MPU3050_PWR_MGM_RESET);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_read_byte_data(client, MPU3050_PWR_MGM);
+	if (ret < 0)
+		return ret;
+
+	ret &= ~MPU3050_PWR_MGM_CLKSEL;
+	ret |= MPU3050_PWR_MGM_PLL_Z;
+	ret = i2c_smbus_write_byte_data(client, MPU3050_PWR_MGM, ret);
+	if (ret < 0)
+		return ret;
+
+	/* Output frequency divider. The poll interval */
+	ret = i2c_smbus_write_byte_data(client, MPU3050_SMPLRT_DIV,
+					MPU3050_DEFAULT_POLL_INTERVAL - 1);
+	if (ret < 0)
+		return ret;
+
+	/* Set low pass filter and full scale */
+	reg = MPU3050_DEFAULT_FS_RANGE;
+	reg |= MPU3050_DLPF_CFG_42HZ << 3;
+	reg |= MPU3050_EXT_SYNC_NONE << 5;
+	ret = i2c_smbus_write_byte_data(client, MPU3050_DLPF_FS_SYNC, reg);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/**
  *	mpu3050_probe	-	device detection callback
  *	@client: i2c client of found device
  *	@id: id match information
@@ -256,10 +361,14 @@ static int __devinit mpu3050_probe(struct i2c_client *client,
 
 	pm_runtime_set_active(&client->dev);
 
+	error = mpu3050_hw_init(sensor);
+	if (error)
+		goto err_pm_set_suspended;
+
 	error = request_threaded_irq(client->irq,
 				     NULL, mpu3050_interrupt_thread,
 				     IRQF_TRIGGER_RISING,
-				     "mpu_int", sensor);
+				     "mpu3050", sensor);
 	if (error) {
 		dev_err(&client->dev,
 			"can't get IRQ %d, error %d\n", client->irq, error);
@@ -348,11 +457,18 @@ static const struct i2c_device_id mpu3050_ids[] = {
 };
 MODULE_DEVICE_TABLE(i2c, mpu3050_ids);
 
+static const struct of_device_id mpu3050_of_match[] = {
+	{ .compatible = "invn,mpu3050", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, mpu3050_of_match);
+
 static struct i2c_driver mpu3050_i2c_driver = {
 	.driver	= {
 		.name	= "mpu3050",
 		.owner	= THIS_MODULE,
 		.pm	= &mpu3050_pm,
+		.of_match_table = mpu3050_of_match,
 	},
 	.probe		= mpu3050_probe,
 	.remove		= __devexit_p(mpu3050_remove),
diff --git a/drivers/input/misc/pcap_keys.c b/drivers/input/misc/pcap_keys.c
index 99335c286250..e09b4fe81913 100644
--- a/drivers/input/misc/pcap_keys.c
+++ b/drivers/input/misc/pcap_keys.c
@@ -125,19 +125,7 @@ static struct platform_driver pcap_keys_device_driver = {
 		.owner	= THIS_MODULE,
 	}
 };
-
-static int __init pcap_keys_init(void)
-{
-	return platform_driver_register(&pcap_keys_device_driver);
-};
-
-static void __exit pcap_keys_exit(void)
-{
-	platform_driver_unregister(&pcap_keys_device_driver);
-};
-
-module_init(pcap_keys_init);
-module_exit(pcap_keys_exit);
+module_platform_driver(pcap_keys_device_driver);
 
 MODULE_DESCRIPTION("Motorola PCAP2 input events driver");
 MODULE_AUTHOR("Ilya Petrov <ilya.muromec@gmail.com>");
diff --git a/drivers/input/misc/pcf50633-input.c b/drivers/input/misc/pcf50633-input.c
index 95562735728d..53891de80b0e 100644
--- a/drivers/input/misc/pcf50633-input.c
+++ b/drivers/input/misc/pcf50633-input.c
@@ -113,18 +113,7 @@ static struct platform_driver pcf50633_input_driver = {
 	.probe = pcf50633_input_probe,
 	.remove = __devexit_p(pcf50633_input_remove),
 };
-
-static int __init pcf50633_input_init(void)
-{
-	return platform_driver_register(&pcf50633_input_driver);
-}
-module_init(pcf50633_input_init);
-
-static void __exit pcf50633_input_exit(void)
-{
-	platform_driver_unregister(&pcf50633_input_driver);
-}
-module_exit(pcf50633_input_exit);
+module_platform_driver(pcf50633_input_driver);
 
 MODULE_AUTHOR("Balaji Rao <balajirrao@openmoko.org>");
 MODULE_DESCRIPTION("PCF50633 input driver");
diff --git a/drivers/input/misc/pcspkr.c b/drivers/input/misc/pcspkr.c
index 34f4d2e0f50f..b2484aa07f32 100644
--- a/drivers/input/misc/pcspkr.c
+++ b/drivers/input/misc/pcspkr.c
@@ -134,17 +134,5 @@ static struct platform_driver pcspkr_platform_driver = {
 	.remove		= __devexit_p(pcspkr_remove),
 	.shutdown	= pcspkr_shutdown,
 };
+module_platform_driver(pcspkr_platform_driver);
 
-
-static int __init pcspkr_init(void)
-{
-	return platform_driver_register(&pcspkr_platform_driver);
-}
-
-static void __exit pcspkr_exit(void)
-{
-	platform_driver_unregister(&pcspkr_platform_driver);
-}
-
-module_init(pcspkr_init);
-module_exit(pcspkr_exit);
diff --git a/drivers/input/misc/pm8xxx-vibrator.c b/drivers/input/misc/pm8xxx-vibrator.c
index 43192930824b..dfbfb463ea5d 100644
--- a/drivers/input/misc/pm8xxx-vibrator.c
+++ b/drivers/input/misc/pm8xxx-vibrator.c
@@ -277,18 +277,7 @@ static struct platform_driver pm8xxx_vib_driver = {
 		.pm	= &pm8xxx_vib_pm_ops,
 	},
 };
-
-static int __init pm8xxx_vib_init(void)
-{
-	return platform_driver_register(&pm8xxx_vib_driver);
-}
-module_init(pm8xxx_vib_init);
-
-static void __exit pm8xxx_vib_exit(void)
-{
-	platform_driver_unregister(&pm8xxx_vib_driver);
-}
-module_exit(pm8xxx_vib_exit);
+module_platform_driver(pm8xxx_vib_driver);
 
 MODULE_ALIAS("platform:pm8xxx_vib");
 MODULE_DESCRIPTION("PMIC8xxx vibrator driver based on ff-memless framework");
diff --git a/drivers/input/misc/pmic8xxx-pwrkey.c b/drivers/input/misc/pmic8xxx-pwrkey.c
index b3cfb9c71e66..0f83d0f1d015 100644
--- a/drivers/input/misc/pmic8xxx-pwrkey.c
+++ b/drivers/input/misc/pmic8xxx-pwrkey.c
@@ -213,18 +213,7 @@ static struct platform_driver pmic8xxx_pwrkey_driver = {
 		.pm	= &pm8xxx_pwr_key_pm_ops,
 	},
 };
-
-static int __init pmic8xxx_pwrkey_init(void)
-{
-	return platform_driver_register(&pmic8xxx_pwrkey_driver);
-}
-module_init(pmic8xxx_pwrkey_init);
-
-static void __exit pmic8xxx_pwrkey_exit(void)
-{
-	platform_driver_unregister(&pmic8xxx_pwrkey_driver);
-}
-module_exit(pmic8xxx_pwrkey_exit);
+module_platform_driver(pmic8xxx_pwrkey_driver);
 
 MODULE_ALIAS("platform:pmic8xxx_pwrkey");
 MODULE_DESCRIPTION("PMIC8XXX Power Key driver");
diff --git a/drivers/input/misc/pwm-beeper.c b/drivers/input/misc/pwm-beeper.c
index 57c294f07198..fc84c8a51147 100644
--- a/drivers/input/misc/pwm-beeper.c
+++ b/drivers/input/misc/pwm-beeper.c
@@ -180,18 +180,7 @@ static struct platform_driver pwm_beeper_driver = {
 		.pm	= PWM_BEEPER_PM_OPS,
 	},
 };
-
-static int __init pwm_beeper_init(void)
-{
-	return platform_driver_register(&pwm_beeper_driver);
-}
-module_init(pwm_beeper_init);
-
-static void __exit pwm_beeper_exit(void)
-{
-	platform_driver_unregister(&pwm_beeper_driver);
-}
-module_exit(pwm_beeper_exit);
+module_platform_driver(pwm_beeper_driver);
 
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
 MODULE_DESCRIPTION("PWM beeper driver");
diff --git a/drivers/input/misc/rb532_button.c b/drivers/input/misc/rb532_button.c
index e2c7f622a0b5..aeb02bcf7233 100644
--- a/drivers/input/misc/rb532_button.c
+++ b/drivers/input/misc/rb532_button.c
@@ -100,19 +100,7 @@ static struct platform_driver rb532_button_driver = {
 		.owner = THIS_MODULE,
 	},
 };
-
-static int __init rb532_button_init(void)
-{
-	return platform_driver_register(&rb532_button_driver);
-}
-
-static void __exit rb532_button_exit(void)
-{
-	platform_driver_unregister(&rb532_button_driver);
-}
-
-module_init(rb532_button_init);
-module_exit(rb532_button_exit);
+module_platform_driver(rb532_button_driver);
 
 MODULE_AUTHOR("Phil Sutter <n0-1@freewrt.org>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/rotary_encoder.c b/drivers/input/misc/rotary_encoder.c
index 2be21694fac1..f07f784198b9 100644
--- a/drivers/input/misc/rotary_encoder.c
+++ b/drivers/input/misc/rotary_encoder.c
@@ -284,19 +284,7 @@ static struct platform_driver rotary_encoder_driver = {
 		.owner	= THIS_MODULE,
 	}
 };
-
-static int __init rotary_encoder_init(void)
-{
-	return platform_driver_register(&rotary_encoder_driver);
-}
-
-static void __exit rotary_encoder_exit(void)
-{
-	platform_driver_unregister(&rotary_encoder_driver);
-}
-
-module_init(rotary_encoder_init);
-module_exit(rotary_encoder_exit);
+module_platform_driver(rotary_encoder_driver);
 
 MODULE_ALIAS("platform:" DRV_NAME);
 MODULE_DESCRIPTION("GPIO rotary encoder driver");
diff --git a/drivers/input/misc/sgi_btns.c b/drivers/input/misc/sgi_btns.c
index 1a80c0dab83b..5d9fd5571199 100644
--- a/drivers/input/misc/sgi_btns.c
+++ b/drivers/input/misc/sgi_btns.c
@@ -164,17 +164,6 @@ static struct platform_driver sgi_buttons_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init sgi_buttons_init(void)
-{
-	return platform_driver_register(&sgi_buttons_driver);
-}
-
-static void __exit sgi_buttons_exit(void)
-{
-	platform_driver_unregister(&sgi_buttons_driver);
-}
+module_platform_driver(sgi_buttons_driver);
 
 MODULE_LICENSE("GPL");
-module_init(sgi_buttons_init);
-module_exit(sgi_buttons_exit);
diff --git a/drivers/input/misc/twl4030-pwrbutton.c b/drivers/input/misc/twl4030-pwrbutton.c
index 38e4b507b94c..19a68828cd86 100644
--- a/drivers/input/misc/twl4030-pwrbutton.c
+++ b/drivers/input/misc/twl4030-pwrbutton.c
@@ -107,25 +107,14 @@ static int __exit twl4030_pwrbutton_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver twl4030_pwrbutton_driver = {
+	.probe		= twl4030_pwrbutton_probe,
 	.remove		= __exit_p(twl4030_pwrbutton_remove),
 	.driver		= {
 		.name	= "twl4030_pwrbutton",
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init twl4030_pwrbutton_init(void)
-{
-	return platform_driver_probe(&twl4030_pwrbutton_driver,
-			twl4030_pwrbutton_probe);
-}
-module_init(twl4030_pwrbutton_init);
-
-static void __exit twl4030_pwrbutton_exit(void)
-{
-	platform_driver_unregister(&twl4030_pwrbutton_driver);
-}
-module_exit(twl4030_pwrbutton_exit);
+module_platform_driver(twl4030_pwrbutton_driver);
 
 MODULE_ALIAS("platform:twl4030_pwrbutton");
 MODULE_DESCRIPTION("Triton2 Power Button");
diff --git a/drivers/input/misc/twl4030-vibra.c b/drivers/input/misc/twl4030-vibra.c
index 3c1a432c14dc..37651373a95b 100644
--- a/drivers/input/misc/twl4030-vibra.c
+++ b/drivers/input/misc/twl4030-vibra.c
@@ -278,21 +278,9 @@ static struct platform_driver twl4030_vibra_driver = {
 #endif
 	},
 };
-
-static int __init twl4030_vibra_init(void)
-{
-	return platform_driver_register(&twl4030_vibra_driver);
-}
-module_init(twl4030_vibra_init);
-
-static void __exit twl4030_vibra_exit(void)
-{
-	platform_driver_unregister(&twl4030_vibra_driver);
-}
-module_exit(twl4030_vibra_exit);
+module_platform_driver(twl4030_vibra_driver);
 
 MODULE_ALIAS("platform:twl4030-vibra");
-
 MODULE_DESCRIPTION("TWL4030 Vibra driver");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Nokia Corporation");
diff --git a/drivers/input/misc/twl6040-vibra.c b/drivers/input/misc/twl6040-vibra.c
index ad153a417eed..45874fed523a 100644
--- a/drivers/input/misc/twl6040-vibra.c
+++ b/drivers/input/misc/twl6040-vibra.c
@@ -410,18 +410,7 @@ static struct platform_driver twl6040_vibra_driver = {
 		.pm	= &twl6040_vibra_pm_ops,
 	},
 };
-
-static int __init twl6040_vibra_init(void)
-{
-	return platform_driver_register(&twl6040_vibra_driver);
-}
-module_init(twl6040_vibra_init);
-
-static void __exit twl6040_vibra_exit(void)
-{
-	platform_driver_unregister(&twl6040_vibra_driver);
-}
-module_exit(twl6040_vibra_exit);
+module_platform_driver(twl6040_vibra_driver);
 
 MODULE_ALIAS("platform:twl6040-vibra");
 MODULE_DESCRIPTION("TWL6040 Vibra driver");
diff --git a/drivers/input/misc/wm831x-on.c b/drivers/input/misc/wm831x-on.c
index c3d7ba5f5b47..47f18d6bce46 100644
--- a/drivers/input/misc/wm831x-on.c
+++ b/drivers/input/misc/wm831x-on.c
@@ -145,18 +145,7 @@ static struct platform_driver wm831x_on_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init wm831x_on_init(void)
-{
-	return platform_driver_register(&wm831x_on_driver);
-}
-module_init(wm831x_on_init);
-
-static void __exit wm831x_on_exit(void)
-{
-	platform_driver_unregister(&wm831x_on_driver);
-}
-module_exit(wm831x_on_exit);
+module_platform_driver(wm831x_on_driver);
 
 MODULE_ALIAS("platform:wm831x-on");
 MODULE_DESCRIPTION("WM831x ON pin");
diff --git a/drivers/input/misc/xen-kbdfront.c b/drivers/input/misc/xen-kbdfront.c
index ad2e51c04db8..02ca8680ea5b 100644
--- a/drivers/input/misc/xen-kbdfront.c
+++ b/drivers/input/misc/xen-kbdfront.c
@@ -361,15 +361,12 @@ static const struct xenbus_device_id xenkbd_ids[] = {
 	{ "" }
 };
 
-static struct xenbus_driver xenkbd_driver = {
-	.name = "vkbd",
-	.owner = THIS_MODULE,
-	.ids = xenkbd_ids,
+static DEFINE_XENBUS_DRIVER(xenkbd, ,
 	.probe = xenkbd_probe,
 	.remove = xenkbd_remove,
 	.resume = xenkbd_resume,
 	.otherend_changed = xenkbd_backend_changed,
-};
+);
 
 static int __init xenkbd_init(void)
 {
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index 003587c71f43..bd87380bd879 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -17,13 +17,63 @@
 
 #include <linux/slab.h>
 #include <linux/input.h>
+#include <linux/input/mt.h>
 #include <linux/serio.h>
 #include <linux/libps2.h>
 
 #include "psmouse.h"
 #include "alps.h"
 
-#define ALPS_OLDPROTO		0x01	/* old style input */
+/*
+ * Definitions for ALPS version 3 and 4 command mode protocol
+ */
+#define ALPS_V3_X_MAX	2000
+#define ALPS_V3_Y_MAX	1400
+
+#define ALPS_BITMAP_X_BITS	15
+#define ALPS_BITMAP_Y_BITS	11
+
+#define ALPS_CMD_NIBBLE_10	0x01f2
+
+static const struct alps_nibble_commands alps_v3_nibble_commands[] = {
+	{ PSMOUSE_CMD_SETPOLL,		0x00 }, /* 0 */
+	{ PSMOUSE_CMD_RESET_DIS,	0x00 }, /* 1 */
+	{ PSMOUSE_CMD_SETSCALE21,	0x00 }, /* 2 */
+	{ PSMOUSE_CMD_SETRATE,		0x0a }, /* 3 */
+	{ PSMOUSE_CMD_SETRATE,		0x14 }, /* 4 */
+	{ PSMOUSE_CMD_SETRATE,		0x28 }, /* 5 */
+	{ PSMOUSE_CMD_SETRATE,		0x3c }, /* 6 */
+	{ PSMOUSE_CMD_SETRATE,		0x50 }, /* 7 */
+	{ PSMOUSE_CMD_SETRATE,		0x64 }, /* 8 */
+	{ PSMOUSE_CMD_SETRATE,		0xc8 }, /* 9 */
+	{ ALPS_CMD_NIBBLE_10,		0x00 }, /* a */
+	{ PSMOUSE_CMD_SETRES,		0x00 }, /* b */
+	{ PSMOUSE_CMD_SETRES,		0x01 }, /* c */
+	{ PSMOUSE_CMD_SETRES,		0x02 }, /* d */
+	{ PSMOUSE_CMD_SETRES,		0x03 }, /* e */
+	{ PSMOUSE_CMD_SETSCALE11,	0x00 }, /* f */
+};
+
+static const struct alps_nibble_commands alps_v4_nibble_commands[] = {
+	{ PSMOUSE_CMD_ENABLE,		0x00 }, /* 0 */
+	{ PSMOUSE_CMD_RESET_DIS,	0x00 }, /* 1 */
+	{ PSMOUSE_CMD_SETSCALE21,	0x00 }, /* 2 */
+	{ PSMOUSE_CMD_SETRATE,		0x0a }, /* 3 */
+	{ PSMOUSE_CMD_SETRATE,		0x14 }, /* 4 */
+	{ PSMOUSE_CMD_SETRATE,		0x28 }, /* 5 */
+	{ PSMOUSE_CMD_SETRATE,		0x3c }, /* 6 */
+	{ PSMOUSE_CMD_SETRATE,		0x50 }, /* 7 */
+	{ PSMOUSE_CMD_SETRATE,		0x64 }, /* 8 */
+	{ PSMOUSE_CMD_SETRATE,		0xc8 }, /* 9 */
+	{ ALPS_CMD_NIBBLE_10,		0x00 }, /* a */
+	{ PSMOUSE_CMD_SETRES,		0x00 }, /* b */
+	{ PSMOUSE_CMD_SETRES,		0x01 }, /* c */
+	{ PSMOUSE_CMD_SETRES,		0x02 }, /* d */
+	{ PSMOUSE_CMD_SETRES,		0x03 }, /* e */
+	{ PSMOUSE_CMD_SETSCALE11,	0x00 }, /* f */
+};
+
+
 #define ALPS_DUALPOINT		0x02	/* touchpad has trackstick */
 #define ALPS_PASS		0x04	/* device has a pass-through port */
 
@@ -35,30 +85,33 @@
 					   6-byte ALPS packet */
 
 static const struct alps_model_info alps_model_data[] = {
-	{ { 0x32, 0x02, 0x14 },	0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT }, /* Toshiba Salellite Pro M10 */
-	{ { 0x33, 0x02, 0x0a },	0x88, 0xf8, ALPS_OLDPROTO },		  /* UMAX-530T */
-	{ { 0x53, 0x02, 0x0a },	0xf8, 0xf8, 0 },
-	{ { 0x53, 0x02, 0x14 },	0xf8, 0xf8, 0 },
-	{ { 0x60, 0x03, 0xc8 }, 0xf8, 0xf8, 0 },			  /* HP ze1115 */
-	{ { 0x63, 0x02, 0x0a },	0xf8, 0xf8, 0 },
-	{ { 0x63, 0x02, 0x14 },	0xf8, 0xf8, 0 },
-	{ { 0x63, 0x02, 0x28 },	0xf8, 0xf8, ALPS_FW_BK_2 },		  /* Fujitsu Siemens S6010 */
-	{ { 0x63, 0x02, 0x3c },	0x8f, 0x8f, ALPS_WHEEL },		  /* Toshiba Satellite S2400-103 */
-	{ { 0x63, 0x02, 0x50 },	0xef, 0xef, ALPS_FW_BK_1 },		  /* NEC Versa L320 */
-	{ { 0x63, 0x02, 0x64 },	0xf8, 0xf8, 0 },
-	{ { 0x63, 0x03, 0xc8 }, 0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT }, /* Dell Latitude D800 */
-	{ { 0x73, 0x00, 0x0a },	0xf8, 0xf8, ALPS_DUALPOINT },		  /* ThinkPad R61 8918-5QG */
-	{ { 0x73, 0x02, 0x0a },	0xf8, 0xf8, 0 },
-	{ { 0x73, 0x02, 0x14 },	0xf8, 0xf8, ALPS_FW_BK_2 },		  /* Ahtec Laptop */
-	{ { 0x20, 0x02, 0x0e },	0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT }, /* XXX */
-	{ { 0x22, 0x02, 0x0a },	0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT },
-	{ { 0x22, 0x02, 0x14 }, 0xff, 0xff, ALPS_PASS | ALPS_DUALPOINT }, /* Dell Latitude D600 */
+	{ { 0x32, 0x02, 0x14 },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT },	/* Toshiba Salellite Pro M10 */
+	{ { 0x33, 0x02, 0x0a },	0x00, ALPS_PROTO_V1, 0x88, 0xf8, 0 },				/* UMAX-530T */
+	{ { 0x53, 0x02, 0x0a },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, 0 },
+	{ { 0x53, 0x02, 0x14 },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, 0 },
+	{ { 0x60, 0x03, 0xc8 }, 0x00, ALPS_PROTO_V2, 0xf8, 0xf8, 0 },				/* HP ze1115 */
+	{ { 0x63, 0x02, 0x0a },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, 0 },
+	{ { 0x63, 0x02, 0x14 },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, 0 },
+	{ { 0x63, 0x02, 0x28 },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, ALPS_FW_BK_2 },		/* Fujitsu Siemens S6010 */
+	{ { 0x63, 0x02, 0x3c },	0x00, ALPS_PROTO_V2, 0x8f, 0x8f, ALPS_WHEEL },			/* Toshiba Satellite S2400-103 */
+	{ { 0x63, 0x02, 0x50 },	0x00, ALPS_PROTO_V2, 0xef, 0xef, ALPS_FW_BK_1 },		/* NEC Versa L320 */
+	{ { 0x63, 0x02, 0x64 },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, 0 },
+	{ { 0x63, 0x03, 0xc8 }, 0x00, ALPS_PROTO_V2, 0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT },	/* Dell Latitude D800 */
+	{ { 0x73, 0x00, 0x0a },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, ALPS_DUALPOINT },		/* ThinkPad R61 8918-5QG */
+	{ { 0x73, 0x02, 0x0a },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, 0 },
+	{ { 0x73, 0x02, 0x14 },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, ALPS_FW_BK_2 },		/* Ahtec Laptop */
+	{ { 0x20, 0x02, 0x0e },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT },	/* XXX */
+	{ { 0x22, 0x02, 0x0a },	0x00, ALPS_PROTO_V2, 0xf8, 0xf8, ALPS_PASS | ALPS_DUALPOINT },
+	{ { 0x22, 0x02, 0x14 }, 0x00, ALPS_PROTO_V2, 0xff, 0xff, ALPS_PASS | ALPS_DUALPOINT },	/* Dell Latitude D600 */
 	/* Dell Latitude E5500, E6400, E6500, Precision M4400 */
-	{ { 0x62, 0x02, 0x14 }, 0xcf, 0xcf,
+	{ { 0x62, 0x02, 0x14 }, 0x00, ALPS_PROTO_V2, 0xcf, 0xcf,
 		ALPS_PASS | ALPS_DUALPOINT | ALPS_PS2_INTERLEAVED },
-	{ { 0x73, 0x02, 0x50 }, 0xcf, 0xcf, ALPS_FOUR_BUTTONS },	  /* Dell Vostro 1400 */
-	{ { 0x52, 0x01, 0x14 }, 0xff, 0xff,
-		ALPS_PASS | ALPS_DUALPOINT | ALPS_PS2_INTERLEAVED },	  /* Toshiba Tecra A11-11L */
+	{ { 0x73, 0x02, 0x50 }, 0x00, ALPS_PROTO_V2, 0xcf, 0xcf, ALPS_FOUR_BUTTONS },		/* Dell Vostro 1400 */
+	{ { 0x52, 0x01, 0x14 }, 0x00, ALPS_PROTO_V2, 0xff, 0xff,
+		ALPS_PASS | ALPS_DUALPOINT | ALPS_PS2_INTERLEAVED },				/* Toshiba Tecra A11-11L */
+	{ { 0x73, 0x02, 0x64 },	0x9b, ALPS_PROTO_V3, 0x8f, 0x8f, ALPS_DUALPOINT },
+	{ { 0x73, 0x02, 0x64 },	0x9d, ALPS_PROTO_V3, 0x8f, 0x8f, ALPS_DUALPOINT },
+	{ { 0x73, 0x02, 0x64 },	0x8a, ALPS_PROTO_V4, 0x8f, 0x8f, 0 },
 };
 
 /*
@@ -67,42 +120,7 @@ static const struct alps_model_info alps_model_data[] = {
  * isn't valid per PS/2 spec.
  */
 
-/*
- * PS/2 packet format
- *
- * byte 0:  0    0 YSGN XSGN    1    M    R    L
- * byte 1: X7   X6   X5   X4   X3   X2   X1   X0
- * byte 2: Y7   Y6   Y5   Y4   Y3   Y2   Y1   Y0
- *
- * Note that the device never signals overflow condition.
- *
- * ALPS absolute Mode - new format
- *
- * byte 0:  1    ?    ?    ?    1    ?    ?    ?
- * byte 1:  0   x6   x5   x4   x3   x2   x1   x0
- * byte 2:  0  x10   x9   x8   x7    ?  fin  ges
- * byte 3:  0   y9   y8   y7    1    M    R    L
- * byte 4:  0   y6   y5   y4   y3   y2   y1   y0
- * byte 5:  0   z6   z5   z4   z3   z2   z1   z0
- *
- * Dualpoint device -- interleaved packet format
- *
- * byte 0:    1    1    0    0    1    1    1    1
- * byte 1:    0   x6   x5   x4   x3   x2   x1   x0
- * byte 2:    0  x10   x9   x8   x7    0  fin  ges
- * byte 3:    0    0 YSGN XSGN    1    1    1    1
- * byte 4:   X7   X6   X5   X4   X3   X2   X1   X0
- * byte 5:   Y7   Y6   Y5   Y4   Y3   Y2   Y1   Y0
- * byte 6:    0   y9   y8   y7    1    m    r    l
- * byte 7:    0   y6   y5   y4   y3   y2   y1   y0
- * byte 8:    0   z6   z5   z4   z3   z2   z1   z0
- *
- * CAPITALS = stick, miniscules = touchpad
- *
- * ?'s can have different meanings on different models,
- * such as wheel rotation, extra buttons, stick buttons
- * on a dualpoint, etc.
- */
+/* Packet formats are described in Documentation/input/alps.txt */
 
 static bool alps_is_valid_first_byte(const struct alps_model_info *model,
 				     unsigned char data)
@@ -137,7 +155,7 @@ static void alps_report_buttons(struct psmouse *psmouse,
 	input_sync(dev2);
 }
 
-static void alps_process_packet(struct psmouse *psmouse)
+static void alps_process_packet_v1_v2(struct psmouse *psmouse)
 {
 	struct alps_data *priv = psmouse->private;
 	const struct alps_model_info *model = priv->i;
@@ -147,7 +165,7 @@ static void alps_process_packet(struct psmouse *psmouse)
 	int x, y, z, ges, fin, left, right, middle;
 	int back = 0, forward = 0;
 
-	if (model->flags & ALPS_OLDPROTO) {
+	if (model->proto_version == ALPS_PROTO_V1) {
 		left = packet[2] & 0x10;
 		right = packet[2] & 0x08;
 		middle = 0;
@@ -239,6 +257,403 @@ static void alps_process_packet(struct psmouse *psmouse)
 	input_sync(dev);
 }
 
+/*
+ * Process bitmap data from v3 and v4 protocols. Returns the number of
+ * fingers detected. A return value of 0 means at least one of the
+ * bitmaps was empty.
+ *
+ * The bitmaps don't have enough data to track fingers, so this function
+ * only generates points representing a bounding box of all contacts.
+ * These points are returned in x1, y1, x2, and y2 when the return value
+ * is greater than 0.
+ */
+static int alps_process_bitmap(unsigned int x_map, unsigned int y_map,
+			       int *x1, int *y1, int *x2, int *y2)
+{
+	struct alps_bitmap_point {
+		int start_bit;
+		int num_bits;
+	};
+
+	int fingers_x = 0, fingers_y = 0, fingers;
+	int i, bit, prev_bit;
+	struct alps_bitmap_point x_low = {0,}, x_high = {0,};
+	struct alps_bitmap_point y_low = {0,}, y_high = {0,};
+	struct alps_bitmap_point *point;
+
+	if (!x_map || !y_map)
+		return 0;
+
+	*x1 = *y1 = *x2 = *y2 = 0;
+
+	prev_bit = 0;
+	point = &x_low;
+	for (i = 0; x_map != 0; i++, x_map >>= 1) {
+		bit = x_map & 1;
+		if (bit) {
+			if (!prev_bit) {
+				point->start_bit = i;
+				fingers_x++;
+			}
+			point->num_bits++;
+		} else {
+			if (prev_bit)
+				point = &x_high;
+			else
+				point->num_bits = 0;
+		}
+		prev_bit = bit;
+	}
+
+	/*
+	 * y bitmap is reversed for what we need (lower positions are in
+	 * higher bits), so we process from the top end.
+	 */
+	y_map = y_map << (sizeof(y_map) * BITS_PER_BYTE - ALPS_BITMAP_Y_BITS);
+	prev_bit = 0;
+	point = &y_low;
+	for (i = 0; y_map != 0; i++, y_map <<= 1) {
+		bit = y_map & (1 << (sizeof(y_map) * BITS_PER_BYTE - 1));
+		if (bit) {
+			if (!prev_bit) {
+				point->start_bit = i;
+				fingers_y++;
+			}
+			point->num_bits++;
+		} else {
+			if (prev_bit)
+				point = &y_high;
+			else
+				point->num_bits = 0;
+		}
+		prev_bit = bit;
+	}
+
+	/*
+	 * Fingers can overlap, so we use the maximum count of fingers
+	 * on either axis as the finger count.
+	 */
+	fingers = max(fingers_x, fingers_y);
+
+	/*
+	 * If total fingers is > 1 but either axis reports only a single
+	 * contact, we have overlapping or adjacent fingers. For the
+	 * purposes of creating a bounding box, divide the single contact
+	 * (roughly) equally between the two points.
+	 */
+	if (fingers > 1) {
+		if (fingers_x == 1) {
+			i = x_low.num_bits / 2;
+			x_low.num_bits = x_low.num_bits - i;
+			x_high.start_bit = x_low.start_bit + i;
+			x_high.num_bits = max(i, 1);
+		} else if (fingers_y == 1) {
+			i = y_low.num_bits / 2;
+			y_low.num_bits = y_low.num_bits - i;
+			y_high.start_bit = y_low.start_bit + i;
+			y_high.num_bits = max(i, 1);
+		}
+	}
+
+	*x1 = (ALPS_V3_X_MAX * (2 * x_low.start_bit + x_low.num_bits - 1)) /
+	      (2 * (ALPS_BITMAP_X_BITS - 1));
+	*y1 = (ALPS_V3_Y_MAX * (2 * y_low.start_bit + y_low.num_bits - 1)) /
+	      (2 * (ALPS_BITMAP_Y_BITS - 1));
+
+	if (fingers > 1) {
+		*x2 = (ALPS_V3_X_MAX * (2 * x_high.start_bit + x_high.num_bits - 1)) /
+		      (2 * (ALPS_BITMAP_X_BITS - 1));
+		*y2 = (ALPS_V3_Y_MAX * (2 * y_high.start_bit + y_high.num_bits - 1)) /
+		      (2 * (ALPS_BITMAP_Y_BITS - 1));
+	}
+
+	return fingers;
+}
+
+static void alps_set_slot(struct input_dev *dev, int slot, bool active,
+			  int x, int y)
+{
+	input_mt_slot(dev, slot);
+	input_mt_report_slot_state(dev, MT_TOOL_FINGER, active);
+	if (active) {
+		input_report_abs(dev, ABS_MT_POSITION_X, x);
+		input_report_abs(dev, ABS_MT_POSITION_Y, y);
+	}
+}
+
+static void alps_report_semi_mt_data(struct input_dev *dev, int num_fingers,
+				     int x1, int y1, int x2, int y2)
+{
+	alps_set_slot(dev, 0, num_fingers != 0, x1, y1);
+	alps_set_slot(dev, 1, num_fingers == 2, x2, y2);
+}
+
+static void alps_process_trackstick_packet_v3(struct psmouse *psmouse)
+{
+	struct alps_data *priv = psmouse->private;
+	unsigned char *packet = psmouse->packet;
+	struct input_dev *dev = priv->dev2;
+	int x, y, z, left, right, middle;
+
+	/* Sanity check packet */
+	if (!(packet[0] & 0x40)) {
+		psmouse_dbg(psmouse, "Bad trackstick packet, discarding\n");
+		return;
+	}
+
+	/*
+	 * There's a special packet that seems to indicate the end
+	 * of a stream of trackstick data. Filter these out.
+	 */
+	if (packet[1] == 0x7f && packet[2] == 0x7f && packet[4] == 0x7f)
+		return;
+
+	x = (s8)(((packet[0] & 0x20) << 2) | (packet[1] & 0x7f));
+	y = (s8)(((packet[0] & 0x10) << 3) | (packet[2] & 0x7f));
+	z = (packet[4] & 0x7c) >> 2;
+
+	/*
+	 * The x and y values tend to be quite large, and when used
+	 * alone the trackstick is difficult to use. Scale them down
+	 * to compensate.
+	 */
+	x /= 8;
+	y /= 8;
+
+	input_report_rel(dev, REL_X, x);
+	input_report_rel(dev, REL_Y, -y);
+
+	/*
+	 * Most ALPS models report the trackstick buttons in the touchpad
+	 * packets, but a few report them here. No reliable way has been
+	 * found to differentiate between the models upfront, so we enable
+	 * the quirk in response to seeing a button press in the trackstick
+	 * packet.
+	 */
+	left = packet[3] & 0x01;
+	right = packet[3] & 0x02;
+	middle = packet[3] & 0x04;
+
+	if (!(priv->quirks & ALPS_QUIRK_TRACKSTICK_BUTTONS) &&
+	    (left || right || middle))
+		priv->quirks |= ALPS_QUIRK_TRACKSTICK_BUTTONS;
+
+	if (priv->quirks & ALPS_QUIRK_TRACKSTICK_BUTTONS) {
+		input_report_key(dev, BTN_LEFT, left);
+		input_report_key(dev, BTN_RIGHT, right);
+		input_report_key(dev, BTN_MIDDLE, middle);
+	}
+
+	input_sync(dev);
+	return;
+}
+
+static void alps_process_touchpad_packet_v3(struct psmouse *psmouse)
+{
+	struct alps_data *priv = psmouse->private;
+	unsigned char *packet = psmouse->packet;
+	struct input_dev *dev = psmouse->dev;
+	struct input_dev *dev2 = priv->dev2;
+	int x, y, z;
+	int left, right, middle;
+	int x1 = 0, y1 = 0, x2 = 0, y2 = 0;
+	int fingers = 0, bmap_fingers;
+	unsigned int x_bitmap, y_bitmap;
+
+	/*
+	 * There's no single feature of touchpad position and bitmap packets
+	 * that can be used to distinguish between them. We rely on the fact
+	 * that a bitmap packet should always follow a position packet with
+	 * bit 6 of packet[4] set.
+	 */
+	if (priv->multi_packet) {
+		/*
+		 * Sometimes a position packet will indicate a multi-packet
+		 * sequence, but then what follows is another position
+		 * packet. Check for this, and when it happens process the
+		 * position packet as usual.
+		 */
+		if (packet[0] & 0x40) {
+			fingers = (packet[5] & 0x3) + 1;
+			x_bitmap = ((packet[4] & 0x7e) << 8) |
+				   ((packet[1] & 0x7f) << 2) |
+				   ((packet[0] & 0x30) >> 4);
+			y_bitmap = ((packet[3] & 0x70) << 4) |
+				   ((packet[2] & 0x7f) << 1) |
+				   (packet[4] & 0x01);
+
+			bmap_fingers = alps_process_bitmap(x_bitmap, y_bitmap,
+							   &x1, &y1, &x2, &y2);
+
+			/*
+			 * We shouldn't report more than one finger if
+			 * we don't have two coordinates.
+			 */
+			if (fingers > 1 && bmap_fingers < 2)
+				fingers = bmap_fingers;
+
+			/* Now process position packet */
+			packet = priv->multi_data;
+		} else {
+			priv->multi_packet = 0;
+		}
+	}
+
+	/*
+	 * Bit 6 of byte 0 is not usually set in position packets. The only
+	 * times it seems to be set is in situations where the data is
+	 * suspect anyway, e.g. a palm resting flat on the touchpad. Given
+	 * this combined with the fact that this bit is useful for filtering
+	 * out misidentified bitmap packets, we reject anything with this
+	 * bit set.
+	 */
+	if (packet[0] & 0x40)
+		return;
+
+	if (!priv->multi_packet && (packet[4] & 0x40)) {
+		priv->multi_packet = 1;
+		memcpy(priv->multi_data, packet, sizeof(priv->multi_data));
+		return;
+	}
+
+	priv->multi_packet = 0;
+
+	left = packet[3] & 0x01;
+	right = packet[3] & 0x02;
+	middle = packet[3] & 0x04;
+
+	x = ((packet[1] & 0x7f) << 4) | ((packet[4] & 0x30) >> 2) |
+	    ((packet[0] & 0x30) >> 4);
+	y = ((packet[2] & 0x7f) << 4) | (packet[4] & 0x0f);
+	z = packet[5] & 0x7f;
+
+	/*
+	 * Sometimes the hardware sends a single packet with z = 0
+	 * in the middle of a stream. Real releases generate packets
+	 * with x, y, and z all zero, so these seem to be flukes.
+	 * Ignore them.
+	 */
+	if (x && y && !z)
+		return;
+
+	/*
+	 * If we don't have MT data or the bitmaps were empty, we have
+	 * to rely on ST data.
+	 */
+	if (!fingers) {
+		x1 = x;
+		y1 = y;
+		fingers = z > 0 ? 1 : 0;
+	}
+
+	if (z >= 64)
+		input_report_key(dev, BTN_TOUCH, 1);
+	else
+		input_report_key(dev, BTN_TOUCH, 0);
+
+	alps_report_semi_mt_data(dev, fingers, x1, y1, x2, y2);
+
+	input_report_key(dev, BTN_TOOL_FINGER, fingers == 1);
+	input_report_key(dev, BTN_TOOL_DOUBLETAP, fingers == 2);
+	input_report_key(dev, BTN_TOOL_TRIPLETAP, fingers == 3);
+	input_report_key(dev, BTN_TOOL_QUADTAP, fingers == 4);
+
+	input_report_key(dev, BTN_LEFT, left);
+	input_report_key(dev, BTN_RIGHT, right);
+	input_report_key(dev, BTN_MIDDLE, middle);
+
+	if (z > 0) {
+		input_report_abs(dev, ABS_X, x);
+		input_report_abs(dev, ABS_Y, y);
+	}
+	input_report_abs(dev, ABS_PRESSURE, z);
+
+	input_sync(dev);
+
+	if (!(priv->quirks & ALPS_QUIRK_TRACKSTICK_BUTTONS)) {
+		left = packet[3] & 0x10;
+		right = packet[3] & 0x20;
+		middle = packet[3] & 0x40;
+
+		input_report_key(dev2, BTN_LEFT, left);
+		input_report_key(dev2, BTN_RIGHT, right);
+		input_report_key(dev2, BTN_MIDDLE, middle);
+		input_sync(dev2);
+	}
+}
+
+static void alps_process_packet_v3(struct psmouse *psmouse)
+{
+	unsigned char *packet = psmouse->packet;
+
+	/*
+	 * v3 protocol packets come in three types, two representing
+	 * touchpad data and one representing trackstick data.
+	 * Trackstick packets seem to be distinguished by always
+	 * having 0x3f in the last byte. This value has never been
+	 * observed in the last byte of either of the other types
+	 * of packets.
+	 */
+	if (packet[5] == 0x3f) {
+		alps_process_trackstick_packet_v3(psmouse);
+		return;
+	}
+
+	alps_process_touchpad_packet_v3(psmouse);
+}
+
+static void alps_process_packet_v4(struct psmouse *psmouse)
+{
+	unsigned char *packet = psmouse->packet;
+	struct input_dev *dev = psmouse->dev;
+	int x, y, z;
+	int left, right;
+
+	left = packet[4] & 0x01;
+	right = packet[4] & 0x02;
+
+	x = ((packet[1] & 0x7f) << 4) | ((packet[3] & 0x30) >> 2) |
+	    ((packet[0] & 0x30) >> 4);
+	y = ((packet[2] & 0x7f) << 4) | (packet[3] & 0x0f);
+	z = packet[5] & 0x7f;
+
+	if (z >= 64)
+		input_report_key(dev, BTN_TOUCH, 1);
+	else
+		input_report_key(dev, BTN_TOUCH, 0);
+
+	if (z > 0) {
+		input_report_abs(dev, ABS_X, x);
+		input_report_abs(dev, ABS_Y, y);
+	}
+	input_report_abs(dev, ABS_PRESSURE, z);
+
+	input_report_key(dev, BTN_TOOL_FINGER, z > 0);
+	input_report_key(dev, BTN_LEFT, left);
+	input_report_key(dev, BTN_RIGHT, right);
+
+	input_sync(dev);
+}
+
+static void alps_process_packet(struct psmouse *psmouse)
+{
+	struct alps_data *priv = psmouse->private;
+	const struct alps_model_info *model = priv->i;
+
+	switch (model->proto_version) {
+	case ALPS_PROTO_V1:
+	case ALPS_PROTO_V2:
+		alps_process_packet_v1_v2(psmouse);
+		break;
+	case ALPS_PROTO_V3:
+		alps_process_packet_v3(psmouse);
+		break;
+	case ALPS_PROTO_V4:
+		alps_process_packet_v4(psmouse);
+		break;
+	}
+}
+
 static void alps_report_bare_ps2_packet(struct psmouse *psmouse,
 					unsigned char packet[],
 					bool report_buttons)
@@ -344,7 +759,7 @@ static void alps_flush_packet(unsigned long data)
 
 	serio_pause_rx(psmouse->ps2dev.serio);
 
-	if (psmouse->pktcnt == 6) {
+	if (psmouse->pktcnt == psmouse->pktsize) {
 
 		/*
 		 * We did not any more data in reasonable amount of time.
@@ -395,8 +810,8 @@ static psmouse_ret_t alps_process_byte(struct psmouse *psmouse)
 		return PSMOUSE_BAD_DATA;
 	}
 
-	/* Bytes 2 - 6 should have 0 in the highest bit */
-	if (psmouse->pktcnt >= 2 && psmouse->pktcnt <= 6 &&
+	/* Bytes 2 - pktsize should have 0 in the highest bit */
+	if (psmouse->pktcnt >= 2 && psmouse->pktcnt <= psmouse->pktsize &&
 	    (psmouse->packet[psmouse->pktcnt - 1] & 0x80)) {
 		psmouse_dbg(psmouse, "refusing packet[%i] = %x\n",
 			    psmouse->pktcnt - 1,
@@ -404,7 +819,7 @@ static psmouse_ret_t alps_process_byte(struct psmouse *psmouse)
 		return PSMOUSE_BAD_DATA;
 	}
 
-	if (psmouse->pktcnt == 6) {
+	if (psmouse->pktcnt == psmouse->pktsize) {
 		alps_process_packet(psmouse);
 		return PSMOUSE_FULL_PACKET;
 	}
@@ -412,11 +827,127 @@ static psmouse_ret_t alps_process_byte(struct psmouse *psmouse)
 	return PSMOUSE_GOOD_DATA;
 }
 
+static int alps_command_mode_send_nibble(struct psmouse *psmouse, int nibble)
+{
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+	struct alps_data *priv = psmouse->private;
+	int command;
+	unsigned char *param;
+	unsigned char dummy[4];
+
+	BUG_ON(nibble > 0xf);
+
+	command = priv->nibble_commands[nibble].command;
+	param = (command & 0x0f00) ?
+		dummy : (unsigned char *)&priv->nibble_commands[nibble].data;
+
+	if (ps2_command(ps2dev, param, command))
+		return -1;
+
+	return 0;
+}
+
+static int alps_command_mode_set_addr(struct psmouse *psmouse, int addr)
+{
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+	struct alps_data *priv = psmouse->private;
+	int i, nibble;
+
+	if (ps2_command(ps2dev, NULL, priv->addr_command))
+		return -1;
+
+	for (i = 12; i >= 0; i -= 4) {
+		nibble = (addr >> i) & 0xf;
+		if (alps_command_mode_send_nibble(psmouse, nibble))
+			return -1;
+	}
+
+	return 0;
+}
+
+static int __alps_command_mode_read_reg(struct psmouse *psmouse, int addr)
+{
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+	unsigned char param[4];
+
+	if (ps2_command(ps2dev, param, PSMOUSE_CMD_GETINFO))
+		return -1;
+
+	/*
+	 * The address being read is returned in the first two bytes
+	 * of the result. Check that this address matches the expected
+	 * address.
+	 */
+	if (addr != ((param[0] << 8) | param[1]))
+		return -1;
+
+	return param[2];
+}
+
+static int alps_command_mode_read_reg(struct psmouse *psmouse, int addr)
+{
+	if (alps_command_mode_set_addr(psmouse, addr))
+		return -1;
+	return __alps_command_mode_read_reg(psmouse, addr);
+}
+
+static int __alps_command_mode_write_reg(struct psmouse *psmouse, u8 value)
+{
+	if (alps_command_mode_send_nibble(psmouse, (value >> 4) & 0xf))
+		return -1;
+	if (alps_command_mode_send_nibble(psmouse, value & 0xf))
+		return -1;
+	return 0;
+}
+
+static int alps_command_mode_write_reg(struct psmouse *psmouse, int addr,
+				       u8 value)
+{
+	if (alps_command_mode_set_addr(psmouse, addr))
+		return -1;
+	return __alps_command_mode_write_reg(psmouse, value);
+}
+
+static int alps_enter_command_mode(struct psmouse *psmouse,
+				   unsigned char *resp)
+{
+	unsigned char param[4];
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+
+	if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_RESET_WRAP) ||
+	    ps2_command(ps2dev, NULL, PSMOUSE_CMD_RESET_WRAP) ||
+	    ps2_command(ps2dev, NULL, PSMOUSE_CMD_RESET_WRAP) ||
+	    ps2_command(ps2dev, param, PSMOUSE_CMD_GETINFO)) {
+		psmouse_err(psmouse, "failed to enter command mode\n");
+		return -1;
+	}
+
+	if (param[0] != 0x88 && param[1] != 0x07) {
+		psmouse_dbg(psmouse,
+			    "unknown response while entering command mode: %2.2x %2.2x %2.2x\n",
+			    param[0], param[1], param[2]);
+		return -1;
+	}
+
+	if (resp)
+		*resp = param[2];
+	return 0;
+}
+
+static inline int alps_exit_command_mode(struct psmouse *psmouse)
+{
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+	if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSTREAM))
+		return -1;
+	return 0;
+}
+
 static const struct alps_model_info *alps_get_model(struct psmouse *psmouse, int *version)
 {
 	struct ps2dev *ps2dev = &psmouse->ps2dev;
 	static const unsigned char rates[] = { 0, 10, 20, 40, 60, 80, 100, 200 };
 	unsigned char param[4];
+	const struct alps_model_info *model = NULL;
 	int i;
 
 	/*
@@ -464,12 +995,41 @@ static const struct alps_model_info *alps_get_model(struct psmouse *psmouse, int
 		*version = (param[0] << 8) | (param[1] << 4) | i;
 	}
 
-	for (i = 0; i < ARRAY_SIZE(alps_model_data); i++)
+	for (i = 0; i < ARRAY_SIZE(alps_model_data); i++) {
 		if (!memcmp(param, alps_model_data[i].signature,
-			    sizeof(alps_model_data[i].signature)))
-			return alps_model_data + i;
+			    sizeof(alps_model_data[i].signature))) {
+			model = alps_model_data + i;
+			break;
+		}
+	}
 
-	return NULL;
+	if (model && model->proto_version > ALPS_PROTO_V2) {
+		/*
+		 * Need to check command mode response to identify
+		 * model
+		 */
+		model = NULL;
+		if (alps_enter_command_mode(psmouse, param)) {
+			psmouse_warn(psmouse,
+				     "touchpad failed to enter command mode\n");
+		} else {
+			for (i = 0; i < ARRAY_SIZE(alps_model_data); i++) {
+				if (alps_model_data[i].proto_version > ALPS_PROTO_V2 &&
+				    alps_model_data[i].command_mode_resp == param[0]) {
+					model = alps_model_data + i;
+					break;
+				}
+			}
+			alps_exit_command_mode(psmouse);
+
+			if (!model)
+				psmouse_dbg(psmouse,
+					    "Unknown command mode response %2.2x\n",
+					    param[0]);
+		}
+	}
+
+	return model;
 }
 
 /*
@@ -477,7 +1037,7 @@ static const struct alps_model_info *alps_get_model(struct psmouse *psmouse, int
  * subsequent commands. It looks like glidepad is behind stickpointer,
  * I'd thought it would be other way around...
  */
-static int alps_passthrough_mode(struct psmouse *psmouse, bool enable)
+static int alps_passthrough_mode_v2(struct psmouse *psmouse, bool enable)
 {
 	struct ps2dev *ps2dev = &psmouse->ps2dev;
 	int cmd = enable ? PSMOUSE_CMD_SETSCALE21 : PSMOUSE_CMD_SETSCALE11;
@@ -494,7 +1054,7 @@ static int alps_passthrough_mode(struct psmouse *psmouse, bool enable)
 	return 0;
 }
 
-static int alps_absolute_mode(struct psmouse *psmouse)
+static int alps_absolute_mode_v1_v2(struct psmouse *psmouse)
 {
 	struct ps2dev *ps2dev = &psmouse->ps2dev;
 
@@ -565,17 +1125,17 @@ static int alps_tap_mode(struct psmouse *psmouse, int enable)
 static int alps_poll(struct psmouse *psmouse)
 {
 	struct alps_data *priv = psmouse->private;
-	unsigned char buf[6];
+	unsigned char buf[sizeof(psmouse->packet)];
 	bool poll_failed;
 
 	if (priv->i->flags & ALPS_PASS)
-		alps_passthrough_mode(psmouse, true);
+		alps_passthrough_mode_v2(psmouse, true);
 
 	poll_failed = ps2_command(&psmouse->ps2dev, buf,
 				  PSMOUSE_CMD_POLL | (psmouse->pktsize << 8)) < 0;
 
 	if (priv->i->flags & ALPS_PASS)
-		alps_passthrough_mode(psmouse, false);
+		alps_passthrough_mode_v2(psmouse, false);
 
 	if (poll_failed || (buf[0] & priv->i->mask0) != priv->i->byte0)
 		return -1;
@@ -592,13 +1152,13 @@ static int alps_poll(struct psmouse *psmouse)
 	return 0;
 }
 
-static int alps_hw_init(struct psmouse *psmouse)
+static int alps_hw_init_v1_v2(struct psmouse *psmouse)
 {
 	struct alps_data *priv = psmouse->private;
 	const struct alps_model_info *model = priv->i;
 
 	if ((model->flags & ALPS_PASS) &&
-	    alps_passthrough_mode(psmouse, true)) {
+	    alps_passthrough_mode_v2(psmouse, true)) {
 		return -1;
 	}
 
@@ -607,13 +1167,13 @@ static int alps_hw_init(struct psmouse *psmouse)
 		return -1;
 	}
 
-	if (alps_absolute_mode(psmouse)) {
+	if (alps_absolute_mode_v1_v2(psmouse)) {
 		psmouse_err(psmouse, "Failed to enable absolute mode\n");
 		return -1;
 	}
 
 	if ((model->flags & ALPS_PASS) &&
-	    alps_passthrough_mode(psmouse, false)) {
+	    alps_passthrough_mode_v2(psmouse, false)) {
 		return -1;
 	}
 
@@ -626,6 +1186,297 @@ static int alps_hw_init(struct psmouse *psmouse)
 	return 0;
 }
 
+/*
+ * Enable or disable passthrough mode to the trackstick. Must be in
+ * command mode when calling this function.
+ */
+static int alps_passthrough_mode_v3(struct psmouse *psmouse, bool enable)
+{
+	int reg_val;
+
+	reg_val = alps_command_mode_read_reg(psmouse, 0x0008);
+	if (reg_val == -1)
+		return -1;
+
+	if (enable)
+		reg_val |= 0x01;
+	else
+		reg_val &= ~0x01;
+
+	if (__alps_command_mode_write_reg(psmouse, reg_val))
+		return -1;
+
+	return 0;
+}
+
+/* Must be in command mode when calling this function */
+static int alps_absolute_mode_v3(struct psmouse *psmouse)
+{
+	int reg_val;
+
+	reg_val = alps_command_mode_read_reg(psmouse, 0x0004);
+	if (reg_val == -1)
+		return -1;
+
+	reg_val |= 0x06;
+	if (__alps_command_mode_write_reg(psmouse, reg_val))
+		return -1;
+
+	return 0;
+}
+
+static int alps_hw_init_v3(struct psmouse *psmouse)
+{
+	struct alps_data *priv = psmouse->private;
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+	int reg_val;
+	unsigned char param[4];
+
+	priv->nibble_commands = alps_v3_nibble_commands;
+	priv->addr_command = PSMOUSE_CMD_RESET_WRAP;
+
+	if (alps_enter_command_mode(psmouse, NULL))
+		goto error;
+
+	/* Check for trackstick */
+	reg_val = alps_command_mode_read_reg(psmouse, 0x0008);
+	if (reg_val == -1)
+		goto error;
+	if (reg_val & 0x80) {
+		if (alps_passthrough_mode_v3(psmouse, true))
+			goto error;
+		if (alps_exit_command_mode(psmouse))
+			goto error;
+
+		/*
+		 * E7 report for the trackstick
+		 *
+		 * There have been reports of failures to seem to trace back
+		 * to the above trackstick check failing. When these occur
+		 * this E7 report fails, so when that happens we continue
+		 * with the assumption that there isn't a trackstick after
+		 * all.
+		 */
+		param[0] = 0x64;
+		if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE21) ||
+		    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE21) ||
+		    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE21) ||
+		    ps2_command(ps2dev, param, PSMOUSE_CMD_GETINFO)) {
+			psmouse_warn(psmouse, "trackstick E7 report failed\n");
+		} else {
+			psmouse_dbg(psmouse,
+				    "trackstick E7 report: %2.2x %2.2x %2.2x\n",
+				    param[0], param[1], param[2]);
+
+			/*
+			 * Not sure what this does, but it is absolutely
+			 * essential. Without it, the touchpad does not
+			 * work at all and the trackstick just emits normal
+			 * PS/2 packets.
+			 */
+			if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
+			    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
+			    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
+			    alps_command_mode_send_nibble(psmouse, 0x9) ||
+			    alps_command_mode_send_nibble(psmouse, 0x4)) {
+				psmouse_err(psmouse,
+					    "Error sending magic E6 sequence\n");
+				goto error_passthrough;
+			}
+		}
+
+		if (alps_enter_command_mode(psmouse, NULL))
+			goto error_passthrough;
+		if (alps_passthrough_mode_v3(psmouse, false))
+			goto error;
+	}
+
+	if (alps_absolute_mode_v3(psmouse)) {
+		psmouse_err(psmouse, "Failed to enter absolute mode\n");
+		goto error;
+	}
+
+	reg_val = alps_command_mode_read_reg(psmouse, 0x0006);
+	if (reg_val == -1)
+		goto error;
+	if (__alps_command_mode_write_reg(psmouse, reg_val | 0x01))
+		goto error;
+
+	reg_val = alps_command_mode_read_reg(psmouse, 0x0007);
+	if (reg_val == -1)
+		goto error;
+	if (__alps_command_mode_write_reg(psmouse, reg_val | 0x01))
+		goto error;
+
+	if (alps_command_mode_read_reg(psmouse, 0x0144) == -1)
+		goto error;
+	if (__alps_command_mode_write_reg(psmouse, 0x04))
+		goto error;
+
+	if (alps_command_mode_read_reg(psmouse, 0x0159) == -1)
+		goto error;
+	if (__alps_command_mode_write_reg(psmouse, 0x03))
+		goto error;
+
+	if (alps_command_mode_read_reg(psmouse, 0x0163) == -1)
+		goto error;
+	if (alps_command_mode_write_reg(psmouse, 0x0163, 0x03))
+		goto error;
+
+	if (alps_command_mode_read_reg(psmouse, 0x0162) == -1)
+		goto error;
+	if (alps_command_mode_write_reg(psmouse, 0x0162, 0x04))
+		goto error;
+
+	/*
+	 * This ensures the trackstick packets are in the format
+	 * supported by this driver. If bit 1 isn't set the packet
+	 * format is different.
+	 */
+	if (alps_command_mode_write_reg(psmouse, 0x0008, 0x82))
+		goto error;
+
+	alps_exit_command_mode(psmouse);
+
+	/* Set rate and enable data reporting */
+	param[0] = 0x64;
+	if (ps2_command(ps2dev, param, PSMOUSE_CMD_SETRATE) ||
+	    ps2_command(ps2dev, NULL, PSMOUSE_CMD_ENABLE)) {
+		psmouse_err(psmouse, "Failed to enable data reporting\n");
+		return -1;
+	}
+
+	return 0;
+
+error_passthrough:
+	/* Something failed while in passthrough mode, so try to get out */
+	if (!alps_enter_command_mode(psmouse, NULL))
+		alps_passthrough_mode_v3(psmouse, false);
+error:
+	/*
+	 * Leaving the touchpad in command mode will essentially render
+	 * it unusable until the machine reboots, so exit it here just
+	 * to be safe
+	 */
+	alps_exit_command_mode(psmouse);
+	return -1;
+}
+
+/* Must be in command mode when calling this function */
+static int alps_absolute_mode_v4(struct psmouse *psmouse)
+{
+	int reg_val;
+
+	reg_val = alps_command_mode_read_reg(psmouse, 0x0004);
+	if (reg_val == -1)
+		return -1;
+
+	reg_val |= 0x02;
+	if (__alps_command_mode_write_reg(psmouse, reg_val))
+		return -1;
+
+	return 0;
+}
+
+static int alps_hw_init_v4(struct psmouse *psmouse)
+{
+	struct alps_data *priv = psmouse->private;
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+	unsigned char param[4];
+
+	priv->nibble_commands = alps_v4_nibble_commands;
+	priv->addr_command = PSMOUSE_CMD_DISABLE;
+
+	if (alps_enter_command_mode(psmouse, NULL))
+		goto error;
+
+	if (alps_absolute_mode_v4(psmouse)) {
+		psmouse_err(psmouse, "Failed to enter absolute mode\n");
+		goto error;
+	}
+
+	if (alps_command_mode_write_reg(psmouse, 0x0007, 0x8c))
+		goto error;
+
+	if (alps_command_mode_write_reg(psmouse, 0x0149, 0x03))
+		goto error;
+
+	if (alps_command_mode_write_reg(psmouse, 0x0160, 0x03))
+		goto error;
+
+	if (alps_command_mode_write_reg(psmouse, 0x017f, 0x15))
+		goto error;
+
+	if (alps_command_mode_write_reg(psmouse, 0x0151, 0x01))
+		goto error;
+
+	if (alps_command_mode_write_reg(psmouse, 0x0168, 0x03))
+		goto error;
+
+	if (alps_command_mode_write_reg(psmouse, 0x014a, 0x03))
+		goto error;
+
+	if (alps_command_mode_write_reg(psmouse, 0x0161, 0x03))
+		goto error;
+
+	alps_exit_command_mode(psmouse);
+
+	/*
+	 * This sequence changes the output from a 9-byte to an
+	 * 8-byte format. All the same data seems to be present,
+	 * just in a more compact format.
+	 */
+	param[0] = 0xc8;
+	param[1] = 0x64;
+	param[2] = 0x50;
+	if (ps2_command(ps2dev, &param[0], PSMOUSE_CMD_SETRATE) ||
+	    ps2_command(ps2dev, &param[1], PSMOUSE_CMD_SETRATE) ||
+	    ps2_command(ps2dev, &param[2], PSMOUSE_CMD_SETRATE) ||
+	    ps2_command(ps2dev, param, PSMOUSE_CMD_GETID))
+		return -1;
+
+	/* Set rate and enable data reporting */
+	param[0] = 0x64;
+	if (ps2_command(ps2dev, param, PSMOUSE_CMD_SETRATE) ||
+	    ps2_command(ps2dev, NULL, PSMOUSE_CMD_ENABLE)) {
+		psmouse_err(psmouse, "Failed to enable data reporting\n");
+		return -1;
+	}
+
+	return 0;
+
+error:
+	/*
+	 * Leaving the touchpad in command mode will essentially render
+	 * it unusable until the machine reboots, so exit it here just
+	 * to be safe
+	 */
+	alps_exit_command_mode(psmouse);
+	return -1;
+}
+
+static int alps_hw_init(struct psmouse *psmouse)
+{
+	struct alps_data *priv = psmouse->private;
+	const struct alps_model_info *model = priv->i;
+	int ret = -1;
+
+	switch (model->proto_version) {
+	case ALPS_PROTO_V1:
+	case ALPS_PROTO_V2:
+		ret = alps_hw_init_v1_v2(psmouse);
+		break;
+	case ALPS_PROTO_V3:
+		ret = alps_hw_init_v3(psmouse);
+		break;
+	case ALPS_PROTO_V4:
+		ret = alps_hw_init_v4(psmouse);
+		break;
+	}
+
+	return ret;
+}
+
 static int alps_reconnect(struct psmouse *psmouse)
 {
 	const struct alps_model_info *model;
@@ -666,6 +1517,8 @@ int alps_init(struct psmouse *psmouse)
 
 	psmouse->private = priv;
 
+	psmouse_reset(psmouse);
+
 	model = alps_get_model(psmouse, &version);
 	if (!model)
 		goto init_fail;
@@ -693,8 +1546,29 @@ int alps_init(struct psmouse *psmouse)
 		BIT_MASK(BTN_LEFT) | BIT_MASK(BTN_RIGHT);
 
 	dev1->evbit[BIT_WORD(EV_ABS)] |= BIT_MASK(EV_ABS);
-	input_set_abs_params(dev1, ABS_X, 0, 1023, 0, 0);
-	input_set_abs_params(dev1, ABS_Y, 0, 767, 0, 0);
+
+	switch (model->proto_version) {
+	case ALPS_PROTO_V1:
+	case ALPS_PROTO_V2:
+		input_set_abs_params(dev1, ABS_X, 0, 1023, 0, 0);
+		input_set_abs_params(dev1, ABS_Y, 0, 767, 0, 0);
+		break;
+	case ALPS_PROTO_V3:
+		set_bit(INPUT_PROP_SEMI_MT, dev1->propbit);
+		input_mt_init_slots(dev1, 2);
+		input_set_abs_params(dev1, ABS_MT_POSITION_X, 0, ALPS_V3_X_MAX, 0, 0);
+		input_set_abs_params(dev1, ABS_MT_POSITION_Y, 0, ALPS_V3_Y_MAX, 0, 0);
+
+		set_bit(BTN_TOOL_DOUBLETAP, dev1->keybit);
+		set_bit(BTN_TOOL_TRIPLETAP, dev1->keybit);
+		set_bit(BTN_TOOL_QUADTAP, dev1->keybit);
+		/* fall through */
+	case ALPS_PROTO_V4:
+		input_set_abs_params(dev1, ABS_X, 0, ALPS_V3_X_MAX, 0, 0);
+		input_set_abs_params(dev1, ABS_Y, 0, ALPS_V3_Y_MAX, 0, 0);
+		break;
+	}
+
 	input_set_abs_params(dev1, ABS_PRESSURE, 0, 127, 0, 0);
 
 	if (model->flags & ALPS_WHEEL) {
@@ -737,7 +1611,7 @@ int alps_init(struct psmouse *psmouse)
 	psmouse->poll = alps_poll;
 	psmouse->disconnect = alps_disconnect;
 	psmouse->reconnect = alps_reconnect;
-	psmouse->pktsize = 6;
+	psmouse->pktsize = model->proto_version == ALPS_PROTO_V4 ? 8 : 6;
 
 	/* We are having trouble resyncing ALPS touchpads so disable it for now */
 	psmouse->resync_time = 0;
diff --git a/drivers/input/mouse/alps.h b/drivers/input/mouse/alps.h
index 904ed8b3c8be..a00a4ab92a0f 100644
--- a/drivers/input/mouse/alps.h
+++ b/drivers/input/mouse/alps.h
@@ -12,20 +12,39 @@
 #ifndef _ALPS_H
 #define _ALPS_H
 
+#define ALPS_PROTO_V1	0
+#define ALPS_PROTO_V2	1
+#define ALPS_PROTO_V3	2
+#define ALPS_PROTO_V4	3
+
 struct alps_model_info {
         unsigned char signature[3];
+	unsigned char command_mode_resp; /* v3/v4 only */
+	unsigned char proto_version;
         unsigned char byte0, mask0;
         unsigned char flags;
 };
 
+struct alps_nibble_commands {
+	int command;
+	unsigned char data;
+};
+
 struct alps_data {
 	struct input_dev *dev2;		/* Relative device */
 	char phys[32];			/* Phys */
 	const struct alps_model_info *i;/* Info */
+	const struct alps_nibble_commands *nibble_commands;
+	int addr_command;		/* Command to set register address */
 	int prev_fin;			/* Finger bit from previous packet */
+	int multi_packet;		/* Multi-packet data in progress */
+	unsigned char multi_data[6];	/* Saved multi-packet data */
+	u8 quirks;
 	struct timer_list timer;
 };
 
+#define ALPS_QUIRK_TRACKSTICK_BUTTONS	1 /* trakcstick buttons in trackstick packet */
+
 #ifdef CONFIG_MOUSE_PS2_ALPS
 int alps_detect(struct psmouse *psmouse, bool set_properties);
 int alps_init(struct psmouse *psmouse);
diff --git a/drivers/input/mouse/amimouse.c b/drivers/input/mouse/amimouse.c
index ff5f61a0fd3a..39be7b82c046 100644
--- a/drivers/input/mouse/amimouse.c
+++ b/drivers/input/mouse/amimouse.c
@@ -140,25 +140,13 @@ static int __exit amimouse_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver amimouse_driver = {
+	.probe = amimouse_probe,
 	.remove = __exit_p(amimouse_remove),
 	.driver   = {
 		.name	= "amiga-mouse",
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init amimouse_init(void)
-{
-	return platform_driver_probe(&amimouse_driver, amimouse_probe);
-}
-
-module_init(amimouse_init);
-
-static void __exit amimouse_exit(void)
-{
-	platform_driver_unregister(&amimouse_driver);
-}
-
-module_exit(amimouse_exit);
+module_platform_driver(amimouse_driver);
 
 MODULE_ALIAS("platform:amiga-mouse");
diff --git a/drivers/input/mouse/elantech.c b/drivers/input/mouse/elantech.c
index e2a9867c19d5..d2c0db159b18 100644
--- a/drivers/input/mouse/elantech.c
+++ b/drivers/input/mouse/elantech.c
@@ -43,6 +43,24 @@ static int synaptics_send_cmd(struct psmouse *psmouse, unsigned char c,
 }
 
 /*
+ * V3 and later support this fast command
+ */
+static int elantech_send_cmd(struct psmouse *psmouse, unsigned char c,
+				unsigned char *param)
+{
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
+
+	if (ps2_command(ps2dev, NULL, ETP_PS2_CUSTOM_COMMAND) ||
+	    ps2_command(ps2dev, NULL, c) ||
+	    ps2_command(ps2dev, param, PSMOUSE_CMD_GETINFO)) {
+		psmouse_err(psmouse, "%s query 0x%02x failed.\n", __func__, c);
+		return -1;
+	}
+
+	return 0;
+}
+
+/*
  * A retrying version of ps2_command
  */
 static int elantech_ps2_command(struct psmouse *psmouse,
@@ -863,13 +881,13 @@ static int elantech_set_range(struct psmouse *psmouse,
 			i = (etd->fw_version > 0x020800 &&
 			     etd->fw_version < 0x020900) ? 1 : 2;
 
-			if (synaptics_send_cmd(psmouse, ETP_FW_ID_QUERY, param))
+			if (etd->send_cmd(psmouse, ETP_FW_ID_QUERY, param))
 				return -1;
 
 			fixed_dpi = param[1] & 0x10;
 
 			if (((etd->fw_version >> 16) == 0x14) && fixed_dpi) {
-				if (synaptics_send_cmd(psmouse, ETP_SAMPLE_QUERY, param))
+				if (etd->send_cmd(psmouse, ETP_SAMPLE_QUERY, param))
 					return -1;
 
 				*x_max = (etd->capabilities[1] - i) * param[1] / 2;
@@ -888,7 +906,7 @@ static int elantech_set_range(struct psmouse *psmouse,
 		break;
 
 	case 3:
-		if (synaptics_send_cmd(psmouse, ETP_FW_ID_QUERY, param))
+		if (etd->send_cmd(psmouse, ETP_FW_ID_QUERY, param))
 			return -1;
 
 		*x_max = (0x0f & param[0]) << 8 | param[1];
@@ -896,7 +914,7 @@ static int elantech_set_range(struct psmouse *psmouse,
 		break;
 
 	case 4:
-		if (synaptics_send_cmd(psmouse, ETP_FW_ID_QUERY, param))
+		if (etd->send_cmd(psmouse, ETP_FW_ID_QUERY, param))
 			return -1;
 
 		*x_max = (0x0f & param[0]) << 8 | param[1];
@@ -913,6 +931,30 @@ static int elantech_set_range(struct psmouse *psmouse,
 }
 
 /*
+ * (value from firmware) * 10 + 790 = dpi
+ * we also have to convert dpi to dots/mm (*10/254 to avoid floating point)
+ */
+static unsigned int elantech_convert_res(unsigned int val)
+{
+	return (val * 10 + 790) * 10 / 254;
+}
+
+static int elantech_get_resolution_v4(struct psmouse *psmouse,
+				      unsigned int *x_res,
+				      unsigned int *y_res)
+{
+	unsigned char param[3];
+
+	if (elantech_send_cmd(psmouse, ETP_RESOLUTION_QUERY, param))
+		return -1;
+
+	*x_res = elantech_convert_res(param[1] & 0x0f);
+	*y_res = elantech_convert_res((param[1] & 0xf0) >> 4);
+
+	return 0;
+}
+
+/*
  * Set the appropriate event bits for the input subsystem
  */
 static int elantech_set_input_params(struct psmouse *psmouse)
@@ -920,6 +962,7 @@ static int elantech_set_input_params(struct psmouse *psmouse)
 	struct input_dev *dev = psmouse->dev;
 	struct elantech_data *etd = psmouse->private;
 	unsigned int x_min = 0, y_min = 0, x_max = 0, y_max = 0, width = 0;
+	unsigned int x_res = 0, y_res = 0;
 
 	if (elantech_set_range(psmouse, &x_min, &y_min, &x_max, &y_max, &width))
 		return -1;
@@ -967,10 +1010,20 @@ static int elantech_set_input_params(struct psmouse *psmouse)
 		break;
 
 	case 4:
+		if (elantech_get_resolution_v4(psmouse, &x_res, &y_res)) {
+			/*
+			 * if query failed, print a warning and leave the values
+			 * zero to resemble synaptics.c behavior.
+			 */
+			psmouse_warn(psmouse, "couldn't query resolution data.\n");
+		}
+
 		__set_bit(BTN_TOOL_QUADTAP, dev->keybit);
 		/* For X to recognize me as touchpad. */
 		input_set_abs_params(dev, ABS_X, x_min, x_max, 0, 0);
 		input_set_abs_params(dev, ABS_Y, y_min, y_max, 0, 0);
+		input_abs_set_res(dev, ABS_X, x_res);
+		input_abs_set_res(dev, ABS_Y, y_res);
 		/*
 		 * range of pressure and width is the same as v2,
 		 * report ABS_PRESSURE, ABS_TOOL_WIDTH for compatibility.
@@ -983,6 +1036,8 @@ static int elantech_set_input_params(struct psmouse *psmouse)
 		input_mt_init_slots(dev, ETP_MAX_FINGERS);
 		input_set_abs_params(dev, ABS_MT_POSITION_X, x_min, x_max, 0, 0);
 		input_set_abs_params(dev, ABS_MT_POSITION_Y, y_min, y_max, 0, 0);
+		input_abs_set_res(dev, ABS_MT_POSITION_X, x_res);
+		input_abs_set_res(dev, ABS_MT_POSITION_Y, y_res);
 		input_set_abs_params(dev, ABS_MT_PRESSURE, ETP_PMIN_V2,
 				     ETP_PMAX_V2, 0, 0);
 		/*
@@ -1031,16 +1086,13 @@ static ssize_t elantech_set_int_attr(struct psmouse *psmouse,
 	struct elantech_data *etd = psmouse->private;
 	struct elantech_attr_data *attr = data;
 	unsigned char *reg = (unsigned char *) etd + attr->field_offset;
-	unsigned long value;
+	unsigned char value;
 	int err;
 
-	err = strict_strtoul(buf, 16, &value);
+	err = kstrtou8(buf, 16, &value);
 	if (err)
 		return err;
 
-	if (value > 0xff)
-		return -EINVAL;
-
 	/* Do we need to preserve some bits for version 2 hardware too? */
 	if (etd->hw_version == 1) {
 		if (attr->reg == 0x10)
@@ -1233,9 +1285,11 @@ static int elantech_set_properties(struct elantech_data *etd)
 		}
 	}
 
-	/*
-	 * Turn on packet checking by default.
-	 */
+	/* decide which send_cmd we're gonna use early */
+	etd->send_cmd = etd->hw_version >= 3 ? elantech_send_cmd :
+					       synaptics_send_cmd;
+
+	/* Turn on packet checking by default */
 	etd->paritycheck = 1;
 
 	/*
@@ -1291,7 +1345,7 @@ int elantech_init(struct psmouse *psmouse)
 		     "assuming hardware version %d (with firmware version 0x%02x%02x%02x)\n",
 		     etd->hw_version, param[0], param[1], param[2]);
 
-	if (synaptics_send_cmd(psmouse, ETP_CAPABILITIES_QUERY,
+	if (etd->send_cmd(psmouse, ETP_CAPABILITIES_QUERY,
 	    etd->capabilities)) {
 		psmouse_err(psmouse, "failed to query capabilities.\n");
 		goto init_fail;
diff --git a/drivers/input/mouse/elantech.h b/drivers/input/mouse/elantech.h
index 9e5f1aabea7e..46db3be45ac9 100644
--- a/drivers/input/mouse/elantech.h
+++ b/drivers/input/mouse/elantech.h
@@ -20,6 +20,7 @@
 #define ETP_FW_VERSION_QUERY		0x01
 #define ETP_CAPABILITIES_QUERY		0x02
 #define ETP_SAMPLE_QUERY		0x03
+#define ETP_RESOLUTION_QUERY		0x04
 
 /*
  * Command values for register reading or writing
@@ -135,6 +136,7 @@ struct elantech_data {
 	unsigned int width;
 	struct finger_pos mt[ETP_MAX_FINGERS];
 	unsigned char parity[256];
+	int (*send_cmd)(struct psmouse *psmouse, unsigned char c, unsigned char *param);
 };
 
 #ifdef CONFIG_MOUSE_PS2_ELANTECH
diff --git a/drivers/input/mouse/gpio_mouse.c b/drivers/input/mouse/gpio_mouse.c
index 58902fbb9896..a9ad8e1402be 100644
--- a/drivers/input/mouse/gpio_mouse.c
+++ b/drivers/input/mouse/gpio_mouse.c
@@ -178,18 +178,7 @@ static struct platform_driver gpio_mouse_device_driver = {
 		.owner	= THIS_MODULE,
 	}
 };
-
-static int __init gpio_mouse_init(void)
-{
-	return platform_driver_register(&gpio_mouse_device_driver);
-}
-module_init(gpio_mouse_init);
-
-static void __exit gpio_mouse_exit(void)
-{
-	platform_driver_unregister(&gpio_mouse_device_driver);
-}
-module_exit(gpio_mouse_exit);
+module_platform_driver(gpio_mouse_device_driver);
 
 MODULE_AUTHOR("Hans-Christian Egtvedt <egtvedt@samfundet.no>");
 MODULE_DESCRIPTION("GPIO mouse driver");
diff --git a/drivers/input/mouse/hgpk.c b/drivers/input/mouse/hgpk.c
index 0470dd46b566..1c5d521de600 100644
--- a/drivers/input/mouse/hgpk.c
+++ b/drivers/input/mouse/hgpk.c
@@ -789,11 +789,14 @@ static ssize_t hgpk_set_powered(struct psmouse *psmouse, void *data,
 				const char *buf, size_t count)
 {
 	struct hgpk_data *priv = psmouse->private;
-	unsigned long value;
+	unsigned int value;
 	int err;
 
-	err = strict_strtoul(buf, 10, &value);
-	if (err || value > 1)
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value > 1)
 		return -EINVAL;
 
 	if (value != priv->powered) {
@@ -881,11 +884,14 @@ static ssize_t hgpk_trigger_recal(struct psmouse *psmouse, void *data,
 				const char *buf, size_t count)
 {
 	struct hgpk_data *priv = psmouse->private;
-	unsigned long value;
+	unsigned int value;
 	int err;
 
-	err = strict_strtoul(buf, 10, &value);
-	if (err || value != 1)
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value != 1)
 		return -EINVAL;
 
 	/*
diff --git a/drivers/input/mouse/logips2pp.c b/drivers/input/mouse/logips2pp.c
index faac2c3bef74..84de2fc6acc1 100644
--- a/drivers/input/mouse/logips2pp.c
+++ b/drivers/input/mouse/logips2pp.c
@@ -155,9 +155,14 @@ static ssize_t ps2pp_attr_show_smartscroll(struct psmouse *psmouse,
 static ssize_t ps2pp_attr_set_smartscroll(struct psmouse *psmouse, void *data,
 					  const char *buf, size_t count)
 {
-	unsigned long value;
+	unsigned int value;
+	int err;
 
-	if (strict_strtoul(buf, 10, &value) || value > 1)
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value > 1)
 		return -EINVAL;
 
 	ps2pp_set_smartscroll(psmouse, value);
diff --git a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
index 9f352fbd7b4f..de7e8bc17b1f 100644
--- a/drivers/input/mouse/psmouse-base.c
+++ b/drivers/input/mouse/psmouse-base.c
@@ -127,7 +127,7 @@ struct psmouse_protocol {
  * relevant events to the input module once full packet has arrived.
  */
 
-static psmouse_ret_t psmouse_process_byte(struct psmouse *psmouse)
+psmouse_ret_t psmouse_process_byte(struct psmouse *psmouse)
 {
 	struct input_dev *dev = psmouse->dev;
 	unsigned char *packet = psmouse->packet;
@@ -418,6 +418,49 @@ int psmouse_reset(struct psmouse *psmouse)
 	return 0;
 }
 
+/*
+ * Here we set the mouse resolution.
+ */
+
+void psmouse_set_resolution(struct psmouse *psmouse, unsigned int resolution)
+{
+	static const unsigned char params[] = { 0, 1, 2, 2, 3 };
+	unsigned char p;
+
+	if (resolution == 0 || resolution > 200)
+		resolution = 200;
+
+	p = params[resolution / 50];
+	ps2_command(&psmouse->ps2dev, &p, PSMOUSE_CMD_SETRES);
+	psmouse->resolution = 25 << p;
+}
+
+/*
+ * Here we set the mouse report rate.
+ */
+
+static void psmouse_set_rate(struct psmouse *psmouse, unsigned int rate)
+{
+	static const unsigned char rates[] = { 200, 100, 80, 60, 40, 20, 10, 0 };
+	unsigned char r;
+	int i = 0;
+
+	while (rates[i] > rate) i++;
+	r = rates[i];
+	ps2_command(&psmouse->ps2dev, &r, PSMOUSE_CMD_SETRATE);
+	psmouse->rate = r;
+}
+
+/*
+ * psmouse_poll() - default poll handler. Everyone except for ALPS uses it.
+ */
+
+static int psmouse_poll(struct psmouse *psmouse)
+{
+	return ps2_command(&psmouse->ps2dev, psmouse->packet,
+			   PSMOUSE_CMD_POLL | (psmouse->pktsize << 8));
+}
+
 
 /*
  * Genius NetMouse magic init.
@@ -603,6 +646,56 @@ static int cortron_detect(struct psmouse *psmouse, bool set_properties)
 }
 
 /*
+ * Apply default settings to the psmouse structure. Most of them will
+ * be overridden by individual protocol initialization routines.
+ */
+
+static void psmouse_apply_defaults(struct psmouse *psmouse)
+{
+	struct input_dev *input_dev = psmouse->dev;
+
+	memset(input_dev->evbit, 0, sizeof(input_dev->evbit));
+	memset(input_dev->keybit, 0, sizeof(input_dev->keybit));
+	memset(input_dev->relbit, 0, sizeof(input_dev->relbit));
+	memset(input_dev->absbit, 0, sizeof(input_dev->absbit));
+	memset(input_dev->mscbit, 0, sizeof(input_dev->mscbit));
+
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(EV_REL, input_dev->evbit);
+
+	__set_bit(BTN_LEFT, input_dev->keybit);
+	__set_bit(BTN_RIGHT, input_dev->keybit);
+
+	__set_bit(REL_X, input_dev->relbit);
+	__set_bit(REL_Y, input_dev->relbit);
+
+	psmouse->set_rate = psmouse_set_rate;
+	psmouse->set_resolution = psmouse_set_resolution;
+	psmouse->poll = psmouse_poll;
+	psmouse->protocol_handler = psmouse_process_byte;
+	psmouse->pktsize = 3;
+	psmouse->reconnect = NULL;
+	psmouse->disconnect = NULL;
+	psmouse->cleanup = NULL;
+	psmouse->pt_activate = NULL;
+	psmouse->pt_deactivate = NULL;
+}
+
+/*
+ * Apply default settings to the psmouse structure and call specified
+ * protocol detection or initialization routine.
+ */
+static int psmouse_do_detect(int (*detect)(struct psmouse *psmouse,
+					   bool set_properties),
+			     struct psmouse *psmouse, bool set_properties)
+{
+	if (set_properties)
+		psmouse_apply_defaults(psmouse);
+
+	return detect(psmouse, set_properties);
+}
+
+/*
  * psmouse_extensions() probes for any extensions to the basic PS/2 protocol
  * the mouse may have.
  */
@@ -616,7 +709,7 @@ static int psmouse_extensions(struct psmouse *psmouse,
  * We always check for lifebook because it does not disturb mouse
  * (it only checks DMI information).
  */
-	if (lifebook_detect(psmouse, set_properties) == 0) {
+	if (psmouse_do_detect(lifebook_detect, psmouse, set_properties) == 0) {
 		if (max_proto > PSMOUSE_IMEX) {
 			if (!set_properties || lifebook_init(psmouse) == 0)
 				return PSMOUSE_LIFEBOOK;
@@ -628,15 +721,18 @@ static int psmouse_extensions(struct psmouse *psmouse,
  * upsets the thinkingmouse).
  */
 
-	if (max_proto > PSMOUSE_IMEX && thinking_detect(psmouse, set_properties) == 0)
+	if (max_proto > PSMOUSE_IMEX &&
+	    psmouse_do_detect(thinking_detect, psmouse, set_properties) == 0) {
 		return PSMOUSE_THINKPS;
+	}
 
 /*
  * Try Synaptics TouchPad. Note that probing is done even if Synaptics protocol
  * support is disabled in config - we need to know if it is synaptics so we
  * can reset it properly after probing for intellimouse.
  */
-	if (max_proto > PSMOUSE_PS2 && synaptics_detect(psmouse, set_properties) == 0) {
+	if (max_proto > PSMOUSE_PS2 &&
+	    psmouse_do_detect(synaptics_detect, psmouse, set_properties) == 0) {
 		synaptics_hardware = true;
 
 		if (max_proto > PSMOUSE_IMEX) {
@@ -667,7 +763,8 @@ static int psmouse_extensions(struct psmouse *psmouse,
  */
 	if (max_proto > PSMOUSE_IMEX) {
 		ps2_command(&psmouse->ps2dev, NULL, PSMOUSE_CMD_RESET_DIS);
-		if (alps_detect(psmouse, set_properties) == 0) {
+		if (psmouse_do_detect(alps_detect,
+				      psmouse, set_properties) == 0) {
 			if (!set_properties || alps_init(psmouse) == 0)
 				return PSMOUSE_ALPS;
 /*
@@ -681,7 +778,7 @@ static int psmouse_extensions(struct psmouse *psmouse,
  * Try OLPC HGPK touchpad.
  */
 	if (max_proto > PSMOUSE_IMEX &&
-			hgpk_detect(psmouse, set_properties) == 0) {
+	    psmouse_do_detect(hgpk_detect, psmouse, set_properties) == 0) {
 		if (!set_properties || hgpk_init(psmouse) == 0)
 			return PSMOUSE_HGPK;
 /*
@@ -694,7 +791,7 @@ static int psmouse_extensions(struct psmouse *psmouse,
  * Try Elantech touchpad.
  */
 	if (max_proto > PSMOUSE_IMEX &&
-			elantech_detect(psmouse, set_properties) == 0) {
+	    psmouse_do_detect(elantech_detect, psmouse, set_properties) == 0) {
 		if (!set_properties || elantech_init(psmouse) == 0)
 			return PSMOUSE_ELANTECH;
 /*
@@ -703,18 +800,21 @@ static int psmouse_extensions(struct psmouse *psmouse,
 		max_proto = PSMOUSE_IMEX;
 	}
 
-
 	if (max_proto > PSMOUSE_IMEX) {
-		if (genius_detect(psmouse, set_properties) == 0)
+		if (psmouse_do_detect(genius_detect,
+				      psmouse, set_properties) == 0)
 			return PSMOUSE_GENPS;
 
-		if (ps2pp_init(psmouse, set_properties) == 0)
+		if (psmouse_do_detect(ps2pp_init,
+				      psmouse, set_properties) == 0)
 			return PSMOUSE_PS2PP;
 
-		if (trackpoint_detect(psmouse, set_properties) == 0)
+		if (psmouse_do_detect(trackpoint_detect,
+				      psmouse, set_properties) == 0)
 			return PSMOUSE_TRACKPOINT;
 
-		if (touchkit_ps2_detect(psmouse, set_properties) == 0)
+		if (psmouse_do_detect(touchkit_ps2_detect,
+				      psmouse, set_properties) == 0)
 			return PSMOUSE_TOUCHKIT_PS2;
 	}
 
@@ -723,7 +823,8 @@ static int psmouse_extensions(struct psmouse *psmouse,
  * Trackpoint devices (causing TP_READ_ID command to time out).
  */
 	if (max_proto > PSMOUSE_IMEX) {
-		if (fsp_detect(psmouse, set_properties) == 0) {
+		if (psmouse_do_detect(fsp_detect,
+				      psmouse, set_properties) == 0) {
 			if (!set_properties || fsp_init(psmouse) == 0)
 				return PSMOUSE_FSP;
 /*
@@ -741,17 +842,23 @@ static int psmouse_extensions(struct psmouse *psmouse,
 	ps2_command(&psmouse->ps2dev, NULL, PSMOUSE_CMD_RESET_DIS);
 	psmouse_reset(psmouse);
 
-	if (max_proto >= PSMOUSE_IMEX && im_explorer_detect(psmouse, set_properties) == 0)
+	if (max_proto >= PSMOUSE_IMEX &&
+	    psmouse_do_detect(im_explorer_detect,
+			      psmouse, set_properties) == 0) {
 		return PSMOUSE_IMEX;
+	}
 
-	if (max_proto >= PSMOUSE_IMPS && intellimouse_detect(psmouse, set_properties) == 0)
+	if (max_proto >= PSMOUSE_IMPS &&
+	    psmouse_do_detect(intellimouse_detect,
+			      psmouse, set_properties) == 0) {
 		return PSMOUSE_IMPS;
+	}
 
 /*
  * Okay, all failed, we have a standard mouse here. The number of the buttons
  * is still a question, though. We assume 3.
  */
-	ps2bare_detect(psmouse, set_properties);
+	psmouse_do_detect(ps2bare_detect, psmouse, set_properties);
 
 	if (synaptics_hardware) {
 /*
@@ -819,6 +926,13 @@ static const struct psmouse_protocol psmouse_protocols[] = {
 		.detect		= synaptics_detect,
 		.init		= synaptics_init,
 	},
+	{
+		.type		= PSMOUSE_SYNAPTICS_RELATIVE,
+		.name		= "SynRelPS/2",
+		.alias		= "synaptics-relative",
+		.detect		= synaptics_detect,
+		.init		= synaptics_init_relative,
+	},
 #endif
 #ifdef CONFIG_MOUSE_PS2_ALPS
 	{
@@ -958,39 +1072,6 @@ static int psmouse_probe(struct psmouse *psmouse)
 }
 
 /*
- * Here we set the mouse resolution.
- */
-
-void psmouse_set_resolution(struct psmouse *psmouse, unsigned int resolution)
-{
-	static const unsigned char params[] = { 0, 1, 2, 2, 3 };
-	unsigned char p;
-
-	if (resolution == 0 || resolution > 200)
-		resolution = 200;
-
-	p = params[resolution / 50];
-	ps2_command(&psmouse->ps2dev, &p, PSMOUSE_CMD_SETRES);
-	psmouse->resolution = 25 << p;
-}
-
-/*
- * Here we set the mouse report rate.
- */
-
-static void psmouse_set_rate(struct psmouse *psmouse, unsigned int rate)
-{
-	static const unsigned char rates[] = { 200, 100, 80, 60, 40, 20, 10, 0 };
-	unsigned char r;
-	int i = 0;
-
-	while (rates[i] > rate) i++;
-	r = rates[i];
-	ps2_command(&psmouse->ps2dev, &r, PSMOUSE_CMD_SETRATE);
-	psmouse->rate = r;
-}
-
-/*
  * psmouse_initialize() initializes the mouse to a sane state.
  */
 
@@ -1035,16 +1116,6 @@ static void psmouse_deactivate(struct psmouse *psmouse)
 	psmouse_set_state(psmouse, PSMOUSE_CMD_MODE);
 }
 
-/*
- * psmouse_poll() - default poll handler. Everyone except for ALPS uses it.
- */
-
-static int psmouse_poll(struct psmouse *psmouse)
-{
-	return ps2_command(&psmouse->ps2dev, psmouse->packet,
-			   PSMOUSE_CMD_POLL | (psmouse->pktsize << 8));
-}
-
 
 /*
  * psmouse_resync() attempts to re-validate current protocol.
@@ -1245,18 +1316,9 @@ static int psmouse_switch_protocol(struct psmouse *psmouse,
 
 	input_dev->dev.parent = &psmouse->ps2dev.serio->dev;
 
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REL);
-	input_dev->keybit[BIT_WORD(BTN_MOUSE)] =
-				BIT_MASK(BTN_LEFT) | BIT_MASK(BTN_RIGHT);
-	input_dev->relbit[0] = BIT_MASK(REL_X) | BIT_MASK(REL_Y);
-
-	psmouse->set_rate = psmouse_set_rate;
-	psmouse->set_resolution = psmouse_set_resolution;
-	psmouse->poll = psmouse_poll;
-	psmouse->protocol_handler = psmouse_process_byte;
-	psmouse->pktsize = 3;
-
 	if (proto && (proto->detect || proto->init)) {
+		psmouse_apply_defaults(psmouse);
+
 		if (proto->detect && proto->detect(psmouse, true) < 0)
 			return -1;
 
@@ -1558,13 +1620,12 @@ static ssize_t psmouse_show_int_attr(struct psmouse *psmouse, void *offset, char
 static ssize_t psmouse_set_int_attr(struct psmouse *psmouse, void *offset, const char *buf, size_t count)
 {
 	unsigned int *field = (unsigned int *)((char *)psmouse + (size_t)offset);
-	unsigned long value;
-
-	if (strict_strtoul(buf, 10, &value))
-		return -EINVAL;
+	unsigned int value;
+	int err;
 
-	if ((unsigned int)value != value)
-		return -EINVAL;
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
 
 	*field = value;
 
@@ -1671,10 +1732,12 @@ static ssize_t psmouse_attr_set_protocol(struct psmouse *psmouse, void *data, co
 
 static ssize_t psmouse_attr_set_rate(struct psmouse *psmouse, void *data, const char *buf, size_t count)
 {
-	unsigned long value;
+	unsigned int value;
+	int err;
 
-	if (strict_strtoul(buf, 10, &value))
-		return -EINVAL;
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
 
 	psmouse->set_rate(psmouse, value);
 	return count;
@@ -1682,10 +1745,12 @@ static ssize_t psmouse_attr_set_rate(struct psmouse *psmouse, void *data, const
 
 static ssize_t psmouse_attr_set_resolution(struct psmouse *psmouse, void *data, const char *buf, size_t count)
 {
-	unsigned long value;
+	unsigned int value;
+	int err;
 
-	if (strict_strtoul(buf, 10, &value))
-		return -EINVAL;
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
 
 	psmouse->set_resolution(psmouse, value);
 	return count;
diff --git a/drivers/input/mouse/psmouse.h b/drivers/input/mouse/psmouse.h
index 9b84b0c4e371..6a417092d010 100644
--- a/drivers/input/mouse/psmouse.h
+++ b/drivers/input/mouse/psmouse.h
@@ -8,6 +8,7 @@
 #define PSMOUSE_CMD_SETSTREAM	0x00ea
 #define PSMOUSE_CMD_SETPOLL	0x00f0
 #define PSMOUSE_CMD_POLL	0x00eb	/* caller sets number of bytes to receive */
+#define PSMOUSE_CMD_RESET_WRAP	0x00ec
 #define PSMOUSE_CMD_GETID	0x02f2
 #define PSMOUSE_CMD_SETRATE	0x10f3
 #define PSMOUSE_CMD_ENABLE	0x00f4
@@ -93,6 +94,7 @@ enum psmouse_type {
 	PSMOUSE_HGPK,
 	PSMOUSE_ELANTECH,
 	PSMOUSE_FSP,
+	PSMOUSE_SYNAPTICS_RELATIVE,
 	PSMOUSE_AUTO		/* This one should always be last */
 };
 
@@ -102,6 +104,7 @@ int psmouse_sliced_command(struct psmouse *psmouse, unsigned char command);
 int psmouse_reset(struct psmouse *psmouse);
 void psmouse_set_state(struct psmouse *psmouse, enum psmouse_state new_state);
 void psmouse_set_resolution(struct psmouse *psmouse, unsigned int resolution);
+psmouse_ret_t psmouse_process_byte(struct psmouse *psmouse);
 
 struct psmouse_attribute {
 	struct device_attribute dattr;
diff --git a/drivers/input/mouse/pxa930_trkball.c b/drivers/input/mouse/pxa930_trkball.c
index ee3b0ca9d592..a9e4bfdf31f4 100644
--- a/drivers/input/mouse/pxa930_trkball.c
+++ b/drivers/input/mouse/pxa930_trkball.c
@@ -250,19 +250,7 @@ static struct platform_driver pxa930_trkball_driver = {
 	.probe		= pxa930_trkball_probe,
 	.remove		= __devexit_p(pxa930_trkball_remove),
 };
-
-static int __init pxa930_trkball_init(void)
-{
-	return platform_driver_register(&pxa930_trkball_driver);
-}
-
-static void __exit pxa930_trkball_exit(void)
-{
-	platform_driver_unregister(&pxa930_trkball_driver);
-}
-
-module_init(pxa930_trkball_init);
-module_exit(pxa930_trkball_exit);
+module_platform_driver(pxa930_trkball_driver);
 
 MODULE_AUTHOR("Yong Yao <yaoyong@marvell.com>");
 MODULE_DESCRIPTION("PXA930 Trackball Mouse Driver");
diff --git a/drivers/input/mouse/sentelic.c b/drivers/input/mouse/sentelic.c
index 86d6f39178b0..e36847de7617 100644
--- a/drivers/input/mouse/sentelic.c
+++ b/drivers/input/mouse/sentelic.c
@@ -408,7 +408,7 @@ static int fsp_onpad_hscr(struct psmouse *psmouse, bool enable)
 static ssize_t fsp_attr_set_setreg(struct psmouse *psmouse, void *data,
 				   const char *buf, size_t count)
 {
-	unsigned long reg, val;
+	int reg, val;
 	char *rest;
 	ssize_t retval;
 
@@ -416,7 +416,11 @@ static ssize_t fsp_attr_set_setreg(struct psmouse *psmouse, void *data,
 	if (rest == buf || *rest != ' ' || reg > 0xff)
 		return -EINVAL;
 
-	if (strict_strtoul(rest + 1, 16, &val) || val > 0xff)
+	retval = kstrtoint(rest + 1, 16, &val);
+	if (retval)
+		return retval;
+
+	if (val > 0xff)
 		return -EINVAL;
 
 	if (fsp_reg_write_enable(psmouse, true))
@@ -448,10 +452,13 @@ static ssize_t fsp_attr_set_getreg(struct psmouse *psmouse, void *data,
 					const char *buf, size_t count)
 {
 	struct fsp_data *pad = psmouse->private;
-	unsigned long reg;
-	int val;
+	int reg, val, err;
+
+	err = kstrtoint(buf, 16, &reg);
+	if (err)
+		return err;
 
-	if (strict_strtoul(buf, 16, &reg) || reg > 0xff)
+	if (reg > 0xff)
 		return -EINVAL;
 
 	if (fsp_reg_read(psmouse, reg, &val))
@@ -480,9 +487,13 @@ static ssize_t fsp_attr_show_pagereg(struct psmouse *psmouse,
 static ssize_t fsp_attr_set_pagereg(struct psmouse *psmouse, void *data,
 					const char *buf, size_t count)
 {
-	unsigned long val;
+	int val, err;
 
-	if (strict_strtoul(buf, 16, &val) || val > 0xff)
+	err = kstrtoint(buf, 16, &val);
+	if (err)
+		return err;
+
+	if (val > 0xff)
 		return -EINVAL;
 
 	if (fsp_page_reg_write(psmouse, val))
@@ -505,9 +516,14 @@ static ssize_t fsp_attr_show_vscroll(struct psmouse *psmouse,
 static ssize_t fsp_attr_set_vscroll(struct psmouse *psmouse, void *data,
 					const char *buf, size_t count)
 {
-	unsigned long val;
+	unsigned int val;
+	int err;
+
+	err = kstrtouint(buf, 10, &val);
+	if (err)
+		return err;
 
-	if (strict_strtoul(buf, 10, &val) || val > 1)
+	if (val > 1)
 		return -EINVAL;
 
 	fsp_onpad_vscr(psmouse, val);
@@ -529,9 +545,14 @@ static ssize_t fsp_attr_show_hscroll(struct psmouse *psmouse,
 static ssize_t fsp_attr_set_hscroll(struct psmouse *psmouse, void *data,
 					const char *buf, size_t count)
 {
-	unsigned long val;
+	unsigned int val;
+	int err;
+
+	err = kstrtouint(buf, 10, &val);
+	if (err)
+		return err;
 
-	if (strict_strtoul(buf, 10, &val) || val > 1)
+	if (val > 1)
 		return -EINVAL;
 
 	fsp_onpad_hscr(psmouse, val);
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index a6dcd18e9adf..8081a0a5d602 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -269,19 +269,49 @@ static int synaptics_query_hardware(struct psmouse *psmouse)
 	return 0;
 }
 
-static int synaptics_set_absolute_mode(struct psmouse *psmouse)
+static int synaptics_set_advanced_gesture_mode(struct psmouse *psmouse)
+{
+	static unsigned char param = 0xc8;
+	struct synaptics_data *priv = psmouse->private;
+
+	if (!SYN_CAP_ADV_GESTURE(priv->ext_cap_0c))
+		return 0;
+
+	if (psmouse_sliced_command(psmouse, SYN_QUE_MODEL))
+		return -1;
+
+	if (ps2_command(&psmouse->ps2dev, &param, PSMOUSE_CMD_SETRATE))
+		return -1;
+
+	/* Advanced gesture mode also sends multi finger data */
+	priv->capabilities |= BIT(1);
+
+	return 0;
+}
+
+static int synaptics_set_mode(struct psmouse *psmouse)
 {
 	struct synaptics_data *priv = psmouse->private;
 
-	priv->mode = SYN_BIT_ABSOLUTE_MODE;
-	if (SYN_ID_MAJOR(priv->identity) >= 4)
+	priv->mode = 0;
+	if (priv->absolute_mode)
+		priv->mode |= SYN_BIT_ABSOLUTE_MODE;
+	if (priv->disable_gesture)
 		priv->mode |= SYN_BIT_DISABLE_GESTURE;
+	if (psmouse->rate >= 80)
+		priv->mode |= SYN_BIT_HIGH_RATE;
 	if (SYN_CAP_EXTENDED(priv->capabilities))
 		priv->mode |= SYN_BIT_W_MODE;
 
 	if (synaptics_mode_cmd(psmouse, priv->mode))
 		return -1;
 
+	if (priv->absolute_mode &&
+	    synaptics_set_advanced_gesture_mode(psmouse)) {
+		psmouse_err(psmouse, "Advanced gesture mode init failed.\n");
+		return -1;
+	}
+
 	return 0;
 }
 
@@ -300,26 +330,6 @@ static void synaptics_set_rate(struct psmouse *psmouse, unsigned int rate)
 	synaptics_mode_cmd(psmouse, priv->mode);
 }
 
-static int synaptics_set_advanced_gesture_mode(struct psmouse *psmouse)
-{
-	static unsigned char param = 0xc8;
-	struct synaptics_data *priv = psmouse->private;
-
-	if (!(SYN_CAP_ADV_GESTURE(priv->ext_cap_0c) ||
-			SYN_CAP_IMAGE_SENSOR(priv->ext_cap_0c)))
-		return 0;
-
-	if (psmouse_sliced_command(psmouse, SYN_QUE_MODEL))
-		return -1;
-	if (ps2_command(&psmouse->ps2dev, &param, PSMOUSE_CMD_SETRATE))
-		return -1;
-
-	/* Advanced gesture mode also sends multi finger data */
-	priv->capabilities |= BIT(1);
-
-	return 0;
-}
-
 /*****************************************************************************
  *	Synaptics pass-through PS/2 port support
  ****************************************************************************/
@@ -1143,8 +1153,24 @@ static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
 {
 	int i;
 
+	/* Things that apply to both modes */
 	__set_bit(INPUT_PROP_POINTER, dev->propbit);
+	__set_bit(EV_KEY, dev->evbit);
+	__set_bit(BTN_LEFT, dev->keybit);
+	__set_bit(BTN_RIGHT, dev->keybit);
 
+	if (SYN_CAP_MIDDLE_BUTTON(priv->capabilities))
+		__set_bit(BTN_MIDDLE, dev->keybit);
+
+	if (!priv->absolute_mode) {
+		/* Relative mode */
+		__set_bit(EV_REL, dev->evbit);
+		__set_bit(REL_X, dev->relbit);
+		__set_bit(REL_Y, dev->relbit);
+		return;
+	}
+
+	/* Absolute mode */
 	__set_bit(EV_ABS, dev->evbit);
 	set_abs_position_params(dev, priv, ABS_X, ABS_Y);
 	input_set_abs_params(dev, ABS_PRESSURE, 0, 255, 0, 0);
@@ -1170,20 +1196,14 @@ static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
 	if (SYN_CAP_PALMDETECT(priv->capabilities))
 		input_set_abs_params(dev, ABS_TOOL_WIDTH, 0, 15, 0, 0);
 
-	__set_bit(EV_KEY, dev->evbit);
 	__set_bit(BTN_TOUCH, dev->keybit);
 	__set_bit(BTN_TOOL_FINGER, dev->keybit);
-	__set_bit(BTN_LEFT, dev->keybit);
-	__set_bit(BTN_RIGHT, dev->keybit);
 
 	if (SYN_CAP_MULTIFINGER(priv->capabilities)) {
 		__set_bit(BTN_TOOL_DOUBLETAP, dev->keybit);
 		__set_bit(BTN_TOOL_TRIPLETAP, dev->keybit);
 	}
 
-	if (SYN_CAP_MIDDLE_BUTTON(priv->capabilities))
-		__set_bit(BTN_MIDDLE, dev->keybit);
-
 	if (SYN_CAP_FOUR_BUTTON(priv->capabilities) ||
 	    SYN_CAP_MIDDLE_BUTTON(priv->capabilities)) {
 		__set_bit(BTN_FORWARD, dev->keybit);
@@ -1205,10 +1225,58 @@ static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
 	}
 }
 
+static ssize_t synaptics_show_disable_gesture(struct psmouse *psmouse,
+					      void *data, char *buf)
+{
+	struct synaptics_data *priv = psmouse->private;
+
+	return sprintf(buf, "%c\n", priv->disable_gesture ? '1' : '0');
+}
+
+static ssize_t synaptics_set_disable_gesture(struct psmouse *psmouse,
+					     void *data, const char *buf,
+					     size_t len)
+{
+	struct synaptics_data *priv = psmouse->private;
+	unsigned int value;
+	int err;
+
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
+
+	if (value > 1)
+		return -EINVAL;
+
+	if (value == priv->disable_gesture)
+		return len;
+
+	priv->disable_gesture = value;
+	if (value)
+		priv->mode |= SYN_BIT_DISABLE_GESTURE;
+	else
+		priv->mode &= ~SYN_BIT_DISABLE_GESTURE;
+
+	if (synaptics_mode_cmd(psmouse, priv->mode))
+		return -EIO;
+
+	return len;
+}
+
+PSMOUSE_DEFINE_ATTR(disable_gesture, S_IWUSR | S_IRUGO, NULL,
+		    synaptics_show_disable_gesture,
+		    synaptics_set_disable_gesture);
+
 static void synaptics_disconnect(struct psmouse *psmouse)
 {
+	struct synaptics_data *priv = psmouse->private;
+
+	if (!priv->absolute_mode && SYN_ID_DISGEST_SUPPORTED(priv->identity))
+		device_remove_file(&psmouse->ps2dev.serio->dev,
+				   &psmouse_attr_disable_gesture.dattr);
+
 	synaptics_reset(psmouse);
-	kfree(psmouse->private);
+	kfree(priv);
 	psmouse->private = NULL;
 }
 
@@ -1245,17 +1313,11 @@ static int synaptics_reconnect(struct psmouse *psmouse)
 		return -1;
 	}
 
-	if (synaptics_set_absolute_mode(psmouse)) {
+	if (synaptics_set_mode(psmouse)) {
 		psmouse_err(psmouse, "Unable to initialize device.\n");
 		return -1;
 	}
 
-	if (synaptics_set_advanced_gesture_mode(psmouse)) {
-		psmouse_err(psmouse,
-			    "Advanced gesture mode reconnect failed.\n");
-		return -1;
-	}
-
 	if (old_priv.identity != priv->identity ||
 	    old_priv.model_id != priv->model_id ||
 	    old_priv.capabilities != priv->capabilities ||
@@ -1332,20 +1394,18 @@ void __init synaptics_module_init(void)
 	broken_olpc_ec = dmi_check_system(olpc_dmi_table);
 }
 
-int synaptics_init(struct psmouse *psmouse)
+static int __synaptics_init(struct psmouse *psmouse, bool absolute_mode)
 {
 	struct synaptics_data *priv;
+	int err = -1;
 
 	/*
-	 * The OLPC XO has issues with Synaptics' absolute mode; similarly to
-	 * the HGPK, it quickly degrades and the hardware becomes jumpy and
-	 * overly sensitive.  Not only that, but the constant packet spew
-	 * (even at a lowered 40pps rate) overloads the EC such that key
-	 * presses on the keyboard are missed.  Given all of that, don't
-	 * even attempt to use Synaptics mode.  Relative mode seems to work
-	 * just fine.
+	 * The OLPC XO has issues with Synaptics' absolute mode; the constant
+	 * packet spew overloads the EC such that key presses on the keyboard
+	 * are missed.  Given that, don't even attempt to use Absolute mode.
+	 * Relative mode seems to work just fine.
 	 */
-	if (broken_olpc_ec) {
+	if (absolute_mode && broken_olpc_ec) {
 		psmouse_info(psmouse,
 			     "OLPC XO detected, not enabling Synaptics protocol.\n");
 		return -ENODEV;
@@ -1362,13 +1422,12 @@ int synaptics_init(struct psmouse *psmouse)
 		goto init_fail;
 	}
 
-	if (synaptics_set_absolute_mode(psmouse)) {
-		psmouse_err(psmouse, "Unable to initialize device.\n");
-		goto init_fail;
-	}
+	priv->absolute_mode = absolute_mode;
+	if (SYN_ID_DISGEST_SUPPORTED(priv->identity))
+		priv->disable_gesture = true;
 
-	if (synaptics_set_advanced_gesture_mode(psmouse)) {
-		psmouse_err(psmouse, "Advanced gesture mode init failed.\n");
+	if (synaptics_set_mode(psmouse)) {
+		psmouse_err(psmouse, "Unable to initialize device.\n");
 		goto init_fail;
 	}
 
@@ -1393,12 +1452,19 @@ int synaptics_init(struct psmouse *psmouse)
 	psmouse->model = ((priv->model_id & 0x00ff0000) >> 8) |
 			  (priv->model_id & 0x000000ff);
 
-	psmouse->protocol_handler = synaptics_process_byte;
+	if (absolute_mode) {
+		psmouse->protocol_handler = synaptics_process_byte;
+		psmouse->pktsize = 6;
+	} else {
+		/* Relative mode follows standard PS/2 mouse protocol */
+		psmouse->protocol_handler = psmouse_process_byte;
+		psmouse->pktsize = 3;
+	}
+
 	psmouse->set_rate = synaptics_set_rate;
 	psmouse->disconnect = synaptics_disconnect;
 	psmouse->reconnect = synaptics_reconnect;
 	psmouse->cleanup = synaptics_reset;
-	psmouse->pktsize = 6;
 	/* Synaptics can usually stay in sync without extra help */
 	psmouse->resync_time = 0;
 
@@ -1417,11 +1483,32 @@ int synaptics_init(struct psmouse *psmouse)
 		psmouse->rate = 40;
 	}
 
+	if (!priv->absolute_mode && SYN_ID_DISGEST_SUPPORTED(priv->identity)) {
+		err = device_create_file(&psmouse->ps2dev.serio->dev,
+					 &psmouse_attr_disable_gesture.dattr);
+		if (err) {
+			psmouse_err(psmouse,
+				    "Failed to create disable_gesture attribute (%d)",
+				    err);
+			goto init_fail;
+		}
+	}
+
 	return 0;
 
  init_fail:
 	kfree(priv);
-	return -1;
+	return err;
+}
+
+int synaptics_init(struct psmouse *psmouse)
+{
+	return __synaptics_init(psmouse, true);
+}
+
+int synaptics_init_relative(struct psmouse *psmouse)
+{
+	return __synaptics_init(psmouse, false);
 }
 
 bool synaptics_supported(void)
diff --git a/drivers/input/mouse/synaptics.h b/drivers/input/mouse/synaptics.h
index 622aea8dd7e0..fd26ccca13d7 100644
--- a/drivers/input/mouse/synaptics.h
+++ b/drivers/input/mouse/synaptics.h
@@ -100,6 +100,7 @@
 #define SYN_ID_MINOR(i)			(((i) >> 16) & 0xff)
 #define SYN_ID_FULL(i)			((SYN_ID_MAJOR(i) << 8) | SYN_ID_MINOR(i))
 #define SYN_ID_IS_SYNAPTICS(i)		((((i) >> 8) & 0xff) == 0x47)
+#define SYN_ID_DISGEST_SUPPORTED(i)	(SYN_ID_MAJOR(i) >= 4)
 
 /* synaptics special commands */
 #define SYN_PS_SET_MODE2		0x14
@@ -159,6 +160,9 @@ struct synaptics_data {
 	unsigned char mode;			/* current mode byte */
 	int scroll;
 
+	bool absolute_mode;			/* run in Absolute mode */
+	bool disable_gesture;			/* disable gestures */
+
 	struct serio *pt_port;			/* Pass-through serio port */
 
 	struct synaptics_mt_state mt_state;	/* Current mt finger state */
@@ -175,6 +179,7 @@ struct synaptics_data {
 void synaptics_module_init(void);
 int synaptics_detect(struct psmouse *psmouse, bool set_properties);
 int synaptics_init(struct psmouse *psmouse);
+int synaptics_init_relative(struct psmouse *psmouse);
 void synaptics_reset(struct psmouse *psmouse);
 bool synaptics_supported(void);
 
diff --git a/drivers/input/mouse/trackpoint.c b/drivers/input/mouse/trackpoint.c
index 54b2fa892e19..22b218018137 100644
--- a/drivers/input/mouse/trackpoint.c
+++ b/drivers/input/mouse/trackpoint.c
@@ -89,10 +89,12 @@ static ssize_t trackpoint_set_int_attr(struct psmouse *psmouse, void *data,
 	struct trackpoint_data *tp = psmouse->private;
 	struct trackpoint_attr_data *attr = data;
 	unsigned char *field = (unsigned char *)((char *)tp + attr->field_offset);
-	unsigned long value;
+	unsigned char value;
+	int err;
 
-	if (strict_strtoul(buf, 10, &value) || value > 255)
-		return -EINVAL;
+	err = kstrtou8(buf, 10, &value);
+	if (err)
+		return err;
 
 	*field = value;
 	trackpoint_write(&psmouse->ps2dev, attr->command, value);
@@ -115,9 +117,14 @@ static ssize_t trackpoint_set_bit_attr(struct psmouse *psmouse, void *data,
 	struct trackpoint_data *tp = psmouse->private;
 	struct trackpoint_attr_data *attr = data;
 	unsigned char *field = (unsigned char *)((char *)tp + attr->field_offset);
-	unsigned long value;
+	unsigned int value;
+	int err;
+
+	err = kstrtouint(buf, 10, &value);
+	if (err)
+		return err;
 
-	if (strict_strtoul(buf, 10, &value) || value > 1)
+	if (value > 1)
 		return -EINVAL;
 
 	if (attr->inverted)
diff --git a/drivers/input/serio/altera_ps2.c b/drivers/input/serio/altera_ps2.c
index d363dc4571a3..35864c6130bb 100644
--- a/drivers/input/serio/altera_ps2.c
+++ b/drivers/input/serio/altera_ps2.c
@@ -196,18 +196,7 @@ static struct platform_driver altera_ps2_driver = {
 		.of_match_table = altera_ps2_match,
 	},
 };
-
-static int __init altera_ps2_init(void)
-{
-	return platform_driver_register(&altera_ps2_driver);
-}
-module_init(altera_ps2_init);
-
-static void __exit altera_ps2_exit(void)
-{
-	platform_driver_unregister(&altera_ps2_driver);
-}
-module_exit(altera_ps2_exit);
+module_platform_driver(altera_ps2_driver);
 
 MODULE_DESCRIPTION("Altera University Program PS2 controller driver");
 MODULE_AUTHOR("Thomas Chou <thomas@wytron.com.tw>");
diff --git a/drivers/input/serio/at32psif.c b/drivers/input/serio/at32psif.c
index 95280f9207e1..421a7442e464 100644
--- a/drivers/input/serio/at32psif.c
+++ b/drivers/input/serio/at32psif.c
@@ -358,19 +358,7 @@ static struct platform_driver psif_driver = {
 	.suspend	= psif_suspend,
 	.resume		= psif_resume,
 };
-
-static int __init psif_init(void)
-{
-	return platform_driver_probe(&psif_driver, psif_probe);
-}
-
-static void __exit psif_exit(void)
-{
-	platform_driver_unregister(&psif_driver);
-}
-
-module_init(psif_init);
-module_exit(psif_exit);
+module_platform_driver(psif_driver);
 
 MODULE_AUTHOR("Hans-Christian Egtvedt <egtvedt@samfundet.no>");
 MODULE_DESCRIPTION("Atmel AVR32 PSIF PS/2 driver");
diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index d37a48e099d0..86564414b75a 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -991,7 +991,7 @@ static int i8042_controller_init(void)
  * Reset the controller and reset CRT to the original value set by BIOS.
  */
 
-static void i8042_controller_reset(void)
+static void i8042_controller_reset(bool force_reset)
 {
 	i8042_flush();
 
@@ -1016,7 +1016,7 @@ static void i8042_controller_reset(void)
  * Reset the controller if requested.
  */
 
-	if (i8042_reset)
+	if (i8042_reset || force_reset)
 		i8042_controller_selftest();
 
 /*
@@ -1139,9 +1139,9 @@ static int i8042_controller_resume(bool force_reset)
  * upsetting it.
  */
 
-static int i8042_pm_reset(struct device *dev)
+static int i8042_pm_suspend(struct device *dev)
 {
-	i8042_controller_reset();
+	i8042_controller_reset(true);
 
 	return 0;
 }
@@ -1163,13 +1163,20 @@ static int i8042_pm_thaw(struct device *dev)
 	return 0;
 }
 
+static int i8042_pm_reset(struct device *dev)
+{
+	i8042_controller_reset(false);
+
+	return 0;
+}
+
 static int i8042_pm_restore(struct device *dev)
 {
 	return i8042_controller_resume(false);
 }
 
 static const struct dev_pm_ops i8042_pm_ops = {
-	.suspend	= i8042_pm_reset,
+	.suspend	= i8042_pm_suspend,
 	.resume		= i8042_pm_resume,
 	.thaw		= i8042_pm_thaw,
 	.poweroff	= i8042_pm_reset,
@@ -1185,7 +1192,7 @@ static const struct dev_pm_ops i8042_pm_ops = {
 
 static void i8042_shutdown(struct platform_device *dev)
 {
-	i8042_controller_reset();
+	i8042_controller_reset(false);
 }
 
 static int __init i8042_create_kbd_port(void)
@@ -1424,7 +1431,7 @@ static int __init i8042_probe(struct platform_device *dev)
  out_fail:
 	i8042_free_aux_ports();	/* in case KBD failed but AUX not */
 	i8042_free_irqs();
-	i8042_controller_reset();
+	i8042_controller_reset(false);
 	i8042_platform_device = NULL;
 
 	return error;
@@ -1434,7 +1441,7 @@ static int __devexit i8042_remove(struct platform_device *dev)
 {
 	i8042_unregister_ports();
 	i8042_free_irqs();
-	i8042_controller_reset();
+	i8042_controller_reset(false);
 	i8042_platform_device = NULL;
 
 	return 0;
diff --git a/drivers/input/serio/rpckbd.c b/drivers/input/serio/rpckbd.c
index 7ec3c97dc1b9..8b44ddc8041c 100644
--- a/drivers/input/serio/rpckbd.c
+++ b/drivers/input/serio/rpckbd.c
@@ -143,16 +143,4 @@ static struct platform_driver rpckbd_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init rpckbd_init(void)
-{
-	return platform_driver_register(&rpckbd_driver);
-}
-
-static void __exit rpckbd_exit(void)
-{
-	platform_driver_unregister(&rpckbd_driver);
-}
-
-module_init(rpckbd_init);
-module_exit(rpckbd_exit);
+module_platform_driver(rpckbd_driver);
diff --git a/drivers/input/serio/xilinx_ps2.c b/drivers/input/serio/xilinx_ps2.c
index d64c5a43aaad..d96d4c2a76a9 100644
--- a/drivers/input/serio/xilinx_ps2.c
+++ b/drivers/input/serio/xilinx_ps2.c
@@ -253,7 +253,7 @@ static int __devinit xps2_of_probe(struct platform_device *ofdev)
 	}
 
 	/* Get IRQ for the device */
-	if (of_irq_to_resource(ofdev->dev.of_node, 0, &r_irq) == NO_IRQ) {
+	if (!of_irq_to_resource(ofdev->dev.of_node, 0, &r_irq)) {
 		dev_err(dev, "no IRQ found\n");
 		return -ENODEV;
 	}
@@ -369,19 +369,7 @@ static struct platform_driver xps2_of_driver = {
 	.probe		= xps2_of_probe,
 	.remove		= __devexit_p(xps2_of_remove),
 };
-
-static int __init xps2_init(void)
-{
-	return platform_driver_register(&xps2_of_driver);
-}
-
-static void __exit xps2_cleanup(void)
-{
-	platform_driver_unregister(&xps2_of_driver);
-}
-
-module_init(xps2_init);
-module_exit(xps2_cleanup);
+module_platform_driver(xps2_of_driver);
 
 MODULE_AUTHOR("Xilinx, Inc.");
 MODULE_DESCRIPTION("Xilinx XPS PS/2 driver");
diff --git a/drivers/input/tablet/aiptek.c b/drivers/input/tablet/aiptek.c
index d5ef3debd045..205d16aab441 100644
--- a/drivers/input/tablet/aiptek.c
+++ b/drivers/input/tablet/aiptek.c
@@ -1198,9 +1198,9 @@ static ssize_t
 store_tabletXtilt(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct aiptek *aiptek = dev_get_drvdata(dev);
-	long x;
+	int x;
 
-	if (strict_strtol(buf, 10, &x)) {
+	if (kstrtoint(buf, 10, &x)) {
 		size_t len = buf[count - 1] == '\n' ? count - 1 : count;
 
 		if (strncmp(buf, "disable", len))
@@ -1240,9 +1240,9 @@ static ssize_t
 store_tabletYtilt(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct aiptek *aiptek = dev_get_drvdata(dev);
-	long y;
+	int y;
 
-	if (strict_strtol(buf, 10, &y)) {
+	if (kstrtoint(buf, 10, &y)) {
 		size_t len = buf[count - 1] == '\n' ? count - 1 : count;
 
 		if (strncmp(buf, "disable", len))
@@ -1277,12 +1277,13 @@ static ssize_t
 store_tabletJitterDelay(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct aiptek *aiptek = dev_get_drvdata(dev);
-	long j;
+	int err, j;
 
-	if (strict_strtol(buf, 10, &j))
-		return -EINVAL;
+	err = kstrtoint(buf, 10, &j);
+	if (err)
+		return err;
 
-	aiptek->newSetting.jitterDelay = (int)j;
+	aiptek->newSetting.jitterDelay = j;
 	return count;
 }
 
@@ -1306,12 +1307,13 @@ static ssize_t
 store_tabletProgrammableDelay(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct aiptek *aiptek = dev_get_drvdata(dev);
-	long d;
+	int err, d;
 
-	if (strict_strtol(buf, 10, &d))
-		return -EINVAL;
+	err = kstrtoint(buf, 10, &d);
+	if (err)
+		return err;
 
-	aiptek->newSetting.programmableDelay = (int)d;
+	aiptek->newSetting.programmableDelay = d;
 	return count;
 }
 
@@ -1557,11 +1559,13 @@ static ssize_t
 store_tabletWheel(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
 {
 	struct aiptek *aiptek = dev_get_drvdata(dev);
-	long w;
+	int err, w;
 
-	if (strict_strtol(buf, 10, &w)) return -EINVAL;
+	err = kstrtoint(buf, 10, &w);
+	if (err)
+		return err;
 
-	aiptek->newSetting.wheel = (int)w;
+	aiptek->newSetting.wheel = w;
 	return count;
 }
 
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index 8f9cde3e0ec2..2a97b7e76db1 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -28,7 +28,9 @@
 #define HID_USAGE_Y_TILT		0x3e
 #define HID_USAGE_FINGER		0x22
 #define HID_USAGE_STYLUS		0x20
-#define HID_COLLECTION			0xc0
+#define HID_COLLECTION			0xa1
+#define HID_COLLECTION_LOGICAL		0x02
+#define HID_COLLECTION_END		0xc0
 
 enum {
 	WCM_UNDEFINED = 0,
@@ -66,7 +68,8 @@ static int wacom_get_report(struct usb_interface *intf, u8 type, u8 id,
 	do {
 		retval = usb_control_msg(dev, usb_rcvctrlpipe(dev, 0),
 				USB_REQ_GET_REPORT,
-				USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+				USB_DIR_IN | USB_TYPE_CLASS |
+				USB_RECIP_INTERFACE,
 				(type << 8) + id,
 				intf->altsetting[0].desc.bInterfaceNumber,
 				buf, size, 100);
@@ -164,7 +167,70 @@ static void wacom_close(struct input_dev *dev)
 		usb_autopm_put_interface(wacom->intf);
 }
 
-static int wacom_parse_hid(struct usb_interface *intf, struct hid_descriptor *hid_desc,
+static int wacom_parse_logical_collection(unsigned char *report,
+					  struct wacom_features *features)
+{
+	int length = 0;
+
+	if (features->type == BAMBOO_PT) {
+
+		/* Logical collection is only used by 3rd gen Bamboo Touch */
+		features->pktlen = WACOM_PKGLEN_BBTOUCH3;
+		features->device_type = BTN_TOOL_DOUBLETAP;
+
+		/*
+		 * Stylus and Touch have same active area
+		 * so compute physical size based on stylus
+		 * data before its overwritten.
+		 */
+		features->x_phy =
+			(features->x_max * features->x_resolution) / 100;
+		features->y_phy =
+			(features->y_max * features->y_resolution) / 100;
+
+		features->x_max = features->y_max =
+			get_unaligned_le16(&report[10]);
+
+		length = 11;
+	}
+	return length;
+}
+
+/*
+ * Interface Descriptor of wacom devices can be incomplete and
+ * inconsistent so wacom_features table is used to store stylus
+ * device's packet lengths, various maximum values, and tablet
+ * resolution based on product ID's.
+ *
+ * For devices that contain 2 interfaces, wacom_features table is
+ * inaccurate for the touch interface.  Since the Interface Descriptor
+ * for touch interfaces has pretty complete data, this function exists
+ * to query tablet for this missing information instead of hard coding in
+ * an additional table.
+ *
+ * A typical Interface Descriptor for a stylus will contain a
+ * boot mouse application collection that is not of interest and this
+ * function will ignore it.
+ *
+ * It also contains a digitizer application collection that also is not
+ * of interest since any information it contains would be duplicate
+ * of what is in wacom_features. Usually it defines a report of an array
+ * of bytes that could be used as max length of the stylus packet returned.
+ * If it happens to define a Digitizer-Stylus Physical Collection then
+ * the X and Y logical values contain valid data but it is ignored.
+ *
+ * A typical Interface Descriptor for a touch interface will contain a
+ * Digitizer-Finger Physical Collection which will define both logical
+ * X/Y maximum as well as the physical size of tablet. Since touch
+ * interfaces haven't supported pressure or distance, this is enough
+ * information to override invalid values in the wacom_features table.
+ *
+ * 3rd gen Bamboo Touch no longer define a Digitizer-Finger Pysical
+ * Collection. Instead they define a Logical Collection with a single
+ * Logical Maximum for both X and Y.
+ */
+static int wacom_parse_hid(struct usb_interface *intf,
+			   struct hid_descriptor *hid_desc,
 			   struct wacom_features *features)
 {
 	struct usb_device *dev = interface_to_usbdev(intf);
@@ -244,8 +310,6 @@ static int wacom_parse_hid(struct usb_interface *intf, struct hid_descriptor *hi
 						/* penabled only accepts exact bytes of data */
 						if (features->type == TABLETPC2FG)
 							features->pktlen = WACOM_PKGLEN_GRAPHIRE;
-						if (features->type == BAMBOO_PT)
-							features->pktlen = WACOM_PKGLEN_BBFUN;
 						features->device_type = BTN_TOOL_PEN;
 						features->x_max =
 							get_unaligned_le16(&report[i + 3]);
@@ -287,8 +351,6 @@ static int wacom_parse_hid(struct usb_interface *intf, struct hid_descriptor *hi
 						/* penabled only accepts exact bytes of data */
 						if (features->type == TABLETPC2FG)
 							features->pktlen = WACOM_PKGLEN_GRAPHIRE;
-						if (features->type == BAMBOO_PT)
-							features->pktlen = WACOM_PKGLEN_BBFUN;
 						features->device_type = BTN_TOOL_PEN;
 						features->y_max =
 							get_unaligned_le16(&report[i + 3]);
@@ -302,6 +364,11 @@ static int wacom_parse_hid(struct usb_interface *intf, struct hid_descriptor *hi
 				i++;
 				break;
 
+			/*
+			 * Requiring Stylus Usage will ignore boot mouse
+			 * X/Y values and some cases of invalid Digitizer X/Y
+			 * values commonly reported.
+			 */
 			case HID_USAGE_STYLUS:
 				pen = 1;
 				i++;
@@ -309,10 +376,20 @@ static int wacom_parse_hid(struct usb_interface *intf, struct hid_descriptor *hi
 			}
 			break;
 
-		case HID_COLLECTION:
+		case HID_COLLECTION_END:
 			/* reset UsagePage and Finger */
 			finger = usage = 0;
 			break;
+
+		case HID_COLLECTION:
+			i++;
+			switch (report[i]) {
+			case HID_COLLECTION_LOGICAL:
+				i += wacom_parse_logical_collection(&report[i],
+								    features);
+				break;
+			}
+			break;
 		}
 	}
 
@@ -348,7 +425,8 @@ static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_feat
 						WAC_HID_FEATURE_REPORT,
 						report_id, rep_data, 4, 1);
 		} while ((error < 0 || rep_data[1] != 4) && limit++ < WAC_MSG_RETRIES);
-	} else if (features->type != TABLETPC) {
+	} else if (features->type != TABLETPC &&
+		   features->device_type == BTN_TOOL_PEN) {
 		do {
 			rep_data[0] = 2;
 			rep_data[1] = 2;
@@ -485,7 +563,8 @@ static int wacom_led_control(struct wacom *wacom)
 	if (!buf)
 		return -ENOMEM;
 
-	if (wacom->wacom_wac.features.type == WACOM_21UX2)
+	if (wacom->wacom_wac.features.type == WACOM_21UX2 ||
+	    wacom->wacom_wac.features.type == WACOM_24HD)
 		led = (wacom->led.select[1] << 4) | 0x40;
 
 	led |=  wacom->led.select[0] | 0x4;
@@ -704,6 +783,7 @@ static int wacom_initialize_leds(struct wacom *wacom)
 					   &intuos4_led_attr_group);
 		break;
 
+	case WACOM_24HD:
 	case WACOM_21UX2:
 		wacom->led.select[0] = 0;
 		wacom->led.select[1] = 0;
@@ -738,6 +818,7 @@ static void wacom_destroy_leds(struct wacom *wacom)
 				   &intuos4_led_attr_group);
 		break;
 
+	case WACOM_24HD:
 	case WACOM_21UX2:
 		sysfs_remove_group(&wacom->intf->dev.kobj,
 				   &cintiq_led_attr_group);
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 2ee47d01a3b4..88672ec296c1 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -452,7 +452,7 @@ static void wacom_intuos_general(struct wacom_wac *wacom)
 	if ((data[1] & 0xb8) == 0xa0) {
 		t = (data[6] << 2) | ((data[7] >> 6) & 3);
 		if ((features->type >= INTUOS4S && features->type <= INTUOS4L) ||
-		    features->type == WACOM_21UX2) {
+		    features->type == WACOM_21UX2 || features->type == WACOM_24HD) {
 			t = (t << 1) | (data[1] & 1);
 		}
 		input_report_abs(input, ABS_PRESSURE, t);
@@ -519,6 +519,56 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 				input_report_key(input, wacom->tool[1], 0);
 				input_report_abs(input, ABS_MISC, 0);
 			}
+		} else if (features->type == WACOM_24HD) {
+			input_report_key(input, BTN_0, (data[6] & 0x01));
+			input_report_key(input, BTN_1, (data[6] & 0x02));
+			input_report_key(input, BTN_2, (data[6] & 0x04));
+			input_report_key(input, BTN_3, (data[6] & 0x08));
+			input_report_key(input, BTN_4, (data[6] & 0x10));
+			input_report_key(input, BTN_5, (data[6] & 0x20));
+			input_report_key(input, BTN_6, (data[6] & 0x40));
+			input_report_key(input, BTN_7, (data[6] & 0x80));
+			input_report_key(input, BTN_8, (data[8] & 0x01));
+			input_report_key(input, BTN_9, (data[8] & 0x02));
+			input_report_key(input, BTN_A, (data[8] & 0x04));
+			input_report_key(input, BTN_B, (data[8] & 0x08));
+			input_report_key(input, BTN_C, (data[8] & 0x10));
+			input_report_key(input, BTN_X, (data[8] & 0x20));
+			input_report_key(input, BTN_Y, (data[8] & 0x40));
+			input_report_key(input, BTN_Z, (data[8] & 0x80));
+
+			/*
+			 * Three "buttons" are available on the 24HD which are
+			 * physically implemented as a touchstrip. Each button
+			 * is approximately 3 bits wide with a 2 bit spacing.
+			 * The raw touchstrip bits are stored at:
+			 *    ((data[3] & 0x1f) << 8) | data[4])
+			 */
+			input_report_key(input, KEY_PROG1, data[4] & 0x07);
+			input_report_key(input, KEY_PROG2, data[4] & 0xE0);
+			input_report_key(input, KEY_PROG3, data[3] & 0x1C);
+
+			if (data[1] & 0x80) {
+				input_report_abs(input, ABS_WHEEL, (data[1] & 0x7f));
+			} else {
+				/* Out of proximity, clear wheel value. */
+				input_report_abs(input, ABS_WHEEL, 0);
+			}
+
+			if (data[2] & 0x80) {
+				input_report_abs(input, ABS_THROTTLE, (data[2] & 0x7f));
+			} else {
+				/* Out of proximity, clear second wheel value. */
+				input_report_abs(input, ABS_THROTTLE, 0);
+			}
+
+			if (data[1] | data[2] | (data[3] & 0x1f) | data[4] | data[6] | data[8]) {
+				input_report_key(input, wacom->tool[1], 1);
+				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
+			} else {
+				input_report_key(input, wacom->tool[1], 0);
+				input_report_abs(input, ABS_MISC, 0);
+			}
 		} else {
 			if (features->type == WACOM_21UX2) {
 				input_report_key(input, BTN_0, (data[5] & 0x01));
@@ -799,6 +849,9 @@ static int wacom_bpt_touch(struct wacom_wac *wacom)
 	unsigned char *data = wacom->data;
 	int i;
 
+	if (data[0] != 0x02)
+	    return 0;
+
 	for (i = 0; i < 2; i++) {
 		int offset = (data[1] & 0x80) ? (8 * i) : (9 * i);
 		bool touch = data[offset + 3] & 0x80;
@@ -837,18 +890,77 @@ static int wacom_bpt_touch(struct wacom_wac *wacom)
 	return 0;
 }
 
+static void wacom_bpt3_touch_msg(struct wacom_wac *wacom, unsigned char *data)
+{
+	struct input_dev *input = wacom->input;
+	int slot_id = data[0] - 2;  /* data[0] is between 2 and 17 */
+	bool touch = data[1] & 0x80;
+
+	touch = touch && !wacom->shared->stylus_in_proximity;
+
+	input_mt_slot(input, slot_id);
+	input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
+
+	if (touch) {
+		int x = (data[2] << 4) | (data[4] >> 4);
+		int y = (data[3] << 4) | (data[4] & 0x0f);
+		int w = data[6];
+
+		input_report_abs(input, ABS_MT_POSITION_X, x);
+		input_report_abs(input, ABS_MT_POSITION_Y, y);
+		input_report_abs(input, ABS_MT_TOUCH_MAJOR, w);
+	}
+}
+
+static void wacom_bpt3_button_msg(struct wacom_wac *wacom, unsigned char *data)
+{
+	struct input_dev *input = wacom->input;
+
+	input_report_key(input, BTN_LEFT, (data[1] & 0x08) != 0);
+	input_report_key(input, BTN_FORWARD, (data[1] & 0x04) != 0);
+	input_report_key(input, BTN_BACK, (data[1] & 0x02) != 0);
+	input_report_key(input, BTN_RIGHT, (data[1] & 0x01) != 0);
+}
+
+static int wacom_bpt3_touch(struct wacom_wac *wacom)
+{
+	struct input_dev *input = wacom->input;
+	unsigned char *data = wacom->data;
+	int count = data[1] & 0x03;
+	int i;
+
+	if (data[0] != 0x02)
+	    return 0;
+
+	/* data has up to 7 fixed sized 8-byte messages starting at data[2] */
+	for (i = 0; i < count; i++) {
+		int offset = (8 * i) + 2;
+		int msg_id = data[offset];
+
+		if (msg_id >= 2 && msg_id <= 17)
+			wacom_bpt3_touch_msg(wacom, data + offset);
+		else if (msg_id == 128)
+			wacom_bpt3_button_msg(wacom, data + offset);
+
+	}
+
+	input_mt_report_pointer_emulation(input, true);
+
+	input_sync(input);
+
+	return 0;
+}
+
 static int wacom_bpt_pen(struct wacom_wac *wacom)
 {
 	struct input_dev *input = wacom->input;
 	unsigned char *data = wacom->data;
 	int prox = 0, x = 0, y = 0, p = 0, d = 0, pen = 0, btn1 = 0, btn2 = 0;
 
-	/*
-	 * Similar to Graphire protocol, data[1] & 0x20 is proximity and
-	 * data[1] & 0x18 is tool ID.  0x30 is safety check to ignore
-	 * 2 unused tool ID's.
-	 */
-	prox = (data[1] & 0x30) == 0x30;
+	if (data[0] != 0x02)
+	    return 0;
+
+	prox = (data[1] & 0x20) == 0x20;
 
 	/*
 	 * All reports shared between PEN and RUBBER tool must be
@@ -912,7 +1024,9 @@ static int wacom_bpt_irq(struct wacom_wac *wacom, size_t len)
 {
 	if (len == WACOM_PKGLEN_BBTOUCH)
 		return wacom_bpt_touch(wacom);
-	else if (len == WACOM_PKGLEN_BBFUN)
+	else if (len == WACOM_PKGLEN_BBTOUCH3)
+		return wacom_bpt3_touch(wacom);
+	else if (len == WACOM_PKGLEN_BBFUN || len == WACOM_PKGLEN_BBPEN)
 		return wacom_bpt_pen(wacom);
 
 	return 0;
@@ -955,6 +1069,7 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 	case CINTIQ:
 	case WACOM_BEE:
 	case WACOM_21UX2:
+	case WACOM_24HD:
 		sync = wacom_intuos_irq(wacom_wac);
 		break;
 
@@ -1031,9 +1146,9 @@ void wacom_setup_device_quirks(struct wacom_features *features)
 	    features->type == BAMBOO_PT)
 		features->quirks |= WACOM_QUIRK_MULTI_INPUT;
 
-	/* quirks for bamboo touch */
+	/* quirk for bamboo touch with 2 low res touches */
 	if (features->type == BAMBOO_PT &&
-	    features->device_type == BTN_TOOL_DOUBLETAP) {
+	    features->pktlen == WACOM_PKGLEN_BBTOUCH) {
 		features->x_max <<= 5;
 		features->y_max <<= 5;
 		features->x_fuzz <<= 5;
@@ -1110,6 +1225,26 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
 		break;
 
+	case WACOM_24HD:
+		__set_bit(BTN_A, input_dev->keybit);
+		__set_bit(BTN_B, input_dev->keybit);
+		__set_bit(BTN_C, input_dev->keybit);
+		__set_bit(BTN_X, input_dev->keybit);
+		__set_bit(BTN_Y, input_dev->keybit);
+		__set_bit(BTN_Z, input_dev->keybit);
+
+		for (i = 0; i < 10; i++)
+			__set_bit(BTN_0 + i, input_dev->keybit);
+
+		__set_bit(KEY_PROG1, input_dev->keybit);
+		__set_bit(KEY_PROG2, input_dev->keybit);
+		__set_bit(KEY_PROG3, input_dev->keybit);
+
+		input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
+		input_set_abs_params(input_dev, ABS_THROTTLE, 0, 71, 0, 0);
+		wacom_setup_cintiq(wacom_wac);
+		break;
+
 	case WACOM_21UX2:
 		__set_bit(BTN_A, input_dev->keybit);
 		__set_bit(BTN_B, input_dev->keybit);
@@ -1240,7 +1375,21 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
 			__set_bit(BTN_TOOL_FINGER, input_dev->keybit);
 			__set_bit(BTN_TOOL_DOUBLETAP, input_dev->keybit);
 
-			input_mt_init_slots(input_dev, 2);
+			if (features->pktlen == WACOM_PKGLEN_BBTOUCH3) {
+				__set_bit(BTN_TOOL_TRIPLETAP,
+					  input_dev->keybit);
+				__set_bit(BTN_TOOL_QUADTAP,
+					  input_dev->keybit);
+
+				input_mt_init_slots(input_dev, 16);
+
+				input_set_abs_params(input_dev,
+						     ABS_MT_TOUCH_MAJOR,
+						     0, 255, 0, 0);
+			} else {
+				input_mt_init_slots(input_dev, 2);
+			}
+
 			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
 					     0, features->x_max,
 					     features->x_fuzz, 0);
@@ -1425,6 +1574,9 @@ static const struct wacom_features wacom_features_0xBB =
 static const struct wacom_features wacom_features_0xBC =
 	{ "Wacom Intuos4 WL",     WACOM_PKGLEN_INTUOS,    40840, 25400, 2047,
 	  63, INTUOS4, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
+static const struct wacom_features wacom_features_0xF4 =
+	{ "Wacom Cintiq 24HD",    WACOM_PKGLEN_INTUOS,   104480, 65600, 2047,
+	  63, WACOM_24HD, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
 static const struct wacom_features wacom_features_0x3F =
 	{ "Wacom Cintiq 21UX",    WACOM_PKGLEN_INTUOS,    87200, 65600, 1023,
 	  63, CINTIQ, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES };
@@ -1509,6 +1661,15 @@ static const struct wacom_features wacom_features_0xDA =
 static struct wacom_features wacom_features_0xDB =
 	{ "Wacom Bamboo 2FG 6x8 SE", WACOM_PKGLEN_BBFUN,  21648, 13700, 1023,
 	  31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0xDD =
+        { "Wacom Bamboo Connect", WACOM_PKGLEN_BBPEN,     14720,  9200, 1023,
+          31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0xDE =
+        { "Wacom Bamboo 16FG 4x5", WACOM_PKGLEN_BBPEN,    14720,  9200, 1023,
+          31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
+static const struct wacom_features wacom_features_0xDF =
+        { "Wacom Bamboo 16FG 6x8", WACOM_PKGLEN_BBPEN,    21648, 13700, 1023,
+          31, BAMBOO_PT, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x6004 =
 	{ "ISD-V4",               WACOM_PKGLEN_GRAPHIRE,  12800,  8000,  255,
 	  0, TABLETPC, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
@@ -1604,6 +1765,9 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0xD8) },
 	{ USB_DEVICE_WACOM(0xDA) },
 	{ USB_DEVICE_WACOM(0xDB) },
+	{ USB_DEVICE_WACOM(0xDD) },
+	{ USB_DEVICE_WACOM(0xDE) },
+	{ USB_DEVICE_WACOM(0xDF) },
 	{ USB_DEVICE_WACOM(0xF0) },
 	{ USB_DEVICE_WACOM(0xCC) },
 	{ USB_DEVICE_WACOM(0x90) },
@@ -1616,6 +1780,7 @@ const struct usb_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0xE6) },
 	{ USB_DEVICE_WACOM(0xEC) },
 	{ USB_DEVICE_WACOM(0x47) },
+	{ USB_DEVICE_WACOM(0xF4) },
 	{ USB_DEVICE_LENOVO(0x6004) },
 	{ }
 };
diff --git a/drivers/input/tablet/wacom_wac.h b/drivers/input/tablet/wacom_wac.h
index 53eb71b68330..050acaefee7d 100644
--- a/drivers/input/tablet/wacom_wac.h
+++ b/drivers/input/tablet/wacom_wac.h
@@ -12,7 +12,7 @@
 #include <linux/types.h>
 
 /* maximum packet length for USB devices */
-#define WACOM_PKGLEN_MAX	32
+#define WACOM_PKGLEN_MAX	64
 
 /* packet length for individual models */
 #define WACOM_PKGLEN_PENPRTN	 7
@@ -22,6 +22,8 @@
 #define WACOM_PKGLEN_TPC1FG	 5
 #define WACOM_PKGLEN_TPC2FG	14
 #define WACOM_PKGLEN_BBTOUCH	20
+#define WACOM_PKGLEN_BBTOUCH3	64
+#define WACOM_PKGLEN_BBPEN	10
 
 /* device IDs */
 #define STYLUS_DEVICE_ID	0x02
@@ -57,6 +59,7 @@ enum {
 	INTUOS4S,
 	INTUOS4,
 	INTUOS4L,
+	WACOM_24HD,
 	WACOM_21UX2,
 	CINTIQ,
 	WACOM_BEE,
diff --git a/drivers/input/touchscreen/88pm860x-ts.c b/drivers/input/touchscreen/88pm860x-ts.c
index b3aebc2166ba..05f30b73c3c3 100644
--- a/drivers/input/touchscreen/88pm860x-ts.c
+++ b/drivers/input/touchscreen/88pm860x-ts.c
@@ -217,18 +217,7 @@ static struct platform_driver pm860x_touch_driver = {
 	.probe	= pm860x_touch_probe,
 	.remove	= __devexit_p(pm860x_touch_remove),
 };
-
-static int __init pm860x_touch_init(void)
-{
-	return platform_driver_register(&pm860x_touch_driver);
-}
-module_init(pm860x_touch_init);
-
-static void __exit pm860x_touch_exit(void)
-{
-	platform_driver_unregister(&pm860x_touch_driver);
-}
-module_exit(pm860x_touch_exit);
+module_platform_driver(pm860x_touch_driver);
 
 MODULE_DESCRIPTION("Touchscreen driver for Marvell Semiconductor 88PM860x");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 3488ffe1fa0a..4af2a18eb3ba 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -98,6 +98,19 @@ config TOUCHSCREEN_ATMEL_MXT
 	  To compile this driver as a module, choose M here: the
 	  module will be called atmel_mxt_ts.
 
+config TOUCHSCREEN_AUO_PIXCIR
+	tristate "AUO in-cell touchscreen using Pixcir ICs"
+	depends on I2C
+	depends on GPIOLIB
+	help
+	  Say Y here if you have a AUO display with in-cell touchscreen
+	  using Pixcir ICs.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called auo-pixcir-ts.
+
 config TOUCHSCREEN_BITSY
 	tristate "Compaq iPAQ H3600 (Bitsy) touchscreen"
 	depends on SA1100_BITSY
@@ -177,6 +190,16 @@ config TOUCHSCREEN_EETI
 	  To compile this driver as a module, choose M here: the
 	  module will be called eeti_ts.
 
+config TOUCHSCREEN_EGALAX
+	tristate "EETI eGalax multi-touch panel support"
+	depends on I2C
+	help
+	  Say Y here to enable support for I2C connected EETI
+	  eGalax multi-touch panels.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called egalax_ts.
+
 config TOUCHSCREEN_FUJITSU
 	tristate "Fujitsu serial touchscreen"
 	select SERIO
@@ -435,6 +458,18 @@ config TOUCHSCREEN_UCB1400
 	  To compile this driver as a module, choose M here: the
 	  module will be called ucb1400_ts.
 
+config TOUCHSCREEN_PIXCIR
+	tristate "PIXCIR I2C touchscreens"
+	depends on I2C
+	help
+	  Say Y here if you have a pixcir i2c touchscreen
+	  controller.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called pixcir_i2c_ts.
+
 config TOUCHSCREEN_WM831X
 	tristate "Support for WM831x touchscreen controllers"
 	depends on MFD_WM831X
@@ -541,6 +576,7 @@ config TOUCHSCREEN_USB_COMPOSITE
 	  - GoTop Super_Q2/GogoPen/PenPower tablets
 	  - JASTEC USB Touch Controller/DigiTech DTR-02U
 	  - Zytronic controllers
+	  - Elo TouchSystems 2700 IntelliTouch
 
 	  Have a look at <http://linux.chapter7.ch/touchkit/> for
 	  a usage description and the required user-space stuff.
@@ -620,6 +656,11 @@ config TOUCHSCREEN_USB_JASTEC
 	bool "JASTEC/DigiTech DTR-02U USB touch controller device support" if EXPERT
 	depends on TOUCHSCREEN_USB_COMPOSITE
 
+config TOUCHSCREEN_USB_ELO
+	default y
+	bool "Elo TouchSystems 2700 IntelliTouch controller device support" if EXPERT
+	depends on TOUCHSCREEN_USB_COMPOSITE
+
 config TOUCHSCREEN_USB_E2I
 	default y
 	bool "e2i Touchscreen controller (e.g. from Mimo 740)"
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index f957676035a4..496091e88460 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_TOUCHSCREEN_AD7879_SPI)	+= ad7879-spi.o
 obj-$(CONFIG_TOUCHSCREEN_ADS7846)	+= ads7846.o
 obj-$(CONFIG_TOUCHSCREEN_ATMEL_MXT)	+= atmel_mxt_ts.o
 obj-$(CONFIG_TOUCHSCREEN_ATMEL_TSADCC)	+= atmel_tsadcc.o
+obj-$(CONFIG_TOUCHSCREEN_AUO_PIXCIR)	+= auo-pixcir-ts.o
 obj-$(CONFIG_TOUCHSCREEN_BITSY)		+= h3600_ts_input.o
 obj-$(CONFIG_TOUCHSCREEN_BU21013)       += bu21013_ts.o
 obj-$(CONFIG_TOUCHSCREEN_CY8CTMG110)	+= cy8ctmg110_ts.o
@@ -23,6 +24,7 @@ obj-$(CONFIG_TOUCHSCREEN_HAMPSHIRE)	+= hampshire.o
 obj-$(CONFIG_TOUCHSCREEN_GUNZE)		+= gunze.o
 obj-$(CONFIG_TOUCHSCREEN_EETI)		+= eeti_ts.o
 obj-$(CONFIG_TOUCHSCREEN_ELO)		+= elo.o
+obj-$(CONFIG_TOUCHSCREEN_EGALAX)	+= egalax_ts.o
 obj-$(CONFIG_TOUCHSCREEN_FUJITSU)	+= fujitsu_ts.o
 obj-$(CONFIG_TOUCHSCREEN_INEXIO)	+= inexio.o
 obj-$(CONFIG_TOUCHSCREEN_INTEL_MID)	+= intel-mid-touch.o
@@ -39,6 +41,7 @@ obj-$(CONFIG_TOUCHSCREEN_HTCPEN)	+= htcpen.o
 obj-$(CONFIG_TOUCHSCREEN_USB_COMPOSITE)	+= usbtouchscreen.o
 obj-$(CONFIG_TOUCHSCREEN_PCAP)		+= pcap_ts.o
 obj-$(CONFIG_TOUCHSCREEN_PENMOUNT)	+= penmount.o
+obj-$(CONFIG_TOUCHSCREEN_PIXCIR)	+= pixcir_i2c_ts.o
 obj-$(CONFIG_TOUCHSCREEN_S3C2410)	+= s3c2410_ts.o
 obj-$(CONFIG_TOUCHSCREEN_ST1232)	+= st1232.o
 obj-$(CONFIG_TOUCHSCREEN_STMPE)		+= stmpe-ts.o
diff --git a/drivers/input/touchscreen/ad7877.c b/drivers/input/touchscreen/ad7877.c
index baa43df6502d..49a36df0b752 100644
--- a/drivers/input/touchscreen/ad7877.c
+++ b/drivers/input/touchscreen/ad7877.c
@@ -488,10 +488,10 @@ static ssize_t ad7877_disable_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ad7877 *ts = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned int val;
 	int error;
 
-	error = strict_strtoul(buf, 10, &val);
+	error = kstrtouint(buf, 10, &val);
 	if (error)
 		return error;
 
@@ -518,10 +518,10 @@ static ssize_t ad7877_dac_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ad7877 *ts = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned int val;
 	int error;
 
-	error = strict_strtoul(buf, 10, &val);
+	error = kstrtouint(buf, 10, &val);
 	if (error)
 		return error;
 
@@ -548,10 +548,10 @@ static ssize_t ad7877_gpio3_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ad7877 *ts = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned int val;
 	int error;
 
-	error = strict_strtoul(buf, 10, &val);
+	error = kstrtouint(buf, 10, &val);
 	if (error)
 		return error;
 
@@ -579,10 +579,10 @@ static ssize_t ad7877_gpio4_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ad7877 *ts = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned int val;
 	int error;
 
-	error = strict_strtoul(buf, 10, &val);
+	error = kstrtouint(buf, 10, &val);
 	if (error)
 		return error;
 
@@ -853,7 +853,6 @@ static SIMPLE_DEV_PM_OPS(ad7877_pm, ad7877_suspend, ad7877_resume);
 static struct spi_driver ad7877_driver = {
 	.driver = {
 		.name	= "ad7877",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 		.pm	= &ad7877_pm,
 	},
diff --git a/drivers/input/touchscreen/ad7879-i2c.c b/drivers/input/touchscreen/ad7879-i2c.c
index c789b974c795..0dac6712f42b 100644
--- a/drivers/input/touchscreen/ad7879-i2c.c
+++ b/drivers/input/touchscreen/ad7879-i2c.c
@@ -16,30 +16,6 @@
 
 #define AD7879_DEVID		0x79	/* AD7879-1/AD7889-1 */
 
-#ifdef CONFIG_PM_SLEEP
-static int ad7879_i2c_suspend(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct ad7879 *ts = i2c_get_clientdata(client);
-
-	ad7879_suspend(ts);
-
-	return 0;
-}
-
-static int ad7879_i2c_resume(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct ad7879 *ts = i2c_get_clientdata(client);
-
-	ad7879_resume(ts);
-
-	return 0;
-}
-#endif
-
-static SIMPLE_DEV_PM_OPS(ad7879_i2c_pm, ad7879_i2c_suspend, ad7879_i2c_resume);
-
 /* All registers are word-sized.
  * AD7879 uses a high-byte first convention.
  */
@@ -47,7 +23,7 @@ static int ad7879_i2c_read(struct device *dev, u8 reg)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
-	return swab16(i2c_smbus_read_word_data(client, reg));
+	return i2c_smbus_read_word_swapped(client, reg);
 }
 
 static int ad7879_i2c_multi_read(struct device *dev,
@@ -68,7 +44,7 @@ static int ad7879_i2c_write(struct device *dev, u8 reg, u16 val)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
-	return i2c_smbus_write_word_data(client, reg, swab16(val));
+	return i2c_smbus_write_word_swapped(client, reg, val);
 }
 
 static const struct ad7879_bus_ops ad7879_i2c_bus_ops = {
@@ -119,7 +95,7 @@ static struct i2c_driver ad7879_i2c_driver = {
 	.driver = {
 		.name	= "ad7879",
 		.owner	= THIS_MODULE,
-		.pm	= &ad7879_i2c_pm,
+		.pm	= &ad7879_pm_ops,
 	},
 	.probe		= ad7879_i2c_probe,
 	.remove		= __devexit_p(ad7879_i2c_remove),
@@ -141,4 +117,3 @@ module_exit(ad7879_i2c_exit);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("AD7879(-1) touchscreen I2C bus driver");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("i2c:ad7879");
diff --git a/drivers/input/touchscreen/ad7879-spi.c b/drivers/input/touchscreen/ad7879-spi.c
index b1643c8fa7c9..9b2e1c2b1971 100644
--- a/drivers/input/touchscreen/ad7879-spi.c
+++ b/drivers/input/touchscreen/ad7879-spi.c
@@ -22,30 +22,6 @@
 #define AD7879_WRITECMD(reg) (AD7879_CMD(reg))
 #define AD7879_READCMD(reg)  (AD7879_CMD(reg) | AD7879_CMD_READ)
 
-#ifdef CONFIG_PM_SLEEP
-static int ad7879_spi_suspend(struct device *dev)
-{
-	struct spi_device *spi = to_spi_device(dev);
-	struct ad7879 *ts = spi_get_drvdata(spi);
-
-	ad7879_suspend(ts);
-
-	return 0;
-}
-
-static int ad7879_spi_resume(struct device *dev)
-{
-	struct spi_device *spi = to_spi_device(dev);
-	struct ad7879 *ts = spi_get_drvdata(spi);
-
-	ad7879_resume(ts);
-
-	return 0;
-}
-#endif
-
-static SIMPLE_DEV_PM_OPS(ad7879_spi_pm, ad7879_spi_suspend, ad7879_spi_resume);
-
 /*
  * ad7879_read/write are only used for initial setup and for sysfs controls.
  * The main traffic is done in ad7879_collect().
@@ -174,9 +150,8 @@ static int __devexit ad7879_spi_remove(struct spi_device *spi)
 static struct spi_driver ad7879_spi_driver = {
 	.driver = {
 		.name	= "ad7879",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
-		.pm	= &ad7879_spi_pm,
+		.pm	= &ad7879_pm_ops,
 	},
 	.probe		= ad7879_spi_probe,
 	.remove		= __devexit_p(ad7879_spi_remove),
diff --git a/drivers/input/touchscreen/ad7879.c b/drivers/input/touchscreen/ad7879.c
index 3b2e9ed2aeec..e2482b40da51 100644
--- a/drivers/input/touchscreen/ad7879.c
+++ b/drivers/input/touchscreen/ad7879.c
@@ -281,8 +281,11 @@ static void ad7879_close(struct input_dev* input)
 		__ad7879_disable(ts);
 }
 
-void ad7879_suspend(struct ad7879 *ts)
+#ifdef CONFIG_PM_SLEEP
+static int ad7879_suspend(struct device *dev)
 {
+	struct ad7879 *ts = dev_get_drvdata(dev);
+
 	mutex_lock(&ts->input->mutex);
 
 	if (!ts->suspended && !ts->disabled && ts->input->users)
@@ -291,11 +294,14 @@ void ad7879_suspend(struct ad7879 *ts)
 	ts->suspended = true;
 
 	mutex_unlock(&ts->input->mutex);
+
+	return 0;
 }
-EXPORT_SYMBOL(ad7879_suspend);
 
-void ad7879_resume(struct ad7879 *ts)
+static int ad7879_resume(struct device *dev)
 {
+	struct ad7879 *ts = dev_get_drvdata(dev);
+
 	mutex_lock(&ts->input->mutex);
 
 	if (ts->suspended && !ts->disabled && ts->input->users)
@@ -304,8 +310,13 @@ void ad7879_resume(struct ad7879 *ts)
 	ts->suspended = false;
 
 	mutex_unlock(&ts->input->mutex);
+
+	return 0;
 }
-EXPORT_SYMBOL(ad7879_resume);
+#endif
+
+SIMPLE_DEV_PM_OPS(ad7879_pm_ops, ad7879_suspend, ad7879_resume);
+EXPORT_SYMBOL(ad7879_pm_ops);
 
 static void ad7879_toggle(struct ad7879 *ts, bool disable)
 {
@@ -340,10 +351,10 @@ static ssize_t ad7879_disable_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ad7879 *ts = dev_get_drvdata(dev);
-	unsigned long val;
+	unsigned int val;
 	int error;
 
-	error = strict_strtoul(buf, 10, &val);
+	error = kstrtouint(buf, 10, &val);
 	if (error)
 		return error;
 
diff --git a/drivers/input/touchscreen/ad7879.h b/drivers/input/touchscreen/ad7879.h
index 6b45a27236c7..6fd13c48d373 100644
--- a/drivers/input/touchscreen/ad7879.h
+++ b/drivers/input/touchscreen/ad7879.h
@@ -21,8 +21,8 @@ struct ad7879_bus_ops {
 	int (*write)(struct device *dev, u8 reg, u16 val);
 };
 
-void ad7879_suspend(struct ad7879 *);
-void ad7879_resume(struct ad7879 *);
+extern const struct dev_pm_ops ad7879_pm_ops;
+
 struct ad7879 *ad7879_probe(struct device *dev, u8 devid, unsigned irq,
 			    const struct ad7879_bus_ops *bops);
 void ad7879_remove(struct ad7879 *);
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index de31ec6fe9e4..23fd90185659 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -602,10 +602,12 @@ static ssize_t ads7846_disable_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
-	unsigned long i;
+	unsigned int i;
+	int err;
 
-	if (strict_strtoul(buf, 10, &i))
-		return -EINVAL;
+	err = kstrtouint(buf, 10, &i);
+	if (err)
+		return err;
 
 	if (i)
 		ads7846_disable(ts);
@@ -1424,7 +1426,6 @@ static int __devexit ads7846_remove(struct spi_device *spi)
 static struct spi_driver ads7846_driver = {
 	.driver = {
 		.name	= "ads7846",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 		.pm	= &ads7846_pm,
 	},
diff --git a/drivers/input/touchscreen/atmel-wm97xx.c b/drivers/input/touchscreen/atmel-wm97xx.c
index 8034cbb20f74..d016cb26d125 100644
--- a/drivers/input/touchscreen/atmel-wm97xx.c
+++ b/drivers/input/touchscreen/atmel-wm97xx.c
@@ -429,18 +429,7 @@ static struct platform_driver atmel_wm97xx_driver = {
 	.suspend	= atmel_wm97xx_suspend,
 	.resume		= atmel_wm97xx_resume,
 };
-
-static int __init atmel_wm97xx_init(void)
-{
-	return platform_driver_probe(&atmel_wm97xx_driver, atmel_wm97xx_probe);
-}
-module_init(atmel_wm97xx_init);
-
-static void __exit atmel_wm97xx_exit(void)
-{
-	platform_driver_unregister(&atmel_wm97xx_driver);
-}
-module_exit(atmel_wm97xx_exit);
+module_platform_driver(atmel_wm97xx_driver);
 
 MODULE_AUTHOR("Hans-Christian Egtvedt <egtvedt@samfundet.no>");
 MODULE_DESCRIPTION("wm97xx continuous touch driver for Atmel AT91 and AVR32");
diff --git a/drivers/input/touchscreen/atmel_tsadcc.c b/drivers/input/touchscreen/atmel_tsadcc.c
index 122a87883659..201b2d2ec1b3 100644
--- a/drivers/input/touchscreen/atmel_tsadcc.c
+++ b/drivers/input/touchscreen/atmel_tsadcc.c
@@ -351,20 +351,7 @@ static struct platform_driver atmel_tsadcc_driver = {
 		.name	= "atmel_tsadcc",
 	},
 };
-
-static int __init atmel_tsadcc_init(void)
-{
-	return platform_driver_register(&atmel_tsadcc_driver);
-}
-
-static void __exit atmel_tsadcc_exit(void)
-{
-	platform_driver_unregister(&atmel_tsadcc_driver);
-}
-
-module_init(atmel_tsadcc_init);
-module_exit(atmel_tsadcc_exit);
-
+module_platform_driver(atmel_tsadcc_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Atmel TouchScreen Driver");
diff --git a/drivers/input/touchscreen/auo-pixcir-ts.c b/drivers/input/touchscreen/auo-pixcir-ts.c
new file mode 100644
index 000000000000..94fb9fbb08a9
--- /dev/null
+++ b/drivers/input/touchscreen/auo-pixcir-ts.c
@@ -0,0 +1,652 @@
+/*
+ * Driver for AUO in-cell touchscreens
+ *
+ * Copyright (c) 2011 Heiko Stuebner <heiko@sntech.de>
+ *
+ * loosely based on auo_touch.c from Dell Streak vendor-kernel
+ *
+ * Copyright (c) 2008 QUALCOMM Incorporated.
+ * Copyright (c) 2008 QUALCOMM USA, INC.
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/jiffies.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/input/auo-pixcir-ts.h>
+
+/*
+ * Coordinate calculation:
+ * X1 = X1_LSB + X1_MSB*256
+ * Y1 = Y1_LSB + Y1_MSB*256
+ * X2 = X2_LSB + X2_MSB*256
+ * Y2 = Y2_LSB + Y2_MSB*256
+ */
+#define AUO_PIXCIR_REG_X1_LSB		0x00
+#define AUO_PIXCIR_REG_X1_MSB		0x01
+#define AUO_PIXCIR_REG_Y1_LSB		0x02
+#define AUO_PIXCIR_REG_Y1_MSB		0x03
+#define AUO_PIXCIR_REG_X2_LSB		0x04
+#define AUO_PIXCIR_REG_X2_MSB		0x05
+#define AUO_PIXCIR_REG_Y2_LSB		0x06
+#define AUO_PIXCIR_REG_Y2_MSB		0x07
+
+#define AUO_PIXCIR_REG_STRENGTH		0x0d
+#define AUO_PIXCIR_REG_STRENGTH_X1_LSB	0x0e
+#define AUO_PIXCIR_REG_STRENGTH_X1_MSB	0x0f
+
+#define AUO_PIXCIR_REG_RAW_DATA_X	0x2b
+#define AUO_PIXCIR_REG_RAW_DATA_Y	0x4f
+
+#define AUO_PIXCIR_REG_X_SENSITIVITY	0x6f
+#define AUO_PIXCIR_REG_Y_SENSITIVITY	0x70
+#define AUO_PIXCIR_REG_INT_SETTING	0x71
+#define AUO_PIXCIR_REG_INT_WIDTH	0x72
+#define AUO_PIXCIR_REG_POWER_MODE	0x73
+
+#define AUO_PIXCIR_REG_VERSION		0x77
+#define AUO_PIXCIR_REG_CALIBRATE	0x78
+
+#define AUO_PIXCIR_REG_TOUCHAREA_X1	0x1e
+#define AUO_PIXCIR_REG_TOUCHAREA_Y1	0x1f
+#define AUO_PIXCIR_REG_TOUCHAREA_X2	0x20
+#define AUO_PIXCIR_REG_TOUCHAREA_Y2	0x21
+
+#define AUO_PIXCIR_REG_EEPROM_CALIB_X	0x42
+#define AUO_PIXCIR_REG_EEPROM_CALIB_Y	0xad
+
+#define AUO_PIXCIR_INT_TPNUM_MASK	0xe0
+#define AUO_PIXCIR_INT_TPNUM_SHIFT	5
+#define AUO_PIXCIR_INT_RELEASE		(1 << 4)
+#define AUO_PIXCIR_INT_ENABLE		(1 << 3)
+#define AUO_PIXCIR_INT_POL_HIGH		(1 << 2)
+#define AUO_PIXCIR_INT_MODE_MASK	0x03
+
+/*
+ * Power modes:
+ * active:	scan speed 60Hz
+ * sleep:	scan speed 10Hz can be auto-activated, wakeup on 1st touch
+ * deep sleep:	scan speed 1Hz can only be entered or left manually.
+ */
+#define AUO_PIXCIR_POWER_ACTIVE		0x00
+#define AUO_PIXCIR_POWER_SLEEP		0x01
+#define AUO_PIXCIR_POWER_DEEP_SLEEP	0x02
+#define AUO_PIXCIR_POWER_MASK		0x03
+
+#define AUO_PIXCIR_POWER_ALLOW_SLEEP	(1 << 2)
+#define AUO_PIXCIR_POWER_IDLE_TIME(ms)	((ms & 0xf) << 4)
+
+#define AUO_PIXCIR_CALIBRATE		0x03
+
+#define AUO_PIXCIR_EEPROM_CALIB_X_LEN	62
+#define AUO_PIXCIR_EEPROM_CALIB_Y_LEN	36
+
+#define AUO_PIXCIR_RAW_DATA_X_LEN	18
+#define AUO_PIXCIR_RAW_DATA_Y_LEN	11
+
+#define AUO_PIXCIR_STRENGTH_ENABLE	(1 << 0)
+
+/* Touchscreen absolute values */
+#define AUO_PIXCIR_REPORT_POINTS	2
+#define AUO_PIXCIR_MAX_AREA		0xff
+#define AUO_PIXCIR_PENUP_TIMEOUT_MS	10
+
+struct auo_pixcir_ts {
+	struct i2c_client	*client;
+	struct input_dev	*input;
+	char			phys[32];
+
+	/* special handling for touch_indicate interupt mode */
+	bool			touch_ind_mode;
+
+	wait_queue_head_t	wait;
+	bool			stopped;
+};
+
+struct auo_point_t {
+	int	coord_x;
+	int	coord_y;
+	int	area_major;
+	int	area_minor;
+	int	orientation;
+};
+
+static int auo_pixcir_collect_data(struct auo_pixcir_ts *ts,
+				   struct auo_point_t *point)
+{
+	struct i2c_client *client = ts->client;
+	const struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	uint8_t raw_coord[8];
+	uint8_t raw_area[4];
+	int i, ret;
+
+	/* touch coordinates */
+	ret = i2c_smbus_read_i2c_block_data(client, AUO_PIXCIR_REG_X1_LSB,
+					    8, raw_coord);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to read coordinate, %d\n", ret);
+		return ret;
+	}
+
+	/* touch area */
+	ret = i2c_smbus_read_i2c_block_data(client, AUO_PIXCIR_REG_TOUCHAREA_X1,
+					    4, raw_area);
+	if (ret < 0) {
+		dev_err(&client->dev, "could not read touch area, %d\n", ret);
+		return ret;
+	}
+
+	for (i = 0; i < AUO_PIXCIR_REPORT_POINTS; i++) {
+		point[i].coord_x =
+			raw_coord[4 * i + 1] << 8 | raw_coord[4 * i];
+		point[i].coord_y =
+			raw_coord[4 * i + 3] << 8 | raw_coord[4 * i + 2];
+
+		if (point[i].coord_x > pdata->x_max ||
+		    point[i].coord_y > pdata->y_max) {
+			dev_warn(&client->dev, "coordinates (%d,%d) invalid\n",
+				point[i].coord_x, point[i].coord_y);
+			point[i].coord_x = point[i].coord_y = 0;
+		}
+
+		/* determine touch major, minor and orientation */
+		point[i].area_major = max(raw_area[2 * i], raw_area[2 * i + 1]);
+		point[i].area_minor = min(raw_area[2 * i], raw_area[2 * i + 1]);
+		point[i].orientation = raw_area[2 * i] > raw_area[2 * i + 1];
+	}
+
+	return 0;
+}
+
+static irqreturn_t auo_pixcir_interrupt(int irq, void *dev_id)
+{
+	struct auo_pixcir_ts *ts = dev_id;
+	struct i2c_client *client = ts->client;
+	const struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	struct auo_point_t point[AUO_PIXCIR_REPORT_POINTS];
+	int i;
+	int ret;
+	int fingers = 0;
+	int abs = -1;
+
+	while (!ts->stopped) {
+
+		/* check for up event in touch touch_ind_mode */
+		if (ts->touch_ind_mode) {
+			if (gpio_get_value(pdata->gpio_int) == 0) {
+				input_mt_sync(ts->input);
+				input_report_key(ts->input, BTN_TOUCH, 0);
+				input_sync(ts->input);
+				break;
+			}
+		}
+
+		ret = auo_pixcir_collect_data(ts, point);
+		if (ret < 0) {
+			/* we want to loop only in touch_ind_mode */
+			if (!ts->touch_ind_mode)
+				break;
+
+			wait_event_timeout(ts->wait, ts->stopped,
+				msecs_to_jiffies(AUO_PIXCIR_PENUP_TIMEOUT_MS));
+			continue;
+		}
+
+		for (i = 0; i < AUO_PIXCIR_REPORT_POINTS; i++) {
+			if (point[i].coord_x > 0 || point[i].coord_y > 0) {
+				input_report_abs(ts->input, ABS_MT_POSITION_X,
+						 point[i].coord_x);
+				input_report_abs(ts->input, ABS_MT_POSITION_Y,
+						 point[i].coord_y);
+				input_report_abs(ts->input, ABS_MT_TOUCH_MAJOR,
+						 point[i].area_major);
+				input_report_abs(ts->input, ABS_MT_TOUCH_MINOR,
+						 point[i].area_minor);
+				input_report_abs(ts->input, ABS_MT_ORIENTATION,
+						 point[i].orientation);
+				input_mt_sync(ts->input);
+
+				/* use first finger as source for singletouch */
+				if (fingers == 0)
+					abs = i;
+
+				/* number of touch points could also be queried
+				 * via i2c but would require an additional call
+				 */
+				fingers++;
+			}
+		}
+
+		input_report_key(ts->input, BTN_TOUCH, fingers > 0);
+
+		if (abs > -1) {
+			input_report_abs(ts->input, ABS_X, point[abs].coord_x);
+			input_report_abs(ts->input, ABS_Y, point[abs].coord_y);
+		}
+
+		input_sync(ts->input);
+
+		/* we want to loop only in touch_ind_mode */
+		if (!ts->touch_ind_mode)
+			break;
+
+		wait_event_timeout(ts->wait, ts->stopped,
+				 msecs_to_jiffies(AUO_PIXCIR_PENUP_TIMEOUT_MS));
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Set the power mode of the device.
+ * Valid modes are
+ * - AUO_PIXCIR_POWER_ACTIVE
+ * - AUO_PIXCIR_POWER_SLEEP - automatically left on first touch
+ * - AUO_PIXCIR_POWER_DEEP_SLEEP
+ */
+static int auo_pixcir_power_mode(struct auo_pixcir_ts *ts, int mode)
+{
+	struct i2c_client *client = ts->client;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, AUO_PIXCIR_REG_POWER_MODE);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to read reg %Xh, %d\n",
+			AUO_PIXCIR_REG_POWER_MODE, ret);
+		return ret;
+	}
+
+	ret &= ~AUO_PIXCIR_POWER_MASK;
+	ret |= mode;
+
+	ret = i2c_smbus_write_byte_data(client, AUO_PIXCIR_REG_POWER_MODE, ret);
+	if (ret) {
+		dev_err(&client->dev, "unable to write reg %Xh, %d\n",
+			AUO_PIXCIR_REG_POWER_MODE, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static __devinit int auo_pixcir_int_config(struct auo_pixcir_ts *ts,
+					   int int_setting)
+{
+	struct i2c_client *client = ts->client;
+	struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, AUO_PIXCIR_REG_INT_SETTING);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to read reg %Xh, %d\n",
+			AUO_PIXCIR_REG_INT_SETTING, ret);
+		return ret;
+	}
+
+	ret &= ~AUO_PIXCIR_INT_MODE_MASK;
+	ret |= int_setting;
+	ret |= AUO_PIXCIR_INT_POL_HIGH; /* always use high for interrupts */
+
+	ret = i2c_smbus_write_byte_data(client, AUO_PIXCIR_REG_INT_SETTING,
+					ret);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to write reg %Xh, %d\n",
+			AUO_PIXCIR_REG_INT_SETTING, ret);
+		return ret;
+	}
+
+	ts->touch_ind_mode = pdata->int_setting == AUO_PIXCIR_INT_TOUCH_IND;
+
+	return 0;
+}
+
+/* control the generation of interrupts on the device side */
+static int auo_pixcir_int_toggle(struct auo_pixcir_ts *ts, bool enable)
+{
+	struct i2c_client *client = ts->client;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, AUO_PIXCIR_REG_INT_SETTING);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to read reg %Xh, %d\n",
+			AUO_PIXCIR_REG_INT_SETTING, ret);
+		return ret;
+	}
+
+	if (enable)
+		ret |= AUO_PIXCIR_INT_ENABLE;
+	else
+		ret &= ~AUO_PIXCIR_INT_ENABLE;
+
+	ret = i2c_smbus_write_byte_data(client, AUO_PIXCIR_REG_INT_SETTING,
+					ret);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to write reg %Xh, %d\n",
+			AUO_PIXCIR_REG_INT_SETTING, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int auo_pixcir_start(struct auo_pixcir_ts *ts)
+{
+	struct i2c_client *client = ts->client;
+	int ret;
+
+	ret = auo_pixcir_power_mode(ts, AUO_PIXCIR_POWER_ACTIVE);
+	if (ret < 0) {
+		dev_err(&client->dev, "could not set power mode, %d\n",
+			ret);
+		return ret;
+	}
+
+	ts->stopped = false;
+	mb();
+	enable_irq(client->irq);
+
+	ret = auo_pixcir_int_toggle(ts, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "could not enable interrupt, %d\n",
+			ret);
+		disable_irq(client->irq);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int auo_pixcir_stop(struct auo_pixcir_ts *ts)
+{
+	struct i2c_client *client = ts->client;
+	int ret;
+
+	ret = auo_pixcir_int_toggle(ts, 0);
+	if (ret < 0) {
+		dev_err(&client->dev, "could not disable interrupt, %d\n",
+			ret);
+		return ret;
+	}
+
+	/* disable receiving of interrupts */
+	disable_irq(client->irq);
+	ts->stopped = true;
+	mb();
+	wake_up(&ts->wait);
+
+	return auo_pixcir_power_mode(ts, AUO_PIXCIR_POWER_DEEP_SLEEP);
+}
+
+static int auo_pixcir_input_open(struct input_dev *dev)
+{
+	struct auo_pixcir_ts *ts = input_get_drvdata(dev);
+	int ret;
+
+	ret = auo_pixcir_start(ts);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void auo_pixcir_input_close(struct input_dev *dev)
+{
+	struct auo_pixcir_ts *ts = input_get_drvdata(dev);
+
+	auo_pixcir_stop(ts);
+
+	return;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int auo_pixcir_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct auo_pixcir_ts *ts = i2c_get_clientdata(client);
+	struct input_dev *input = ts->input;
+	int ret = 0;
+
+	mutex_lock(&input->mutex);
+
+	/* when configured as wakeup source, device should always wake system
+	 * therefore start device if necessary
+	 */
+	if (device_may_wakeup(&client->dev)) {
+		/* need to start device if not open, to be wakeup source */
+		if (!input->users) {
+			ret = auo_pixcir_start(ts);
+			if (ret)
+				goto unlock;
+		}
+
+		enable_irq_wake(client->irq);
+		ret = auo_pixcir_power_mode(ts, AUO_PIXCIR_POWER_SLEEP);
+	} else if (input->users) {
+		ret = auo_pixcir_stop(ts);
+	}
+
+unlock:
+	mutex_unlock(&input->mutex);
+
+	return ret;
+}
+
+static int auo_pixcir_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct auo_pixcir_ts *ts = i2c_get_clientdata(client);
+	struct input_dev *input = ts->input;
+	int ret = 0;
+
+	mutex_lock(&input->mutex);
+
+	if (device_may_wakeup(&client->dev)) {
+		disable_irq_wake(client->irq);
+
+		/* need to stop device if it was not open on suspend */
+		if (!input->users) {
+			ret = auo_pixcir_stop(ts);
+			if (ret)
+				goto unlock;
+		}
+
+		/* device wakes automatically from SLEEP */
+	} else if (input->users) {
+		ret = auo_pixcir_start(ts);
+	}
+
+unlock:
+	mutex_unlock(&input->mutex);
+
+	return ret;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(auo_pixcir_pm_ops, auo_pixcir_suspend,
+			 auo_pixcir_resume);
+
+static int __devinit auo_pixcir_probe(struct i2c_client *client,
+				      const struct i2c_device_id *id)
+{
+	const struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+	struct auo_pixcir_ts *ts;
+	struct input_dev *input_dev;
+	int ret;
+
+	if (!pdata)
+		return -EINVAL;
+
+	ts = kzalloc(sizeof(struct auo_pixcir_ts), GFP_KERNEL);
+	if (!ts)
+		return -ENOMEM;
+
+	ret = gpio_request(pdata->gpio_int, "auo_pixcir_ts_int");
+	if (ret) {
+		dev_err(&client->dev, "request of gpio %d failed, %d\n",
+			pdata->gpio_int, ret);
+		goto err_gpio_int;
+	}
+
+	if (pdata->init_hw)
+		pdata->init_hw(client);
+
+	ts->client = client;
+	ts->touch_ind_mode = 0;
+	init_waitqueue_head(&ts->wait);
+
+	snprintf(ts->phys, sizeof(ts->phys),
+		 "%s/input0", dev_name(&client->dev));
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(&client->dev, "could not allocate input device\n");
+		goto err_input_alloc;
+	}
+
+	ts->input = input_dev;
+
+	input_dev->name = "AUO-Pixcir touchscreen";
+	input_dev->phys = ts->phys;
+	input_dev->id.bustype = BUS_I2C;
+	input_dev->dev.parent = &client->dev;
+
+	input_dev->open = auo_pixcir_input_open;
+	input_dev->close = auo_pixcir_input_close;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(EV_KEY, input_dev->evbit);
+
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	/* For single touch */
+	input_set_abs_params(input_dev, ABS_X, 0, pdata->x_max, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, pdata->y_max, 0, 0);
+
+	/* For multi touch */
+	input_set_abs_params(input_dev, ABS_MT_POSITION_X, 0,
+			     pdata->x_max, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_Y, 0,
+			     pdata->y_max, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR, 0,
+			     AUO_PIXCIR_MAX_AREA, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_TOUCH_MINOR, 0,
+			     AUO_PIXCIR_MAX_AREA, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_ORIENTATION, 0, 1, 0, 0);
+
+	ret = i2c_smbus_read_byte_data(client, AUO_PIXCIR_REG_VERSION);
+	if (ret < 0)
+		goto err_fw_vers;
+	dev_info(&client->dev, "firmware version 0x%X\n", ret);
+
+	ret = auo_pixcir_int_config(ts, pdata->int_setting);
+	if (ret)
+		goto err_fw_vers;
+
+	input_set_drvdata(ts->input, ts);
+	ts->stopped = true;
+
+	ret = request_threaded_irq(client->irq, NULL, auo_pixcir_interrupt,
+				   IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+				   input_dev->name, ts);
+	if (ret) {
+		dev_err(&client->dev, "irq %d requested failed\n", client->irq);
+		goto err_fw_vers;
+	}
+
+	/* stop device and put it into deep sleep until it is opened */
+	ret = auo_pixcir_stop(ts);
+	if (ret < 0)
+		goto err_input_register;
+
+	ret = input_register_device(input_dev);
+	if (ret) {
+		dev_err(&client->dev, "could not register input device\n");
+		goto err_input_register;
+	}
+
+	i2c_set_clientdata(client, ts);
+
+	return 0;
+
+err_input_register:
+	free_irq(client->irq, ts);
+err_fw_vers:
+	input_free_device(input_dev);
+err_input_alloc:
+	if (pdata->exit_hw)
+		pdata->exit_hw(client);
+	gpio_free(pdata->gpio_int);
+err_gpio_int:
+	kfree(ts);
+
+	return ret;
+}
+
+static int __devexit auo_pixcir_remove(struct i2c_client *client)
+{
+	struct auo_pixcir_ts *ts = i2c_get_clientdata(client);
+	const struct auo_pixcir_ts_platdata *pdata = client->dev.platform_data;
+
+	free_irq(client->irq, ts);
+
+	input_unregister_device(ts->input);
+
+	if (pdata->exit_hw)
+		pdata->exit_hw(client);
+
+	gpio_free(pdata->gpio_int);
+
+	kfree(ts);
+
+	return 0;
+}
+
+static const struct i2c_device_id auo_pixcir_idtable[] = {
+	{ "auo_pixcir_ts", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, auo_pixcir_idtable);
+
+static struct i2c_driver auo_pixcir_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "auo_pixcir_ts",
+		.pm	= &auo_pixcir_pm_ops,
+	},
+	.probe		= auo_pixcir_probe,
+	.remove		= __devexit_p(auo_pixcir_remove),
+	.id_table	= auo_pixcir_idtable,
+};
+
+static int __init auo_pixcir_init(void)
+{
+	return i2c_add_driver(&auo_pixcir_driver);
+}
+module_init(auo_pixcir_init);
+
+static void __exit auo_pixcir_exit(void)
+{
+	i2c_del_driver(&auo_pixcir_driver);
+}
+module_exit(auo_pixcir_exit);
+
+MODULE_DESCRIPTION("AUO-PIXCIR touchscreen driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Heiko Stuebner <heiko@sntech.de>");
diff --git a/drivers/input/touchscreen/da9034-ts.c b/drivers/input/touchscreen/da9034-ts.c
index 2b72a5923c16..36b65cf10d7f 100644
--- a/drivers/input/touchscreen/da9034-ts.c
+++ b/drivers/input/touchscreen/da9034-ts.c
@@ -379,18 +379,7 @@ static struct platform_driver da9034_touch_driver = {
 	.probe		= da9034_touch_probe,
 	.remove		= __devexit_p(da9034_touch_remove),
 };
-
-static int __init da9034_touch_init(void)
-{
-	return platform_driver_register(&da9034_touch_driver);
-}
-module_init(da9034_touch_init);
-
-static void __exit da9034_touch_exit(void)
-{
-	platform_driver_unregister(&da9034_touch_driver);
-}
-module_exit(da9034_touch_exit);
+module_platform_driver(da9034_touch_driver);
 
 MODULE_DESCRIPTION("Touchscreen driver for Dialog Semiconductor DA9034");
 MODULE_AUTHOR("Eric Miao <eric.miao@marvell.com>, Bin Yang <bin.yang@marvell.com>");
diff --git a/drivers/input/touchscreen/egalax_ts.c b/drivers/input/touchscreen/egalax_ts.c
new file mode 100644
index 000000000000..eadcc2e83c77
--- /dev/null
+++ b/drivers/input/touchscreen/egalax_ts.c
@@ -0,0 +1,303 @@
+/*
+ * Driver for EETI eGalax Multiple Touch Controller
+ *
+ * Copyright (C) 2011 Freescale Semiconductor, Inc.
+ *
+ * based on max11801_ts.c
+ *
+ * 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.
+ */
+
+/* EETI eGalax serial touch screen controller is a I2C based multiple
+ * touch screen controller, it supports 5 point multiple touch. */
+
+/* TODO:
+  - auto idle mode support
+*/
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/input.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/bitops.h>
+#include <linux/input/mt.h>
+
+/*
+ * Mouse Mode: some panel may configure the controller to mouse mode,
+ * which can only report one point at a given time.
+ * This driver will ignore events in this mode.
+ */
+#define REPORT_MODE_MOUSE		0x1
+/*
+ * Vendor Mode: this mode is used to transfer some vendor specific
+ * messages.
+ * This driver will ignore events in this mode.
+ */
+#define REPORT_MODE_VENDOR		0x3
+/* Multiple Touch Mode */
+#define REPORT_MODE_MTTOUCH		0x4
+
+#define MAX_SUPPORT_POINTS		5
+
+#define EVENT_VALID_OFFSET	7
+#define EVENT_VALID_MASK	(0x1 << EVENT_VALID_OFFSET)
+#define EVENT_ID_OFFSET		2
+#define EVENT_ID_MASK		(0xf << EVENT_ID_OFFSET)
+#define EVENT_IN_RANGE		(0x1 << 1)
+#define EVENT_DOWN_UP		(0X1 << 0)
+
+#define MAX_I2C_DATA_LEN	10
+
+#define EGALAX_MAX_X	32760
+#define EGALAX_MAX_Y	32760
+#define EGALAX_MAX_TRIES 100
+
+struct egalax_ts {
+	struct i2c_client		*client;
+	struct input_dev		*input_dev;
+};
+
+static irqreturn_t egalax_ts_interrupt(int irq, void *dev_id)
+{
+	struct egalax_ts *ts = dev_id;
+	struct input_dev *input_dev = ts->input_dev;
+	struct i2c_client *client = ts->client;
+	u8 buf[MAX_I2C_DATA_LEN];
+	int id, ret, x, y, z;
+	int tries = 0;
+	bool down, valid;
+	u8 state;
+
+	do {
+		ret = i2c_master_recv(client, buf, MAX_I2C_DATA_LEN);
+	} while (ret == -EAGAIN && tries++ < EGALAX_MAX_TRIES);
+
+	if (ret < 0)
+		return IRQ_HANDLED;
+
+	if (buf[0] != REPORT_MODE_MTTOUCH) {
+		/* ignore mouse events and vendor events */
+		return IRQ_HANDLED;
+	}
+
+	state = buf[1];
+	x = (buf[3] << 8) | buf[2];
+	y = (buf[5] << 8) | buf[4];
+	z = (buf[7] << 8) | buf[6];
+
+	valid = state & EVENT_VALID_MASK;
+	id = (state & EVENT_ID_MASK) >> EVENT_ID_OFFSET;
+	down = state & EVENT_DOWN_UP;
+
+	if (!valid || id > MAX_SUPPORT_POINTS) {
+		dev_dbg(&client->dev, "point invalid\n");
+		return IRQ_HANDLED;
+	}
+
+	input_mt_slot(input_dev, id);
+	input_mt_report_slot_state(input_dev, MT_TOOL_FINGER, down);
+
+	dev_dbg(&client->dev, "%s id:%d x:%d y:%d z:%d",
+		down ? "down" : "up", id, x, y, z);
+
+	if (down) {
+		input_report_abs(input_dev, ABS_MT_POSITION_X, x);
+		input_report_abs(input_dev, ABS_MT_POSITION_Y, y);
+		input_report_abs(input_dev, ABS_MT_PRESSURE, z);
+	}
+
+	input_mt_report_pointer_emulation(input_dev, true);
+	input_sync(input_dev);
+
+	return IRQ_HANDLED;
+}
+
+/* wake up controller by an falling edge of interrupt gpio.  */
+static int egalax_wake_up_device(struct i2c_client *client)
+{
+	int gpio = irq_to_gpio(client->irq);
+	int ret;
+
+	ret = gpio_request(gpio, "egalax_irq");
+	if (ret < 0) {
+		dev_err(&client->dev,
+			"request gpio failed, cannot wake up controller: %d\n",
+			ret);
+		return ret;
+	}
+
+	/* wake up controller via an falling edge on IRQ gpio. */
+	gpio_direction_output(gpio, 0);
+	gpio_set_value(gpio, 1);
+
+	/* controller should be waken up, return irq.  */
+	gpio_direction_input(gpio);
+	gpio_free(gpio);
+
+	return 0;
+}
+
+static int __devinit egalax_firmware_version(struct i2c_client *client)
+{
+	static const u8 cmd[MAX_I2C_DATA_LEN] = { 0x03, 0x03, 0xa, 0x01, 0x41 };
+	int ret;
+
+	ret = i2c_master_send(client, cmd, MAX_I2C_DATA_LEN);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int __devinit egalax_ts_probe(struct i2c_client *client,
+				       const struct i2c_device_id *id)
+{
+	struct egalax_ts *ts;
+	struct input_dev *input_dev;
+	int ret;
+	int error;
+
+	ts = kzalloc(sizeof(struct egalax_ts), GFP_KERNEL);
+	if (!ts) {
+		dev_err(&client->dev, "Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(&client->dev, "Failed to allocate memory\n");
+		error = -ENOMEM;
+		goto err_free_ts;
+	}
+
+	ts->client = client;
+	ts->input_dev = input_dev;
+
+	/* controller may be in sleep, wake it up. */
+	egalax_wake_up_device(client);
+
+	ret = egalax_firmware_version(client);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed to read firmware version\n");
+		error = -EIO;
+		goto err_free_dev;
+	}
+
+	input_dev->name = "EETI eGalax Touch Screen";
+	input_dev->id.bustype = BUS_I2C;
+	input_dev->dev.parent = &client->dev;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	input_set_abs_params(input_dev, ABS_X, 0, EGALAX_MAX_X, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, EGALAX_MAX_Y, 0, 0);
+	input_set_abs_params(input_dev,
+			     ABS_MT_POSITION_X, 0, EGALAX_MAX_X, 0, 0);
+	input_set_abs_params(input_dev,
+			     ABS_MT_POSITION_X, 0, EGALAX_MAX_Y, 0, 0);
+	input_mt_init_slots(input_dev, MAX_SUPPORT_POINTS);
+
+	input_set_drvdata(input_dev, ts);
+
+	error = request_threaded_irq(client->irq, NULL, egalax_ts_interrupt,
+				     IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+				     "egalax_ts", ts);
+	if (error < 0) {
+		dev_err(&client->dev, "Failed to register interrupt\n");
+		goto err_free_dev;
+	}
+
+	error = input_register_device(ts->input_dev);
+	if (error)
+		goto err_free_irq;
+
+	i2c_set_clientdata(client, ts);
+	return 0;
+
+err_free_irq:
+	free_irq(client->irq, ts);
+err_free_dev:
+	input_free_device(input_dev);
+err_free_ts:
+	kfree(ts);
+
+	return error;
+}
+
+static __devexit int egalax_ts_remove(struct i2c_client *client)
+{
+	struct egalax_ts *ts = i2c_get_clientdata(client);
+
+	free_irq(client->irq, ts);
+
+	input_unregister_device(ts->input_dev);
+	kfree(ts);
+
+	return 0;
+}
+
+static const struct i2c_device_id egalax_ts_id[] = {
+	{ "egalax_ts", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, egalax_ts_id);
+
+#ifdef CONFIG_PM_SLEEP
+static int egalax_ts_suspend(struct device *dev)
+{
+	static const u8 suspend_cmd[MAX_I2C_DATA_LEN] = {
+		0x3, 0x6, 0xa, 0x3, 0x36, 0x3f, 0x2, 0, 0, 0
+	};
+	struct i2c_client *client = to_i2c_client(dev);
+	int ret;
+
+	ret = i2c_master_send(client, suspend_cmd, MAX_I2C_DATA_LEN);
+	return ret > 0 ? 0 : ret;
+}
+
+static int egalax_ts_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	return egalax_wake_up_device(client);
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(egalax_ts_pm_ops, egalax_ts_suspend, egalax_ts_resume);
+
+static struct i2c_driver egalax_ts_driver = {
+	.driver = {
+		.name	= "egalax_ts",
+		.owner	= THIS_MODULE,
+		.pm	= &egalax_ts_pm_ops,
+	},
+	.id_table	= egalax_ts_id,
+	.probe		= egalax_ts_probe,
+	.remove		= __devexit_p(egalax_ts_remove),
+};
+
+static int __init egalax_ts_init(void)
+{
+	return i2c_add_driver(&egalax_ts_driver);
+}
+
+static void __exit egalax_ts_exit(void)
+{
+	i2c_del_driver(&egalax_ts_driver);
+}
+
+module_init(egalax_ts_init);
+module_exit(egalax_ts_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Touchscreen driver for EETI eGalax touch controller");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/htcpen.c b/drivers/input/touchscreen/htcpen.c
index 62811de6f18f..81e338623944 100644
--- a/drivers/input/touchscreen/htcpen.c
+++ b/drivers/input/touchscreen/htcpen.c
@@ -47,12 +47,6 @@ static int invert_y;
 module_param(invert_y, bool, 0644);
 MODULE_PARM_DESC(invert_y, "If set, Y axis is inverted");
 
-static struct pnp_device_id pnp_ids[] = {
-	{ .id = "PNP0cc0" },
-	{ .id = "" }
-};
-MODULE_DEVICE_TABLE(pnp, pnp_ids);
-
 static irqreturn_t htcpen_interrupt(int irq, void *handle)
 {
 	struct input_dev *htcpen_dev = handle;
@@ -237,6 +231,7 @@ static struct dmi_system_id __initdata htcshift_dmi_table[] = {
 	},
 	{ }
 };
+MODULE_DEVICE_TABLE(dmi, htcshift_dmi_table);
 
 static int __init htcpen_isa_init(void)
 {
diff --git a/drivers/input/touchscreen/intel-mid-touch.c b/drivers/input/touchscreen/intel-mid-touch.c
index 327695268e06..3cd7a837f82b 100644
--- a/drivers/input/touchscreen/intel-mid-touch.c
+++ b/drivers/input/touchscreen/intel-mid-touch.c
@@ -664,18 +664,7 @@ static struct platform_driver mrstouch_driver = {
 	.probe		= mrstouch_probe,
 	.remove		= __devexit_p(mrstouch_remove),
 };
-
-static int __init mrstouch_init(void)
-{
-	return platform_driver_register(&mrstouch_driver);
-}
-module_init(mrstouch_init);
-
-static void __exit mrstouch_exit(void)
-{
-	platform_driver_unregister(&mrstouch_driver);
-}
-module_exit(mrstouch_exit);
+module_platform_driver(mrstouch_driver);
 
 MODULE_AUTHOR("Sreedhara Murthy. D.S, sreedhara.ds@intel.com");
 MODULE_DESCRIPTION("Intel Moorestown Resistive Touch Screen Driver");
diff --git a/drivers/input/touchscreen/jornada720_ts.c b/drivers/input/touchscreen/jornada720_ts.c
index 50076c2d59e2..c3848ad2325b 100644
--- a/drivers/input/touchscreen/jornada720_ts.c
+++ b/drivers/input/touchscreen/jornada720_ts.c
@@ -172,16 +172,4 @@ static struct platform_driver jornada720_ts_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init jornada720_ts_init(void)
-{
-	return platform_driver_register(&jornada720_ts_driver);
-}
-
-static void __exit jornada720_ts_exit(void)
-{
-	platform_driver_unregister(&jornada720_ts_driver);
-}
-
-module_init(jornada720_ts_init);
-module_exit(jornada720_ts_exit);
+module_platform_driver(jornada720_ts_driver);
diff --git a/drivers/input/touchscreen/lpc32xx_ts.c b/drivers/input/touchscreen/lpc32xx_ts.c
index 0a484ed5295c..afcd0691ec67 100644
--- a/drivers/input/touchscreen/lpc32xx_ts.c
+++ b/drivers/input/touchscreen/lpc32xx_ts.c
@@ -392,18 +392,7 @@ static struct platform_driver lpc32xx_ts_driver = {
 		.pm	= LPC32XX_TS_PM_OPS,
 	},
 };
-
-static int __init lpc32xx_ts_init(void)
-{
-	return platform_driver_register(&lpc32xx_ts_driver);
-}
-module_init(lpc32xx_ts_init);
-
-static void __exit lpc32xx_ts_exit(void)
-{
-	platform_driver_unregister(&lpc32xx_ts_driver);
-}
-module_exit(lpc32xx_ts_exit);
+module_platform_driver(lpc32xx_ts_driver);
 
 MODULE_AUTHOR("Kevin Wells <kevin.wells@nxp.com");
 MODULE_DESCRIPTION("LPC32XX TSC Driver");
diff --git a/drivers/input/touchscreen/mainstone-wm97xx.c b/drivers/input/touchscreen/mainstone-wm97xx.c
index e966c29ff1bb..7d2b2136e5ad 100644
--- a/drivers/input/touchscreen/mainstone-wm97xx.c
+++ b/drivers/input/touchscreen/mainstone-wm97xx.c
@@ -302,19 +302,7 @@ static struct platform_driver mainstone_wm97xx_driver = {
 		.name = "wm97xx-touch",
 	},
 };
-
-static int __init mainstone_wm97xx_init(void)
-{
-	return platform_driver_register(&mainstone_wm97xx_driver);
-}
-
-static void __exit mainstone_wm97xx_exit(void)
-{
-	platform_driver_unregister(&mainstone_wm97xx_driver);
-}
-
-module_init(mainstone_wm97xx_init);
-module_exit(mainstone_wm97xx_exit);
+module_platform_driver(mainstone_wm97xx_driver);
 
 /* Module information */
 MODULE_AUTHOR("Liam Girdwood <lrg@slimlogic.co.uk>");
diff --git a/drivers/input/touchscreen/mc13783_ts.c b/drivers/input/touchscreen/mc13783_ts.c
index ede02743eac1..68f86f7dabbc 100644
--- a/drivers/input/touchscreen/mc13783_ts.c
+++ b/drivers/input/touchscreen/mc13783_ts.c
@@ -240,18 +240,7 @@ static struct platform_driver mc13783_ts_driver = {
 		.name	= MC13783_TS_NAME,
 	},
 };
-
-static int __init mc13783_ts_init(void)
-{
-	return platform_driver_probe(&mc13783_ts_driver, &mc13783_ts_probe);
-}
-module_init(mc13783_ts_init);
-
-static void __exit mc13783_ts_exit(void)
-{
-	platform_driver_unregister(&mc13783_ts_driver);
-}
-module_exit(mc13783_ts_exit);
+module_platform_driver(mc13783_ts_driver);
 
 MODULE_DESCRIPTION("MC13783 input touchscreen driver");
 MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
diff --git a/drivers/input/touchscreen/migor_ts.c b/drivers/input/touchscreen/migor_ts.c
index 5803bd0c1cca..5226194aa78e 100644
--- a/drivers/input/touchscreen/migor_ts.c
+++ b/drivers/input/touchscreen/migor_ts.c
@@ -36,7 +36,6 @@
 struct migor_ts_priv {
 	struct i2c_client *client;
 	struct input_dev *input;
-	struct delayed_work work;
 	int irq;
 };
 
@@ -44,15 +43,24 @@ static const u_int8_t migor_ts_ena_seq[17] = { 0x33, 0x22, 0x11,
 					       0x01, 0x06, 0x07, };
 static const u_int8_t migor_ts_dis_seq[17] = { };
 
-static void migor_ts_poscheck(struct work_struct *work)
+static irqreturn_t migor_ts_isr(int irq, void *dev_id)
 {
-	struct migor_ts_priv *priv = container_of(work,
-						  struct migor_ts_priv,
-						  work.work);
+	struct migor_ts_priv *priv = dev_id;
 	unsigned short xpos, ypos;
 	unsigned char event;
 	u_int8_t buf[16];
 
+	/*
+	 * The touch screen controller chip is hooked up to the CPU
+	 * using I2C and a single interrupt line. The interrupt line
+	 * is pulled low whenever someone taps the screen. To deassert
+	 * the interrupt line we need to acknowledge the interrupt by
+	 * communicating with the controller over the slow i2c bus.
+	 *
+	 * Since I2C bus controller may sleep we are using threaded
+	 * IRQ here.
+	 */
+
 	memset(buf, 0, sizeof(buf));
 
 	/* Set Index 0 */
@@ -72,41 +80,25 @@ static void migor_ts_poscheck(struct work_struct *work)
 	xpos = ((buf[11] & 0x03) << 8 | buf[10]);
 	event = buf[12];
 
-	if (event == EVENT_PENDOWN || event == EVENT_REPEAT) {
+	switch (event) {
+	case EVENT_PENDOWN:
+	case EVENT_REPEAT:
 		input_report_key(priv->input, BTN_TOUCH, 1);
 		input_report_abs(priv->input, ABS_X, ypos); /*X-Y swap*/
 		input_report_abs(priv->input, ABS_Y, xpos);
 		input_sync(priv->input);
-	} else if (event == EVENT_PENUP) {
+		break;
+
+	case EVENT_PENUP:
 		input_report_key(priv->input, BTN_TOUCH, 0);
 		input_sync(priv->input);
+		break;
 	}
- out:
-	enable_irq(priv->irq);
-}
-
-static irqreturn_t migor_ts_isr(int irq, void *dev_id)
-{
-	struct migor_ts_priv *priv = dev_id;
-
-	/* the touch screen controller chip is hooked up to the cpu
-	 * using i2c and a single interrupt line. the interrupt line
-	 * is pulled low whenever someone taps the screen. to deassert
-	 * the interrupt line we need to acknowledge the interrupt by
-	 * communicating with the controller over the slow i2c bus.
-	 *
-	 * we can't acknowledge from interrupt context since the i2c
-	 * bus controller may sleep, so we just disable the interrupt
-	 * here and handle the acknowledge using delayed work.
-	 */
-
-	disable_irq_nosync(irq);
-	schedule_delayed_work(&priv->work, HZ / 20);
 
+ out:
 	return IRQ_HANDLED;
 }
 
-
 static int migor_ts_open(struct input_dev *dev)
 {
 	struct migor_ts_priv *priv = input_get_drvdata(dev);
@@ -131,15 +123,6 @@ static void migor_ts_close(struct input_dev *dev)
 
 	disable_irq(priv->irq);
 
-	/* cancel pending work and wait for migor_ts_poscheck() to finish */
-	if (cancel_delayed_work_sync(&priv->work)) {
-		/*
-		 * if migor_ts_poscheck was canceled we need to enable IRQ
-		 * here to balance disable done in migor_ts_isr.
-		 */
-		enable_irq(priv->irq);
-	}
-
 	/* disable controller */
 	i2c_master_send(client, migor_ts_dis_seq, sizeof(migor_ts_dis_seq));
 
@@ -154,23 +137,20 @@ static int migor_ts_probe(struct i2c_client *client,
 	int error;
 
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&client->dev, "failed to allocate driver data\n");
-		error = -ENOMEM;
-		goto err0;
-	}
-
-	dev_set_drvdata(&client->dev, priv);
-
 	input = input_allocate_device();
-	if (!input) {
-		dev_err(&client->dev, "Failed to allocate input device.\n");
+	if (!priv || !input) {
+		dev_err(&client->dev, "failed to allocate memory\n");
 		error = -ENOMEM;
-		goto err1;
+		goto err_free_mem;
 	}
 
+	priv->client = client;
+	priv->input = input;
+	priv->irq = client->irq;
+
 	input->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
-	input->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+	__set_bit(BTN_TOUCH, input->keybit);
 
 	input_set_abs_params(input, ABS_X, 95, 955, 0, 0);
 	input_set_abs_params(input, ABS_Y, 85, 935, 0, 0);
@@ -184,39 +164,34 @@ static int migor_ts_probe(struct i2c_client *client,
 
 	input_set_drvdata(input, priv);
 
-	priv->client = client;
-	priv->input = input;
-	INIT_DELAYED_WORK(&priv->work, migor_ts_poscheck);
-	priv->irq = client->irq;
-
-	error = input_register_device(input);
-	if (error)
-		goto err1;
-
-	error = request_irq(priv->irq, migor_ts_isr, IRQF_TRIGGER_LOW,
-			    client->name, priv);
+	error = request_threaded_irq(priv->irq, NULL, migor_ts_isr,
+                                     IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+                                     client->name, priv);
 	if (error) {
 		dev_err(&client->dev, "Unable to request touchscreen IRQ.\n");
-		goto err2;
+		goto err_free_mem;
 	}
 
+	error = input_register_device(input);
+	if (error)
+		goto err_free_irq;
+
+	i2c_set_clientdata(client, priv);
 	device_init_wakeup(&client->dev, 1);
+
 	return 0;
 
- err2:
-	input_unregister_device(input);
-	input = NULL; /* so we dont try to free it below */
- err1:
+ err_free_irq:
+	free_irq(priv->irq, priv);
+ err_free_mem:
 	input_free_device(input);
 	kfree(priv);
- err0:
-	dev_set_drvdata(&client->dev, NULL);
 	return error;
 }
 
 static int migor_ts_remove(struct i2c_client *client)
 {
-	struct migor_ts_priv *priv = dev_get_drvdata(&client->dev);
+	struct migor_ts_priv *priv = i2c_get_clientdata(client);
 
 	free_irq(priv->irq, priv);
 	input_unregister_device(priv->input);
@@ -230,7 +205,7 @@ static int migor_ts_remove(struct i2c_client *client)
 static int migor_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
-	struct migor_ts_priv *priv = dev_get_drvdata(&client->dev);
+	struct migor_ts_priv *priv = i2c_get_clientdata(client);
 
 	if (device_may_wakeup(&client->dev))
 		enable_irq_wake(priv->irq);
@@ -241,7 +216,7 @@ static int migor_ts_suspend(struct device *dev)
 static int migor_ts_resume(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
-	struct migor_ts_priv *priv = dev_get_drvdata(&client->dev);
+	struct migor_ts_priv *priv = i2c_get_clientdata(client);
 
 	if (device_may_wakeup(&client->dev))
 		disable_irq_wake(priv->irq);
diff --git a/drivers/input/touchscreen/pcap_ts.c b/drivers/input/touchscreen/pcap_ts.c
index ea6ef16e59b4..f57aeb80f7e3 100644
--- a/drivers/input/touchscreen/pcap_ts.c
+++ b/drivers/input/touchscreen/pcap_ts.c
@@ -252,19 +252,7 @@ static struct platform_driver pcap_ts_driver = {
 		.pm	= PCAP_TS_PM_OPS,
 	},
 };
-
-static int __init pcap_ts_init(void)
-{
-	return platform_driver_register(&pcap_ts_driver);
-}
-
-static void __exit pcap_ts_exit(void)
-{
-	platform_driver_unregister(&pcap_ts_driver);
-}
-
-module_init(pcap_ts_init);
-module_exit(pcap_ts_exit);
+module_platform_driver(pcap_ts_driver);
 
 MODULE_DESCRIPTION("Motorola PCAP2 touchscreen driver");
 MODULE_AUTHOR("Daniel Ribeiro / Harald Welte");
diff --git a/drivers/input/touchscreen/pixcir_i2c_ts.c b/drivers/input/touchscreen/pixcir_i2c_ts.c
new file mode 100644
index 000000000000..d5ac09a1ee56
--- /dev/null
+++ b/drivers/input/touchscreen/pixcir_i2c_ts.c
@@ -0,0 +1,239 @@
+/*
+ * Driver for Pixcir I2C touchscreen controllers.
+ *
+ * Copyright (C) 2010-2011 Pixcir, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ */
+
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/input/pixcir_ts.h>
+
+struct pixcir_i2c_ts_data {
+	struct i2c_client *client;
+	struct input_dev *input;
+	const struct pixcir_ts_platform_data *chip;
+	bool exiting;
+};
+
+static void pixcir_ts_poscheck(struct pixcir_i2c_ts_data *data)
+{
+	struct pixcir_i2c_ts_data *tsdata = data;
+	u8 rdbuf[10], wrbuf[1] = { 0 };
+	u8 touch;
+	int ret;
+
+	ret = i2c_master_send(tsdata->client, wrbuf, sizeof(wrbuf));
+	if (ret != sizeof(wrbuf)) {
+		dev_err(&tsdata->client->dev,
+			"%s: i2c_master_send failed(), ret=%d\n",
+			__func__, ret);
+		return;
+	}
+
+	ret = i2c_master_recv(tsdata->client, rdbuf, sizeof(rdbuf));
+	if (ret != sizeof(rdbuf)) {
+		dev_err(&tsdata->client->dev,
+			"%s: i2c_master_recv failed(), ret=%d\n",
+			__func__, ret);
+		return;
+	}
+
+	touch = rdbuf[0];
+	if (touch) {
+		u16 posx1 = (rdbuf[3] << 8) | rdbuf[2];
+		u16 posy1 = (rdbuf[5] << 8) | rdbuf[4];
+		u16 posx2 = (rdbuf[7] << 8) | rdbuf[6];
+		u16 posy2 = (rdbuf[9] << 8) | rdbuf[8];
+
+		input_report_key(tsdata->input, BTN_TOUCH, 1);
+		input_report_abs(tsdata->input, ABS_X, posx1);
+		input_report_abs(tsdata->input, ABS_Y, posy1);
+
+		input_report_abs(tsdata->input, ABS_MT_POSITION_X, posx1);
+		input_report_abs(tsdata->input, ABS_MT_POSITION_Y, posy1);
+		input_mt_sync(tsdata->input);
+
+		if (touch == 2) {
+			input_report_abs(tsdata->input,
+					 ABS_MT_POSITION_X, posx2);
+			input_report_abs(tsdata->input,
+					 ABS_MT_POSITION_Y, posy2);
+			input_mt_sync(tsdata->input);
+		}
+	} else {
+		input_report_key(tsdata->input, BTN_TOUCH, 0);
+	}
+
+	input_sync(tsdata->input);
+}
+
+static irqreturn_t pixcir_ts_isr(int irq, void *dev_id)
+{
+	struct pixcir_i2c_ts_data *tsdata = dev_id;
+
+	while (!tsdata->exiting) {
+		pixcir_ts_poscheck(tsdata);
+
+		if (tsdata->chip->attb_read_val())
+			break;
+
+		msleep(20);
+	}
+
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int pixcir_i2c_ts_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	if (device_may_wakeup(&client->dev))
+		enable_irq_wake(client->irq);
+
+	return 0;
+}
+
+static int pixcir_i2c_ts_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	if (device_may_wakeup(&client->dev))
+		disable_irq_wake(client->irq);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(pixcir_dev_pm_ops,
+			 pixcir_i2c_ts_suspend, pixcir_i2c_ts_resume);
+
+static int __devinit pixcir_i2c_ts_probe(struct i2c_client *client,
+					 const struct i2c_device_id *id)
+{
+	const struct pixcir_ts_platform_data *pdata = client->dev.platform_data;
+	struct pixcir_i2c_ts_data *tsdata;
+	struct input_dev *input;
+	int error;
+
+	if (!pdata) {
+		dev_err(&client->dev, "platform data not defined\n");
+		return -EINVAL;
+	}
+
+	tsdata = kzalloc(sizeof(*tsdata), GFP_KERNEL);
+	input = input_allocate_device();
+	if (!tsdata || !input) {
+		dev_err(&client->dev, "Failed to allocate driver data!\n");
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	tsdata->client = client;
+	tsdata->input = input;
+	tsdata->chip = pdata;
+
+	input->name = client->name;
+	input->id.bustype = BUS_I2C;
+	input->dev.parent = &client->dev;
+
+	__set_bit(EV_KEY, input->evbit);
+	__set_bit(EV_ABS, input->evbit);
+	__set_bit(BTN_TOUCH, input->keybit);
+	input_set_abs_params(input, ABS_X, 0, pdata->x_max, 0, 0);
+	input_set_abs_params(input, ABS_Y, 0, pdata->y_max, 0, 0);
+	input_set_abs_params(input, ABS_MT_POSITION_X, 0, pdata->x_max, 0, 0);
+	input_set_abs_params(input, ABS_MT_POSITION_Y, 0, pdata->y_max, 0, 0);
+
+	input_set_drvdata(input, tsdata);
+
+	error = request_threaded_irq(client->irq, NULL, pixcir_ts_isr,
+				     IRQF_TRIGGER_FALLING,
+				     client->name, tsdata);
+	if (error) {
+		dev_err(&client->dev, "Unable to request touchscreen IRQ.\n");
+		goto err_free_mem;
+	}
+
+	error = input_register_device(input);
+	if (error)
+		goto err_free_irq;
+
+	i2c_set_clientdata(client, tsdata);
+	device_init_wakeup(&client->dev, 1);
+
+	return 0;
+
+err_free_irq:
+	free_irq(client->irq, tsdata);
+err_free_mem:
+	input_free_device(input);
+	kfree(tsdata);
+	return error;
+}
+
+static int __devexit pixcir_i2c_ts_remove(struct i2c_client *client)
+{
+	struct pixcir_i2c_ts_data *tsdata = i2c_get_clientdata(client);
+
+	device_init_wakeup(&client->dev, 0);
+
+	tsdata->exiting = true;
+	mb();
+	free_irq(client->irq, tsdata);
+
+	input_unregister_device(tsdata->input);
+	kfree(tsdata);
+
+	return 0;
+}
+
+static const struct i2c_device_id pixcir_i2c_ts_id[] = {
+	{ "pixcir_ts", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, pixcir_i2c_ts_id);
+
+static struct i2c_driver pixcir_i2c_ts_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "pixcir_ts",
+		.pm	= &pixcir_dev_pm_ops,
+	},
+	.probe		= pixcir_i2c_ts_probe,
+	.remove		= __devexit_p(pixcir_i2c_ts_remove),
+	.id_table	= pixcir_i2c_ts_id,
+};
+
+static int __init pixcir_i2c_ts_init(void)
+{
+	return i2c_add_driver(&pixcir_i2c_ts_driver);
+}
+module_init(pixcir_i2c_ts_init);
+
+static void __exit pixcir_i2c_ts_exit(void)
+{
+	i2c_del_driver(&pixcir_i2c_ts_driver);
+}
+module_exit(pixcir_i2c_ts_exit);
+
+MODULE_AUTHOR("Jianchun Bian <jcbian@pixcir.com.cn>, Dequan Meng <dqmeng@pixcir.com.cn>");
+MODULE_DESCRIPTION("Pixcir I2C Touchscreen Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/s3c2410_ts.c b/drivers/input/touchscreen/s3c2410_ts.c
index 64ce697a3456..bf1a06400067 100644
--- a/drivers/input/touchscreen/s3c2410_ts.c
+++ b/drivers/input/touchscreen/s3c2410_ts.c
@@ -432,19 +432,7 @@ static struct platform_driver s3c_ts_driver = {
 	.probe		= s3c2410ts_probe,
 	.remove		= __devexit_p(s3c2410ts_remove),
 };
-
-static int __init s3c2410ts_init(void)
-{
-	return platform_driver_register(&s3c_ts_driver);
-}
-
-static void __exit s3c2410ts_exit(void)
-{
-	platform_driver_unregister(&s3c_ts_driver);
-}
-
-module_init(s3c2410ts_init);
-module_exit(s3c2410ts_exit);
+module_platform_driver(s3c_ts_driver);
 
 MODULE_AUTHOR("Arnaud Patard <arnaud.patard@rtp-net.org>, "
 	      "Ben Dooks <ben@simtec.co.uk>, "
diff --git a/drivers/input/touchscreen/stmpe-ts.c b/drivers/input/touchscreen/stmpe-ts.c
index ae88e13c99ff..692b685720ce 100644
--- a/drivers/input/touchscreen/stmpe-ts.c
+++ b/drivers/input/touchscreen/stmpe-ts.c
@@ -379,20 +379,7 @@ static struct platform_driver stmpe_ts_driver = {
 	.probe = stmpe_input_probe,
 	.remove = __devexit_p(stmpe_ts_remove),
 };
-
-static int __init stmpe_ts_init(void)
-{
-	return platform_driver_register(&stmpe_ts_driver);
-}
-
-module_init(stmpe_ts_init);
-
-static void __exit stmpe_ts_exit(void)
-{
-	platform_driver_unregister(&stmpe_ts_driver);
-}
-
-module_exit(stmpe_ts_exit);
+module_platform_driver(stmpe_ts_driver);
 
 MODULE_AUTHOR("Luotao Fu <l.fu@pengutronix.de>");
 MODULE_DESCRIPTION("STMPEXXX touchscreen driver");
diff --git a/drivers/input/touchscreen/tnetv107x-ts.c b/drivers/input/touchscreen/tnetv107x-ts.c
index 0e8f63e5b36f..7e7488097359 100644
--- a/drivers/input/touchscreen/tnetv107x-ts.c
+++ b/drivers/input/touchscreen/tnetv107x-ts.c
@@ -378,19 +378,7 @@ static struct platform_driver tsc_driver = {
 	.driver.name	= "tnetv107x-ts",
 	.driver.owner	= THIS_MODULE,
 };
-
-static int __init tsc_init(void)
-{
-	return platform_driver_register(&tsc_driver);
-}
-
-static void __exit tsc_exit(void)
-{
-	platform_driver_unregister(&tsc_driver);
-}
-
-module_init(tsc_init);
-module_exit(tsc_exit);
+module_platform_driver(tsc_driver);
 
 MODULE_AUTHOR("Cyril Chemparathy");
 MODULE_DESCRIPTION("TNETV107X Touchscreen Driver");
diff --git a/drivers/input/touchscreen/tps6507x-ts.c b/drivers/input/touchscreen/tps6507x-ts.c
index 43031492d733..6c6f6d8ea9b4 100644
--- a/drivers/input/touchscreen/tps6507x-ts.c
+++ b/drivers/input/touchscreen/tps6507x-ts.c
@@ -371,18 +371,7 @@ static struct platform_driver tps6507x_ts_driver = {
 	.probe = tps6507x_ts_probe,
 	.remove = __devexit_p(tps6507x_ts_remove),
 };
-
-static int __init tps6507x_ts_init(void)
-{
-	return platform_driver_register(&tps6507x_ts_driver);
-}
-module_init(tps6507x_ts_init);
-
-static void __exit tps6507x_ts_exit(void)
-{
-	platform_driver_unregister(&tps6507x_ts_driver);
-}
-module_exit(tps6507x_ts_exit);
+module_platform_driver(tps6507x_ts_driver);
 
 MODULE_AUTHOR("Todd Fischer <todd.fischer@ridgerun.com>");
 MODULE_DESCRIPTION("TPS6507x - TouchScreen driver");
diff --git a/drivers/input/touchscreen/ucb1400_ts.c b/drivers/input/touchscreen/ucb1400_ts.c
index 3b5b5df04dd6..d2b57536feea 100644
--- a/drivers/input/touchscreen/ucb1400_ts.c
+++ b/drivers/input/touchscreen/ucb1400_ts.c
@@ -20,24 +20,24 @@
 
 #include <linux/module.h>
 #include <linux/init.h>
-#include <linux/completion.h>
 #include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
 #include <linux/input.h>
 #include <linux/device.h>
 #include <linux/interrupt.h>
-#include <linux/suspend.h>
-#include <linux/kthread.h>
-#include <linux/freezer.h>
 #include <linux/ucb1400.h>
 
+#define UCB1400_TS_POLL_PERIOD	10 /* ms */
+
 static int adcsync;
 static int ts_delay = 55; /* us */
 static int ts_delay_pressure;	/* us */
 
 /* Switch to interrupt mode. */
-static inline void ucb1400_ts_mode_int(struct snd_ac97 *ac97)
+static void ucb1400_ts_mode_int(struct ucb1400_ts *ucb)
 {
-	ucb1400_reg_write(ac97, UCB_TS_CR,
+	ucb1400_reg_write(ucb->ac97, UCB_TS_CR,
 			UCB_TS_CR_TSMX_POW | UCB_TS_CR_TSPX_POW |
 			UCB_TS_CR_TSMY_GND | UCB_TS_CR_TSPY_GND |
 			UCB_TS_CR_MODE_INT);
@@ -47,13 +47,15 @@ static inline void ucb1400_ts_mode_int(struct snd_ac97 *ac97)
  * Switch to pressure mode, and read pressure.  We don't need to wait
  * here, since both plates are being driven.
  */
-static inline unsigned int ucb1400_ts_read_pressure(struct ucb1400_ts *ucb)
+static unsigned int ucb1400_ts_read_pressure(struct ucb1400_ts *ucb)
 {
 	ucb1400_reg_write(ucb->ac97, UCB_TS_CR,
 			UCB_TS_CR_TSMX_POW | UCB_TS_CR_TSPX_POW |
 			UCB_TS_CR_TSMY_GND | UCB_TS_CR_TSPY_GND |
 			UCB_TS_CR_MODE_PRES | UCB_TS_CR_BIAS_ENA);
+
 	udelay(ts_delay_pressure);
+
 	return ucb1400_adc_read(ucb->ac97, UCB_ADC_INP_TSPY, adcsync);
 }
 
@@ -63,7 +65,7 @@ static inline unsigned int ucb1400_ts_read_pressure(struct ucb1400_ts *ucb)
  * gives a faster response time.  Even so, we need to wait about 55us
  * for things to stabilise.
  */
-static inline unsigned int ucb1400_ts_read_xpos(struct ucb1400_ts *ucb)
+static unsigned int ucb1400_ts_read_xpos(struct ucb1400_ts *ucb)
 {
 	ucb1400_reg_write(ucb->ac97, UCB_TS_CR,
 			UCB_TS_CR_TSMX_GND | UCB_TS_CR_TSPX_POW |
@@ -86,7 +88,7 @@ static inline unsigned int ucb1400_ts_read_xpos(struct ucb1400_ts *ucb)
  * gives a faster response time.  Even so, we need to wait about 55us
  * for things to stabilise.
  */
-static inline unsigned int ucb1400_ts_read_ypos(struct ucb1400_ts *ucb)
+static int ucb1400_ts_read_ypos(struct ucb1400_ts *ucb)
 {
 	ucb1400_reg_write(ucb->ac97, UCB_TS_CR,
 			UCB_TS_CR_TSMY_GND | UCB_TS_CR_TSPY_POW |
@@ -107,7 +109,7 @@ static inline unsigned int ucb1400_ts_read_ypos(struct ucb1400_ts *ucb)
  * Switch to X plate resistance mode.  Set MX to ground, PX to
  * supply.  Measure current.
  */
-static inline unsigned int ucb1400_ts_read_xres(struct ucb1400_ts *ucb)
+static unsigned int ucb1400_ts_read_xres(struct ucb1400_ts *ucb)
 {
 	ucb1400_reg_write(ucb->ac97, UCB_TS_CR,
 			UCB_TS_CR_TSMX_GND | UCB_TS_CR_TSPX_POW |
@@ -119,7 +121,7 @@ static inline unsigned int ucb1400_ts_read_xres(struct ucb1400_ts *ucb)
  * Switch to Y plate resistance mode.  Set MY to ground, PY to
  * supply.  Measure current.
  */
-static inline unsigned int ucb1400_ts_read_yres(struct ucb1400_ts *ucb)
+static unsigned int ucb1400_ts_read_yres(struct ucb1400_ts *ucb)
 {
 	ucb1400_reg_write(ucb->ac97, UCB_TS_CR,
 			UCB_TS_CR_TSMY_GND | UCB_TS_CR_TSPY_POW |
@@ -127,26 +129,26 @@ static inline unsigned int ucb1400_ts_read_yres(struct ucb1400_ts *ucb)
 	return ucb1400_adc_read(ucb->ac97, 0, adcsync);
 }
 
-static inline int ucb1400_ts_pen_up(struct snd_ac97 *ac97)
+static int ucb1400_ts_pen_up(struct ucb1400_ts *ucb)
 {
-	unsigned short val = ucb1400_reg_read(ac97, UCB_TS_CR);
+	unsigned short val = ucb1400_reg_read(ucb->ac97, UCB_TS_CR);
 
 	return val & (UCB_TS_CR_TSPX_LOW | UCB_TS_CR_TSMX_LOW);
 }
 
-static inline void ucb1400_ts_irq_enable(struct snd_ac97 *ac97)
+static void ucb1400_ts_irq_enable(struct ucb1400_ts *ucb)
 {
-	ucb1400_reg_write(ac97, UCB_IE_CLEAR, UCB_IE_TSPX);
-	ucb1400_reg_write(ac97, UCB_IE_CLEAR, 0);
-	ucb1400_reg_write(ac97, UCB_IE_FAL, UCB_IE_TSPX);
+	ucb1400_reg_write(ucb->ac97, UCB_IE_CLEAR, UCB_IE_TSPX);
+	ucb1400_reg_write(ucb->ac97, UCB_IE_CLEAR, 0);
+	ucb1400_reg_write(ucb->ac97, UCB_IE_FAL, UCB_IE_TSPX);
 }
 
-static inline void ucb1400_ts_irq_disable(struct snd_ac97 *ac97)
+static void ucb1400_ts_irq_disable(struct ucb1400_ts *ucb)
 {
-	ucb1400_reg_write(ac97, UCB_IE_FAL, 0);
+	ucb1400_reg_write(ucb->ac97, UCB_IE_FAL, 0);
 }
 
-static void ucb1400_ts_evt_add(struct input_dev *idev, u16 pressure, u16 x, u16 y)
+static void ucb1400_ts_report_event(struct input_dev *idev, u16 pressure, u16 x, u16 y)
 {
 	input_report_abs(idev, ABS_X, x);
 	input_report_abs(idev, ABS_Y, y);
@@ -162,7 +164,7 @@ static void ucb1400_ts_event_release(struct input_dev *idev)
 	input_sync(idev);
 }
 
-static void ucb1400_handle_pending_irq(struct ucb1400_ts *ucb)
+static void ucb1400_clear_pending_irq(struct ucb1400_ts *ucb)
 {
 	unsigned int isr;
 
@@ -171,32 +173,34 @@ static void ucb1400_handle_pending_irq(struct ucb1400_ts *ucb)
 	ucb1400_reg_write(ucb->ac97, UCB_IE_CLEAR, 0);
 
 	if (isr & UCB_IE_TSPX)
-		ucb1400_ts_irq_disable(ucb->ac97);
+		ucb1400_ts_irq_disable(ucb);
 	else
-		dev_dbg(&ucb->ts_idev->dev, "ucb1400: unexpected IE_STATUS = %#x\n", isr);
-	enable_irq(ucb->irq);
+		dev_dbg(&ucb->ts_idev->dev,
+			"ucb1400: unexpected IE_STATUS = %#x\n", isr);
 }
 
-static int ucb1400_ts_thread(void *_ucb)
+/*
+ * A restriction with interrupts exists when using the ucb1400, as
+ * the codec read/write routines may sleep while waiting for codec
+ * access completion and uses semaphores for access control to the
+ * AC97 bus. Therefore the driver is forced to use threaded interrupt
+ * handler.
+ */
+static irqreturn_t ucb1400_irq(int irqnr, void *devid)
 {
-	struct ucb1400_ts *ucb = _ucb;
-	struct task_struct *tsk = current;
-	int valid = 0;
-	struct sched_param param = { .sched_priority = 1 };
+	struct ucb1400_ts *ucb = devid;
+	unsigned int x, y, p;
+	bool penup;
 
-	sched_setscheduler(tsk, SCHED_FIFO, &param);
+	if (unlikely(irqnr != ucb->irq))
+		return IRQ_NONE;
 
-	set_freezable();
-	while (!kthread_should_stop()) {
-		unsigned int x, y, p;
-		long timeout;
+	ucb1400_clear_pending_irq(ucb);
 
-		ucb->ts_restart = 0;
+	/* Start with a small delay before checking pendown state */
+	msleep(UCB1400_TS_POLL_PERIOD);
 
-		if (ucb->irq_pending) {
-			ucb->irq_pending = 0;
-			ucb1400_handle_pending_irq(ucb);
-		}
+	while (!ucb->stopped && !(penup = ucb1400_ts_pen_up(ucb))) {
 
 		ucb1400_adc_enable(ucb->ac97);
 		x = ucb1400_ts_read_xpos(ucb);
@@ -204,91 +208,62 @@ static int ucb1400_ts_thread(void *_ucb)
 		p = ucb1400_ts_read_pressure(ucb);
 		ucb1400_adc_disable(ucb->ac97);
 
-		/* Switch back to interrupt mode. */
-		ucb1400_ts_mode_int(ucb->ac97);
-
-		msleep(10);
-
-		if (ucb1400_ts_pen_up(ucb->ac97)) {
-			ucb1400_ts_irq_enable(ucb->ac97);
-
-			/*
-			 * If we spat out a valid sample set last time,
-			 * spit out a "pen off" sample here.
-			 */
-			if (valid) {
-				ucb1400_ts_event_release(ucb->ts_idev);
-				valid = 0;
-			}
-
-			timeout = MAX_SCHEDULE_TIMEOUT;
-		} else {
-			valid = 1;
-			ucb1400_ts_evt_add(ucb->ts_idev, p, x, y);
-			timeout = msecs_to_jiffies(10);
-		}
+		ucb1400_ts_report_event(ucb->ts_idev, p, x, y);
 
-		wait_event_freezable_timeout(ucb->ts_wait,
-			ucb->irq_pending || ucb->ts_restart ||
-			kthread_should_stop(), timeout);
+		wait_event_timeout(ucb->ts_wait, ucb->stopped,
+				   msecs_to_jiffies(UCB1400_TS_POLL_PERIOD));
 	}
 
-	/* Send the "pen off" if we are stopping with the pen still active */
-	if (valid)
-		ucb1400_ts_event_release(ucb->ts_idev);
+	ucb1400_ts_event_release(ucb->ts_idev);
 
-	ucb->ts_task = NULL;
-	return 0;
+	if (!ucb->stopped) {
+		/* Switch back to interrupt mode. */
+		ucb1400_ts_mode_int(ucb);
+		ucb1400_ts_irq_enable(ucb);
+	}
+
+	return IRQ_HANDLED;
 }
 
-/*
- * A restriction with interrupts exists when using the ucb1400, as
- * the codec read/write routines may sleep while waiting for codec
- * access completion and uses semaphores for access control to the
- * AC97 bus.  A complete codec read cycle could take  anywhere from
- * 60 to 100uSec so we *definitely* don't want to spin inside the
- * interrupt handler waiting for codec access.  So, we handle the
- * interrupt by scheduling a RT kernel thread to run in process
- * context instead of interrupt context.
- */
-static irqreturn_t ucb1400_hard_irq(int irqnr, void *devid)
+static void ucb1400_ts_stop(struct ucb1400_ts *ucb)
 {
-	struct ucb1400_ts *ucb = devid;
+	/* Signal IRQ thread to stop polling and disable the handler. */
+	ucb->stopped = true;
+	mb();
+	wake_up(&ucb->ts_wait);
+	disable_irq(ucb->irq);
 
-	if (irqnr == ucb->irq) {
-		disable_irq_nosync(ucb->irq);
-		ucb->irq_pending = 1;
-		wake_up(&ucb->ts_wait);
-		return IRQ_HANDLED;
-	}
-	return IRQ_NONE;
+	ucb1400_ts_irq_disable(ucb);
+	ucb1400_reg_write(ucb->ac97, UCB_TS_CR, 0);
+}
+
+/* Must be called with ts->lock held */
+static void ucb1400_ts_start(struct ucb1400_ts *ucb)
+{
+	/* Tell IRQ thread that it may poll the device. */
+	ucb->stopped = false;
+	mb();
+
+	ucb1400_ts_mode_int(ucb);
+	ucb1400_ts_irq_enable(ucb);
+
+	enable_irq(ucb->irq);
 }
 
 static int ucb1400_ts_open(struct input_dev *idev)
 {
 	struct ucb1400_ts *ucb = input_get_drvdata(idev);
-	int ret = 0;
 
-	BUG_ON(ucb->ts_task);
+	ucb1400_ts_start(ucb);
 
-	ucb->ts_task = kthread_run(ucb1400_ts_thread, ucb, "UCB1400_ts");
-	if (IS_ERR(ucb->ts_task)) {
-		ret = PTR_ERR(ucb->ts_task);
-		ucb->ts_task = NULL;
-	}
-
-	return ret;
+	return 0;
 }
 
 static void ucb1400_ts_close(struct input_dev *idev)
 {
 	struct ucb1400_ts *ucb = input_get_drvdata(idev);
 
-	if (ucb->ts_task)
-		kthread_stop(ucb->ts_task);
-
-	ucb1400_ts_irq_disable(ucb->ac97);
-	ucb1400_reg_write(ucb->ac97, UCB_TS_CR, 0);
+	ucb1400_ts_stop(ucb);
 }
 
 #ifndef NO_IRQ
@@ -299,7 +274,8 @@ static void ucb1400_ts_close(struct input_dev *idev)
  * Try to probe our interrupt, rather than relying on lots of
  * hard-coded machine dependencies.
  */
-static int ucb1400_ts_detect_irq(struct ucb1400_ts *ucb)
+static int __devinit ucb1400_ts_detect_irq(struct ucb1400_ts *ucb,
+					   struct platform_device *pdev)
 {
 	unsigned long mask, timeout;
 
@@ -321,7 +297,7 @@ static int ucb1400_ts_detect_irq(struct ucb1400_ts *ucb)
 						UCB_ADC_DAT_VALID)) {
 		cpu_relax();
 		if (time_after(jiffies, timeout)) {
-			printk(KERN_ERR "ucb1400: timed out in IRQ probe\n");
+			dev_err(&pdev->dev, "timed out in IRQ probe\n");
 			probe_irq_off(mask);
 			return -ENODEV;
 		}
@@ -342,11 +318,11 @@ static int ucb1400_ts_detect_irq(struct ucb1400_ts *ucb)
 	return 0;
 }
 
-static int ucb1400_ts_probe(struct platform_device *dev)
+static int __devinit ucb1400_ts_probe(struct platform_device *pdev)
 {
+	struct ucb1400_ts *ucb = pdev->dev.platform_data;
 	int error, x_res, y_res;
 	u16 fcsr;
-	struct ucb1400_ts *ucb = dev->dev.platform_data;
 
 	ucb->ts_idev = input_allocate_device();
 	if (!ucb->ts_idev) {
@@ -356,27 +332,19 @@ static int ucb1400_ts_probe(struct platform_device *dev)
 
 	/* Only in case the IRQ line wasn't supplied, try detecting it */
 	if (ucb->irq < 0) {
-		error = ucb1400_ts_detect_irq(ucb);
+		error = ucb1400_ts_detect_irq(ucb, pdev);
 		if (error) {
-			printk(KERN_ERR "UCB1400: IRQ probe failed\n");
+			dev_err(&pdev->dev, "IRQ probe failed\n");
 			goto err_free_devs;
 		}
 	}
+	dev_dbg(&pdev->dev, "found IRQ %d\n", ucb->irq);
 
 	init_waitqueue_head(&ucb->ts_wait);
 
-	error = request_irq(ucb->irq, ucb1400_hard_irq, IRQF_TRIGGER_RISING,
-				"UCB1400", ucb);
-	if (error) {
-		printk(KERN_ERR "ucb1400: unable to grab irq%d: %d\n",
-				ucb->irq, error);
-		goto err_free_devs;
-	}
-	printk(KERN_DEBUG "UCB1400: found IRQ %d\n", ucb->irq);
-
 	input_set_drvdata(ucb->ts_idev, ucb);
 
-	ucb->ts_idev->dev.parent	= &dev->dev;
+	ucb->ts_idev->dev.parent	= &pdev->dev;
 	ucb->ts_idev->name		= "UCB1400 touchscreen interface";
 	ucb->ts_idev->id.vendor		= ucb1400_reg_read(ucb->ac97,
 						AC97_VENDOR_ID1);
@@ -398,12 +366,23 @@ static int ucb1400_ts_probe(struct platform_device *dev)
 	x_res = ucb1400_ts_read_xres(ucb);
 	y_res = ucb1400_ts_read_yres(ucb);
 	ucb1400_adc_disable(ucb->ac97);
-	printk(KERN_DEBUG "UCB1400: x/y = %d/%d\n", x_res, y_res);
+	dev_dbg(&pdev->dev, "x/y = %d/%d\n", x_res, y_res);
 
 	input_set_abs_params(ucb->ts_idev, ABS_X, 0, x_res, 0, 0);
 	input_set_abs_params(ucb->ts_idev, ABS_Y, 0, y_res, 0, 0);
 	input_set_abs_params(ucb->ts_idev, ABS_PRESSURE, 0, 0, 0, 0);
 
+	ucb1400_ts_stop(ucb);
+
+	error = request_threaded_irq(ucb->irq, NULL, ucb1400_irq,
+				     IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+				     "UCB1400", ucb);
+	if (error) {
+		dev_err(&pdev->dev,
+			"unable to grab irq%d: %d\n", ucb->irq, error);
+		goto err_free_devs;
+	}
+
 	error = input_register_device(ucb->ts_idev);
 	if (error)
 		goto err_free_irq;
@@ -416,56 +395,61 @@ err_free_devs:
 	input_free_device(ucb->ts_idev);
 err:
 	return error;
-
 }
 
-static int ucb1400_ts_remove(struct platform_device *dev)
+static int __devexit ucb1400_ts_remove(struct platform_device *pdev)
 {
-	struct ucb1400_ts *ucb = dev->dev.platform_data;
+	struct ucb1400_ts *ucb = pdev->dev.platform_data;
 
 	free_irq(ucb->irq, ucb);
 	input_unregister_device(ucb->ts_idev);
+
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int ucb1400_ts_resume(struct platform_device *dev)
+#ifdef CONFIG_PM_SLEEP
+static int ucb1400_ts_suspend(struct device *dev)
 {
-	struct ucb1400_ts *ucb = dev->dev.platform_data;
-
-	if (ucb->ts_task) {
-		/*
-		 * Restart the TS thread to ensure the
-		 * TS interrupt mode is set up again
-		 * after sleep.
-		 */
-		ucb->ts_restart = 1;
-		wake_up(&ucb->ts_wait);
-	}
+	struct ucb1400_ts *ucb = dev->platform_data;
+	struct input_dev *idev = ucb->ts_idev;
+
+	mutex_lock(&idev->mutex);
+
+	if (idev->users)
+		ucb1400_ts_start(ucb);
+
+	mutex_unlock(&idev->mutex);
+	return 0;
+}
+
+static int ucb1400_ts_resume(struct device *dev)
+{
+	struct ucb1400_ts *ucb = dev->platform_data;
+	struct input_dev *idev = ucb->ts_idev;
+
+	mutex_lock(&idev->mutex);
+
+	if (idev->users)
+		ucb1400_ts_stop(ucb);
+
+	mutex_unlock(&idev->mutex);
 	return 0;
 }
-#else
-#define ucb1400_ts_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(ucb1400_ts_pm_ops,
+			 ucb1400_ts_suspend, ucb1400_ts_resume);
+
 static struct platform_driver ucb1400_ts_driver = {
 	.probe	= ucb1400_ts_probe,
-	.remove	= ucb1400_ts_remove,
-	.resume	= ucb1400_ts_resume,
+	.remove	= __devexit_p(ucb1400_ts_remove),
 	.driver	= {
 		.name	= "ucb1400_ts",
+		.owner	= THIS_MODULE,
+		.pm	= &ucb1400_ts_pm_ops,
 	},
 };
-
-static int __init ucb1400_ts_init(void)
-{
-	return platform_driver_register(&ucb1400_ts_driver);
-}
-
-static void __exit ucb1400_ts_exit(void)
-{
-	platform_driver_unregister(&ucb1400_ts_driver);
-}
+module_platform_driver(ucb1400_ts_driver);
 
 module_param(adcsync, bool, 0444);
 MODULE_PARM_DESC(adcsync, "Synchronize touch readings with ADCSYNC pin.");
@@ -479,8 +463,5 @@ MODULE_PARM_DESC(ts_delay_pressure,
 		"delay between panel setup and pressure read."
 		"  Default = 0us.");
 
-module_init(ucb1400_ts_init);
-module_exit(ucb1400_ts_exit);
-
 MODULE_DESCRIPTION("Philips UCB1400 touchscreen driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/usbtouchscreen.c b/drivers/input/touchscreen/usbtouchscreen.c
index e539d92cc626..06cef3ccc63a 100644
--- a/drivers/input/touchscreen/usbtouchscreen.c
+++ b/drivers/input/touchscreen/usbtouchscreen.c
@@ -16,6 +16,7 @@
  *  - JASTEC USB touch controller/DigiTech DTR-02U
  *  - Zytronic capacitive touchscreen
  *  - NEXIO/iNexio
+ *  - Elo TouchSystems 2700 IntelliTouch
  *
  * Copyright (C) 2004-2007 by Daniel Ritz <daniel.ritz@gmx.ch>
  * Copyright (C) by Todd E. Johnson (mtouchusb.c)
@@ -138,6 +139,7 @@ enum {
 	DEVTYPE_ZYTRONIC,
 	DEVTYPE_TC45USB,
 	DEVTYPE_NEXIO,
+	DEVTYPE_ELO,
 };
 
 #define USB_DEVICE_HID_CLASS(vend, prod) \
@@ -239,6 +241,10 @@ static const struct usb_device_id usbtouch_devices[] = {
 		.driver_info = DEVTYPE_NEXIO},
 #endif
 
+#ifdef CONFIG_TOUCHSCREEN_USB_ELO
+	{USB_DEVICE(0x04e7, 0x0020), .driver_info = DEVTYPE_ELO},
+#endif
+
 	{}
 };
 
@@ -945,6 +951,24 @@ static int nexio_read_data(struct usbtouch_usb *usbtouch, unsigned char *pkt)
 
 
 /*****************************************************************************
+ * ELO part
+ */
+
+#ifdef CONFIG_TOUCHSCREEN_USB_ELO
+
+static int elo_read_data(struct usbtouch_usb *dev, unsigned char *pkt)
+{
+	dev->x = (pkt[3] << 8) | pkt[2];
+	dev->y = (pkt[5] << 8) | pkt[4];
+	dev->touch = pkt[6] > 0;
+	dev->press = pkt[6];
+
+	return 1;
+}
+#endif
+
+
+/*****************************************************************************
  * the different device descriptors
  */
 #ifdef MULTI_PACKET
@@ -953,6 +977,18 @@ static void usbtouch_process_multi(struct usbtouch_usb *usbtouch,
 #endif
 
 static struct usbtouch_device_info usbtouch_dev_info[] = {
+#ifdef CONFIG_TOUCHSCREEN_USB_ELO
+	[DEVTYPE_ELO] = {
+		.min_xc		= 0x0,
+		.max_xc		= 0x0fff,
+		.min_yc		= 0x0,
+		.max_yc		= 0x0fff,
+		.max_press	= 0xff,
+		.rept_size	= 8,
+		.read_data	= elo_read_data,
+	},
+#endif
+
 #ifdef CONFIG_TOUCHSCREEN_USB_EGALAX
 	[DEVTYPE_EGALAX] = {
 		.min_xc		= 0x0,
diff --git a/drivers/input/touchscreen/w90p910_ts.c b/drivers/input/touchscreen/w90p910_ts.c
index 217aa51135c5..9396b21d0e8f 100644
--- a/drivers/input/touchscreen/w90p910_ts.c
+++ b/drivers/input/touchscreen/w90p910_ts.c
@@ -331,19 +331,7 @@ static struct platform_driver w90x900ts_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-
-static int __init w90x900ts_init(void)
-{
-	return platform_driver_register(&w90x900ts_driver);
-}
-
-static void __exit w90x900ts_exit(void)
-{
-	platform_driver_unregister(&w90x900ts_driver);
-}
-
-module_init(w90x900ts_init);
-module_exit(w90x900ts_exit);
+module_platform_driver(w90x900ts_driver);
 
 MODULE_AUTHOR("Wan ZongShun <mcuos.com@gmail.com>");
 MODULE_DESCRIPTION("w90p910 touch screen driver!");
diff --git a/drivers/input/touchscreen/wm831x-ts.c b/drivers/input/touchscreen/wm831x-ts.c
index 9175d49d2546..4bc851a9dc3d 100644
--- a/drivers/input/touchscreen/wm831x-ts.c
+++ b/drivers/input/touchscreen/wm831x-ts.c
@@ -401,18 +401,7 @@ static struct platform_driver wm831x_ts_driver = {
 	.probe = wm831x_ts_probe,
 	.remove = __devexit_p(wm831x_ts_remove),
 };
-
-static int __init wm831x_ts_init(void)
-{
-	return platform_driver_register(&wm831x_ts_driver);
-}
-module_init(wm831x_ts_init);
-
-static void __exit wm831x_ts_exit(void)
-{
-	platform_driver_unregister(&wm831x_ts_driver);
-}
-module_exit(wm831x_ts_exit);
+module_platform_driver(wm831x_ts_driver);
 
 /* Module information */
 MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
diff --git a/drivers/input/touchscreen/zylonite-wm97xx.c b/drivers/input/touchscreen/zylonite-wm97xx.c
index f6328c0cded6..bf0869a7a78e 100644
--- a/drivers/input/touchscreen/zylonite-wm97xx.c
+++ b/drivers/input/touchscreen/zylonite-wm97xx.c
@@ -22,6 +22,7 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/delay.h>
+#include <linux/gpio.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
@@ -192,8 +193,8 @@ static int zylonite_wm97xx_probe(struct platform_device *pdev)
 	else
 		gpio_touch_irq = mfp_to_gpio(MFP_PIN_GPIO26);
 
-	wm->pen_irq = IRQ_GPIO(gpio_touch_irq);
-	irq_set_irq_type(IRQ_GPIO(gpio_touch_irq), IRQ_TYPE_EDGE_BOTH);
+	wm->pen_irq = gpio_to_irq(gpio_touch_irq);
+	irq_set_irq_type(wm->pen_irq, IRQ_TYPE_EDGE_BOTH);
 
 	wm97xx_config_gpio(wm, WM97XX_GPIO_13, WM97XX_GPIO_IN,
 			   WM97XX_GPIO_POL_HIGH,
@@ -223,19 +224,7 @@ static struct platform_driver zylonite_wm97xx_driver = {
 		.name	= "wm97xx-touch",
 	},
 };
-
-static int __init zylonite_wm97xx_init(void)
-{
-	return platform_driver_register(&zylonite_wm97xx_driver);
-}
-
-static void __exit zylonite_wm97xx_exit(void)
-{
-	platform_driver_unregister(&zylonite_wm97xx_driver);
-}
-
-module_init(zylonite_wm97xx_init);
-module_exit(zylonite_wm97xx_exit);
+module_platform_driver(zylonite_wm97xx_driver);
 
 /* Module information */
 MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index 5414253b185a..6bea6962f8ee 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -34,7 +34,9 @@ config AMD_IOMMU
 	bool "AMD IOMMU support"
 	select SWIOTLB
 	select PCI_MSI
-	select PCI_IOV
+	select PCI_ATS
+	select PCI_PRI
+	select PCI_PASID
 	select IOMMU_API
 	depends on X86_64 && PCI && ACPI
 	---help---
@@ -58,6 +60,15 @@ config AMD_IOMMU_STATS
 	  information to userspace via debugfs.
 	  If unsure, say N.
 
+config AMD_IOMMU_V2
+	tristate "AMD IOMMU Version 2 driver (EXPERIMENTAL)"
+	depends on AMD_IOMMU && PROFILING && EXPERIMENTAL
+	select MMU_NOTIFIER
+	---help---
+	  This option enables support for the AMD IOMMUv2 features of the IOMMU
+	  hardware. Select this option if you want to use devices that support
+	  the the PCI PRI and PASID interface.
+
 # Intel IOMMU support
 config DMAR_TABLE
 	bool
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 2f4448794bc7..0e36b4934aff 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -1,6 +1,7 @@
 obj-$(CONFIG_IOMMU_API) += iommu.o
 obj-$(CONFIG_MSM_IOMMU) += msm_iommu.o msm_iommu_dev.o
 obj-$(CONFIG_AMD_IOMMU) += amd_iommu.o amd_iommu_init.o
+obj-$(CONFIG_AMD_IOMMU_V2) += amd_iommu_v2.o
 obj-$(CONFIG_DMAR_TABLE) += dmar.o
 obj-$(CONFIG_INTEL_IOMMU) += iova.o intel-iommu.o
 obj-$(CONFIG_IRQ_REMAP) += intr_remapping.o
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 4ee277a8521a..cce1f03b8895 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -17,6 +17,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  */
 
+#include <linux/ratelimit.h>
 #include <linux/pci.h>
 #include <linux/pci-ats.h>
 #include <linux/bitmap.h>
@@ -28,6 +29,8 @@
 #include <linux/iommu.h>
 #include <linux/delay.h>
 #include <linux/amd-iommu.h>
+#include <linux/notifier.h>
+#include <linux/export.h>
 #include <asm/msidef.h>
 #include <asm/proto.h>
 #include <asm/iommu.h>
@@ -41,6 +44,24 @@
 
 #define LOOP_TIMEOUT	100000
 
+/*
+ * This bitmap is used to advertise the page sizes our hardware support
+ * to the IOMMU core, which will then use this information to split
+ * physically contiguous memory regions it is mapping into page sizes
+ * that we support.
+ *
+ * Traditionally the IOMMU core just handed us the mappings directly,
+ * after making sure the size is an order of a 4KiB page and that the
+ * mapping has natural alignment.
+ *
+ * To retain this behavior, we currently advertise that we support
+ * all page sizes that are an order of 4KiB.
+ *
+ * If at some point we'd like to utilize the IOMMU core's new behavior,
+ * we could change this to advertise the real page sizes we support.
+ */
+#define AMD_IOMMU_PGSIZES	(~0xFFFUL)
+
 static DEFINE_RWLOCK(amd_iommu_devtable_lock);
 
 /* A list of preallocated protection domains */
@@ -59,6 +80,9 @@ static struct protection_domain *pt_domain;
 
 static struct iommu_ops amd_iommu_ops;
 
+static ATOMIC_NOTIFIER_HEAD(ppr_notifier);
+int amd_iommu_max_glx_val = -1;
+
 /*
  * general struct to manage commands send to an IOMMU
  */
@@ -67,6 +91,7 @@ struct iommu_cmd {
 };
 
 static void update_domain(struct protection_domain *domain);
+static int __init alloc_passthrough_domain(void);
 
 /****************************************************************************
  *
@@ -147,6 +172,33 @@ static struct iommu_dev_data *get_dev_data(struct device *dev)
 	return dev->archdata.iommu;
 }
 
+static bool pci_iommuv2_capable(struct pci_dev *pdev)
+{
+	static const int caps[] = {
+		PCI_EXT_CAP_ID_ATS,
+		PCI_EXT_CAP_ID_PRI,
+		PCI_EXT_CAP_ID_PASID,
+	};
+	int i, pos;
+
+	for (i = 0; i < 3; ++i) {
+		pos = pci_find_ext_capability(pdev, caps[i]);
+		if (pos == 0)
+			return false;
+	}
+
+	return true;
+}
+
+static bool pdev_pri_erratum(struct pci_dev *pdev, u32 erratum)
+{
+	struct iommu_dev_data *dev_data;
+
+	dev_data = get_dev_data(&pdev->dev);
+
+	return dev_data->errata & (1 << erratum) ? true : false;
+}
+
 /*
  * In this function the list of preallocated protection domains is traversed to
  * find the domain for a specific device
@@ -204,6 +256,7 @@ static bool check_device(struct device *dev)
 
 static int iommu_init_device(struct device *dev)
 {
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct iommu_dev_data *dev_data;
 	u16 alias;
 
@@ -228,6 +281,13 @@ static int iommu_init_device(struct device *dev)
 		dev_data->alias_data = alias_data;
 	}
 
+	if (pci_iommuv2_capable(pdev)) {
+		struct amd_iommu *iommu;
+
+		iommu              = amd_iommu_rlookup_table[dev_data->devid];
+		dev_data->iommu_v2 = iommu->is_iommu_v2;
+	}
+
 	dev->archdata.iommu = dev_data;
 
 	return 0;
@@ -317,6 +377,11 @@ DECLARE_STATS_COUNTER(domain_flush_single);
 DECLARE_STATS_COUNTER(domain_flush_all);
 DECLARE_STATS_COUNTER(alloced_io_mem);
 DECLARE_STATS_COUNTER(total_map_requests);
+DECLARE_STATS_COUNTER(complete_ppr);
+DECLARE_STATS_COUNTER(invalidate_iotlb);
+DECLARE_STATS_COUNTER(invalidate_iotlb_all);
+DECLARE_STATS_COUNTER(pri_requests);
+
 
 static struct dentry *stats_dir;
 static struct dentry *de_fflush;
@@ -351,6 +416,10 @@ static void amd_iommu_stats_init(void)
 	amd_iommu_stats_add(&domain_flush_all);
 	amd_iommu_stats_add(&alloced_io_mem);
 	amd_iommu_stats_add(&total_map_requests);
+	amd_iommu_stats_add(&complete_ppr);
+	amd_iommu_stats_add(&invalidate_iotlb);
+	amd_iommu_stats_add(&invalidate_iotlb_all);
+	amd_iommu_stats_add(&pri_requests);
 }
 
 #endif
@@ -365,8 +434,8 @@ static void dump_dte_entry(u16 devid)
 {
 	int i;
 
-	for (i = 0; i < 8; ++i)
-		pr_err("AMD-Vi: DTE[%d]: %08x\n", i,
+	for (i = 0; i < 4; ++i)
+		pr_err("AMD-Vi: DTE[%d]: %016llx\n", i,
 			amd_iommu_dev_table[devid].data[i]);
 }
 
@@ -461,12 +530,84 @@ static void iommu_poll_events(struct amd_iommu *iommu)
 	spin_unlock_irqrestore(&iommu->lock, flags);
 }
 
+static void iommu_handle_ppr_entry(struct amd_iommu *iommu, u32 head)
+{
+	struct amd_iommu_fault fault;
+	volatile u64 *raw;
+	int i;
+
+	INC_STATS_COUNTER(pri_requests);
+
+	raw = (u64 *)(iommu->ppr_log + head);
+
+	/*
+	 * Hardware bug: Interrupt may arrive before the entry is written to
+	 * memory. If this happens we need to wait for the entry to arrive.
+	 */
+	for (i = 0; i < LOOP_TIMEOUT; ++i) {
+		if (PPR_REQ_TYPE(raw[0]) != 0)
+			break;
+		udelay(1);
+	}
+
+	if (PPR_REQ_TYPE(raw[0]) != PPR_REQ_FAULT) {
+		pr_err_ratelimited("AMD-Vi: Unknown PPR request received\n");
+		return;
+	}
+
+	fault.address   = raw[1];
+	fault.pasid     = PPR_PASID(raw[0]);
+	fault.device_id = PPR_DEVID(raw[0]);
+	fault.tag       = PPR_TAG(raw[0]);
+	fault.flags     = PPR_FLAGS(raw[0]);
+
+	/*
+	 * To detect the hardware bug we need to clear the entry
+	 * to back to zero.
+	 */
+	raw[0] = raw[1] = 0;
+
+	atomic_notifier_call_chain(&ppr_notifier, 0, &fault);
+}
+
+static void iommu_poll_ppr_log(struct amd_iommu *iommu)
+{
+	unsigned long flags;
+	u32 head, tail;
+
+	if (iommu->ppr_log == NULL)
+		return;
+
+	spin_lock_irqsave(&iommu->lock, flags);
+
+	head = readl(iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
+	tail = readl(iommu->mmio_base + MMIO_PPR_TAIL_OFFSET);
+
+	while (head != tail) {
+
+		/* Handle PPR entry */
+		iommu_handle_ppr_entry(iommu, head);
+
+		/* Update and refresh ring-buffer state*/
+		head = (head + PPR_ENTRY_SIZE) % PPR_LOG_SIZE;
+		writel(head, iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
+		tail = readl(iommu->mmio_base + MMIO_PPR_TAIL_OFFSET);
+	}
+
+	/* enable ppr interrupts again */
+	writel(MMIO_STATUS_PPR_INT_MASK, iommu->mmio_base + MMIO_STATUS_OFFSET);
+
+	spin_unlock_irqrestore(&iommu->lock, flags);
+}
+
 irqreturn_t amd_iommu_int_thread(int irq, void *data)
 {
 	struct amd_iommu *iommu;
 
-	for_each_iommu(iommu)
+	for_each_iommu(iommu) {
 		iommu_poll_events(iommu);
+		iommu_poll_ppr_log(iommu);
+	}
 
 	return IRQ_HANDLED;
 }
@@ -595,6 +736,60 @@ static void build_inv_iotlb_pages(struct iommu_cmd *cmd, u16 devid, int qdep,
 		cmd->data[2] |= CMD_INV_IOMMU_PAGES_SIZE_MASK;
 }
 
+static void build_inv_iommu_pasid(struct iommu_cmd *cmd, u16 domid, int pasid,
+				  u64 address, bool size)
+{
+	memset(cmd, 0, sizeof(*cmd));
+
+	address &= ~(0xfffULL);
+
+	cmd->data[0]  = pasid & PASID_MASK;
+	cmd->data[1]  = domid;
+	cmd->data[2]  = lower_32_bits(address);
+	cmd->data[3]  = upper_32_bits(address);
+	cmd->data[2] |= CMD_INV_IOMMU_PAGES_PDE_MASK;
+	cmd->data[2] |= CMD_INV_IOMMU_PAGES_GN_MASK;
+	if (size)
+		cmd->data[2] |= CMD_INV_IOMMU_PAGES_SIZE_MASK;
+	CMD_SET_TYPE(cmd, CMD_INV_IOMMU_PAGES);
+}
+
+static void build_inv_iotlb_pasid(struct iommu_cmd *cmd, u16 devid, int pasid,
+				  int qdep, u64 address, bool size)
+{
+	memset(cmd, 0, sizeof(*cmd));
+
+	address &= ~(0xfffULL);
+
+	cmd->data[0]  = devid;
+	cmd->data[0] |= (pasid & 0xff) << 16;
+	cmd->data[0] |= (qdep  & 0xff) << 24;
+	cmd->data[1]  = devid;
+	cmd->data[1] |= ((pasid >> 8) & 0xfff) << 16;
+	cmd->data[2]  = lower_32_bits(address);
+	cmd->data[2] |= CMD_INV_IOMMU_PAGES_GN_MASK;
+	cmd->data[3]  = upper_32_bits(address);
+	if (size)
+		cmd->data[2] |= CMD_INV_IOMMU_PAGES_SIZE_MASK;
+	CMD_SET_TYPE(cmd, CMD_INV_IOTLB_PAGES);
+}
+
+static void build_complete_ppr(struct iommu_cmd *cmd, u16 devid, int pasid,
+			       int status, int tag, bool gn)
+{
+	memset(cmd, 0, sizeof(*cmd));
+
+	cmd->data[0]  = devid;
+	if (gn) {
+		cmd->data[1]  = pasid & PASID_MASK;
+		cmd->data[2]  = CMD_INV_IOMMU_PAGES_GN_MASK;
+	}
+	cmd->data[3]  = tag & 0x1ff;
+	cmd->data[3] |= (status & PPR_STATUS_MASK) << PPR_STATUS_SHIFT;
+
+	CMD_SET_TYPE(cmd, CMD_COMPLETE_PPR);
+}
+
 static void build_inv_all(struct iommu_cmd *cmd)
 {
 	memset(cmd, 0, sizeof(*cmd));
@@ -1496,6 +1691,48 @@ static void free_pagetable(struct protection_domain *domain)
 	domain->pt_root = NULL;
 }
 
+static void free_gcr3_tbl_level1(u64 *tbl)
+{
+	u64 *ptr;
+	int i;
+
+	for (i = 0; i < 512; ++i) {
+		if (!(tbl[i] & GCR3_VALID))
+			continue;
+
+		ptr = __va(tbl[i] & PAGE_MASK);
+
+		free_page((unsigned long)ptr);
+	}
+}
+
+static void free_gcr3_tbl_level2(u64 *tbl)
+{
+	u64 *ptr;
+	int i;
+
+	for (i = 0; i < 512; ++i) {
+		if (!(tbl[i] & GCR3_VALID))
+			continue;
+
+		ptr = __va(tbl[i] & PAGE_MASK);
+
+		free_gcr3_tbl_level1(ptr);
+	}
+}
+
+static void free_gcr3_table(struct protection_domain *domain)
+{
+	if (domain->glx == 2)
+		free_gcr3_tbl_level2(domain->gcr3_tbl);
+	else if (domain->glx == 1)
+		free_gcr3_tbl_level1(domain->gcr3_tbl);
+	else if (domain->glx != 0)
+		BUG();
+
+	free_page((unsigned long)domain->gcr3_tbl);
+}
+
 /*
  * Free a domain, only used if something went wrong in the
  * allocation path and we need to free an already allocated page table
@@ -1582,20 +1819,52 @@ static bool dma_ops_domain(struct protection_domain *domain)
 
 static void set_dte_entry(u16 devid, struct protection_domain *domain, bool ats)
 {
-	u64 pte_root = virt_to_phys(domain->pt_root);
-	u32 flags = 0;
+	u64 pte_root = 0;
+	u64 flags = 0;
+
+	if (domain->mode != PAGE_MODE_NONE)
+		pte_root = virt_to_phys(domain->pt_root);
 
 	pte_root |= (domain->mode & DEV_ENTRY_MODE_MASK)
 		    << DEV_ENTRY_MODE_SHIFT;
 	pte_root |= IOMMU_PTE_IR | IOMMU_PTE_IW | IOMMU_PTE_P | IOMMU_PTE_TV;
 
+	flags = amd_iommu_dev_table[devid].data[1];
+
 	if (ats)
 		flags |= DTE_FLAG_IOTLB;
 
-	amd_iommu_dev_table[devid].data[3] |= flags;
-	amd_iommu_dev_table[devid].data[2]  = domain->id;
-	amd_iommu_dev_table[devid].data[1]  = upper_32_bits(pte_root);
-	amd_iommu_dev_table[devid].data[0]  = lower_32_bits(pte_root);
+	if (domain->flags & PD_IOMMUV2_MASK) {
+		u64 gcr3 = __pa(domain->gcr3_tbl);
+		u64 glx  = domain->glx;
+		u64 tmp;
+
+		pte_root |= DTE_FLAG_GV;
+		pte_root |= (glx & DTE_GLX_MASK) << DTE_GLX_SHIFT;
+
+		/* First mask out possible old values for GCR3 table */
+		tmp = DTE_GCR3_VAL_B(~0ULL) << DTE_GCR3_SHIFT_B;
+		flags    &= ~tmp;
+
+		tmp = DTE_GCR3_VAL_C(~0ULL) << DTE_GCR3_SHIFT_C;
+		flags    &= ~tmp;
+
+		/* Encode GCR3 table into DTE */
+		tmp = DTE_GCR3_VAL_A(gcr3) << DTE_GCR3_SHIFT_A;
+		pte_root |= tmp;
+
+		tmp = DTE_GCR3_VAL_B(gcr3) << DTE_GCR3_SHIFT_B;
+		flags    |= tmp;
+
+		tmp = DTE_GCR3_VAL_C(gcr3) << DTE_GCR3_SHIFT_C;
+		flags    |= tmp;
+	}
+
+	flags &= ~(0xffffUL);
+	flags |= domain->id;
+
+	amd_iommu_dev_table[devid].data[1]  = flags;
+	amd_iommu_dev_table[devid].data[0]  = pte_root;
 }
 
 static void clear_dte_entry(u16 devid)
@@ -1603,7 +1872,6 @@ static void clear_dte_entry(u16 devid)
 	/* remove entry from the device table seen by the hardware */
 	amd_iommu_dev_table[devid].data[0] = IOMMU_PTE_P | IOMMU_PTE_TV;
 	amd_iommu_dev_table[devid].data[1] = 0;
-	amd_iommu_dev_table[devid].data[2] = 0;
 
 	amd_iommu_apply_erratum_63(devid);
 }
@@ -1696,6 +1964,93 @@ out_unlock:
 	return ret;
 }
 
+
+static void pdev_iommuv2_disable(struct pci_dev *pdev)
+{
+	pci_disable_ats(pdev);
+	pci_disable_pri(pdev);
+	pci_disable_pasid(pdev);
+}
+
+/* FIXME: Change generic reset-function to do the same */
+static int pri_reset_while_enabled(struct pci_dev *pdev)
+{
+	u16 control;
+	int pos;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
+	if (!pos)
+		return -EINVAL;
+
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+	control |= PCI_PRI_CTRL_RESET;
+	pci_write_config_word(pdev, pos + PCI_PRI_CTRL, control);
+
+	return 0;
+}
+
+static int pdev_iommuv2_enable(struct pci_dev *pdev)
+{
+	bool reset_enable;
+	int reqs, ret;
+
+	/* FIXME: Hardcode number of outstanding requests for now */
+	reqs = 32;
+	if (pdev_pri_erratum(pdev, AMD_PRI_DEV_ERRATUM_LIMIT_REQ_ONE))
+		reqs = 1;
+	reset_enable = pdev_pri_erratum(pdev, AMD_PRI_DEV_ERRATUM_ENABLE_RESET);
+
+	/* Only allow access to user-accessible pages */
+	ret = pci_enable_pasid(pdev, 0);
+	if (ret)
+		goto out_err;
+
+	/* First reset the PRI state of the device */
+	ret = pci_reset_pri(pdev);
+	if (ret)
+		goto out_err;
+
+	/* Enable PRI */
+	ret = pci_enable_pri(pdev, reqs);
+	if (ret)
+		goto out_err;
+
+	if (reset_enable) {
+		ret = pri_reset_while_enabled(pdev);
+		if (ret)
+			goto out_err;
+	}
+
+	ret = pci_enable_ats(pdev, PAGE_SHIFT);
+	if (ret)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	pci_disable_pri(pdev);
+	pci_disable_pasid(pdev);
+
+	return ret;
+}
+
+/* FIXME: Move this to PCI code */
+#define PCI_PRI_TLP_OFF		(1 << 2)
+
+bool pci_pri_tlp_required(struct pci_dev *pdev)
+{
+	u16 control;
+	int pos;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
+	if (!pos)
+		return false;
+
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+
+	return (control & PCI_PRI_TLP_OFF) ? true : false;
+}
+
 /*
  * If a device is not yet associated with a domain, this function does
  * assigns it visible for the hardware
@@ -1710,7 +2065,18 @@ static int attach_device(struct device *dev,
 
 	dev_data = get_dev_data(dev);
 
-	if (amd_iommu_iotlb_sup && pci_enable_ats(pdev, PAGE_SHIFT) == 0) {
+	if (domain->flags & PD_IOMMUV2_MASK) {
+		if (!dev_data->iommu_v2 || !dev_data->passthrough)
+			return -EINVAL;
+
+		if (pdev_iommuv2_enable(pdev) != 0)
+			return -EINVAL;
+
+		dev_data->ats.enabled = true;
+		dev_data->ats.qdep    = pci_ats_queue_depth(pdev);
+		dev_data->pri_tlp     = pci_pri_tlp_required(pdev);
+	} else if (amd_iommu_iotlb_sup &&
+		   pci_enable_ats(pdev, PAGE_SHIFT) == 0) {
 		dev_data->ats.enabled = true;
 		dev_data->ats.qdep    = pci_ats_queue_depth(pdev);
 	}
@@ -1760,7 +2126,7 @@ static void __detach_device(struct iommu_dev_data *dev_data)
 	 * passthrough domain if it is detached from any other domain.
 	 * Make sure we can deassign from the pt_domain itself.
 	 */
-	if (iommu_pass_through &&
+	if (dev_data->passthrough &&
 	    (dev_data->domain == NULL && domain != pt_domain))
 		__attach_device(dev_data, pt_domain);
 }
@@ -1770,20 +2136,24 @@ static void __detach_device(struct iommu_dev_data *dev_data)
  */
 static void detach_device(struct device *dev)
 {
+	struct protection_domain *domain;
 	struct iommu_dev_data *dev_data;
 	unsigned long flags;
 
 	dev_data = get_dev_data(dev);
+	domain   = dev_data->domain;
 
 	/* lock device table */
 	write_lock_irqsave(&amd_iommu_devtable_lock, flags);
 	__detach_device(dev_data);
 	write_unlock_irqrestore(&amd_iommu_devtable_lock, flags);
 
-	if (dev_data->ats.enabled) {
+	if (domain->flags & PD_IOMMUV2_MASK)
+		pdev_iommuv2_disable(to_pci_dev(dev));
+	else if (dev_data->ats.enabled)
 		pci_disable_ats(to_pci_dev(dev));
-		dev_data->ats.enabled = false;
-	}
+
+	dev_data->ats.enabled = false;
 }
 
 /*
@@ -1818,18 +2188,20 @@ static struct protection_domain *domain_for_device(struct device *dev)
 static int device_change_notifier(struct notifier_block *nb,
 				  unsigned long action, void *data)
 {
-	struct device *dev = data;
-	u16 devid;
-	struct protection_domain *domain;
 	struct dma_ops_domain *dma_domain;
+	struct protection_domain *domain;
+	struct iommu_dev_data *dev_data;
+	struct device *dev = data;
 	struct amd_iommu *iommu;
 	unsigned long flags;
+	u16 devid;
 
 	if (!check_device(dev))
 		return 0;
 
-	devid  = get_device_id(dev);
-	iommu  = amd_iommu_rlookup_table[devid];
+	devid    = get_device_id(dev);
+	iommu    = amd_iommu_rlookup_table[devid];
+	dev_data = get_dev_data(dev);
 
 	switch (action) {
 	case BUS_NOTIFY_UNBOUND_DRIVER:
@@ -1838,7 +2210,7 @@ static int device_change_notifier(struct notifier_block *nb,
 
 		if (!domain)
 			goto out;
-		if (iommu_pass_through)
+		if (dev_data->passthrough)
 			break;
 		detach_device(dev);
 		break;
@@ -2434,8 +2806,9 @@ static int amd_iommu_dma_supported(struct device *dev, u64 mask)
  */
 static void prealloc_protection_domains(void)
 {
-	struct pci_dev *dev = NULL;
+	struct iommu_dev_data *dev_data;
 	struct dma_ops_domain *dma_dom;
+	struct pci_dev *dev = NULL;
 	u16 devid;
 
 	for_each_pci_dev(dev) {
@@ -2444,6 +2817,16 @@ static void prealloc_protection_domains(void)
 		if (!check_device(&dev->dev))
 			continue;
 
+		dev_data = get_dev_data(&dev->dev);
+		if (!amd_iommu_force_isolation && dev_data->iommu_v2) {
+			/* Make sure passthrough domain is allocated */
+			alloc_passthrough_domain();
+			dev_data->passthrough = true;
+			attach_device(&dev->dev, pt_domain);
+			pr_info("AMD-Vi: Using passthough domain for device %s\n",
+				dev_name(&dev->dev));
+		}
+
 		/* Is there already any domain for it? */
 		if (domain_for_device(&dev->dev))
 			continue;
@@ -2474,6 +2857,7 @@ static struct dma_map_ops amd_iommu_dma_ops = {
 
 static unsigned device_dma_ops_init(void)
 {
+	struct iommu_dev_data *dev_data;
 	struct pci_dev *pdev = NULL;
 	unsigned unhandled = 0;
 
@@ -2483,7 +2867,12 @@ static unsigned device_dma_ops_init(void)
 			continue;
 		}
 
-		pdev->dev.archdata.dma_ops = &amd_iommu_dma_ops;
+		dev_data = get_dev_data(&pdev->dev);
+
+		if (!dev_data->passthrough)
+			pdev->dev.archdata.dma_ops = &amd_iommu_dma_ops;
+		else
+			pdev->dev.archdata.dma_ops = &nommu_dma_ops;
 	}
 
 	return unhandled;
@@ -2610,6 +2999,20 @@ out_err:
 	return NULL;
 }
 
+static int __init alloc_passthrough_domain(void)
+{
+	if (pt_domain != NULL)
+		return 0;
+
+	/* allocate passthrough domain */
+	pt_domain = protection_domain_alloc();
+	if (!pt_domain)
+		return -ENOMEM;
+
+	pt_domain->mode = PAGE_MODE_NONE;
+
+	return 0;
+}
 static int amd_iommu_domain_init(struct iommu_domain *dom)
 {
 	struct protection_domain *domain;
@@ -2623,6 +3026,8 @@ static int amd_iommu_domain_init(struct iommu_domain *dom)
 	if (!domain->pt_root)
 		goto out_free;
 
+	domain->iommu_domain = dom;
+
 	dom->priv = domain;
 
 	return 0;
@@ -2645,7 +3050,11 @@ static void amd_iommu_domain_destroy(struct iommu_domain *dom)
 
 	BUG_ON(domain->dev_cnt != 0);
 
-	free_pagetable(domain);
+	if (domain->mode != PAGE_MODE_NONE)
+		free_pagetable(domain);
+
+	if (domain->flags & PD_IOMMUV2_MASK)
+		free_gcr3_table(domain);
 
 	protection_domain_free(domain);
 
@@ -2702,13 +3111,15 @@ static int amd_iommu_attach_device(struct iommu_domain *dom,
 }
 
 static int amd_iommu_map(struct iommu_domain *dom, unsigned long iova,
-			 phys_addr_t paddr, int gfp_order, int iommu_prot)
+			 phys_addr_t paddr, size_t page_size, int iommu_prot)
 {
-	unsigned long page_size = 0x1000UL << gfp_order;
 	struct protection_domain *domain = dom->priv;
 	int prot = 0;
 	int ret;
 
+	if (domain->mode == PAGE_MODE_NONE)
+		return -EINVAL;
+
 	if (iommu_prot & IOMMU_READ)
 		prot |= IOMMU_PROT_IR;
 	if (iommu_prot & IOMMU_WRITE)
@@ -2721,13 +3132,14 @@ static int amd_iommu_map(struct iommu_domain *dom, unsigned long iova,
 	return ret;
 }
 
-static int amd_iommu_unmap(struct iommu_domain *dom, unsigned long iova,
-			   int gfp_order)
+static size_t amd_iommu_unmap(struct iommu_domain *dom, unsigned long iova,
+			   size_t page_size)
 {
 	struct protection_domain *domain = dom->priv;
-	unsigned long page_size, unmap_size;
+	size_t unmap_size;
 
-	page_size  = 0x1000UL << gfp_order;
+	if (domain->mode == PAGE_MODE_NONE)
+		return -EINVAL;
 
 	mutex_lock(&domain->api_lock);
 	unmap_size = iommu_unmap_page(domain, iova, page_size);
@@ -2735,7 +3147,7 @@ static int amd_iommu_unmap(struct iommu_domain *dom, unsigned long iova,
 
 	domain_flush_tlb_pde(domain);
 
-	return get_order(unmap_size);
+	return unmap_size;
 }
 
 static phys_addr_t amd_iommu_iova_to_phys(struct iommu_domain *dom,
@@ -2746,6 +3158,9 @@ static phys_addr_t amd_iommu_iova_to_phys(struct iommu_domain *dom,
 	phys_addr_t paddr;
 	u64 *pte, __pte;
 
+	if (domain->mode == PAGE_MODE_NONE)
+		return iova;
+
 	pte = fetch_pte(domain, iova);
 
 	if (!pte || !IOMMU_PTE_PRESENT(*pte))
@@ -2773,6 +3188,26 @@ static int amd_iommu_domain_has_cap(struct iommu_domain *domain,
 	return 0;
 }
 
+static int amd_iommu_device_group(struct device *dev, unsigned int *groupid)
+{
+	struct iommu_dev_data *dev_data = dev->archdata.iommu;
+	struct pci_dev *pdev = to_pci_dev(dev);
+	u16 devid;
+
+	if (!dev_data)
+		return -ENODEV;
+
+	if (pdev->is_virtfn || !iommu_group_mf)
+		devid = dev_data->devid;
+	else
+		devid = calc_devid(pdev->bus->number,
+				   PCI_DEVFN(PCI_SLOT(pdev->devfn), 0));
+
+	*groupid = amd_iommu_alias_table[devid];
+
+	return 0;
+}
+
 static struct iommu_ops amd_iommu_ops = {
 	.domain_init = amd_iommu_domain_init,
 	.domain_destroy = amd_iommu_domain_destroy,
@@ -2782,6 +3217,8 @@ static struct iommu_ops amd_iommu_ops = {
 	.unmap = amd_iommu_unmap,
 	.iova_to_phys = amd_iommu_iova_to_phys,
 	.domain_has_cap = amd_iommu_domain_has_cap,
+	.device_group = amd_iommu_device_group,
+	.pgsize_bitmap	= AMD_IOMMU_PGSIZES,
 };
 
 /*****************************************************************************
@@ -2796,21 +3233,23 @@ static struct iommu_ops amd_iommu_ops = {
 
 int __init amd_iommu_init_passthrough(void)
 {
-	struct amd_iommu *iommu;
+	struct iommu_dev_data *dev_data;
 	struct pci_dev *dev = NULL;
+	struct amd_iommu *iommu;
 	u16 devid;
+	int ret;
 
-	/* allocate passthrough domain */
-	pt_domain = protection_domain_alloc();
-	if (!pt_domain)
-		return -ENOMEM;
-
-	pt_domain->mode |= PAGE_MODE_NONE;
+	ret = alloc_passthrough_domain();
+	if (ret)
+		return ret;
 
 	for_each_pci_dev(dev) {
 		if (!check_device(&dev->dev))
 			continue;
 
+		dev_data = get_dev_data(&dev->dev);
+		dev_data->passthrough = true;
+
 		devid = get_device_id(&dev->dev);
 
 		iommu = amd_iommu_rlookup_table[devid];
@@ -2820,7 +3259,375 @@ int __init amd_iommu_init_passthrough(void)
 		attach_device(&dev->dev, pt_domain);
 	}
 
+	amd_iommu_stats_init();
+
 	pr_info("AMD-Vi: Initialized for Passthrough Mode\n");
 
 	return 0;
 }
+
+/* IOMMUv2 specific functions */
+int amd_iommu_register_ppr_notifier(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&ppr_notifier, nb);
+}
+EXPORT_SYMBOL(amd_iommu_register_ppr_notifier);
+
+int amd_iommu_unregister_ppr_notifier(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_unregister(&ppr_notifier, nb);
+}
+EXPORT_SYMBOL(amd_iommu_unregister_ppr_notifier);
+
+void amd_iommu_domain_direct_map(struct iommu_domain *dom)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&domain->lock, flags);
+
+	/* Update data structure */
+	domain->mode    = PAGE_MODE_NONE;
+	domain->updated = true;
+
+	/* Make changes visible to IOMMUs */
+	update_domain(domain);
+
+	/* Page-table is not visible to IOMMU anymore, so free it */
+	free_pagetable(domain);
+
+	spin_unlock_irqrestore(&domain->lock, flags);
+}
+EXPORT_SYMBOL(amd_iommu_domain_direct_map);
+
+int amd_iommu_domain_enable_v2(struct iommu_domain *dom, int pasids)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int levels, ret;
+
+	if (pasids <= 0 || pasids > (PASID_MASK + 1))
+		return -EINVAL;
+
+	/* Number of GCR3 table levels required */
+	for (levels = 0; (pasids - 1) & ~0x1ff; pasids >>= 9)
+		levels += 1;
+
+	if (levels > amd_iommu_max_glx_val)
+		return -EINVAL;
+
+	spin_lock_irqsave(&domain->lock, flags);
+
+	/*
+	 * Save us all sanity checks whether devices already in the
+	 * domain support IOMMUv2. Just force that the domain has no
+	 * devices attached when it is switched into IOMMUv2 mode.
+	 */
+	ret = -EBUSY;
+	if (domain->dev_cnt > 0 || domain->flags & PD_IOMMUV2_MASK)
+		goto out;
+
+	ret = -ENOMEM;
+	domain->gcr3_tbl = (void *)get_zeroed_page(GFP_ATOMIC);
+	if (domain->gcr3_tbl == NULL)
+		goto out;
+
+	domain->glx      = levels;
+	domain->flags   |= PD_IOMMUV2_MASK;
+	domain->updated  = true;
+
+	update_domain(domain);
+
+	ret = 0;
+
+out:
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_domain_enable_v2);
+
+static int __flush_pasid(struct protection_domain *domain, int pasid,
+			 u64 address, bool size)
+{
+	struct iommu_dev_data *dev_data;
+	struct iommu_cmd cmd;
+	int i, ret;
+
+	if (!(domain->flags & PD_IOMMUV2_MASK))
+		return -EINVAL;
+
+	build_inv_iommu_pasid(&cmd, domain->id, pasid, address, size);
+
+	/*
+	 * IOMMU TLB needs to be flushed before Device TLB to
+	 * prevent device TLB refill from IOMMU TLB
+	 */
+	for (i = 0; i < amd_iommus_present; ++i) {
+		if (domain->dev_iommu[i] == 0)
+			continue;
+
+		ret = iommu_queue_command(amd_iommus[i], &cmd);
+		if (ret != 0)
+			goto out;
+	}
+
+	/* Wait until IOMMU TLB flushes are complete */
+	domain_flush_complete(domain);
+
+	/* Now flush device TLBs */
+	list_for_each_entry(dev_data, &domain->dev_list, list) {
+		struct amd_iommu *iommu;
+		int qdep;
+
+		BUG_ON(!dev_data->ats.enabled);
+
+		qdep  = dev_data->ats.qdep;
+		iommu = amd_iommu_rlookup_table[dev_data->devid];
+
+		build_inv_iotlb_pasid(&cmd, dev_data->devid, pasid,
+				      qdep, address, size);
+
+		ret = iommu_queue_command(iommu, &cmd);
+		if (ret != 0)
+			goto out;
+	}
+
+	/* Wait until all device TLBs are flushed */
+	domain_flush_complete(domain);
+
+	ret = 0;
+
+out:
+
+	return ret;
+}
+
+static int __amd_iommu_flush_page(struct protection_domain *domain, int pasid,
+				  u64 address)
+{
+	INC_STATS_COUNTER(invalidate_iotlb);
+
+	return __flush_pasid(domain, pasid, address, false);
+}
+
+int amd_iommu_flush_page(struct iommu_domain *dom, int pasid,
+			 u64 address)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&domain->lock, flags);
+	ret = __amd_iommu_flush_page(domain, pasid, address);
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_flush_page);
+
+static int __amd_iommu_flush_tlb(struct protection_domain *domain, int pasid)
+{
+	INC_STATS_COUNTER(invalidate_iotlb_all);
+
+	return __flush_pasid(domain, pasid, CMD_INV_IOMMU_ALL_PAGES_ADDRESS,
+			     true);
+}
+
+int amd_iommu_flush_tlb(struct iommu_domain *dom, int pasid)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&domain->lock, flags);
+	ret = __amd_iommu_flush_tlb(domain, pasid);
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_flush_tlb);
+
+static u64 *__get_gcr3_pte(u64 *root, int level, int pasid, bool alloc)
+{
+	int index;
+	u64 *pte;
+
+	while (true) {
+
+		index = (pasid >> (9 * level)) & 0x1ff;
+		pte   = &root[index];
+
+		if (level == 0)
+			break;
+
+		if (!(*pte & GCR3_VALID)) {
+			if (!alloc)
+				return NULL;
+
+			root = (void *)get_zeroed_page(GFP_ATOMIC);
+			if (root == NULL)
+				return NULL;
+
+			*pte = __pa(root) | GCR3_VALID;
+		}
+
+		root = __va(*pte & PAGE_MASK);
+
+		level -= 1;
+	}
+
+	return pte;
+}
+
+static int __set_gcr3(struct protection_domain *domain, int pasid,
+		      unsigned long cr3)
+{
+	u64 *pte;
+
+	if (domain->mode != PAGE_MODE_NONE)
+		return -EINVAL;
+
+	pte = __get_gcr3_pte(domain->gcr3_tbl, domain->glx, pasid, true);
+	if (pte == NULL)
+		return -ENOMEM;
+
+	*pte = (cr3 & PAGE_MASK) | GCR3_VALID;
+
+	return __amd_iommu_flush_tlb(domain, pasid);
+}
+
+static int __clear_gcr3(struct protection_domain *domain, int pasid)
+{
+	u64 *pte;
+
+	if (domain->mode != PAGE_MODE_NONE)
+		return -EINVAL;
+
+	pte = __get_gcr3_pte(domain->gcr3_tbl, domain->glx, pasid, false);
+	if (pte == NULL)
+		return 0;
+
+	*pte = 0;
+
+	return __amd_iommu_flush_tlb(domain, pasid);
+}
+
+int amd_iommu_domain_set_gcr3(struct iommu_domain *dom, int pasid,
+			      unsigned long cr3)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&domain->lock, flags);
+	ret = __set_gcr3(domain, pasid, cr3);
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_domain_set_gcr3);
+
+int amd_iommu_domain_clear_gcr3(struct iommu_domain *dom, int pasid)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&domain->lock, flags);
+	ret = __clear_gcr3(domain, pasid);
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_domain_clear_gcr3);
+
+int amd_iommu_complete_ppr(struct pci_dev *pdev, int pasid,
+			   int status, int tag)
+{
+	struct iommu_dev_data *dev_data;
+	struct amd_iommu *iommu;
+	struct iommu_cmd cmd;
+
+	INC_STATS_COUNTER(complete_ppr);
+
+	dev_data = get_dev_data(&pdev->dev);
+	iommu    = amd_iommu_rlookup_table[dev_data->devid];
+
+	build_complete_ppr(&cmd, dev_data->devid, pasid, status,
+			   tag, dev_data->pri_tlp);
+
+	return iommu_queue_command(iommu, &cmd);
+}
+EXPORT_SYMBOL(amd_iommu_complete_ppr);
+
+struct iommu_domain *amd_iommu_get_v2_domain(struct pci_dev *pdev)
+{
+	struct protection_domain *domain;
+
+	domain = get_domain(&pdev->dev);
+	if (IS_ERR(domain))
+		return NULL;
+
+	/* Only return IOMMUv2 domains */
+	if (!(domain->flags & PD_IOMMUV2_MASK))
+		return NULL;
+
+	return domain->iommu_domain;
+}
+EXPORT_SYMBOL(amd_iommu_get_v2_domain);
+
+void amd_iommu_enable_device_erratum(struct pci_dev *pdev, u32 erratum)
+{
+	struct iommu_dev_data *dev_data;
+
+	if (!amd_iommu_v2_supported())
+		return;
+
+	dev_data = get_dev_data(&pdev->dev);
+	dev_data->errata |= (1 << erratum);
+}
+EXPORT_SYMBOL(amd_iommu_enable_device_erratum);
+
+int amd_iommu_device_info(struct pci_dev *pdev,
+                          struct amd_iommu_device_info *info)
+{
+	int max_pasids;
+	int pos;
+
+	if (pdev == NULL || info == NULL)
+		return -EINVAL;
+
+	if (!amd_iommu_v2_supported())
+		return -EINVAL;
+
+	memset(info, 0, sizeof(*info));
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_ATS);
+	if (pos)
+		info->flags |= AMD_IOMMU_DEVICE_FLAG_ATS_SUP;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
+	if (pos)
+		info->flags |= AMD_IOMMU_DEVICE_FLAG_PRI_SUP;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PASID);
+	if (pos) {
+		int features;
+
+		max_pasids = 1 << (9 * (amd_iommu_max_glx_val + 1));
+		max_pasids = min(max_pasids, (1 << 20));
+
+		info->flags |= AMD_IOMMU_DEVICE_FLAG_PASID_SUP;
+		info->max_pasids = min(pci_max_pasids(pdev), max_pasids);
+
+		features = pci_pasid_features(pdev);
+		if (features & PCI_PASID_CAP_EXEC)
+			info->flags |= AMD_IOMMU_DEVICE_FLAG_EXEC_SUP;
+		if (features & PCI_PASID_CAP_PRIV)
+			info->flags |= AMD_IOMMU_DEVICE_FLAG_PRIV_SUP;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(amd_iommu_device_info);
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 82d2410f4205..bdea288dc185 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -25,6 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/msi.h>
 #include <linux/amd-iommu.h>
+#include <linux/export.h>
 #include <asm/pci-direct.h>
 #include <asm/iommu.h>
 #include <asm/gart.h>
@@ -141,6 +142,12 @@ int amd_iommus_present;
 bool amd_iommu_np_cache __read_mostly;
 bool amd_iommu_iotlb_sup __read_mostly = true;
 
+u32 amd_iommu_max_pasids __read_mostly = ~0;
+
+bool amd_iommu_v2_present __read_mostly;
+
+bool amd_iommu_force_isolation __read_mostly;
+
 /*
  * The ACPI table parsing functions set this variable on an error
  */
@@ -299,6 +306,16 @@ static void iommu_feature_disable(struct amd_iommu *iommu, u8 bit)
 	writel(ctrl, iommu->mmio_base + MMIO_CONTROL_OFFSET);
 }
 
+static void iommu_set_inv_tlb_timeout(struct amd_iommu *iommu, int timeout)
+{
+	u32 ctrl;
+
+	ctrl = readl(iommu->mmio_base + MMIO_CONTROL_OFFSET);
+	ctrl &= ~CTRL_INV_TO_MASK;
+	ctrl |= (timeout << CONTROL_INV_TIMEOUT) & CTRL_INV_TO_MASK;
+	writel(ctrl, iommu->mmio_base + MMIO_CONTROL_OFFSET);
+}
+
 /* Function to enable the hardware */
 static void iommu_enable(struct amd_iommu *iommu)
 {
@@ -581,21 +598,69 @@ static void __init free_event_buffer(struct amd_iommu *iommu)
 	free_pages((unsigned long)iommu->evt_buf, get_order(EVT_BUFFER_SIZE));
 }
 
+/* allocates the memory where the IOMMU will log its events to */
+static u8 * __init alloc_ppr_log(struct amd_iommu *iommu)
+{
+	iommu->ppr_log = (u8 *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
+						get_order(PPR_LOG_SIZE));
+
+	if (iommu->ppr_log == NULL)
+		return NULL;
+
+	return iommu->ppr_log;
+}
+
+static void iommu_enable_ppr_log(struct amd_iommu *iommu)
+{
+	u64 entry;
+
+	if (iommu->ppr_log == NULL)
+		return;
+
+	entry = (u64)virt_to_phys(iommu->ppr_log) | PPR_LOG_SIZE_512;
+
+	memcpy_toio(iommu->mmio_base + MMIO_PPR_LOG_OFFSET,
+		    &entry, sizeof(entry));
+
+	/* set head and tail to zero manually */
+	writel(0x00, iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
+	writel(0x00, iommu->mmio_base + MMIO_PPR_TAIL_OFFSET);
+
+	iommu_feature_enable(iommu, CONTROL_PPFLOG_EN);
+	iommu_feature_enable(iommu, CONTROL_PPR_EN);
+}
+
+static void __init free_ppr_log(struct amd_iommu *iommu)
+{
+	if (iommu->ppr_log == NULL)
+		return;
+
+	free_pages((unsigned long)iommu->ppr_log, get_order(PPR_LOG_SIZE));
+}
+
+static void iommu_enable_gt(struct amd_iommu *iommu)
+{
+	if (!iommu_feature(iommu, FEATURE_GT))
+		return;
+
+	iommu_feature_enable(iommu, CONTROL_GT_EN);
+}
+
 /* sets a specific bit in the device table entry. */
 static void set_dev_entry_bit(u16 devid, u8 bit)
 {
-	int i = (bit >> 5) & 0x07;
-	int _bit = bit & 0x1f;
+	int i = (bit >> 6) & 0x03;
+	int _bit = bit & 0x3f;
 
-	amd_iommu_dev_table[devid].data[i] |= (1 << _bit);
+	amd_iommu_dev_table[devid].data[i] |= (1UL << _bit);
 }
 
 static int get_dev_entry_bit(u16 devid, u8 bit)
 {
-	int i = (bit >> 5) & 0x07;
-	int _bit = bit & 0x1f;
+	int i = (bit >> 6) & 0x03;
+	int _bit = bit & 0x3f;
 
-	return (amd_iommu_dev_table[devid].data[i] & (1 << _bit)) >> _bit;
+	return (amd_iommu_dev_table[devid].data[i] & (1UL << _bit)) >> _bit;
 }
 
 
@@ -699,6 +764,32 @@ static void __init init_iommu_from_pci(struct amd_iommu *iommu)
 
 	iommu->features = ((u64)high << 32) | low;
 
+	if (iommu_feature(iommu, FEATURE_GT)) {
+		int glxval;
+		u32 pasids;
+		u64 shift;
+
+		shift   = iommu->features & FEATURE_PASID_MASK;
+		shift >>= FEATURE_PASID_SHIFT;
+		pasids  = (1 << shift);
+
+		amd_iommu_max_pasids = min(amd_iommu_max_pasids, pasids);
+
+		glxval   = iommu->features & FEATURE_GLXVAL_MASK;
+		glxval >>= FEATURE_GLXVAL_SHIFT;
+
+		if (amd_iommu_max_glx_val == -1)
+			amd_iommu_max_glx_val = glxval;
+		else
+			amd_iommu_max_glx_val = min(amd_iommu_max_glx_val, glxval);
+	}
+
+	if (iommu_feature(iommu, FEATURE_GT) &&
+	    iommu_feature(iommu, FEATURE_PPR)) {
+		iommu->is_iommu_v2   = true;
+		amd_iommu_v2_present = true;
+	}
+
 	if (!is_rd890_iommu(iommu->dev))
 		return;
 
@@ -901,6 +992,7 @@ static void __init free_iommu_one(struct amd_iommu *iommu)
 {
 	free_command_buffer(iommu);
 	free_event_buffer(iommu);
+	free_ppr_log(iommu);
 	iommu_unmap_mmio_space(iommu);
 }
 
@@ -964,6 +1056,12 @@ static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 	init_iommu_from_acpi(iommu, h);
 	init_iommu_devices(iommu);
 
+	if (iommu_feature(iommu, FEATURE_PPR)) {
+		iommu->ppr_log = alloc_ppr_log(iommu);
+		if (!iommu->ppr_log)
+			return -ENOMEM;
+	}
+
 	if (iommu->cap & (1UL << IOMMU_CAP_NPCACHE))
 		amd_iommu_np_cache = true;
 
@@ -1050,6 +1148,9 @@ static int iommu_setup_msi(struct amd_iommu *iommu)
 	iommu->int_enabled = true;
 	iommu_feature_enable(iommu, CONTROL_EVT_INT_EN);
 
+	if (iommu->ppr_log != NULL)
+		iommu_feature_enable(iommu, CONTROL_PPFINT_EN);
+
 	return 0;
 }
 
@@ -1209,6 +1310,9 @@ static void iommu_init_flags(struct amd_iommu *iommu)
 	 * make IOMMU memory accesses cache coherent
 	 */
 	iommu_feature_enable(iommu, CONTROL_COHERENT_EN);
+
+	/* Set IOTLB invalidation timeout to 1s */
+	iommu_set_inv_tlb_timeout(iommu, CTRL_INV_TO_1S);
 }
 
 static void iommu_apply_resume_quirks(struct amd_iommu *iommu)
@@ -1274,6 +1378,8 @@ static void enable_iommus(void)
 		iommu_set_device_table(iommu);
 		iommu_enable_command_buffer(iommu);
 		iommu_enable_event_buffer(iommu);
+		iommu_enable_ppr_log(iommu);
+		iommu_enable_gt(iommu);
 		iommu_set_exclusion_range(iommu);
 		iommu_init_msi(iommu);
 		iommu_enable(iommu);
@@ -1303,13 +1409,6 @@ static void amd_iommu_resume(void)
 
 	/* re-load the hardware */
 	enable_iommus();
-
-	/*
-	 * we have to flush after the IOMMUs are enabled because a
-	 * disabled IOMMU will never execute the commands we send
-	 */
-	for_each_iommu(iommu)
-		iommu_flush_all_caches(iommu);
 }
 
 static int amd_iommu_suspend(void)
@@ -1560,6 +1659,8 @@ static int __init parse_amd_iommu_options(char *str)
 			amd_iommu_unmap_flush = true;
 		if (strncmp(str, "off", 3) == 0)
 			amd_iommu_disabled = true;
+		if (strncmp(str, "force_isolation", 15) == 0)
+			amd_iommu_force_isolation = true;
 	}
 
 	return 1;
@@ -1572,3 +1673,9 @@ IOMMU_INIT_FINISH(amd_iommu_detect,
 		  gart_iommu_hole_init,
 		  0,
 		  0);
+
+bool amd_iommu_v2_supported(void)
+{
+	return amd_iommu_v2_present;
+}
+EXPORT_SYMBOL(amd_iommu_v2_supported);
diff --git a/drivers/iommu/amd_iommu_proto.h b/drivers/iommu/amd_iommu_proto.h
index 7ffaa64410b0..1a7f41c6cc66 100644
--- a/drivers/iommu/amd_iommu_proto.h
+++ b/drivers/iommu/amd_iommu_proto.h
@@ -31,6 +31,30 @@ extern int amd_iommu_init_devices(void);
 extern void amd_iommu_uninit_devices(void);
 extern void amd_iommu_init_notifier(void);
 extern void amd_iommu_init_api(void);
+
+/* IOMMUv2 specific functions */
+struct iommu_domain;
+
+extern bool amd_iommu_v2_supported(void);
+extern int amd_iommu_register_ppr_notifier(struct notifier_block *nb);
+extern int amd_iommu_unregister_ppr_notifier(struct notifier_block *nb);
+extern void amd_iommu_domain_direct_map(struct iommu_domain *dom);
+extern int amd_iommu_domain_enable_v2(struct iommu_domain *dom, int pasids);
+extern int amd_iommu_flush_page(struct iommu_domain *dom, int pasid,
+				u64 address);
+extern int amd_iommu_flush_tlb(struct iommu_domain *dom, int pasid);
+extern int amd_iommu_domain_set_gcr3(struct iommu_domain *dom, int pasid,
+				     unsigned long cr3);
+extern int amd_iommu_domain_clear_gcr3(struct iommu_domain *dom, int pasid);
+extern struct iommu_domain *amd_iommu_get_v2_domain(struct pci_dev *pdev);
+
+#define PPR_SUCCESS			0x0
+#define PPR_INVALID			0x1
+#define PPR_FAILURE			0xf
+
+extern int amd_iommu_complete_ppr(struct pci_dev *pdev, int pasid,
+				  int status, int tag);
+
 #ifndef CONFIG_AMD_IOMMU_STATS
 
 static inline void amd_iommu_stats_init(void) { }
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index 5b9c5075e81a..2452f3b71736 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -69,11 +69,14 @@
 #define MMIO_EXCL_BASE_OFFSET   0x0020
 #define MMIO_EXCL_LIMIT_OFFSET  0x0028
 #define MMIO_EXT_FEATURES	0x0030
+#define MMIO_PPR_LOG_OFFSET	0x0038
 #define MMIO_CMD_HEAD_OFFSET	0x2000
 #define MMIO_CMD_TAIL_OFFSET	0x2008
 #define MMIO_EVT_HEAD_OFFSET	0x2010
 #define MMIO_EVT_TAIL_OFFSET	0x2018
 #define MMIO_STATUS_OFFSET	0x2020
+#define MMIO_PPR_HEAD_OFFSET	0x2030
+#define MMIO_PPR_TAIL_OFFSET	0x2038
 
 
 /* Extended Feature Bits */
@@ -87,8 +90,17 @@
 #define FEATURE_HE		(1ULL<<8)
 #define FEATURE_PC		(1ULL<<9)
 
+#define FEATURE_PASID_SHIFT	32
+#define FEATURE_PASID_MASK	(0x1fULL << FEATURE_PASID_SHIFT)
+
+#define FEATURE_GLXVAL_SHIFT	14
+#define FEATURE_GLXVAL_MASK	(0x03ULL << FEATURE_GLXVAL_SHIFT)
+
+#define PASID_MASK		0x000fffff
+
 /* MMIO status bits */
-#define MMIO_STATUS_COM_WAIT_INT_MASK	0x04
+#define MMIO_STATUS_COM_WAIT_INT_MASK	(1 << 2)
+#define MMIO_STATUS_PPR_INT_MASK	(1 << 6)
 
 /* event logging constants */
 #define EVENT_ENTRY_SIZE	0x10
@@ -115,6 +127,7 @@
 #define CONTROL_EVT_LOG_EN      0x02ULL
 #define CONTROL_EVT_INT_EN      0x03ULL
 #define CONTROL_COMWAIT_EN      0x04ULL
+#define CONTROL_INV_TIMEOUT	0x05ULL
 #define CONTROL_PASSPW_EN       0x08ULL
 #define CONTROL_RESPASSPW_EN    0x09ULL
 #define CONTROL_COHERENT_EN     0x0aULL
@@ -122,18 +135,34 @@
 #define CONTROL_CMDBUF_EN       0x0cULL
 #define CONTROL_PPFLOG_EN       0x0dULL
 #define CONTROL_PPFINT_EN       0x0eULL
+#define CONTROL_PPR_EN          0x0fULL
+#define CONTROL_GT_EN           0x10ULL
+
+#define CTRL_INV_TO_MASK	(7 << CONTROL_INV_TIMEOUT)
+#define CTRL_INV_TO_NONE	0
+#define CTRL_INV_TO_1MS		1
+#define CTRL_INV_TO_10MS	2
+#define CTRL_INV_TO_100MS	3
+#define CTRL_INV_TO_1S		4
+#define CTRL_INV_TO_10S		5
+#define CTRL_INV_TO_100S	6
 
 /* command specific defines */
 #define CMD_COMPL_WAIT          0x01
 #define CMD_INV_DEV_ENTRY       0x02
 #define CMD_INV_IOMMU_PAGES	0x03
 #define CMD_INV_IOTLB_PAGES	0x04
+#define CMD_COMPLETE_PPR	0x07
 #define CMD_INV_ALL		0x08
 
 #define CMD_COMPL_WAIT_STORE_MASK	0x01
 #define CMD_COMPL_WAIT_INT_MASK		0x02
 #define CMD_INV_IOMMU_PAGES_SIZE_MASK	0x01
 #define CMD_INV_IOMMU_PAGES_PDE_MASK	0x02
+#define CMD_INV_IOMMU_PAGES_GN_MASK	0x04
+
+#define PPR_STATUS_MASK			0xf
+#define PPR_STATUS_SHIFT		12
 
 #define CMD_INV_IOMMU_ALL_PAGES_ADDRESS	0x7fffffffffffffffULL
 
@@ -165,6 +194,23 @@
 #define EVT_BUFFER_SIZE		8192 /* 512 entries */
 #define EVT_LEN_MASK		(0x9ULL << 56)
 
+/* Constants for PPR Log handling */
+#define PPR_LOG_ENTRIES		512
+#define PPR_LOG_SIZE_SHIFT	56
+#define PPR_LOG_SIZE_512	(0x9ULL << PPR_LOG_SIZE_SHIFT)
+#define PPR_ENTRY_SIZE		16
+#define PPR_LOG_SIZE		(PPR_ENTRY_SIZE * PPR_LOG_ENTRIES)
+
+#define PPR_REQ_TYPE(x)		(((x) >> 60) & 0xfULL)
+#define PPR_FLAGS(x)		(((x) >> 48) & 0xfffULL)
+#define PPR_DEVID(x)		((x) & 0xffffULL)
+#define PPR_TAG(x)		(((x) >> 32) & 0x3ffULL)
+#define PPR_PASID1(x)		(((x) >> 16) & 0xffffULL)
+#define PPR_PASID2(x)		(((x) >> 42) & 0xfULL)
+#define PPR_PASID(x)		((PPR_PASID2(x) << 16) | PPR_PASID1(x))
+
+#define PPR_REQ_FAULT		0x01
+
 #define PAGE_MODE_NONE    0x00
 #define PAGE_MODE_1_LEVEL 0x01
 #define PAGE_MODE_2_LEVEL 0x02
@@ -230,7 +276,24 @@
 #define IOMMU_PTE_IR (1ULL << 61)
 #define IOMMU_PTE_IW (1ULL << 62)
 
-#define DTE_FLAG_IOTLB	0x01
+#define DTE_FLAG_IOTLB	(0x01UL << 32)
+#define DTE_FLAG_GV	(0x01ULL << 55)
+#define DTE_GLX_SHIFT	(56)
+#define DTE_GLX_MASK	(3)
+
+#define DTE_GCR3_VAL_A(x)	(((x) >> 12) & 0x00007ULL)
+#define DTE_GCR3_VAL_B(x)	(((x) >> 15) & 0x0ffffULL)
+#define DTE_GCR3_VAL_C(x)	(((x) >> 31) & 0xfffffULL)
+
+#define DTE_GCR3_INDEX_A	0
+#define DTE_GCR3_INDEX_B	1
+#define DTE_GCR3_INDEX_C	1
+
+#define DTE_GCR3_SHIFT_A	58
+#define DTE_GCR3_SHIFT_B	16
+#define DTE_GCR3_SHIFT_C	43
+
+#define GCR3_VALID		0x01ULL
 
 #define IOMMU_PAGE_MASK (((1ULL << 52) - 1) & ~0xfffULL)
 #define IOMMU_PTE_PRESENT(pte) ((pte) & IOMMU_PTE_P)
@@ -257,6 +320,7 @@
 					      domain for an IOMMU */
 #define PD_PASSTHROUGH_MASK	(1UL << 2) /* domain has no page
 					      translation */
+#define PD_IOMMUV2_MASK		(1UL << 3) /* domain has gcr3 table */
 
 extern bool amd_iommu_dump;
 #define DUMP_printk(format, arg...)					\
@@ -285,6 +349,29 @@ extern bool amd_iommu_iotlb_sup;
 #define APERTURE_RANGE_INDEX(a)	((a) >> APERTURE_RANGE_SHIFT)
 #define APERTURE_PAGE_INDEX(a)	(((a) >> 21) & 0x3fULL)
 
+
+/*
+ * This struct is used to pass information about
+ * incoming PPR faults around.
+ */
+struct amd_iommu_fault {
+	u64 address;    /* IO virtual address of the fault*/
+	u32 pasid;      /* Address space identifier */
+	u16 device_id;  /* Originating PCI device id */
+	u16 tag;        /* PPR tag */
+	u16 flags;      /* Fault flags */
+
+};
+
+#define PPR_FAULT_EXEC	(1 << 1)
+#define PPR_FAULT_READ  (1 << 2)
+#define PPR_FAULT_WRITE (1 << 5)
+#define PPR_FAULT_USER  (1 << 6)
+#define PPR_FAULT_RSVD  (1 << 7)
+#define PPR_FAULT_GN    (1 << 8)
+
+struct iommu_domain;
+
 /*
  * This structure contains generic data for  IOMMU protection domains
  * independent of their use.
@@ -297,11 +384,15 @@ struct protection_domain {
 	u16 id;			/* the domain id written to the device table */
 	int mode;		/* paging mode (0-6 levels) */
 	u64 *pt_root;		/* page table root pointer */
+	int glx;		/* Number of levels for GCR3 table */
+	u64 *gcr3_tbl;		/* Guest CR3 table */
 	unsigned long flags;	/* flags to find out type of domain */
 	bool updated;		/* complete domain flush required */
 	unsigned dev_cnt;	/* devices assigned to this domain */
 	unsigned dev_iommu[MAX_IOMMUS]; /* per-IOMMU reference count */
 	void *priv;		/* private data */
+	struct iommu_domain *iommu_domain; /* Pointer to generic
+					      domain structure */
 
 };
 
@@ -315,10 +406,15 @@ struct iommu_dev_data {
 	struct protection_domain *domain; /* Domain the device is bound to */
 	atomic_t bind;			  /* Domain attach reverent count */
 	u16 devid;			  /* PCI Device ID */
+	bool iommu_v2;			  /* Device can make use of IOMMUv2 */
+	bool passthrough;		  /* Default for device is pt_domain */
 	struct {
 		bool enabled;
 		int qdep;
 	} ats;				  /* ATS state */
+	bool pri_tlp;			  /* PASID TLB required for
+					     PPR completions */
+	u32 errata;			  /* Bitmap for errata to apply */
 };
 
 /*
@@ -399,6 +495,9 @@ struct amd_iommu {
 	/* Extended features */
 	u64 features;
 
+	/* IOMMUv2 */
+	bool is_iommu_v2;
+
 	/*
 	 * Capability pointer. There could be more than one IOMMU per PCI
 	 * device function if there are more than one AMD IOMMU capability
@@ -431,6 +530,9 @@ struct amd_iommu {
 	/* MSI number for event interrupt */
 	u16 evt_msi_num;
 
+	/* Base of the PPR log, if present */
+	u8 *ppr_log;
+
 	/* true if interrupts for this IOMMU are already enabled */
 	bool int_enabled;
 
@@ -484,7 +586,7 @@ extern struct list_head amd_iommu_pd_list;
  * Structure defining one entry in the device table
  */
 struct dev_table_entry {
-	u32 data[8];
+	u64 data[4];
 };
 
 /*
@@ -549,6 +651,16 @@ extern unsigned long *amd_iommu_pd_alloc_bitmap;
  */
 extern bool amd_iommu_unmap_flush;
 
+/* Smallest number of PASIDs supported by any IOMMU in the system */
+extern u32 amd_iommu_max_pasids;
+
+extern bool amd_iommu_v2_present;
+
+extern bool amd_iommu_force_isolation;
+
+/* Max levels of glxval supported */
+extern int amd_iommu_max_glx_val;
+
 /* takes bus and device/function and returns the device id
  * FIXME: should that be in generic PCI code? */
 static inline u16 calc_devid(u8 bus, u8 devfn)
diff --git a/drivers/iommu/amd_iommu_v2.c b/drivers/iommu/amd_iommu_v2.c
new file mode 100644
index 000000000000..8add9f125d3e
--- /dev/null
+++ b/drivers/iommu/amd_iommu_v2.c
@@ -0,0 +1,994 @@
+/*
+ * Copyright (C) 2010-2012 Advanced Micro Devices, Inc.
+ * Author: Joerg Roedel <joerg.roedel@amd.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ */
+
+#include <linux/mmu_notifier.h>
+#include <linux/amd-iommu.h>
+#include <linux/mm_types.h>
+#include <linux/profile.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/iommu.h>
+#include <linux/wait.h>
+#include <linux/pci.h>
+#include <linux/gfp.h>
+
+#include "amd_iommu_types.h"
+#include "amd_iommu_proto.h"
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Joerg Roedel <joerg.roedel@amd.com>");
+
+#define MAX_DEVICES		0x10000
+#define PRI_QUEUE_SIZE		512
+
+struct pri_queue {
+	atomic_t inflight;
+	bool finish;
+	int status;
+};
+
+struct pasid_state {
+	struct list_head list;			/* For global state-list */
+	atomic_t count;				/* Reference count */
+	struct task_struct *task;		/* Task bound to this PASID */
+	struct mm_struct *mm;			/* mm_struct for the faults */
+	struct mmu_notifier mn;                 /* mmu_otifier handle */
+	struct pri_queue pri[PRI_QUEUE_SIZE];	/* PRI tag states */
+	struct device_state *device_state;	/* Link to our device_state */
+	int pasid;				/* PASID index */
+	spinlock_t lock;			/* Protect pri_queues */
+	wait_queue_head_t wq;			/* To wait for count == 0 */
+};
+
+struct device_state {
+	atomic_t count;
+	struct pci_dev *pdev;
+	struct pasid_state **states;
+	struct iommu_domain *domain;
+	int pasid_levels;
+	int max_pasids;
+	amd_iommu_invalid_ppr_cb inv_ppr_cb;
+	amd_iommu_invalidate_ctx inv_ctx_cb;
+	spinlock_t lock;
+	wait_queue_head_t wq;
+};
+
+struct fault {
+	struct work_struct work;
+	struct device_state *dev_state;
+	struct pasid_state *state;
+	struct mm_struct *mm;
+	u64 address;
+	u16 devid;
+	u16 pasid;
+	u16 tag;
+	u16 finish;
+	u16 flags;
+};
+
+struct device_state **state_table;
+static spinlock_t state_lock;
+
+/* List and lock for all pasid_states */
+static LIST_HEAD(pasid_state_list);
+static DEFINE_SPINLOCK(ps_lock);
+
+static struct workqueue_struct *iommu_wq;
+
+/*
+ * Empty page table - Used between
+ * mmu_notifier_invalidate_range_start and
+ * mmu_notifier_invalidate_range_end
+ */
+static u64 *empty_page_table;
+
+static void free_pasid_states(struct device_state *dev_state);
+static void unbind_pasid(struct device_state *dev_state, int pasid);
+static int task_exit(struct notifier_block *nb, unsigned long e, void *data);
+
+static u16 device_id(struct pci_dev *pdev)
+{
+	u16 devid;
+
+	devid = pdev->bus->number;
+	devid = (devid << 8) | pdev->devfn;
+
+	return devid;
+}
+
+static struct device_state *get_device_state(u16 devid)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+
+	spin_lock_irqsave(&state_lock, flags);
+	dev_state = state_table[devid];
+	if (dev_state != NULL)
+		atomic_inc(&dev_state->count);
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	return dev_state;
+}
+
+static void free_device_state(struct device_state *dev_state)
+{
+	/*
+	 * First detach device from domain - No more PRI requests will arrive
+	 * from that device after it is unbound from the IOMMUv2 domain.
+	 */
+	iommu_detach_device(dev_state->domain, &dev_state->pdev->dev);
+
+	/* Everything is down now, free the IOMMUv2 domain */
+	iommu_domain_free(dev_state->domain);
+
+	/* Finally get rid of the device-state */
+	kfree(dev_state);
+}
+
+static void put_device_state(struct device_state *dev_state)
+{
+	if (atomic_dec_and_test(&dev_state->count))
+		wake_up(&dev_state->wq);
+}
+
+static void put_device_state_wait(struct device_state *dev_state)
+{
+	DEFINE_WAIT(wait);
+
+	prepare_to_wait(&dev_state->wq, &wait, TASK_UNINTERRUPTIBLE);
+	if (!atomic_dec_and_test(&dev_state->count))
+		schedule();
+	finish_wait(&dev_state->wq, &wait);
+
+	free_device_state(dev_state);
+}
+
+static struct notifier_block profile_nb = {
+	.notifier_call = task_exit,
+};
+
+static void link_pasid_state(struct pasid_state *pasid_state)
+{
+	spin_lock(&ps_lock);
+	list_add_tail(&pasid_state->list, &pasid_state_list);
+	spin_unlock(&ps_lock);
+}
+
+static void __unlink_pasid_state(struct pasid_state *pasid_state)
+{
+	list_del(&pasid_state->list);
+}
+
+static void unlink_pasid_state(struct pasid_state *pasid_state)
+{
+	spin_lock(&ps_lock);
+	__unlink_pasid_state(pasid_state);
+	spin_unlock(&ps_lock);
+}
+
+/* Must be called under dev_state->lock */
+static struct pasid_state **__get_pasid_state_ptr(struct device_state *dev_state,
+						  int pasid, bool alloc)
+{
+	struct pasid_state **root, **ptr;
+	int level, index;
+
+	level = dev_state->pasid_levels;
+	root  = dev_state->states;
+
+	while (true) {
+
+		index = (pasid >> (9 * level)) & 0x1ff;
+		ptr   = &root[index];
+
+		if (level == 0)
+			break;
+
+		if (*ptr == NULL) {
+			if (!alloc)
+				return NULL;
+
+			*ptr = (void *)get_zeroed_page(GFP_ATOMIC);
+			if (*ptr == NULL)
+				return NULL;
+		}
+
+		root   = (struct pasid_state **)*ptr;
+		level -= 1;
+	}
+
+	return ptr;
+}
+
+static int set_pasid_state(struct device_state *dev_state,
+			   struct pasid_state *pasid_state,
+			   int pasid)
+{
+	struct pasid_state **ptr;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&dev_state->lock, flags);
+	ptr = __get_pasid_state_ptr(dev_state, pasid, true);
+
+	ret = -ENOMEM;
+	if (ptr == NULL)
+		goto out_unlock;
+
+	ret = -ENOMEM;
+	if (*ptr != NULL)
+		goto out_unlock;
+
+	*ptr = pasid_state;
+
+	ret = 0;
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_state->lock, flags);
+
+	return ret;
+}
+
+static void clear_pasid_state(struct device_state *dev_state, int pasid)
+{
+	struct pasid_state **ptr;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_state->lock, flags);
+	ptr = __get_pasid_state_ptr(dev_state, pasid, true);
+
+	if (ptr == NULL)
+		goto out_unlock;
+
+	*ptr = NULL;
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_state->lock, flags);
+}
+
+static struct pasid_state *get_pasid_state(struct device_state *dev_state,
+					   int pasid)
+{
+	struct pasid_state **ptr, *ret = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_state->lock, flags);
+	ptr = __get_pasid_state_ptr(dev_state, pasid, false);
+
+	if (ptr == NULL)
+		goto out_unlock;
+
+	ret = *ptr;
+	if (ret)
+		atomic_inc(&ret->count);
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_state->lock, flags);
+
+	return ret;
+}
+
+static void free_pasid_state(struct pasid_state *pasid_state)
+{
+	kfree(pasid_state);
+}
+
+static void put_pasid_state(struct pasid_state *pasid_state)
+{
+	if (atomic_dec_and_test(&pasid_state->count)) {
+		put_device_state(pasid_state->device_state);
+		wake_up(&pasid_state->wq);
+	}
+}
+
+static void put_pasid_state_wait(struct pasid_state *pasid_state)
+{
+	DEFINE_WAIT(wait);
+
+	prepare_to_wait(&pasid_state->wq, &wait, TASK_UNINTERRUPTIBLE);
+
+	if (atomic_dec_and_test(&pasid_state->count))
+		put_device_state(pasid_state->device_state);
+	else
+		schedule();
+
+	finish_wait(&pasid_state->wq, &wait);
+	mmput(pasid_state->mm);
+	free_pasid_state(pasid_state);
+}
+
+static void __unbind_pasid(struct pasid_state *pasid_state)
+{
+	struct iommu_domain *domain;
+
+	domain = pasid_state->device_state->domain;
+
+	amd_iommu_domain_clear_gcr3(domain, pasid_state->pasid);
+	clear_pasid_state(pasid_state->device_state, pasid_state->pasid);
+
+	/* Make sure no more pending faults are in the queue */
+	flush_workqueue(iommu_wq);
+
+	mmu_notifier_unregister(&pasid_state->mn, pasid_state->mm);
+
+	put_pasid_state(pasid_state); /* Reference taken in bind() function */
+}
+
+static void unbind_pasid(struct device_state *dev_state, int pasid)
+{
+	struct pasid_state *pasid_state;
+
+	pasid_state = get_pasid_state(dev_state, pasid);
+	if (pasid_state == NULL)
+		return;
+
+	unlink_pasid_state(pasid_state);
+	__unbind_pasid(pasid_state);
+	put_pasid_state_wait(pasid_state); /* Reference taken in this function */
+}
+
+static void free_pasid_states_level1(struct pasid_state **tbl)
+{
+	int i;
+
+	for (i = 0; i < 512; ++i) {
+		if (tbl[i] == NULL)
+			continue;
+
+		free_page((unsigned long)tbl[i]);
+	}
+}
+
+static void free_pasid_states_level2(struct pasid_state **tbl)
+{
+	struct pasid_state **ptr;
+	int i;
+
+	for (i = 0; i < 512; ++i) {
+		if (tbl[i] == NULL)
+			continue;
+
+		ptr = (struct pasid_state **)tbl[i];
+		free_pasid_states_level1(ptr);
+	}
+}
+
+static void free_pasid_states(struct device_state *dev_state)
+{
+	struct pasid_state *pasid_state;
+	int i;
+
+	for (i = 0; i < dev_state->max_pasids; ++i) {
+		pasid_state = get_pasid_state(dev_state, i);
+		if (pasid_state == NULL)
+			continue;
+
+		put_pasid_state(pasid_state);
+		unbind_pasid(dev_state, i);
+	}
+
+	if (dev_state->pasid_levels == 2)
+		free_pasid_states_level2(dev_state->states);
+	else if (dev_state->pasid_levels == 1)
+		free_pasid_states_level1(dev_state->states);
+	else if (dev_state->pasid_levels != 0)
+		BUG();
+
+	free_page((unsigned long)dev_state->states);
+}
+
+static struct pasid_state *mn_to_state(struct mmu_notifier *mn)
+{
+	return container_of(mn, struct pasid_state, mn);
+}
+
+static void __mn_flush_page(struct mmu_notifier *mn,
+			    unsigned long address)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+
+	pasid_state = mn_to_state(mn);
+	dev_state   = pasid_state->device_state;
+
+	amd_iommu_flush_page(dev_state->domain, pasid_state->pasid, address);
+}
+
+static int mn_clear_flush_young(struct mmu_notifier *mn,
+				struct mm_struct *mm,
+				unsigned long address)
+{
+	__mn_flush_page(mn, address);
+
+	return 0;
+}
+
+static void mn_change_pte(struct mmu_notifier *mn,
+			  struct mm_struct *mm,
+			  unsigned long address,
+			  pte_t pte)
+{
+	__mn_flush_page(mn, address);
+}
+
+static void mn_invalidate_page(struct mmu_notifier *mn,
+			       struct mm_struct *mm,
+			       unsigned long address)
+{
+	__mn_flush_page(mn, address);
+}
+
+static void mn_invalidate_range_start(struct mmu_notifier *mn,
+				      struct mm_struct *mm,
+				      unsigned long start, unsigned long end)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+
+	pasid_state = mn_to_state(mn);
+	dev_state   = pasid_state->device_state;
+
+	amd_iommu_domain_set_gcr3(dev_state->domain, pasid_state->pasid,
+				  __pa(empty_page_table));
+}
+
+static void mn_invalidate_range_end(struct mmu_notifier *mn,
+				    struct mm_struct *mm,
+				    unsigned long start, unsigned long end)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+
+	pasid_state = mn_to_state(mn);
+	dev_state   = pasid_state->device_state;
+
+	amd_iommu_domain_set_gcr3(dev_state->domain, pasid_state->pasid,
+				  __pa(pasid_state->mm->pgd));
+}
+
+static struct mmu_notifier_ops iommu_mn = {
+	.clear_flush_young      = mn_clear_flush_young,
+	.change_pte             = mn_change_pte,
+	.invalidate_page        = mn_invalidate_page,
+	.invalidate_range_start = mn_invalidate_range_start,
+	.invalidate_range_end   = mn_invalidate_range_end,
+};
+
+static void set_pri_tag_status(struct pasid_state *pasid_state,
+			       u16 tag, int status)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&pasid_state->lock, flags);
+	pasid_state->pri[tag].status = status;
+	spin_unlock_irqrestore(&pasid_state->lock, flags);
+}
+
+static void finish_pri_tag(struct device_state *dev_state,
+			   struct pasid_state *pasid_state,
+			   u16 tag)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&pasid_state->lock, flags);
+	if (atomic_dec_and_test(&pasid_state->pri[tag].inflight) &&
+	    pasid_state->pri[tag].finish) {
+		amd_iommu_complete_ppr(dev_state->pdev, pasid_state->pasid,
+				       pasid_state->pri[tag].status, tag);
+		pasid_state->pri[tag].finish = false;
+		pasid_state->pri[tag].status = PPR_SUCCESS;
+	}
+	spin_unlock_irqrestore(&pasid_state->lock, flags);
+}
+
+static void do_fault(struct work_struct *work)
+{
+	struct fault *fault = container_of(work, struct fault, work);
+	int npages, write;
+	struct page *page;
+
+	write = !!(fault->flags & PPR_FAULT_WRITE);
+
+	npages = get_user_pages(fault->state->task, fault->state->mm,
+				fault->address, 1, write, 0, &page, NULL);
+
+	if (npages == 1) {
+		put_page(page);
+	} else if (fault->dev_state->inv_ppr_cb) {
+		int status;
+
+		status = fault->dev_state->inv_ppr_cb(fault->dev_state->pdev,
+						      fault->pasid,
+						      fault->address,
+						      fault->flags);
+		switch (status) {
+		case AMD_IOMMU_INV_PRI_RSP_SUCCESS:
+			set_pri_tag_status(fault->state, fault->tag, PPR_SUCCESS);
+			break;
+		case AMD_IOMMU_INV_PRI_RSP_INVALID:
+			set_pri_tag_status(fault->state, fault->tag, PPR_INVALID);
+			break;
+		case AMD_IOMMU_INV_PRI_RSP_FAIL:
+			set_pri_tag_status(fault->state, fault->tag, PPR_FAILURE);
+			break;
+		default:
+			BUG();
+		}
+	} else {
+		set_pri_tag_status(fault->state, fault->tag, PPR_INVALID);
+	}
+
+	finish_pri_tag(fault->dev_state, fault->state, fault->tag);
+
+	put_pasid_state(fault->state);
+
+	kfree(fault);
+}
+
+static int ppr_notifier(struct notifier_block *nb, unsigned long e, void *data)
+{
+	struct amd_iommu_fault *iommu_fault;
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+	unsigned long flags;
+	struct fault *fault;
+	bool finish;
+	u16 tag;
+	int ret;
+
+	iommu_fault = data;
+	tag         = iommu_fault->tag & 0x1ff;
+	finish      = (iommu_fault->tag >> 9) & 1;
+
+	ret = NOTIFY_DONE;
+	dev_state = get_device_state(iommu_fault->device_id);
+	if (dev_state == NULL)
+		goto out;
+
+	pasid_state = get_pasid_state(dev_state, iommu_fault->pasid);
+	if (pasid_state == NULL) {
+		/* We know the device but not the PASID -> send INVALID */
+		amd_iommu_complete_ppr(dev_state->pdev, iommu_fault->pasid,
+				       PPR_INVALID, tag);
+		goto out_drop_state;
+	}
+
+	spin_lock_irqsave(&pasid_state->lock, flags);
+	atomic_inc(&pasid_state->pri[tag].inflight);
+	if (finish)
+		pasid_state->pri[tag].finish = true;
+	spin_unlock_irqrestore(&pasid_state->lock, flags);
+
+	fault = kzalloc(sizeof(*fault), GFP_ATOMIC);
+	if (fault == NULL) {
+		/* We are OOM - send success and let the device re-fault */
+		finish_pri_tag(dev_state, pasid_state, tag);
+		goto out_drop_state;
+	}
+
+	fault->dev_state = dev_state;
+	fault->address   = iommu_fault->address;
+	fault->state     = pasid_state;
+	fault->tag       = tag;
+	fault->finish    = finish;
+	fault->flags     = iommu_fault->flags;
+	INIT_WORK(&fault->work, do_fault);
+
+	queue_work(iommu_wq, &fault->work);
+
+	ret = NOTIFY_OK;
+
+out_drop_state:
+	put_device_state(dev_state);
+
+out:
+	return ret;
+}
+
+static struct notifier_block ppr_nb = {
+	.notifier_call = ppr_notifier,
+};
+
+static int task_exit(struct notifier_block *nb, unsigned long e, void *data)
+{
+	struct pasid_state *pasid_state;
+	struct task_struct *task;
+
+	task = data;
+
+	/*
+	 * Using this notifier is a hack - but there is no other choice
+	 * at the moment. What I really want is a sleeping notifier that
+	 * is called when an MM goes down. But such a notifier doesn't
+	 * exist yet. The notifier needs to sleep because it has to make
+	 * sure that the device does not use the PASID and the address
+	 * space anymore before it is destroyed. This includes waiting
+	 * for pending PRI requests to pass the workqueue. The
+	 * MMU-Notifiers would be a good fit, but they use RCU and so
+	 * they are not allowed to sleep. Lets see how we can solve this
+	 * in a more intelligent way in the future.
+	 */
+again:
+	spin_lock(&ps_lock);
+	list_for_each_entry(pasid_state, &pasid_state_list, list) {
+		struct device_state *dev_state;
+		int pasid;
+
+		if (pasid_state->task != task)
+			continue;
+
+		/* Drop Lock and unbind */
+		spin_unlock(&ps_lock);
+
+		dev_state = pasid_state->device_state;
+		pasid     = pasid_state->pasid;
+
+		if (pasid_state->device_state->inv_ctx_cb)
+			dev_state->inv_ctx_cb(dev_state->pdev, pasid);
+
+		unbind_pasid(dev_state, pasid);
+
+		/* Task may be in the list multiple times */
+		goto again;
+	}
+	spin_unlock(&ps_lock);
+
+	return NOTIFY_OK;
+}
+
+int amd_iommu_bind_pasid(struct pci_dev *pdev, int pasid,
+			 struct task_struct *task)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+	u16 devid;
+	int ret;
+
+	might_sleep();
+
+	if (!amd_iommu_v2_supported())
+		return -ENODEV;
+
+	devid     = device_id(pdev);
+	dev_state = get_device_state(devid);
+
+	if (dev_state == NULL)
+		return -EINVAL;
+
+	ret = -EINVAL;
+	if (pasid < 0 || pasid >= dev_state->max_pasids)
+		goto out;
+
+	ret = -ENOMEM;
+	pasid_state = kzalloc(sizeof(*pasid_state), GFP_KERNEL);
+	if (pasid_state == NULL)
+		goto out;
+
+	atomic_set(&pasid_state->count, 1);
+	init_waitqueue_head(&pasid_state->wq);
+	pasid_state->task         = task;
+	pasid_state->mm           = get_task_mm(task);
+	pasid_state->device_state = dev_state;
+	pasid_state->pasid        = pasid;
+	pasid_state->mn.ops       = &iommu_mn;
+
+	if (pasid_state->mm == NULL)
+		goto out_free;
+
+	mmu_notifier_register(&pasid_state->mn, pasid_state->mm);
+
+	ret = set_pasid_state(dev_state, pasid_state, pasid);
+	if (ret)
+		goto out_unregister;
+
+	ret = amd_iommu_domain_set_gcr3(dev_state->domain, pasid,
+					__pa(pasid_state->mm->pgd));
+	if (ret)
+		goto out_clear_state;
+
+	link_pasid_state(pasid_state);
+
+	return 0;
+
+out_clear_state:
+	clear_pasid_state(dev_state, pasid);
+
+out_unregister:
+	mmu_notifier_unregister(&pasid_state->mn, pasid_state->mm);
+
+out_free:
+	free_pasid_state(pasid_state);
+
+out:
+	put_device_state(dev_state);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_bind_pasid);
+
+void amd_iommu_unbind_pasid(struct pci_dev *pdev, int pasid)
+{
+	struct device_state *dev_state;
+	u16 devid;
+
+	might_sleep();
+
+	if (!amd_iommu_v2_supported())
+		return;
+
+	devid = device_id(pdev);
+	dev_state = get_device_state(devid);
+	if (dev_state == NULL)
+		return;
+
+	if (pasid < 0 || pasid >= dev_state->max_pasids)
+		goto out;
+
+	unbind_pasid(dev_state, pasid);
+
+out:
+	put_device_state(dev_state);
+}
+EXPORT_SYMBOL(amd_iommu_unbind_pasid);
+
+int amd_iommu_init_device(struct pci_dev *pdev, int pasids)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+	int ret, tmp;
+	u16 devid;
+
+	might_sleep();
+
+	if (!amd_iommu_v2_supported())
+		return -ENODEV;
+
+	if (pasids <= 0 || pasids > (PASID_MASK + 1))
+		return -EINVAL;
+
+	devid = device_id(pdev);
+
+	dev_state = kzalloc(sizeof(*dev_state), GFP_KERNEL);
+	if (dev_state == NULL)
+		return -ENOMEM;
+
+	spin_lock_init(&dev_state->lock);
+	init_waitqueue_head(&dev_state->wq);
+	dev_state->pdev = pdev;
+
+	tmp = pasids;
+	for (dev_state->pasid_levels = 0; (tmp - 1) & ~0x1ff; tmp >>= 9)
+		dev_state->pasid_levels += 1;
+
+	atomic_set(&dev_state->count, 1);
+	dev_state->max_pasids = pasids;
+
+	ret = -ENOMEM;
+	dev_state->states = (void *)get_zeroed_page(GFP_KERNEL);
+	if (dev_state->states == NULL)
+		goto out_free_dev_state;
+
+	dev_state->domain = iommu_domain_alloc(&pci_bus_type);
+	if (dev_state->domain == NULL)
+		goto out_free_states;
+
+	amd_iommu_domain_direct_map(dev_state->domain);
+
+	ret = amd_iommu_domain_enable_v2(dev_state->domain, pasids);
+	if (ret)
+		goto out_free_domain;
+
+	ret = iommu_attach_device(dev_state->domain, &pdev->dev);
+	if (ret != 0)
+		goto out_free_domain;
+
+	spin_lock_irqsave(&state_lock, flags);
+
+	if (state_table[devid] != NULL) {
+		spin_unlock_irqrestore(&state_lock, flags);
+		ret = -EBUSY;
+		goto out_free_domain;
+	}
+
+	state_table[devid] = dev_state;
+
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	return 0;
+
+out_free_domain:
+	iommu_domain_free(dev_state->domain);
+
+out_free_states:
+	free_page((unsigned long)dev_state->states);
+
+out_free_dev_state:
+	kfree(dev_state);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_init_device);
+
+void amd_iommu_free_device(struct pci_dev *pdev)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+	u16 devid;
+
+	if (!amd_iommu_v2_supported())
+		return;
+
+	devid = device_id(pdev);
+
+	spin_lock_irqsave(&state_lock, flags);
+
+	dev_state = state_table[devid];
+	if (dev_state == NULL) {
+		spin_unlock_irqrestore(&state_lock, flags);
+		return;
+	}
+
+	state_table[devid] = NULL;
+
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	/* Get rid of any remaining pasid states */
+	free_pasid_states(dev_state);
+
+	put_device_state_wait(dev_state);
+}
+EXPORT_SYMBOL(amd_iommu_free_device);
+
+int amd_iommu_set_invalid_ppr_cb(struct pci_dev *pdev,
+				 amd_iommu_invalid_ppr_cb cb)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+	u16 devid;
+	int ret;
+
+	if (!amd_iommu_v2_supported())
+		return -ENODEV;
+
+	devid = device_id(pdev);
+
+	spin_lock_irqsave(&state_lock, flags);
+
+	ret = -EINVAL;
+	dev_state = state_table[devid];
+	if (dev_state == NULL)
+		goto out_unlock;
+
+	dev_state->inv_ppr_cb = cb;
+
+	ret = 0;
+
+out_unlock:
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_set_invalid_ppr_cb);
+
+int amd_iommu_set_invalidate_ctx_cb(struct pci_dev *pdev,
+				    amd_iommu_invalidate_ctx cb)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+	u16 devid;
+	int ret;
+
+	if (!amd_iommu_v2_supported())
+		return -ENODEV;
+
+	devid = device_id(pdev);
+
+	spin_lock_irqsave(&state_lock, flags);
+
+	ret = -EINVAL;
+	dev_state = state_table[devid];
+	if (dev_state == NULL)
+		goto out_unlock;
+
+	dev_state->inv_ctx_cb = cb;
+
+	ret = 0;
+
+out_unlock:
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_set_invalidate_ctx_cb);
+
+static int __init amd_iommu_v2_init(void)
+{
+	size_t state_table_size;
+	int ret;
+
+	pr_info("AMD IOMMUv2 driver by Joerg Roedel <joerg.roedel@amd.com>");
+
+	spin_lock_init(&state_lock);
+
+	state_table_size = MAX_DEVICES * sizeof(struct device_state *);
+	state_table = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
+					       get_order(state_table_size));
+	if (state_table == NULL)
+		return -ENOMEM;
+
+	ret = -ENOMEM;
+	iommu_wq = create_workqueue("amd_iommu_v2");
+	if (iommu_wq == NULL)
+		goto out_free;
+
+	ret = -ENOMEM;
+	empty_page_table = (u64 *)get_zeroed_page(GFP_KERNEL);
+	if (empty_page_table == NULL)
+		goto out_destroy_wq;
+
+	amd_iommu_register_ppr_notifier(&ppr_nb);
+	profile_event_register(PROFILE_TASK_EXIT, &profile_nb);
+
+	return 0;
+
+out_destroy_wq:
+	destroy_workqueue(iommu_wq);
+
+out_free:
+	free_pages((unsigned long)state_table, get_order(state_table_size));
+
+	return ret;
+}
+
+static void __exit amd_iommu_v2_exit(void)
+{
+	struct device_state *dev_state;
+	size_t state_table_size;
+	int i;
+
+	profile_event_unregister(PROFILE_TASK_EXIT, &profile_nb);
+	amd_iommu_unregister_ppr_notifier(&ppr_nb);
+
+	flush_workqueue(iommu_wq);
+
+	/*
+	 * The loop below might call flush_workqueue(), so call
+	 * destroy_workqueue() after it
+	 */
+	for (i = 0; i < MAX_DEVICES; ++i) {
+		dev_state = get_device_state(i);
+
+		if (dev_state == NULL)
+			continue;
+
+		WARN_ON_ONCE(1);
+
+		put_device_state(dev_state);
+		amd_iommu_free_device(dev_state->pdev);
+	}
+
+	destroy_workqueue(iommu_wq);
+
+	state_table_size = MAX_DEVICES * sizeof(struct device_state *);
+	free_pages((unsigned long)state_table, get_order(state_table_size));
+
+	free_page((unsigned long)empty_page_table);
+}
+
+module_init(amd_iommu_v2_init);
+module_exit(amd_iommu_v2_exit);
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index 31053a951c34..c9c6053198d4 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -79,6 +79,24 @@
 #define LEVEL_STRIDE		(9)
 #define LEVEL_MASK		(((u64)1 << LEVEL_STRIDE) - 1)
 
+/*
+ * This bitmap is used to advertise the page sizes our hardware support
+ * to the IOMMU core, which will then use this information to split
+ * physically contiguous memory regions it is mapping into page sizes
+ * that we support.
+ *
+ * Traditionally the IOMMU core just handed us the mappings directly,
+ * after making sure the size is an order of a 4KiB page and that the
+ * mapping has natural alignment.
+ *
+ * To retain this behavior, we currently advertise that we support
+ * all page sizes that are an order of 4KiB.
+ *
+ * If at some point we'd like to utilize the IOMMU core's new behavior,
+ * we could change this to advertise the real page sizes we support.
+ */
+#define INTEL_IOMMU_PGSIZES	(~0xFFFUL)
+
 static inline int agaw_to_level(int agaw)
 {
 	return agaw + 2;
@@ -3979,12 +3997,11 @@ static void intel_iommu_detach_device(struct iommu_domain *domain,
 
 static int intel_iommu_map(struct iommu_domain *domain,
 			   unsigned long iova, phys_addr_t hpa,
-			   int gfp_order, int iommu_prot)
+			   size_t size, int iommu_prot)
 {
 	struct dmar_domain *dmar_domain = domain->priv;
 	u64 max_addr;
 	int prot = 0;
-	size_t size;
 	int ret;
 
 	if (iommu_prot & IOMMU_READ)
@@ -3994,7 +4011,6 @@ static int intel_iommu_map(struct iommu_domain *domain,
 	if ((iommu_prot & IOMMU_CACHE) && dmar_domain->iommu_snooping)
 		prot |= DMA_PTE_SNP;
 
-	size     = PAGE_SIZE << gfp_order;
 	max_addr = iova + size;
 	if (dmar_domain->max_addr < max_addr) {
 		u64 end;
@@ -4017,11 +4033,10 @@ static int intel_iommu_map(struct iommu_domain *domain,
 	return ret;
 }
 
-static int intel_iommu_unmap(struct iommu_domain *domain,
-			     unsigned long iova, int gfp_order)
+static size_t intel_iommu_unmap(struct iommu_domain *domain,
+			     unsigned long iova, size_t size)
 {
 	struct dmar_domain *dmar_domain = domain->priv;
-	size_t size = PAGE_SIZE << gfp_order;
 	int order;
 
 	order = dma_pte_clear_range(dmar_domain, iova >> VTD_PAGE_SHIFT,
@@ -4030,7 +4045,7 @@ static int intel_iommu_unmap(struct iommu_domain *domain,
 	if (dmar_domain->max_addr == iova + size)
 		dmar_domain->max_addr = iova;
 
-	return order;
+	return PAGE_SIZE << order;
 }
 
 static phys_addr_t intel_iommu_iova_to_phys(struct iommu_domain *domain,
@@ -4060,6 +4075,54 @@ static int intel_iommu_domain_has_cap(struct iommu_domain *domain,
 	return 0;
 }
 
+/*
+ * Group numbers are arbitrary.  Device with the same group number
+ * indicate the iommu cannot differentiate between them.  To avoid
+ * tracking used groups we just use the seg|bus|devfn of the lowest
+ * level we're able to differentiate devices
+ */
+static int intel_iommu_device_group(struct device *dev, unsigned int *groupid)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pci_dev *bridge;
+	union {
+		struct {
+			u8 devfn;
+			u8 bus;
+			u16 segment;
+		} pci;
+		u32 group;
+	} id;
+
+	if (iommu_no_mapping(dev))
+		return -ENODEV;
+
+	id.pci.segment = pci_domain_nr(pdev->bus);
+	id.pci.bus = pdev->bus->number;
+	id.pci.devfn = pdev->devfn;
+
+	if (!device_to_iommu(id.pci.segment, id.pci.bus, id.pci.devfn))
+		return -ENODEV;
+
+	bridge = pci_find_upstream_pcie_bridge(pdev);
+	if (bridge) {
+		if (pci_is_pcie(bridge)) {
+			id.pci.bus = bridge->subordinate->number;
+			id.pci.devfn = 0;
+		} else {
+			id.pci.bus = bridge->bus->number;
+			id.pci.devfn = bridge->devfn;
+		}
+	}
+
+	if (!pdev->is_virtfn && iommu_group_mf)
+		id.pci.devfn = PCI_DEVFN(PCI_SLOT(id.pci.devfn), 0);
+
+	*groupid = id.group;
+
+	return 0;
+}
+
 static struct iommu_ops intel_iommu_ops = {
 	.domain_init	= intel_iommu_domain_init,
 	.domain_destroy = intel_iommu_domain_destroy,
@@ -4069,6 +4132,8 @@ static struct iommu_ops intel_iommu_ops = {
 	.unmap		= intel_iommu_unmap,
 	.iova_to_phys	= intel_iommu_iova_to_phys,
 	.domain_has_cap = intel_iommu_domain_has_cap,
+	.device_group	= intel_iommu_device_group,
+	.pgsize_bitmap	= INTEL_IOMMU_PGSIZES,
 };
 
 static void __devinit quirk_iommu_rwbf(struct pci_dev *dev)
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index 5b5fa5cdaa31..2198b2dbbcd3 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -16,6 +16,8 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  */
 
+#define pr_fmt(fmt)    "%s: " fmt, __func__
+
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/bug.h>
@@ -25,8 +27,59 @@
 #include <linux/errno.h>
 #include <linux/iommu.h>
 
+static ssize_t show_iommu_group(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	unsigned int groupid;
+
+	if (iommu_device_group(dev, &groupid))
+		return 0;
+
+	return sprintf(buf, "%u", groupid);
+}
+static DEVICE_ATTR(iommu_group, S_IRUGO, show_iommu_group, NULL);
+
+static int add_iommu_group(struct device *dev, void *data)
+{
+	unsigned int groupid;
+
+	if (iommu_device_group(dev, &groupid) == 0)
+		return device_create_file(dev, &dev_attr_iommu_group);
+
+	return 0;
+}
+
+static int remove_iommu_group(struct device *dev)
+{
+	unsigned int groupid;
+
+	if (iommu_device_group(dev, &groupid) == 0)
+		device_remove_file(dev, &dev_attr_iommu_group);
+
+	return 0;
+}
+
+static int iommu_device_notifier(struct notifier_block *nb,
+				 unsigned long action, void *data)
+{
+	struct device *dev = data;
+
+	if (action == BUS_NOTIFY_ADD_DEVICE)
+		return add_iommu_group(dev, NULL);
+	else if (action == BUS_NOTIFY_DEL_DEVICE)
+		return remove_iommu_group(dev);
+
+	return 0;
+}
+
+static struct notifier_block iommu_device_nb = {
+	.notifier_call = iommu_device_notifier,
+};
+
 static void iommu_bus_init(struct bus_type *bus, struct iommu_ops *ops)
 {
+	bus_register_notifier(bus, &iommu_device_nb);
+	bus_for_each_dev(bus, NULL, NULL, add_iommu_group);
 }
 
 /**
@@ -157,32 +210,134 @@ int iommu_domain_has_cap(struct iommu_domain *domain,
 EXPORT_SYMBOL_GPL(iommu_domain_has_cap);
 
 int iommu_map(struct iommu_domain *domain, unsigned long iova,
-	      phys_addr_t paddr, int gfp_order, int prot)
+	      phys_addr_t paddr, size_t size, int prot)
 {
-	size_t size;
+	unsigned long orig_iova = iova;
+	unsigned int min_pagesz;
+	size_t orig_size = size;
+	int ret = 0;
 
 	if (unlikely(domain->ops->map == NULL))
 		return -ENODEV;
 
-	size         = PAGE_SIZE << gfp_order;
+	/* find out the minimum page size supported */
+	min_pagesz = 1 << __ffs(domain->ops->pgsize_bitmap);
+
+	/*
+	 * both the virtual address and the physical one, as well as
+	 * the size of the mapping, must be aligned (at least) to the
+	 * size of the smallest page supported by the hardware
+	 */
+	if (!IS_ALIGNED(iova | paddr | size, min_pagesz)) {
+		pr_err("unaligned: iova 0x%lx pa 0x%lx size 0x%lx min_pagesz "
+			"0x%x\n", iova, (unsigned long)paddr,
+			(unsigned long)size, min_pagesz);
+		return -EINVAL;
+	}
+
+	pr_debug("map: iova 0x%lx pa 0x%lx size 0x%lx\n", iova,
+				(unsigned long)paddr, (unsigned long)size);
+
+	while (size) {
+		unsigned long pgsize, addr_merge = iova | paddr;
+		unsigned int pgsize_idx;
+
+		/* Max page size that still fits into 'size' */
+		pgsize_idx = __fls(size);
+
+		/* need to consider alignment requirements ? */
+		if (likely(addr_merge)) {
+			/* Max page size allowed by both iova and paddr */
+			unsigned int align_pgsize_idx = __ffs(addr_merge);
+
+			pgsize_idx = min(pgsize_idx, align_pgsize_idx);
+		}
+
+		/* build a mask of acceptable page sizes */
+		pgsize = (1UL << (pgsize_idx + 1)) - 1;
 
-	BUG_ON(!IS_ALIGNED(iova | paddr, size));
+		/* throw away page sizes not supported by the hardware */
+		pgsize &= domain->ops->pgsize_bitmap;
 
-	return domain->ops->map(domain, iova, paddr, gfp_order, prot);
+		/* make sure we're still sane */
+		BUG_ON(!pgsize);
+
+		/* pick the biggest page */
+		pgsize_idx = __fls(pgsize);
+		pgsize = 1UL << pgsize_idx;
+
+		pr_debug("mapping: iova 0x%lx pa 0x%lx pgsize %lu\n", iova,
+					(unsigned long)paddr, pgsize);
+
+		ret = domain->ops->map(domain, iova, paddr, pgsize, prot);
+		if (ret)
+			break;
+
+		iova += pgsize;
+		paddr += pgsize;
+		size -= pgsize;
+	}
+
+	/* unroll mapping in case something went wrong */
+	if (ret)
+		iommu_unmap(domain, orig_iova, orig_size - size);
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(iommu_map);
 
-int iommu_unmap(struct iommu_domain *domain, unsigned long iova, int gfp_order)
+size_t iommu_unmap(struct iommu_domain *domain, unsigned long iova, size_t size)
 {
-	size_t size;
+	size_t unmapped_page, unmapped = 0;
+	unsigned int min_pagesz;
 
 	if (unlikely(domain->ops->unmap == NULL))
 		return -ENODEV;
 
-	size         = PAGE_SIZE << gfp_order;
+	/* find out the minimum page size supported */
+	min_pagesz = 1 << __ffs(domain->ops->pgsize_bitmap);
+
+	/*
+	 * The virtual address, as well as the size of the mapping, must be
+	 * aligned (at least) to the size of the smallest page supported
+	 * by the hardware
+	 */
+	if (!IS_ALIGNED(iova | size, min_pagesz)) {
+		pr_err("unaligned: iova 0x%lx size 0x%lx min_pagesz 0x%x\n",
+					iova, (unsigned long)size, min_pagesz);
+		return -EINVAL;
+	}
+
+	pr_debug("unmap this: iova 0x%lx size 0x%lx\n", iova,
+							(unsigned long)size);
+
+	/*
+	 * Keep iterating until we either unmap 'size' bytes (or more)
+	 * or we hit an area that isn't mapped.
+	 */
+	while (unmapped < size) {
+		size_t left = size - unmapped;
+
+		unmapped_page = domain->ops->unmap(domain, iova, left);
+		if (!unmapped_page)
+			break;
+
+		pr_debug("unmapped: iova 0x%lx size %lx\n", iova,
+					(unsigned long)unmapped_page);
+
+		iova += unmapped_page;
+		unmapped += unmapped_page;
+	}
+
+	return unmapped;
+}
+EXPORT_SYMBOL_GPL(iommu_unmap);
 
-	BUG_ON(!IS_ALIGNED(iova, size));
+int iommu_device_group(struct device *dev, unsigned int *groupid)
+{
+	if (iommu_present(dev->bus) && dev->bus->iommu_ops->device_group)
+		return dev->bus->iommu_ops->device_group(dev, groupid);
 
-	return domain->ops->unmap(domain, iova, gfp_order);
+	return -ENODEV;
 }
-EXPORT_SYMBOL_GPL(iommu_unmap);
+EXPORT_SYMBOL_GPL(iommu_device_group);
diff --git a/drivers/iommu/msm_iommu.c b/drivers/iommu/msm_iommu.c
index 5865dd2e28f9..08a90b88e40d 100644
--- a/drivers/iommu/msm_iommu.c
+++ b/drivers/iommu/msm_iommu.c
@@ -42,6 +42,9 @@ __asm__ __volatile__ (							\
 #define RCP15_PRRR(reg)		MRC(reg, p15, 0, c10, c2, 0)
 #define RCP15_NMRR(reg)		MRC(reg, p15, 0, c10, c2, 1)
 
+/* bitmap of the page sizes currently supported */
+#define MSM_IOMMU_PGSIZES	(SZ_4K | SZ_64K | SZ_1M | SZ_16M)
+
 static int msm_iommu_tex_class[4];
 
 DEFINE_SPINLOCK(msm_iommu_lock);
@@ -352,7 +355,7 @@ fail:
 }
 
 static int msm_iommu_map(struct iommu_domain *domain, unsigned long va,
-			 phys_addr_t pa, int order, int prot)
+			 phys_addr_t pa, size_t len, int prot)
 {
 	struct msm_priv *priv;
 	unsigned long flags;
@@ -363,7 +366,6 @@ static int msm_iommu_map(struct iommu_domain *domain, unsigned long va,
 	unsigned long *sl_pte;
 	unsigned long sl_offset;
 	unsigned int pgprot;
-	size_t len = 0x1000UL << order;
 	int ret = 0, tex, sh;
 
 	spin_lock_irqsave(&msm_iommu_lock, flags);
@@ -463,8 +465,8 @@ fail:
 	return ret;
 }
 
-static int msm_iommu_unmap(struct iommu_domain *domain, unsigned long va,
-			    int order)
+static size_t msm_iommu_unmap(struct iommu_domain *domain, unsigned long va,
+			    size_t len)
 {
 	struct msm_priv *priv;
 	unsigned long flags;
@@ -474,7 +476,6 @@ static int msm_iommu_unmap(struct iommu_domain *domain, unsigned long va,
 	unsigned long *sl_table;
 	unsigned long *sl_pte;
 	unsigned long sl_offset;
-	size_t len = 0x1000UL << order;
 	int i, ret = 0;
 
 	spin_lock_irqsave(&msm_iommu_lock, flags);
@@ -544,15 +545,12 @@ static int msm_iommu_unmap(struct iommu_domain *domain, unsigned long va,
 
 	ret = __flush_iotlb(domain);
 
-	/*
-	 * the IOMMU API requires us to return the order of the unmapped
-	 * page (on success).
-	 */
-	if (!ret)
-		ret = order;
 fail:
 	spin_unlock_irqrestore(&msm_iommu_lock, flags);
-	return ret;
+
+	/* the IOMMU API requires us to return how many bytes were unmapped */
+	len = ret ? 0 : len;
+	return len;
 }
 
 static phys_addr_t msm_iommu_iova_to_phys(struct iommu_domain *domain,
@@ -684,7 +682,8 @@ static struct iommu_ops msm_iommu_ops = {
 	.map = msm_iommu_map,
 	.unmap = msm_iommu_unmap,
 	.iova_to_phys = msm_iommu_iova_to_phys,
-	.domain_has_cap = msm_iommu_domain_has_cap
+	.domain_has_cap = msm_iommu_domain_has_cap,
+	.pgsize_bitmap = MSM_IOMMU_PGSIZES,
 };
 
 static int __init get_tex_class(int icp, int ocp, int mt, int nos)
diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
index 8f32b2bf7587..d8edd979d01b 100644
--- a/drivers/iommu/omap-iommu.c
+++ b/drivers/iommu/omap-iommu.c
@@ -33,6 +33,9 @@
 	     (__i < (n)) && (cr = __iotlb_read_cr((obj), __i), true);	\
 	     __i++)
 
+/* bitmap of the page sizes currently supported */
+#define OMAP_IOMMU_PGSIZES	(SZ_4K | SZ_64K | SZ_1M | SZ_16M)
+
 /**
  * struct omap_iommu_domain - omap iommu domain
  * @pgtable:	the page table
@@ -86,20 +89,24 @@ EXPORT_SYMBOL_GPL(omap_uninstall_iommu_arch);
 
 /**
  * omap_iommu_save_ctx - Save registers for pm off-mode support
- * @obj:	target iommu
+ * @dev:	client device
  **/
-void omap_iommu_save_ctx(struct omap_iommu *obj)
+void omap_iommu_save_ctx(struct device *dev)
 {
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+
 	arch_iommu->save_ctx(obj);
 }
 EXPORT_SYMBOL_GPL(omap_iommu_save_ctx);
 
 /**
  * omap_iommu_restore_ctx - Restore registers for pm off-mode support
- * @obj:	target iommu
+ * @dev:	client device
  **/
-void omap_iommu_restore_ctx(struct omap_iommu *obj)
+void omap_iommu_restore_ctx(struct device *dev)
 {
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+
 	arch_iommu->restore_ctx(obj);
 }
 EXPORT_SYMBOL_GPL(omap_iommu_restore_ctx);
@@ -820,35 +827,23 @@ static int device_match_by_alias(struct device *dev, void *data)
 }
 
 /**
- * omap_find_iommu_device() - find an omap iommu device by name
- * @name:	name of the iommu device
- *
- * The generic iommu API requires the caller to provide the device
- * he wishes to attach to a certain iommu domain.
- *
- * Drivers generally should not bother with this as it should just
- * be taken care of by the DMA-API using dev_archdata.
- *
- * This function is provided as an interim solution until the latter
- * materializes, and omap3isp is fully migrated to the DMA-API.
- */
-struct device *omap_find_iommu_device(const char *name)
-{
-	return driver_find_device(&omap_iommu_driver.driver, NULL,
-				(void *)name,
-				device_match_by_alias);
-}
-EXPORT_SYMBOL_GPL(omap_find_iommu_device);
-
-/**
  * omap_iommu_attach() - attach iommu device to an iommu domain
- * @dev:	target omap iommu device
+ * @name:	name of target omap iommu device
  * @iopgd:	page table
  **/
-static struct omap_iommu *omap_iommu_attach(struct device *dev, u32 *iopgd)
+static struct omap_iommu *omap_iommu_attach(const char *name, u32 *iopgd)
 {
 	int err = -ENOMEM;
-	struct omap_iommu *obj = to_iommu(dev);
+	struct device *dev;
+	struct omap_iommu *obj;
+
+	dev = driver_find_device(&omap_iommu_driver.driver, NULL,
+				(void *)name,
+				device_match_by_alias);
+	if (!dev)
+		return NULL;
+
+	obj = to_iommu(dev);
 
 	spin_lock(&obj->iommu_lock);
 
@@ -1019,12 +1014,11 @@ static void iopte_cachep_ctor(void *iopte)
 }
 
 static int omap_iommu_map(struct iommu_domain *domain, unsigned long da,
-			 phys_addr_t pa, int order, int prot)
+			 phys_addr_t pa, size_t bytes, int prot)
 {
 	struct omap_iommu_domain *omap_domain = domain->priv;
 	struct omap_iommu *oiommu = omap_domain->iommu_dev;
 	struct device *dev = oiommu->dev;
-	size_t bytes = PAGE_SIZE << order;
 	struct iotlb_entry e;
 	int omap_pgsz;
 	u32 ret, flags;
@@ -1049,19 +1043,16 @@ static int omap_iommu_map(struct iommu_domain *domain, unsigned long da,
 	return ret;
 }
 
-static int omap_iommu_unmap(struct iommu_domain *domain, unsigned long da,
-			    int order)
+static size_t omap_iommu_unmap(struct iommu_domain *domain, unsigned long da,
+			    size_t size)
 {
 	struct omap_iommu_domain *omap_domain = domain->priv;
 	struct omap_iommu *oiommu = omap_domain->iommu_dev;
 	struct device *dev = oiommu->dev;
-	size_t unmap_size;
-
-	dev_dbg(dev, "unmapping da 0x%lx order %d\n", da, order);
 
-	unmap_size = iopgtable_clear_entry(oiommu, da);
+	dev_dbg(dev, "unmapping da 0x%lx size %u\n", da, size);
 
-	return unmap_size ? get_order(unmap_size) : -EINVAL;
+	return iopgtable_clear_entry(oiommu, da);
 }
 
 static int
@@ -1069,6 +1060,7 @@ omap_iommu_attach_dev(struct iommu_domain *domain, struct device *dev)
 {
 	struct omap_iommu_domain *omap_domain = domain->priv;
 	struct omap_iommu *oiommu;
+	struct omap_iommu_arch_data *arch_data = dev->archdata.iommu;
 	int ret = 0;
 
 	spin_lock(&omap_domain->lock);
@@ -1081,14 +1073,14 @@ omap_iommu_attach_dev(struct iommu_domain *domain, struct device *dev)
 	}
 
 	/* get a handle to and enable the omap iommu */
-	oiommu = omap_iommu_attach(dev, omap_domain->pgtable);
+	oiommu = omap_iommu_attach(arch_data->name, omap_domain->pgtable);
 	if (IS_ERR(oiommu)) {
 		ret = PTR_ERR(oiommu);
 		dev_err(dev, "can't get omap iommu: %d\n", ret);
 		goto out;
 	}
 
-	omap_domain->iommu_dev = oiommu;
+	omap_domain->iommu_dev = arch_data->iommu_dev = oiommu;
 	oiommu->domain = domain;
 
 out:
@@ -1100,7 +1092,8 @@ static void omap_iommu_detach_dev(struct iommu_domain *domain,
 				 struct device *dev)
 {
 	struct omap_iommu_domain *omap_domain = domain->priv;
-	struct omap_iommu *oiommu = to_iommu(dev);
+	struct omap_iommu_arch_data *arch_data = dev->archdata.iommu;
+	struct omap_iommu *oiommu = dev_to_omap_iommu(dev);
 
 	spin_lock(&omap_domain->lock);
 
@@ -1114,7 +1107,7 @@ static void omap_iommu_detach_dev(struct iommu_domain *domain,
 
 	omap_iommu_detach(oiommu);
 
-	omap_domain->iommu_dev = NULL;
+	omap_domain->iommu_dev = arch_data->iommu_dev = NULL;
 
 out:
 	spin_unlock(&omap_domain->lock);
@@ -1183,14 +1176,14 @@ static phys_addr_t omap_iommu_iova_to_phys(struct iommu_domain *domain,
 		else if (iopte_is_large(*pte))
 			ret = omap_iommu_translate(*pte, da, IOLARGE_MASK);
 		else
-			dev_err(dev, "bogus pte 0x%x", *pte);
+			dev_err(dev, "bogus pte 0x%x, da 0x%lx", *pte, da);
 	} else {
 		if (iopgd_is_section(*pgd))
 			ret = omap_iommu_translate(*pgd, da, IOSECTION_MASK);
 		else if (iopgd_is_super(*pgd))
 			ret = omap_iommu_translate(*pgd, da, IOSUPER_MASK);
 		else
-			dev_err(dev, "bogus pgd 0x%x", *pgd);
+			dev_err(dev, "bogus pgd 0x%x, da 0x%lx", *pgd, da);
 	}
 
 	return ret;
@@ -1211,6 +1204,7 @@ static struct iommu_ops omap_iommu_ops = {
 	.unmap		= omap_iommu_unmap,
 	.iova_to_phys	= omap_iommu_iova_to_phys,
 	.domain_has_cap	= omap_iommu_domain_has_cap,
+	.pgsize_bitmap	= OMAP_IOMMU_PGSIZES,
 };
 
 static int __init omap_iommu_init(void)
diff --git a/drivers/iommu/omap-iovmm.c b/drivers/iommu/omap-iovmm.c
index 46be456fcc00..2e10c3e0a7ae 100644
--- a/drivers/iommu/omap-iovmm.c
+++ b/drivers/iommu/omap-iovmm.c
@@ -231,12 +231,14 @@ static struct iovm_struct *__find_iovm_area(struct omap_iommu *obj,
 
 /**
  * omap_find_iovm_area  -  find iovma which includes @da
+ * @dev:	client device
  * @da:		iommu device virtual address
  *
  * Find the existing iovma starting at @da
  */
-struct iovm_struct *omap_find_iovm_area(struct omap_iommu *obj, u32 da)
+struct iovm_struct *omap_find_iovm_area(struct device *dev, u32 da)
 {
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
 	struct iovm_struct *area;
 
 	mutex_lock(&obj->mmap_lock);
@@ -343,14 +345,15 @@ static void free_iovm_area(struct omap_iommu *obj, struct iovm_struct *area)
 
 /**
  * omap_da_to_va - convert (d) to (v)
- * @obj:	objective iommu
+ * @dev:	client device
  * @da:		iommu device virtual address
  * @va:		mpu virtual address
  *
  * Returns mpu virtual addr which corresponds to a given device virtual addr
  */
-void *omap_da_to_va(struct omap_iommu *obj, u32 da)
+void *omap_da_to_va(struct device *dev, u32 da)
 {
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
 	void *va = NULL;
 	struct iovm_struct *area;
 
@@ -410,7 +413,6 @@ static int map_iovm_area(struct iommu_domain *domain, struct iovm_struct *new,
 	unsigned int i, j;
 	struct scatterlist *sg;
 	u32 da = new->da_start;
-	int order;
 
 	if (!domain || !sgt)
 		return -EINVAL;
@@ -429,12 +431,10 @@ static int map_iovm_area(struct iommu_domain *domain, struct iovm_struct *new,
 		if (bytes_to_iopgsz(bytes) < 0)
 			goto err_out;
 
-		order = get_order(bytes);
-
 		pr_debug("%s: [%d] %08x %08x(%x)\n", __func__,
 			 i, da, pa, bytes);
 
-		err = iommu_map(domain, da, pa, order, flags);
+		err = iommu_map(domain, da, pa, bytes, flags);
 		if (err)
 			goto err_out;
 
@@ -449,10 +449,9 @@ err_out:
 		size_t bytes;
 
 		bytes = sg->length + sg->offset;
-		order = get_order(bytes);
 
 		/* ignore failures.. we're already handling one */
-		iommu_unmap(domain, da, order);
+		iommu_unmap(domain, da, bytes);
 
 		da += bytes;
 	}
@@ -467,7 +466,8 @@ static void unmap_iovm_area(struct iommu_domain *domain, struct omap_iommu *obj,
 	size_t total = area->da_end - area->da_start;
 	const struct sg_table *sgt = area->sgt;
 	struct scatterlist *sg;
-	int i, err;
+	int i;
+	size_t unmapped;
 
 	BUG_ON(!sgtable_ok(sgt));
 	BUG_ON((!total) || !IS_ALIGNED(total, PAGE_SIZE));
@@ -475,13 +475,11 @@ static void unmap_iovm_area(struct iommu_domain *domain, struct omap_iommu *obj,
 	start = area->da_start;
 	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
 		size_t bytes;
-		int order;
 
 		bytes = sg->length + sg->offset;
-		order = get_order(bytes);
 
-		err = iommu_unmap(domain, start, order);
-		if (err < 0)
+		unmapped = iommu_unmap(domain, start, bytes);
+		if (unmapped < bytes)
 			break;
 
 		dev_dbg(obj->dev, "%s: unmap %08x(%x) %08x\n",
@@ -582,16 +580,18 @@ __iommu_vmap(struct iommu_domain *domain, struct omap_iommu *obj,
 
 /**
  * omap_iommu_vmap  -  (d)-(p)-(v) address mapper
- * @obj:	objective iommu
+ * @domain:	iommu domain
+ * @dev:	client device
  * @sgt:	address of scatter gather table
  * @flags:	iovma and page property
  *
  * Creates 1-n-1 mapping with given @sgt and returns @da.
  * All @sgt element must be io page size aligned.
  */
-u32 omap_iommu_vmap(struct iommu_domain *domain, struct omap_iommu *obj, u32 da,
+u32 omap_iommu_vmap(struct iommu_domain *domain, struct device *dev, u32 da,
 		const struct sg_table *sgt, u32 flags)
 {
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
 	size_t bytes;
 	void *va = NULL;
 
@@ -622,15 +622,17 @@ EXPORT_SYMBOL_GPL(omap_iommu_vmap);
 
 /**
  * omap_iommu_vunmap  -  release virtual mapping obtained by 'omap_iommu_vmap()'
- * @obj:	objective iommu
+ * @domain:	iommu domain
+ * @dev:	client device
  * @da:		iommu device virtual address
  *
  * Free the iommu virtually contiguous memory area starting at
  * @da, which was returned by 'omap_iommu_vmap()'.
  */
 struct sg_table *
-omap_iommu_vunmap(struct iommu_domain *domain, struct omap_iommu *obj, u32 da)
+omap_iommu_vunmap(struct iommu_domain *domain, struct device *dev, u32 da)
 {
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
 	struct sg_table *sgt;
 	/*
 	 * 'sgt' is allocated before 'omap_iommu_vmalloc()' is called.
@@ -647,7 +649,7 @@ EXPORT_SYMBOL_GPL(omap_iommu_vunmap);
 
 /**
  * omap_iommu_vmalloc  -  (d)-(p)-(v) address allocator and mapper
- * @obj:	objective iommu
+ * @dev:	client device
  * @da:		contiguous iommu virtual memory
  * @bytes:	allocation size
  * @flags:	iovma and page property
@@ -656,9 +658,10 @@ EXPORT_SYMBOL_GPL(omap_iommu_vunmap);
  * @da again, which might be adjusted if 'IOVMF_DA_FIXED' is not set.
  */
 u32
-omap_iommu_vmalloc(struct iommu_domain *domain, struct omap_iommu *obj, u32 da,
+omap_iommu_vmalloc(struct iommu_domain *domain, struct device *dev, u32 da,
 						size_t bytes, u32 flags)
 {
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
 	void *va;
 	struct sg_table *sgt;
 
@@ -698,15 +701,16 @@ EXPORT_SYMBOL_GPL(omap_iommu_vmalloc);
 
 /**
  * omap_iommu_vfree  -  release memory allocated by 'omap_iommu_vmalloc()'
- * @obj:	objective iommu
+ * @dev:	client device
  * @da:		iommu device virtual address
  *
  * Frees the iommu virtually continuous memory area starting at
  * @da, as obtained from 'omap_iommu_vmalloc()'.
  */
-void omap_iommu_vfree(struct iommu_domain *domain, struct omap_iommu *obj,
+void omap_iommu_vfree(struct iommu_domain *domain, struct device *dev,
 								const u32 da)
 {
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
 	struct sg_table *sgt;
 
 	sgt = unmap_vm_area(domain, obj, da, vfree,
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 1b75a56ebd08..897a77dfa9d7 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -388,6 +388,14 @@ config LEDS_RENESAS_TPU
 	  pin function. The latter to support brightness control.
 	  Brightness control is supported but hardware blinking is not.
 
+config LEDS_TCA6507
+	tristate "LED Support for TCA6507 I2C chip"
+	depends on LEDS_CLASS && I2C
+	help
+	  This option enables support for LEDs connected to TC6507
+	  LED driver chips accessed via the I2C bus.
+	  Driver support brightness control and hardware-assisted blinking.
+
 config LEDS_TRIGGERS
 	bool "LED Trigger support"
 	depends on LEDS_CLASS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index e4f6bf568880..5c9dc4b000d5 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -25,6 +25,7 @@ obj-$(CONFIG_LEDS_GPIO)			+= leds-gpio.o
 obj-$(CONFIG_LEDS_LP3944)		+= leds-lp3944.o
 obj-$(CONFIG_LEDS_LP5521)		+= leds-lp5521.o
 obj-$(CONFIG_LEDS_LP5523)		+= leds-lp5523.o
+obj-$(CONFIG_LEDS_TCA6507)		+= leds-tca6507.o
 obj-$(CONFIG_LEDS_CLEVO_MAIL)		+= leds-clevo-mail.o
 obj-$(CONFIG_LEDS_HP6XX)		+= leds-hp6xx.o
 obj-$(CONFIG_LEDS_FSG)			+= leds-fsg.o
diff --git a/drivers/leds/leds-88pm860x.c b/drivers/leds/leds-88pm860x.c
index 0810604dc701..4ca00624bd18 100644
--- a/drivers/leds/leds-88pm860x.c
+++ b/drivers/leds/leds-88pm860x.c
@@ -238,17 +238,7 @@ static struct platform_driver pm860x_led_driver = {
 	.remove	= pm860x_led_remove,
 };
 
-static int __devinit pm860x_led_init(void)
-{
-	return platform_driver_register(&pm860x_led_driver);
-}
-module_init(pm860x_led_init);
-
-static void __devexit pm860x_led_exit(void)
-{
-	platform_driver_unregister(&pm860x_led_driver);
-}
-module_exit(pm860x_led_exit);
+module_platform_driver(pm860x_led_driver);
 
 MODULE_DESCRIPTION("LED driver for Marvell PM860x");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
diff --git a/drivers/leds/leds-adp5520.c b/drivers/leds/leds-adp5520.c
index 7ba4c7b5b97e..b1400db3f839 100644
--- a/drivers/leds/leds-adp5520.c
+++ b/drivers/leds/leds-adp5520.c
@@ -213,17 +213,7 @@ static struct platform_driver adp5520_led_driver = {
 	.remove		= __devexit_p(adp5520_led_remove),
 };
 
-static int __init adp5520_led_init(void)
-{
-	return platform_driver_register(&adp5520_led_driver);
-}
-module_init(adp5520_led_init);
-
-static void __exit adp5520_led_exit(void)
-{
-	platform_driver_unregister(&adp5520_led_driver);
-}
-module_exit(adp5520_led_exit);
+module_platform_driver(adp5520_led_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("LEDS ADP5520(01) Driver");
diff --git a/drivers/leds/leds-ams-delta.c b/drivers/leds/leds-ams-delta.c
index 8c00937bf7e7..07428357c83f 100644
--- a/drivers/leds/leds-ams-delta.c
+++ b/drivers/leds/leds-ams-delta.c
@@ -118,18 +118,7 @@ static struct platform_driver ams_delta_led_driver = {
 	},
 };
 
-static int __init ams_delta_led_init(void)
-{
-	return platform_driver_register(&ams_delta_led_driver);
-}
-
-static void __exit ams_delta_led_exit(void)
-{
-	platform_driver_unregister(&ams_delta_led_driver);
-}
-
-module_init(ams_delta_led_init);
-module_exit(ams_delta_led_exit);
+module_platform_driver(ams_delta_led_driver);
 
 MODULE_AUTHOR("Jonathan McDowell <noodles@earth.li>");
 MODULE_DESCRIPTION("Amstrad Delta LED driver");
diff --git a/drivers/leds/leds-asic3.c b/drivers/leds/leds-asic3.c
index 48d9fe61bdfc..525a92492837 100644
--- a/drivers/leds/leds-asic3.c
+++ b/drivers/leds/leds-asic3.c
@@ -179,21 +179,9 @@ static struct platform_driver asic3_led_driver = {
 	},
 };
 
-MODULE_ALIAS("platform:leds-asic3");
-
-static int __init asic3_led_init(void)
-{
-	return platform_driver_register(&asic3_led_driver);
-}
-
-static void __exit asic3_led_exit(void)
-{
-	platform_driver_unregister(&asic3_led_driver);
-}
-
-module_init(asic3_led_init);
-module_exit(asic3_led_exit);
+module_platform_driver(asic3_led_driver);
 
 MODULE_AUTHOR("Paul Parsons <lost.distance@yahoo.com>");
 MODULE_DESCRIPTION("HTC ASIC3 LED driver");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:leds-asic3");
diff --git a/drivers/leds/leds-atmel-pwm.c b/drivers/leds/leds-atmel-pwm.c
index 109c875ea233..800243b6037e 100644
--- a/drivers/leds/leds-atmel-pwm.c
+++ b/drivers/leds/leds-atmel-pwm.c
@@ -134,29 +134,18 @@ static int __exit pwmled_remove(struct platform_device *pdev)
 	return 0;
 }
 
-/* work with hotplug and coldplug */
-MODULE_ALIAS("platform:leds-atmel-pwm");
-
 static struct platform_driver pwmled_driver = {
 	.driver = {
 		.name =		"leds-atmel-pwm",
 		.owner =	THIS_MODULE,
 	},
 	/* REVISIT add suspend() and resume() methods */
+	.probe =	pwmled_probe,
 	.remove =	__exit_p(pwmled_remove),
 };
 
-static int __init modinit(void)
-{
-	return platform_driver_probe(&pwmled_driver, pwmled_probe);
-}
-module_init(modinit);
-
-static void __exit modexit(void)
-{
-	platform_driver_unregister(&pwmled_driver);
-}
-module_exit(modexit);
+module_platform_driver(pwmled_driver);
 
 MODULE_DESCRIPTION("Driver for LEDs with PWM-controlled brightness");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:leds-atmel-pwm");
diff --git a/drivers/leds/leds-bd2802.c b/drivers/leds/leds-bd2802.c
index ea2185531f82..591cbdf5a046 100644
--- a/drivers/leds/leds-bd2802.c
+++ b/drivers/leds/leds-bd2802.c
@@ -688,8 +688,7 @@ static int __devinit bd2802_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, led);
 
 	/* Configure RESET GPIO (L: RESET, H: RESET cancel) */
-	gpio_request(pdata->reset_gpio, "RGB_RESETB");
-	gpio_direction_output(pdata->reset_gpio, 1);
+	gpio_request_one(pdata->reset_gpio, GPIOF_OUT_INIT_HIGH, "RGB_RESETB");
 
 	/* Tacss = min 0.1ms */
 	udelay(100);
@@ -813,17 +812,7 @@ static struct i2c_driver bd2802_i2c_driver = {
 	.id_table	= bd2802_id,
 };
 
-static int __init bd2802_init(void)
-{
-	return i2c_add_driver(&bd2802_i2c_driver);
-}
-module_init(bd2802_init);
-
-static void __exit bd2802_exit(void)
-{
-	i2c_del_driver(&bd2802_i2c_driver);
-}
-module_exit(bd2802_exit);
+module_i2c_driver(bd2802_i2c_driver);
 
 MODULE_AUTHOR("Kim Kyuwon <q1.kim@samsung.com>");
 MODULE_DESCRIPTION("BD2802 LED driver");
diff --git a/drivers/leds/leds-cobalt-qube.c b/drivers/leds/leds-cobalt-qube.c
index da5fb016b1a5..6a8725cc7b4d 100644
--- a/drivers/leds/leds-cobalt-qube.c
+++ b/drivers/leds/leds-cobalt-qube.c
@@ -75,9 +75,6 @@ static int __devexit cobalt_qube_led_remove(struct platform_device *pdev)
 	return 0;
 }
 
-/* work with hotplug and coldplug */
-MODULE_ALIAS("platform:cobalt-qube-leds");
-
 static struct platform_driver cobalt_qube_led_driver = {
 	.probe	= cobalt_qube_led_probe,
 	.remove	= __devexit_p(cobalt_qube_led_remove),
@@ -87,19 +84,9 @@ static struct platform_driver cobalt_qube_led_driver = {
 	},
 };
 
-static int __init cobalt_qube_led_init(void)
-{
-	return platform_driver_register(&cobalt_qube_led_driver);
-}
-
-static void __exit cobalt_qube_led_exit(void)
-{
-	platform_driver_unregister(&cobalt_qube_led_driver);
-}
-
-module_init(cobalt_qube_led_init);
-module_exit(cobalt_qube_led_exit);
+module_platform_driver(cobalt_qube_led_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Front LED support for Cobalt Server");
 MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
+MODULE_ALIAS("platform:cobalt-qube-leds");
diff --git a/drivers/leds/leds-da903x.c b/drivers/leds/leds-da903x.c
index f28931cf6781..d9cd73ebd6c4 100644
--- a/drivers/leds/leds-da903x.c
+++ b/drivers/leds/leds-da903x.c
@@ -158,17 +158,7 @@ static struct platform_driver da903x_led_driver = {
 	.remove		= __devexit_p(da903x_led_remove),
 };
 
-static int __init da903x_led_init(void)
-{
-	return platform_driver_register(&da903x_led_driver);
-}
-module_init(da903x_led_init);
-
-static void __exit da903x_led_exit(void)
-{
-	platform_driver_unregister(&da903x_led_driver);
-}
-module_exit(da903x_led_exit);
+module_platform_driver(da903x_led_driver);
 
 MODULE_DESCRIPTION("LEDs driver for Dialog Semiconductor DA9030/DA9034");
 MODULE_AUTHOR("Eric Miao <eric.miao@marvell.com>"
diff --git a/drivers/leds/leds-dac124s085.c b/drivers/leds/leds-dac124s085.c
index 31cf0d60a9a5..d56c14269ff0 100644
--- a/drivers/leds/leds-dac124s085.c
+++ b/drivers/leds/leds-dac124s085.c
@@ -131,18 +131,7 @@ static struct spi_driver dac124s085_driver = {
 	},
 };
 
-static int __init dac124s085_leds_init(void)
-{
-	return spi_register_driver(&dac124s085_driver);
-}
-
-static void __exit dac124s085_leds_exit(void)
-{
-	spi_unregister_driver(&dac124s085_driver);
-}
-
-module_init(dac124s085_leds_init);
-module_exit(dac124s085_leds_exit);
+module_spi_driver(dac124s085_driver);
 
 MODULE_AUTHOR("Guennadi Liakhovetski <lg@denx.de>");
 MODULE_DESCRIPTION("DAC124S085 LED driver");
diff --git a/drivers/leds/leds-fsg.c b/drivers/leds/leds-fsg.c
index 49aceffaa5b6..b9053fa6e253 100644
--- a/drivers/leds/leds-fsg.c
+++ b/drivers/leds/leds-fsg.c
@@ -224,20 +224,7 @@ static struct platform_driver fsg_led_driver = {
 	},
 };
 
-
-static int __init fsg_led_init(void)
-{
-	return platform_driver_register(&fsg_led_driver);
-}
-
-static void __exit fsg_led_exit(void)
-{
-	platform_driver_unregister(&fsg_led_driver);
-}
-
-
-module_init(fsg_led_init);
-module_exit(fsg_led_exit);
+module_platform_driver(fsg_led_driver);
 
 MODULE_AUTHOR("Rod Whitby <rod@whitby.id.au>");
 MODULE_DESCRIPTION("Freecom FSG-3 LED driver");
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index 399a86f2013a..7df74cb97e70 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -293,21 +293,9 @@ static struct platform_driver gpio_led_driver = {
 	},
 };
 
-MODULE_ALIAS("platform:leds-gpio");
-
-static int __init gpio_led_init(void)
-{
-	return platform_driver_register(&gpio_led_driver);
-}
-
-static void __exit gpio_led_exit(void)
-{
-	platform_driver_unregister(&gpio_led_driver);
-}
-
-module_init(gpio_led_init);
-module_exit(gpio_led_exit);
+module_platform_driver(gpio_led_driver);
 
 MODULE_AUTHOR("Raphael Assenat <raph@8d.com>, Trent Piepho <tpiepho@freescale.com>");
 MODULE_DESCRIPTION("GPIO LED driver");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:leds-gpio");
diff --git a/drivers/leds/leds-hp6xx.c b/drivers/leds/leds-hp6xx.c
index bcfbd3a60eab..366b6055e330 100644
--- a/drivers/leds/leds-hp6xx.c
+++ b/drivers/leds/leds-hp6xx.c
@@ -79,9 +79,6 @@ static int hp6xxled_remove(struct platform_device *pdev)
 	return 0;
 }
 
-/* work with hotplug and coldplug */
-MODULE_ALIAS("platform:hp6xx-led");
-
 static struct platform_driver hp6xxled_driver = {
 	.probe		= hp6xxled_probe,
 	.remove		= hp6xxled_remove,
@@ -91,19 +88,9 @@ static struct platform_driver hp6xxled_driver = {
 	},
 };
 
-static int __init hp6xxled_init(void)
-{
-	return platform_driver_register(&hp6xxled_driver);
-}
-
-static void __exit hp6xxled_exit(void)
-{
-	platform_driver_unregister(&hp6xxled_driver);
-}
-
-module_init(hp6xxled_init);
-module_exit(hp6xxled_exit);
+module_platform_driver(hp6xxled_driver);
 
 MODULE_AUTHOR("Kristoffer Ericson <kristoffer.ericson@gmail.com>");
 MODULE_DESCRIPTION("HP Jornada 6xx LED driver");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:hp6xx-led");
diff --git a/drivers/leds/leds-lm3530.c b/drivers/leds/leds-lm3530.c
index 0630e4f4b286..45e6878d7374 100644
--- a/drivers/leds/leds-lm3530.c
+++ b/drivers/leds/leds-lm3530.c
@@ -457,18 +457,7 @@ static struct i2c_driver lm3530_i2c_driver = {
 	},
 };
 
-static int __init lm3530_init(void)
-{
-	return i2c_add_driver(&lm3530_i2c_driver);
-}
-
-static void __exit lm3530_exit(void)
-{
-	i2c_del_driver(&lm3530_i2c_driver);
-}
-
-module_init(lm3530_init);
-module_exit(lm3530_exit);
+module_i2c_driver(lm3530_i2c_driver);
 
 MODULE_DESCRIPTION("Back Light driver for LM3530");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lp3944.c b/drivers/leds/leds-lp3944.c
index 9010c054615e..b8f9f0a5d431 100644
--- a/drivers/leds/leds-lp3944.c
+++ b/drivers/leds/leds-lp3944.c
@@ -453,18 +453,7 @@ static struct i2c_driver lp3944_driver = {
 	.id_table = lp3944_id,
 };
 
-static int __init lp3944_module_init(void)
-{
-	return i2c_add_driver(&lp3944_driver);
-}
-
-static void __exit lp3944_module_exit(void)
-{
-	i2c_del_driver(&lp3944_driver);
-}
-
-module_init(lp3944_module_init);
-module_exit(lp3944_module_exit);
+module_i2c_driver(lp3944_driver);
 
 MODULE_AUTHOR("Antonio Ospite <ospite@studenti.unina.it>");
 MODULE_DESCRIPTION("LP3944 Fun Light Chip");
diff --git a/drivers/leds/leds-lp5521.c b/drivers/leds/leds-lp5521.c
index cb641f1b3342..d62a7982a5e6 100644
--- a/drivers/leds/leds-lp5521.c
+++ b/drivers/leds/leds-lp5521.c
@@ -797,25 +797,7 @@ static struct i2c_driver lp5521_driver = {
 	.id_table	= lp5521_id,
 };
 
-static int __init lp5521_init(void)
-{
-	int ret;
-
-	ret = i2c_add_driver(&lp5521_driver);
-
-	if (ret < 0)
-		printk(KERN_ALERT "Adding lp5521 driver failed\n");
-
-	return ret;
-}
-
-static void __exit lp5521_exit(void)
-{
-	i2c_del_driver(&lp5521_driver);
-}
-
-module_init(lp5521_init);
-module_exit(lp5521_exit);
+module_i2c_driver(lp5521_driver);
 
 MODULE_AUTHOR("Mathias Nyman, Yuri Zaporozhets, Samu Onkalo");
 MODULE_DESCRIPTION("LP5521 LED engine");
diff --git a/drivers/leds/leds-lp5523.c b/drivers/leds/leds-lp5523.c
index 5971e309b234..73e791ae7259 100644
--- a/drivers/leds/leds-lp5523.c
+++ b/drivers/leds/leds-lp5523.c
@@ -870,8 +870,6 @@ static int __devinit lp5523_init_led(struct lp5523_led *led, struct device *dev,
 	return 0;
 }
 
-static struct i2c_driver lp5523_driver;
-
 static int __devinit lp5523_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
@@ -1021,25 +1019,7 @@ static struct i2c_driver lp5523_driver = {
 	.id_table	= lp5523_id,
 };
 
-static int __init lp5523_init(void)
-{
-	int ret;
-
-	ret = i2c_add_driver(&lp5523_driver);
-
-	if (ret < 0)
-		printk(KERN_ALERT "Adding lp5523 driver failed\n");
-
-	return ret;
-}
-
-static void __exit lp5523_exit(void)
-{
-	i2c_del_driver(&lp5523_driver);
-}
-
-module_init(lp5523_init);
-module_exit(lp5523_exit);
+module_i2c_driver(lp5523_driver);
 
 MODULE_AUTHOR("Mathias Nyman <mathias.nyman@nokia.com>");
 MODULE_DESCRIPTION("LP5523 LED engine");
diff --git a/drivers/leds/leds-lt3593.c b/drivers/leds/leds-lt3593.c
index 53f67b8ce55d..e311a96c4469 100644
--- a/drivers/leds/leds-lt3593.c
+++ b/drivers/leds/leds-lt3593.c
@@ -199,21 +199,9 @@ static struct platform_driver lt3593_led_driver = {
 	},
 };
 
-MODULE_ALIAS("platform:leds-lt3593");
-
-static int __init lt3593_led_init(void)
-{
-	return platform_driver_register(&lt3593_led_driver);
-}
-
-static void __exit lt3593_led_exit(void)
-{
-	platform_driver_unregister(&lt3593_led_driver);
-}
-
-module_init(lt3593_led_init);
-module_exit(lt3593_led_exit);
+module_platform_driver(lt3593_led_driver);
 
 MODULE_AUTHOR("Daniel Mack <daniel@caiaq.de>");
 MODULE_DESCRIPTION("LED driver for LT3593 controllers");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:leds-lt3593");
diff --git a/drivers/leds/leds-mc13783.c b/drivers/leds/leds-mc13783.c
index b3393a9f2139..8bc491541550 100644
--- a/drivers/leds/leds-mc13783.c
+++ b/drivers/leds/leds-mc13783.c
@@ -275,7 +275,7 @@ static int __devinit mc13783_led_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	if (pdata->num_leds < 1 || pdata->num_leds > MC13783_LED_MAX) {
+	if (pdata->num_leds < 1 || pdata->num_leds > (MC13783_LED_MAX + 1)) {
 		dev_err(&pdev->dev, "Invalid led count %d\n", pdata->num_leds);
 		return -EINVAL;
 	}
@@ -385,17 +385,7 @@ static struct platform_driver mc13783_led_driver = {
 	.remove		= __devexit_p(mc13783_led_remove),
 };
 
-static int __init mc13783_led_init(void)
-{
-	return platform_driver_register(&mc13783_led_driver);
-}
-module_init(mc13783_led_init);
-
-static void __exit mc13783_led_exit(void)
-{
-	platform_driver_unregister(&mc13783_led_driver);
-}
-module_exit(mc13783_led_exit);
+module_platform_driver(mc13783_led_driver);
 
 MODULE_DESCRIPTION("LEDs driver for Freescale MC13783 PMIC");
 MODULE_AUTHOR("Philippe Retornaz <philippe.retornaz@epfl.ch>");
diff --git a/drivers/leds/leds-netxbig.c b/drivers/leds/leds-netxbig.c
index f2e51c134399..d8433f2d53bc 100644
--- a/drivers/leds/leds-netxbig.c
+++ b/drivers/leds/leds-netxbig.c
@@ -81,35 +81,23 @@ static int __devinit gpio_ext_init(struct netxbig_gpio_ext *gpio_ext)
 
 	/* Configure address GPIOs. */
 	for (i = 0; i < gpio_ext->num_addr; i++) {
-		err = gpio_request(gpio_ext->addr[i], "GPIO extension addr");
+		err = gpio_request_one(gpio_ext->addr[i], GPIOF_OUT_INIT_LOW,
+				       "GPIO extension addr");
 		if (err)
 			goto err_free_addr;
-		err = gpio_direction_output(gpio_ext->addr[i], 0);
-		if (err) {
-			gpio_free(gpio_ext->addr[i]);
-			goto err_free_addr;
-		}
 	}
 	/* Configure data GPIOs. */
 	for (i = 0; i < gpio_ext->num_data; i++) {
-		err = gpio_request(gpio_ext->data[i], "GPIO extension data");
+		err = gpio_request_one(gpio_ext->data[i], GPIOF_OUT_INIT_LOW,
+				   "GPIO extension data");
 		if (err)
 			goto err_free_data;
-		err = gpio_direction_output(gpio_ext->data[i], 0);
-		if (err) {
-			gpio_free(gpio_ext->data[i]);
-			goto err_free_data;
-		}
 	}
 	/* Configure "enable select" GPIO. */
-	err = gpio_request(gpio_ext->enable, "GPIO extension enable");
+	err = gpio_request_one(gpio_ext->enable, GPIOF_OUT_INIT_LOW,
+			       "GPIO extension enable");
 	if (err)
 		goto err_free_data;
-	err = gpio_direction_output(gpio_ext->enable, 0);
-	if (err) {
-		gpio_free(gpio_ext->enable);
-		goto err_free_data;
-	}
 
 	return 0;
 
@@ -429,21 +417,10 @@ static struct platform_driver netxbig_led_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-MODULE_ALIAS("platform:leds-netxbig");
 
-static int __init netxbig_led_init(void)
-{
-	return platform_driver_register(&netxbig_led_driver);
-}
-
-static void __exit netxbig_led_exit(void)
-{
-	platform_driver_unregister(&netxbig_led_driver);
-}
-
-module_init(netxbig_led_init);
-module_exit(netxbig_led_exit);
+module_platform_driver(netxbig_led_driver);
 
 MODULE_AUTHOR("Simon Guinot <sguinot@lacie.com>");
 MODULE_DESCRIPTION("LED driver for LaCie xBig Network boards");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:leds-netxbig");
diff --git a/drivers/leds/leds-ns2.c b/drivers/leds/leds-ns2.c
index 37b7d0cfe586..2f0a14421a73 100644
--- a/drivers/leds/leds-ns2.c
+++ b/drivers/leds/leds-ns2.c
@@ -323,21 +323,10 @@ static struct platform_driver ns2_led_driver = {
 		.owner	= THIS_MODULE,
 	},
 };
-MODULE_ALIAS("platform:leds-ns2");
-
-static int __init ns2_led_init(void)
-{
-	return platform_driver_register(&ns2_led_driver);
-}
 
-static void __exit ns2_led_exit(void)
-{
-	platform_driver_unregister(&ns2_led_driver);
-}
-
-module_init(ns2_led_init);
-module_exit(ns2_led_exit);
+module_platform_driver(ns2_led_driver);
 
 MODULE_AUTHOR("Simon Guinot <sguinot@lacie.com>");
 MODULE_DESCRIPTION("Network Space v2 LED driver");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:leds-ns2");
diff --git a/drivers/leds/leds-pca9532.c b/drivers/leds/leds-pca9532.c
index a2c874623e35..ceccab44b5b8 100644
--- a/drivers/leds/leds-pca9532.c
+++ b/drivers/leds/leds-pca9532.c
@@ -489,20 +489,8 @@ static int pca9532_remove(struct i2c_client *client)
 	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_i2c_driver(pca9532_driver);
 
 MODULE_AUTHOR("Riku Voipio");
 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
index 66aa3e8e786f..dcc3bc3d38db 100644
--- a/drivers/leds/leds-pca955x.c
+++ b/drivers/leds/leds-pca955x.c
@@ -371,18 +371,7 @@ static struct i2c_driver pca955x_driver = {
 	.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_i2c_driver(pca955x_driver);
 
 MODULE_AUTHOR("Nate Case <ncase@xes-inc.com>");
 MODULE_DESCRIPTION("PCA955x LED driver");
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index 666daf77872e..3ed92f34bd44 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -135,18 +135,7 @@ static struct platform_driver led_pwm_driver = {
 	},
 };
 
-static int __init led_pwm_init(void)
-{
-	return platform_driver_register(&led_pwm_driver);
-}
-
-static void __exit led_pwm_exit(void)
-{
-	platform_driver_unregister(&led_pwm_driver);
-}
-
-module_init(led_pwm_init);
-module_exit(led_pwm_exit);
+module_platform_driver(led_pwm_driver);
 
 MODULE_AUTHOR("Luotao Fu <l.fu@pengutronix.de>");
 MODULE_DESCRIPTION("PWM LED driver for PXA");
diff --git a/drivers/leds/leds-rb532.c b/drivers/leds/leds-rb532.c
index c3525f37f73d..a7815b6cd856 100644
--- a/drivers/leds/leds-rb532.c
+++ b/drivers/leds/leds-rb532.c
@@ -57,21 +57,9 @@ static struct platform_driver rb532_led_driver = {
 	},
 };
 
-static int __init rb532_led_init(void)
-{
-	return platform_driver_register(&rb532_led_driver);
-}
-
-static void __exit rb532_led_exit(void)
-{
-	platform_driver_unregister(&rb532_led_driver);
-}
-
-module_init(rb532_led_init);
-module_exit(rb532_led_exit);
-
-MODULE_ALIAS("platform:rb532-led");
+module_platform_driver(rb532_led_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("User LED support for Routerboard532");
 MODULE_AUTHOR("Phil Sutter <n0-1@freewrt.org>");
+MODULE_ALIAS("platform:rb532-led");
diff --git a/drivers/leds/leds-regulator.c b/drivers/leds/leds-regulator.c
index 8497f56f8e46..df7e963bddd3 100644
--- a/drivers/leds/leds-regulator.c
+++ b/drivers/leds/leds-regulator.c
@@ -229,17 +229,7 @@ static struct platform_driver regulator_led_driver = {
 	.remove = __devexit_p(regulator_led_remove),
 };
 
-static int __init regulator_led_init(void)
-{
-	return platform_driver_register(&regulator_led_driver);
-}
-module_init(regulator_led_init);
-
-static void __exit regulator_led_exit(void)
-{
-	platform_driver_unregister(&regulator_led_driver);
-}
-module_exit(regulator_led_exit);
+module_platform_driver(regulator_led_driver);
 
 MODULE_AUTHOR("Antonio Ospite <ospite@studenti.unina.it>");
 MODULE_DESCRIPTION("Regulator driven LED driver");
diff --git a/drivers/leds/leds-renesas-tpu.c b/drivers/leds/leds-renesas-tpu.c
index 3ee540eb127e..32fe337d5c68 100644
--- a/drivers/leds/leds-renesas-tpu.c
+++ b/drivers/leds/leds-renesas-tpu.c
@@ -339,18 +339,7 @@ static struct platform_driver r_tpu_device_driver = {
 	}
 };
 
-static int __init r_tpu_init(void)
-{
-	return platform_driver_register(&r_tpu_device_driver);
-}
-
-static void __exit r_tpu_exit(void)
-{
-	platform_driver_unregister(&r_tpu_device_driver);
-}
-
-module_init(r_tpu_init);
-module_exit(r_tpu_exit);
+module_platform_driver(r_tpu_device_driver);
 
 MODULE_AUTHOR("Magnus Damm");
 MODULE_DESCRIPTION("Renesas TPU LED Driver");
diff --git a/drivers/leds/leds-s3c24xx.c b/drivers/leds/leds-s3c24xx.c
index 29f8b0f0e2c6..bd0a5ed49c42 100644
--- a/drivers/leds/leds-s3c24xx.c
+++ b/drivers/leds/leds-s3c24xx.c
@@ -121,18 +121,7 @@ static struct platform_driver s3c24xx_led_driver = {
 	},
 };
 
-static int __init s3c24xx_led_init(void)
-{
-	return platform_driver_register(&s3c24xx_led_driver);
-}
-
-static void __exit s3c24xx_led_exit(void)
-{
-	platform_driver_unregister(&s3c24xx_led_driver);
-}
-
-module_init(s3c24xx_led_init);
-module_exit(s3c24xx_led_exit);
+module_platform_driver(s3c24xx_led_driver);
 
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
 MODULE_DESCRIPTION("S3C24XX LED driver");
diff --git a/drivers/leds/leds-tca6507.c b/drivers/leds/leds-tca6507.c
new file mode 100644
index 000000000000..133f89fb7071
--- /dev/null
+++ b/drivers/leds/leds-tca6507.c
@@ -0,0 +1,779 @@
+/*
+ * leds-tca6507
+ *
+ * The TCA6507 is a programmable LED controller that can drive 7
+ * separate lines either by holding them low, or by pulsing them
+ * with modulated width.
+ * The modulation can be varied in a simple pattern to produce a blink or
+ * double-blink.
+ *
+ * This driver can configure each line either as a 'GPIO' which is out-only
+ * (no pull-up) or as an LED with variable brightness and hardware-assisted
+ * blinking.
+ *
+ * Apart from OFF and ON there are three programmable brightness levels which
+ * can be programmed from 0 to 15 and indicate how many 500usec intervals in
+ * each 8msec that the led is 'on'.  The levels are named MASTER, BANK0 and
+ * BANK1.
+ *
+ * There are two different blink rates that can be programmed, each with
+ * separate time for rise, on, fall, off and second-off.  Thus if 3 or more
+ * different non-trivial rates are required, software must be used for the extra
+ * rates. The two different blink rates must align with the two levels BANK0 and
+ * BANK1.
+ * This driver does not support double-blink so 'second-off' always matches
+ * 'off'.
+ *
+ * Only 16 different times can be programmed in a roughly logarithmic scale from
+ * 64ms to 16320ms.  To be precise the possible times are:
+ *    0, 64, 128, 192, 256, 384, 512, 768,
+ *    1024, 1536, 2048, 3072, 4096, 5760, 8128, 16320
+ *
+ * Times that cannot be closely matched with these must be
+ * handled in software.  This driver allows 12.5% error in matching.
+ *
+ * This driver does not allow rise/fall rates to be set explicitly.  When trying
+ * to match a given 'on' or 'off' period, an appropriate pair of 'change' and
+ * 'hold' times are chosen to get a close match.  If the target delay is even,
+ * the 'change' number will be the smaller; if odd, the 'hold' number will be
+ * the smaller.
+
+ * Choosing pairs of delays with 12.5% errors allows us to match delays in the
+ * ranges: 56-72, 112-144, 168-216, 224-27504, 28560-36720.
+ * 26% of the achievable sums can be matched by multiple pairings. For example
+ * 1536 == 1536+0, 1024+512, or 768+768.  This driver will always choose the
+ * pairing with the least maximum - 768+768 in this case.  Other pairings are
+ * not available.
+ *
+ * Access to the 3 levels and 2 blinks are on a first-come, first-served basis.
+ * Access can be shared by multiple leds if they have the same level and
+ * either same blink rates, or some don't blink.
+ * When a led changes, it relinquishes access and tries again, so it might
+ * lose access to hardware blink.
+ * If a blink engine cannot be allocated, software blink is used.
+ * If the desired brightness cannot be allocated, the closest available non-zero
+ * brightness is used.  As 'full' is always available, the worst case would be
+ * to have two different blink rates at '1', with Max at '2', then other leds
+ * will have to choose between '2' and '16'.  Hopefully this is not likely.
+ *
+ * Each bank (BANK0 and BANK1) has two usage counts - LEDs using the brightness
+ * and LEDs using the blink.  It can only be reprogrammed when the appropriate
+ * counter is zero.  The MASTER level has a single usage count.
+ *
+ * Each Led has programmable 'on' and 'off' time as milliseconds.  With each
+ * there is a flag saying if it was explicitly requested or defaulted.
+ * Similarly the banks know if each time was explicit or a default.  Defaults
+ * are permitted to be changed freely - they are not recognised when matching.
+ *
+ *
+ * An led-tca6507 device must be provided with platform data.  This data
+ * lists for each output: the name, default trigger, and whether the signal
+ * is being used as a GPiO rather than an led.  'struct led_plaform_data'
+ * is used for this.  If 'name' is NULL, the output isn't used.  If 'flags'
+ * is TCA6507_MAKE_CPIO, the output is a GPO.
+ * The "struct led_platform_data" can be embedded in a
+ * "struct tca6507_platform_data" which adds a 'gpio_base' for the GPiOs,
+ * and a 'setup' callback which is called once the GPiOs are available.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/leds.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/workqueue.h>
+#include <linux/leds-tca6507.h>
+
+/* LED select registers determine the source that drives LED outputs */
+#define TCA6507_LS_LED_OFF	0x0	/* Output HI-Z (off) */
+#define TCA6507_LS_LED_OFF1	0x1	/* Output HI-Z (off) - not used */
+#define TCA6507_LS_LED_PWM0	0x2	/* Output LOW with Bank0 rate */
+#define TCA6507_LS_LED_PWM1	0x3	/* Output LOW with Bank1 rate */
+#define TCA6507_LS_LED_ON	0x4	/* Output LOW (on) */
+#define TCA6507_LS_LED_MIR	0x5	/* Output LOW with Master Intensity */
+#define TCA6507_LS_BLINK0	0x6	/* Blink at Bank0 rate */
+#define TCA6507_LS_BLINK1	0x7	/* Blink at Bank1 rate */
+
+enum {
+	BANK0,
+	BANK1,
+	MASTER,
+};
+static int bank_source[3] = {
+	TCA6507_LS_LED_PWM0,
+	TCA6507_LS_LED_PWM1,
+	TCA6507_LS_LED_MIR,
+};
+static int blink_source[2] = {
+	TCA6507_LS_BLINK0,
+	TCA6507_LS_BLINK1,
+};
+
+/* PWM registers */
+#define	TCA6507_REG_CNT			11
+
+/*
+ * 0x00, 0x01, 0x02 encode the TCA6507_LS_* values, each output
+ * owns one bit in each register
+ */
+#define	TCA6507_FADE_ON			0x03
+#define	TCA6507_FULL_ON			0x04
+#define	TCA6507_FADE_OFF		0x05
+#define	TCA6507_FIRST_OFF		0x06
+#define	TCA6507_SECOND_OFF		0x07
+#define	TCA6507_MAX_INTENSITY		0x08
+#define	TCA6507_MASTER_INTENSITY	0x09
+#define	TCA6507_INITIALIZE		0x0A
+
+#define	INIT_CODE			0x8
+
+#define TIMECODES 16
+static int time_codes[TIMECODES] = {
+	0, 64, 128, 192, 256, 384, 512, 768,
+	1024, 1536, 2048, 3072, 4096, 5760, 8128, 16320
+};
+
+/* Convert an led.brightness level (0..255) to a TCA6507 level (0..15) */
+static inline int TO_LEVEL(int brightness)
+{
+	return brightness >> 4;
+}
+
+/* ...and convert back */
+static inline int TO_BRIGHT(int level)
+{
+	if (level)
+		return (level << 4) | 0xf;
+	return 0;
+}
+
+#define NUM_LEDS 7
+struct tca6507_chip {
+	int			reg_set;	/* One bit per register where
+						 * a '1' means the register
+						 * should be written */
+	u8			reg_file[TCA6507_REG_CNT];
+	/* Bank 2 is Master Intensity and doesn't use times */
+	struct bank {
+		int level;
+		int ontime, offtime;
+		int on_dflt, off_dflt;
+		int time_use, level_use;
+	} bank[3];
+	struct i2c_client	*client;
+	struct work_struct	work;
+	spinlock_t		lock;
+
+	struct tca6507_led {
+		struct tca6507_chip	*chip;
+		struct led_classdev	led_cdev;
+		int			num;
+		int			ontime, offtime;
+		int			on_dflt, off_dflt;
+		int			bank;	/* Bank used, or -1 */
+		int			blink;	/* Set if hardware-blinking */
+	} leds[NUM_LEDS];
+#ifdef CONFIG_GPIOLIB
+	struct gpio_chip		gpio;
+	const char			*gpio_name[NUM_LEDS];
+	int				gpio_map[NUM_LEDS];
+#endif
+};
+
+static const struct i2c_device_id tca6507_id[] = {
+	{ "tca6507" },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tca6507_id);
+
+static int choose_times(int msec, int *c1p, int *c2p)
+{
+	/*
+	 * Choose two timecodes which add to 'msec' as near as possible.
+	 * The first returned is the 'on' or 'off' time.  The second is to be
+	 * used as a 'fade-on' or 'fade-off' time.  If 'msec' is even,
+	 * the first will not be smaller than the second.  If 'msec' is odd,
+	 * the first will not be larger than the second.
+	 * If we cannot get a sum within 1/8 of 'msec' fail with -EINVAL,
+	 * otherwise return the sum that was achieved, plus 1 if the first is
+	 * smaller.
+	 * If two possibilities are equally good (e.g. 512+0, 256+256), choose
+	 * the first pair so there is more change-time visible (i.e. it is
+	 * softer).
+	 */
+	int c1, c2;
+	int tmax = msec * 9 / 8;
+	int tmin = msec * 7 / 8;
+	int diff = 65536;
+
+	/* We start at '1' to ensure we never even think of choosing a
+	 * total time of '0'.
+	 */
+	for (c1 = 1; c1 < TIMECODES; c1++) {
+		int t = time_codes[c1];
+		if (t*2 < tmin)
+			continue;
+		if (t > tmax)
+			break;
+		for (c2 = 0; c2 <= c1; c2++) {
+			int tt = t + time_codes[c2];
+			int d;
+			if (tt < tmin)
+				continue;
+			if (tt > tmax)
+				break;
+			/* This works! */
+			d = abs(msec - tt);
+			if (d >= diff)
+				continue;
+			/* Best yet */
+			*c1p = c1;
+			*c2p = c2;
+			diff = d;
+			if (d == 0)
+				return msec;
+		}
+	}
+	if (diff < 65536) {
+		int actual;
+		if (msec & 1) {
+			c1 = *c2p;
+			*c2p = *c1p;
+			*c1p = c1;
+		}
+		actual = time_codes[*c1p] + time_codes[*c2p];
+		if (*c1p < *c2p)
+			return actual + 1;
+		else
+			return actual;
+	}
+	/* No close match */
+	return -EINVAL;
+}
+
+/*
+ * Update the register file with the appropriate 3-bit state for
+ * the given led.
+ */
+static void set_select(struct tca6507_chip *tca, int led, int val)
+{
+	int mask = (1 << led);
+	int bit;
+
+	for (bit = 0; bit < 3; bit++) {
+		int n = tca->reg_file[bit] & ~mask;
+		if (val & (1 << bit))
+			n |= mask;
+		if (tca->reg_file[bit] != n) {
+			tca->reg_file[bit] = n;
+			tca->reg_set |= (1 << bit);
+		}
+	}
+}
+
+/* Update the register file with the appropriate 4-bit code for
+ * one bank or other.  This can be used for timers, for levels, or
+ * for initialisation.
+ */
+static void set_code(struct tca6507_chip *tca, int reg, int bank, int new)
+{
+	int mask = 0xF;
+	int n;
+	if (bank) {
+		mask <<= 4;
+		new <<= 4;
+	}
+	n = tca->reg_file[reg] & ~mask;
+	n |= new;
+	if (tca->reg_file[reg] != n) {
+		tca->reg_file[reg] = n;
+		tca->reg_set |= 1 << reg;
+	}
+}
+
+/* Update brightness level. */
+static void set_level(struct tca6507_chip *tca, int bank, int level)
+{
+	switch (bank) {
+	case BANK0:
+	case BANK1:
+		set_code(tca, TCA6507_MAX_INTENSITY, bank, level);
+		break;
+	case MASTER:
+		set_code(tca, TCA6507_MASTER_INTENSITY, 0, level);
+		break;
+	}
+	tca->bank[bank].level = level;
+}
+
+/* Record all relevant time code for a given bank */
+static void set_times(struct tca6507_chip *tca, int bank)
+{
+	int c1, c2;
+	int result;
+
+	result = choose_times(tca->bank[bank].ontime, &c1, &c2);
+	dev_dbg(&tca->client->dev,
+		"Chose on  times %d(%d) %d(%d) for %dms\n", c1, time_codes[c1],
+		c2, time_codes[c2], tca->bank[bank].ontime);
+	set_code(tca, TCA6507_FADE_ON, bank, c2);
+	set_code(tca, TCA6507_FULL_ON, bank, c1);
+	tca->bank[bank].ontime = result;
+
+	result = choose_times(tca->bank[bank].offtime, &c1, &c2);
+	dev_dbg(&tca->client->dev,
+		"Chose off times %d(%d) %d(%d) for %dms\n", c1, time_codes[c1],
+		c2, time_codes[c2], tca->bank[bank].offtime);
+	set_code(tca, TCA6507_FADE_OFF, bank, c2);
+	set_code(tca, TCA6507_FIRST_OFF, bank, c1);
+	set_code(tca, TCA6507_SECOND_OFF, bank, c1);
+	tca->bank[bank].offtime = result;
+
+	set_code(tca, TCA6507_INITIALIZE, bank, INIT_CODE);
+}
+
+/* Write all needed register of tca6507 */
+
+static void tca6507_work(struct work_struct *work)
+{
+	struct tca6507_chip *tca = container_of(work, struct tca6507_chip,
+						work);
+	struct i2c_client *cl = tca->client;
+	int set;
+	u8 file[TCA6507_REG_CNT];
+	int r;
+
+	spin_lock_irq(&tca->lock);
+	set = tca->reg_set;
+	memcpy(file, tca->reg_file, TCA6507_REG_CNT);
+	tca->reg_set = 0;
+	spin_unlock_irq(&tca->lock);
+
+	for (r = 0; r < TCA6507_REG_CNT; r++)
+		if (set & (1<<r))
+			i2c_smbus_write_byte_data(cl, r, file[r]);
+}
+
+static void led_release(struct tca6507_led *led)
+{
+	/* If led owns any resource, release it. */
+	struct tca6507_chip *tca = led->chip;
+	if (led->bank >= 0) {
+		struct bank *b = tca->bank + led->bank;
+		if (led->blink)
+			b->time_use--;
+		b->level_use--;
+	}
+	led->blink = 0;
+	led->bank = -1;
+}
+
+static int led_prepare(struct tca6507_led *led)
+{
+	/* Assign this led to a bank, configuring that bank if necessary. */
+	int level = TO_LEVEL(led->led_cdev.brightness);
+	struct tca6507_chip *tca = led->chip;
+	int c1, c2;
+	int i;
+	struct bank *b;
+	int need_init = 0;
+
+	led->led_cdev.brightness = TO_BRIGHT(level);
+	if (level == 0) {
+		set_select(tca, led->num, TCA6507_LS_LED_OFF);
+		return 0;
+	}
+
+	if (led->ontime == 0 || led->offtime == 0) {
+		/*
+		 * Just set the brightness, choosing first usable bank.
+		 * If none perfect, choose best.
+		 * Count backwards so we check MASTER bank first
+		 * to avoid wasting a timer.
+		 */
+		int best = -1;/* full-on */
+		int diff = 15-level;
+
+		if (level == 15) {
+			set_select(tca, led->num, TCA6507_LS_LED_ON);
+			return 0;
+		}
+
+		for (i = MASTER; i >= BANK0; i--) {
+			int d;
+			if (tca->bank[i].level == level ||
+			    tca->bank[i].level_use == 0) {
+				best = i;
+				break;
+			}
+			d = abs(level - tca->bank[i].level);
+			if (d < diff) {
+				diff = d;
+				best = i;
+			}
+		}
+		if (best == -1) {
+			/* Best brightness is full-on */
+			set_select(tca, led->num, TCA6507_LS_LED_ON);
+			led->led_cdev.brightness = LED_FULL;
+			return 0;
+		}
+
+		if (!tca->bank[best].level_use)
+			set_level(tca, best, level);
+
+		tca->bank[best].level_use++;
+		led->bank = best;
+		set_select(tca, led->num, bank_source[best]);
+		led->led_cdev.brightness = TO_BRIGHT(tca->bank[best].level);
+		return 0;
+	}
+
+	/*
+	 * We have on/off time so we need to try to allocate a timing bank.
+	 * First check if times are compatible with hardware and give up if
+	 * not.
+	 */
+	if (choose_times(led->ontime, &c1, &c2) < 0)
+		return -EINVAL;
+	if (choose_times(led->offtime, &c1, &c2) < 0)
+		return -EINVAL;
+
+	for (i = BANK0; i <= BANK1; i++) {
+		if (tca->bank[i].level_use == 0)
+			/* not in use - it is ours! */
+			break;
+		if (tca->bank[i].level != level)
+			/* Incompatible level - skip */
+			/* FIX: if timer matches we maybe should consider
+			 * this anyway...
+			 */
+			continue;
+
+		if (tca->bank[i].time_use == 0)
+			/* Timer not in use, and level matches - use it */
+			break;
+
+		if (!(tca->bank[i].on_dflt ||
+		      led->on_dflt ||
+		      tca->bank[i].ontime == led->ontime))
+			/* on time is incompatible */
+			continue;
+
+		if (!(tca->bank[i].off_dflt ||
+		      led->off_dflt ||
+		      tca->bank[i].offtime == led->offtime))
+			/* off time is incompatible */
+			continue;
+
+		/* looks like a suitable match */
+		break;
+	}
+
+	if (i > BANK1)
+		/* Nothing matches - how sad */
+		return -EINVAL;
+
+	b = &tca->bank[i];
+	if (b->level_use == 0)
+		set_level(tca, i, level);
+	b->level_use++;
+	led->bank = i;
+
+	if (b->on_dflt ||
+	    !led->on_dflt ||
+	    b->time_use == 0) {
+		b->ontime = led->ontime;
+		b->on_dflt = led->on_dflt;
+		need_init = 1;
+	}
+
+	if (b->off_dflt ||
+	    !led->off_dflt ||
+	    b->time_use == 0) {
+		b->offtime = led->offtime;
+		b->off_dflt = led->off_dflt;
+		need_init = 1;
+	}
+
+	if (need_init)
+		set_times(tca, i);
+
+	led->ontime = b->ontime;
+	led->offtime = b->offtime;
+
+	b->time_use++;
+	led->blink = 1;
+	led->led_cdev.brightness = TO_BRIGHT(b->level);
+	set_select(tca, led->num, blink_source[i]);
+	return 0;
+}
+
+static int led_assign(struct tca6507_led *led)
+{
+	struct tca6507_chip *tca = led->chip;
+	int err;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tca->lock, flags);
+	led_release(led);
+	err = led_prepare(led);
+	if (err) {
+		/*
+		 * Can only fail on timer setup.  In that case we need to
+		 * re-establish as steady level.
+		 */
+		led->ontime = 0;
+		led->offtime = 0;
+		led_prepare(led);
+	}
+	spin_unlock_irqrestore(&tca->lock, flags);
+
+	if (tca->reg_set)
+		schedule_work(&tca->work);
+	return err;
+}
+
+static void tca6507_brightness_set(struct led_classdev *led_cdev,
+				   enum led_brightness brightness)
+{
+	struct tca6507_led *led = container_of(led_cdev, struct tca6507_led,
+					       led_cdev);
+	led->led_cdev.brightness = brightness;
+	led->ontime = 0;
+	led->offtime = 0;
+	led_assign(led);
+}
+
+static int tca6507_blink_set(struct led_classdev *led_cdev,
+			     unsigned long *delay_on,
+			     unsigned long *delay_off)
+{
+	struct tca6507_led *led = container_of(led_cdev, struct tca6507_led,
+					       led_cdev);
+
+	if (*delay_on == 0)
+		led->on_dflt = 1;
+	else if (delay_on != &led_cdev->blink_delay_on)
+		led->on_dflt = 0;
+	led->ontime = *delay_on;
+
+	if (*delay_off == 0)
+		led->off_dflt = 1;
+	else if (delay_off != &led_cdev->blink_delay_off)
+		led->off_dflt = 0;
+	led->offtime = *delay_off;
+
+	if (led->ontime == 0)
+		led->ontime = 512;
+	if (led->offtime == 0)
+		led->offtime = 512;
+
+	if (led->led_cdev.brightness == LED_OFF)
+		led->led_cdev.brightness = LED_FULL;
+	if (led_assign(led) < 0) {
+		led->ontime = 0;
+		led->offtime = 0;
+		led->led_cdev.brightness = LED_OFF;
+		return -EINVAL;
+	}
+	*delay_on = led->ontime;
+	*delay_off = led->offtime;
+	return 0;
+}
+
+#ifdef CONFIG_GPIOLIB
+static void tca6507_gpio_set_value(struct gpio_chip *gc,
+				   unsigned offset, int val)
+{
+	struct tca6507_chip *tca = container_of(gc, struct tca6507_chip, gpio);
+	unsigned long flags;
+
+	spin_lock_irqsave(&tca->lock, flags);
+	/*
+	 * 'OFF' is floating high, and 'ON' is pulled down, so it has the
+	 * inverse sense of 'val'.
+	 */
+	set_select(tca, tca->gpio_map[offset],
+		   val ? TCA6507_LS_LED_OFF : TCA6507_LS_LED_ON);
+	spin_unlock_irqrestore(&tca->lock, flags);
+	if (tca->reg_set)
+		schedule_work(&tca->work);
+}
+
+static int tca6507_gpio_direction_output(struct gpio_chip *gc,
+					  unsigned offset, int val)
+{
+	tca6507_gpio_set_value(gc, offset, val);
+	return 0;
+}
+
+static int tca6507_probe_gpios(struct i2c_client *client,
+			       struct tca6507_chip *tca,
+			       struct tca6507_platform_data *pdata)
+{
+	int err;
+	int i = 0;
+	int gpios = 0;
+
+	for (i = 0; i < NUM_LEDS; i++)
+		if (pdata->leds.leds[i].name && pdata->leds.leds[i].flags) {
+			/* Configure as a gpio */
+			tca->gpio_name[gpios] = pdata->leds.leds[i].name;
+			tca->gpio_map[gpios] = i;
+			gpios++;
+		}
+
+	if (!gpios)
+		return 0;
+
+	tca->gpio.label = "gpio-tca6507";
+	tca->gpio.names = tca->gpio_name;
+	tca->gpio.ngpio = gpios;
+	tca->gpio.base = pdata->gpio_base;
+	tca->gpio.owner = THIS_MODULE;
+	tca->gpio.direction_output = tca6507_gpio_direction_output;
+	tca->gpio.set = tca6507_gpio_set_value;
+	tca->gpio.dev = &client->dev;
+	err = gpiochip_add(&tca->gpio);
+	if (err) {
+		tca->gpio.ngpio = 0;
+		return err;
+	}
+	if (pdata->setup)
+		pdata->setup(tca->gpio.base, tca->gpio.ngpio);
+	return 0;
+}
+
+static void tca6507_remove_gpio(struct tca6507_chip *tca)
+{
+	if (tca->gpio.ngpio) {
+		int err = gpiochip_remove(&tca->gpio);
+		dev_err(&tca->client->dev, "%s failed, %d\n",
+			"gpiochip_remove()", err);
+	}
+}
+#else /* CONFIG_GPIOLIB */
+static int tca6507_probe_gpios(struct i2c_client *client,
+			       struct tca6507_chip *tca,
+			       struct tca6507_platform_data *pdata)
+{
+	return 0;
+}
+static void tca6507_remove_gpio(struct tca6507_chip *tca)
+{
+}
+#endif /* CONFIG_GPIOLIB */
+
+static int __devinit tca6507_probe(struct i2c_client *client,
+				   const struct i2c_device_id *id)
+{
+	struct tca6507_chip *tca;
+	struct i2c_adapter *adapter;
+	struct tca6507_platform_data *pdata;
+	int err;
+	int i = 0;
+
+	adapter = to_i2c_adapter(client->dev.parent);
+	pdata = client->dev.platform_data;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -EIO;
+
+	if (!pdata || pdata->leds.num_leds != NUM_LEDS) {
+		dev_err(&client->dev, "Need %d entries in platform-data list\n",
+			NUM_LEDS);
+		return -ENODEV;
+	}
+	err = -ENOMEM;
+	tca = kzalloc(sizeof(*tca), GFP_KERNEL);
+	if (!tca)
+		goto exit;
+
+	tca->client = client;
+	INIT_WORK(&tca->work, tca6507_work);
+	spin_lock_init(&tca->lock);
+	i2c_set_clientdata(client, tca);
+
+	for (i = 0; i < NUM_LEDS; i++) {
+		struct tca6507_led *l = tca->leds + i;
+
+		l->chip = tca;
+		l->num = i;
+		if (pdata->leds.leds[i].name && !pdata->leds.leds[i].flags) {
+			l->led_cdev.name = pdata->leds.leds[i].name;
+			l->led_cdev.default_trigger
+				= pdata->leds.leds[i].default_trigger;
+			l->led_cdev.brightness_set = tca6507_brightness_set;
+			l->led_cdev.blink_set = tca6507_blink_set;
+			l->bank = -1;
+			err = led_classdev_register(&client->dev,
+						    &l->led_cdev);
+			if (err < 0)
+				goto exit;
+		}
+	}
+	err = tca6507_probe_gpios(client, tca, pdata);
+	if (err)
+		goto exit;
+	/* set all registers to known state - zero */
+	tca->reg_set = 0x7f;
+	schedule_work(&tca->work);
+
+	return 0;
+exit:
+	while (i--)
+		if (tca->leds[i].led_cdev.name)
+			led_classdev_unregister(&tca->leds[i].led_cdev);
+	cancel_work_sync(&tca->work);
+	i2c_set_clientdata(client, NULL);
+	kfree(tca);
+	return err;
+}
+
+static int __devexit tca6507_remove(struct i2c_client *client)
+{
+	int i;
+	struct tca6507_chip *tca = i2c_get_clientdata(client);
+	struct tca6507_led *tca_leds = tca->leds;
+
+	for (i = 0; i < NUM_LEDS; i++) {
+		if (tca_leds[i].led_cdev.name)
+			led_classdev_unregister(&tca_leds[i].led_cdev);
+	}
+	tca6507_remove_gpio(tca);
+	cancel_work_sync(&tca->work);
+	i2c_set_clientdata(client, NULL);
+	kfree(tca);
+
+	return 0;
+}
+
+static struct i2c_driver tca6507_driver = {
+	.driver   = {
+		.name    = "leds-tca6507",
+		.owner   = THIS_MODULE,
+	},
+	.probe    = tca6507_probe,
+	.remove   = __devexit_p(tca6507_remove),
+	.id_table = tca6507_id,
+};
+
+static int __init tca6507_leds_init(void)
+{
+	return i2c_add_driver(&tca6507_driver);
+}
+
+static void __exit tca6507_leds_exit(void)
+{
+	i2c_del_driver(&tca6507_driver);
+}
+
+module_init(tca6507_leds_init);
+module_exit(tca6507_leds_exit);
+
+MODULE_AUTHOR("NeilBrown <neilb@suse.de>");
+MODULE_DESCRIPTION("TCA6507 LED/GPO driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-wm831x-status.c b/drivers/leds/leds-wm831x-status.c
index b1eb34c3e81f..74a24cf897c3 100644
--- a/drivers/leds/leds-wm831x-status.c
+++ b/drivers/leds/leds-wm831x-status.c
@@ -237,7 +237,8 @@ static int wm831x_status_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	drvdata = kzalloc(sizeof(struct wm831x_status), GFP_KERNEL);
+	drvdata = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_status),
+			       GFP_KERNEL);
 	if (!drvdata)
 		return -ENOMEM;
 	dev_set_drvdata(&pdev->dev, drvdata);
@@ -300,7 +301,6 @@ static int wm831x_status_probe(struct platform_device *pdev)
 
 err_led:
 	led_classdev_unregister(&drvdata->cdev);
-	kfree(drvdata);
 err:
 	return ret;
 }
@@ -311,7 +311,6 @@ static int wm831x_status_remove(struct platform_device *pdev)
 
 	device_remove_file(drvdata->cdev.dev, &dev_attr_src);
 	led_classdev_unregister(&drvdata->cdev);
-	kfree(drvdata);
 
 	return 0;
 }
@@ -325,17 +324,7 @@ static struct platform_driver wm831x_status_driver = {
 	.remove = wm831x_status_remove,
 };
 
-static int __devinit wm831x_status_init(void)
-{
-	return platform_driver_register(&wm831x_status_driver);
-}
-module_init(wm831x_status_init);
-
-static void wm831x_status_exit(void)
-{
-	platform_driver_unregister(&wm831x_status_driver);
-}
-module_exit(wm831x_status_exit);
+module_platform_driver(wm831x_status_driver);
 
 MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
 MODULE_DESCRIPTION("WM831x status LED driver");
diff --git a/drivers/leds/leds-wm8350.c b/drivers/leds/leds-wm8350.c
index 4a1276578352..918d4baff1c7 100644
--- a/drivers/leds/leds-wm8350.c
+++ b/drivers/leds/leds-wm8350.c
@@ -227,7 +227,7 @@ static int wm8350_led_probe(struct platform_device *pdev)
 		goto err_isink;
 	}
 
-	led = kzalloc(sizeof(*led), GFP_KERNEL);
+	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
 	if (led == NULL) {
 		ret = -ENOMEM;
 		goto err_dcdc;
@@ -259,12 +259,10 @@ static int wm8350_led_probe(struct platform_device *pdev)
 
 	ret = led_classdev_register(&pdev->dev, &led->cdev);
 	if (ret < 0)
-		goto err_led;
+		goto err_dcdc;
 
 	return 0;
 
- err_led:
-	kfree(led);
  err_dcdc:
 	regulator_put(dcdc);
  err_isink:
@@ -281,7 +279,6 @@ static int wm8350_led_remove(struct platform_device *pdev)
 	wm8350_led_disable(led);
 	regulator_put(led->dcdc);
 	regulator_put(led->isink);
-	kfree(led);
 	return 0;
 }
 
@@ -295,17 +292,7 @@ static struct platform_driver wm8350_led_driver = {
 	.shutdown = wm8350_led_shutdown,
 };
 
-static int __devinit wm8350_led_init(void)
-{
-	return platform_driver_register(&wm8350_led_driver);
-}
-module_init(wm8350_led_init);
-
-static void wm8350_led_exit(void)
-{
-	platform_driver_unregister(&wm8350_led_driver);
-}
-module_exit(wm8350_led_exit);
+module_platform_driver(wm8350_led_driver);
 
 MODULE_AUTHOR("Mark Brown");
 MODULE_DESCRIPTION("WM8350 LED driver");
diff --git a/drivers/media/video/davinci/vpif.h b/drivers/media/video/davinci/vpif.h
index 10550bd93b06..25036cb11bea 100644
--- a/drivers/media/video/davinci/vpif.h
+++ b/drivers/media/video/davinci/vpif.h
@@ -20,6 +20,7 @@
 #include <linux/videodev2.h>
 #include <mach/hardware.h>
 #include <mach/dm646x.h>
+#include <media/davinci/vpif_types.h>
 
 /* Maximum channel allowed */
 #define VPIF_NUM_CHANNELS		(4)
diff --git a/drivers/media/video/davinci/vpif_capture.h b/drivers/media/video/davinci/vpif_capture.h
index 064550f5ce4a..a693d4ebda55 100644
--- a/drivers/media/video/davinci/vpif_capture.h
+++ b/drivers/media/video/davinci/vpif_capture.h
@@ -27,7 +27,7 @@
 #include <media/v4l2-device.h>
 #include <media/videobuf-core.h>
 #include <media/videobuf-dma-contig.h>
-#include <mach/dm646x.h>
+#include <media/davinci/vpif_types.h>
 
 #include "vpif.h"
 
diff --git a/drivers/media/video/davinci/vpif_display.h b/drivers/media/video/davinci/vpif_display.h
index 5d1936dafed2..56879d1a0684 100644
--- a/drivers/media/video/davinci/vpif_display.h
+++ b/drivers/media/video/davinci/vpif_display.h
@@ -22,6 +22,7 @@
 #include <media/v4l2-device.h>
 #include <media/videobuf-core.h>
 #include <media/videobuf-dma-contig.h>
+#include <media/davinci/vpif_types.h>
 
 #include "vpif.h"
 
diff --git a/drivers/media/video/omap3isp/isp.c b/drivers/media/video/omap3isp/isp.c
index b818cacf420f..d4c48ef227fb 100644
--- a/drivers/media/video/omap3isp/isp.c
+++ b/drivers/media/video/omap3isp/isp.c
@@ -80,13 +80,6 @@
 #include "isph3a.h"
 #include "isphist.h"
 
-/*
- * this is provided as an interim solution until omap3isp doesn't need
- * any omap-specific iommu API
- */
-#define to_iommu(dev)							\
-	(struct omap_iommu *)platform_get_drvdata(to_platform_device(dev))
-
 static unsigned int autoidle;
 module_param(autoidle, int, 0444);
 MODULE_PARM_DESC(autoidle, "Enable OMAP3ISP AUTOIDLE support");
@@ -1114,8 +1107,7 @@ isp_restore_context(struct isp_device *isp, struct isp_reg *reg_list)
 static void isp_save_ctx(struct isp_device *isp)
 {
 	isp_save_context(isp, isp_reg_list);
-	if (isp->iommu)
-		omap_iommu_save_ctx(isp->iommu);
+	omap_iommu_save_ctx(isp->dev);
 }
 
 /*
@@ -1128,8 +1120,7 @@ static void isp_save_ctx(struct isp_device *isp)
 static void isp_restore_ctx(struct isp_device *isp)
 {
 	isp_restore_context(isp, isp_reg_list);
-	if (isp->iommu)
-		omap_iommu_restore_ctx(isp->iommu);
+	omap_iommu_restore_ctx(isp->dev);
 	omap3isp_ccdc_restore_context(isp);
 	omap3isp_preview_restore_context(isp);
 }
@@ -1983,7 +1974,7 @@ static int isp_remove(struct platform_device *pdev)
 	isp_cleanup_modules(isp);
 
 	omap3isp_get(isp);
-	iommu_detach_device(isp->domain, isp->iommu_dev);
+	iommu_detach_device(isp->domain, &pdev->dev);
 	iommu_domain_free(isp->domain);
 	omap3isp_put(isp);
 
@@ -2131,17 +2122,6 @@ static int isp_probe(struct platform_device *pdev)
 		}
 	}
 
-	/* IOMMU */
-	isp->iommu_dev = omap_find_iommu_device("isp");
-	if (!isp->iommu_dev) {
-		dev_err(isp->dev, "omap_find_iommu_device failed\n");
-		ret = -ENODEV;
-		goto error_isp;
-	}
-
-	/* to be removed once iommu migration is complete */
-	isp->iommu = to_iommu(isp->iommu_dev);
-
 	isp->domain = iommu_domain_alloc(pdev->dev.bus);
 	if (!isp->domain) {
 		dev_err(isp->dev, "can't alloc iommu domain\n");
@@ -2149,7 +2129,7 @@ static int isp_probe(struct platform_device *pdev)
 		goto error_isp;
 	}
 
-	ret = iommu_attach_device(isp->domain, isp->iommu_dev);
+	ret = iommu_attach_device(isp->domain, &pdev->dev);
 	if (ret) {
 		dev_err(&pdev->dev, "can't attach iommu device: %d\n", ret);
 		goto free_domain;
@@ -2188,7 +2168,7 @@ error_modules:
 error_irq:
 	free_irq(isp->irq_num, isp);
 detach_dev:
-	iommu_detach_device(isp->domain, isp->iommu_dev);
+	iommu_detach_device(isp->domain, &pdev->dev);
 free_domain:
 	iommu_domain_free(isp->domain);
 error_isp:
diff --git a/drivers/media/video/omap3isp/isp.h b/drivers/media/video/omap3isp/isp.h
index 705946ef4d60..d96603eb0d17 100644
--- a/drivers/media/video/omap3isp/isp.h
+++ b/drivers/media/video/omap3isp/isp.h
@@ -212,9 +212,7 @@ struct isp_device {
 	unsigned int sbl_resources;
 	unsigned int subclk_resources;
 
-	struct omap_iommu *iommu;
 	struct iommu_domain *domain;
-	struct device *iommu_dev;
 
 	struct isp_platform_callback platform_cb;
 };
diff --git a/drivers/media/video/omap3isp/ispccdc.c b/drivers/media/video/omap3isp/ispccdc.c
index 54a4a3f22e2e..d341ba12593f 100644
--- a/drivers/media/video/omap3isp/ispccdc.c
+++ b/drivers/media/video/omap3isp/ispccdc.c
@@ -366,7 +366,7 @@ static void ccdc_lsc_free_request(struct isp_ccdc_device *ccdc,
 		dma_unmap_sg(isp->dev, req->iovm->sgt->sgl,
 			     req->iovm->sgt->nents, DMA_TO_DEVICE);
 	if (req->table)
-		omap_iommu_vfree(isp->domain, isp->iommu, req->table);
+		omap_iommu_vfree(isp->domain, isp->dev, req->table);
 	kfree(req);
 }
 
@@ -438,7 +438,7 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
 
 		req->enable = 1;
 
-		req->table = omap_iommu_vmalloc(isp->domain, isp->iommu, 0,
+		req->table = omap_iommu_vmalloc(isp->domain, isp->dev, 0,
 					req->config.size, IOMMU_FLAG);
 		if (IS_ERR_VALUE(req->table)) {
 			req->table = 0;
@@ -446,7 +446,7 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
 			goto done;
 		}
 
-		req->iovm = omap_find_iovm_area(isp->iommu, req->table);
+		req->iovm = omap_find_iovm_area(isp->dev, req->table);
 		if (req->iovm == NULL) {
 			ret = -ENOMEM;
 			goto done;
@@ -462,7 +462,7 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
 		dma_sync_sg_for_cpu(isp->dev, req->iovm->sgt->sgl,
 				    req->iovm->sgt->nents, DMA_TO_DEVICE);
 
-		table = omap_da_to_va(isp->iommu, req->table);
+		table = omap_da_to_va(isp->dev, req->table);
 		if (copy_from_user(table, config->lsc, req->config.size)) {
 			ret = -EFAULT;
 			goto done;
@@ -734,15 +734,15 @@ static int ccdc_config(struct isp_ccdc_device *ccdc,
 			 * already done by omap_iommu_vmalloc().
 			 */
 			size = ccdc->fpc.fpnum * 4;
-			table_new = omap_iommu_vmalloc(isp->domain, isp->iommu,
+			table_new = omap_iommu_vmalloc(isp->domain, isp->dev,
 							0, size, IOMMU_FLAG);
 			if (IS_ERR_VALUE(table_new))
 				return -ENOMEM;
 
-			if (copy_from_user(omap_da_to_va(isp->iommu, table_new),
+			if (copy_from_user(omap_da_to_va(isp->dev, table_new),
 					   (__force void __user *)
 					   ccdc->fpc.fpcaddr, size)) {
-				omap_iommu_vfree(isp->domain, isp->iommu,
+				omap_iommu_vfree(isp->domain, isp->dev,
 								table_new);
 				return -EFAULT;
 			}
@@ -753,7 +753,7 @@ static int ccdc_config(struct isp_ccdc_device *ccdc,
 
 		ccdc_configure_fpc(ccdc);
 		if (table_old != 0)
-			omap_iommu_vfree(isp->domain, isp->iommu, table_old);
+			omap_iommu_vfree(isp->domain, isp->dev, table_old);
 	}
 
 	return ccdc_lsc_config(ccdc, ccdc_struct);
@@ -2309,7 +2309,7 @@ void omap3isp_ccdc_cleanup(struct isp_device *isp)
 	ccdc_lsc_free_queue(ccdc, &ccdc->lsc.free_queue);
 
 	if (ccdc->fpc.fpcaddr != 0)
-		omap_iommu_vfree(isp->domain, isp->iommu, ccdc->fpc.fpcaddr);
+		omap_iommu_vfree(isp->domain, isp->dev, ccdc->fpc.fpcaddr);
 
 	mutex_destroy(&ccdc->ioctl_lock);
 }
diff --git a/drivers/media/video/omap3isp/ispstat.c b/drivers/media/video/omap3isp/ispstat.c
index bc0b2c7349b9..11871ecc6d25 100644
--- a/drivers/media/video/omap3isp/ispstat.c
+++ b/drivers/media/video/omap3isp/ispstat.c
@@ -366,7 +366,7 @@ static void isp_stat_bufs_free(struct ispstat *stat)
 				dma_unmap_sg(isp->dev, buf->iovm->sgt->sgl,
 					     buf->iovm->sgt->nents,
 					     DMA_FROM_DEVICE);
-			omap_iommu_vfree(isp->domain, isp->iommu,
+			omap_iommu_vfree(isp->domain, isp->dev,
 							buf->iommu_addr);
 		} else {
 			if (!buf->virt_addr)
@@ -400,7 +400,7 @@ static int isp_stat_bufs_alloc_iommu(struct ispstat *stat, unsigned int size)
 		struct iovm_struct *iovm;
 
 		WARN_ON(buf->dma_addr);
-		buf->iommu_addr = omap_iommu_vmalloc(isp->domain, isp->iommu, 0,
+		buf->iommu_addr = omap_iommu_vmalloc(isp->domain, isp->dev, 0,
 							size, IOMMU_FLAG);
 		if (IS_ERR((void *)buf->iommu_addr)) {
 			dev_err(stat->isp->dev,
@@ -410,7 +410,7 @@ static int isp_stat_bufs_alloc_iommu(struct ispstat *stat, unsigned int size)
 			return -ENOMEM;
 		}
 
-		iovm = omap_find_iovm_area(isp->iommu, buf->iommu_addr);
+		iovm = omap_find_iovm_area(isp->dev, buf->iommu_addr);
 		if (!iovm ||
 		    !dma_map_sg(isp->dev, iovm->sgt->sgl, iovm->sgt->nents,
 				DMA_FROM_DEVICE)) {
@@ -419,7 +419,7 @@ static int isp_stat_bufs_alloc_iommu(struct ispstat *stat, unsigned int size)
 		}
 		buf->iovm = iovm;
 
-		buf->virt_addr = omap_da_to_va(stat->isp->iommu,
+		buf->virt_addr = omap_da_to_va(stat->isp->dev,
 					  (u32)buf->iommu_addr);
 		buf->empty = 1;
 		dev_dbg(stat->isp->dev, "%s: buffer[%d] allocated."
diff --git a/drivers/media/video/omap3isp/ispvideo.c b/drivers/media/video/omap3isp/ispvideo.c
index f2290578448c..bd3aebafafa0 100644
--- a/drivers/media/video/omap3isp/ispvideo.c
+++ b/drivers/media/video/omap3isp/ispvideo.c
@@ -453,7 +453,7 @@ ispmmu_vmap(struct isp_device *isp, const struct scatterlist *sglist, int sglen)
 	sgt->nents = sglen;
 	sgt->orig_nents = sglen;
 
-	da = omap_iommu_vmap(isp->domain, isp->iommu, 0, sgt, IOMMU_FLAG);
+	da = omap_iommu_vmap(isp->domain, isp->dev, 0, sgt, IOMMU_FLAG);
 	if (IS_ERR_VALUE(da))
 		kfree(sgt);
 
@@ -469,7 +469,7 @@ static void ispmmu_vunmap(struct isp_device *isp, dma_addr_t da)
 {
 	struct sg_table *sgt;
 
-	sgt = omap_iommu_vunmap(isp->domain, isp->iommu, (u32)da);
+	sgt = omap_iommu_vunmap(isp->domain, isp->dev, (u32)da);
 	kfree(sgt);
 }
 
diff --git a/drivers/message/fusion/lsi/mpi_cnfg.h b/drivers/message/fusion/lsi/mpi_cnfg.h
index 22027e7946f7..d9bcfba6b049 100644
--- a/drivers/message/fusion/lsi/mpi_cnfg.h
+++ b/drivers/message/fusion/lsi/mpi_cnfg.h
@@ -583,6 +583,7 @@ typedef struct _MSG_CONFIG_REPLY
 #define MPI_MANUFACTPAGE_DEVID_SAS1066E             (0x005A)
 #define MPI_MANUFACTPAGE_DEVID_SAS1068              (0x0054)
 #define MPI_MANUFACTPAGE_DEVID_SAS1068E             (0x0058)
+#define MPI_MANUFACTPAGE_DEVID_SAS1068_820XELP      (0x0059)
 #define MPI_MANUFACTPAGE_DEVID_SAS1078              (0x0062)
 
 
diff --git a/drivers/message/fusion/mptbase.c b/drivers/message/fusion/mptbase.c
index e9c6a6047a00..a7dc4672d996 100644
--- a/drivers/message/fusion/mptbase.c
+++ b/drivers/message/fusion/mptbase.c
@@ -115,7 +115,8 @@ module_param(mpt_fwfault_debug, int, 0600);
 MODULE_PARM_DESC(mpt_fwfault_debug,
 		 "Enable detection of Firmware fault and halt Firmware on fault - (default=0)");
 
-static char	MptCallbacksName[MPT_MAX_PROTOCOL_DRIVERS][50];
+static char	MptCallbacksName[MPT_MAX_PROTOCOL_DRIVERS]
+				[MPT_MAX_CALLBACKNAME_LEN+1];
 
 #ifdef MFCNT
 static int mfcounter = 0;
@@ -717,8 +718,8 @@ mpt_register(MPT_CALLBACK cbfunc, MPT_DRIVER_CLASS dclass, char *func_name)
 			MptDriverClass[cb_idx] = dclass;
 			MptEvHandlers[cb_idx] = NULL;
 			last_drv_idx = cb_idx;
-			memcpy(MptCallbacksName[cb_idx], func_name,
-			    strlen(func_name) > 50 ? 50 : strlen(func_name));
+			strlcpy(MptCallbacksName[cb_idx], func_name,
+				MPT_MAX_CALLBACKNAME_LEN+1);
 			break;
 		}
 	}
diff --git a/drivers/message/fusion/mptbase.h b/drivers/message/fusion/mptbase.h
index b4d24dc081ae..76c05bc24cb7 100644
--- a/drivers/message/fusion/mptbase.h
+++ b/drivers/message/fusion/mptbase.h
@@ -89,6 +89,7 @@
  */
 #define MPT_MAX_ADAPTERS		18
 #define MPT_MAX_PROTOCOL_DRIVERS	16
+#define MPT_MAX_CALLBACKNAME_LEN	49
 #define MPT_MAX_BUS			1	/* Do not change */
 #define MPT_MAX_FC_DEVICES		255
 #define MPT_MAX_SCSI_DEVICES		16
diff --git a/drivers/message/fusion/mptsas.c b/drivers/message/fusion/mptsas.c
index 9d9504298549..551262e4b96e 100644
--- a/drivers/message/fusion/mptsas.c
+++ b/drivers/message/fusion/mptsas.c
@@ -5376,6 +5376,8 @@ static struct pci_device_id mptsas_pci_table[] = {
 		PCI_ANY_ID, PCI_ANY_ID },
 	{ PCI_VENDOR_ID_LSI_LOGIC, MPI_MANUFACTPAGE_DEVID_SAS1078,
 		PCI_ANY_ID, PCI_ANY_ID },
+	{ PCI_VENDOR_ID_LSI_LOGIC, MPI_MANUFACTPAGE_DEVID_SAS1068_820XELP,
+		PCI_ANY_ID, PCI_ANY_ID },
 	{0}	/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(pci, mptsas_pci_table);
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index a25ab9c6b5af..af8e0efedbe4 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -2102,14 +2102,11 @@ static struct irq_chip prcmu_irq_chip = {
 void __init db8500_prcmu_early_init(void)
 {
 	unsigned int i;
-
-	if (cpu_is_u8500v1()) {
-		tcdm_base = __io_address(U8500_PRCMU_TCDM_BASE_V1);
-	} else if (cpu_is_u8500v2()) {
+	if (cpu_is_u8500v2()) {
 		void *tcpm_base = ioremap_nocache(U8500_PRCMU_TCPM_BASE, SZ_4K);
 
 		if (tcpm_base != NULL) {
-			int version;
+			u32 version;
 			version = readl(tcpm_base + PRCMU_FW_VERSION_OFFSET);
 			prcmu_version.project_number = version & 0xFF;
 			prcmu_version.api_version = (version >> 8) & 0xFF;
diff --git a/drivers/mfd/omap-usb-host.c b/drivers/mfd/omap-usb-host.c
index 86e14583a082..3f565ef3e149 100644
--- a/drivers/mfd/omap-usb-host.c
+++ b/drivers/mfd/omap-usb-host.c
@@ -27,8 +27,9 @@
 #include <linux/spinlock.h>
 #include <linux/gpio.h>
 #include <plat/usb.h>
+#include <linux/pm_runtime.h>
 
-#define USBHS_DRIVER_NAME	"usbhs-omap"
+#define USBHS_DRIVER_NAME	"usbhs_omap"
 #define OMAP_EHCI_DEVICE	"ehci-omap"
 #define OMAP_OHCI_DEVICE	"ohci-omap3"
 
@@ -147,9 +148,6 @@
 
 
 struct usbhs_hcd_omap {
-	struct clk			*usbhost_ick;
-	struct clk			*usbhost_hs_fck;
-	struct clk			*usbhost_fs_fck;
 	struct clk			*xclk60mhsp1_ck;
 	struct clk			*xclk60mhsp2_ck;
 	struct clk			*utmi_p1_fck;
@@ -159,8 +157,7 @@ struct usbhs_hcd_omap {
 	struct clk			*usbhost_p2_fck;
 	struct clk			*usbtll_p2_fck;
 	struct clk			*init_60m_fclk;
-	struct clk			*usbtll_fck;
-	struct clk			*usbtll_ick;
+	struct clk			*ehci_logic_fck;
 
 	void __iomem			*uhh_base;
 	void __iomem			*tll_base;
@@ -169,7 +166,6 @@ struct usbhs_hcd_omap {
 
 	u32				usbhs_rev;
 	spinlock_t			lock;
-	int				count;
 };
 /*-------------------------------------------------------------------------*/
 
@@ -319,269 +315,6 @@ err_end:
 	return ret;
 }
 
-/**
- * usbhs_omap_probe - initialize TI-based HCDs
- *
- * Allocates basic resources for this USB host controller.
- */
-static int __devinit usbhs_omap_probe(struct platform_device *pdev)
-{
-	struct device			*dev =  &pdev->dev;
-	struct usbhs_omap_platform_data	*pdata = dev->platform_data;
-	struct usbhs_hcd_omap		*omap;
-	struct resource			*res;
-	int				ret = 0;
-	int				i;
-
-	if (!pdata) {
-		dev_err(dev, "Missing platform data\n");
-		ret = -ENOMEM;
-		goto end_probe;
-	}
-
-	omap = kzalloc(sizeof(*omap), GFP_KERNEL);
-	if (!omap) {
-		dev_err(dev, "Memory allocation failed\n");
-		ret = -ENOMEM;
-		goto end_probe;
-	}
-
-	spin_lock_init(&omap->lock);
-
-	for (i = 0; i < OMAP3_HS_USB_PORTS; i++)
-		omap->platdata.port_mode[i] = pdata->port_mode[i];
-
-	omap->platdata.ehci_data = pdata->ehci_data;
-	omap->platdata.ohci_data = pdata->ohci_data;
-
-	omap->usbhost_ick = clk_get(dev, "usbhost_ick");
-	if (IS_ERR(omap->usbhost_ick)) {
-		ret =  PTR_ERR(omap->usbhost_ick);
-		dev_err(dev, "usbhost_ick failed error:%d\n", ret);
-		goto err_end;
-	}
-
-	omap->usbhost_hs_fck = clk_get(dev, "hs_fck");
-	if (IS_ERR(omap->usbhost_hs_fck)) {
-		ret = PTR_ERR(omap->usbhost_hs_fck);
-		dev_err(dev, "usbhost_hs_fck failed error:%d\n", ret);
-		goto err_usbhost_ick;
-	}
-
-	omap->usbhost_fs_fck = clk_get(dev, "fs_fck");
-	if (IS_ERR(omap->usbhost_fs_fck)) {
-		ret = PTR_ERR(omap->usbhost_fs_fck);
-		dev_err(dev, "usbhost_fs_fck failed error:%d\n", ret);
-		goto err_usbhost_hs_fck;
-	}
-
-	omap->usbtll_fck = clk_get(dev, "usbtll_fck");
-	if (IS_ERR(omap->usbtll_fck)) {
-		ret = PTR_ERR(omap->usbtll_fck);
-		dev_err(dev, "usbtll_fck failed error:%d\n", ret);
-		goto err_usbhost_fs_fck;
-	}
-
-	omap->usbtll_ick = clk_get(dev, "usbtll_ick");
-	if (IS_ERR(omap->usbtll_ick)) {
-		ret = PTR_ERR(omap->usbtll_ick);
-		dev_err(dev, "usbtll_ick failed error:%d\n", ret);
-		goto err_usbtll_fck;
-	}
-
-	omap->utmi_p1_fck = clk_get(dev, "utmi_p1_gfclk");
-	if (IS_ERR(omap->utmi_p1_fck)) {
-		ret = PTR_ERR(omap->utmi_p1_fck);
-		dev_err(dev, "utmi_p1_gfclk failed error:%d\n",	ret);
-		goto err_usbtll_ick;
-	}
-
-	omap->xclk60mhsp1_ck = clk_get(dev, "xclk60mhsp1_ck");
-	if (IS_ERR(omap->xclk60mhsp1_ck)) {
-		ret = PTR_ERR(omap->xclk60mhsp1_ck);
-		dev_err(dev, "xclk60mhsp1_ck failed error:%d\n", ret);
-		goto err_utmi_p1_fck;
-	}
-
-	omap->utmi_p2_fck = clk_get(dev, "utmi_p2_gfclk");
-	if (IS_ERR(omap->utmi_p2_fck)) {
-		ret = PTR_ERR(omap->utmi_p2_fck);
-		dev_err(dev, "utmi_p2_gfclk failed error:%d\n", ret);
-		goto err_xclk60mhsp1_ck;
-	}
-
-	omap->xclk60mhsp2_ck = clk_get(dev, "xclk60mhsp2_ck");
-	if (IS_ERR(omap->xclk60mhsp2_ck)) {
-		ret = PTR_ERR(omap->xclk60mhsp2_ck);
-		dev_err(dev, "xclk60mhsp2_ck failed error:%d\n", ret);
-		goto err_utmi_p2_fck;
-	}
-
-	omap->usbhost_p1_fck = clk_get(dev, "usb_host_hs_utmi_p1_clk");
-	if (IS_ERR(omap->usbhost_p1_fck)) {
-		ret = PTR_ERR(omap->usbhost_p1_fck);
-		dev_err(dev, "usbhost_p1_fck failed error:%d\n", ret);
-		goto err_xclk60mhsp2_ck;
-	}
-
-	omap->usbtll_p1_fck = clk_get(dev, "usb_tll_hs_usb_ch0_clk");
-	if (IS_ERR(omap->usbtll_p1_fck)) {
-		ret = PTR_ERR(omap->usbtll_p1_fck);
-		dev_err(dev, "usbtll_p1_fck failed error:%d\n", ret);
-		goto err_usbhost_p1_fck;
-	}
-
-	omap->usbhost_p2_fck = clk_get(dev, "usb_host_hs_utmi_p2_clk");
-	if (IS_ERR(omap->usbhost_p2_fck)) {
-		ret = PTR_ERR(omap->usbhost_p2_fck);
-		dev_err(dev, "usbhost_p2_fck failed error:%d\n", ret);
-		goto err_usbtll_p1_fck;
-	}
-
-	omap->usbtll_p2_fck = clk_get(dev, "usb_tll_hs_usb_ch1_clk");
-	if (IS_ERR(omap->usbtll_p2_fck)) {
-		ret = PTR_ERR(omap->usbtll_p2_fck);
-		dev_err(dev, "usbtll_p2_fck failed error:%d\n", ret);
-		goto err_usbhost_p2_fck;
-	}
-
-	omap->init_60m_fclk = clk_get(dev, "init_60m_fclk");
-	if (IS_ERR(omap->init_60m_fclk)) {
-		ret = PTR_ERR(omap->init_60m_fclk);
-		dev_err(dev, "init_60m_fclk failed error:%d\n", ret);
-		goto err_usbtll_p2_fck;
-	}
-
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "uhh");
-	if (!res) {
-		dev_err(dev, "UHH EHCI get resource failed\n");
-		ret = -ENODEV;
-		goto err_init_60m_fclk;
-	}
-
-	omap->uhh_base = ioremap(res->start, resource_size(res));
-	if (!omap->uhh_base) {
-		dev_err(dev, "UHH ioremap failed\n");
-		ret = -ENOMEM;
-		goto err_init_60m_fclk;
-	}
-
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "tll");
-	if (!res) {
-		dev_err(dev, "UHH EHCI get resource failed\n");
-		ret = -ENODEV;
-		goto err_tll;
-	}
-
-	omap->tll_base = ioremap(res->start, resource_size(res));
-	if (!omap->tll_base) {
-		dev_err(dev, "TLL ioremap failed\n");
-		ret = -ENOMEM;
-		goto err_tll;
-	}
-
-	platform_set_drvdata(pdev, omap);
-
-	ret = omap_usbhs_alloc_children(pdev);
-	if (ret) {
-		dev_err(dev, "omap_usbhs_alloc_children failed\n");
-		goto err_alloc;
-	}
-
-	goto end_probe;
-
-err_alloc:
-	iounmap(omap->tll_base);
-
-err_tll:
-	iounmap(omap->uhh_base);
-
-err_init_60m_fclk:
-	clk_put(omap->init_60m_fclk);
-
-err_usbtll_p2_fck:
-	clk_put(omap->usbtll_p2_fck);
-
-err_usbhost_p2_fck:
-	clk_put(omap->usbhost_p2_fck);
-
-err_usbtll_p1_fck:
-	clk_put(omap->usbtll_p1_fck);
-
-err_usbhost_p1_fck:
-	clk_put(omap->usbhost_p1_fck);
-
-err_xclk60mhsp2_ck:
-	clk_put(omap->xclk60mhsp2_ck);
-
-err_utmi_p2_fck:
-	clk_put(omap->utmi_p2_fck);
-
-err_xclk60mhsp1_ck:
-	clk_put(omap->xclk60mhsp1_ck);
-
-err_utmi_p1_fck:
-	clk_put(omap->utmi_p1_fck);
-
-err_usbtll_ick:
-	clk_put(omap->usbtll_ick);
-
-err_usbtll_fck:
-	clk_put(omap->usbtll_fck);
-
-err_usbhost_fs_fck:
-	clk_put(omap->usbhost_fs_fck);
-
-err_usbhost_hs_fck:
-	clk_put(omap->usbhost_hs_fck);
-
-err_usbhost_ick:
-	clk_put(omap->usbhost_ick);
-
-err_end:
-	kfree(omap);
-
-end_probe:
-	return ret;
-}
-
-/**
- * usbhs_omap_remove - shutdown processing for UHH & TLL HCDs
- * @pdev: USB Host Controller being removed
- *
- * Reverses the effect of usbhs_omap_probe().
- */
-static int __devexit usbhs_omap_remove(struct platform_device *pdev)
-{
-	struct usbhs_hcd_omap *omap = platform_get_drvdata(pdev);
-
-	if (omap->count != 0) {
-		dev_err(&pdev->dev,
-			"Either EHCI or OHCI is still using usbhs core\n");
-		return -EBUSY;
-	}
-
-	iounmap(omap->tll_base);
-	iounmap(omap->uhh_base);
-	clk_put(omap->init_60m_fclk);
-	clk_put(omap->usbtll_p2_fck);
-	clk_put(omap->usbhost_p2_fck);
-	clk_put(omap->usbtll_p1_fck);
-	clk_put(omap->usbhost_p1_fck);
-	clk_put(omap->xclk60mhsp2_ck);
-	clk_put(omap->utmi_p2_fck);
-	clk_put(omap->xclk60mhsp1_ck);
-	clk_put(omap->utmi_p1_fck);
-	clk_put(omap->usbtll_ick);
-	clk_put(omap->usbtll_fck);
-	clk_put(omap->usbhost_fs_fck);
-	clk_put(omap->usbhost_hs_fck);
-	clk_put(omap->usbhost_ick);
-	kfree(omap);
-
-	return 0;
-}
-
 static bool is_ohci_port(enum usbhs_omap_port_mode pmode)
 {
 	switch (pmode) {
@@ -689,30 +422,85 @@ static void usbhs_omap_tll_init(struct device *dev, u8 tll_channel_count)
 	}
 }
 
-static int usbhs_enable(struct device *dev)
+static int usbhs_runtime_resume(struct device *dev)
 {
 	struct usbhs_hcd_omap		*omap = dev_get_drvdata(dev);
 	struct usbhs_omap_platform_data	*pdata = &omap->platdata;
-	unsigned long			flags = 0;
-	int				ret = 0;
-	unsigned long			timeout;
-	unsigned			reg;
+	unsigned long			flags;
+
+	dev_dbg(dev, "usbhs_runtime_resume\n");
 
-	dev_dbg(dev, "starting TI HSUSB Controller\n");
 	if (!pdata) {
 		dev_dbg(dev, "missing platform_data\n");
 		return  -ENODEV;
 	}
 
 	spin_lock_irqsave(&omap->lock, flags);
-	if (omap->count > 0)
-		goto end_count;
 
-	clk_enable(omap->usbhost_ick);
-	clk_enable(omap->usbhost_hs_fck);
-	clk_enable(omap->usbhost_fs_fck);
-	clk_enable(omap->usbtll_fck);
-	clk_enable(omap->usbtll_ick);
+	if (omap->ehci_logic_fck && !IS_ERR(omap->ehci_logic_fck))
+		clk_enable(omap->ehci_logic_fck);
+
+	if (is_ehci_tll_mode(pdata->port_mode[0])) {
+		clk_enable(omap->usbhost_p1_fck);
+		clk_enable(omap->usbtll_p1_fck);
+	}
+	if (is_ehci_tll_mode(pdata->port_mode[1])) {
+		clk_enable(omap->usbhost_p2_fck);
+		clk_enable(omap->usbtll_p2_fck);
+	}
+	clk_enable(omap->utmi_p1_fck);
+	clk_enable(omap->utmi_p2_fck);
+
+	spin_unlock_irqrestore(&omap->lock, flags);
+
+	return 0;
+}
+
+static int usbhs_runtime_suspend(struct device *dev)
+{
+	struct usbhs_hcd_omap		*omap = dev_get_drvdata(dev);
+	struct usbhs_omap_platform_data	*pdata = &omap->platdata;
+	unsigned long			flags;
+
+	dev_dbg(dev, "usbhs_runtime_suspend\n");
+
+	if (!pdata) {
+		dev_dbg(dev, "missing platform_data\n");
+		return  -ENODEV;
+	}
+
+	spin_lock_irqsave(&omap->lock, flags);
+
+	if (is_ehci_tll_mode(pdata->port_mode[0])) {
+		clk_disable(omap->usbhost_p1_fck);
+		clk_disable(omap->usbtll_p1_fck);
+	}
+	if (is_ehci_tll_mode(pdata->port_mode[1])) {
+		clk_disable(omap->usbhost_p2_fck);
+		clk_disable(omap->usbtll_p2_fck);
+	}
+	clk_disable(omap->utmi_p2_fck);
+	clk_disable(omap->utmi_p1_fck);
+
+	if (omap->ehci_logic_fck && !IS_ERR(omap->ehci_logic_fck))
+		clk_disable(omap->ehci_logic_fck);
+
+	spin_unlock_irqrestore(&omap->lock, flags);
+
+	return 0;
+}
+
+static void omap_usbhs_init(struct device *dev)
+{
+	struct usbhs_hcd_omap		*omap = dev_get_drvdata(dev);
+	struct usbhs_omap_platform_data	*pdata = &omap->platdata;
+	unsigned long			flags;
+	unsigned			reg;
+
+	dev_dbg(dev, "starting TI HSUSB Controller\n");
+
+	pm_runtime_get_sync(dev);
+	spin_lock_irqsave(&omap->lock, flags);
 
 	if (pdata->ehci_data->phy_reset) {
 		if (gpio_is_valid(pdata->ehci_data->reset_gpio_port[0])) {
@@ -736,50 +524,6 @@ static int usbhs_enable(struct device *dev)
 	omap->usbhs_rev = usbhs_read(omap->uhh_base, OMAP_UHH_REVISION);
 	dev_dbg(dev, "OMAP UHH_REVISION 0x%x\n", omap->usbhs_rev);
 
-	/* perform TLL soft reset, and wait until reset is complete */
-	usbhs_write(omap->tll_base, OMAP_USBTLL_SYSCONFIG,
-			OMAP_USBTLL_SYSCONFIG_SOFTRESET);
-
-	/* Wait for TLL reset to complete */
-	timeout = jiffies + msecs_to_jiffies(1000);
-	while (!(usbhs_read(omap->tll_base, OMAP_USBTLL_SYSSTATUS)
-			& OMAP_USBTLL_SYSSTATUS_RESETDONE)) {
-		cpu_relax();
-
-		if (time_after(jiffies, timeout)) {
-			dev_dbg(dev, "operation timed out\n");
-			ret = -EINVAL;
-			goto err_tll;
-		}
-	}
-
-	dev_dbg(dev, "TLL RESET DONE\n");
-
-	/* (1<<3) = no idle mode only for initial debugging */
-	usbhs_write(omap->tll_base, OMAP_USBTLL_SYSCONFIG,
-			OMAP_USBTLL_SYSCONFIG_ENAWAKEUP |
-			OMAP_USBTLL_SYSCONFIG_SIDLEMODE |
-			OMAP_USBTLL_SYSCONFIG_AUTOIDLE);
-
-	/* Put UHH in NoIdle/NoStandby mode */
-	reg = usbhs_read(omap->uhh_base, OMAP_UHH_SYSCONFIG);
-	if (is_omap_usbhs_rev1(omap)) {
-		reg |= (OMAP_UHH_SYSCONFIG_ENAWAKEUP
-				| OMAP_UHH_SYSCONFIG_SIDLEMODE
-				| OMAP_UHH_SYSCONFIG_CACTIVITY
-				| OMAP_UHH_SYSCONFIG_MIDLEMODE);
-		reg &= ~OMAP_UHH_SYSCONFIG_AUTOIDLE;
-
-
-	} else if (is_omap_usbhs_rev2(omap)) {
-		reg &= ~OMAP4_UHH_SYSCONFIG_IDLEMODE_CLEAR;
-		reg |= OMAP4_UHH_SYSCONFIG_NOIDLE;
-		reg &= ~OMAP4_UHH_SYSCONFIG_STDBYMODE_CLEAR;
-		reg |= OMAP4_UHH_SYSCONFIG_NOSTDBY;
-	}
-
-	usbhs_write(omap->uhh_base, OMAP_UHH_SYSCONFIG, reg);
-
 	reg = usbhs_read(omap->uhh_base, OMAP_UHH_HOSTCONFIG);
 	/* setup ULPI bypass and burst configurations */
 	reg |= (OMAP_UHH_HOSTCONFIG_INCR4_BURST_EN
@@ -825,49 +569,6 @@ static int usbhs_enable(struct device *dev)
 		reg &= ~OMAP4_P1_MODE_CLEAR;
 		reg &= ~OMAP4_P2_MODE_CLEAR;
 
-		if (is_ehci_phy_mode(pdata->port_mode[0])) {
-			ret = clk_set_parent(omap->utmi_p1_fck,
-						omap->xclk60mhsp1_ck);
-			if (ret != 0) {
-				dev_err(dev, "xclk60mhsp1_ck set parent"
-				"failed error:%d\n", ret);
-				goto err_tll;
-			}
-		} else if (is_ehci_tll_mode(pdata->port_mode[0])) {
-			ret = clk_set_parent(omap->utmi_p1_fck,
-						omap->init_60m_fclk);
-			if (ret != 0) {
-				dev_err(dev, "init_60m_fclk set parent"
-				"failed error:%d\n", ret);
-				goto err_tll;
-			}
-			clk_enable(omap->usbhost_p1_fck);
-			clk_enable(omap->usbtll_p1_fck);
-		}
-
-		if (is_ehci_phy_mode(pdata->port_mode[1])) {
-			ret = clk_set_parent(omap->utmi_p2_fck,
-						omap->xclk60mhsp2_ck);
-			if (ret != 0) {
-				dev_err(dev, "xclk60mhsp1_ck set parent"
-					"failed error:%d\n", ret);
-				goto err_tll;
-			}
-		} else if (is_ehci_tll_mode(pdata->port_mode[1])) {
-			ret = clk_set_parent(omap->utmi_p2_fck,
-						omap->init_60m_fclk);
-			if (ret != 0) {
-				dev_err(dev, "init_60m_fclk set parent"
-				"failed error:%d\n", ret);
-				goto err_tll;
-			}
-			clk_enable(omap->usbhost_p2_fck);
-			clk_enable(omap->usbtll_p2_fck);
-		}
-
-		clk_enable(omap->utmi_p1_fck);
-		clk_enable(omap->utmi_p2_fck);
-
 		if (is_ehci_tll_mode(pdata->port_mode[0]) ||
 			(is_ohci_port(pdata->port_mode[0])))
 			reg |= OMAP4_P1_MODE_TLL;
@@ -913,12 +614,15 @@ static int usbhs_enable(struct device *dev)
 				(pdata->ehci_data->reset_gpio_port[1], 1);
 	}
 
-end_count:
-	omap->count++;
 	spin_unlock_irqrestore(&omap->lock, flags);
-	return 0;
+	pm_runtime_put_sync(dev);
+}
+
+static void omap_usbhs_deinit(struct device *dev)
+{
+	struct usbhs_hcd_omap		*omap = dev_get_drvdata(dev);
+	struct usbhs_omap_platform_data	*pdata = &omap->platdata;
 
-err_tll:
 	if (pdata->ehci_data->phy_reset) {
 		if (gpio_is_valid(pdata->ehci_data->reset_gpio_port[0]))
 			gpio_free(pdata->ehci_data->reset_gpio_port[0]);
@@ -926,123 +630,272 @@ err_tll:
 		if (gpio_is_valid(pdata->ehci_data->reset_gpio_port[1]))
 			gpio_free(pdata->ehci_data->reset_gpio_port[1]);
 	}
-
-	clk_disable(omap->usbtll_ick);
-	clk_disable(omap->usbtll_fck);
-	clk_disable(omap->usbhost_fs_fck);
-	clk_disable(omap->usbhost_hs_fck);
-	clk_disable(omap->usbhost_ick);
-	spin_unlock_irqrestore(&omap->lock, flags);
-	return ret;
 }
 
-static void usbhs_disable(struct device *dev)
+
+/**
+ * usbhs_omap_probe - initialize TI-based HCDs
+ *
+ * Allocates basic resources for this USB host controller.
+ */
+static int __devinit usbhs_omap_probe(struct platform_device *pdev)
 {
-	struct usbhs_hcd_omap		*omap = dev_get_drvdata(dev);
-	struct usbhs_omap_platform_data	*pdata = &omap->platdata;
-	unsigned long			flags = 0;
-	unsigned long			timeout;
+	struct device			*dev =  &pdev->dev;
+	struct usbhs_omap_platform_data	*pdata = dev->platform_data;
+	struct usbhs_hcd_omap		*omap;
+	struct resource			*res;
+	int				ret = 0;
+	int				i;
 
-	dev_dbg(dev, "stopping TI HSUSB Controller\n");
+	if (!pdata) {
+		dev_err(dev, "Missing platform data\n");
+		ret = -ENOMEM;
+		goto end_probe;
+	}
 
-	spin_lock_irqsave(&omap->lock, flags);
+	omap = kzalloc(sizeof(*omap), GFP_KERNEL);
+	if (!omap) {
+		dev_err(dev, "Memory allocation failed\n");
+		ret = -ENOMEM;
+		goto end_probe;
+	}
 
-	if (omap->count == 0)
-		goto end_disble;
+	spin_lock_init(&omap->lock);
 
-	omap->count--;
+	for (i = 0; i < OMAP3_HS_USB_PORTS; i++)
+		omap->platdata.port_mode[i] = pdata->port_mode[i];
+
+	omap->platdata.ehci_data = pdata->ehci_data;
+	omap->platdata.ohci_data = pdata->ohci_data;
 
-	if (omap->count != 0)
-		goto end_disble;
+	pm_runtime_enable(dev);
 
-	/* Reset OMAP modules for insmod/rmmod to work */
-	usbhs_write(omap->uhh_base, OMAP_UHH_SYSCONFIG,
-			is_omap_usbhs_rev2(omap) ?
-			OMAP4_UHH_SYSCONFIG_SOFTRESET :
-			OMAP_UHH_SYSCONFIG_SOFTRESET);
 
-	timeout = jiffies + msecs_to_jiffies(100);
-	while (!(usbhs_read(omap->uhh_base, OMAP_UHH_SYSSTATUS)
-				& (1 << 0))) {
-		cpu_relax();
+	for (i = 0; i < OMAP3_HS_USB_PORTS; i++)
+		if (is_ehci_phy_mode(i) || is_ehci_tll_mode(i) ||
+			is_ehci_hsic_mode(i)) {
+			omap->ehci_logic_fck = clk_get(dev, "ehci_logic_fck");
+			if (IS_ERR(omap->ehci_logic_fck)) {
+				ret = PTR_ERR(omap->ehci_logic_fck);
+				dev_warn(dev, "ehci_logic_fck failed:%d\n",
+					 ret);
+			}
+			break;
+		}
 
-		if (time_after(jiffies, timeout))
-			dev_dbg(dev, "operation timed out\n");
+	omap->utmi_p1_fck = clk_get(dev, "utmi_p1_gfclk");
+	if (IS_ERR(omap->utmi_p1_fck)) {
+		ret = PTR_ERR(omap->utmi_p1_fck);
+		dev_err(dev, "utmi_p1_gfclk failed error:%d\n",	ret);
+		goto err_end;
 	}
 
-	while (!(usbhs_read(omap->uhh_base, OMAP_UHH_SYSSTATUS)
-				& (1 << 1))) {
-		cpu_relax();
+	omap->xclk60mhsp1_ck = clk_get(dev, "xclk60mhsp1_ck");
+	if (IS_ERR(omap->xclk60mhsp1_ck)) {
+		ret = PTR_ERR(omap->xclk60mhsp1_ck);
+		dev_err(dev, "xclk60mhsp1_ck failed error:%d\n", ret);
+		goto err_utmi_p1_fck;
+	}
 
-		if (time_after(jiffies, timeout))
-			dev_dbg(dev, "operation timed out\n");
+	omap->utmi_p2_fck = clk_get(dev, "utmi_p2_gfclk");
+	if (IS_ERR(omap->utmi_p2_fck)) {
+		ret = PTR_ERR(omap->utmi_p2_fck);
+		dev_err(dev, "utmi_p2_gfclk failed error:%d\n", ret);
+		goto err_xclk60mhsp1_ck;
 	}
 
-	while (!(usbhs_read(omap->uhh_base, OMAP_UHH_SYSSTATUS)
-				& (1 << 2))) {
-		cpu_relax();
+	omap->xclk60mhsp2_ck = clk_get(dev, "xclk60mhsp2_ck");
+	if (IS_ERR(omap->xclk60mhsp2_ck)) {
+		ret = PTR_ERR(omap->xclk60mhsp2_ck);
+		dev_err(dev, "xclk60mhsp2_ck failed error:%d\n", ret);
+		goto err_utmi_p2_fck;
+	}
 
-		if (time_after(jiffies, timeout))
-			dev_dbg(dev, "operation timed out\n");
+	omap->usbhost_p1_fck = clk_get(dev, "usb_host_hs_utmi_p1_clk");
+	if (IS_ERR(omap->usbhost_p1_fck)) {
+		ret = PTR_ERR(omap->usbhost_p1_fck);
+		dev_err(dev, "usbhost_p1_fck failed error:%d\n", ret);
+		goto err_xclk60mhsp2_ck;
 	}
 
-	usbhs_write(omap->tll_base, OMAP_USBTLL_SYSCONFIG, (1 << 1));
+	omap->usbtll_p1_fck = clk_get(dev, "usb_tll_hs_usb_ch0_clk");
+	if (IS_ERR(omap->usbtll_p1_fck)) {
+		ret = PTR_ERR(omap->usbtll_p1_fck);
+		dev_err(dev, "usbtll_p1_fck failed error:%d\n", ret);
+		goto err_usbhost_p1_fck;
+	}
 
-	while (!(usbhs_read(omap->tll_base, OMAP_USBTLL_SYSSTATUS)
-				& (1 << 0))) {
-		cpu_relax();
+	omap->usbhost_p2_fck = clk_get(dev, "usb_host_hs_utmi_p2_clk");
+	if (IS_ERR(omap->usbhost_p2_fck)) {
+		ret = PTR_ERR(omap->usbhost_p2_fck);
+		dev_err(dev, "usbhost_p2_fck failed error:%d\n", ret);
+		goto err_usbtll_p1_fck;
+	}
 
-		if (time_after(jiffies, timeout))
-			dev_dbg(dev, "operation timed out\n");
+	omap->usbtll_p2_fck = clk_get(dev, "usb_tll_hs_usb_ch1_clk");
+	if (IS_ERR(omap->usbtll_p2_fck)) {
+		ret = PTR_ERR(omap->usbtll_p2_fck);
+		dev_err(dev, "usbtll_p2_fck failed error:%d\n", ret);
+		goto err_usbhost_p2_fck;
 	}
 
-	if (is_omap_usbhs_rev2(omap)) {
-		if (is_ehci_tll_mode(pdata->port_mode[0]))
-			clk_disable(omap->usbtll_p1_fck);
-		if (is_ehci_tll_mode(pdata->port_mode[1]))
-			clk_disable(omap->usbtll_p2_fck);
-		clk_disable(omap->utmi_p2_fck);
-		clk_disable(omap->utmi_p1_fck);
+	omap->init_60m_fclk = clk_get(dev, "init_60m_fclk");
+	if (IS_ERR(omap->init_60m_fclk)) {
+		ret = PTR_ERR(omap->init_60m_fclk);
+		dev_err(dev, "init_60m_fclk failed error:%d\n", ret);
+		goto err_usbtll_p2_fck;
 	}
 
-	clk_disable(omap->usbtll_ick);
-	clk_disable(omap->usbtll_fck);
-	clk_disable(omap->usbhost_fs_fck);
-	clk_disable(omap->usbhost_hs_fck);
-	clk_disable(omap->usbhost_ick);
+	if (is_ehci_phy_mode(pdata->port_mode[0])) {
+		/* for OMAP3 , the clk set paretn fails */
+		ret = clk_set_parent(omap->utmi_p1_fck,
+					omap->xclk60mhsp1_ck);
+		if (ret != 0)
+			dev_err(dev, "xclk60mhsp1_ck set parent"
+				"failed error:%d\n", ret);
+	} else if (is_ehci_tll_mode(pdata->port_mode[0])) {
+		ret = clk_set_parent(omap->utmi_p1_fck,
+					omap->init_60m_fclk);
+		if (ret != 0)
+			dev_err(dev, "init_60m_fclk set parent"
+				"failed error:%d\n", ret);
+	}
 
-	/* The gpio_free migh sleep; so unlock the spinlock */
-	spin_unlock_irqrestore(&omap->lock, flags);
+	if (is_ehci_phy_mode(pdata->port_mode[1])) {
+		ret = clk_set_parent(omap->utmi_p2_fck,
+					omap->xclk60mhsp2_ck);
+		if (ret != 0)
+			dev_err(dev, "xclk60mhsp2_ck set parent"
+					"failed error:%d\n", ret);
+	} else if (is_ehci_tll_mode(pdata->port_mode[1])) {
+		ret = clk_set_parent(omap->utmi_p2_fck,
+						omap->init_60m_fclk);
+		if (ret != 0)
+			dev_err(dev, "init_60m_fclk set parent"
+				"failed error:%d\n", ret);
+	}
 
-	if (pdata->ehci_data->phy_reset) {
-		if (gpio_is_valid(pdata->ehci_data->reset_gpio_port[0]))
-			gpio_free(pdata->ehci_data->reset_gpio_port[0]);
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "uhh");
+	if (!res) {
+		dev_err(dev, "UHH EHCI get resource failed\n");
+		ret = -ENODEV;
+		goto err_init_60m_fclk;
+	}
 
-		if (gpio_is_valid(pdata->ehci_data->reset_gpio_port[1]))
-			gpio_free(pdata->ehci_data->reset_gpio_port[1]);
+	omap->uhh_base = ioremap(res->start, resource_size(res));
+	if (!omap->uhh_base) {
+		dev_err(dev, "UHH ioremap failed\n");
+		ret = -ENOMEM;
+		goto err_init_60m_fclk;
 	}
-	return;
 
-end_disble:
-	spin_unlock_irqrestore(&omap->lock, flags);
-}
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "tll");
+	if (!res) {
+		dev_err(dev, "UHH EHCI get resource failed\n");
+		ret = -ENODEV;
+		goto err_tll;
+	}
 
-int omap_usbhs_enable(struct device *dev)
-{
-	return  usbhs_enable(dev->parent);
+	omap->tll_base = ioremap(res->start, resource_size(res));
+	if (!omap->tll_base) {
+		dev_err(dev, "TLL ioremap failed\n");
+		ret = -ENOMEM;
+		goto err_tll;
+	}
+
+	platform_set_drvdata(pdev, omap);
+
+	ret = omap_usbhs_alloc_children(pdev);
+	if (ret) {
+		dev_err(dev, "omap_usbhs_alloc_children failed\n");
+		goto err_alloc;
+	}
+
+	omap_usbhs_init(dev);
+
+	goto end_probe;
+
+err_alloc:
+	iounmap(omap->tll_base);
+
+err_tll:
+	iounmap(omap->uhh_base);
+
+err_init_60m_fclk:
+	clk_put(omap->init_60m_fclk);
+
+err_usbtll_p2_fck:
+	clk_put(omap->usbtll_p2_fck);
+
+err_usbhost_p2_fck:
+	clk_put(omap->usbhost_p2_fck);
+
+err_usbtll_p1_fck:
+	clk_put(omap->usbtll_p1_fck);
+
+err_usbhost_p1_fck:
+	clk_put(omap->usbhost_p1_fck);
+
+err_xclk60mhsp2_ck:
+	clk_put(omap->xclk60mhsp2_ck);
+
+err_utmi_p2_fck:
+	clk_put(omap->utmi_p2_fck);
+
+err_xclk60mhsp1_ck:
+	clk_put(omap->xclk60mhsp1_ck);
+
+err_utmi_p1_fck:
+	clk_put(omap->utmi_p1_fck);
+
+err_end:
+	clk_put(omap->ehci_logic_fck);
+	pm_runtime_disable(dev);
+	kfree(omap);
+
+end_probe:
+	return ret;
 }
-EXPORT_SYMBOL_GPL(omap_usbhs_enable);
 
-void omap_usbhs_disable(struct device *dev)
+/**
+ * usbhs_omap_remove - shutdown processing for UHH & TLL HCDs
+ * @pdev: USB Host Controller being removed
+ *
+ * Reverses the effect of usbhs_omap_probe().
+ */
+static int __devexit usbhs_omap_remove(struct platform_device *pdev)
 {
-	usbhs_disable(dev->parent);
+	struct usbhs_hcd_omap *omap = platform_get_drvdata(pdev);
+
+	omap_usbhs_deinit(&pdev->dev);
+	iounmap(omap->tll_base);
+	iounmap(omap->uhh_base);
+	clk_put(omap->init_60m_fclk);
+	clk_put(omap->usbtll_p2_fck);
+	clk_put(omap->usbhost_p2_fck);
+	clk_put(omap->usbtll_p1_fck);
+	clk_put(omap->usbhost_p1_fck);
+	clk_put(omap->xclk60mhsp2_ck);
+	clk_put(omap->utmi_p2_fck);
+	clk_put(omap->xclk60mhsp1_ck);
+	clk_put(omap->utmi_p1_fck);
+	clk_put(omap->ehci_logic_fck);
+	pm_runtime_disable(&pdev->dev);
+	kfree(omap);
+
+	return 0;
 }
-EXPORT_SYMBOL_GPL(omap_usbhs_disable);
+
+static const struct dev_pm_ops usbhsomap_dev_pm_ops = {
+	.runtime_suspend	= usbhs_runtime_suspend,
+	.runtime_resume		= usbhs_runtime_resume,
+};
 
 static struct platform_driver usbhs_omap_driver = {
 	.driver = {
 		.name		= (char *)usbhs_driver_name,
 		.owner		= THIS_MODULE,
+		.pm		= &usbhsomap_dev_pm_ops,
 	},
 	.remove		= __exit_p(usbhs_omap_remove),
 };
diff --git a/drivers/misc/ad525x_dpot-i2c.c b/drivers/misc/ad525x_dpot-i2c.c
index a39e0555df63..83adab69bfd4 100644
--- a/drivers/misc/ad525x_dpot-i2c.c
+++ b/drivers/misc/ad525x_dpot-i2c.c
@@ -1,7 +1,7 @@
 /*
  * Driver for the Analog Devices digital potentiometers (I2C bus)
  *
- * Copyright (C) 2010 Michael Hennerich, Analog Devices Inc.
+ * Copyright (C) 2010-2011 Michael Hennerich, Analog Devices Inc.
  *
  * Licensed under the GPL-2 or later.
  */
@@ -11,7 +11,6 @@
 
 #include "ad525x_dpot.h"
 
-/* ------------------------------------------------------------------------- */
 /* I2C bus functions */
 static int write_d8(void *client, u8 val)
 {
@@ -60,18 +59,13 @@ static int __devinit ad_dpot_i2c_probe(struct i2c_client *client,
 		.bops = &bops,
 	};
 
-	struct ad_dpot_id dpot_id = {
-		.name = (char *) &id->name,
-		.devid = id->driver_data,
-	};
-
 	if (!i2c_check_functionality(client->adapter,
 				     I2C_FUNC_SMBUS_WORD_DATA)) {
 		dev_err(&client->dev, "SMBUS Word Data not Supported\n");
 		return -EIO;
 	}
 
-	return ad_dpot_probe(&client->dev, &bdata, &dpot_id);
+	return ad_dpot_probe(&client->dev, &bdata, id->driver_data, id->name);
 }
 
 static int __devexit ad_dpot_i2c_remove(struct i2c_client *client)
diff --git a/drivers/misc/ad525x_dpot-spi.c b/drivers/misc/ad525x_dpot-spi.c
index 7f9a55afe05d..822749e41fea 100644
--- a/drivers/misc/ad525x_dpot-spi.c
+++ b/drivers/misc/ad525x_dpot-spi.c
@@ -1,7 +1,7 @@
 /*
  * Driver for the Analog Devices digital potentiometers (SPI bus)
  *
- * Copyright (C) 2010 Michael Hennerich, Analog Devices Inc.
+ * Copyright (C) 2010-2011 Michael Hennerich, Analog Devices Inc.
  *
  * Licensed under the GPL-2 or later.
  */
@@ -11,40 +11,6 @@
 
 #include "ad525x_dpot.h"
 
-static const struct ad_dpot_id ad_dpot_spi_devlist[] = {
-	{.name = "ad5160", .devid = AD5160_ID},
-	{.name = "ad5161", .devid = AD5161_ID},
-	{.name = "ad5162", .devid = AD5162_ID},
-	{.name = "ad5165", .devid = AD5165_ID},
-	{.name = "ad5200", .devid = AD5200_ID},
-	{.name = "ad5201", .devid = AD5201_ID},
-	{.name = "ad5203", .devid = AD5203_ID},
-	{.name = "ad5204", .devid = AD5204_ID},
-	{.name = "ad5206", .devid = AD5206_ID},
-	{.name = "ad5207", .devid = AD5207_ID},
-	{.name = "ad5231", .devid = AD5231_ID},
-	{.name = "ad5232", .devid = AD5232_ID},
-	{.name = "ad5233", .devid = AD5233_ID},
-	{.name = "ad5235", .devid = AD5235_ID},
-	{.name = "ad5260", .devid = AD5260_ID},
-	{.name = "ad5262", .devid = AD5262_ID},
-	{.name = "ad5263", .devid = AD5263_ID},
-	{.name = "ad5290", .devid = AD5290_ID},
-	{.name = "ad5291", .devid = AD5291_ID},
-	{.name = "ad5292", .devid = AD5292_ID},
-	{.name = "ad5293", .devid = AD5293_ID},
-	{.name = "ad7376", .devid = AD7376_ID},
-	{.name = "ad8400", .devid = AD8400_ID},
-	{.name = "ad8402", .devid = AD8402_ID},
-	{.name = "ad8403", .devid = AD8403_ID},
-	{.name = "adn2850", .devid = ADN2850_ID},
-	{.name = "ad5270", .devid = AD5270_ID},
-	{.name = "ad5271", .devid = AD5271_ID},
-	{}
-};
-
-/* ------------------------------------------------------------------------- */
-
 /* SPI bus functions */
 static int write8(void *client, u8 val)
 {
@@ -109,36 +75,16 @@ static const struct ad_dpot_bus_ops bops = {
 	.write_r8d8	= write16,
 	.write_r8d16	= write24,
 };
-
-static const struct ad_dpot_id *dpot_match_id(const struct ad_dpot_id *id,
-						char *name)
-{
-	while (id->name && id->name[0]) {
-		if (strcmp(name, id->name) == 0)
-			return id;
-		id++;
-	}
-	return NULL;
-}
-
 static int __devinit ad_dpot_spi_probe(struct spi_device *spi)
 {
-	char *name = spi->dev.platform_data;
-	const struct ad_dpot_id *dpot_id;
-
 	struct ad_dpot_bus_data bdata = {
 		.client = spi,
 		.bops = &bops,
 	};
 
-	dpot_id = dpot_match_id(ad_dpot_spi_devlist, name);
-
-	if (dpot_id == NULL) {
-		dev_err(&spi->dev, "%s not in supported device list", name);
-		return -ENODEV;
-	}
-
-	return ad_dpot_probe(&spi->dev, &bdata, dpot_id);
+	return ad_dpot_probe(&spi->dev, &bdata,
+			     spi_get_device_id(spi)->driver_data,
+			     spi_get_device_id(spi)->name);
 }
 
 static int __devexit ad_dpot_spi_remove(struct spi_device *spi)
@@ -146,14 +92,47 @@ static int __devexit ad_dpot_spi_remove(struct spi_device *spi)
 	return ad_dpot_remove(&spi->dev);
 }
 
+static const struct spi_device_id ad_dpot_spi_id[] = {
+	{"ad5160", AD5160_ID},
+	{"ad5161", AD5161_ID},
+	{"ad5162", AD5162_ID},
+	{"ad5165", AD5165_ID},
+	{"ad5200", AD5200_ID},
+	{"ad5201", AD5201_ID},
+	{"ad5203", AD5203_ID},
+	{"ad5204", AD5204_ID},
+	{"ad5206", AD5206_ID},
+	{"ad5207", AD5207_ID},
+	{"ad5231", AD5231_ID},
+	{"ad5232", AD5232_ID},
+	{"ad5233", AD5233_ID},
+	{"ad5235", AD5235_ID},
+	{"ad5260", AD5260_ID},
+	{"ad5262", AD5262_ID},
+	{"ad5263", AD5263_ID},
+	{"ad5290", AD5290_ID},
+	{"ad5291", AD5291_ID},
+	{"ad5292", AD5292_ID},
+	{"ad5293", AD5293_ID},
+	{"ad7376", AD7376_ID},
+	{"ad8400", AD8400_ID},
+	{"ad8402", AD8402_ID},
+	{"ad8403", AD8403_ID},
+	{"adn2850", ADN2850_ID},
+	{"ad5270", AD5270_ID},
+	{"ad5271", AD5271_ID},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ad_dpot_spi_id);
+
 static struct spi_driver ad_dpot_spi_driver = {
 	.driver = {
 		.name	= "ad_dpot",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad_dpot_spi_probe,
 	.remove		= __devexit_p(ad_dpot_spi_remove),
+	.id_table	= ad_dpot_spi_id,
 };
 
 static int __init ad_dpot_spi_init(void)
diff --git a/drivers/misc/ad525x_dpot.c b/drivers/misc/ad525x_dpot.c
index 7cb911028d09..1d1d42615915 100644
--- a/drivers/misc/ad525x_dpot.c
+++ b/drivers/misc/ad525x_dpot.c
@@ -64,7 +64,7 @@
  * Author: Chris Verges <chrisv@cyberswitching.com>
  *
  * derived from ad5252.c
- * Copyright (c) 2006 Michael Hennerich <hennerich@blackfin.uclinux.org>
+ * Copyright (c) 2006-2011 Michael Hennerich <hennerich@blackfin.uclinux.org>
  *
  * Licensed under the GPL-2 or later.
  */
@@ -76,8 +76,6 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 
-#define DRIVER_VERSION			"0.2"
-
 #include "ad525x_dpot.h"
 
 /*
@@ -687,8 +685,9 @@ inline void ad_dpot_remove_files(struct device *dev,
 	}
 }
 
-__devinit int ad_dpot_probe(struct device *dev,
-		struct ad_dpot_bus_data *bdata, const struct ad_dpot_id *id)
+int __devinit ad_dpot_probe(struct device *dev,
+		struct ad_dpot_bus_data *bdata, unsigned long devid,
+			    const char *name)
 {
 
 	struct dpot_data *data;
@@ -704,13 +703,13 @@ __devinit int ad_dpot_probe(struct device *dev,
 	mutex_init(&data->update_lock);
 
 	data->bdata = *bdata;
-	data->devid = id->devid;
+	data->devid = devid;
 
-	data->max_pos = 1 << DPOT_MAX_POS(data->devid);
+	data->max_pos = 1 << DPOT_MAX_POS(devid);
 	data->rdac_mask = data->max_pos - 1;
-	data->feat = DPOT_FEAT(data->devid);
-	data->uid = DPOT_UID(data->devid);
-	data->wipers = DPOT_WIPERS(data->devid);
+	data->feat = DPOT_FEAT(devid);
+	data->uid = DPOT_UID(devid);
+	data->wipers = DPOT_WIPERS(devid);
 
 	for (i = DPOT_RDAC0; i < MAX_RDACS; i++)
 		if (data->wipers & (1 << i)) {
@@ -731,7 +730,7 @@ __devinit int ad_dpot_probe(struct device *dev,
 	}
 
 	dev_info(dev, "%s %d-Position Digital Potentiometer registered\n",
-		 id->name, data->max_pos);
+		 name, data->max_pos);
 
 	return 0;
 
@@ -745,7 +744,7 @@ exit_free:
 	dev_set_drvdata(dev, NULL);
 exit:
 	dev_err(dev, "failed to create client for %s ID 0x%lX\n",
-			id->name, id->devid);
+		name, devid);
 	return err;
 }
 EXPORT_SYMBOL(ad_dpot_probe);
@@ -770,4 +769,3 @@ MODULE_AUTHOR("Chris Verges <chrisv@cyberswitching.com>, "
 	      "Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Digital potentiometer driver");
 MODULE_LICENSE("GPL");
-MODULE_VERSION(DRIVER_VERSION);
diff --git a/drivers/misc/ad525x_dpot.h b/drivers/misc/ad525x_dpot.h
index 82b2cb77ae19..6bd1eba23bc0 100644
--- a/drivers/misc/ad525x_dpot.h
+++ b/drivers/misc/ad525x_dpot.h
@@ -208,12 +208,8 @@ struct ad_dpot_bus_data {
 	const struct ad_dpot_bus_ops *bops;
 };
 
-struct ad_dpot_id {
-	char *name;
-	unsigned long devid;
-};
-
-int ad_dpot_probe(struct device *dev, struct ad_dpot_bus_data *bdata, const struct ad_dpot_id *id);
+int ad_dpot_probe(struct device *dev, struct ad_dpot_bus_data *bdata,
+		  unsigned long devid, const char *name);
 int ad_dpot_remove(struct device *dev);
 
 #endif
diff --git a/drivers/misc/bmp085.c b/drivers/misc/bmp085.c
index 5f898cb706a6..b29a2be24591 100644
--- a/drivers/misc/bmp085.c
+++ b/drivers/misc/bmp085.c
@@ -216,7 +216,7 @@ static s32 bmp085_get_temperature(struct bmp085_data *data, int *temperature)
 		*temperature = (x1+x2+8) >> 4;
 
 exit:
-	return status;;
+	return status;
 }
 
 /*
diff --git a/drivers/misc/isl29020.c b/drivers/misc/isl29020.c
index 307aada5fffe..3d6cce663bea 100644
--- a/drivers/misc/isl29020.c
+++ b/drivers/misc/isl29020.c
@@ -158,7 +158,7 @@ static int als_set_default_config(struct i2c_client *client)
 		dev_err(&client->dev, "default write failed.");
 		return retval;
 	}
-	return 0;;
+	return 0;
 }
 
 static int  isl29020_probe(struct i2c_client *client,
diff --git a/drivers/misc/ti-st/st_core.c b/drivers/misc/ti-st/st_core.c
index ba168a7d54d4..2b62232c2c6a 100644
--- a/drivers/misc/ti-st/st_core.c
+++ b/drivers/misc/ti-st/st_core.c
@@ -137,6 +137,8 @@ void st_send_frame(unsigned char chnl_id, struct st_data_s *st_gdata)
  * st_reg_complete -
  * to call registration complete callbacks
  * of all protocol stack drivers
+ * This function is being called with spin lock held, protocol drivers are
+ * only expected to complete their waits and do nothing more than that.
  */
 void st_reg_complete(struct st_data_s *st_gdata, char err)
 {
@@ -538,11 +540,12 @@ long st_register(struct st_proto_s *new_proto)
 		set_bit(ST_REG_IN_PROGRESS, &st_gdata->st_state);
 		st_recv = st_kim_recv;
 
+		/* enable the ST LL - to set default chip state */
+		st_ll_enable(st_gdata);
+
 		/* release lock previously held - re-locked below */
 		spin_unlock_irqrestore(&st_gdata->lock, flags);
 
-		/* enable the ST LL - to set default chip state */
-		st_ll_enable(st_gdata);
 		/* this may take a while to complete
 		 * since it involves BT fw download
 		 */
@@ -553,10 +556,13 @@ long st_register(struct st_proto_s *new_proto)
 			    (test_bit(ST_REG_PENDING, &st_gdata->st_state))) {
 				pr_err(" KIM failure complete callback ");
 				st_reg_complete(st_gdata, err);
+				clear_bit(ST_REG_PENDING, &st_gdata->st_state);
 			}
 			return -EINVAL;
 		}
 
+		spin_lock_irqsave(&st_gdata->lock, flags);
+
 		clear_bit(ST_REG_IN_PROGRESS, &st_gdata->st_state);
 		st_recv = st_int_recv;
 
@@ -576,10 +582,10 @@ long st_register(struct st_proto_s *new_proto)
 		if (st_gdata->is_registered[new_proto->chnl_id] == true) {
 			pr_err(" proto %d already registered ",
 				   new_proto->chnl_id);
+			spin_unlock_irqrestore(&st_gdata->lock, flags);
 			return -EALREADY;
 		}
 
-		spin_lock_irqsave(&st_gdata->lock, flags);
 		add_channel_to_table(st_gdata, new_proto);
 		st_gdata->protos_registered++;
 		new_proto->write = st_write;
@@ -619,7 +625,7 @@ long st_unregister(struct st_proto_s *proto)
 
 	spin_lock_irqsave(&st_gdata->lock, flags);
 
-	if (st_gdata->list[proto->chnl_id] == NULL) {
+	if (st_gdata->is_registered[proto->chnl_id] == false) {
 		pr_err(" chnl_id %d not registered", proto->chnl_id);
 		spin_unlock_irqrestore(&st_gdata->lock, flags);
 		return -EPROTONOSUPPORT;
@@ -629,6 +635,10 @@ long st_unregister(struct st_proto_s *proto)
 	remove_channel_from_table(st_gdata, proto);
 	spin_unlock_irqrestore(&st_gdata->lock, flags);
 
+	/* paranoid check */
+	if (st_gdata->protos_registered < ST_EMPTY)
+		st_gdata->protos_registered = ST_EMPTY;
+
 	if ((st_gdata->protos_registered == ST_EMPTY) &&
 	    (!test_bit(ST_REG_PENDING, &st_gdata->st_state))) {
 		pr_info(" all chnl_ids unregistered ");
diff --git a/drivers/misc/ti-st/st_kim.c b/drivers/misc/ti-st/st_kim.c
index 43ef8d162f2d..a7a861ceee2d 100644
--- a/drivers/misc/ti-st/st_kim.c
+++ b/drivers/misc/ti-st/st_kim.c
@@ -469,37 +469,21 @@ long st_kim_start(void *kim_data)
 		/* wait for ldisc to be installed */
 		err = wait_for_completion_timeout(&kim_gdata->ldisc_installed,
 				msecs_to_jiffies(LDISC_TIME));
-		if (!err) {	/* timeout */
-			pr_err("line disc installation timed out ");
-			kim_gdata->ldisc_install = 0;
-			pr_info("ldisc_install = 0");
-			sysfs_notify(&kim_gdata->kim_pdev->dev.kobj,
-					NULL, "install");
-			/* the following wait is never going to be completed,
-			 * since the ldisc was never installed, hence serving
-			 * as a mdelay of LDISC_TIME msecs */
-			err = wait_for_completion_timeout
-				(&kim_gdata->ldisc_installed,
-				 msecs_to_jiffies(LDISC_TIME));
-			err = -ETIMEDOUT;
+		if (!err) {
+			/* ldisc installation timeout,
+			 * flush uart, power cycle BT_EN */
+			pr_err("ldisc installation timeout");
+			err = st_kim_stop(kim_gdata);
 			continue;
 		} else {
 			/* ldisc installed now */
-			pr_info(" line discipline installed ");
+			pr_info("line discipline installed");
 			err = download_firmware(kim_gdata);
 			if (err != 0) {
+				/* ldisc installed but fw download failed,
+				 * flush uart & power cycle BT_EN */
 				pr_err("download firmware failed");
-				kim_gdata->ldisc_install = 0;
-				pr_info("ldisc_install = 0");
-				sysfs_notify(&kim_gdata->kim_pdev->dev.kobj,
-						NULL, "install");
-				/* this wait might be completed, though in the
-				 * tty_close() since the ldisc is already
-				 * installed */
-				err = wait_for_completion_timeout
-					(&kim_gdata->ldisc_installed,
-					 msecs_to_jiffies(LDISC_TIME));
-				err = -EINVAL;
+				err = st_kim_stop(kim_gdata);
 				continue;
 			} else {	/* on success don't retry */
 				break;
@@ -510,8 +494,14 @@ long st_kim_start(void *kim_data)
 }
 
 /**
- * st_kim_stop - called from ST Core, on the last un-registration
- *	toggle low the chip enable gpio
+ * st_kim_stop - stop communication with chip.
+ *	This can be called from ST Core/KIM, on the-
+ *	(a) last un-register when chip need not be powered there-after,
+ *	(b) upon failure to either install ldisc or download firmware.
+ *	The function is responsible to (a) notify UIM about un-installation,
+ *	(b) flush UART if the ldisc was installed.
+ *	(c) reset BT_EN - pull down nshutdown at the end.
+ *	(d) invoke platform's chip disabling routine.
  */
 long st_kim_stop(void *kim_data)
 {
@@ -519,12 +509,16 @@ long st_kim_stop(void *kim_data)
 	struct kim_data_s	*kim_gdata = (struct kim_data_s *)kim_data;
 	struct ti_st_plat_data	*pdata =
 		kim_gdata->kim_pdev->dev.platform_data;
+	struct tty_struct	*tty = kim_gdata->core_data->tty;
 
 	INIT_COMPLETION(kim_gdata->ldisc_installed);
 
-	/* Flush any pending characters in the driver and discipline. */
-	tty_ldisc_flush(kim_gdata->core_data->tty);
-	tty_driver_flush_buffer(kim_gdata->core_data->tty);
+	if (tty) {	/* can be called before ldisc is installed */
+		/* Flush any pending characters in the driver and discipline. */
+		tty_ldisc_flush(tty);
+		tty_driver_flush_buffer(tty);
+		tty->ops->flush_buffer(tty);
+	}
 
 	/* send uninstall notification to UIM */
 	pr_info("ldisc_install = 0");
@@ -579,6 +573,28 @@ static ssize_t show_install(struct device *dev,
 	return sprintf(buf, "%d\n", kim_data->ldisc_install);
 }
 
+#ifdef DEBUG
+static ssize_t store_dev_name(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct kim_data_s *kim_data = dev_get_drvdata(dev);
+	pr_debug("storing dev name >%s<", buf);
+	strncpy(kim_data->dev_name, buf, count);
+	pr_debug("stored dev name >%s<", kim_data->dev_name);
+	return count;
+}
+
+static ssize_t store_baud_rate(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct kim_data_s *kim_data = dev_get_drvdata(dev);
+	pr_debug("storing baud rate >%s<", buf);
+	sscanf(buf, "%ld", &kim_data->baud_rate);
+	pr_debug("stored baud rate >%ld<", kim_data->baud_rate);
+	return count;
+}
+#endif	/* if DEBUG */
+
 static ssize_t show_dev_name(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -605,10 +621,18 @@ static struct kobj_attribute ldisc_install =
 __ATTR(install, 0444, (void *)show_install, NULL);
 
 static struct kobj_attribute uart_dev_name =
+#ifdef DEBUG	/* TODO: move this to debug-fs if possible */
+__ATTR(dev_name, 0644, (void *)show_dev_name, (void *)store_dev_name);
+#else
 __ATTR(dev_name, 0444, (void *)show_dev_name, NULL);
+#endif
 
 static struct kobj_attribute uart_baud_rate =
+#ifdef DEBUG	/* TODO: move to debugfs */
+__ATTR(baud_rate, 0644, (void *)show_baud_rate, (void *)store_baud_rate);
+#else
 __ATTR(baud_rate, 0444, (void *)show_baud_rate, NULL);
+#endif
 
 static struct kobj_attribute uart_flow_cntrl =
 __ATTR(flow_cntrl, 0444, (void *)show_flow_cntrl, NULL);
diff --git a/drivers/mmc/host/at91_mci.c b/drivers/mmc/host/at91_mci.c
index a8b4d2aa18e5..f437c3e6f3aa 100644
--- a/drivers/mmc/host/at91_mci.c
+++ b/drivers/mmc/host/at91_mci.c
@@ -741,7 +741,7 @@ static void at91_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	at91_mci_write(host, AT91_MCI_MR, (at91_mci_read(host, AT91_MCI_MR) & ~AT91_MCI_CLKDIV) | clkdiv);
 
 	/* maybe switch power to the card */
-	if (host->board->vcc_pin) {
+	if (gpio_is_valid(host->board->vcc_pin)) {
 		switch (ios->power_mode) {
 			case MMC_POWER_OFF:
 				gpio_set_value(host->board->vcc_pin, 0);
@@ -897,7 +897,7 @@ static int at91_mci_get_ro(struct mmc_host *mmc)
 {
 	struct at91mci_host *host = mmc_priv(mmc);
 
-	if (host->board->wp_pin)
+	if (gpio_is_valid(host->board->wp_pin))
 		return !!gpio_get_value(host->board->wp_pin);
 	/*
 	 * Board doesn't support read only detection; let the mmc core
@@ -991,21 +991,21 @@ static int __init at91_mci_probe(struct platform_device *pdev)
 	 * Reserve GPIOs ... board init code makes sure these pins are set
 	 * up as GPIOs with the right direction (input, except for vcc)
 	 */
-	if (host->board->det_pin) {
+	if (gpio_is_valid(host->board->det_pin)) {
 		ret = gpio_request(host->board->det_pin, "mmc_detect");
 		if (ret < 0) {
 			dev_dbg(&pdev->dev, "couldn't claim card detect pin\n");
 			goto fail4b;
 		}
 	}
-	if (host->board->wp_pin) {
+	if (gpio_is_valid(host->board->wp_pin)) {
 		ret = gpio_request(host->board->wp_pin, "mmc_wp");
 		if (ret < 0) {
 			dev_dbg(&pdev->dev, "couldn't claim wp sense pin\n");
 			goto fail4;
 		}
 	}
-	if (host->board->vcc_pin) {
+	if (gpio_is_valid(host->board->vcc_pin)) {
 		ret = gpio_request(host->board->vcc_pin, "mmc_vcc");
 		if (ret < 0) {
 			dev_dbg(&pdev->dev, "couldn't claim vcc switch pin\n");
@@ -1057,7 +1057,7 @@ static int __init at91_mci_probe(struct platform_device *pdev)
 	/*
 	 * Add host to MMC layer
 	 */
-	if (host->board->det_pin) {
+	if (gpio_is_valid(host->board->det_pin)) {
 		host->present = !gpio_get_value(host->board->det_pin);
 	}
 	else
@@ -1068,7 +1068,7 @@ static int __init at91_mci_probe(struct platform_device *pdev)
 	/*
 	 * monitor card insertion/removal if we can
 	 */
-	if (host->board->det_pin) {
+	if (gpio_is_valid(host->board->det_pin)) {
 		ret = request_irq(gpio_to_irq(host->board->det_pin),
 				at91_mmc_det_irq, 0, mmc_hostname(mmc), host);
 		if (ret)
@@ -1087,13 +1087,13 @@ fail0:
 fail1:
 	clk_put(host->mci_clk);
 fail2:
-	if (host->board->vcc_pin)
+	if (gpio_is_valid(host->board->vcc_pin))
 		gpio_free(host->board->vcc_pin);
 fail3:
-	if (host->board->wp_pin)
+	if (gpio_is_valid(host->board->wp_pin))
 		gpio_free(host->board->wp_pin);
 fail4:
-	if (host->board->det_pin)
+	if (gpio_is_valid(host->board->det_pin))
 		gpio_free(host->board->det_pin);
 fail4b:
 	if (host->buffer)
@@ -1125,7 +1125,7 @@ static int __exit at91_mci_remove(struct platform_device *pdev)
 		dma_free_coherent(&pdev->dev, MCI_BUFSIZE,
 				host->buffer, host->physical_address);
 
-	if (host->board->det_pin) {
+	if (gpio_is_valid(host->board->det_pin)) {
 		if (device_can_wakeup(&pdev->dev))
 			free_irq(gpio_to_irq(host->board->det_pin), host);
 		device_init_wakeup(&pdev->dev, 0);
@@ -1140,9 +1140,9 @@ static int __exit at91_mci_remove(struct platform_device *pdev)
 	clk_disable(host->mci_clk);			/* Disable the peripheral clock */
 	clk_put(host->mci_clk);
 
-	if (host->board->vcc_pin)
+	if (gpio_is_valid(host->board->vcc_pin))
 		gpio_free(host->board->vcc_pin);
-	if (host->board->wp_pin)
+	if (gpio_is_valid(host->board->wp_pin))
 		gpio_free(host->board->wp_pin);
 
 	iounmap(host->baseaddr);
@@ -1163,7 +1163,7 @@ static int at91_mci_suspend(struct platform_device *pdev, pm_message_t state)
 	struct at91mci_host *host = mmc_priv(mmc);
 	int ret = 0;
 
-	if (host->board->det_pin && device_may_wakeup(&pdev->dev))
+	if (gpio_is_valid(host->board->det_pin) && device_may_wakeup(&pdev->dev))
 		enable_irq_wake(host->board->det_pin);
 
 	if (mmc)
@@ -1178,7 +1178,7 @@ static int at91_mci_resume(struct platform_device *pdev)
 	struct at91mci_host *host = mmc_priv(mmc);
 	int ret = 0;
 
-	if (host->board->det_pin && device_may_wakeup(&pdev->dev))
+	if (gpio_is_valid(host->board->det_pin) && device_may_wakeup(&pdev->dev))
 		disable_irq_wake(host->board->det_pin);
 
 	if (mmc)
diff --git a/drivers/mmc/host/mvsdio.c b/drivers/mmc/host/mvsdio.c
index 211a4959c293..eeb8cd125b0c 100644
--- a/drivers/mmc/host/mvsdio.c
+++ b/drivers/mmc/host/mvsdio.c
@@ -679,8 +679,9 @@ static const struct mmc_host_ops mvsd_ops = {
 	.enable_sdio_irq	= mvsd_enable_sdio_irq,
 };
 
-static void __init mv_conf_mbus_windows(struct mvsd_host *host,
-					struct mbus_dram_target_info *dram)
+static void __init
+mv_conf_mbus_windows(struct mvsd_host *host,
+		     const struct mbus_dram_target_info *dram)
 {
 	void __iomem *iobase = host->base;
 	int i;
@@ -691,7 +692,7 @@ static void __init mv_conf_mbus_windows(struct mvsd_host *host,
 	}
 
 	for (i = 0; i < dram->num_cs; i++) {
-		struct mbus_dram_window *cs = dram->cs + i;
+		const struct mbus_dram_window *cs = dram->cs + i;
 		writel(((cs->size - 1) & 0xffff0000) |
 		       (cs->mbus_attr << 8) |
 		       (dram->mbus_dram_target_id << 4) | 1,
@@ -705,6 +706,7 @@ static int __init mvsd_probe(struct platform_device *pdev)
 	struct mmc_host *mmc = NULL;
 	struct mvsd_host *host = NULL;
 	const struct mvsdio_platform_data *mvsd_data;
+	const struct mbus_dram_target_info *dram;
 	struct resource *r;
 	int ret, irq;
 
@@ -755,8 +757,9 @@ static int __init mvsd_probe(struct platform_device *pdev)
 	}
 
 	/* (Re-)program MBUS remapping windows if we are asked to. */
-	if (mvsd_data->dram != NULL)
-		mv_conf_mbus_windows(host, mvsd_data->dram);
+	dram = mv_mbus_dram_info();
+	if (dram)
+		mv_conf_mbus_windows(host, dram);
 
 	mvsd_power_down(host);
 
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index 99b449d26a4d..973011f9a298 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -713,7 +713,7 @@ static int mxs_mmc_probe(struct platform_device *pdev)
 		ret = PTR_ERR(host->clk);
 		goto out_iounmap;
 	}
-	clk_enable(host->clk);
+	clk_prepare_enable(host->clk);
 
 	mxs_mmc_reset(host);
 
@@ -772,7 +772,7 @@ out_free_dma:
 	if (host->dmach)
 		dma_release_channel(host->dmach);
 out_clk_put:
-	clk_disable(host->clk);
+	clk_disable_unprepare(host->clk);
 	clk_put(host->clk);
 out_iounmap:
 	iounmap(host->base);
@@ -798,7 +798,7 @@ static int mxs_mmc_remove(struct platform_device *pdev)
 	if (host->dmach)
 		dma_release_channel(host->dmach);
 
-	clk_disable(host->clk);
+	clk_disable_unprepare(host->clk);
 	clk_put(host->clk);
 
 	iounmap(host->base);
@@ -819,7 +819,7 @@ static int mxs_mmc_suspend(struct device *dev)
 
 	ret = mmc_suspend_host(mmc);
 
-	clk_disable(host->clk);
+	clk_disable_unprepare(host->clk);
 
 	return ret;
 }
@@ -830,7 +830,7 @@ static int mxs_mmc_resume(struct device *dev)
 	struct mxs_mmc_host *host = mmc_priv(mmc);
 	int ret = 0;
 
-	clk_enable(host->clk);
+	clk_prepare_enable(host->clk);
 
 	ret = mmc_resume_host(mmc);
 
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index d5fe43d53c51..d1fb561e089d 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -1991,6 +1991,8 @@ static int __init omap_hsmmc_probe(struct platform_device *pdev)
 	if (mmc_slot(host).nonremovable)
 		mmc->caps |= MMC_CAP_NONREMOVABLE;
 
+	mmc->pm_caps = mmc_slot(host).pm_caps;
+
 	omap_hsmmc_conf_bus_power(host);
 
 	/* Select DMA lines */
@@ -2179,13 +2181,7 @@ static int omap_hsmmc_suspend(struct device *dev)
 		cancel_work_sync(&host->mmc_carddetect_work);
 		ret = mmc_suspend_host(host->mmc);
 
-		if (ret == 0) {
-			omap_hsmmc_disable_irq(host);
-			OMAP_HSMMC_WRITE(host->base, HCTL,
-				OMAP_HSMMC_READ(host->base, HCTL) & ~SDBP);
-			if (host->got_dbclk)
-				clk_disable(host->dbclk);
-		} else {
+		if (ret) {
 			host->suspended = 0;
 			if (host->pdata->resume) {
 				ret = host->pdata->resume(&pdev->dev,
@@ -2194,9 +2190,20 @@ static int omap_hsmmc_suspend(struct device *dev)
 					dev_dbg(mmc_dev(host->mmc),
 						"Unmask interrupt failed\n");
 			}
+			goto err;
 		}
-		pm_runtime_put_sync(host->dev);
+
+		if (!(host->mmc->pm_flags & MMC_PM_KEEP_POWER)) {
+			omap_hsmmc_disable_irq(host);
+			OMAP_HSMMC_WRITE(host->base, HCTL,
+				OMAP_HSMMC_READ(host->base, HCTL) & ~SDBP);
+		}
+		if (host->got_dbclk)
+			clk_disable(host->dbclk);
+
 	}
+err:
+	pm_runtime_put_sync(host->dev);
 	return ret;
 }
 
@@ -2216,7 +2223,8 @@ static int omap_hsmmc_resume(struct device *dev)
 		if (host->got_dbclk)
 			clk_enable(host->dbclk);
 
-		omap_hsmmc_conf_bus_power(host);
+		if (!(host->mmc->pm_flags & MMC_PM_KEEP_POWER))
+			omap_hsmmc_conf_bus_power(host);
 
 		if (host->pdata->resume) {
 			ret = host->pdata->resume(&pdev->dev, host->slot_id);
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index 0d33ff0d67fb..9a20d1f55bb7 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -435,14 +435,11 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 
 	for (clks = 0, ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
 		struct clk *clk;
-		char *name = pdata->clocks[ptr];
-
-		if (name == NULL)
-			continue;
+		char name[14];
 
+		snprintf(name, 14, "mmc_busclk.%d", ptr);
 		clk = clk_get(dev, name);
 		if (IS_ERR(clk)) {
-			dev_err(dev, "failed to get clock %s\n", name);
 			continue;
 		}
 
diff --git a/drivers/mtd/Kconfig b/drivers/mtd/Kconfig
index 318a869286ab..1be621841400 100644
--- a/drivers/mtd/Kconfig
+++ b/drivers/mtd/Kconfig
@@ -140,6 +140,14 @@ config MTD_AR7_PARTS
 	---help---
 	  TI AR7 partitioning support
 
+config MTD_BCM63XX_PARTS
+	tristate "BCM63XX CFE partitioning support"
+	depends on BCM63XX
+	select CRC32
+	help
+	  This provides partions parsing for BCM63xx devices with CFE
+	  bootloaders.
+
 comment "User Modules And Translation Layers"
 
 config MTD_CHAR
diff --git a/drivers/mtd/Makefile b/drivers/mtd/Makefile
index 9aaac3ac89f3..f90135429dc7 100644
--- a/drivers/mtd/Makefile
+++ b/drivers/mtd/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_MTD_REDBOOT_PARTS) += redboot.o
 obj-$(CONFIG_MTD_CMDLINE_PARTS) += cmdlinepart.o
 obj-$(CONFIG_MTD_AFS_PARTS)	+= afs.o
 obj-$(CONFIG_MTD_AR7_PARTS)	+= ar7part.o
+obj-$(CONFIG_MTD_BCM63XX_PARTS)	+= bcm63xxpart.o
 
 # 'Users' - code which presents functionality to userspace.
 obj-$(CONFIG_MTD_CHAR)		+= mtdchar.o
diff --git a/drivers/mtd/afs.c b/drivers/mtd/afs.c
index 89a02f6f65dc..5a3942bf109c 100644
--- a/drivers/mtd/afs.c
+++ b/drivers/mtd/afs.c
@@ -75,7 +75,7 @@ afs_read_footer(struct mtd_info *mtd, u_int *img_start, u_int *iis_start,
 	size_t sz;
 	int ret;
 
-	ret = mtd->read(mtd, ptr, sizeof(fs), &sz, (u_char *) &fs);
+	ret = mtd_read(mtd, ptr, sizeof(fs), &sz, (u_char *)&fs);
 	if (ret >= 0 && sz != sizeof(fs))
 		ret = -EINVAL;
 
@@ -132,7 +132,7 @@ afs_read_iis(struct mtd_info *mtd, struct image_info_struct *iis, u_int ptr)
 	int ret, i;
 
 	memset(iis, 0, sizeof(*iis));
-	ret = mtd->read(mtd, ptr, sizeof(*iis), &sz, (u_char *) iis);
+	ret = mtd_read(mtd, ptr, sizeof(*iis), &sz, (u_char *)iis);
 	if (ret < 0)
 		goto failed;
 
diff --git a/drivers/mtd/ar7part.c b/drivers/mtd/ar7part.c
index f40ea4547554..945393129952 100644
--- a/drivers/mtd/ar7part.c
+++ b/drivers/mtd/ar7part.c
@@ -73,8 +73,8 @@ static int create_mtd_partitions(struct mtd_info *master,
 
 	do { /* Try 10 blocks starting from master->erasesize */
 		offset = pre_size;
-		master->read(master, offset,
-			     sizeof(header), &len, (uint8_t *)&header);
+		mtd_read(master, offset, sizeof(header), &len,
+			 (uint8_t *)&header);
 		if (!strncmp((char *)&header, "TIENV0.8", 8))
 			ar7_parts[1].offset = pre_size;
 		if (header.checksum == LOADER_MAGIC1)
@@ -95,16 +95,16 @@ static int create_mtd_partitions(struct mtd_info *master,
 	case LOADER_MAGIC1:
 		while (header.length) {
 			offset += sizeof(header) + header.length;
-			master->read(master, offset, sizeof(header),
-				     &len, (uint8_t *)&header);
+			mtd_read(master, offset, sizeof(header), &len,
+				 (uint8_t *)&header);
 		}
 		root_offset = offset + sizeof(header) + 4;
 		break;
 	case LOADER_MAGIC2:
 		while (header.length) {
 			offset += sizeof(header) + header.length;
-			master->read(master, offset, sizeof(header),
-				     &len, (uint8_t *)&header);
+			mtd_read(master, offset, sizeof(header), &len,
+				 (uint8_t *)&header);
 		}
 		root_offset = offset + sizeof(header) + 4 + 0xff;
 		root_offset &= ~(uint32_t)0xff;
@@ -114,8 +114,7 @@ static int create_mtd_partitions(struct mtd_info *master,
 		break;
 	}
 
-	master->read(master, root_offset,
-		sizeof(header), &len, (u8 *)&header);
+	mtd_read(master, root_offset, sizeof(header), &len, (u8 *)&header);
 	if (header.checksum != SQUASHFS_MAGIC) {
 		root_offset += master->erasesize - 1;
 		root_offset &= ~(master->erasesize - 1);
diff --git a/drivers/mtd/bcm63xxpart.c b/drivers/mtd/bcm63xxpart.c
new file mode 100644
index 000000000000..608321ee056e
--- /dev/null
+++ b/drivers/mtd/bcm63xxpart.c
@@ -0,0 +1,222 @@
+/*
+ * BCM63XX CFE image tag parser
+ *
+ * Copyright © 2006-2008  Florian Fainelli <florian@openwrt.org>
+ *			  Mike Albon <malbon@openwrt.org>
+ * Copyright © 2009-2010  Daniel Dickinson <openwrt@cshore.neomailbox.net>
+ * Copyright © 2011 Jonas Gorski <jonas.gorski@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/crc32.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+
+#include <asm/mach-bcm63xx/bcm963xx_tag.h>
+#include <asm/mach-bcm63xx/board_bcm963xx.h>
+
+#define BCM63XX_EXTENDED_SIZE	0xBFC00000	/* Extended flash address */
+
+#define BCM63XX_MIN_CFE_SIZE	0x10000		/* always at least 64KiB */
+#define BCM63XX_MIN_NVRAM_SIZE	0x10000		/* always at least 64KiB */
+
+#define BCM63XX_CFE_MAGIC_OFFSET 0x4e0
+
+static int bcm63xx_detect_cfe(struct mtd_info *master)
+{
+	char buf[9];
+	int ret;
+	size_t retlen;
+
+	ret = mtd_read(master, BCM963XX_CFE_VERSION_OFFSET, 5, &retlen,
+		       (void *)buf);
+	buf[retlen] = 0;
+
+	if (ret)
+		return ret;
+
+	if (strncmp("cfe-v", buf, 5) == 0)
+		return 0;
+
+	/* very old CFE's do not have the cfe-v string, so check for magic */
+	ret = mtd_read(master, BCM63XX_CFE_MAGIC_OFFSET, 8, &retlen,
+		       (void *)buf);
+	buf[retlen] = 0;
+
+	return strncmp("CFE1CFE1", buf, 8);
+}
+
+static int bcm63xx_parse_cfe_partitions(struct mtd_info *master,
+					struct mtd_partition **pparts,
+					struct mtd_part_parser_data *data)
+{
+	/* CFE, NVRAM and global Linux are always present */
+	int nrparts = 3, curpart = 0;
+	struct bcm_tag *buf;
+	struct mtd_partition *parts;
+	int ret;
+	size_t retlen;
+	unsigned int rootfsaddr, kerneladdr, spareaddr;
+	unsigned int rootfslen, kernellen, sparelen, totallen;
+	unsigned int cfelen, nvramlen;
+	int namelen = 0;
+	int i;
+	u32 computed_crc;
+
+	if (bcm63xx_detect_cfe(master))
+		return -EINVAL;
+
+	cfelen = max_t(uint32_t, master->erasesize, BCM63XX_MIN_CFE_SIZE);
+	nvramlen = max_t(uint32_t, master->erasesize, BCM63XX_MIN_NVRAM_SIZE);
+
+	/* Allocate memory for buffer */
+	buf = vmalloc(sizeof(struct bcm_tag));
+	if (!buf)
+		return -ENOMEM;
+
+	/* Get the tag */
+	ret = mtd_read(master, cfelen, sizeof(struct bcm_tag), &retlen,
+		       (void *)buf);
+
+	if (retlen != sizeof(struct bcm_tag)) {
+		vfree(buf);
+		return -EIO;
+	}
+
+	computed_crc = crc32_le(IMAGETAG_CRC_START, (u8 *)buf,
+				offsetof(struct bcm_tag, header_crc));
+	if (computed_crc == buf->header_crc) {
+		char *boardid = &(buf->board_id[0]);
+		char *tagversion = &(buf->tag_version[0]);
+
+		sscanf(buf->kernel_address, "%u", &kerneladdr);
+		sscanf(buf->kernel_length, "%u", &kernellen);
+		sscanf(buf->total_length, "%u", &totallen);
+
+		pr_info("CFE boot tag found with version %s and board type %s\n",
+			tagversion, boardid);
+
+		kerneladdr = kerneladdr - BCM63XX_EXTENDED_SIZE;
+		rootfsaddr = kerneladdr + kernellen;
+		spareaddr = roundup(totallen, master->erasesize) + cfelen;
+		sparelen = master->size - spareaddr - nvramlen;
+		rootfslen = spareaddr - rootfsaddr;
+	} else {
+		pr_warn("CFE boot tag CRC invalid (expected %08x, actual %08x)\n",
+			buf->header_crc, computed_crc);
+		kernellen = 0;
+		rootfslen = 0;
+		rootfsaddr = 0;
+		spareaddr = cfelen;
+		sparelen = master->size - cfelen - nvramlen;
+	}
+
+	/* Determine number of partitions */
+	namelen = 8;
+	if (rootfslen > 0) {
+		nrparts++;
+		namelen += 6;
+	}
+	if (kernellen > 0) {
+		nrparts++;
+		namelen += 6;
+	}
+
+	/* Ask kernel for more memory */
+	parts = kzalloc(sizeof(*parts) * nrparts + 10 * nrparts, GFP_KERNEL);
+	if (!parts) {
+		vfree(buf);
+		return -ENOMEM;
+	}
+
+	/* Start building partition list */
+	parts[curpart].name = "CFE";
+	parts[curpart].offset = 0;
+	parts[curpart].size = cfelen;
+	curpart++;
+
+	if (kernellen > 0) {
+		parts[curpart].name = "kernel";
+		parts[curpart].offset = kerneladdr;
+		parts[curpart].size = kernellen;
+		curpart++;
+	}
+
+	if (rootfslen > 0) {
+		parts[curpart].name = "rootfs";
+		parts[curpart].offset = rootfsaddr;
+		parts[curpart].size = rootfslen;
+		if (sparelen > 0)
+			parts[curpart].size += sparelen;
+		curpart++;
+	}
+
+	parts[curpart].name = "nvram";
+	parts[curpart].offset = master->size - nvramlen;
+	parts[curpart].size = nvramlen;
+
+	/* Global partition "linux" to make easy firmware upgrade */
+	curpart++;
+	parts[curpart].name = "linux";
+	parts[curpart].offset = cfelen;
+	parts[curpart].size = master->size - cfelen - nvramlen;
+
+	for (i = 0; i < nrparts; i++)
+		pr_info("Partition %d is %s offset %lx and length %lx\n", i,
+			parts[i].name, (long unsigned int)(parts[i].offset),
+			(long unsigned int)(parts[i].size));
+
+	pr_info("Spare partition is offset %x and length %x\n",	spareaddr,
+		sparelen);
+
+	*pparts = parts;
+	vfree(buf);
+
+	return nrparts;
+};
+
+static struct mtd_part_parser bcm63xx_cfe_parser = {
+	.owner = THIS_MODULE,
+	.parse_fn = bcm63xx_parse_cfe_partitions,
+	.name = "bcm63xxpart",
+};
+
+static int __init bcm63xx_cfe_parser_init(void)
+{
+	return register_mtd_parser(&bcm63xx_cfe_parser);
+}
+
+static void __exit bcm63xx_cfe_parser_exit(void)
+{
+	deregister_mtd_parser(&bcm63xx_cfe_parser);
+}
+
+module_init(bcm63xx_cfe_parser_init);
+module_exit(bcm63xx_cfe_parser_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Daniel Dickinson <openwrt@cshore.neomailbox.net>");
+MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
+MODULE_AUTHOR("Mike Albon <malbon@openwrt.org>");
+MODULE_AUTHOR("Jonas Gorski <jonas.gorski@gmail.com");
+MODULE_DESCRIPTION("MTD partitioning for BCM63XX CFE bootloaders");
diff --git a/drivers/mtd/chips/cfi_cmdset_0020.c b/drivers/mtd/chips/cfi_cmdset_0020.c
index 179814a95f3a..85e80180b65b 100644
--- a/drivers/mtd/chips/cfi_cmdset_0020.c
+++ b/drivers/mtd/chips/cfi_cmdset_0020.c
@@ -139,8 +139,9 @@ struct mtd_info *cfi_cmdset_0020(struct map_info *map, int primary)
 		}
 
 		/* Do some byteswapping if necessary */
-		extp->FeatureSupport = cfi32_to_cpu(extp->FeatureSupport);
-		extp->BlkStatusRegMask = cfi32_to_cpu(extp->BlkStatusRegMask);
+		extp->FeatureSupport = cfi32_to_cpu(map, extp->FeatureSupport);
+		extp->BlkStatusRegMask = cfi32_to_cpu(map,
+						extp->BlkStatusRegMask);
 
 #ifdef DEBUG_CFI_FEATURES
 		/* Tell the user about it in lots of lovely detail */
@@ -698,7 +699,8 @@ cfi_staa_writev(struct mtd_info *mtd, const struct kvec *vecs,
 				continue;
 			}
 			memcpy(buffer+buflen, elem_base, ECCBUF_SIZE-buflen);
-			ret = mtd->write(mtd, to, ECCBUF_SIZE, &thislen, buffer);
+			ret = mtd_write(mtd, to, ECCBUF_SIZE, &thislen,
+					buffer);
 			totlen += thislen;
 			if (ret || thislen != ECCBUF_SIZE)
 				goto write_error;
@@ -707,7 +709,8 @@ cfi_staa_writev(struct mtd_info *mtd, const struct kvec *vecs,
 			to += ECCBUF_SIZE;
 		}
 		if (ECCBUF_DIV(elem_len)) { /* write clean aligned data */
-			ret = mtd->write(mtd, to, ECCBUF_DIV(elem_len), &thislen, elem_base);
+			ret = mtd_write(mtd, to, ECCBUF_DIV(elem_len),
+					&thislen, elem_base);
 			totlen += thislen;
 			if (ret || thislen != ECCBUF_DIV(elem_len))
 				goto write_error;
@@ -721,7 +724,7 @@ cfi_staa_writev(struct mtd_info *mtd, const struct kvec *vecs,
 	}
 	if (buflen) { /* flush last page, even if not full */
 		/* This is sometimes intended behaviour, really */
-		ret = mtd->write(mtd, to, buflen, &thislen, buffer);
+		ret = mtd_write(mtd, to, buflen, &thislen, buffer);
 		totlen += thislen;
 		if (ret || thislen != ECCBUF_SIZE)
 			goto write_error;
diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index 283d887f7825..37b05c3f2792 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -191,6 +191,7 @@ comment "Disk-On-Chip Device Drivers"
 
 config MTD_DOC2000
 	tristate "M-Systems Disk-On-Chip 2000 and Millennium (DEPRECATED)"
+	depends on MTD_NAND
 	select MTD_DOCPROBE
 	select MTD_NAND_IDS
 	---help---
@@ -213,6 +214,7 @@ config MTD_DOC2000
 
 config MTD_DOC2001
 	tristate "M-Systems Disk-On-Chip Millennium-only alternative driver (DEPRECATED)"
+	depends on MTD_NAND
 	select MTD_DOCPROBE
 	select MTD_NAND_IDS
 	---help---
@@ -234,6 +236,7 @@ config MTD_DOC2001
 
 config MTD_DOC2001PLUS
 	tristate "M-Systems Disk-On-Chip Millennium Plus"
+	depends on MTD_NAND
 	select MTD_DOCPROBE
 	select MTD_NAND_IDS
 	---help---
@@ -251,6 +254,8 @@ config MTD_DOC2001PLUS
 
 config MTD_DOCG3
 	tristate "M-Systems Disk-On-Chip G3"
+	select BCH
+	select BCH_CONST_PARAMS
 	---help---
 	  This provides an MTD device driver for the M-Systems DiskOnChip
 	  G3 devices.
@@ -259,6 +264,13 @@ config MTD_DOCG3
 	  M-Systems and now Sandisk. The support is very experimental,
 	  and doesn't give access to any write operations.
 
+if MTD_DOCG3
+config BCH_CONST_M
+	default 14
+config BCH_CONST_T
+	default 4
+endif
+
 config MTD_DOCPROBE
 	tristate
 	select MTD_DOCECC
diff --git a/drivers/mtd/devices/block2mtd.c b/drivers/mtd/devices/block2mtd.c
index ebeabc727f70..e7e46d1e7463 100644
--- a/drivers/mtd/devices/block2mtd.c
+++ b/drivers/mtd/devices/block2mtd.c
@@ -287,7 +287,7 @@ static struct block2mtd_dev *add_device(char *devname, int erase_size)
 	dev->mtd.flags = MTD_CAP_RAM;
 	dev->mtd.erase = block2mtd_erase;
 	dev->mtd.write = block2mtd_write;
-	dev->mtd.writev = default_mtd_writev;
+	dev->mtd.writev = mtd_writev;
 	dev->mtd.sync = block2mtd_sync;
 	dev->mtd.read = block2mtd_read;
 	dev->mtd.priv = dev;
diff --git a/drivers/mtd/devices/doc2000.c b/drivers/mtd/devices/doc2000.c
index e9fad9151219..b1cdf6479019 100644
--- a/drivers/mtd/devices/doc2000.c
+++ b/drivers/mtd/devices/doc2000.c
@@ -562,23 +562,14 @@ void DoC2k_init(struct mtd_info *mtd)
 
 	mtd->type = MTD_NANDFLASH;
 	mtd->flags = MTD_CAP_NANDFLASH;
-	mtd->size = 0;
-	mtd->erasesize = 0;
 	mtd->writesize = 512;
 	mtd->oobsize = 16;
 	mtd->owner = THIS_MODULE;
 	mtd->erase = doc_erase;
-	mtd->point = NULL;
-	mtd->unpoint = NULL;
 	mtd->read = doc_read;
 	mtd->write = doc_write;
 	mtd->read_oob = doc_read_oob;
 	mtd->write_oob = doc_write_oob;
-	mtd->sync = NULL;
-
-	this->totlen = 0;
-	this->numchips = 0;
-
 	this->curfloor = -1;
 	this->curchip = -1;
 	mutex_init(&this->lock);
diff --git a/drivers/mtd/devices/doc2001.c b/drivers/mtd/devices/doc2001.c
index a3f7a27499be..7543b98f46c4 100644
--- a/drivers/mtd/devices/doc2001.c
+++ b/drivers/mtd/devices/doc2001.c
@@ -343,25 +343,17 @@ void DoCMil_init(struct mtd_info *mtd)
 
 	mtd->type = MTD_NANDFLASH;
 	mtd->flags = MTD_CAP_NANDFLASH;
-	mtd->size = 0;
 
 	/* FIXME: erase size is not always 8KiB */
 	mtd->erasesize = 0x2000;
-
 	mtd->writesize = 512;
 	mtd->oobsize = 16;
 	mtd->owner = THIS_MODULE;
 	mtd->erase = doc_erase;
-	mtd->point = NULL;
-	mtd->unpoint = NULL;
 	mtd->read = doc_read;
 	mtd->write = doc_write;
 	mtd->read_oob = doc_read_oob;
 	mtd->write_oob = doc_write_oob;
-	mtd->sync = NULL;
-
-	this->totlen = 0;
-	this->numchips = 0;
 	this->curfloor = -1;
 	this->curchip = -1;
 
diff --git a/drivers/mtd/devices/doc2001plus.c b/drivers/mtd/devices/doc2001plus.c
index 99351bc3e0ed..177510d0e7ee 100644
--- a/drivers/mtd/devices/doc2001plus.c
+++ b/drivers/mtd/devices/doc2001plus.c
@@ -467,23 +467,14 @@ void DoCMilPlus_init(struct mtd_info *mtd)
 
 	mtd->type = MTD_NANDFLASH;
 	mtd->flags = MTD_CAP_NANDFLASH;
-	mtd->size = 0;
-
-	mtd->erasesize = 0;
 	mtd->writesize = 512;
 	mtd->oobsize = 16;
 	mtd->owner = THIS_MODULE;
 	mtd->erase = doc_erase;
-	mtd->point = NULL;
-	mtd->unpoint = NULL;
 	mtd->read = doc_read;
 	mtd->write = doc_write;
 	mtd->read_oob = doc_read_oob;
 	mtd->write_oob = doc_write_oob;
-	mtd->sync = NULL;
-
-	this->totlen = 0;
-	this->numchips = 0;
 	this->curfloor = -1;
 	this->curchip = -1;
 
diff --git a/drivers/mtd/devices/docg3.c b/drivers/mtd/devices/docg3.c
index bdcf5df982e8..ad11ef0a81f4 100644
--- a/drivers/mtd/devices/docg3.c
+++ b/drivers/mtd/devices/docg3.c
@@ -29,6 +29,9 @@
 #include <linux/delay.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
+#include <linux/bitmap.h>
+#include <linux/bitrev.h>
+#include <linux/bch.h>
 
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
@@ -41,11 +44,7 @@
  *
  * As no specification is available from M-Systems/Sandisk, this drivers lacks
  * several functions available on the chip, as :
- *  - block erase
- *  - page write
  *  - IPL write
- *  - ECC fixing (lack of BCH algorith understanding)
- *  - powerdown / powerup
  *
  * The bus data width (8bits versus 16bits) is not handled (if_cfg flag), and
  * the driver assumes a 16bits data bus.
@@ -53,8 +52,7 @@
  * DocG3 relies on 2 ECC algorithms, which are handled in hardware :
  *  - a 1 byte Hamming code stored in the OOB for each page
  *  - a 7 bytes BCH code stored in the OOB for each page
- * The BCH part is only used for check purpose, no correction is available as
- * some information is missing. What is known is that :
+ * The BCH ECC is :
  *  - BCH is in GF(2^14)
  *  - BCH is over data of 520 bytes (512 page + 7 page_info bytes
  *                                   + 1 hamming byte)
@@ -63,6 +61,30 @@
  *
  */
 
+static unsigned int reliable_mode;
+module_param(reliable_mode, uint, 0);
+MODULE_PARM_DESC(reliable_mode, "Set the docg3 mode (0=normal MLC, 1=fast, "
+		 "2=reliable) : MLC normal operations are in normal mode");
+
+/**
+ * struct docg3_oobinfo - DiskOnChip G3 OOB layout
+ * @eccbytes: 8 bytes are used (1 for Hamming ECC, 7 for BCH ECC)
+ * @eccpos: ecc positions (byte 7 is Hamming ECC, byte 8-14 are BCH ECC)
+ * @oobfree: free pageinfo bytes (byte 0 until byte 6, byte 15
+ * @oobavail: 8 available bytes remaining after ECC toll
+ */
+static struct nand_ecclayout docg3_oobinfo = {
+	.eccbytes = 8,
+	.eccpos = {7, 8, 9, 10, 11, 12, 13, 14},
+	.oobfree = {{0, 7}, {15, 1} },
+	.oobavail = 8,
+};
+
+/**
+ * struct docg3_bch - BCH engine
+ */
+static struct bch_control *docg3_bch;
+
 static inline u8 doc_readb(struct docg3 *docg3, u16 reg)
 {
 	u8 val = readb(docg3->base + reg);
@@ -82,7 +104,7 @@ static inline u16 doc_readw(struct docg3 *docg3, u16 reg)
 static inline void doc_writeb(struct docg3 *docg3, u8 val, u16 reg)
 {
 	writeb(val, docg3->base + reg);
-	trace_docg3_io(1, 16, reg, val);
+	trace_docg3_io(1, 8, reg, val);
 }
 
 static inline void doc_writew(struct docg3 *docg3, u16 val, u16 reg)
@@ -143,7 +165,7 @@ static void doc_delay(struct docg3 *docg3, int nbNOPs)
 {
 	int i;
 
-	doc_dbg("NOP x %d\n", nbNOPs);
+	doc_vdbg("NOP x %d\n", nbNOPs);
 	for (i = 0; i < nbNOPs; i++)
 		doc_writeb(docg3, 0, DOC_NOP);
 }
@@ -196,8 +218,8 @@ static int doc_reset_seq(struct docg3 *docg3)
 /**
  * doc_read_data_area - Read data from data area
  * @docg3: the device
- * @buf: the buffer to fill in
- * @len: the lenght to read
+ * @buf: the buffer to fill in (might be NULL is dummy reads)
+ * @len: the length to read
  * @first: first time read, DOC_READADDRESS should be set
  *
  * Reads bytes from flash data. Handles the single byte / even bytes reads.
@@ -218,8 +240,10 @@ static void doc_read_data_area(struct docg3 *docg3, void *buf, int len,
 	dst16 = buf;
 	for (i = 0; i < len4; i += 2) {
 		data16 = doc_readw(docg3, DOC_IOSPACE_DATA);
-		*dst16 = data16;
-		dst16++;
+		if (dst16) {
+			*dst16 = data16;
+			dst16++;
+		}
 	}
 
 	if (cdr) {
@@ -229,26 +253,84 @@ static void doc_read_data_area(struct docg3 *docg3, void *buf, int len,
 		dst8 = (u8 *)dst16;
 		for (i = 0; i < cdr; i++) {
 			data8 = doc_readb(docg3, DOC_IOSPACE_DATA);
-			*dst8 = data8;
-			dst8++;
+			if (dst8) {
+				*dst8 = data8;
+				dst8++;
+			}
 		}
 	}
 }
 
 /**
- * doc_set_data_mode - Sets the flash to reliable data mode
+ * doc_write_data_area - Write data into data area
+ * @docg3: the device
+ * @buf: the buffer to get input bytes from
+ * @len: the length to write
+ *
+ * Writes bytes into flash data. Handles the single byte / even bytes writes.
+ */
+static void doc_write_data_area(struct docg3 *docg3, const void *buf, int len)
+{
+	int i, cdr, len4;
+	u16 *src16;
+	u8 *src8;
+
+	doc_dbg("doc_write_data_area(buf=%p, len=%d)\n", buf, len);
+	cdr = len & 0x3;
+	len4 = len - cdr;
+
+	doc_writew(docg3, DOC_IOSPACE_DATA, DOC_READADDRESS);
+	src16 = (u16 *)buf;
+	for (i = 0; i < len4; i += 2) {
+		doc_writew(docg3, *src16, DOC_IOSPACE_DATA);
+		src16++;
+	}
+
+	src8 = (u8 *)src16;
+	for (i = 0; i < cdr; i++) {
+		doc_writew(docg3, DOC_IOSPACE_DATA | DOC_READADDR_ONE_BYTE,
+			   DOC_READADDRESS);
+		doc_writeb(docg3, *src8, DOC_IOSPACE_DATA);
+		src8++;
+	}
+}
+
+/**
+ * doc_set_data_mode - Sets the flash to normal or reliable data mode
  * @docg3: the device
  *
  * The reliable data mode is a bit slower than the fast mode, but less errors
  * occur.  Entering the reliable mode cannot be done without entering the fast
  * mode first.
+ *
+ * In reliable mode, pages 2*n and 2*n+1 are clones. Writing to page 0 of blocks
+ * (4,5) make the hardware write also to page 1 of blocks blocks(4,5). Reading
+ * from page 0 of blocks (4,5) or from page 1 of blocks (4,5) gives the same
+ * result, which is a logical and between bytes from page 0 and page 1 (which is
+ * consistent with the fact that writing to a page is _clearing_ bits of that
+ * page).
  */
 static void doc_set_reliable_mode(struct docg3 *docg3)
 {
-	doc_dbg("doc_set_reliable_mode()\n");
-	doc_flash_sequence(docg3, DOC_SEQ_SET_MODE);
-	doc_flash_command(docg3, DOC_CMD_FAST_MODE);
-	doc_flash_command(docg3, DOC_CMD_RELIABLE_MODE);
+	static char *strmode[] = { "normal", "fast", "reliable", "invalid" };
+
+	doc_dbg("doc_set_reliable_mode(%s)\n", strmode[docg3->reliable]);
+	switch (docg3->reliable) {
+	case 0:
+		break;
+	case 1:
+		doc_flash_sequence(docg3, DOC_SEQ_SET_FASTMODE);
+		doc_flash_command(docg3, DOC_CMD_FAST_MODE);
+		break;
+	case 2:
+		doc_flash_sequence(docg3, DOC_SEQ_SET_RELIABLEMODE);
+		doc_flash_command(docg3, DOC_CMD_FAST_MODE);
+		doc_flash_command(docg3, DOC_CMD_RELIABLE_MODE);
+		break;
+	default:
+		doc_err("doc_set_reliable_mode(): invalid mode\n");
+		break;
+	}
 	doc_delay(docg3, 2);
 }
 
@@ -325,6 +407,37 @@ static int doc_set_extra_page_mode(struct docg3 *docg3)
 }
 
 /**
+ * doc_setup_addr_sector - Setup blocks/page/ofs address for one plane
+ * @docg3: the device
+ * @sector: the sector
+ */
+static void doc_setup_addr_sector(struct docg3 *docg3, int sector)
+{
+	doc_delay(docg3, 1);
+	doc_flash_address(docg3, sector & 0xff);
+	doc_flash_address(docg3, (sector >> 8) & 0xff);
+	doc_flash_address(docg3, (sector >> 16) & 0xff);
+	doc_delay(docg3, 1);
+}
+
+/**
+ * doc_setup_writeaddr_sector - Setup blocks/page/ofs address for one plane
+ * @docg3: the device
+ * @sector: the sector
+ * @ofs: the offset in the page, between 0 and (512 + 16 + 512)
+ */
+static void doc_setup_writeaddr_sector(struct docg3 *docg3, int sector, int ofs)
+{
+	ofs = ofs >> 2;
+	doc_delay(docg3, 1);
+	doc_flash_address(docg3, ofs & 0xff);
+	doc_flash_address(docg3, sector & 0xff);
+	doc_flash_address(docg3, (sector >> 8) & 0xff);
+	doc_flash_address(docg3, (sector >> 16) & 0xff);
+	doc_delay(docg3, 1);
+}
+
+/**
  * doc_seek - Set both flash planes to the specified block, page for reading
  * @docg3: the device
  * @block0: the first plane block index
@@ -360,34 +473,80 @@ static int doc_read_seek(struct docg3 *docg3, int block0, int block1, int page,
 	if (ret)
 		goto out;
 
-	sector = (block0 << DOC_ADDR_BLOCK_SHIFT) + (page & DOC_ADDR_PAGE_MASK);
 	doc_flash_sequence(docg3, DOC_SEQ_READ);
+	sector = (block0 << DOC_ADDR_BLOCK_SHIFT) + (page & DOC_ADDR_PAGE_MASK);
 	doc_flash_command(docg3, DOC_CMD_PROG_BLOCK_ADDR);
-	doc_delay(docg3, 1);
-	doc_flash_address(docg3, sector & 0xff);
-	doc_flash_address(docg3, (sector >> 8) & 0xff);
-	doc_flash_address(docg3, (sector >> 16) & 0xff);
-	doc_delay(docg3, 1);
+	doc_setup_addr_sector(docg3, sector);
 
 	sector = (block1 << DOC_ADDR_BLOCK_SHIFT) + (page & DOC_ADDR_PAGE_MASK);
 	doc_flash_command(docg3, DOC_CMD_PROG_BLOCK_ADDR);
+	doc_setup_addr_sector(docg3, sector);
 	doc_delay(docg3, 1);
-	doc_flash_address(docg3, sector & 0xff);
-	doc_flash_address(docg3, (sector >> 8) & 0xff);
-	doc_flash_address(docg3, (sector >> 16) & 0xff);
+
+out:
+	return ret;
+}
+
+/**
+ * doc_write_seek - Set both flash planes to the specified block, page for writing
+ * @docg3: the device
+ * @block0: the first plane block index
+ * @block1: the second plane block index
+ * @page: the page index within the block
+ * @ofs: offset in page to write
+ *
+ * Programs the flash even and odd planes to the specific block and page.
+ * Alternatively, programs the flash to the wear area of the specified page.
+ */
+static int doc_write_seek(struct docg3 *docg3, int block0, int block1, int page,
+			 int ofs)
+{
+	int ret = 0, sector;
+
+	doc_dbg("doc_write_seek(blocks=(%d,%d), page=%d, ofs=%d)\n",
+		block0, block1, page, ofs);
+
+	doc_set_reliable_mode(docg3);
+
+	if (ofs < 2 * DOC_LAYOUT_PAGE_SIZE) {
+		doc_flash_sequence(docg3, DOC_SEQ_SET_PLANE1);
+		doc_flash_command(docg3, DOC_CMD_READ_PLANE1);
+		doc_delay(docg3, 2);
+	} else {
+		doc_flash_sequence(docg3, DOC_SEQ_SET_PLANE2);
+		doc_flash_command(docg3, DOC_CMD_READ_PLANE2);
+		doc_delay(docg3, 2);
+	}
+
+	doc_flash_sequence(docg3, DOC_SEQ_PAGE_SETUP);
+	doc_flash_command(docg3, DOC_CMD_PROG_CYCLE1);
+
+	sector = (block0 << DOC_ADDR_BLOCK_SHIFT) + (page & DOC_ADDR_PAGE_MASK);
+	doc_setup_writeaddr_sector(docg3, sector, ofs);
+
+	doc_flash_command(docg3, DOC_CMD_PROG_CYCLE3);
 	doc_delay(docg3, 2);
+	ret = doc_wait_ready(docg3);
+	if (ret)
+		goto out;
+
+	doc_flash_command(docg3, DOC_CMD_PROG_CYCLE1);
+	sector = (block1 << DOC_ADDR_BLOCK_SHIFT) + (page & DOC_ADDR_PAGE_MASK);
+	doc_setup_writeaddr_sector(docg3, sector, ofs);
+	doc_delay(docg3, 1);
 
 out:
 	return ret;
 }
 
+
 /**
  * doc_read_page_ecc_init - Initialize hardware ECC engine
  * @docg3: the device
  * @len: the number of bytes covered by the ECC (BCH covered)
  *
  * The function does initialize the hardware ECC engine to compute the Hamming
- * ECC (on 1 byte) and the BCH Syndroms (on 7 bytes).
+ * ECC (on 1 byte) and the BCH hardware ECC (on 7 bytes).
  *
  * Return 0 if succeeded, -EIO on error
  */
@@ -403,6 +562,106 @@ static int doc_read_page_ecc_init(struct docg3 *docg3, int len)
 }
 
 /**
+ * doc_write_page_ecc_init - Initialize hardware BCH ECC engine
+ * @docg3: the device
+ * @len: the number of bytes covered by the ECC (BCH covered)
+ *
+ * The function does initialize the hardware ECC engine to compute the Hamming
+ * ECC (on 1 byte) and the BCH hardware ECC (on 7 bytes).
+ *
+ * Return 0 if succeeded, -EIO on error
+ */
+static int doc_write_page_ecc_init(struct docg3 *docg3, int len)
+{
+	doc_writew(docg3, DOC_ECCCONF0_WRITE_MODE
+		   | DOC_ECCCONF0_BCH_ENABLE | DOC_ECCCONF0_HAMMING_ENABLE
+		   | (len & DOC_ECCCONF0_DATA_BYTES_MASK),
+		   DOC_ECCCONF0);
+	doc_delay(docg3, 4);
+	doc_register_readb(docg3, DOC_FLASHCONTROL);
+	return doc_wait_ready(docg3);
+}
+
+/**
+ * doc_ecc_disable - Disable Hamming and BCH ECC hardware calculator
+ * @docg3: the device
+ *
+ * Disables the hardware ECC generator and checker, for unchecked reads (as when
+ * reading OOB only or write status byte).
+ */
+static void doc_ecc_disable(struct docg3 *docg3)
+{
+	doc_writew(docg3, DOC_ECCCONF0_READ_MODE, DOC_ECCCONF0);
+	doc_delay(docg3, 4);
+}
+
+/**
+ * doc_hamming_ecc_init - Initialize hardware Hamming ECC engine
+ * @docg3: the device
+ * @nb_bytes: the number of bytes covered by the ECC (Hamming covered)
+ *
+ * This function programs the ECC hardware to compute the hamming code on the
+ * last provided N bytes to the hardware generator.
+ */
+static void doc_hamming_ecc_init(struct docg3 *docg3, int nb_bytes)
+{
+	u8 ecc_conf1;
+
+	ecc_conf1 = doc_register_readb(docg3, DOC_ECCCONF1);
+	ecc_conf1 &= ~DOC_ECCCONF1_HAMMING_BITS_MASK;
+	ecc_conf1 |= (nb_bytes & DOC_ECCCONF1_HAMMING_BITS_MASK);
+	doc_writeb(docg3, ecc_conf1, DOC_ECCCONF1);
+}
+
+/**
+ * doc_ecc_bch_fix_data - Fix if need be read data from flash
+ * @docg3: the device
+ * @buf: the buffer of read data (512 + 7 + 1 bytes)
+ * @hwecc: the hardware calculated ECC.
+ *         It's in fact recv_ecc ^ calc_ecc, where recv_ecc was read from OOB
+ *         area data, and calc_ecc the ECC calculated by the hardware generator.
+ *
+ * Checks if the received data matches the ECC, and if an error is detected,
+ * tries to fix the bit flips (at most 4) in the buffer buf.  As the docg3
+ * understands the (data, ecc, syndroms) in an inverted order in comparison to
+ * the BCH library, the function reverses the order of bits (ie. bit7 and bit0,
+ * bit6 and bit 1, ...) for all ECC data.
+ *
+ * The hardware ecc unit produces oob_ecc ^ calc_ecc.  The kernel's bch
+ * algorithm is used to decode this.  However the hw operates on page
+ * data in a bit order that is the reverse of that of the bch alg,
+ * requiring that the bits be reversed on the result.  Thanks to Ivan
+ * Djelic for his analysis.
+ *
+ * Returns number of fixed bits (0, 1, 2, 3, 4) or -EBADMSG if too many bit
+ * errors were detected and cannot be fixed.
+ */
+static int doc_ecc_bch_fix_data(struct docg3 *docg3, void *buf, u8 *hwecc)
+{
+	u8 ecc[DOC_ECC_BCH_SIZE];
+	int errorpos[DOC_ECC_BCH_T], i, numerrs;
+
+	for (i = 0; i < DOC_ECC_BCH_SIZE; i++)
+		ecc[i] = bitrev8(hwecc[i]);
+	numerrs = decode_bch(docg3_bch, NULL, DOC_ECC_BCH_COVERED_BYTES,
+			     NULL, ecc, NULL, errorpos);
+	BUG_ON(numerrs == -EINVAL);
+	if (numerrs < 0)
+		goto out;
+
+	for (i = 0; i < numerrs; i++)
+		errorpos[i] = (errorpos[i] & ~7) | (7 - (errorpos[i] & 7));
+	for (i = 0; i < numerrs; i++)
+		if (errorpos[i] < DOC_ECC_BCH_COVERED_BYTES*8)
+			/* error is located in data, correct it */
+			change_bit(errorpos[i], buf);
+out:
+	doc_dbg("doc_ecc_bch_fix_data: flipped %d bits\n", numerrs);
+	return numerrs;
+}
+
+
+/**
  * doc_read_page_prepare - Prepares reading data from a flash page
  * @docg3: the device
  * @block0: the first plane block index on flash memory
@@ -488,16 +747,40 @@ static int doc_read_page_getbytes(struct docg3 *docg3, int len, u_char *buf,
 }
 
 /**
- * doc_get_hw_bch_syndroms - Get hardware calculated BCH syndroms
+ * doc_write_page_putbytes - Writes bytes into a prepared page
+ * @docg3: the device
+ * @len: the number of bytes to be written
+ * @buf: the buffer of input bytes
+ *
+ */
+static void doc_write_page_putbytes(struct docg3 *docg3, int len,
+				    const u_char *buf)
+{
+	doc_write_data_area(docg3, buf, len);
+	doc_delay(docg3, 2);
+}
+
+/**
+ * doc_get_bch_hw_ecc - Get hardware calculated BCH ECC
  * @docg3: the device
- * @syns:  the array of 7 integers where the syndroms will be stored
+ * @hwecc:  the array of 7 integers where the hardware ecc will be stored
  */
-static void doc_get_hw_bch_syndroms(struct docg3 *docg3, int *syns)
+static void doc_get_bch_hw_ecc(struct docg3 *docg3, u8 *hwecc)
 {
 	int i;
 
 	for (i = 0; i < DOC_ECC_BCH_SIZE; i++)
-		syns[i] = doc_register_readb(docg3, DOC_BCH_SYNDROM(i));
+		hwecc[i] = doc_register_readb(docg3, DOC_BCH_HW_ECC(i));
+}
+
+/**
+ * doc_page_finish - Ends reading/writing of a flash page
+ * @docg3: the device
+ */
+static void doc_page_finish(struct docg3 *docg3)
+{
+	doc_writeb(docg3, 0, DOC_DATAEND);
+	doc_delay(docg3, 2);
 }
 
 /**
@@ -510,8 +793,7 @@ static void doc_get_hw_bch_syndroms(struct docg3 *docg3, int *syns)
  */
 static void doc_read_page_finish(struct docg3 *docg3)
 {
-	doc_writeb(docg3, 0, DOC_DATAEND);
-	doc_delay(docg3, 2);
+	doc_page_finish(docg3);
 	doc_set_device_id(docg3, 0);
 }
 
@@ -523,18 +805,29 @@ static void doc_read_page_finish(struct docg3 *docg3)
  * @block1: second plane block index calculated
  * @page: page calculated
  * @ofs: offset in page
+ * @reliable: 0 if docg3 in normal mode, 1 if docg3 in fast mode, 2 if docg3 in
+ * reliable mode.
+ *
+ * The calculation is based on the reliable/normal mode. In normal mode, the 64
+ * pages of a block are available. In reliable mode, as pages 2*n and 2*n+1 are
+ * clones, only 32 pages per block are available.
  */
 static void calc_block_sector(loff_t from, int *block0, int *block1, int *page,
-			      int *ofs)
+			      int *ofs, int reliable)
 {
-	uint sector;
+	uint sector, pages_biblock;
+
+	pages_biblock = DOC_LAYOUT_PAGES_PER_BLOCK * DOC_LAYOUT_NBPLANES;
+	if (reliable == 1 || reliable == 2)
+		pages_biblock /= 2;
 
 	sector = from / DOC_LAYOUT_PAGE_SIZE;
-	*block0 = sector / (DOC_LAYOUT_PAGES_PER_BLOCK * DOC_LAYOUT_NBPLANES)
-		* DOC_LAYOUT_NBPLANES;
+	*block0 = sector / pages_biblock * DOC_LAYOUT_NBPLANES;
 	*block1 = *block0 + 1;
-	*page = sector % (DOC_LAYOUT_PAGES_PER_BLOCK * DOC_LAYOUT_NBPLANES);
+	*page = sector % pages_biblock;
 	*page /= DOC_LAYOUT_NBPLANES;
+	if (reliable == 1 || reliable == 2)
+		*page *= 2;
 	if (sector % 2)
 		*ofs = DOC_LAYOUT_PAGE_OOB_SIZE;
 	else
@@ -542,99 +835,124 @@ static void calc_block_sector(loff_t from, int *block0, int *block1, int *page,
 }
 
 /**
- * doc_read - Read bytes from flash
+ * doc_read_oob - Read out of band bytes from flash
  * @mtd: the device
  * @from: the offset from first block and first page, in bytes, aligned on page
  *        size
- * @len: the number of bytes to read (must be a multiple of 4)
- * @retlen: the number of bytes actually read
- * @buf: the filled in buffer
+ * @ops: the mtd oob structure
  *
- * Reads flash memory pages. This function does not read the OOB chunk, but only
- * the page data.
+ * Reads flash memory OOB area of pages.
  *
  * Returns 0 if read successfull, of -EIO, -EINVAL if an error occured
  */
-static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
-	     size_t *retlen, u_char *buf)
+static int doc_read_oob(struct mtd_info *mtd, loff_t from,
+			struct mtd_oob_ops *ops)
 {
 	struct docg3 *docg3 = mtd->priv;
-	int block0, block1, page, readlen, ret, ofs = 0;
-	int syn[DOC_ECC_BCH_SIZE], eccconf1;
-	u8 oob[DOC_LAYOUT_OOB_SIZE];
+	int block0, block1, page, ret, ofs = 0;
+	u8 *oobbuf = ops->oobbuf;
+	u8 *buf = ops->datbuf;
+	size_t len, ooblen, nbdata, nboob;
+	u8 hwecc[DOC_ECC_BCH_SIZE], eccconf1;
+
+	if (buf)
+		len = ops->len;
+	else
+		len = 0;
+	if (oobbuf)
+		ooblen = ops->ooblen;
+	else
+		ooblen = 0;
+
+	if (oobbuf && ops->mode == MTD_OPS_PLACE_OOB)
+		oobbuf += ops->ooboffs;
+
+	doc_dbg("doc_read_oob(from=%lld, mode=%d, data=(%p:%zu), oob=(%p:%zu))\n",
+		from, ops->mode, buf, len, oobbuf, ooblen);
+	if ((len % DOC_LAYOUT_PAGE_SIZE) || (ooblen % DOC_LAYOUT_OOB_SIZE) ||
+	    (from % DOC_LAYOUT_PAGE_SIZE))
+		return -EINVAL;
 
 	ret = -EINVAL;
-	doc_dbg("doc_read(from=%lld, len=%zu, buf=%p)\n", from, len, buf);
-	if (from % DOC_LAYOUT_PAGE_SIZE)
-		goto err;
-	if (len % 4)
-		goto err;
-	calc_block_sector(from, &block0, &block1, &page, &ofs);
+	calc_block_sector(from + len, &block0, &block1, &page, &ofs,
+			  docg3->reliable);
 	if (block1 > docg3->max_block)
 		goto err;
 
-	*retlen = 0;
+	ops->oobretlen = 0;
+	ops->retlen = 0;
 	ret = 0;
-	readlen = min_t(size_t, len, (size_t)DOC_LAYOUT_PAGE_SIZE);
-	while (!ret && len > 0) {
-		readlen = min_t(size_t, len, (size_t)DOC_LAYOUT_PAGE_SIZE);
+	while (!ret && (len > 0 || ooblen > 0)) {
+		calc_block_sector(from, &block0, &block1, &page, &ofs,
+			docg3->reliable);
+		nbdata = min_t(size_t, len, (size_t)DOC_LAYOUT_PAGE_SIZE);
+		nboob = min_t(size_t, ooblen, (size_t)DOC_LAYOUT_OOB_SIZE);
 		ret = doc_read_page_prepare(docg3, block0, block1, page, ofs);
 		if (ret < 0)
 			goto err;
-		ret = doc_read_page_ecc_init(docg3, DOC_ECC_BCH_COVERED_BYTES);
+		ret = doc_read_page_ecc_init(docg3, DOC_ECC_BCH_TOTAL_BYTES);
 		if (ret < 0)
 			goto err_in_read;
-		ret = doc_read_page_getbytes(docg3, readlen, buf, 1);
-		if (ret < readlen)
+		ret = doc_read_page_getbytes(docg3, nbdata, buf, 1);
+		if (ret < nbdata)
 			goto err_in_read;
-		ret = doc_read_page_getbytes(docg3, DOC_LAYOUT_OOB_SIZE,
-					     oob, 0);
-		if (ret < DOC_LAYOUT_OOB_SIZE)
+		doc_read_page_getbytes(docg3, DOC_LAYOUT_PAGE_SIZE - nbdata,
+				       NULL, 0);
+		ret = doc_read_page_getbytes(docg3, nboob, oobbuf, 0);
+		if (ret < nboob)
 			goto err_in_read;
+		doc_read_page_getbytes(docg3, DOC_LAYOUT_OOB_SIZE - nboob,
+				       NULL, 0);
 
-		*retlen += readlen;
-		buf += readlen;
-		len -= readlen;
-
-		ofs ^= DOC_LAYOUT_PAGE_OOB_SIZE;
-		if (ofs == 0)
-			page += 2;
-		if (page > DOC_ADDR_PAGE_MASK) {
-			page = 0;
-			block0 += 2;
-			block1 += 2;
-		}
-
-		/*
-		 * There should be a BCH bitstream fixing algorithm here ...
-		 * By now, a page read failure is triggered by BCH error
-		 */
-		doc_get_hw_bch_syndroms(docg3, syn);
+		doc_get_bch_hw_ecc(docg3, hwecc);
 		eccconf1 = doc_register_readb(docg3, DOC_ECCCONF1);
 
-		doc_dbg("OOB - INFO: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-			 oob[0], oob[1], oob[2], oob[3], oob[4],
-			 oob[5], oob[6]);
-		doc_dbg("OOB - HAMMING: %02x\n", oob[7]);
-		doc_dbg("OOB - BCH_ECC: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-			 oob[8], oob[9], oob[10], oob[11], oob[12],
-			 oob[13], oob[14]);
-		doc_dbg("OOB - UNUSED: %02x\n", oob[15]);
+		if (nboob >= DOC_LAYOUT_OOB_SIZE) {
+			doc_dbg("OOB - INFO: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+				oobbuf[0], oobbuf[1], oobbuf[2], oobbuf[3],
+				oobbuf[4], oobbuf[5], oobbuf[6]);
+			doc_dbg("OOB - HAMMING: %02x\n", oobbuf[7]);
+			doc_dbg("OOB - BCH_ECC: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+				oobbuf[8], oobbuf[9], oobbuf[10], oobbuf[11],
+				oobbuf[12], oobbuf[13], oobbuf[14]);
+			doc_dbg("OOB - UNUSED: %02x\n", oobbuf[15]);
+		}
 		doc_dbg("ECC checks: ECCConf1=%x\n", eccconf1);
-		doc_dbg("ECC BCH syndrom: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-			syn[0], syn[1], syn[2], syn[3], syn[4], syn[5], syn[6]);
-
-		ret = -EBADMSG;
-		if (block0 >= DOC_LAYOUT_BLOCK_FIRST_DATA) {
-			if (eccconf1 & DOC_ECCCONF1_BCH_SYNDROM_ERR)
-				goto err_in_read;
-			if (is_prot_seq_error(docg3))
-				goto err_in_read;
+		doc_dbg("ECC HW_ECC: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			hwecc[0], hwecc[1], hwecc[2], hwecc[3], hwecc[4],
+			hwecc[5], hwecc[6]);
+
+		ret = -EIO;
+		if (is_prot_seq_error(docg3))
+			goto err_in_read;
+		ret = 0;
+		if ((block0 >= DOC_LAYOUT_BLOCK_FIRST_DATA) &&
+		    (eccconf1 & DOC_ECCCONF1_BCH_SYNDROM_ERR) &&
+		    (eccconf1 & DOC_ECCCONF1_PAGE_IS_WRITTEN) &&
+		    (ops->mode != MTD_OPS_RAW) &&
+		    (nbdata == DOC_LAYOUT_PAGE_SIZE)) {
+			ret = doc_ecc_bch_fix_data(docg3, buf, hwecc);
+			if (ret < 0) {
+				mtd->ecc_stats.failed++;
+				ret = -EBADMSG;
+			}
+			if (ret > 0) {
+				mtd->ecc_stats.corrected += ret;
+				ret = -EUCLEAN;
+			}
 		}
+
 		doc_read_page_finish(docg3);
+		ops->retlen += nbdata;
+		ops->oobretlen += nboob;
+		buf += nbdata;
+		oobbuf += nboob;
+		len -= nbdata;
+		ooblen -= nboob;
+		from += DOC_LAYOUT_PAGE_SIZE;
 	}
 
-	return 0;
+	return ret;
 err_in_read:
 	doc_read_page_finish(docg3);
 err:
@@ -642,54 +960,33 @@ err:
 }
 
 /**
- * doc_read_oob - Read out of band bytes from flash
+ * doc_read - Read bytes from flash
  * @mtd: the device
  * @from: the offset from first block and first page, in bytes, aligned on page
  *        size
- * @ops: the mtd oob structure
+ * @len: the number of bytes to read (must be a multiple of 4)
+ * @retlen: the number of bytes actually read
+ * @buf: the filled in buffer
  *
- * Reads flash memory OOB area of pages.
+ * Reads flash memory pages. This function does not read the OOB chunk, but only
+ * the page data.
  *
  * Returns 0 if read successfull, of -EIO, -EINVAL if an error occured
  */
-static int doc_read_oob(struct mtd_info *mtd, loff_t from,
-			struct mtd_oob_ops *ops)
+static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
+	     size_t *retlen, u_char *buf)
 {
-	struct docg3 *docg3 = mtd->priv;
-	int block0, block1, page, ofs, ret;
-	u8 *buf = ops->oobbuf;
-	size_t len = ops->ooblen;
-
-	doc_dbg("doc_read_oob(from=%lld, buf=%p, len=%zu)\n", from, buf, len);
-	if (len != DOC_LAYOUT_OOB_SIZE)
-		return -EINVAL;
-
-	switch (ops->mode) {
-	case MTD_OPS_PLACE_OOB:
-		buf += ops->ooboffs;
-		break;
-	default:
-		break;
-	}
+	struct mtd_oob_ops ops;
+	size_t ret;
 
-	calc_block_sector(from, &block0, &block1, &page, &ofs);
-	if (block1 > docg3->max_block)
-		return -EINVAL;
-
-	ret = doc_read_page_prepare(docg3, block0, block1, page,
-				    ofs + DOC_LAYOUT_PAGE_SIZE);
-	if (!ret)
-		ret = doc_read_page_ecc_init(docg3, DOC_LAYOUT_OOB_SIZE);
-	if (!ret)
-		ret = doc_read_page_getbytes(docg3, DOC_LAYOUT_OOB_SIZE,
-					     buf, 1);
-	doc_read_page_finish(docg3);
+	memset(&ops, 0, sizeof(ops));
+	ops.datbuf = buf;
+	ops.len = len;
+	ops.mode = MTD_OPS_AUTO_OOB;
 
-	if (ret > 0)
-		ops->oobretlen = ret;
-	else
-		ops->oobretlen = 0;
-	return (ret > 0) ? 0 : ret;
+	ret = doc_read_oob(mtd, from, &ops);
+	*retlen = ops.retlen;
+	return ret;
 }
 
 static int doc_reload_bbt(struct docg3 *docg3)
@@ -726,7 +1023,8 @@ static int doc_block_isbad(struct mtd_info *mtd, loff_t from)
 	struct docg3 *docg3 = mtd->priv;
 	int block0, block1, page, ofs, is_good;
 
-	calc_block_sector(from, &block0, &block1, &page, &ofs);
+	calc_block_sector(from, &block0, &block1, &page, &ofs,
+		docg3->reliable);
 	doc_dbg("doc_block_isbad(from=%lld) => block=(%d,%d), page=%d, ofs=%d\n",
 		from, block0, block1, page, ofs);
 
@@ -739,6 +1037,7 @@ static int doc_block_isbad(struct mtd_info *mtd, loff_t from)
 	return !is_good;
 }
 
+#if 0
 /**
  * doc_get_erase_count - Get block erase count
  * @docg3: the device
@@ -758,7 +1057,7 @@ static int doc_get_erase_count(struct docg3 *docg3, loff_t from)
 	doc_dbg("doc_get_erase_count(from=%lld, buf=%p)\n", from, buf);
 	if (from % DOC_LAYOUT_PAGE_SIZE)
 		return -EINVAL;
-	calc_block_sector(from, &block0, &block1, &page, &ofs);
+	calc_block_sector(from, &block0, &block1, &page, &ofs, docg3->reliable);
 	if (block1 > docg3->max_block)
 		return -EINVAL;
 
@@ -780,6 +1079,558 @@ static int doc_get_erase_count(struct docg3 *docg3, loff_t from)
 
 	return max(plane1_erase_count, plane2_erase_count);
 }
+#endif
+
+/**
+ * doc_get_op_status - get erase/write operation status
+ * @docg3: the device
+ *
+ * Queries the status from the chip, and returns it
+ *
+ * Returns the status (bits DOC_PLANES_STATUS_*)
+ */
+static int doc_get_op_status(struct docg3 *docg3)
+{
+	u8 status;
+
+	doc_flash_sequence(docg3, DOC_SEQ_PLANES_STATUS);
+	doc_flash_command(docg3, DOC_CMD_PLANES_STATUS);
+	doc_delay(docg3, 5);
+
+	doc_ecc_disable(docg3);
+	doc_read_data_area(docg3, &status, 1, 1);
+	return status;
+}
+
+/**
+ * doc_write_erase_wait_status - wait for write or erase completion
+ * @docg3: the device
+ *
+ * Wait for the chip to be ready again after erase or write operation, and check
+ * erase/write status.
+ *
+ * Returns 0 if erase successfull, -EIO if erase/write issue, -ETIMEOUT if
+ * timeout
+ */
+static int doc_write_erase_wait_status(struct docg3 *docg3)
+{
+	int status, ret = 0;
+
+	if (!doc_is_ready(docg3))
+		usleep_range(3000, 3000);
+	if (!doc_is_ready(docg3)) {
+		doc_dbg("Timeout reached and the chip is still not ready\n");
+		ret = -EAGAIN;
+		goto out;
+	}
+
+	status = doc_get_op_status(docg3);
+	if (status & DOC_PLANES_STATUS_FAIL) {
+		doc_dbg("Erase/Write failed on (a) plane(s), status = %x\n",
+			status);
+		ret = -EIO;
+	}
+
+out:
+	doc_page_finish(docg3);
+	return ret;
+}
+
+/**
+ * doc_erase_block - Erase a couple of blocks
+ * @docg3: the device
+ * @block0: the first block to erase (leftmost plane)
+ * @block1: the second block to erase (rightmost plane)
+ *
+ * Erase both blocks, and return operation status
+ *
+ * Returns 0 if erase successful, -EIO if erase issue, -ETIMEOUT if chip not
+ * ready for too long
+ */
+static int doc_erase_block(struct docg3 *docg3, int block0, int block1)
+{
+	int ret, sector;
+
+	doc_dbg("doc_erase_block(blocks=(%d,%d))\n", block0, block1);
+	ret = doc_reset_seq(docg3);
+	if (ret)
+		return -EIO;
+
+	doc_set_reliable_mode(docg3);
+	doc_flash_sequence(docg3, DOC_SEQ_ERASE);
+
+	sector = block0 << DOC_ADDR_BLOCK_SHIFT;
+	doc_flash_command(docg3, DOC_CMD_PROG_BLOCK_ADDR);
+	doc_setup_addr_sector(docg3, sector);
+	sector = block1 << DOC_ADDR_BLOCK_SHIFT;
+	doc_flash_command(docg3, DOC_CMD_PROG_BLOCK_ADDR);
+	doc_setup_addr_sector(docg3, sector);
+	doc_delay(docg3, 1);
+
+	doc_flash_command(docg3, DOC_CMD_ERASECYCLE2);
+	doc_delay(docg3, 2);
+
+	if (is_prot_seq_error(docg3)) {
+		doc_err("Erase blocks %d,%d error\n", block0, block1);
+		return -EIO;
+	}
+
+	return doc_write_erase_wait_status(docg3);
+}
+
+/**
+ * doc_erase - Erase a portion of the chip
+ * @mtd: the device
+ * @info: the erase info
+ *
+ * Erase a bunch of contiguous blocks, by pairs, as a "mtd" page of 1024 is
+ * split into 2 pages of 512 bytes on 2 contiguous blocks.
+ *
+ * Returns 0 if erase successful, -EINVAL if adressing error, -EIO if erase
+ * issue
+ */
+static int doc_erase(struct mtd_info *mtd, struct erase_info *info)
+{
+	struct docg3 *docg3 = mtd->priv;
+	uint64_t len;
+	int block0, block1, page, ret, ofs = 0;
+
+	doc_dbg("doc_erase(from=%lld, len=%lld\n", info->addr, info->len);
+	doc_set_device_id(docg3, docg3->device_id);
+
+	info->state = MTD_ERASE_PENDING;
+	calc_block_sector(info->addr + info->len, &block0, &block1, &page,
+			  &ofs, docg3->reliable);
+	ret = -EINVAL;
+	if (block1 > docg3->max_block || page || ofs)
+		goto reset_err;
+
+	ret = 0;
+	calc_block_sector(info->addr, &block0, &block1, &page, &ofs,
+			  docg3->reliable);
+	doc_set_reliable_mode(docg3);
+	for (len = info->len; !ret && len > 0; len -= mtd->erasesize) {
+		info->state = MTD_ERASING;
+		ret = doc_erase_block(docg3, block0, block1);
+		block0 += 2;
+		block1 += 2;
+	}
+
+	if (ret)
+		goto reset_err;
+
+	info->state = MTD_ERASE_DONE;
+	return 0;
+
+reset_err:
+	info->state = MTD_ERASE_FAILED;
+	return ret;
+}
+
+/**
+ * doc_write_page - Write a single page to the chip
+ * @docg3: the device
+ * @to: the offset from first block and first page, in bytes, aligned on page
+ *      size
+ * @buf: buffer to get bytes from
+ * @oob: buffer to get out of band bytes from (can be NULL if no OOB should be
+ *       written)
+ * @autoecc: if 0, all 16 bytes from OOB are taken, regardless of HW Hamming or
+ *           BCH computations. If 1, only bytes 0-7 and byte 15 are taken,
+ *           remaining ones are filled with hardware Hamming and BCH
+ *           computations. Its value is not meaningfull is oob == NULL.
+ *
+ * Write one full page (ie. 1 page split on two planes), of 512 bytes, with the
+ * OOB data. The OOB ECC is automatically computed by the hardware Hamming and
+ * BCH generator if autoecc is not null.
+ *
+ * Returns 0 if write successful, -EIO if write error, -EAGAIN if timeout
+ */
+static int doc_write_page(struct docg3 *docg3, loff_t to, const u_char *buf,
+			  const u_char *oob, int autoecc)
+{
+	int block0, block1, page, ret, ofs = 0;
+	u8 hwecc[DOC_ECC_BCH_SIZE], hamming;
+
+	doc_dbg("doc_write_page(to=%lld)\n", to);
+	calc_block_sector(to, &block0, &block1, &page, &ofs, docg3->reliable);
+
+	doc_set_device_id(docg3, docg3->device_id);
+	ret = doc_reset_seq(docg3);
+	if (ret)
+		goto err;
+
+	/* Program the flash address block and page */
+	ret = doc_write_seek(docg3, block0, block1, page, ofs);
+	if (ret)
+		goto err;
+
+	doc_write_page_ecc_init(docg3, DOC_ECC_BCH_TOTAL_BYTES);
+	doc_delay(docg3, 2);
+	doc_write_page_putbytes(docg3, DOC_LAYOUT_PAGE_SIZE, buf);
+
+	if (oob && autoecc) {
+		doc_write_page_putbytes(docg3, DOC_LAYOUT_OOB_PAGEINFO_SZ, oob);
+		doc_delay(docg3, 2);
+		oob += DOC_LAYOUT_OOB_UNUSED_OFS;
+
+		hamming = doc_register_readb(docg3, DOC_HAMMINGPARITY);
+		doc_delay(docg3, 2);
+		doc_write_page_putbytes(docg3, DOC_LAYOUT_OOB_HAMMING_SZ,
+					&hamming);
+		doc_delay(docg3, 2);
+
+		doc_get_bch_hw_ecc(docg3, hwecc);
+		doc_write_page_putbytes(docg3, DOC_LAYOUT_OOB_BCH_SZ, hwecc);
+		doc_delay(docg3, 2);
+
+		doc_write_page_putbytes(docg3, DOC_LAYOUT_OOB_UNUSED_SZ, oob);
+	}
+	if (oob && !autoecc)
+		doc_write_page_putbytes(docg3, DOC_LAYOUT_OOB_SIZE, oob);
+
+	doc_delay(docg3, 2);
+	doc_page_finish(docg3);
+	doc_delay(docg3, 2);
+	doc_flash_command(docg3, DOC_CMD_PROG_CYCLE2);
+	doc_delay(docg3, 2);
+
+	/*
+	 * The wait status will perform another doc_page_finish() call, but that
+	 * seems to please the docg3, so leave it.
+	 */
+	ret = doc_write_erase_wait_status(docg3);
+	return ret;
+err:
+	doc_read_page_finish(docg3);
+	return ret;
+}
+
+/**
+ * doc_guess_autoecc - Guess autoecc mode from mbd_oob_ops
+ * @ops: the oob operations
+ *
+ * Returns 0 or 1 if success, -EINVAL if invalid oob mode
+ */
+static int doc_guess_autoecc(struct mtd_oob_ops *ops)
+{
+	int autoecc;
+
+	switch (ops->mode) {
+	case MTD_OPS_PLACE_OOB:
+	case MTD_OPS_AUTO_OOB:
+		autoecc = 1;
+		break;
+	case MTD_OPS_RAW:
+		autoecc = 0;
+		break;
+	default:
+		autoecc = -EINVAL;
+	}
+	return autoecc;
+}
+
+/**
+ * doc_fill_autooob - Fill a 16 bytes OOB from 8 non-ECC bytes
+ * @dst: the target 16 bytes OOB buffer
+ * @oobsrc: the source 8 bytes non-ECC OOB buffer
+ *
+ */
+static void doc_fill_autooob(u8 *dst, u8 *oobsrc)
+{
+	memcpy(dst, oobsrc, DOC_LAYOUT_OOB_PAGEINFO_SZ);
+	dst[DOC_LAYOUT_OOB_UNUSED_OFS] = oobsrc[DOC_LAYOUT_OOB_PAGEINFO_SZ];
+}
+
+/**
+ * doc_backup_oob - Backup OOB into docg3 structure
+ * @docg3: the device
+ * @to: the page offset in the chip
+ * @ops: the OOB size and buffer
+ *
+ * As the docg3 should write a page with its OOB in one pass, and some userland
+ * applications do write_oob() to setup the OOB and then write(), store the OOB
+ * into a temporary storage. This is very dangerous, as 2 concurrent
+ * applications could store an OOB, and then write their pages (which will
+ * result into one having its OOB corrupted).
+ *
+ * The only reliable way would be for userland to call doc_write_oob() with both
+ * the page data _and_ the OOB area.
+ *
+ * Returns 0 if success, -EINVAL if ops content invalid
+ */
+static int doc_backup_oob(struct docg3 *docg3, loff_t to,
+			  struct mtd_oob_ops *ops)
+{
+	int ooblen = ops->ooblen, autoecc;
+
+	if (ooblen != DOC_LAYOUT_OOB_SIZE)
+		return -EINVAL;
+	autoecc = doc_guess_autoecc(ops);
+	if (autoecc < 0)
+		return autoecc;
+
+	docg3->oob_write_ofs = to;
+	docg3->oob_autoecc = autoecc;
+	if (ops->mode == MTD_OPS_AUTO_OOB) {
+		doc_fill_autooob(docg3->oob_write_buf, ops->oobbuf);
+		ops->oobretlen = 8;
+	} else {
+		memcpy(docg3->oob_write_buf, ops->oobbuf, DOC_LAYOUT_OOB_SIZE);
+		ops->oobretlen = DOC_LAYOUT_OOB_SIZE;
+	}
+	return 0;
+}
+
+/**
+ * doc_write_oob - Write out of band bytes to flash
+ * @mtd: the device
+ * @ofs: the offset from first block and first page, in bytes, aligned on page
+ *       size
+ * @ops: the mtd oob structure
+ *
+ * Either write OOB data into a temporary buffer, for the subsequent write
+ * page. The provided OOB should be 16 bytes long. If a data buffer is provided
+ * as well, issue the page write.
+ * Or provide data without OOB, and then a all zeroed OOB will be used (ECC will
+ * still be filled in if asked for).
+ *
+ * Returns 0 is successfull, EINVAL if length is not 14 bytes
+ */
+static int doc_write_oob(struct mtd_info *mtd, loff_t ofs,
+			 struct mtd_oob_ops *ops)
+{
+	struct docg3 *docg3 = mtd->priv;
+	int block0, block1, page, ret, pofs = 0, autoecc, oobdelta;
+	u8 *oobbuf = ops->oobbuf;
+	u8 *buf = ops->datbuf;
+	size_t len, ooblen;
+	u8 oob[DOC_LAYOUT_OOB_SIZE];
+
+	if (buf)
+		len = ops->len;
+	else
+		len = 0;
+	if (oobbuf)
+		ooblen = ops->ooblen;
+	else
+		ooblen = 0;
+
+	if (oobbuf && ops->mode == MTD_OPS_PLACE_OOB)
+		oobbuf += ops->ooboffs;
+
+	doc_dbg("doc_write_oob(from=%lld, mode=%d, data=(%p:%zu), oob=(%p:%zu))\n",
+		ofs, ops->mode, buf, len, oobbuf, ooblen);
+	switch (ops->mode) {
+	case MTD_OPS_PLACE_OOB:
+	case MTD_OPS_RAW:
+		oobdelta = mtd->oobsize;
+		break;
+	case MTD_OPS_AUTO_OOB:
+		oobdelta = mtd->ecclayout->oobavail;
+		break;
+	default:
+		oobdelta = 0;
+	}
+	if ((len % DOC_LAYOUT_PAGE_SIZE) || (ooblen % oobdelta) ||
+	    (ofs % DOC_LAYOUT_PAGE_SIZE))
+		return -EINVAL;
+	if (len && ooblen &&
+	    (len / DOC_LAYOUT_PAGE_SIZE) != (ooblen / oobdelta))
+		return -EINVAL;
+
+	ret = -EINVAL;
+	calc_block_sector(ofs + len, &block0, &block1, &page, &pofs,
+			  docg3->reliable);
+	if (block1 > docg3->max_block)
+		goto err;
+
+	ops->oobretlen = 0;
+	ops->retlen = 0;
+	ret = 0;
+	if (len == 0 && ooblen == 0)
+		return -EINVAL;
+	if (len == 0 && ooblen > 0)
+		return doc_backup_oob(docg3, ofs, ops);
+
+	autoecc = doc_guess_autoecc(ops);
+	if (autoecc < 0)
+		return autoecc;
+
+	while (!ret && len > 0) {
+		memset(oob, 0, sizeof(oob));
+		if (ofs == docg3->oob_write_ofs)
+			memcpy(oob, docg3->oob_write_buf, DOC_LAYOUT_OOB_SIZE);
+		else if (ooblen > 0 && ops->mode == MTD_OPS_AUTO_OOB)
+			doc_fill_autooob(oob, oobbuf);
+		else if (ooblen > 0)
+			memcpy(oob, oobbuf, DOC_LAYOUT_OOB_SIZE);
+		ret = doc_write_page(docg3, ofs, buf, oob, autoecc);
+
+		ofs += DOC_LAYOUT_PAGE_SIZE;
+		len -= DOC_LAYOUT_PAGE_SIZE;
+		buf += DOC_LAYOUT_PAGE_SIZE;
+		if (ooblen) {
+			oobbuf += oobdelta;
+			ooblen -= oobdelta;
+			ops->oobretlen += oobdelta;
+		}
+		ops->retlen += DOC_LAYOUT_PAGE_SIZE;
+	}
+err:
+	doc_set_device_id(docg3, 0);
+	return ret;
+}
+
+/**
+ * doc_write - Write a buffer to the chip
+ * @mtd: the device
+ * @to: the offset from first block and first page, in bytes, aligned on page
+ *      size
+ * @len: the number of bytes to write (must be a full page size, ie. 512)
+ * @retlen: the number of bytes actually written (0 or 512)
+ * @buf: the buffer to get bytes from
+ *
+ * Writes data to the chip.
+ *
+ * Returns 0 if write successful, -EIO if write error
+ */
+static int doc_write(struct mtd_info *mtd, loff_t to, size_t len,
+		     size_t *retlen, const u_char *buf)
+{
+	struct docg3 *docg3 = mtd->priv;
+	int ret;
+	struct mtd_oob_ops ops;
+
+	doc_dbg("doc_write(to=%lld, len=%zu)\n", to, len);
+	ops.datbuf = (char *)buf;
+	ops.len = len;
+	ops.mode = MTD_OPS_PLACE_OOB;
+	ops.oobbuf = NULL;
+	ops.ooblen = 0;
+	ops.ooboffs = 0;
+
+	ret = doc_write_oob(mtd, to, &ops);
+	*retlen = ops.retlen;
+	return ret;
+}
+
+static struct docg3 *sysfs_dev2docg3(struct device *dev,
+				     struct device_attribute *attr)
+{
+	int floor;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct mtd_info **docg3_floors = platform_get_drvdata(pdev);
+
+	floor = attr->attr.name[1] - '0';
+	if (floor < 0 || floor >= DOC_MAX_NBFLOORS)
+		return NULL;
+	else
+		return docg3_floors[floor]->priv;
+}
+
+static ssize_t dps0_is_key_locked(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct docg3 *docg3 = sysfs_dev2docg3(dev, attr);
+	int dps0;
+
+	doc_set_device_id(docg3, docg3->device_id);
+	dps0 = doc_register_readb(docg3, DOC_DPS0_STATUS);
+	doc_set_device_id(docg3, 0);
+
+	return sprintf(buf, "%d\n", !(dps0 & DOC_DPS_KEY_OK));
+}
+
+static ssize_t dps1_is_key_locked(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct docg3 *docg3 = sysfs_dev2docg3(dev, attr);
+	int dps1;
+
+	doc_set_device_id(docg3, docg3->device_id);
+	dps1 = doc_register_readb(docg3, DOC_DPS1_STATUS);
+	doc_set_device_id(docg3, 0);
+
+	return sprintf(buf, "%d\n", !(dps1 & DOC_DPS_KEY_OK));
+}
+
+static ssize_t dps0_insert_key(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct docg3 *docg3 = sysfs_dev2docg3(dev, attr);
+	int i;
+
+	if (count != DOC_LAYOUT_DPS_KEY_LENGTH)
+		return -EINVAL;
+
+	doc_set_device_id(docg3, docg3->device_id);
+	for (i = 0; i < DOC_LAYOUT_DPS_KEY_LENGTH; i++)
+		doc_writeb(docg3, buf[i], DOC_DPS0_KEY);
+	doc_set_device_id(docg3, 0);
+	return count;
+}
+
+static ssize_t dps1_insert_key(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct docg3 *docg3 = sysfs_dev2docg3(dev, attr);
+	int i;
+
+	if (count != DOC_LAYOUT_DPS_KEY_LENGTH)
+		return -EINVAL;
+
+	doc_set_device_id(docg3, docg3->device_id);
+	for (i = 0; i < DOC_LAYOUT_DPS_KEY_LENGTH; i++)
+		doc_writeb(docg3, buf[i], DOC_DPS1_KEY);
+	doc_set_device_id(docg3, 0);
+	return count;
+}
+
+#define FLOOR_SYSFS(id) { \
+	__ATTR(f##id##_dps0_is_keylocked, S_IRUGO, dps0_is_key_locked, NULL), \
+	__ATTR(f##id##_dps1_is_keylocked, S_IRUGO, dps1_is_key_locked, NULL), \
+	__ATTR(f##id##_dps0_protection_key, S_IWUGO, NULL, dps0_insert_key), \
+	__ATTR(f##id##_dps1_protection_key, S_IWUGO, NULL, dps1_insert_key), \
+}
+
+static struct device_attribute doc_sys_attrs[DOC_MAX_NBFLOORS][4] = {
+	FLOOR_SYSFS(0), FLOOR_SYSFS(1), FLOOR_SYSFS(2), FLOOR_SYSFS(3)
+};
+
+static int doc_register_sysfs(struct platform_device *pdev,
+			      struct mtd_info **floors)
+{
+	int ret = 0, floor, i = 0;
+	struct device *dev = &pdev->dev;
+
+	for (floor = 0; !ret && floor < DOC_MAX_NBFLOORS && floors[floor];
+	     floor++)
+		for (i = 0; !ret && i < 4; i++)
+			ret = device_create_file(dev, &doc_sys_attrs[floor][i]);
+	if (!ret)
+		return 0;
+	do {
+		while (--i >= 0)
+			device_remove_file(dev, &doc_sys_attrs[floor][i]);
+		i = 4;
+	} while (--floor >= 0);
+	return ret;
+}
+
+static void doc_unregister_sysfs(struct platform_device *pdev,
+				 struct mtd_info **floors)
+{
+	struct device *dev = &pdev->dev;
+	int floor, i;
+
+	for (floor = 0; floor < DOC_MAX_NBFLOORS && floors[floor];
+	     floor++)
+		for (i = 0; i < 4; i++)
+			device_remove_file(dev, &doc_sys_attrs[floor][i]);
+}
 
 /*
  * Debug sysfs entries
@@ -852,13 +1703,15 @@ static int dbg_protection_show(struct seq_file *s, void *p)
 {
 	struct docg3 *docg3 = (struct docg3 *)s->private;
 	int pos = 0;
-	int protect = doc_register_readb(docg3, DOC_PROTECTION);
-	int dps0 = doc_register_readb(docg3, DOC_DPS0_STATUS);
-	int dps0_low = doc_register_readb(docg3, DOC_DPS0_ADDRLOW);
-	int dps0_high = doc_register_readb(docg3, DOC_DPS0_ADDRHIGH);
-	int dps1 = doc_register_readb(docg3, DOC_DPS1_STATUS);
-	int dps1_low = doc_register_readb(docg3, DOC_DPS1_ADDRLOW);
-	int dps1_high = doc_register_readb(docg3, DOC_DPS1_ADDRHIGH);
+	int protect, dps0, dps0_low, dps0_high, dps1, dps1_low, dps1_high;
+
+	protect = doc_register_readb(docg3, DOC_PROTECTION);
+	dps0 = doc_register_readb(docg3, DOC_DPS0_STATUS);
+	dps0_low = doc_register_readw(docg3, DOC_DPS0_ADDRLOW);
+	dps0_high = doc_register_readw(docg3, DOC_DPS0_ADDRHIGH);
+	dps1 = doc_register_readb(docg3, DOC_DPS1_STATUS);
+	dps1_low = doc_register_readw(docg3, DOC_DPS1_ADDRLOW);
+	dps1_high = doc_register_readw(docg3, DOC_DPS1_ADDRHIGH);
 
 	pos += seq_printf(s, "Protection = 0x%02x (",
 			 protect);
@@ -947,52 +1800,54 @@ static void __init doc_set_driver_info(int chip_id, struct mtd_info *mtd)
 
 	cfg = doc_register_readb(docg3, DOC_CONFIGURATION);
 	docg3->if_cfg = (cfg & DOC_CONF_IF_CFG ? 1 : 0);
+	docg3->reliable = reliable_mode;
 
 	switch (chip_id) {
 	case DOC_CHIPID_G3:
-		mtd->name = "DiskOnChip G3";
+		mtd->name = kasprintf(GFP_KERNEL, "DiskOnChip G3 floor %d",
+				      docg3->device_id);
 		docg3->max_block = 2047;
 		break;
 	}
 	mtd->type = MTD_NANDFLASH;
-	/*
-	 * Once write methods are added, the correct flags will be set.
-	 * mtd->flags = MTD_CAP_NANDFLASH;
-	 */
-	mtd->flags = MTD_CAP_ROM;
+	mtd->flags = MTD_CAP_NANDFLASH;
 	mtd->size = (docg3->max_block + 1) * DOC_LAYOUT_BLOCK_SIZE;
+	if (docg3->reliable == 2)
+		mtd->size /= 2;
 	mtd->erasesize = DOC_LAYOUT_BLOCK_SIZE * DOC_LAYOUT_NBPLANES;
+	if (docg3->reliable == 2)
+		mtd->erasesize /= 2;
 	mtd->writesize = DOC_LAYOUT_PAGE_SIZE;
 	mtd->oobsize = DOC_LAYOUT_OOB_SIZE;
 	mtd->owner = THIS_MODULE;
-	mtd->erase = NULL;
-	mtd->point = NULL;
-	mtd->unpoint = NULL;
+	mtd->erase = doc_erase;
 	mtd->read = doc_read;
-	mtd->write = NULL;
+	mtd->write = doc_write;
 	mtd->read_oob = doc_read_oob;
-	mtd->write_oob = NULL;
-	mtd->sync = NULL;
+	mtd->write_oob = doc_write_oob;
 	mtd->block_isbad = doc_block_isbad;
+	mtd->ecclayout = &docg3_oobinfo;
 }
 
 /**
- * doc_probe - Probe the IO space for a DiskOnChip G3 chip
- * @pdev: platform device
+ * doc_probe_device - Check if a device is available
+ * @base: the io space where the device is probed
+ * @floor: the floor of the probed device
+ * @dev: the device
  *
- * Probes for a G3 chip at the specified IO space in the platform data
- * ressources.
+ * Checks whether a device at the specified IO range, and floor is available.
  *
- * Returns 0 on success, -ENOMEM, -ENXIO on error
+ * Returns a mtd_info struct if there is a device, ENODEV if none found, ENOMEM
+ * if a memory allocation failed. If floor 0 is checked, a reset of the ASIC is
+ * launched.
  */
-static int __init docg3_probe(struct platform_device *pdev)
+static struct mtd_info *doc_probe_device(void __iomem *base, int floor,
+					 struct device *dev)
 {
-	struct device *dev = &pdev->dev;
-	struct docg3 *docg3;
-	struct mtd_info *mtd;
-	struct resource *ress;
 	int ret, bbt_nbpages;
 	u16 chip_id, chip_id_inv;
+	struct docg3 *docg3;
+	struct mtd_info *mtd;
 
 	ret = -ENOMEM;
 	docg3 = kzalloc(sizeof(struct docg3), GFP_KERNEL);
@@ -1002,69 +1857,218 @@ static int __init docg3_probe(struct platform_device *pdev)
 	if (!mtd)
 		goto nomem2;
 	mtd->priv = docg3;
+	bbt_nbpages = DIV_ROUND_UP(docg3->max_block + 1,
+				   8 * DOC_LAYOUT_PAGE_SIZE);
+	docg3->bbt = kzalloc(bbt_nbpages * DOC_LAYOUT_PAGE_SIZE, GFP_KERNEL);
+	if (!docg3->bbt)
+		goto nomem3;
 
-	ret = -ENXIO;
-	ress = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!ress) {
-		dev_err(dev, "No I/O memory resource defined\n");
-		goto noress;
-	}
-	docg3->base = ioremap(ress->start, DOC_IOSPACE_SIZE);
-
-	docg3->dev = &pdev->dev;
-	docg3->device_id = 0;
+	docg3->dev = dev;
+	docg3->device_id = floor;
+	docg3->base = base;
 	doc_set_device_id(docg3, docg3->device_id);
-	doc_set_asic_mode(docg3, DOC_ASICMODE_RESET);
+	if (!floor)
+		doc_set_asic_mode(docg3, DOC_ASICMODE_RESET);
 	doc_set_asic_mode(docg3, DOC_ASICMODE_NORMAL);
 
 	chip_id = doc_register_readw(docg3, DOC_CHIPID);
 	chip_id_inv = doc_register_readw(docg3, DOC_CHIPID_INV);
 
-	ret = -ENODEV;
+	ret = 0;
 	if (chip_id != (u16)(~chip_id_inv)) {
-		doc_info("No device found at IO addr %p\n",
-			 (void *)ress->start);
-		goto nochipfound;
+		goto nomem3;
 	}
 
 	switch (chip_id) {
 	case DOC_CHIPID_G3:
-		doc_info("Found a G3 DiskOnChip at addr %p\n",
-			 (void *)ress->start);
+		doc_info("Found a G3 DiskOnChip at addr %p, floor %d\n",
+			 base, floor);
 		break;
 	default:
 		doc_err("Chip id %04x is not a DiskOnChip G3 chip\n", chip_id);
-		goto nochipfound;
+		goto nomem3;
 	}
 
 	doc_set_driver_info(chip_id, mtd);
-	platform_set_drvdata(pdev, mtd);
 
-	ret = -ENOMEM;
-	bbt_nbpages = DIV_ROUND_UP(docg3->max_block + 1,
-				   8 * DOC_LAYOUT_PAGE_SIZE);
-	docg3->bbt = kzalloc(bbt_nbpages * DOC_LAYOUT_PAGE_SIZE, GFP_KERNEL);
-	if (!docg3->bbt)
-		goto nochipfound;
+	doc_hamming_ecc_init(docg3, DOC_LAYOUT_OOB_PAGEINFO_SZ);
 	doc_reload_bbt(docg3);
+	return mtd;
 
-	ret = mtd_device_parse_register(mtd, part_probes,
-					NULL, NULL, 0);
-	if (ret)
-		goto register_error;
+nomem3:
+	kfree(mtd);
+nomem2:
+	kfree(docg3);
+nomem1:
+	return ERR_PTR(ret);
+}
 
-	doc_dbg_register(docg3);
-	return 0;
+/**
+ * doc_release_device - Release a docg3 floor
+ * @mtd: the device
+ */
+static void doc_release_device(struct mtd_info *mtd)
+{
+	struct docg3 *docg3 = mtd->priv;
 
-register_error:
+	mtd_device_unregister(mtd);
 	kfree(docg3->bbt);
-nochipfound:
-	iounmap(docg3->base);
-noress:
+	kfree(docg3);
+	kfree(mtd->name);
 	kfree(mtd);
+}
+
+/**
+ * docg3_resume - Awakens docg3 floor
+ * @pdev: platfrom device
+ *
+ * Returns 0 (always successfull)
+ */
+static int docg3_resume(struct platform_device *pdev)
+{
+	int i;
+	struct mtd_info **docg3_floors, *mtd;
+	struct docg3 *docg3;
+
+	docg3_floors = platform_get_drvdata(pdev);
+	mtd = docg3_floors[0];
+	docg3 = mtd->priv;
+
+	doc_dbg("docg3_resume()\n");
+	for (i = 0; i < 12; i++)
+		doc_readb(docg3, DOC_IOSPACE_IPL);
+	return 0;
+}
+
+/**
+ * docg3_suspend - Put in low power mode the docg3 floor
+ * @pdev: platform device
+ * @state: power state
+ *
+ * Shuts off most of docg3 circuitery to lower power consumption.
+ *
+ * Returns 0 if suspend succeeded, -EIO if chip refused suspend
+ */
+static int docg3_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int floor, i;
+	struct mtd_info **docg3_floors, *mtd;
+	struct docg3 *docg3;
+	u8 ctrl, pwr_down;
+
+	docg3_floors = platform_get_drvdata(pdev);
+	for (floor = 0; floor < DOC_MAX_NBFLOORS; floor++) {
+		mtd = docg3_floors[floor];
+		if (!mtd)
+			continue;
+		docg3 = mtd->priv;
+
+		doc_writeb(docg3, floor, DOC_DEVICESELECT);
+		ctrl = doc_register_readb(docg3, DOC_FLASHCONTROL);
+		ctrl &= ~DOC_CTRL_VIOLATION & ~DOC_CTRL_CE;
+		doc_writeb(docg3, ctrl, DOC_FLASHCONTROL);
+
+		for (i = 0; i < 10; i++) {
+			usleep_range(3000, 4000);
+			pwr_down = doc_register_readb(docg3, DOC_POWERMODE);
+			if (pwr_down & DOC_POWERDOWN_READY)
+				break;
+		}
+		if (pwr_down & DOC_POWERDOWN_READY) {
+			doc_dbg("docg3_suspend(): floor %d powerdown ok\n",
+				floor);
+		} else {
+			doc_err("docg3_suspend(): floor %d powerdown failed\n",
+				floor);
+			return -EIO;
+		}
+	}
+
+	mtd = docg3_floors[0];
+	docg3 = mtd->priv;
+	doc_set_asic_mode(docg3, DOC_ASICMODE_POWERDOWN);
+	return 0;
+}
+
+/**
+ * doc_probe - Probe the IO space for a DiskOnChip G3 chip
+ * @pdev: platform device
+ *
+ * Probes for a G3 chip at the specified IO space in the platform data
+ * ressources. The floor 0 must be available.
+ *
+ * Returns 0 on success, -ENOMEM, -ENXIO on error
+ */
+static int __init docg3_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mtd_info *mtd;
+	struct resource *ress;
+	void __iomem *base;
+	int ret, floor, found = 0;
+	struct mtd_info **docg3_floors;
+
+	ret = -ENXIO;
+	ress = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!ress) {
+		dev_err(dev, "No I/O memory resource defined\n");
+		goto noress;
+	}
+	base = ioremap(ress->start, DOC_IOSPACE_SIZE);
+
+	ret = -ENOMEM;
+	docg3_floors = kzalloc(sizeof(*docg3_floors) * DOC_MAX_NBFLOORS,
+			       GFP_KERNEL);
+	if (!docg3_floors)
+		goto nomem1;
+	docg3_bch = init_bch(DOC_ECC_BCH_M, DOC_ECC_BCH_T,
+			     DOC_ECC_BCH_PRIMPOLY);
+	if (!docg3_bch)
+		goto nomem2;
+
+	for (floor = 0; floor < DOC_MAX_NBFLOORS; floor++) {
+		mtd = doc_probe_device(base, floor, dev);
+		if (IS_ERR(mtd)) {
+			ret = PTR_ERR(mtd);
+			goto err_probe;
+		}
+		if (!mtd) {
+			if (floor == 0)
+				goto notfound;
+			else
+				continue;
+		}
+		docg3_floors[floor] = mtd;
+		ret = mtd_device_parse_register(mtd, part_probes, NULL, NULL,
+						0);
+		if (ret)
+			goto err_probe;
+		found++;
+	}
+
+	ret = doc_register_sysfs(pdev, docg3_floors);
+	if (ret)
+		goto err_probe;
+	if (!found)
+		goto notfound;
+
+	platform_set_drvdata(pdev, docg3_floors);
+	doc_dbg_register(docg3_floors[0]->priv);
+	return 0;
+
+notfound:
+	ret = -ENODEV;
+	dev_info(dev, "No supported DiskOnChip found\n");
+err_probe:
+	free_bch(docg3_bch);
+	for (floor = 0; floor < DOC_MAX_NBFLOORS; floor++)
+		if (docg3_floors[floor])
+			doc_release_device(docg3_floors[floor]);
 nomem2:
-	kfree(docg3);
+	kfree(docg3_floors);
 nomem1:
+	iounmap(base);
+noress:
 	return ret;
 }
 
@@ -1076,15 +2080,20 @@ nomem1:
  */
 static int __exit docg3_release(struct platform_device *pdev)
 {
-	struct mtd_info *mtd = platform_get_drvdata(pdev);
-	struct docg3 *docg3 = mtd->priv;
+	struct mtd_info **docg3_floors = platform_get_drvdata(pdev);
+	struct docg3 *docg3 = docg3_floors[0]->priv;
+	void __iomem *base = docg3->base;
+	int floor;
 
+	doc_unregister_sysfs(pdev, docg3_floors);
 	doc_dbg_unregister(docg3);
-	mtd_device_unregister(mtd);
-	iounmap(docg3->base);
-	kfree(docg3->bbt);
-	kfree(docg3);
-	kfree(mtd);
+	for (floor = 0; floor < DOC_MAX_NBFLOORS; floor++)
+		if (docg3_floors[floor])
+			doc_release_device(docg3_floors[floor]);
+
+	kfree(docg3_floors);
+	free_bch(docg3_bch);
+	iounmap(base);
 	return 0;
 }
 
@@ -1093,6 +2102,8 @@ static struct platform_driver g3_driver = {
 		.name	= "docg3",
 		.owner	= THIS_MODULE,
 	},
+	.suspend	= docg3_suspend,
+	.resume		= docg3_resume,
 	.remove		= __exit_p(docg3_release),
 };
 
diff --git a/drivers/mtd/devices/docg3.h b/drivers/mtd/devices/docg3.h
index 0d407be24594..db0da436b493 100644
--- a/drivers/mtd/devices/docg3.h
+++ b/drivers/mtd/devices/docg3.h
@@ -51,10 +51,19 @@
 #define DOC_LAYOUT_WEAR_OFFSET		(DOC_LAYOUT_PAGE_OOB_SIZE * 2)
 #define DOC_LAYOUT_BLOCK_SIZE					\
 	(DOC_LAYOUT_PAGES_PER_BLOCK * DOC_LAYOUT_PAGE_SIZE)
+
+/*
+ * ECC related constants
+ */
+#define DOC_ECC_BCH_M			14
+#define DOC_ECC_BCH_T			4
+#define DOC_ECC_BCH_PRIMPOLY		0x4443
 #define DOC_ECC_BCH_SIZE		7
 #define DOC_ECC_BCH_COVERED_BYTES				\
 	(DOC_LAYOUT_PAGE_SIZE + DOC_LAYOUT_OOB_PAGEINFO_SZ +	\
-	 DOC_LAYOUT_OOB_HAMMING_SZ + DOC_LAYOUT_OOB_BCH_SZ)
+	 DOC_LAYOUT_OOB_HAMMING_SZ)
+#define DOC_ECC_BCH_TOTAL_BYTES					\
+	(DOC_ECC_BCH_COVERED_BYTES + DOC_LAYOUT_OOB_BCH_SZ)
 
 /*
  * Blocks distribution
@@ -80,6 +89,7 @@
 
 #define DOC_CHIPID_G3			0x200
 #define DOC_ERASE_MARK			0xaa
+#define DOC_MAX_NBFLOORS		4
 /*
  * Flash registers
  */
@@ -105,9 +115,11 @@
 #define DOC_ECCCONF1			0x1042
 #define DOC_ECCPRESET			0x1044
 #define DOC_HAMMINGPARITY		0x1046
-#define DOC_BCH_SYNDROM(idx)		(0x1048 + (idx << 1))
+#define DOC_BCH_HW_ECC(idx)		(0x1048 + idx)
 
 #define DOC_PROTECTION			0x1056
+#define DOC_DPS0_KEY			0x105c
+#define DOC_DPS1_KEY			0x105e
 #define DOC_DPS0_ADDRLOW		0x1060
 #define DOC_DPS0_ADDRHIGH		0x1062
 #define DOC_DPS1_ADDRLOW		0x1064
@@ -117,6 +129,7 @@
 
 #define DOC_ASICMODECONFIRM		0x1072
 #define DOC_CHIPID_INV			0x1074
+#define DOC_POWERMODE			0x107c
 
 /*
  * Flash sequences
@@ -124,11 +137,14 @@
  */
 #define DOC_SEQ_RESET			0x00
 #define DOC_SEQ_PAGE_SIZE_532		0x03
-#define DOC_SEQ_SET_MODE		0x09
+#define DOC_SEQ_SET_FASTMODE		0x05
+#define DOC_SEQ_SET_RELIABLEMODE	0x09
 #define DOC_SEQ_READ			0x12
 #define DOC_SEQ_SET_PLANE1		0x0e
 #define DOC_SEQ_SET_PLANE2		0x10
 #define DOC_SEQ_PAGE_SETUP		0x1d
+#define DOC_SEQ_ERASE			0x27
+#define DOC_SEQ_PLANES_STATUS		0x31
 
 /*
  * Flash commands
@@ -143,7 +159,10 @@
 #define DOC_CMD_PROG_BLOCK_ADDR		0x60
 #define DOC_CMD_PROG_CYCLE1		0x80
 #define DOC_CMD_PROG_CYCLE2		0x10
+#define DOC_CMD_PROG_CYCLE3		0x11
 #define DOC_CMD_ERASECYCLE2		0xd0
+#define DOC_CMD_READ_STATUS		0x70
+#define DOC_CMD_PLANES_STATUS		0x71
 
 #define DOC_CMD_RELIABLE_MODE		0x22
 #define DOC_CMD_FAST_MODE		0xa2
@@ -174,6 +193,7 @@
 /*
  * Flash register : DOC_ECCCONF0
  */
+#define DOC_ECCCONF0_WRITE_MODE		0x0000
 #define DOC_ECCCONF0_READ_MODE		0x8000
 #define DOC_ECCCONF0_AUTO_ECC_ENABLE	0x4000
 #define DOC_ECCCONF0_HAMMING_ENABLE	0x1000
@@ -185,7 +205,7 @@
  */
 #define DOC_ECCCONF1_BCH_SYNDROM_ERR	0x80
 #define DOC_ECCCONF1_UNKOWN1		0x40
-#define DOC_ECCCONF1_UNKOWN2		0x20
+#define DOC_ECCCONF1_PAGE_IS_WRITTEN	0x20
 #define DOC_ECCCONF1_UNKOWN3		0x10
 #define DOC_ECCCONF1_HAMMING_BITS_MASK	0x0f
 
@@ -223,13 +243,46 @@
 #define DOC_READADDR_ONE_BYTE		0x4000
 #define DOC_READADDR_ADDR_MASK		0x1fff
 
+/*
+ * Flash register : DOC_POWERMODE
+ */
+#define DOC_POWERDOWN_READY		0x80
+
+/*
+ * Status of erase and write operation
+ */
+#define DOC_PLANES_STATUS_FAIL		0x01
+#define DOC_PLANES_STATUS_PLANE0_KO	0x02
+#define DOC_PLANES_STATUS_PLANE1_KO	0x04
+
+/*
+ * DPS key management
+ *
+ * Each floor of docg3 has 2 protection areas: DPS0 and DPS1. These areas span
+ * across block boundaries, and define whether these blocks can be read or
+ * written.
+ * The definition is dynamically stored in page 0 of blocks (2,3) for DPS0, and
+ * page 0 of blocks (4,5) for DPS1.
+ */
+#define DOC_LAYOUT_DPS_KEY_LENGTH	8
+
 /**
  * struct docg3 - DiskOnChip driver private data
  * @dev: the device currently under control
  * @base: mapped IO space
  * @device_id: number of the cascaded DoCG3 device (0, 1, 2 or 3)
  * @if_cfg: if true, reads are on 16bits, else reads are on 8bits
+
+ * @reliable: if 0, docg3 in normal mode, if 1 docg3 in fast mode, if 2 in
+ *            reliable mode
+ *            Fast mode implies more errors than normal mode.
+ *            Reliable mode implies that page 2*n and 2*n+1 are clones.
  * @bbt: bad block table cache
+ * @oob_write_ofs: offset of the MTD where this OOB should belong (ie. in next
+ *                 page_write)
+ * @oob_autoecc: if 1, use only bytes 0-7, 15, and fill the others with HW ECC
+ *               if 0, use all the 16 bytes.
+ * @oob_write_buf: prepared OOB for next page_write
  * @debugfs_root: debugfs root node
  */
 struct docg3 {
@@ -237,8 +290,12 @@ struct docg3 {
 	void __iomem *base;
 	unsigned int device_id:4;
 	unsigned int if_cfg:1;
+	unsigned int reliable:2;
 	int max_block;
 	u8 *bbt;
+	loff_t oob_write_ofs;
+	int oob_autoecc;
+	u8 oob_write_buf[DOC_LAYOUT_OOB_SIZE];
 	struct dentry *debugfs_root;
 };
 
diff --git a/drivers/mtd/devices/docprobe.c b/drivers/mtd/devices/docprobe.c
index 45116bb30297..706b847b46b3 100644
--- a/drivers/mtd/devices/docprobe.c
+++ b/drivers/mtd/devices/docprobe.c
@@ -241,8 +241,7 @@ static void __init DoC_Probe(unsigned long physadr)
 			return;
 		}
 		docfound = 1;
-		mtd = kmalloc(sizeof(struct DiskOnChip) + sizeof(struct mtd_info), GFP_KERNEL);
-
+		mtd = kzalloc(sizeof(struct DiskOnChip) + sizeof(struct mtd_info), GFP_KERNEL);
 		if (!mtd) {
 			printk(KERN_WARNING "Cannot allocate memory for data structures. Dropping.\n");
 			iounmap(docptr);
@@ -250,10 +249,6 @@ static void __init DoC_Probe(unsigned long physadr)
 		}
 
 		this = (struct DiskOnChip *)(&mtd[1]);
-
-		memset((char *)mtd,0, sizeof(struct mtd_info));
-		memset((char *)this, 0, sizeof(struct DiskOnChip));
-
 		mtd->priv = this;
 		this->virtadr = docptr;
 		this->physadr = physadr;
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 884904d3f9d2..7c60dddbefc0 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -992,7 +992,6 @@ static int __devexit m25p_remove(struct spi_device *spi)
 static struct spi_driver m25p80_driver = {
 	.driver = {
 		.name	= "m25p80",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.id_table	= m25p_ids,
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index d75c7af18a63..236057ead0d2 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -936,7 +936,6 @@ static int __devexit dataflash_remove(struct spi_device *spi)
 static struct spi_driver dataflash_driver = {
 	.driver = {
 		.name		= "mtd_dataflash",
-		.bus		= &spi_bus_type,
 		.owner		= THIS_MODULE,
 		.of_match_table = dataflash_dt_ids,
 	},
diff --git a/drivers/mtd/devices/sst25l.c b/drivers/mtd/devices/sst25l.c
index d38ef3bffe8d..5fc198350b94 100644
--- a/drivers/mtd/devices/sst25l.c
+++ b/drivers/mtd/devices/sst25l.c
@@ -378,7 +378,7 @@ static int __devinit sst25l_probe(struct spi_device *spi)
 	struct flash_info *flash_info;
 	struct sst25l_flash *flash;
 	struct flash_platform_data *data;
-	int ret, i;
+	int ret;
 
 	flash_info = sst25l_match_device(spi);
 	if (!flash_info)
@@ -444,7 +444,6 @@ static int __devexit sst25l_remove(struct spi_device *spi)
 static struct spi_driver sst25l_driver = {
 	.driver = {
 		.name	= "sst25l",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= sst25l_probe,
diff --git a/drivers/mtd/ftl.c b/drivers/mtd/ftl.c
index c7382bb686c6..19d637266fcd 100644
--- a/drivers/mtd/ftl.c
+++ b/drivers/mtd/ftl.c
@@ -168,8 +168,8 @@ static int scan_header(partition_t *part)
 	 (offset + sizeof(header)) < max_offset;
 	 offset += part->mbd.mtd->erasesize ? : 0x2000) {
 
-	err = part->mbd.mtd->read(part->mbd.mtd, offset, sizeof(header), &ret,
-			      (unsigned char *)&header);
+	err = mtd_read(part->mbd.mtd, offset, sizeof(header), &ret,
+                       (unsigned char *)&header);
 
 	if (err)
 	    return err;
@@ -224,8 +224,8 @@ static int build_maps(partition_t *part)
     for (i = 0; i < le16_to_cpu(part->header.NumEraseUnits); i++) {
 	offset = ((i + le16_to_cpu(part->header.FirstPhysicalEUN))
 		      << part->header.EraseUnitSize);
-	ret = part->mbd.mtd->read(part->mbd.mtd, offset, sizeof(header), &retval,
-			      (unsigned char *)&header);
+	ret = mtd_read(part->mbd.mtd, offset, sizeof(header), &retval,
+                       (unsigned char *)&header);
 
 	if (ret)
 	    goto out_XferInfo;
@@ -289,9 +289,9 @@ static int build_maps(partition_t *part)
 	part->EUNInfo[i].Deleted = 0;
 	offset = part->EUNInfo[i].Offset + le32_to_cpu(header.BAMOffset);
 
-	ret = part->mbd.mtd->read(part->mbd.mtd, offset,
-			      part->BlocksPerUnit * sizeof(uint32_t), &retval,
-			      (unsigned char *)part->bam_cache);
+	ret = mtd_read(part->mbd.mtd, offset,
+                       part->BlocksPerUnit * sizeof(uint32_t), &retval,
+                       (unsigned char *)part->bam_cache);
 
 	if (ret)
 		goto out_bam_cache;
@@ -355,7 +355,7 @@ static int erase_xfer(partition_t *part,
     erase->len = 1 << part->header.EraseUnitSize;
     erase->priv = (u_long)part;
 
-    ret = part->mbd.mtd->erase(part->mbd.mtd, erase);
+    ret = mtd_erase(part->mbd.mtd, erase);
 
     if (!ret)
 	    xfer->EraseCount++;
@@ -422,8 +422,8 @@ static int prepare_xfer(partition_t *part, int i)
     header.LogicalEUN = cpu_to_le16(0xffff);
     header.EraseCount = cpu_to_le32(xfer->EraseCount);
 
-    ret = part->mbd.mtd->write(part->mbd.mtd, xfer->Offset, sizeof(header),
-			   &retlen, (u_char *)&header);
+    ret = mtd_write(part->mbd.mtd, xfer->Offset, sizeof(header), &retlen,
+                    (u_char *)&header);
 
     if (ret) {
 	return ret;
@@ -438,8 +438,8 @@ static int prepare_xfer(partition_t *part, int i)
 
     for (i = 0; i < nbam; i++, offset += sizeof(uint32_t)) {
 
-	ret = part->mbd.mtd->write(part->mbd.mtd, offset, sizeof(uint32_t),
-			       &retlen, (u_char *)&ctl);
+	ret = mtd_write(part->mbd.mtd, offset, sizeof(uint32_t), &retlen,
+                        (u_char *)&ctl);
 
 	if (ret)
 	    return ret;
@@ -485,9 +485,9 @@ static int copy_erase_unit(partition_t *part, uint16_t srcunit,
 
 	offset = eun->Offset + le32_to_cpu(part->header.BAMOffset);
 
-	ret = part->mbd.mtd->read(part->mbd.mtd, offset,
-			      part->BlocksPerUnit * sizeof(uint32_t),
-			      &retlen, (u_char *) (part->bam_cache));
+	ret = mtd_read(part->mbd.mtd, offset,
+                       part->BlocksPerUnit * sizeof(uint32_t), &retlen,
+                       (u_char *)(part->bam_cache));
 
 	/* mark the cache bad, in case we get an error later */
 	part->bam_index = 0xffff;
@@ -503,8 +503,8 @@ static int copy_erase_unit(partition_t *part, uint16_t srcunit,
     offset = xfer->Offset + 20; /* Bad! */
     unit = cpu_to_le16(0x7fff);
 
-    ret = part->mbd.mtd->write(part->mbd.mtd, offset, sizeof(uint16_t),
-			   &retlen, (u_char *) &unit);
+    ret = mtd_write(part->mbd.mtd, offset, sizeof(uint16_t), &retlen,
+                    (u_char *)&unit);
 
     if (ret) {
 	printk( KERN_WARNING "ftl: Failed to write back to BAM cache in copy_erase_unit()!\n");
@@ -523,16 +523,16 @@ static int copy_erase_unit(partition_t *part, uint16_t srcunit,
 	    break;
 	case BLOCK_DATA:
 	case BLOCK_REPLACEMENT:
-	    ret = part->mbd.mtd->read(part->mbd.mtd, src, SECTOR_SIZE,
-                        &retlen, (u_char *) buf);
+	    ret = mtd_read(part->mbd.mtd, src, SECTOR_SIZE, &retlen,
+                           (u_char *)buf);
 	    if (ret) {
 		printk(KERN_WARNING "ftl: Error reading old xfer unit in copy_erase_unit\n");
 		return ret;
             }
 
 
-	    ret = part->mbd.mtd->write(part->mbd.mtd, dest, SECTOR_SIZE,
-                        &retlen, (u_char *) buf);
+	    ret = mtd_write(part->mbd.mtd, dest, SECTOR_SIZE, &retlen,
+                            (u_char *)buf);
 	    if (ret)  {
 		printk(KERN_WARNING "ftl: Error writing new xfer unit in copy_erase_unit\n");
 		return ret;
@@ -550,9 +550,11 @@ static int copy_erase_unit(partition_t *part, uint16_t srcunit,
     }
 
     /* Write the BAM to the transfer unit */
-    ret = part->mbd.mtd->write(part->mbd.mtd, xfer->Offset + le32_to_cpu(part->header.BAMOffset),
-                    part->BlocksPerUnit * sizeof(int32_t), &retlen,
-		    (u_char *)part->bam_cache);
+    ret = mtd_write(part->mbd.mtd,
+                    xfer->Offset + le32_to_cpu(part->header.BAMOffset),
+                    part->BlocksPerUnit * sizeof(int32_t),
+                    &retlen,
+                    (u_char *)part->bam_cache);
     if (ret) {
 	printk( KERN_WARNING "ftl: Error writing BAM in copy_erase_unit\n");
 	return ret;
@@ -560,8 +562,8 @@ static int copy_erase_unit(partition_t *part, uint16_t srcunit,
 
 
     /* All clear? Then update the LogicalEUN again */
-    ret = part->mbd.mtd->write(part->mbd.mtd, xfer->Offset + 20, sizeof(uint16_t),
-			   &retlen, (u_char *)&srcunitswap);
+    ret = mtd_write(part->mbd.mtd, xfer->Offset + 20, sizeof(uint16_t),
+                    &retlen, (u_char *)&srcunitswap);
 
     if (ret) {
 	printk(KERN_WARNING "ftl: Error writing new LogicalEUN in copy_erase_unit\n");
@@ -648,8 +650,7 @@ static int reclaim_block(partition_t *part)
 	    if (queued) {
 		pr_debug("ftl_cs: waiting for transfer "
 		      "unit to be prepared...\n");
-		if (part->mbd.mtd->sync)
-			part->mbd.mtd->sync(part->mbd.mtd);
+		mtd_sync(part->mbd.mtd);
 	    } else {
 		static int ne = 0;
 		if (++ne < 5)
@@ -747,10 +748,11 @@ static uint32_t find_free(partition_t *part)
 	/* Invalidate cache */
 	part->bam_index = 0xffff;
 
-	ret = part->mbd.mtd->read(part->mbd.mtd,
-		       part->EUNInfo[eun].Offset + le32_to_cpu(part->header.BAMOffset),
-		       part->BlocksPerUnit * sizeof(uint32_t),
-		       &retlen, (u_char *) (part->bam_cache));
+	ret = mtd_read(part->mbd.mtd,
+                       part->EUNInfo[eun].Offset + le32_to_cpu(part->header.BAMOffset),
+                       part->BlocksPerUnit * sizeof(uint32_t),
+                       &retlen,
+                       (u_char *)(part->bam_cache));
 
 	if (ret) {
 	    printk(KERN_WARNING"ftl: Error reading BAM in find_free\n");
@@ -810,8 +812,8 @@ static int ftl_read(partition_t *part, caddr_t buffer,
 	else {
 	    offset = (part->EUNInfo[log_addr / bsize].Offset
 			  + (log_addr % bsize));
-	    ret = part->mbd.mtd->read(part->mbd.mtd, offset, SECTOR_SIZE,
-			   &retlen, (u_char *) buffer);
+	    ret = mtd_read(part->mbd.mtd, offset, SECTOR_SIZE, &retlen,
+                           (u_char *)buffer);
 
 	    if (ret) {
 		printk(KERN_WARNING "Error reading MTD device in ftl_read()\n");
@@ -849,8 +851,8 @@ static int set_bam_entry(partition_t *part, uint32_t log_addr,
 		  le32_to_cpu(part->header.BAMOffset));
 
 #ifdef PSYCHO_DEBUG
-    ret = part->mbd.mtd->read(part->mbd.mtd, offset, sizeof(uint32_t),
-                        &retlen, (u_char *)&old_addr);
+    ret = mtd_read(part->mbd.mtd, offset, sizeof(uint32_t), &retlen,
+                   (u_char *)&old_addr);
     if (ret) {
 	printk(KERN_WARNING"ftl: Error reading old_addr in set_bam_entry: %d\n",ret);
 	return ret;
@@ -886,8 +888,8 @@ static int set_bam_entry(partition_t *part, uint32_t log_addr,
 #endif
 	part->bam_cache[blk] = le_virt_addr;
     }
-    ret = part->mbd.mtd->write(part->mbd.mtd, offset, sizeof(uint32_t),
-                            &retlen, (u_char *)&le_virt_addr);
+    ret = mtd_write(part->mbd.mtd, offset, sizeof(uint32_t), &retlen,
+                    (u_char *)&le_virt_addr);
 
     if (ret) {
 	printk(KERN_NOTICE "ftl_cs: set_bam_entry() failed!\n");
@@ -946,8 +948,7 @@ static int ftl_write(partition_t *part, caddr_t buffer,
 	part->EUNInfo[part->bam_index].Deleted++;
 	offset = (part->EUNInfo[part->bam_index].Offset +
 		      blk * SECTOR_SIZE);
-	ret = part->mbd.mtd->write(part->mbd.mtd, offset, SECTOR_SIZE, &retlen,
-                                     buffer);
+	ret = mtd_write(part->mbd.mtd, offset, SECTOR_SIZE, &retlen, buffer);
 
 	if (ret) {
 	    printk(KERN_NOTICE "ftl_cs: block write failed!\n");
diff --git a/drivers/mtd/inftlcore.c b/drivers/mtd/inftlcore.c
index dd034efd1875..28646c95cfb8 100644
--- a/drivers/mtd/inftlcore.c
+++ b/drivers/mtd/inftlcore.c
@@ -158,7 +158,7 @@ int inftl_read_oob(struct mtd_info *mtd, loff_t offs, size_t len,
 	ops.oobbuf = buf;
 	ops.datbuf = NULL;
 
-	res = mtd->read_oob(mtd, offs & ~(mtd->writesize - 1), &ops);
+	res = mtd_read_oob(mtd, offs & ~(mtd->writesize - 1), &ops);
 	*retlen = ops.oobretlen;
 	return res;
 }
@@ -178,7 +178,7 @@ int inftl_write_oob(struct mtd_info *mtd, loff_t offs, size_t len,
 	ops.oobbuf = buf;
 	ops.datbuf = NULL;
 
-	res = mtd->write_oob(mtd, offs & ~(mtd->writesize - 1), &ops);
+	res = mtd_write_oob(mtd, offs & ~(mtd->writesize - 1), &ops);
 	*retlen = ops.oobretlen;
 	return res;
 }
@@ -199,7 +199,7 @@ static int inftl_write(struct mtd_info *mtd, loff_t offs, size_t len,
 	ops.datbuf = buf;
 	ops.len = len;
 
-	res = mtd->write_oob(mtd, offs & ~(mtd->writesize - 1), &ops);
+	res = mtd_write_oob(mtd, offs & ~(mtd->writesize - 1), &ops);
 	*retlen = ops.retlen;
 	return res;
 }
@@ -343,14 +343,17 @@ static u16 INFTL_foldchain(struct INFTLrecord *inftl, unsigned thisVUC, unsigned
 		if (BlockMap[block] == BLOCK_NIL)
 			continue;
 
-		ret = mtd->read(mtd, (inftl->EraseSize * BlockMap[block]) +
-				(block * SECTORSIZE), SECTORSIZE, &retlen,
-				movebuf);
+		ret = mtd_read(mtd,
+			       (inftl->EraseSize * BlockMap[block]) + (block * SECTORSIZE),
+			       SECTORSIZE,
+			       &retlen,
+			       movebuf);
 		if (ret < 0 && !mtd_is_bitflip(ret)) {
-			ret = mtd->read(mtd,
-					(inftl->EraseSize * BlockMap[block]) +
-					(block * SECTORSIZE), SECTORSIZE,
-					&retlen, movebuf);
+			ret = mtd_read(mtd,
+				       (inftl->EraseSize * BlockMap[block]) + (block * SECTORSIZE),
+				       SECTORSIZE,
+				       &retlen,
+				       movebuf);
 			if (ret != -EIO)
 				pr_debug("INFTL: error went away on retry?\n");
 		}
@@ -914,7 +917,7 @@ foundit:
 	} else {
 		size_t retlen;
 		loff_t ptr = (thisEUN * inftl->EraseSize) + blockofs;
-		int ret = mtd->read(mtd, ptr, SECTORSIZE, &retlen, buffer);
+		int ret = mtd_read(mtd, ptr, SECTORSIZE, &retlen, buffer);
 
 		/* Handle corrected bit flips gracefully */
 		if (ret < 0 && !mtd_is_bitflip(ret))
diff --git a/drivers/mtd/inftlmount.c b/drivers/mtd/inftlmount.c
index 2ff601f816ce..4adc0374fb6b 100644
--- a/drivers/mtd/inftlmount.c
+++ b/drivers/mtd/inftlmount.c
@@ -73,8 +73,8 @@ static int find_boot_record(struct INFTLrecord *inftl)
 		 * Check for BNAND header first. Then whinge if it's found
 		 * but later checks fail.
 		 */
-		ret = mtd->read(mtd, block * inftl->EraseSize,
-				SECTORSIZE, &retlen, buf);
+		ret = mtd_read(mtd, block * inftl->EraseSize, SECTORSIZE,
+			       &retlen, buf);
 		/* We ignore ret in case the ECC of the MediaHeader is invalid
 		   (which is apparently acceptable) */
 		if (retlen != SECTORSIZE) {
@@ -118,8 +118,8 @@ static int find_boot_record(struct INFTLrecord *inftl)
 		memcpy(mh, buf, sizeof(struct INFTLMediaHeader));
 
 		/* Read the spare media header at offset 4096 */
-		mtd->read(mtd, block * inftl->EraseSize + 4096,
-			  SECTORSIZE, &retlen, buf);
+		mtd_read(mtd, block * inftl->EraseSize + 4096, SECTORSIZE,
+			 &retlen, buf);
 		if (retlen != SECTORSIZE) {
 			printk(KERN_WARNING "INFTL: Unable to read spare "
 			       "Media Header\n");
@@ -220,7 +220,7 @@ static int find_boot_record(struct INFTLrecord *inftl)
 				 */
 				instr->addr = ip->Reserved0 * inftl->EraseSize;
 				instr->len = inftl->EraseSize;
-				mtd->erase(mtd, instr);
+				mtd_erase(mtd, instr);
 			}
 			if ((ip->lastUnit - ip->firstUnit + 1) < ip->virtualUnits) {
 				printk(KERN_WARNING "INFTL: Media Header "
@@ -306,7 +306,8 @@ static int find_boot_record(struct INFTLrecord *inftl)
 			/* If any of the physical eraseblocks are bad, don't
 			   use the unit. */
 			for (physblock = 0; physblock < inftl->EraseSize; physblock += inftl->mbd.mtd->erasesize) {
-				if (inftl->mbd.mtd->block_isbad(inftl->mbd.mtd, i * inftl->EraseSize + physblock))
+				if (mtd_block_isbad(inftl->mbd.mtd,
+						    i * inftl->EraseSize + physblock))
 					inftl->PUtable[i] = BLOCK_RESERVED;
 			}
 		}
@@ -342,7 +343,7 @@ static int check_free_sectors(struct INFTLrecord *inftl, unsigned int address,
 	int i;
 
 	for (i = 0; i < len; i += SECTORSIZE) {
-		if (mtd->read(mtd, address, SECTORSIZE, &retlen, buf))
+		if (mtd_read(mtd, address, SECTORSIZE, &retlen, buf))
 			return -1;
 		if (memcmpb(buf, 0xff, SECTORSIZE) != 0)
 			return -1;
@@ -393,7 +394,7 @@ int INFTL_formatblock(struct INFTLrecord *inftl, int block)
 	   mark only the failed block in the bbt. */
 	for (physblock = 0; physblock < inftl->EraseSize;
 	     physblock += instr->len, instr->addr += instr->len) {
-		mtd->erase(inftl->mbd.mtd, instr);
+		mtd_erase(inftl->mbd.mtd, instr);
 
 		if (instr->state == MTD_ERASE_FAILED) {
 			printk(KERN_WARNING "INFTL: error while formatting block %d\n",
@@ -423,7 +424,7 @@ int INFTL_formatblock(struct INFTLrecord *inftl, int block)
 fail:
 	/* could not format, update the bad block table (caller is responsible
 	   for setting the PUtable to BLOCK_RESERVED on failure) */
-	inftl->mbd.mtd->block_markbad(inftl->mbd.mtd, instr->addr);
+	mtd_block_markbad(inftl->mbd.mtd, instr->addr);
 	return -1;
 }
 
diff --git a/drivers/mtd/lpddr/lpddr_cmds.c b/drivers/mtd/lpddr/lpddr_cmds.c
index 1dca31d9a8b3..536bbceaeaad 100644
--- a/drivers/mtd/lpddr/lpddr_cmds.c
+++ b/drivers/mtd/lpddr/lpddr_cmds.c
@@ -70,19 +70,12 @@ struct mtd_info *lpddr_cmdset(struct map_info *map)
 	mtd->erase = lpddr_erase;
 	mtd->write = lpddr_write_buffers;
 	mtd->writev = lpddr_writev;
-	mtd->read_oob = NULL;
-	mtd->write_oob = NULL;
-	mtd->sync = NULL;
 	mtd->lock = lpddr_lock;
 	mtd->unlock = lpddr_unlock;
-	mtd->suspend = NULL;
-	mtd->resume = NULL;
 	if (map_is_linear(map)) {
 		mtd->point = lpddr_point;
 		mtd->unpoint = lpddr_unpoint;
 	}
-	mtd->block_isbad = NULL;
-	mtd->block_markbad = NULL;
 	mtd->size = 1 << lpddr->qinfo->DevSizeShift;
 	mtd->erasesize = 1 << lpddr->qinfo->UniformBlockSizeShift;
 	mtd->writesize = 1 << lpddr->qinfo->BufSizeShift;
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index 8e0c4bf9f7fb..6c5c431c64af 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -242,15 +242,6 @@ config MTD_NETtel
 	help
 	  Support for flash chips on NETtel/SecureEdge/SnapGear boards.
 
-config MTD_BCM963XX
-        tristate "Map driver for Broadcom BCM963xx boards"
-        depends on BCM63XX
-	select MTD_MAP_BANK_WIDTH_2
-	select MTD_CFI_I1
-        help
-	  Support for parsing CFE image tag and creating MTD partitions on
-	  Broadcom BCM63xx boards.
-
 config MTD_LANTIQ
 	tristate "Lantiq SoC NOR support"
 	depends on LANTIQ
diff --git a/drivers/mtd/maps/Makefile b/drivers/mtd/maps/Makefile
index 45dcb8b14f22..68a9a91d344f 100644
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -55,6 +55,5 @@ obj-$(CONFIG_MTD_BFIN_ASYNC)	+= bfin-async-flash.o
 obj-$(CONFIG_MTD_RBTX4939)	+= rbtx4939-flash.o
 obj-$(CONFIG_MTD_VMU)		+= vmu-flash.o
 obj-$(CONFIG_MTD_GPIO_ADDR)	+= gpio-addr-flash.o
-obj-$(CONFIG_MTD_BCM963XX)	+= bcm963xx-flash.o
 obj-$(CONFIG_MTD_LATCH_ADDR)	+= latch-addr-flash.o
 obj-$(CONFIG_MTD_LANTIQ)	+= lantiq-flash.o
diff --git a/drivers/mtd/maps/bcm963xx-flash.c b/drivers/mtd/maps/bcm963xx-flash.c
deleted file mode 100644
index 736ca10ca9f1..000000000000
--- a/drivers/mtd/maps/bcm963xx-flash.c
+++ /dev/null
@@ -1,277 +0,0 @@
-/*
- * Copyright © 2006-2008  Florian Fainelli <florian@openwrt.org>
- *			  Mike Albon <malbon@openwrt.org>
- * Copyright © 2009-2010  Daniel Dickinson <openwrt@cshore.neomailbox.net>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/partitions.h>
-#include <linux/vmalloc.h>
-#include <linux/platform_device.h>
-#include <linux/io.h>
-
-#include <asm/mach-bcm63xx/bcm963xx_tag.h>
-
-#define BCM63XX_BUSWIDTH	2		/* Buswidth */
-#define BCM63XX_EXTENDED_SIZE	0xBFC00000	/* Extended flash address */
-
-#define PFX KBUILD_MODNAME ": "
-
-static struct mtd_partition *parsed_parts;
-
-static struct mtd_info *bcm963xx_mtd_info;
-
-static struct map_info bcm963xx_map = {
-	.name		= "bcm963xx",
-	.bankwidth	= BCM63XX_BUSWIDTH,
-};
-
-static int parse_cfe_partitions(struct mtd_info *master,
-						struct mtd_partition **pparts)
-{
-	/* CFE, NVRAM and global Linux are always present */
-	int nrparts = 3, curpart = 0;
-	struct bcm_tag *buf;
-	struct mtd_partition *parts;
-	int ret;
-	size_t retlen;
-	unsigned int rootfsaddr, kerneladdr, spareaddr;
-	unsigned int rootfslen, kernellen, sparelen, totallen;
-	int namelen = 0;
-	int i;
-	char *boardid;
-	char *tagversion;
-
-	/* Allocate memory for buffer */
-	buf = vmalloc(sizeof(struct bcm_tag));
-	if (!buf)
-		return -ENOMEM;
-
-	/* Get the tag */
-	ret = master->read(master, master->erasesize, sizeof(struct bcm_tag),
-							&retlen, (void *)buf);
-	if (retlen != sizeof(struct bcm_tag)) {
-		vfree(buf);
-		return -EIO;
-	}
-
-	sscanf(buf->kernel_address, "%u", &kerneladdr);
-	sscanf(buf->kernel_length, "%u", &kernellen);
-	sscanf(buf->total_length, "%u", &totallen);
-	tagversion = &(buf->tag_version[0]);
-	boardid = &(buf->board_id[0]);
-
-	printk(KERN_INFO PFX "CFE boot tag found with version %s "
-				"and board type %s\n", tagversion, boardid);
-
-	kerneladdr = kerneladdr - BCM63XX_EXTENDED_SIZE;
-	rootfsaddr = kerneladdr + kernellen;
-	spareaddr = roundup(totallen, master->erasesize) + master->erasesize;
-	sparelen = master->size - spareaddr - master->erasesize;
-	rootfslen = spareaddr - rootfsaddr;
-
-	/* Determine number of partitions */
-	namelen = 8;
-	if (rootfslen > 0) {
-		nrparts++;
-		namelen += 6;
-	};
-	if (kernellen > 0) {
-		nrparts++;
-		namelen += 6;
-	};
-
-	/* Ask kernel for more memory */
-	parts = kzalloc(sizeof(*parts) * nrparts + 10 * nrparts, GFP_KERNEL);
-	if (!parts) {
-		vfree(buf);
-		return -ENOMEM;
-	};
-
-	/* Start building partition list */
-	parts[curpart].name = "CFE";
-	parts[curpart].offset = 0;
-	parts[curpart].size = master->erasesize;
-	curpart++;
-
-	if (kernellen > 0) {
-		parts[curpart].name = "kernel";
-		parts[curpart].offset = kerneladdr;
-		parts[curpart].size = kernellen;
-		curpart++;
-	};
-
-	if (rootfslen > 0) {
-		parts[curpart].name = "rootfs";
-		parts[curpart].offset = rootfsaddr;
-		parts[curpart].size = rootfslen;
-		if (sparelen > 0)
-			parts[curpart].size += sparelen;
-		curpart++;
-	};
-
-	parts[curpart].name = "nvram";
-	parts[curpart].offset = master->size - master->erasesize;
-	parts[curpart].size = master->erasesize;
-
-	/* Global partition "linux" to make easy firmware upgrade */
-	curpart++;
-	parts[curpart].name = "linux";
-	parts[curpart].offset = parts[0].size;
-	parts[curpart].size = master->size - parts[0].size - parts[3].size;
-
-	for (i = 0; i < nrparts; i++)
-		printk(KERN_INFO PFX "Partition %d is %s offset %lx and "
-					"length %lx\n", i, parts[i].name,
-					(long unsigned int)(parts[i].offset),
-					(long unsigned int)(parts[i].size));
-
-	printk(KERN_INFO PFX "Spare partition is %x offset and length %x\n",
-							spareaddr, sparelen);
-	*pparts = parts;
-	vfree(buf);
-
-	return nrparts;
-};
-
-static int bcm963xx_detect_cfe(struct mtd_info *master)
-{
-	int idoffset = 0x4e0;
-	static char idstring[8] = "CFE1CFE1";
-	char buf[9];
-	int ret;
-	size_t retlen;
-
-	ret = master->read(master, idoffset, 8, &retlen, (void *)buf);
-	buf[retlen] = 0;
-	printk(KERN_INFO PFX "Read Signature value of %s\n", buf);
-
-	return strncmp(idstring, buf, 8);
-}
-
-static int bcm963xx_probe(struct platform_device *pdev)
-{
-	int err = 0;
-	int parsed_nr_parts = 0;
-	char *part_type;
-	struct resource *r;
-
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!r) {
-		dev_err(&pdev->dev, "no resource supplied\n");
-		return -ENODEV;
-	}
-
-	bcm963xx_map.phys = r->start;
-	bcm963xx_map.size = resource_size(r);
-	bcm963xx_map.virt = ioremap(r->start, resource_size(r));
-	if (!bcm963xx_map.virt) {
-		dev_err(&pdev->dev, "failed to ioremap\n");
-		return -EIO;
-	}
-
-	dev_info(&pdev->dev, "0x%08lx at 0x%08x\n",
-					bcm963xx_map.size, bcm963xx_map.phys);
-
-	simple_map_init(&bcm963xx_map);
-
-	bcm963xx_mtd_info = do_map_probe("cfi_probe", &bcm963xx_map);
-	if (!bcm963xx_mtd_info) {
-		dev_err(&pdev->dev, "failed to probe using CFI\n");
-		bcm963xx_mtd_info = do_map_probe("jedec_probe", &bcm963xx_map);
-		if (bcm963xx_mtd_info)
-			goto probe_ok;
-		dev_err(&pdev->dev, "failed to probe using JEDEC\n");
-		err = -EIO;
-		goto err_probe;
-	}
-
-probe_ok:
-	bcm963xx_mtd_info->owner = THIS_MODULE;
-
-	/* This is mutually exclusive */
-	if (bcm963xx_detect_cfe(bcm963xx_mtd_info) == 0) {
-		dev_info(&pdev->dev, "CFE bootloader detected\n");
-		if (parsed_nr_parts == 0) {
-			int ret = parse_cfe_partitions(bcm963xx_mtd_info,
-							&parsed_parts);
-			if (ret > 0) {
-				part_type = "CFE";
-				parsed_nr_parts = ret;
-			}
-		}
-	} else {
-		dev_info(&pdev->dev, "unsupported bootloader\n");
-		err = -ENODEV;
-		goto err_probe;
-	}
-
-	return mtd_device_register(bcm963xx_mtd_info, parsed_parts,
-				   parsed_nr_parts);
-
-err_probe:
-	iounmap(bcm963xx_map.virt);
-	return err;
-}
-
-static int bcm963xx_remove(struct platform_device *pdev)
-{
-	if (bcm963xx_mtd_info) {
-		mtd_device_unregister(bcm963xx_mtd_info);
-		map_destroy(bcm963xx_mtd_info);
-	}
-
-	if (bcm963xx_map.virt) {
-		iounmap(bcm963xx_map.virt);
-		bcm963xx_map.virt = 0;
-	}
-
-	return 0;
-}
-
-static struct platform_driver bcm63xx_mtd_dev = {
-	.probe	= bcm963xx_probe,
-	.remove = bcm963xx_remove,
-	.driver = {
-		.name	= "bcm963xx-flash",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init bcm963xx_mtd_init(void)
-{
-	return platform_driver_register(&bcm63xx_mtd_dev);
-}
-
-static void __exit bcm963xx_mtd_exit(void)
-{
-	platform_driver_unregister(&bcm63xx_mtd_dev);
-}
-
-module_init(bcm963xx_mtd_init);
-module_exit(bcm963xx_mtd_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Broadcom BCM63xx MTD driver for CFE and RedBoot");
-MODULE_AUTHOR("Daniel Dickinson <openwrt@cshore.neomailbox.net>");
-MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
-MODULE_AUTHOR("Mike Albon <malbon@openwrt.org>");
diff --git a/drivers/mtd/maps/bfin-async-flash.c b/drivers/mtd/maps/bfin-async-flash.c
index 6d6b2b5674ee..650126c361f1 100644
--- a/drivers/mtd/maps/bfin-async-flash.c
+++ b/drivers/mtd/maps/bfin-async-flash.c
@@ -190,17 +190,7 @@ static struct platform_driver bfin_flash_driver = {
 	},
 };
 
-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_platform_driver(bfin_flash_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("MTD map driver for Blackfins with flash/ethernet on same async bank");
diff --git a/drivers/mtd/maps/gpio-addr-flash.c b/drivers/mtd/maps/gpio-addr-flash.c
index 1ec66f031c51..33cce895859f 100644
--- a/drivers/mtd/maps/gpio-addr-flash.c
+++ b/drivers/mtd/maps/gpio-addr-flash.c
@@ -279,17 +279,7 @@ static struct platform_driver gpio_flash_driver = {
 	},
 };
 
-static int __init gpio_flash_init(void)
-{
-	return platform_driver_register(&gpio_flash_driver);
-}
-module_init(gpio_flash_init);
-
-static void __exit gpio_flash_exit(void)
-{
-	platform_driver_unregister(&gpio_flash_driver);
-}
-module_exit(gpio_flash_exit);
+module_platform_driver(gpio_flash_driver);
 
 MODULE_AUTHOR("Mike Frysinger <vapier@gentoo.org>");
 MODULE_DESCRIPTION("MTD map driver for flashes addressed physically and with gpios");
diff --git a/drivers/mtd/maps/ixp2000.c b/drivers/mtd/maps/ixp2000.c
index 437fcd2f352f..fc7d4d0d9a4e 100644
--- a/drivers/mtd/maps/ixp2000.c
+++ b/drivers/mtd/maps/ixp2000.c
@@ -246,18 +246,8 @@ static struct platform_driver ixp2000_flash_driver = {
 	},
 };
 
-static int __init ixp2000_flash_init(void)
-{
-	return platform_driver_register(&ixp2000_flash_driver);
-}
-
-static void __exit ixp2000_flash_exit(void)
-{
-	platform_driver_unregister(&ixp2000_flash_driver);
-}
+module_platform_driver(ixp2000_flash_driver);
 
-module_init(ixp2000_flash_init);
-module_exit(ixp2000_flash_exit);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Deepak Saxena <dsaxena@plexity.net>");
 MODULE_ALIAS("platform:IXP2000-Flash");
diff --git a/drivers/mtd/maps/ixp4xx.c b/drivers/mtd/maps/ixp4xx.c
index 30409015a3de..8b5410162d70 100644
--- a/drivers/mtd/maps/ixp4xx.c
+++ b/drivers/mtd/maps/ixp4xx.c
@@ -270,19 +270,7 @@ static struct platform_driver ixp4xx_flash_driver = {
 	},
 };
 
-static int __init ixp4xx_flash_init(void)
-{
-	return platform_driver_register(&ixp4xx_flash_driver);
-}
-
-static void __exit ixp4xx_flash_exit(void)
-{
-	platform_driver_unregister(&ixp4xx_flash_driver);
-}
-
-
-module_init(ixp4xx_flash_init);
-module_exit(ixp4xx_flash_exit);
+module_platform_driver(ixp4xx_flash_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("MTD map driver for Intel IXP4xx systems");
diff --git a/drivers/mtd/maps/lantiq-flash.c b/drivers/mtd/maps/lantiq-flash.c
index 4f10e27ada55..7b889de9477b 100644
--- a/drivers/mtd/maps/lantiq-flash.c
+++ b/drivers/mtd/maps/lantiq-flash.c
@@ -159,7 +159,7 @@ ltq_mtd_probe(struct platform_device *pdev)
 	if (!ltq_mtd->mtd) {
 		dev_err(&pdev->dev, "probing failed\n");
 		err = -ENXIO;
-		goto err_unmap;
+		goto err_free;
 	}
 
 	ltq_mtd->mtd->owner = THIS_MODULE;
@@ -179,8 +179,6 @@ ltq_mtd_probe(struct platform_device *pdev)
 
 err_destroy:
 	map_destroy(ltq_mtd->mtd);
-err_unmap:
-	iounmap(ltq_mtd->map->virt);
 err_free:
 	kfree(ltq_mtd->map);
 err_out:
@@ -198,8 +196,6 @@ ltq_mtd_remove(struct platform_device *pdev)
 			mtd_device_unregister(ltq_mtd->mtd);
 			map_destroy(ltq_mtd->mtd);
 		}
-		if (ltq_mtd->map->virt)
-			iounmap(ltq_mtd->map->virt);
 		kfree(ltq_mtd->map);
 		kfree(ltq_mtd);
 	}
diff --git a/drivers/mtd/maps/latch-addr-flash.c b/drivers/mtd/maps/latch-addr-flash.c
index 119baa7d7477..8fed58e3a4a8 100644
--- a/drivers/mtd/maps/latch-addr-flash.c
+++ b/drivers/mtd/maps/latch-addr-flash.c
@@ -223,17 +223,7 @@ static struct platform_driver latch_addr_flash_driver = {
 	},
 };
 
-static int __init latch_addr_flash_init(void)
-{
-	return platform_driver_register(&latch_addr_flash_driver);
-}
-module_init(latch_addr_flash_init);
-
-static void __exit latch_addr_flash_exit(void)
-{
-	platform_driver_unregister(&latch_addr_flash_driver);
-}
-module_exit(latch_addr_flash_exit);
+module_platform_driver(latch_addr_flash_driver);
 
 MODULE_AUTHOR("David Griego <dgriego@mvista.com>");
 MODULE_DESCRIPTION("MTD map driver for flashes addressed physically with upper "
diff --git a/drivers/mtd/maps/physmap.c b/drivers/mtd/maps/physmap.c
index 66e8200079c2..abc562653b31 100644
--- a/drivers/mtd/maps/physmap.c
+++ b/drivers/mtd/maps/physmap.c
@@ -85,6 +85,7 @@ static int physmap_flash_probe(struct platform_device *dev)
 	struct physmap_flash_data *physmap_data;
 	struct physmap_flash_info *info;
 	const char **probe_type;
+	const char **part_types;
 	int err = 0;
 	int i;
 	int devices_found = 0;
@@ -171,7 +172,9 @@ static int physmap_flash_probe(struct platform_device *dev)
 	if (err)
 		goto err_out;
 
-	mtd_device_parse_register(info->cmtd, part_probe_types, 0,
+	part_types = physmap_data->part_probe_types ? : part_probe_types;
+
+	mtd_device_parse_register(info->cmtd, part_types, 0,
 				  physmap_data->parts, physmap_data->nr_parts);
 	return 0;
 
@@ -187,9 +190,8 @@ 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]->resume)
-			if (info->mtd[i]->suspend(info->mtd[i]) == 0)
-				info->mtd[i]->resume(info->mtd[i]);
+		if (mtd_suspend(info->mtd[i]) == 0)
+			mtd_resume(info->mtd[i]);
 }
 #else
 #define physmap_flash_shutdown NULL
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index 7d65f9d3e690..2e6fb6831d55 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -338,18 +338,7 @@ static struct platform_driver of_flash_driver = {
 	.remove		= of_flash_remove,
 };
 
-static int __init of_flash_init(void)
-{
-	return platform_driver_register(&of_flash_driver);
-}
-
-static void __exit of_flash_exit(void)
-{
-	platform_driver_unregister(&of_flash_driver);
-}
-
-module_init(of_flash_init);
-module_exit(of_flash_exit);
+module_platform_driver(of_flash_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Vitaly Wool <vwool@ru.mvista.com>");
diff --git a/drivers/mtd/maps/pxa2xx-flash.c b/drivers/mtd/maps/pxa2xx-flash.c
index 2a25b6789af4..436d121185b1 100644
--- a/drivers/mtd/maps/pxa2xx-flash.c
+++ b/drivers/mtd/maps/pxa2xx-flash.c
@@ -125,8 +125,8 @@ static void pxa2xx_flash_shutdown(struct platform_device *dev)
 {
 	struct pxa2xx_flash_info *info = platform_get_drvdata(dev);
 
-	if (info && info->mtd->suspend(info->mtd) == 0)
-		info->mtd->resume(info->mtd);
+	if (info && mtd_suspend(info->mtd) == 0)
+		mtd_resume(info->mtd);
 }
 #else
 #define pxa2xx_flash_shutdown NULL
@@ -142,18 +142,7 @@ static struct platform_driver pxa2xx_flash_driver = {
 	.shutdown	= pxa2xx_flash_shutdown,
 };
 
-static int __init init_pxa2xx_flash(void)
-{
-	return platform_driver_register(&pxa2xx_flash_driver);
-}
-
-static void __exit cleanup_pxa2xx_flash(void)
-{
-	platform_driver_unregister(&pxa2xx_flash_driver);
-}
-
-module_init(init_pxa2xx_flash);
-module_exit(cleanup_pxa2xx_flash);
+module_platform_driver(pxa2xx_flash_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Nicolas Pitre <nico@fluxnic.net>");
diff --git a/drivers/mtd/maps/rbtx4939-flash.c b/drivers/mtd/maps/rbtx4939-flash.c
index 0237f197fd12..3da63fc6f16e 100644
--- a/drivers/mtd/maps/rbtx4939-flash.c
+++ b/drivers/mtd/maps/rbtx4939-flash.c
@@ -119,9 +119,8 @@ static void rbtx4939_flash_shutdown(struct platform_device *dev)
 {
 	struct rbtx4939_flash_info *info = platform_get_drvdata(dev);
 
-	if (info->mtd->suspend && info->mtd->resume)
-		if (info->mtd->suspend(info->mtd) == 0)
-			info->mtd->resume(info->mtd);
+	if (mtd_suspend(info->mtd) == 0)
+		mtd_resume(info->mtd);
 }
 #else
 #define rbtx4939_flash_shutdown NULL
@@ -137,18 +136,7 @@ static struct platform_driver rbtx4939_flash_driver = {
 	},
 };
 
-static int __init rbtx4939_flash_init(void)
-{
-	return platform_driver_register(&rbtx4939_flash_driver);
-}
-
-static void __exit rbtx4939_flash_exit(void)
-{
-	platform_driver_unregister(&rbtx4939_flash_driver);
-}
-
-module_init(rbtx4939_flash_init);
-module_exit(rbtx4939_flash_exit);
+module_platform_driver(rbtx4939_flash_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("RBTX4939 MTD map driver");
diff --git a/drivers/mtd/maps/sa1100-flash.c b/drivers/mtd/maps/sa1100-flash.c
index fa9c0a9670cd..502821997707 100644
--- a/drivers/mtd/maps/sa1100-flash.c
+++ b/drivers/mtd/maps/sa1100-flash.c
@@ -377,8 +377,8 @@ static int __exit sa1100_mtd_remove(struct platform_device *pdev)
 static void sa1100_mtd_shutdown(struct platform_device *dev)
 {
 	struct sa_info *info = platform_get_drvdata(dev);
-	if (info && info->mtd->suspend(info->mtd) == 0)
-		info->mtd->resume(info->mtd);
+	if (info && mtd_suspend(info->mtd) == 0)
+		mtd_resume(info->mtd);
 }
 #else
 #define sa1100_mtd_shutdown NULL
@@ -394,18 +394,7 @@ static struct platform_driver sa1100_mtd_driver = {
 	},
 };
 
-static int __init sa1100_mtd_init(void)
-{
-	return platform_driver_register(&sa1100_mtd_driver);
-}
-
-static void __exit sa1100_mtd_exit(void)
-{
-	platform_driver_unregister(&sa1100_mtd_driver);
-}
-
-module_init(sa1100_mtd_init);
-module_exit(sa1100_mtd_exit);
+module_platform_driver(sa1100_mtd_driver);
 
 MODULE_AUTHOR("Nicolas Pitre");
 MODULE_DESCRIPTION("SA1100 CFI map driver");
diff --git a/drivers/mtd/maps/scb2_flash.c b/drivers/mtd/maps/scb2_flash.c
index d88c8426bb0f..934a72c80078 100644
--- a/drivers/mtd/maps/scb2_flash.c
+++ b/drivers/mtd/maps/scb2_flash.c
@@ -204,8 +204,7 @@ scb2_flash_remove(struct pci_dev *dev)
 		return;
 
 	/* disable flash writes */
-	if (scb2_mtd->lock)
-		scb2_mtd->lock(scb2_mtd, 0, scb2_mtd->size);
+	mtd_lock(scb2_mtd, 0, scb2_mtd->size);
 
 	mtd_device_unregister(scb2_mtd);
 	map_destroy(scb2_mtd);
diff --git a/drivers/mtd/maps/sun_uflash.c b/drivers/mtd/maps/sun_uflash.c
index 2d66234f57cb..175e537b444f 100644
--- a/drivers/mtd/maps/sun_uflash.c
+++ b/drivers/mtd/maps/sun_uflash.c
@@ -158,15 +158,4 @@ static struct platform_driver uflash_driver = {
 	.remove		= __devexit_p(uflash_remove),
 };
 
-static int __init uflash_init(void)
-{
-	return platform_driver_register(&uflash_driver);
-}
-
-static void __exit uflash_exit(void)
-{
-	platform_driver_unregister(&uflash_driver);
-}
-
-module_init(uflash_init);
-module_exit(uflash_exit);
+module_platform_driver(uflash_driver);
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index ed8b5e744b12..424ca5f93c6c 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -215,7 +215,7 @@ static int blktrans_open(struct block_device *bdev, fmode_t mode)
 
 	mutex_lock(&dev->lock);
 
-	if (dev->open++)
+	if (dev->open)
 		goto unlock;
 
 	kref_get(&dev->ref);
@@ -235,6 +235,7 @@ static int blktrans_open(struct block_device *bdev, fmode_t mode)
 		goto error_release;
 
 unlock:
+	dev->open++;
 	mutex_unlock(&dev->lock);
 	blktrans_dev_put(dev);
 	return ret;
diff --git a/drivers/mtd/mtdblock.c b/drivers/mtd/mtdblock.c
index 7c1dc908a174..af6591237b9b 100644
--- a/drivers/mtd/mtdblock.c
+++ b/drivers/mtd/mtdblock.c
@@ -85,7 +85,7 @@ static int erase_write (struct mtd_info *mtd, unsigned long pos,
 	set_current_state(TASK_INTERRUPTIBLE);
 	add_wait_queue(&wait_q, &wait);
 
-	ret = mtd->erase(mtd, &erase);
+	ret = mtd_erase(mtd, &erase);
 	if (ret) {
 		set_current_state(TASK_RUNNING);
 		remove_wait_queue(&wait_q, &wait);
@@ -102,7 +102,7 @@ static int erase_write (struct mtd_info *mtd, unsigned long pos,
 	 * Next, write the data to flash.
 	 */
 
-	ret = mtd->write(mtd, pos, len, &retlen, buf);
+	ret = mtd_write(mtd, pos, len, &retlen, buf);
 	if (ret)
 		return ret;
 	if (retlen != len)
@@ -152,7 +152,7 @@ static int do_cached_write (struct mtdblk_dev *mtdblk, unsigned long pos,
 		mtd->name, pos, len);
 
 	if (!sect_size)
-		return mtd->write(mtd, pos, len, &retlen, buf);
+		return mtd_write(mtd, pos, len, &retlen, buf);
 
 	while (len > 0) {
 		unsigned long sect_start = (pos/sect_size)*sect_size;
@@ -184,8 +184,8 @@ static int do_cached_write (struct mtdblk_dev *mtdblk, unsigned long pos,
 			    mtdblk->cache_offset != sect_start) {
 				/* fill the cache with the current sector */
 				mtdblk->cache_state = STATE_EMPTY;
-				ret = mtd->read(mtd, sect_start, sect_size,
-						&retlen, mtdblk->cache_data);
+				ret = mtd_read(mtd, sect_start, sect_size,
+					       &retlen, mtdblk->cache_data);
 				if (ret)
 					return ret;
 				if (retlen != sect_size)
@@ -222,7 +222,7 @@ static int do_cached_read (struct mtdblk_dev *mtdblk, unsigned long pos,
 			mtd->name, pos, len);
 
 	if (!sect_size)
-		return mtd->read(mtd, pos, len, &retlen, buf);
+		return mtd_read(mtd, pos, len, &retlen, buf);
 
 	while (len > 0) {
 		unsigned long sect_start = (pos/sect_size)*sect_size;
@@ -241,7 +241,7 @@ static int do_cached_read (struct mtdblk_dev *mtdblk, unsigned long pos,
 		    mtdblk->cache_offset == sect_start) {
 			memcpy (buf, mtdblk->cache_data + offset, size);
 		} else {
-			ret = mtd->read(mtd, pos, size, &retlen, buf);
+			ret = mtd_read(mtd, pos, size, &retlen, buf);
 			if (ret)
 				return ret;
 			if (retlen != size)
@@ -322,8 +322,7 @@ static int mtdblock_release(struct mtd_blktrans_dev *mbd)
 
 	if (!--mtdblk->count) {
 		/* It was the last usage. Free the cache */
-		if (mbd->mtd->sync)
-			mbd->mtd->sync(mbd->mtd);
+		mtd_sync(mbd->mtd);
 		vfree(mtdblk->cache_data);
 	}
 
@@ -341,9 +340,7 @@ static int mtdblock_flush(struct mtd_blktrans_dev *dev)
 	mutex_lock(&mtdblk->cache_mutex);
 	write_cached_data(mtdblk);
 	mutex_unlock(&mtdblk->cache_mutex);
-
-	if (dev->mtd->sync)
-		dev->mtd->sync(dev->mtd);
+	mtd_sync(dev->mtd);
 	return 0;
 }
 
diff --git a/drivers/mtd/mtdblock_ro.c b/drivers/mtd/mtdblock_ro.c
index 0470a6e86309..92759a9d2985 100644
--- a/drivers/mtd/mtdblock_ro.c
+++ b/drivers/mtd/mtdblock_ro.c
@@ -30,7 +30,7 @@ static int mtdblock_readsect(struct mtd_blktrans_dev *dev,
 {
 	size_t retlen;
 
-	if (dev->mtd->read(dev->mtd, (block * 512), 512, &retlen, buf))
+	if (mtd_read(dev->mtd, (block * 512), 512, &retlen, buf))
 		return 1;
 	return 0;
 }
@@ -40,7 +40,7 @@ static int mtdblock_writesect(struct mtd_blktrans_dev *dev,
 {
 	size_t retlen;
 
-	if (dev->mtd->write(dev->mtd, (block * 512), 512, &retlen, buf))
+	if (mtd_write(dev->mtd, (block * 512), 512, &retlen, buf))
 		return 1;
 	return 0;
 }
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index e7dc732ddabc..50c6a1e7f675 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -51,7 +51,7 @@ struct mtd_file_info {
 	enum mtd_file_modes mode;
 };
 
-static loff_t mtd_lseek (struct file *file, loff_t offset, int orig)
+static loff_t mtdchar_lseek(struct file *file, loff_t offset, int orig)
 {
 	struct mtd_file_info *mfi = file->private_data;
 	struct mtd_info *mtd = mfi->mtd;
@@ -77,7 +77,7 @@ static loff_t mtd_lseek (struct file *file, loff_t offset, int orig)
 
 
 
-static int mtd_open(struct inode *inode, struct file *file)
+static int mtdchar_open(struct inode *inode, struct file *file)
 {
 	int minor = iminor(inode);
 	int devnum = minor >> 1;
@@ -142,11 +142,11 @@ static int mtd_open(struct inode *inode, struct file *file)
 out:
 	mutex_unlock(&mtd_mutex);
 	return ret;
-} /* mtd_open */
+} /* mtdchar_open */
 
 /*====================================================================*/
 
-static int mtd_close(struct inode *inode, struct file *file)
+static int mtdchar_close(struct inode *inode, struct file *file)
 {
 	struct mtd_file_info *mfi = file->private_data;
 	struct mtd_info *mtd = mfi->mtd;
@@ -154,8 +154,8 @@ static int mtd_close(struct inode *inode, struct file *file)
 	pr_debug("MTD_close\n");
 
 	/* Only sync if opened RW */
-	if ((file->f_mode & FMODE_WRITE) && mtd->sync)
-		mtd->sync(mtd);
+	if ((file->f_mode & FMODE_WRITE))
+		mtd_sync(mtd);
 
 	iput(mfi->ino);
 
@@ -164,7 +164,7 @@ static int mtd_close(struct inode *inode, struct file *file)
 	kfree(mfi);
 
 	return 0;
-} /* mtd_close */
+} /* mtdchar_close */
 
 /* Back in June 2001, dwmw2 wrote:
  *
@@ -184,11 +184,12 @@ static int mtd_close(struct inode *inode, struct file *file)
  * alignment requirements are not met in the NAND subdriver.
  */
 
-static ssize_t mtd_read(struct file *file, char __user *buf, size_t count,loff_t *ppos)
+static ssize_t mtdchar_read(struct file *file, char __user *buf, size_t count,
+			loff_t *ppos)
 {
 	struct mtd_file_info *mfi = file->private_data;
 	struct mtd_info *mtd = mfi->mtd;
-	size_t retlen=0;
+	size_t retlen;
 	size_t total_retlen=0;
 	int ret=0;
 	int len;
@@ -212,10 +213,12 @@ static ssize_t mtd_read(struct file *file, char __user *buf, size_t count,loff_t
 
 		switch (mfi->mode) {
 		case MTD_FILE_MODE_OTP_FACTORY:
-			ret = mtd->read_fact_prot_reg(mtd, *ppos, len, &retlen, kbuf);
+			ret = mtd_read_fact_prot_reg(mtd, *ppos, len,
+						     &retlen, kbuf);
 			break;
 		case MTD_FILE_MODE_OTP_USER:
-			ret = mtd->read_user_prot_reg(mtd, *ppos, len, &retlen, kbuf);
+			ret = mtd_read_user_prot_reg(mtd, *ppos, len,
+						     &retlen, kbuf);
 			break;
 		case MTD_FILE_MODE_RAW:
 		{
@@ -226,12 +229,12 @@ static ssize_t mtd_read(struct file *file, char __user *buf, size_t count,loff_t
 			ops.oobbuf = NULL;
 			ops.len = len;
 
-			ret = mtd->read_oob(mtd, *ppos, &ops);
+			ret = mtd_read_oob(mtd, *ppos, &ops);
 			retlen = ops.retlen;
 			break;
 		}
 		default:
-			ret = mtd->read(mtd, *ppos, len, &retlen, kbuf);
+			ret = mtd_read(mtd, *ppos, len, &retlen, kbuf);
 		}
 		/* Nand returns -EBADMSG on ECC errors, but it returns
 		 * the data. For our userspace tools it is important
@@ -265,9 +268,10 @@ static ssize_t mtd_read(struct file *file, char __user *buf, size_t count,loff_t
 
 	kfree(kbuf);
 	return total_retlen;
-} /* mtd_read */
+} /* mtdchar_read */
 
-static ssize_t mtd_write(struct file *file, const char __user *buf, size_t count,loff_t *ppos)
+static ssize_t mtdchar_write(struct file *file, const char __user *buf, size_t count,
+			loff_t *ppos)
 {
 	struct mtd_file_info *mfi = file->private_data;
 	struct mtd_info *mtd = mfi->mtd;
@@ -306,11 +310,8 @@ static ssize_t mtd_write(struct file *file, const char __user *buf, size_t count
 			ret = -EROFS;
 			break;
 		case MTD_FILE_MODE_OTP_USER:
-			if (!mtd->write_user_prot_reg) {
-				ret = -EOPNOTSUPP;
-				break;
-			}
-			ret = mtd->write_user_prot_reg(mtd, *ppos, len, &retlen, kbuf);
+			ret = mtd_write_user_prot_reg(mtd, *ppos, len,
+						      &retlen, kbuf);
 			break;
 
 		case MTD_FILE_MODE_RAW:
@@ -323,13 +324,13 @@ static ssize_t mtd_write(struct file *file, const char __user *buf, size_t count
 			ops.ooboffs = 0;
 			ops.len = len;
 
-			ret = mtd->write_oob(mtd, *ppos, &ops);
+			ret = mtd_write_oob(mtd, *ppos, &ops);
 			retlen = ops.retlen;
 			break;
 		}
 
 		default:
-			ret = (*(mtd->write))(mtd, *ppos, len, &retlen, kbuf);
+			ret = mtd_write(mtd, *ppos, len, &retlen, kbuf);
 		}
 		if (!ret) {
 			*ppos += retlen;
@@ -345,7 +346,7 @@ static ssize_t mtd_write(struct file *file, const char __user *buf, size_t count
 
 	kfree(kbuf);
 	return total_retlen;
-} /* mtd_write */
+} /* mtdchar_write */
 
 /*======================================================================
 
@@ -361,20 +362,22 @@ static void mtdchar_erase_callback (struct erase_info *instr)
 static int otp_select_filemode(struct mtd_file_info *mfi, int mode)
 {
 	struct mtd_info *mtd = mfi->mtd;
+	size_t retlen;
 	int ret = 0;
 
+	/*
+	 * Make a fake call to mtd_read_fact_prot_reg() to check if OTP
+	 * operations are supported.
+	 */
+	if (mtd_read_fact_prot_reg(mtd, -1, -1, &retlen, NULL) == -EOPNOTSUPP)
+		return -EOPNOTSUPP;
+
 	switch (mode) {
 	case MTD_OTP_FACTORY:
-		if (!mtd->read_fact_prot_reg)
-			ret = -EOPNOTSUPP;
-		else
-			mfi->mode = MTD_FILE_MODE_OTP_FACTORY;
+		mfi->mode = MTD_FILE_MODE_OTP_FACTORY;
 		break;
 	case MTD_OTP_USER:
-		if (!mtd->read_fact_prot_reg)
-			ret = -EOPNOTSUPP;
-		else
-			mfi->mode = MTD_FILE_MODE_OTP_USER;
+		mfi->mode = MTD_FILE_MODE_OTP_USER;
 		break;
 	default:
 		ret = -EINVAL;
@@ -387,7 +390,7 @@ static int otp_select_filemode(struct mtd_file_info *mfi, int mode)
 # define otp_select_filemode(f,m)	-EOPNOTSUPP
 #endif
 
-static int mtd_do_writeoob(struct file *file, struct mtd_info *mtd,
+static int mtdchar_writeoob(struct file *file, struct mtd_info *mtd,
 	uint64_t start, uint32_t length, void __user *ptr,
 	uint32_t __user *retp)
 {
@@ -424,7 +427,7 @@ static int mtd_do_writeoob(struct file *file, struct mtd_info *mtd,
 		return PTR_ERR(ops.oobbuf);
 
 	start &= ~((uint64_t)mtd->writesize - 1);
-	ret = mtd->write_oob(mtd, start, &ops);
+	ret = mtd_write_oob(mtd, start, &ops);
 
 	if (ops.oobretlen > 0xFFFFFFFFU)
 		ret = -EOVERFLOW;
@@ -436,7 +439,7 @@ static int mtd_do_writeoob(struct file *file, struct mtd_info *mtd,
 	return ret;
 }
 
-static int mtd_do_readoob(struct file *file, struct mtd_info *mtd,
+static int mtdchar_readoob(struct file *file, struct mtd_info *mtd,
 	uint64_t start, uint32_t length, void __user *ptr,
 	uint32_t __user *retp)
 {
@@ -447,13 +450,8 @@ static int mtd_do_readoob(struct file *file, struct mtd_info *mtd,
 	if (length > 4096)
 		return -EINVAL;
 
-	if (!mtd->read_oob)
-		ret = -EOPNOTSUPP;
-	else
-		ret = access_ok(VERIFY_WRITE, ptr,
-				length) ? 0 : -EFAULT;
-	if (ret)
-		return ret;
+	if (!access_ok(VERIFY_WRITE, ptr, length))
+		return -EFAULT;
 
 	ops.ooblen = length;
 	ops.ooboffs = start & (mtd->writesize - 1);
@@ -469,7 +467,7 @@ static int mtd_do_readoob(struct file *file, struct mtd_info *mtd,
 		return -ENOMEM;
 
 	start &= ~((uint64_t)mtd->writesize - 1);
-	ret = mtd->read_oob(mtd, start, &ops);
+	ret = mtd_read_oob(mtd, start, &ops);
 
 	if (put_user(ops.oobretlen, retp))
 		ret = -EFAULT;
@@ -530,7 +528,7 @@ static int shrink_ecclayout(const struct nand_ecclayout *from,
 	return 0;
 }
 
-static int mtd_blkpg_ioctl(struct mtd_info *mtd,
+static int mtdchar_blkpg_ioctl(struct mtd_info *mtd,
 			   struct blkpg_ioctl_arg __user *arg)
 {
 	struct blkpg_ioctl_arg a;
@@ -566,7 +564,7 @@ static int mtd_blkpg_ioctl(struct mtd_info *mtd,
 	}
 }
 
-static int mtd_write_ioctl(struct mtd_info *mtd,
+static int mtdchar_write_ioctl(struct mtd_info *mtd,
 		struct mtd_write_req __user *argp)
 {
 	struct mtd_write_req req;
@@ -607,7 +605,7 @@ static int mtd_write_ioctl(struct mtd_info *mtd,
 		ops.oobbuf = NULL;
 	}
 
-	ret = mtd->write_oob(mtd, (loff_t)req.start, &ops);
+	ret = mtd_write_oob(mtd, (loff_t)req.start, &ops);
 
 	kfree(ops.datbuf);
 	kfree(ops.oobbuf);
@@ -615,7 +613,7 @@ static int mtd_write_ioctl(struct mtd_info *mtd,
 	return ret;
 }
 
-static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
+static int mtdchar_ioctl(struct file *file, u_int cmd, u_long arg)
 {
 	struct mtd_file_info *mfi = file->private_data;
 	struct mtd_info *mtd = mfi->mtd;
@@ -729,7 +727,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 			  wq_head is no longer there when the
 			  callback routine tries to wake us up.
 			*/
-			ret = mtd->erase(mtd, erase);
+			ret = mtd_erase(mtd, erase);
 			if (!ret) {
 				set_current_state(TASK_UNINTERRUPTIBLE);
 				add_wait_queue(&waitq, &wait);
@@ -755,7 +753,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 		if (copy_from_user(&buf, argp, sizeof(buf)))
 			ret = -EFAULT;
 		else
-			ret = mtd_do_writeoob(file, mtd, buf.start, buf.length,
+			ret = mtdchar_writeoob(file, mtd, buf.start, buf.length,
 				buf.ptr, &buf_user->length);
 		break;
 	}
@@ -769,7 +767,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 		if (copy_from_user(&buf, argp, sizeof(buf)))
 			ret = -EFAULT;
 		else
-			ret = mtd_do_readoob(file, mtd, buf.start, buf.length,
+			ret = mtdchar_readoob(file, mtd, buf.start, buf.length,
 				buf.ptr, &buf_user->start);
 		break;
 	}
@@ -782,7 +780,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 		if (copy_from_user(&buf, argp, sizeof(buf)))
 			ret = -EFAULT;
 		else
-			ret = mtd_do_writeoob(file, mtd, buf.start, buf.length,
+			ret = mtdchar_writeoob(file, mtd, buf.start, buf.length,
 				(void __user *)(uintptr_t)buf.usr_ptr,
 				&buf_user->length);
 		break;
@@ -796,7 +794,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 		if (copy_from_user(&buf, argp, sizeof(buf)))
 			ret = -EFAULT;
 		else
-			ret = mtd_do_readoob(file, mtd, buf.start, buf.length,
+			ret = mtdchar_readoob(file, mtd, buf.start, buf.length,
 				(void __user *)(uintptr_t)buf.usr_ptr,
 				&buf_user->length);
 		break;
@@ -804,7 +802,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 
 	case MEMWRITE:
 	{
-		ret = mtd_write_ioctl(mtd,
+		ret = mtdchar_write_ioctl(mtd,
 		      (struct mtd_write_req __user *)arg);
 		break;
 	}
@@ -816,10 +814,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 		if (copy_from_user(&einfo, argp, sizeof(einfo)))
 			return -EFAULT;
 
-		if (!mtd->lock)
-			ret = -EOPNOTSUPP;
-		else
-			ret = mtd->lock(mtd, einfo.start, einfo.length);
+		ret = mtd_lock(mtd, einfo.start, einfo.length);
 		break;
 	}
 
@@ -830,10 +825,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 		if (copy_from_user(&einfo, argp, sizeof(einfo)))
 			return -EFAULT;
 
-		if (!mtd->unlock)
-			ret = -EOPNOTSUPP;
-		else
-			ret = mtd->unlock(mtd, einfo.start, einfo.length);
+		ret = mtd_unlock(mtd, einfo.start, einfo.length);
 		break;
 	}
 
@@ -844,10 +836,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 		if (copy_from_user(&einfo, argp, sizeof(einfo)))
 			return -EFAULT;
 
-		if (!mtd->is_locked)
-			ret = -EOPNOTSUPP;
-		else
-			ret = mtd->is_locked(mtd, einfo.start, einfo.length);
+		ret = mtd_is_locked(mtd, einfo.start, einfo.length);
 		break;
 	}
 
@@ -878,10 +867,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 
 		if (copy_from_user(&offs, argp, sizeof(loff_t)))
 			return -EFAULT;
-		if (!mtd->block_isbad)
-			ret = -EOPNOTSUPP;
-		else
-			return mtd->block_isbad(mtd, offs);
+		return mtd_block_isbad(mtd, offs);
 		break;
 	}
 
@@ -891,10 +877,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 
 		if (copy_from_user(&offs, argp, sizeof(loff_t)))
 			return -EFAULT;
-		if (!mtd->block_markbad)
-			ret = -EOPNOTSUPP;
-		else
-			return mtd->block_markbad(mtd, offs);
+		return mtd_block_markbad(mtd, offs);
 		break;
 	}
 
@@ -919,17 +902,15 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 		struct otp_info *buf = kmalloc(4096, GFP_KERNEL);
 		if (!buf)
 			return -ENOMEM;
-		ret = -EOPNOTSUPP;
 		switch (mfi->mode) {
 		case MTD_FILE_MODE_OTP_FACTORY:
-			if (mtd->get_fact_prot_info)
-				ret = mtd->get_fact_prot_info(mtd, buf, 4096);
+			ret = mtd_get_fact_prot_info(mtd, buf, 4096);
 			break;
 		case MTD_FILE_MODE_OTP_USER:
-			if (mtd->get_user_prot_info)
-				ret = mtd->get_user_prot_info(mtd, buf, 4096);
+			ret = mtd_get_user_prot_info(mtd, buf, 4096);
 			break;
 		default:
+			ret = -EINVAL;
 			break;
 		}
 		if (ret >= 0) {
@@ -953,9 +934,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 			return -EINVAL;
 		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, oinfo.start, oinfo.length);
+		ret = mtd_lock_user_prot_reg(mtd, oinfo.start, oinfo.length);
 		break;
 	}
 #endif
@@ -999,7 +978,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 			break;
 
 		case MTD_FILE_MODE_RAW:
-			if (!mtd->read_oob || !mtd->write_oob)
+			if (!mtd_has_oob(mtd))
 				return -EOPNOTSUPP;
 			mfi->mode = arg;
 
@@ -1014,7 +993,7 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 
 	case BLKPG:
 	{
-		ret = mtd_blkpg_ioctl(mtd,
+		ret = mtdchar_blkpg_ioctl(mtd,
 		      (struct blkpg_ioctl_arg __user *)arg);
 		break;
 	}
@@ -1033,12 +1012,12 @@ static int mtd_ioctl(struct file *file, u_int cmd, u_long arg)
 	return ret;
 } /* memory_ioctl */
 
-static long mtd_unlocked_ioctl(struct file *file, u_int cmd, u_long arg)
+static long mtdchar_unlocked_ioctl(struct file *file, u_int cmd, u_long arg)
 {
 	int ret;
 
 	mutex_lock(&mtd_mutex);
-	ret = mtd_ioctl(file, cmd, arg);
+	ret = mtdchar_ioctl(file, cmd, arg);
 	mutex_unlock(&mtd_mutex);
 
 	return ret;
@@ -1055,7 +1034,7 @@ struct mtd_oob_buf32 {
 #define MEMWRITEOOB32		_IOWR('M', 3, struct mtd_oob_buf32)
 #define MEMREADOOB32		_IOWR('M', 4, struct mtd_oob_buf32)
 
-static long mtd_compat_ioctl(struct file *file, unsigned int cmd,
+static long mtdchar_compat_ioctl(struct file *file, unsigned int cmd,
 	unsigned long arg)
 {
 	struct mtd_file_info *mfi = file->private_data;
@@ -1074,7 +1053,7 @@ static long mtd_compat_ioctl(struct file *file, unsigned int cmd,
 		if (copy_from_user(&buf, argp, sizeof(buf)))
 			ret = -EFAULT;
 		else
-			ret = mtd_do_writeoob(file, mtd, buf.start,
+			ret = mtdchar_writeoob(file, mtd, buf.start,
 				buf.length, compat_ptr(buf.ptr),
 				&buf_user->length);
 		break;
@@ -1089,13 +1068,13 @@ static long mtd_compat_ioctl(struct file *file, unsigned int cmd,
 		if (copy_from_user(&buf, argp, sizeof(buf)))
 			ret = -EFAULT;
 		else
-			ret = mtd_do_readoob(file, mtd, buf.start,
+			ret = mtdchar_readoob(file, mtd, buf.start,
 				buf.length, compat_ptr(buf.ptr),
 				&buf_user->start);
 		break;
 	}
 	default:
-		ret = mtd_ioctl(file, cmd, (unsigned long)argp);
+		ret = mtdchar_ioctl(file, cmd, (unsigned long)argp);
 	}
 
 	mutex_unlock(&mtd_mutex);
@@ -1111,7 +1090,7 @@ static long mtd_compat_ioctl(struct file *file, unsigned int cmd,
  *   mappings)
  */
 #ifndef CONFIG_MMU
-static unsigned long mtd_get_unmapped_area(struct file *file,
+static unsigned long mtdchar_get_unmapped_area(struct file *file,
 					   unsigned long addr,
 					   unsigned long len,
 					   unsigned long pgoff,
@@ -1119,32 +1098,28 @@ static unsigned long mtd_get_unmapped_area(struct file *file,
 {
 	struct mtd_file_info *mfi = file->private_data;
 	struct mtd_info *mtd = mfi->mtd;
+	unsigned long offset;
+	int ret;
 
-	if (mtd->get_unmapped_area) {
-		unsigned long offset;
-
-		if (addr != 0)
-			return (unsigned long) -EINVAL;
-
-		if (len > mtd->size || pgoff >= (mtd->size >> PAGE_SHIFT))
-			return (unsigned long) -EINVAL;
+	if (addr != 0)
+		return (unsigned long) -EINVAL;
 
-		offset = pgoff << PAGE_SHIFT;
-		if (offset > mtd->size - len)
-			return (unsigned long) -EINVAL;
+	if (len > mtd->size || pgoff >= (mtd->size >> PAGE_SHIFT))
+		return (unsigned long) -EINVAL;
 
-		return mtd->get_unmapped_area(mtd, len, offset, flags);
-	}
+	offset = pgoff << PAGE_SHIFT;
+	if (offset > mtd->size - len)
+		return (unsigned long) -EINVAL;
 
-	/* can't map directly */
-	return (unsigned long) -ENOSYS;
+	ret = mtd_get_unmapped_area(mtd, len, offset, flags);
+	return ret == -EOPNOTSUPP ? -ENOSYS : ret;
 }
 #endif
 
 /*
  * set up a mapping for shared memory segments
  */
-static int mtd_mmap(struct file *file, struct vm_area_struct *vma)
+static int mtdchar_mmap(struct file *file, struct vm_area_struct *vma)
 {
 #ifdef CONFIG_MMU
 	struct mtd_file_info *mfi = file->private_data;
@@ -1185,18 +1160,18 @@ static int mtd_mmap(struct file *file, struct vm_area_struct *vma)
 
 static const struct file_operations mtd_fops = {
 	.owner		= THIS_MODULE,
-	.llseek		= mtd_lseek,
-	.read		= mtd_read,
-	.write		= mtd_write,
-	.unlocked_ioctl	= mtd_unlocked_ioctl,
+	.llseek		= mtdchar_lseek,
+	.read		= mtdchar_read,
+	.write		= mtdchar_write,
+	.unlocked_ioctl	= mtdchar_unlocked_ioctl,
 #ifdef CONFIG_COMPAT
-	.compat_ioctl	= mtd_compat_ioctl,
+	.compat_ioctl	= mtdchar_compat_ioctl,
 #endif
-	.open		= mtd_open,
-	.release	= mtd_close,
-	.mmap		= mtd_mmap,
+	.open		= mtdchar_open,
+	.release	= mtdchar_close,
+	.mmap		= mtdchar_mmap,
 #ifndef CONFIG_MMU
-	.get_unmapped_area = mtd_get_unmapped_area,
+	.get_unmapped_area = mtdchar_get_unmapped_area,
 #endif
 };
 
diff --git a/drivers/mtd/mtdconcat.c b/drivers/mtd/mtdconcat.c
index 6df4d4d4eb92..1ed5103b219b 100644
--- a/drivers/mtd/mtdconcat.c
+++ b/drivers/mtd/mtdconcat.c
@@ -91,7 +91,7 @@ concat_read(struct mtd_info *mtd, loff_t from, size_t len,
 			/* Entire transaction goes into this subdev */
 			size = len;
 
-		err = subdev->read(subdev, from, size, &retsize, buf);
+		err = mtd_read(subdev, from, size, &retsize, buf);
 
 		/* Save information about bitflips! */
 		if (unlikely(err)) {
@@ -148,7 +148,7 @@ concat_write(struct mtd_info *mtd, loff_t to, size_t len,
 		if (!(subdev->flags & MTD_WRITEABLE))
 			err = -EROFS;
 		else
-			err = subdev->write(subdev, to, size, &retsize, buf);
+			err = mtd_write(subdev, to, size, &retsize, buf);
 
 		if (err)
 			break;
@@ -227,8 +227,9 @@ concat_writev(struct mtd_info *mtd, const struct kvec *vecs,
 		if (!(subdev->flags & MTD_WRITEABLE))
 			err = -EROFS;
 		else
-			err = subdev->writev(subdev, &vecs_copy[entry_low],
-				entry_high - entry_low + 1, to, &retsize);
+			err = mtd_writev(subdev, &vecs_copy[entry_low],
+					 entry_high - entry_low + 1, to,
+					 &retsize);
 
 		vecs_copy[entry_high].iov_len = old_iov_len - size;
 		vecs_copy[entry_high].iov_base += size;
@@ -273,7 +274,7 @@ concat_read_oob(struct mtd_info *mtd, loff_t from, struct mtd_oob_ops *ops)
 		if (from + devops.len > subdev->size)
 			devops.len = subdev->size - from;
 
-		err = subdev->read_oob(subdev, from, &devops);
+		err = mtd_read_oob(subdev, from, &devops);
 		ops->retlen += devops.retlen;
 		ops->oobretlen += devops.oobretlen;
 
@@ -333,7 +334,7 @@ concat_write_oob(struct mtd_info *mtd, loff_t to, struct mtd_oob_ops *ops)
 		if (to + devops.len > subdev->size)
 			devops.len = subdev->size - to;
 
-		err = subdev->write_oob(subdev, to, &devops);
+		err = mtd_write_oob(subdev, to, &devops);
 		ops->retlen += devops.oobretlen;
 		if (err)
 			return err;
@@ -379,7 +380,7 @@ static int concat_dev_erase(struct mtd_info *mtd, struct erase_info *erase)
 	 * FIXME: Allow INTERRUPTIBLE. Which means
 	 * not having the wait_queue head on the stack.
 	 */
-	err = mtd->erase(mtd, erase);
+	err = mtd_erase(mtd, erase);
 	if (!err) {
 		set_current_state(TASK_UNINTERRUPTIBLE);
 		add_wait_queue(&waitq, &wait);
@@ -554,12 +555,9 @@ static int concat_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 		else
 			size = len;
 
-		if (subdev->lock) {
-			err = subdev->lock(subdev, ofs, size);
-			if (err)
-				break;
-		} else
-			err = -EOPNOTSUPP;
+		err = mtd_lock(subdev, ofs, size);
+		if (err)
+			break;
 
 		len -= size;
 		if (len == 0)
@@ -594,12 +592,9 @@ static int concat_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 		else
 			size = len;
 
-		if (subdev->unlock) {
-			err = subdev->unlock(subdev, ofs, size);
-			if (err)
-				break;
-		} else
-			err = -EOPNOTSUPP;
+		err = mtd_unlock(subdev, ofs, size);
+		if (err)
+			break;
 
 		len -= size;
 		if (len == 0)
@@ -619,7 +614,7 @@ static void concat_sync(struct mtd_info *mtd)
 
 	for (i = 0; i < concat->num_subdev; i++) {
 		struct mtd_info *subdev = concat->subdev[i];
-		subdev->sync(subdev);
+		mtd_sync(subdev);
 	}
 }
 
@@ -630,7 +625,7 @@ static int concat_suspend(struct mtd_info *mtd)
 
 	for (i = 0; i < concat->num_subdev; i++) {
 		struct mtd_info *subdev = concat->subdev[i];
-		if ((rc = subdev->suspend(subdev)) < 0)
+		if ((rc = mtd_suspend(subdev)) < 0)
 			return rc;
 	}
 	return rc;
@@ -643,7 +638,7 @@ static void concat_resume(struct mtd_info *mtd)
 
 	for (i = 0; i < concat->num_subdev; i++) {
 		struct mtd_info *subdev = concat->subdev[i];
-		subdev->resume(subdev);
+		mtd_resume(subdev);
 	}
 }
 
@@ -652,7 +647,7 @@ static int concat_block_isbad(struct mtd_info *mtd, loff_t ofs)
 	struct mtd_concat *concat = CONCAT(mtd);
 	int i, res = 0;
 
-	if (!concat->subdev[0]->block_isbad)
+	if (!mtd_can_have_bb(concat->subdev[0]))
 		return res;
 
 	if (ofs > mtd->size)
@@ -666,7 +661,7 @@ static int concat_block_isbad(struct mtd_info *mtd, loff_t ofs)
 			continue;
 		}
 
-		res = subdev->block_isbad(subdev, ofs);
+		res = mtd_block_isbad(subdev, ofs);
 		break;
 	}
 
@@ -678,7 +673,7 @@ static int concat_block_markbad(struct mtd_info *mtd, loff_t ofs)
 	struct mtd_concat *concat = CONCAT(mtd);
 	int i, err = -EINVAL;
 
-	if (!concat->subdev[0]->block_markbad)
+	if (!mtd_can_have_bb(concat->subdev[0]))
 		return 0;
 
 	if (ofs > mtd->size)
@@ -692,7 +687,7 @@ static int concat_block_markbad(struct mtd_info *mtd, loff_t ofs)
 			continue;
 		}
 
-		err = subdev->block_markbad(subdev, ofs);
+		err = mtd_block_markbad(subdev, ofs);
 		if (!err)
 			mtd->ecc_stats.badblocks++;
 		break;
@@ -725,11 +720,7 @@ static unsigned long concat_get_unmapped_area(struct mtd_info *mtd,
 		if (offset + len > subdev->size)
 			return (unsigned long) -EINVAL;
 
-		if (subdev->get_unmapped_area)
-			return subdev->get_unmapped_area(subdev, len, offset,
-							 flags);
-
-		break;
+		return mtd_get_unmapped_area(subdev, len, offset, flags);
 	}
 
 	return (unsigned long) -ENOSYS;
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index b01993ea260e..6ae9ca01388b 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -107,7 +107,8 @@ static LIST_HEAD(mtd_notifiers);
  */
 static void mtd_release(struct device *dev)
 {
-	dev_t index = MTD_DEVT(dev_to_mtd(dev)->index);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
+	dev_t index = MTD_DEVT(mtd->index);
 
 	/* remove /dev/mtdXro node if needed */
 	if (index)
@@ -116,27 +117,24 @@ static void mtd_release(struct device *dev)
 
 static int mtd_cls_suspend(struct device *dev, pm_message_t state)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 
-	if (mtd && mtd->suspend)
-		return mtd->suspend(mtd);
-	else
-		return 0;
+	return mtd_suspend(mtd);
 }
 
 static int mtd_cls_resume(struct device *dev)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
-	
+	struct mtd_info *mtd = dev_get_drvdata(dev);
+
 	if (mtd && mtd->resume)
-		mtd->resume(mtd);
+		mtd_resume(mtd);
 	return 0;
 }
 
 static ssize_t mtd_type_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 	char *type;
 
 	switch (mtd->type) {
@@ -172,7 +170,7 @@ static DEVICE_ATTR(type, S_IRUGO, mtd_type_show, NULL);
 static ssize_t mtd_flags_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 
 	return snprintf(buf, PAGE_SIZE, "0x%lx\n", (unsigned long)mtd->flags);
 
@@ -182,7 +180,7 @@ static DEVICE_ATTR(flags, S_IRUGO, mtd_flags_show, NULL);
 static ssize_t mtd_size_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 
 	return snprintf(buf, PAGE_SIZE, "%llu\n",
 		(unsigned long long)mtd->size);
@@ -193,7 +191,7 @@ static DEVICE_ATTR(size, S_IRUGO, mtd_size_show, NULL);
 static ssize_t mtd_erasesize_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 
 	return snprintf(buf, PAGE_SIZE, "%lu\n", (unsigned long)mtd->erasesize);
 
@@ -203,7 +201,7 @@ static DEVICE_ATTR(erasesize, S_IRUGO, mtd_erasesize_show, NULL);
 static ssize_t mtd_writesize_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 
 	return snprintf(buf, PAGE_SIZE, "%lu\n", (unsigned long)mtd->writesize);
 
@@ -213,7 +211,7 @@ static DEVICE_ATTR(writesize, S_IRUGO, mtd_writesize_show, NULL);
 static ssize_t mtd_subpagesize_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 	unsigned int subpagesize = mtd->writesize >> mtd->subpage_sft;
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", subpagesize);
@@ -224,7 +222,7 @@ static DEVICE_ATTR(subpagesize, S_IRUGO, mtd_subpagesize_show, NULL);
 static ssize_t mtd_oobsize_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 
 	return snprintf(buf, PAGE_SIZE, "%lu\n", (unsigned long)mtd->oobsize);
 
@@ -234,7 +232,7 @@ static DEVICE_ATTR(oobsize, S_IRUGO, mtd_oobsize_show, NULL);
 static ssize_t mtd_numeraseregions_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", mtd->numeraseregions);
 
@@ -245,7 +243,7 @@ static DEVICE_ATTR(numeraseregions, S_IRUGO, mtd_numeraseregions_show,
 static ssize_t mtd_name_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct mtd_info *mtd = dev_to_mtd(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 
 	return snprintf(buf, PAGE_SIZE, "%s\n", mtd->name);
 
@@ -338,9 +336,9 @@ int add_mtd_device(struct mtd_info *mtd)
 	mtd->writesize_mask = (1 << mtd->writesize_shift) - 1;
 
 	/* Some chips always power up locked. Unlock them now */
-	if ((mtd->flags & MTD_WRITEABLE)
-	    && (mtd->flags & MTD_POWERUP_LOCK) && mtd->unlock) {
-		if (mtd->unlock(mtd, 0, mtd->size))
+	if ((mtd->flags & MTD_WRITEABLE) && (mtd->flags & MTD_POWERUP_LOCK)) {
+		error = mtd_unlock(mtd, 0, mtd->size);
+		if (error && error != -EOPNOTSUPP)
 			printk(KERN_WARNING
 			       "%s: unlock failed, writes may not work\n",
 			       mtd->name);
@@ -516,7 +514,6 @@ EXPORT_SYMBOL_GPL(mtd_device_unregister);
  *	or removal of MTD devices. Causes the 'add' callback to be immediately
  *	invoked for each MTD device currently present in the system.
  */
-
 void register_mtd_user (struct mtd_notifier *new)
 {
 	struct mtd_info *mtd;
@@ -532,6 +529,7 @@ void register_mtd_user (struct mtd_notifier *new)
 
 	mutex_unlock(&mtd_table_mutex);
 }
+EXPORT_SYMBOL_GPL(register_mtd_user);
 
 /**
  *	unregister_mtd_user - unregister a 'user' of MTD devices.
@@ -542,7 +540,6 @@ void register_mtd_user (struct mtd_notifier *new)
  *	'remove' callback to be immediately invoked for each MTD device
  *	currently present in the system.
  */
-
 int unregister_mtd_user (struct mtd_notifier *old)
 {
 	struct mtd_info *mtd;
@@ -558,7 +555,7 @@ int unregister_mtd_user (struct mtd_notifier *old)
 	mutex_unlock(&mtd_table_mutex);
 	return 0;
 }
-
+EXPORT_SYMBOL_GPL(unregister_mtd_user);
 
 /**
  *	get_mtd_device - obtain a validated handle for an MTD device
@@ -571,7 +568,6 @@ int unregister_mtd_user (struct mtd_notifier *old)
  *	both, return the num'th driver only if its address matches. Return
  *	error code if not.
  */
-
 struct mtd_info *get_mtd_device(struct mtd_info *mtd, int num)
 {
 	struct mtd_info *ret = NULL, *other;
@@ -604,6 +600,7 @@ out:
 	mutex_unlock(&mtd_table_mutex);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(get_mtd_device);
 
 
 int __get_mtd_device(struct mtd_info *mtd)
@@ -624,6 +621,7 @@ int __get_mtd_device(struct mtd_info *mtd)
 	mtd->usecount++;
 	return 0;
 }
+EXPORT_SYMBOL_GPL(__get_mtd_device);
 
 /**
  *	get_mtd_device_nm - obtain a validated handle for an MTD device by
@@ -633,7 +631,6 @@ int __get_mtd_device(struct mtd_info *mtd)
  * 	This function returns MTD device description structure in case of
  * 	success and an error code in case of failure.
  */
-
 struct mtd_info *get_mtd_device_nm(const char *name)
 {
 	int err = -ENODEV;
@@ -662,6 +659,7 @@ out_unlock:
 	mutex_unlock(&mtd_table_mutex);
 	return ERR_PTR(err);
 }
+EXPORT_SYMBOL_GPL(get_mtd_device_nm);
 
 void put_mtd_device(struct mtd_info *mtd)
 {
@@ -670,6 +668,7 @@ void put_mtd_device(struct mtd_info *mtd)
 	mutex_unlock(&mtd_table_mutex);
 
 }
+EXPORT_SYMBOL_GPL(put_mtd_device);
 
 void __put_mtd_device(struct mtd_info *mtd)
 {
@@ -681,39 +680,65 @@ void __put_mtd_device(struct mtd_info *mtd)
 
 	module_put(mtd->owner);
 }
+EXPORT_SYMBOL_GPL(__put_mtd_device);
 
-/* default_mtd_writev - default mtd writev method for MTD devices that
- *			don't implement their own
+/*
+ * default_mtd_writev - the default writev method
+ * @mtd: mtd device description object pointer
+ * @vecs: the vectors to write
+ * @count: count of vectors in @vecs
+ * @to: the MTD device offset to write to
+ * @retlen: on exit contains the count of bytes written to the MTD device.
+ *
+ * This function returns zero in case of success and a negative error code in
+ * case of failure.
  */
-
-int default_mtd_writev(struct mtd_info *mtd, const struct kvec *vecs,
-		       unsigned long count, loff_t to, size_t *retlen)
+static int default_mtd_writev(struct mtd_info *mtd, const struct kvec *vecs,
+			      unsigned long count, loff_t to, size_t *retlen)
 {
 	unsigned long i;
 	size_t totlen = 0, thislen;
 	int ret = 0;
 
-	if(!mtd->write) {
-		ret = -EROFS;
-	} else {
-		for (i=0; i<count; i++) {
-			if (!vecs[i].iov_len)
-				continue;
-			ret = mtd->write(mtd, to, vecs[i].iov_len, &thislen, vecs[i].iov_base);
-			totlen += thislen;
-			if (ret || thislen != vecs[i].iov_len)
-				break;
-			to += vecs[i].iov_len;
-		}
+	for (i = 0; i < count; i++) {
+		if (!vecs[i].iov_len)
+			continue;
+		ret = mtd_write(mtd, to, vecs[i].iov_len, &thislen,
+				vecs[i].iov_base);
+		totlen += thislen;
+		if (ret || thislen != vecs[i].iov_len)
+			break;
+		to += vecs[i].iov_len;
 	}
-	if (retlen)
-		*retlen = totlen;
+	*retlen = totlen;
 	return ret;
 }
 
+/*
+ * mtd_writev - the vector-based MTD write method
+ * @mtd: mtd device description object pointer
+ * @vecs: the vectors to write
+ * @count: count of vectors in @vecs
+ * @to: the MTD device offset to write to
+ * @retlen: on exit contains the count of bytes written to the MTD device.
+ *
+ * This function returns zero in case of success and a negative error code in
+ * case of failure.
+ */
+int mtd_writev(struct mtd_info *mtd, const struct kvec *vecs,
+	       unsigned long count, loff_t to, size_t *retlen)
+{
+	*retlen = 0;
+	if (!mtd->writev)
+		return default_mtd_writev(mtd, vecs, count, to, retlen);
+	return mtd->writev(mtd, vecs, count, to, retlen);
+}
+EXPORT_SYMBOL_GPL(mtd_writev);
+
 /**
  * mtd_kmalloc_up_to - allocate a contiguous buffer up to the specified size
- * @size: A pointer to the ideal or maximum size of the allocation. Points
+ * @mtd: mtd device description object pointer
+ * @size: a pointer to the ideal or maximum size of the allocation, points
  *        to the actual allocation size on success.
  *
  * This routine attempts to allocate a contiguous kernel buffer up to
@@ -758,15 +783,6 @@ void *mtd_kmalloc_up_to(const struct mtd_info *mtd, size_t *size)
 	 */
 	return kmalloc(*size, GFP_KERNEL);
 }
-
-EXPORT_SYMBOL_GPL(get_mtd_device);
-EXPORT_SYMBOL_GPL(get_mtd_device_nm);
-EXPORT_SYMBOL_GPL(__get_mtd_device);
-EXPORT_SYMBOL_GPL(put_mtd_device);
-EXPORT_SYMBOL_GPL(__put_mtd_device);
-EXPORT_SYMBOL_GPL(register_mtd_user);
-EXPORT_SYMBOL_GPL(unregister_mtd_user);
-EXPORT_SYMBOL_GPL(default_mtd_writev);
 EXPORT_SYMBOL_GPL(mtd_kmalloc_up_to);
 
 #ifdef CONFIG_PROC_FS
diff --git a/drivers/mtd/mtdoops.c b/drivers/mtd/mtdoops.c
index 1e2fa6236705..db8e8272d69b 100644
--- a/drivers/mtd/mtdoops.c
+++ b/drivers/mtd/mtdoops.c
@@ -112,7 +112,7 @@ static int mtdoops_erase_block(struct mtdoops_context *cxt, int offset)
 	set_current_state(TASK_INTERRUPTIBLE);
 	add_wait_queue(&wait_q, &wait);
 
-	ret = mtd->erase(mtd, &erase);
+	ret = mtd_erase(mtd, &erase);
 	if (ret) {
 		set_current_state(TASK_RUNNING);
 		remove_wait_queue(&wait_q, &wait);
@@ -169,8 +169,8 @@ static void mtdoops_workfunc_erase(struct work_struct *work)
 			cxt->nextpage = 0;
 	}
 
-	while (mtd->block_isbad) {
-		ret = mtd->block_isbad(mtd, cxt->nextpage * record_size);
+	while (mtd_can_have_bb(mtd)) {
+		ret = mtd_block_isbad(mtd, cxt->nextpage * record_size);
 		if (!ret)
 			break;
 		if (ret < 0) {
@@ -199,8 +199,8 @@ badblock:
 		return;
 	}
 
-	if (mtd->block_markbad && ret == -EIO) {
-		ret = mtd->block_markbad(mtd, cxt->nextpage * record_size);
+	if (mtd_can_have_bb(mtd) && ret == -EIO) {
+		ret = mtd_block_markbad(mtd, cxt->nextpage * record_size);
 		if (ret < 0) {
 			printk(KERN_ERR "mtdoops: block_markbad failed, aborting\n");
 			return;
@@ -221,12 +221,16 @@ static void mtdoops_write(struct mtdoops_context *cxt, int panic)
 	hdr[0] = cxt->nextcount;
 	hdr[1] = MTDOOPS_KERNMSG_MAGIC;
 
-	if (panic)
-		ret = mtd->panic_write(mtd, cxt->nextpage * record_size,
-					record_size, &retlen, cxt->oops_buf);
-	else
-		ret = mtd->write(mtd, cxt->nextpage * record_size,
-					record_size, &retlen, cxt->oops_buf);
+	if (panic) {
+		ret = mtd_panic_write(mtd, cxt->nextpage * record_size,
+				      record_size, &retlen, cxt->oops_buf);
+		if (ret == -EOPNOTSUPP) {
+			printk(KERN_ERR "mtdoops: Cannot write from panic without panic_write\n");
+			return;
+		}
+	} else
+		ret = mtd_write(mtd, cxt->nextpage * record_size,
+				record_size, &retlen, cxt->oops_buf);
 
 	if (retlen != record_size || ret < 0)
 		printk(KERN_ERR "mtdoops: write failure at %ld (%td of %ld written), error %d\n",
@@ -253,10 +257,13 @@ static void find_next_position(struct mtdoops_context *cxt)
 	size_t retlen;
 
 	for (page = 0; page < cxt->oops_pages; page++) {
+		if (mtd_can_have_bb(mtd) &&
+		    mtd_block_isbad(mtd, page * record_size))
+			continue;
 		/* Assume the page is used */
 		mark_page_used(cxt, page);
-		ret = mtd->read(mtd, page * record_size, MTDOOPS_HEADER_SIZE,
-				&retlen, (u_char *) &count[0]);
+		ret = mtd_read(mtd, page * record_size, MTDOOPS_HEADER_SIZE,
+			       &retlen, (u_char *)&count[0]);
 		if (retlen != MTDOOPS_HEADER_SIZE ||
 				(ret < 0 && !mtd_is_bitflip(ret))) {
 			printk(KERN_ERR "mtdoops: read failure at %ld (%td of %d read), err %d\n",
@@ -327,13 +334,8 @@ static void mtdoops_do_dump(struct kmsg_dumper *dumper,
 	memcpy(dst + l1_cpy, s2 + s2_start, l2_cpy);
 
 	/* Panics must be written immediately */
-	if (reason != KMSG_DUMP_OOPS) {
-		if (!cxt->mtd->panic_write)
-			printk(KERN_ERR "mtdoops: Cannot write from panic without panic_write\n");
-		else
-			mtdoops_write(cxt, 1);
-		return;
-	}
+	if (reason != KMSG_DUMP_OOPS)
+		mtdoops_write(cxt, 1);
 
 	/* For other cases, schedule work to write it "nicely" */
 	schedule_work(&cxt->work_write);
@@ -369,7 +371,7 @@ static void mtdoops_notify_add(struct mtd_info *mtd)
 
 	/* oops_page_used is a bit field */
 	cxt->oops_page_used = vmalloc(DIV_ROUND_UP(mtdoops_pages,
-			BITS_PER_LONG));
+			BITS_PER_LONG) * sizeof(unsigned long));
 	if (!cxt->oops_page_used) {
 		printk(KERN_ERR "mtdoops: could not allocate page array\n");
 		return;
diff --git a/drivers/mtd/mtdpart.c b/drivers/mtd/mtdpart.c
index a0bd2de4752b..a3d44c3416b4 100644
--- a/drivers/mtd/mtdpart.c
+++ b/drivers/mtd/mtdpart.c
@@ -70,8 +70,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,
-				   len, retlen, buf);
+	res = mtd_read(part->master, from + part->offset, len, retlen, buf);
 	if (unlikely(res)) {
 		if (mtd_is_bitflip(res))
 			mtd->ecc_stats.corrected += part->master->ecc_stats.corrected - stats.corrected;
@@ -89,15 +88,15 @@ static int part_point(struct mtd_info *mtd, loff_t from, size_t len,
 		len = 0;
 	else if (from + len > mtd->size)
 		len = mtd->size - from;
-	return part->master->point (part->master, from + part->offset,
-				    len, retlen, virt, phys);
+	return mtd_point(part->master, from + part->offset, len, retlen,
+			 virt, phys);
 }
 
 static void part_unpoint(struct mtd_info *mtd, loff_t from, size_t len)
 {
 	struct mtd_part *part = PART(mtd);
 
-	part->master->unpoint(part->master, from + part->offset, len);
+	mtd_unpoint(part->master, from + part->offset, len);
 }
 
 static unsigned long part_get_unmapped_area(struct mtd_info *mtd,
@@ -108,8 +107,7 @@ static unsigned long part_get_unmapped_area(struct mtd_info *mtd,
 	struct mtd_part *part = PART(mtd);
 
 	offset += part->offset;
-	return part->master->get_unmapped_area(part->master, len, offset,
-					       flags);
+	return mtd_get_unmapped_area(part->master, len, offset, flags);
 }
 
 static int part_read_oob(struct mtd_info *mtd, loff_t from,
@@ -140,7 +138,7 @@ static int part_read_oob(struct mtd_info *mtd, loff_t from,
 			return -EINVAL;
 	}
 
-	res = part->master->read_oob(part->master, from + part->offset, ops);
+	res = mtd_read_oob(part->master, from + part->offset, ops);
 	if (unlikely(res)) {
 		if (mtd_is_bitflip(res))
 			mtd->ecc_stats.corrected++;
@@ -154,30 +152,28 @@ 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,
-					len, retlen, buf);
+	return mtd_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)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->get_user_prot_info(part->master, buf, len);
+	return mtd_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)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->read_fact_prot_reg(part->master, from,
-					len, retlen, buf);
+	return mtd_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)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->get_fact_prot_info(part->master, buf, len);
+	return mtd_get_fact_prot_info(part->master, buf, len);
 }
 
 static int part_write(struct mtd_info *mtd, loff_t to, size_t len,
@@ -190,8 +186,7 @@ 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,
-				    len, retlen, buf);
+	return mtd_write(part->master, to + part->offset, len, retlen, buf);
 }
 
 static int part_panic_write(struct mtd_info *mtd, loff_t to, size_t len,
@@ -204,8 +199,8 @@ 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,
-				    len, retlen, buf);
+	return mtd_panic_write(part->master, to + part->offset, len, retlen,
+			       buf);
 }
 
 static int part_write_oob(struct mtd_info *mtd, loff_t to,
@@ -220,22 +215,21 @@ static int part_write_oob(struct mtd_info *mtd, loff_t to,
 		return -EINVAL;
 	if (ops->datbuf && to + ops->len > mtd->size)
 		return -EINVAL;
-	return part->master->write_oob(part->master, to + part->offset, ops);
+	return mtd_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)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->write_user_prot_reg(part->master, from,
-					len, retlen, buf);
+	return mtd_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)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->lock_user_prot_reg(part->master, from, len);
+	return mtd_lock_user_prot_reg(part->master, from, len);
 }
 
 static int part_writev(struct mtd_info *mtd, const struct kvec *vecs,
@@ -244,8 +238,8 @@ static int part_writev(struct mtd_info *mtd, const struct kvec *vecs,
 	struct mtd_part *part = PART(mtd);
 	if (!(mtd->flags & MTD_WRITEABLE))
 		return -EROFS;
-	return part->master->writev(part->master, vecs, count,
-					to + part->offset, retlen);
+	return mtd_writev(part->master, vecs, count, to + part->offset,
+			  retlen);
 }
 
 static int part_erase(struct mtd_info *mtd, struct erase_info *instr)
@@ -257,7 +251,7 @@ static int part_erase(struct mtd_info *mtd, struct erase_info *instr)
 	if (instr->addr >= mtd->size)
 		return -EINVAL;
 	instr->addr += part->offset;
-	ret = part->master->erase(part->master, instr);
+	ret = mtd_erase(part->master, instr);
 	if (ret) {
 		if (instr->fail_addr != MTD_FAIL_ADDR_UNKNOWN)
 			instr->fail_addr -= part->offset;
@@ -285,7 +279,7 @@ static int part_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 	struct mtd_part *part = PART(mtd);
 	if ((len + ofs) > mtd->size)
 		return -EINVAL;
-	return part->master->lock(part->master, ofs + part->offset, len);
+	return mtd_lock(part->master, ofs + part->offset, len);
 }
 
 static int part_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
@@ -293,7 +287,7 @@ static int part_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 	struct mtd_part *part = PART(mtd);
 	if ((len + ofs) > mtd->size)
 		return -EINVAL;
-	return part->master->unlock(part->master, ofs + part->offset, len);
+	return mtd_unlock(part->master, ofs + part->offset, len);
 }
 
 static int part_is_locked(struct mtd_info *mtd, loff_t ofs, uint64_t len)
@@ -301,25 +295,25 @@ static int part_is_locked(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 	struct mtd_part *part = PART(mtd);
 	if ((len + ofs) > mtd->size)
 		return -EINVAL;
-	return part->master->is_locked(part->master, ofs + part->offset, len);
+	return mtd_is_locked(part->master, ofs + part->offset, len);
 }
 
 static void part_sync(struct mtd_info *mtd)
 {
 	struct mtd_part *part = PART(mtd);
-	part->master->sync(part->master);
+	mtd_sync(part->master);
 }
 
 static int part_suspend(struct mtd_info *mtd)
 {
 	struct mtd_part *part = PART(mtd);
-	return part->master->suspend(part->master);
+	return mtd_suspend(part->master);
 }
 
 static void part_resume(struct mtd_info *mtd)
 {
 	struct mtd_part *part = PART(mtd);
-	part->master->resume(part->master);
+	mtd_resume(part->master);
 }
 
 static int part_block_isbad(struct mtd_info *mtd, loff_t ofs)
@@ -328,7 +322,7 @@ static int part_block_isbad(struct mtd_info *mtd, loff_t ofs)
 	if (ofs >= mtd->size)
 		return -EINVAL;
 	ofs += part->offset;
-	return part->master->block_isbad(part->master, ofs);
+	return mtd_block_isbad(part->master, ofs);
 }
 
 static int part_block_markbad(struct mtd_info *mtd, loff_t ofs)
@@ -341,7 +335,7 @@ static int part_block_markbad(struct mtd_info *mtd, loff_t ofs)
 	if (ofs >= mtd->size)
 		return -EINVAL;
 	ofs += part->offset;
-	res = part->master->block_markbad(part->master, ofs);
+	res = mtd_block_markbad(part->master, ofs);
 	if (!res)
 		mtd->ecc_stats.badblocks++;
 	return res;
@@ -559,8 +553,7 @@ static struct mtd_part *allocate_partition(struct mtd_info *master,
 		uint64_t offs = 0;
 
 		while (offs < slave->mtd.size) {
-			if (master->block_isbad(master,
-						offs + slave->offset))
+			if (mtd_block_isbad(master, offs + slave->offset))
 				slave->mtd.ecc_stats.badblocks++;
 			offs += slave->mtd.erasesize;
 		}
diff --git a/drivers/mtd/mtdswap.c b/drivers/mtd/mtdswap.c
index bd9590c723e4..c92f0f6bc130 100644
--- a/drivers/mtd/mtdswap.c
+++ b/drivers/mtd/mtdswap.c
@@ -274,12 +274,12 @@ static int mtdswap_handle_badblock(struct mtdswap_dev *d, struct swap_eb *eb)
 	eb->root = NULL;
 
 	/* badblocks not supported */
-	if (!d->mtd->block_markbad)
+	if (!mtd_can_have_bb(d->mtd))
 		return 1;
 
 	offset = mtdswap_eb_offset(d, eb);
 	dev_warn(d->dev, "Marking bad block at %08llx\n", offset);
-	ret = d->mtd->block_markbad(d->mtd, offset);
+	ret = mtd_block_markbad(d->mtd, offset);
 
 	if (ret) {
 		dev_warn(d->dev, "Mark block bad failed for block at %08llx "
@@ -312,7 +312,7 @@ static int mtdswap_handle_write_error(struct mtdswap_dev *d, struct swap_eb *eb)
 static int mtdswap_read_oob(struct mtdswap_dev *d, loff_t from,
 			struct mtd_oob_ops *ops)
 {
-	int ret = d->mtd->read_oob(d->mtd, from, ops);
+	int ret = mtd_read_oob(d->mtd, from, ops);
 
 	if (mtd_is_bitflip(ret))
 		return ret;
@@ -343,7 +343,7 @@ static int mtdswap_read_markers(struct mtdswap_dev *d, struct swap_eb *eb)
 	offset = mtdswap_eb_offset(d, eb);
 
 	/* Check first if the block is bad. */
-	if (d->mtd->block_isbad && d->mtd->block_isbad(d->mtd, offset))
+	if (mtd_can_have_bb(d->mtd) && mtd_block_isbad(d->mtd, offset))
 		return MTDSWAP_SCANNED_BAD;
 
 	ops.ooblen = 2 * d->mtd->ecclayout->oobavail;
@@ -403,7 +403,7 @@ static int mtdswap_write_marker(struct mtdswap_dev *d, struct swap_eb *eb,
 		offset = mtdswap_eb_offset(d, eb) + d->mtd->writesize;
 	}
 
-	ret = d->mtd->write_oob(d->mtd, offset , &ops);
+	ret = mtd_write_oob(d->mtd, offset, &ops);
 
 	if (ret) {
 		dev_warn(d->dev, "Write OOB failed for block at %08llx "
@@ -567,7 +567,7 @@ retry:
 	erase.len	= mtd->erasesize;
 	erase.priv	= (u_long)&wq;
 
-	ret = mtd->erase(mtd, &erase);
+	ret = mtd_erase(mtd, &erase);
 	if (ret) {
 		if (retries++ < MTDSWAP_ERASE_RETRIES) {
 			dev_warn(d->dev,
@@ -689,7 +689,7 @@ retry:
 		return ret;
 
 	writepos = (loff_t)*bp << PAGE_SHIFT;
-	ret =  mtd->write(mtd, writepos, PAGE_SIZE, &retlen, buf);
+	ret =  mtd_write(mtd, writepos, PAGE_SIZE, &retlen, buf);
 	if (ret == -EIO || mtd_is_eccerr(ret)) {
 		d->curr_write_pos--;
 		eb->active_count--;
@@ -736,7 +736,7 @@ static int mtdswap_move_block(struct mtdswap_dev *d, unsigned int oldblock,
 	retries = 0;
 
 retry:
-	ret = mtd->read(mtd, readpos, PAGE_SIZE, &retlen, d->page_buf);
+	ret = mtd_read(mtd, readpos, PAGE_SIZE, &retlen, d->page_buf);
 
 	if (ret < 0 && !mtd_is_bitflip(ret)) {
 		oldeb = d->eb_data + oldblock / d->pages_per_eblk;
@@ -946,7 +946,7 @@ static unsigned int mtdswap_eblk_passes(struct mtdswap_dev *d,
 			patt = mtdswap_test_patt(test + i);
 			memset(d->page_buf, patt, mtd->writesize);
 			memset(d->oob_buf, patt, mtd->ecclayout->oobavail);
-			ret = mtd->write_oob(mtd, pos, &ops);
+			ret = mtd_write_oob(mtd, pos, &ops);
 			if (ret)
 				goto error;
 
@@ -955,7 +955,7 @@ static unsigned int mtdswap_eblk_passes(struct mtdswap_dev *d,
 
 		pos = base;
 		for (i = 0; i < mtd_pages; i++) {
-			ret = mtd->read_oob(mtd, pos, &ops);
+			ret = mtd_read_oob(mtd, pos, &ops);
 			if (ret)
 				goto error;
 
@@ -1047,8 +1047,7 @@ static int mtdswap_flush(struct mtd_blktrans_dev *dev)
 {
 	struct mtdswap_dev *d = MTDSWAP_MBD_TO_MTDSWAP(dev);
 
-	if (d->mtd->sync)
-		d->mtd->sync(d->mtd);
+	mtd_sync(d->mtd);
 	return 0;
 }
 
@@ -1059,9 +1058,9 @@ static unsigned int mtdswap_badblocks(struct mtd_info *mtd, uint64_t size)
 
 	badcnt = 0;
 
-	if (mtd->block_isbad)
+	if (mtd_can_have_bb(mtd))
 		for (offset = 0; offset < size; offset += mtd->erasesize)
-			if (mtd->block_isbad(mtd, offset))
+			if (mtd_block_isbad(mtd, offset))
 				badcnt++;
 
 	return badcnt;
@@ -1161,7 +1160,7 @@ static int mtdswap_readsect(struct mtd_blktrans_dev *dev,
 	retries = 0;
 
 retry:
-	ret = mtd->read(mtd, readpos, PAGE_SIZE, &retlen, buf);
+	ret = mtd_read(mtd, readpos, PAGE_SIZE, &retlen, buf);
 
 	d->mtd_read_count++;
 	if (mtd_is_bitflip(ret)) {
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index dd0279249bf4..31b034b7eba3 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -110,7 +110,7 @@ config MTD_NAND_AMS_DELTA
 
 config MTD_NAND_OMAP2
 	tristate "NAND Flash device on OMAP2, OMAP3 and OMAP4"
-	depends on ARM && (ARCH_OMAP2 || ARCH_OMAP3 || ARCH_OMAP4)
+	depends on ARCH_OMAP2PLUS
 	help
           Support for NAND flash on Texas Instruments OMAP2, OMAP3 and OMAP4
 	  platforms.
diff --git a/drivers/mtd/nand/ams-delta.c b/drivers/mtd/nand/ams-delta.c
index 9e6b498c9beb..3197e9764fcd 100644
--- a/drivers/mtd/nand/ams-delta.c
+++ b/drivers/mtd/nand/ams-delta.c
@@ -280,17 +280,7 @@ static struct platform_driver ams_delta_nand_driver = {
 	},
 };
 
-static int __init ams_delta_nand_init(void)
-{
-	return platform_driver_register(&ams_delta_nand_driver);
-}
-module_init(ams_delta_nand_init);
-
-static void __exit ams_delta_nand_exit(void)
-{
-	platform_driver_unregister(&ams_delta_nand_driver);
-}
-module_exit(ams_delta_nand_exit);
+module_platform_driver(ams_delta_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Jonathan McDowell <noodles@earth.li>");
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index 23e5d77c39fc..4dd056e2e16a 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -113,7 +113,7 @@ static int cpu_has_dma(void)
  */
 static void atmel_nand_enable(struct atmel_nand_host *host)
 {
-	if (host->board->enable_pin)
+	if (gpio_is_valid(host->board->enable_pin))
 		gpio_set_value(host->board->enable_pin, 0);
 }
 
@@ -122,7 +122,7 @@ static void atmel_nand_enable(struct atmel_nand_host *host)
  */
 static void atmel_nand_disable(struct atmel_nand_host *host)
 {
-	if (host->board->enable_pin)
+	if (gpio_is_valid(host->board->enable_pin))
 		gpio_set_value(host->board->enable_pin, 1);
 }
 
@@ -492,7 +492,7 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 	nand_chip->IO_ADDR_W = host->io_base;
 	nand_chip->cmd_ctrl = atmel_nand_cmd_ctrl;
 
-	if (host->board->rdy_pin)
+	if (gpio_is_valid(host->board->rdy_pin))
 		nand_chip->dev_ready = atmel_nand_device_ready;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 1);
@@ -530,7 +530,7 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, host);
 	atmel_nand_enable(host);
 
-	if (host->board->det_pin) {
+	if (gpio_is_valid(host->board->det_pin)) {
 		if (gpio_get_value(host->board->det_pin)) {
 			printk(KERN_INFO "No SmartMedia card inserted.\n");
 			res = -ENXIO;
diff --git a/drivers/mtd/nand/bcm_umi_nand.c b/drivers/mtd/nand/bcm_umi_nand.c
index 46b58d672847..50387fd4009b 100644
--- a/drivers/mtd/nand/bcm_umi_nand.c
+++ b/drivers/mtd/nand/bcm_umi_nand.c
@@ -546,18 +546,7 @@ static struct platform_driver nand_driver = {
 	.resume = bcm_umi_nand_resume,
 };
 
-static int __init nand_init(void)
-{
-	return platform_driver_register(&nand_driver);
-}
-
-static void __exit nand_exit(void)
-{
-	platform_driver_unregister(&nand_driver);
-}
-
-module_init(nand_init);
-module_exit(nand_exit);
+module_platform_driver(nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Broadcom");
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index c153e1f77f90..6e566156956f 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -675,7 +675,9 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
 
 	davinci_nand_writel(info, A1CR_OFFSET + info->core_chipsel * 4, val);
 
-	ret = davinci_aemif_setup_timing(info->timing, info->base,
+	ret = 0;
+	if (info->timing)
+		ret = davinci_aemif_setup_timing(info->timing, info->base,
 							info->core_chipsel);
 	if (ret < 0) {
 		dev_dbg(&pdev->dev, "NAND timing values setup fail\n");
diff --git a/drivers/mtd/nand/diskonchip.c b/drivers/mtd/nand/diskonchip.c
index 5780dbab6113..df921e7a496c 100644
--- a/drivers/mtd/nand/diskonchip.c
+++ b/drivers/mtd/nand/diskonchip.c
@@ -1072,7 +1072,7 @@ static int __init find_media_headers(struct mtd_info *mtd, u_char *buf, const ch
 	size_t retlen;
 
 	for (offs = 0; offs < mtd->size; offs += mtd->erasesize) {
-		ret = mtd->read(mtd, offs, mtd->writesize, &retlen, buf);
+		ret = mtd_read(mtd, offs, mtd->writesize, &retlen, buf);
 		if (retlen != mtd->writesize)
 			continue;
 		if (ret) {
@@ -1097,7 +1097,7 @@ static int __init find_media_headers(struct mtd_info *mtd, u_char *buf, const ch
 	/* Only one mediaheader was found.  We want buf to contain a
 	   mediaheader on return, so we'll have to re-read the one we found. */
 	offs = doc->mh0_page << this->page_shift;
-	ret = mtd->read(mtd, offs, mtd->writesize, &retlen, buf);
+	ret = mtd_read(mtd, offs, mtd->writesize, &retlen, buf);
 	if (retlen != mtd->writesize) {
 		/* Insanity.  Give up. */
 		printk(KERN_ERR "Read DiskOnChip Media Header once, but can't reread it???\n");
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index eedd8ee2c9ac..7195ee6efe12 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -166,15 +166,22 @@ static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
 
 	elbc_fcm_ctrl->page = page_addr;
 
-	out_be32(&lbc->fbar,
-	         page_addr >> (chip->phys_erase_shift - chip->page_shift));
-
 	if (priv->page_size) {
+		/*
+		 * large page size chip : FPAR[PI] save the lowest 6 bits,
+		 *                        FBAR[BLK] save the other bits.
+		 */
+		out_be32(&lbc->fbar, page_addr >> 6);
 		out_be32(&lbc->fpar,
 		         ((page_addr << FPAR_LP_PI_SHIFT) & FPAR_LP_PI) |
 		         (oob ? FPAR_LP_MS : 0) | column);
 		buf_num = (page_addr & 1) << 2;
 	} else {
+		/*
+		 * small page size chip : FPAR[PI] save the lowest 5 bits,
+		 *                        FBAR[BLK] save the other bits.
+		 */
+		out_be32(&lbc->fbar, page_addr >> 5);
 		out_be32(&lbc->fpar,
 		         ((page_addr << FPAR_SP_PI_SHIFT) & FPAR_SP_PI) |
 		         (oob ? FPAR_SP_MS : 0) | column);
@@ -349,20 +356,22 @@ static void fsl_elbc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 		fsl_elbc_run_command(mtd);
 		return;
 
-	/* READID must read all 5 possible bytes while CEB is active */
 	case NAND_CMD_READID:
-		dev_vdbg(priv->dev, "fsl_elbc_cmdfunc: NAND_CMD_READID.\n");
+	case NAND_CMD_PARAM:
+		dev_vdbg(priv->dev, "fsl_elbc_cmdfunc: NAND_CMD %x\n", command);
 
 		out_be32(&lbc->fir, (FIR_OP_CM0 << FIR_OP0_SHIFT) |
 		                    (FIR_OP_UA  << FIR_OP1_SHIFT) |
 		                    (FIR_OP_RBW << FIR_OP2_SHIFT));
-		out_be32(&lbc->fcr, NAND_CMD_READID << FCR_CMD0_SHIFT);
-		/* nand_get_flash_type() reads 8 bytes of entire ID string */
-		out_be32(&lbc->fbcr, 8);
-		elbc_fcm_ctrl->read_bytes = 8;
+		out_be32(&lbc->fcr, command << FCR_CMD0_SHIFT);
+		/*
+		 * although currently it's 8 bytes for READID, we always read
+		 * the maximum 256 bytes(for PARAM)
+		 */
+		out_be32(&lbc->fbcr, 256);
+		elbc_fcm_ctrl->read_bytes = 256;
 		elbc_fcm_ctrl->use_mdr = 1;
-		elbc_fcm_ctrl->mdr = 0;
-
+		elbc_fcm_ctrl->mdr = column;
 		set_addr(mtd, 0, 0, 0);
 		fsl_elbc_run_command(mtd);
 		return;
@@ -407,9 +416,17 @@ static void fsl_elbc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 		         page_addr, column);
 
 		elbc_fcm_ctrl->column = column;
-		elbc_fcm_ctrl->oob = 0;
 		elbc_fcm_ctrl->use_mdr = 1;
 
+		if (column >= mtd->writesize) {
+			/* OOB area */
+			column -= mtd->writesize;
+			elbc_fcm_ctrl->oob = 1;
+		} else {
+			WARN_ON(column != 0);
+			elbc_fcm_ctrl->oob = 0;
+		}
+
 		fcr = (NAND_CMD_STATUS   << FCR_CMD1_SHIFT) |
 		      (NAND_CMD_SEQIN    << FCR_CMD2_SHIFT) |
 		      (NAND_CMD_PAGEPROG << FCR_CMD3_SHIFT);
@@ -434,16 +451,12 @@ static void fsl_elbc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 			         (FIR_OP_CW1 << FIR_OP6_SHIFT) |
 			         (FIR_OP_RS  << FIR_OP7_SHIFT));
 
-			if (column >= mtd->writesize) {
+			if (elbc_fcm_ctrl->oob)
 				/* OOB area --> READOOB */
-				column -= mtd->writesize;
 				fcr |= NAND_CMD_READOOB << FCR_CMD0_SHIFT;
-				elbc_fcm_ctrl->oob = 1;
-			} else {
-				WARN_ON(column != 0);
+			else
 				/* First 256 bytes --> READ0 */
 				fcr |= NAND_CMD_READ0 << FCR_CMD0_SHIFT;
-			}
 		}
 
 		out_be32(&lbc->fcr, fcr);
@@ -463,7 +476,8 @@ static void fsl_elbc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 		 */
 		if (elbc_fcm_ctrl->oob || elbc_fcm_ctrl->column != 0 ||
 		    elbc_fcm_ctrl->index != mtd->writesize + mtd->oobsize)
-			out_be32(&lbc->fbcr, elbc_fcm_ctrl->index);
+			out_be32(&lbc->fbcr,
+				elbc_fcm_ctrl->index - elbc_fcm_ctrl->column);
 		else
 			out_be32(&lbc->fbcr, 0);
 
@@ -659,9 +673,7 @@ static int fsl_elbc_chip_init_tail(struct mtd_info *mtd)
 	if (chip->pagemask & 0xff000000)
 		al++;
 
-	/* add to ECCM mode set in fsl_elbc_init */
-	priv->fmr |= (12 << FMR_CWTO_SHIFT) |  /* Timeout > 12 ms */
-	             (al << FMR_AL_SHIFT);
+	priv->fmr |= al << FMR_AL_SHIFT;
 
 	dev_dbg(priv->dev, "fsl_elbc_init: nand->numchips = %d\n",
 	        chip->numchips);
@@ -764,8 +776,10 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 	priv->mtd.priv = chip;
 	priv->mtd.owner = THIS_MODULE;
 
-	/* Set the ECCM according to the settings in bootloader.*/
-	priv->fmr = in_be32(&lbc->fmr) & FMR_ECCM;
+	/* set timeout to maximum */
+	priv->fmr = 15 << FMR_CWTO_SHIFT;
+	if (in_be32(&lbc->bank[priv->bank].or) & OR_FCM_PGS)
+		priv->fmr |= FMR_ECCM;
 
 	/* fill in nand_chip structure */
 	/* set up function call table */
@@ -971,18 +985,7 @@ static struct platform_driver fsl_elbc_nand_driver = {
 	.remove = fsl_elbc_nand_remove,
 };
 
-static int __init fsl_elbc_nand_init(void)
-{
-	return platform_driver_register(&fsl_elbc_nand_driver);
-}
-
-static void __exit fsl_elbc_nand_exit(void)
-{
-	platform_driver_unregister(&fsl_elbc_nand_driver);
-}
-
-module_init(fsl_elbc_nand_init);
-module_exit(fsl_elbc_nand_exit);
+module_platform_driver(fsl_elbc_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Freescale");
diff --git a/drivers/mtd/nand/fsl_upm.c b/drivers/mtd/nand/fsl_upm.c
index b4f3cc9f32fb..45df542b9c61 100644
--- a/drivers/mtd/nand/fsl_upm.c
+++ b/drivers/mtd/nand/fsl_upm.c
@@ -353,17 +353,7 @@ static struct platform_driver of_fun_driver = {
 	.remove		= __devexit_p(fun_remove),
 };
 
-static int __init fun_module_init(void)
-{
-	return platform_driver_register(&of_fun_driver);
-}
-module_init(fun_module_init);
-
-static void __exit fun_module_exit(void)
-{
-	platform_driver_unregister(&of_fun_driver);
-}
-module_exit(fun_module_exit);
+module_platform_driver(of_fun_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Anton Vorontsov <avorontsov@ru.mvista.com>");
diff --git a/drivers/mtd/nand/gpio.c b/drivers/mtd/nand/gpio.c
index 2c2060b2800e..27000a5f5f47 100644
--- a/drivers/mtd/nand/gpio.c
+++ b/drivers/mtd/nand/gpio.c
@@ -27,6 +27,9 @@
 #include <linux/mtd/nand.h>
 #include <linux/mtd/partitions.h>
 #include <linux/mtd/nand-gpio.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_gpio.h>
 
 struct gpiomtd {
 	void __iomem		*io_sync;
@@ -171,6 +174,96 @@ static int gpio_nand_devready(struct mtd_info *mtd)
 	return gpio_get_value(gpiomtd->plat.gpio_rdy);
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id gpio_nand_id_table[] = {
+	{ .compatible = "gpio-control-nand" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, gpio_nand_id_table);
+
+static int gpio_nand_get_config_of(const struct device *dev,
+				   struct gpio_nand_platdata *plat)
+{
+	u32 val;
+
+	if (!of_property_read_u32(dev->of_node, "bank-width", &val)) {
+		if (val == 2) {
+			plat->options |= NAND_BUSWIDTH_16;
+		} else if (val != 1) {
+			dev_err(dev, "invalid bank-width %u\n", val);
+			return -EINVAL;
+		}
+	}
+
+	plat->gpio_rdy = of_get_gpio(dev->of_node, 0);
+	plat->gpio_nce = of_get_gpio(dev->of_node, 1);
+	plat->gpio_ale = of_get_gpio(dev->of_node, 2);
+	plat->gpio_cle = of_get_gpio(dev->of_node, 3);
+	plat->gpio_nwp = of_get_gpio(dev->of_node, 4);
+
+	if (!of_property_read_u32(dev->of_node, "chip-delay", &val))
+		plat->chip_delay = val;
+
+	return 0;
+}
+
+static struct resource *gpio_nand_get_io_sync_of(struct platform_device *pdev)
+{
+	struct resource *r = devm_kzalloc(&pdev->dev, sizeof(*r), GFP_KERNEL);
+	u64 addr;
+
+	if (!r || of_property_read_u64(pdev->dev.of_node,
+				       "gpio-control-nand,io-sync-reg", &addr))
+		return NULL;
+
+	r->start = addr;
+	r->end = r->start + 0x3;
+	r->flags = IORESOURCE_MEM;
+
+	return r;
+}
+#else /* CONFIG_OF */
+#define gpio_nand_id_table NULL
+static inline int gpio_nand_get_config_of(const struct device *dev,
+					  struct gpio_nand_platdata *plat)
+{
+	return -ENOSYS;
+}
+
+static inline struct resource *
+gpio_nand_get_io_sync_of(struct platform_device *pdev)
+{
+	return NULL;
+}
+#endif /* CONFIG_OF */
+
+static inline int gpio_nand_get_config(const struct device *dev,
+				       struct gpio_nand_platdata *plat)
+{
+	int ret = gpio_nand_get_config_of(dev, plat);
+
+	if (!ret)
+		return ret;
+
+	if (dev->platform_data) {
+		memcpy(plat, dev->platform_data, sizeof(*plat));
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static inline struct resource *
+gpio_nand_get_io_sync(struct platform_device *pdev)
+{
+	struct resource *r = gpio_nand_get_io_sync_of(pdev);
+
+	if (r)
+		return r;
+
+	return platform_get_resource(pdev, IORESOURCE_MEM, 1);
+}
+
 static int __devexit gpio_nand_remove(struct platform_device *dev)
 {
 	struct gpiomtd *gpiomtd = platform_get_drvdata(dev);
@@ -178,7 +271,7 @@ static int __devexit gpio_nand_remove(struct platform_device *dev)
 
 	nand_release(&gpiomtd->mtd_info);
 
-	res = platform_get_resource(dev, IORESOURCE_MEM, 1);
+	res = gpio_nand_get_io_sync(dev);
 	iounmap(gpiomtd->io_sync);
 	if (res)
 		release_mem_region(res->start, resource_size(res));
@@ -226,9 +319,10 @@ static int __devinit gpio_nand_probe(struct platform_device *dev)
 	struct gpiomtd *gpiomtd;
 	struct nand_chip *this;
 	struct resource *res0, *res1;
-	int ret;
+	struct mtd_part_parser_data ppdata = {};
+	int ret = 0;
 
-	if (!dev->dev.platform_data)
+	if (!dev->dev.of_node && !dev->dev.platform_data)
 		return -EINVAL;
 
 	res0 = platform_get_resource(dev, IORESOURCE_MEM, 0);
@@ -248,7 +342,7 @@ static int __devinit gpio_nand_probe(struct platform_device *dev)
 		goto err_map;
 	}
 
-	res1 = platform_get_resource(dev, IORESOURCE_MEM, 1);
+	res1 = gpio_nand_get_io_sync(dev);
 	if (res1) {
 		gpiomtd->io_sync = request_and_remap(res1, 4, "NAND sync", &ret);
 		if (!gpiomtd->io_sync) {
@@ -257,7 +351,9 @@ static int __devinit gpio_nand_probe(struct platform_device *dev)
 		}
 	}
 
-	memcpy(&gpiomtd->plat, dev->dev.platform_data, sizeof(gpiomtd->plat));
+	ret = gpio_nand_get_config(&dev->dev, &gpiomtd->plat);
+	if (ret)
+		goto err_nce;
 
 	ret = gpio_request(gpiomtd->plat.gpio_nce, "NAND NCE");
 	if (ret)
@@ -316,8 +412,12 @@ static int __devinit gpio_nand_probe(struct platform_device *dev)
 		gpiomtd->plat.adjust_parts(&gpiomtd->plat,
 					   gpiomtd->mtd_info.size);
 
-	mtd_device_register(&gpiomtd->mtd_info, gpiomtd->plat.parts,
-			    gpiomtd->plat.num_parts);
+	ppdata.of_node = dev->dev.of_node;
+	ret = mtd_device_parse_register(&gpiomtd->mtd_info, NULL, &ppdata,
+					gpiomtd->plat.parts,
+					gpiomtd->plat.num_parts);
+	if (ret)
+		goto err_wp;
 	platform_set_drvdata(dev, gpiomtd);
 
 	return 0;
@@ -352,6 +452,7 @@ static struct platform_driver gpio_nand_driver = {
 	.remove		= gpio_nand_remove,
 	.driver		= {
 		.name	= "gpio-nand",
+		.of_match_table = gpio_nand_id_table,
 	},
 };
 
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
index de4db7604a3f..2a56fc6f399a 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
@@ -126,7 +126,7 @@ int gpmi_init(struct gpmi_nand_data *this)
 	struct resources *r = &this->resources;
 	int ret;
 
-	ret = clk_enable(r->clock);
+	ret = clk_prepare_enable(r->clock);
 	if (ret)
 		goto err_out;
 	ret = gpmi_reset_block(r->gpmi_regs, false);
@@ -146,7 +146,7 @@ int gpmi_init(struct gpmi_nand_data *this)
 	/* Select BCH ECC. */
 	writel(BM_GPMI_CTRL1_BCH_MODE, r->gpmi_regs + HW_GPMI_CTRL1_SET);
 
-	clk_disable(r->clock);
+	clk_disable_unprepare(r->clock);
 	return 0;
 err_out:
 	return ret;
@@ -202,7 +202,7 @@ int bch_set_geometry(struct gpmi_nand_data *this)
 	ecc_strength  = bch_geo->ecc_strength >> 1;
 	page_size     = bch_geo->page_size;
 
-	ret = clk_enable(r->clock);
+	ret = clk_prepare_enable(r->clock);
 	if (ret)
 		goto err_out;
 
@@ -229,7 +229,7 @@ int bch_set_geometry(struct gpmi_nand_data *this)
 	writel(BM_BCH_CTRL_COMPLETE_IRQ_EN,
 				r->bch_regs + HW_BCH_CTRL_SET);
 
-	clk_disable(r->clock);
+	clk_disable_unprepare(r->clock);
 	return 0;
 err_out:
 	return ret;
@@ -704,7 +704,7 @@ void gpmi_begin(struct gpmi_nand_data *this)
 	int ret;
 
 	/* Enable the clock. */
-	ret = clk_enable(r->clock);
+	ret = clk_prepare_enable(r->clock);
 	if (ret) {
 		pr_err("We failed in enable the clk\n");
 		goto err_out;
@@ -773,7 +773,7 @@ err_out:
 void gpmi_end(struct gpmi_nand_data *this)
 {
 	struct resources *r = &this->resources;
-	clk_disable(r->clock);
+	clk_disable_unprepare(r->clock);
 }
 
 /* Clears a BCH interrupt. */
diff --git a/drivers/mtd/nand/jz4740_nand.c b/drivers/mtd/nand/jz4740_nand.c
index e2664073a89b..ac3b9f255e00 100644
--- a/drivers/mtd/nand/jz4740_nand.c
+++ b/drivers/mtd/nand/jz4740_nand.c
@@ -423,17 +423,7 @@ static struct platform_driver jz_nand_driver = {
 	},
 };
 
-static int __init jz_nand_init(void)
-{
-	return platform_driver_register(&jz_nand_driver);
-}
-module_init(jz_nand_init);
-
-static void __exit jz_nand_exit(void)
-{
-	platform_driver_unregister(&jz_nand_driver);
-}
-module_exit(jz_nand_exit);
+module_platform_driver(jz_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index 5ede64706346..c240cf1af961 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -879,19 +879,7 @@ static struct platform_driver mpc5121_nfc_driver = {
 	},
 };
 
-static int __init mpc5121_nfc_init(void)
-{
-	return platform_driver_register(&mpc5121_nfc_driver);
-}
-
-module_init(mpc5121_nfc_init);
-
-static void __exit mpc5121_nfc_cleanup(void)
-{
-	platform_driver_unregister(&mpc5121_nfc_driver);
-}
-
-module_exit(mpc5121_nfc_cleanup);
+module_platform_driver(mpc5121_nfc_driver);
 
 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
 MODULE_DESCRIPTION("MPC5121 NAND MTD driver");
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 3ed9c5e4d34e..35b4565050f1 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -3132,8 +3132,8 @@ ident_done:
 	 * Bad block marker is stored in the last page of each block
 	 * on Samsung and Hynix MLC devices; stored in first two pages
 	 * of each block on Micron devices with 2KiB pages and on
-	 * SLC Samsung, Hynix, Toshiba and AMD/Spansion. All others scan
-	 * only the first page.
+	 * SLC Samsung, Hynix, Toshiba, AMD/Spansion, and Macronix.
+	 * All others scan only the first page.
 	 */
 	if ((chip->cellinfo & NAND_CI_CELLTYPE_MSK) &&
 			(*maf_id == NAND_MFR_SAMSUNG ||
@@ -3143,7 +3143,8 @@ ident_done:
 				(*maf_id == NAND_MFR_SAMSUNG ||
 				 *maf_id == NAND_MFR_HYNIX ||
 				 *maf_id == NAND_MFR_TOSHIBA ||
-				 *maf_id == NAND_MFR_AMD)) ||
+				 *maf_id == NAND_MFR_AMD ||
+				 *maf_id == NAND_MFR_MACRONIX)) ||
 			(mtd->writesize == 2048 &&
 			 *maf_id == NAND_MFR_MICRON))
 		chip->bbt_options |= NAND_BBT_SCAN2NDPAGE;
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 69148ae3bf58..20a112f591fe 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -201,7 +201,7 @@ static int read_bbt(struct mtd_info *mtd, uint8_t *buf, int page, int num,
 			from += marker_len;
 			marker_len = 0;
 		}
-		res = mtd->read(mtd, from, len, &retlen, buf);
+		res = mtd_read(mtd, from, len, &retlen, buf);
 		if (res < 0) {
 			if (mtd_is_eccerr(res)) {
 				pr_info("nand_bbt: ECC error in BBT at "
@@ -298,7 +298,7 @@ static int scan_read_raw_data(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
 	if (td->options & NAND_BBT_VERSION)
 		len++;
 
-	return mtd->read(mtd, offs, len, &retlen, buf);
+	return mtd_read(mtd, offs, len, &retlen, buf);
 }
 
 /* Scan read raw data from flash */
@@ -317,7 +317,7 @@ static int scan_read_raw_oob(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
 		ops.len = min(len, (size_t)mtd->writesize);
 		ops.oobbuf = buf + ops.len;
 
-		res = mtd->read_oob(mtd, offs, &ops);
+		res = mtd_read_oob(mtd, offs, &ops);
 
 		if (res)
 			return res;
@@ -350,7 +350,7 @@ static int scan_write_bbt(struct mtd_info *mtd, loff_t offs, size_t len,
 	ops.oobbuf = oob;
 	ops.len = len;
 
-	return mtd->write_oob(mtd, offs, &ops);
+	return mtd_write_oob(mtd, offs, &ops);
 }
 
 static u32 bbt_get_ver_offs(struct mtd_info *mtd, struct nand_bbt_descr *td)
@@ -434,7 +434,7 @@ static int scan_block_fast(struct mtd_info *mtd, struct nand_bbt_descr *bd,
 		 * Read the full oob until read_oob is fixed to handle single
 		 * byte reads for 16 bit buswidth.
 		 */
-		ret = mtd->read_oob(mtd, offs, &ops);
+		ret = mtd_read_oob(mtd, offs, &ops);
 		/* Ignore ECC errors when checking for BBM */
 		if (ret && !mtd_is_bitflip_or_eccerr(ret))
 			return ret;
@@ -756,7 +756,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 			/* Make it block aligned */
 			to &= ~((loff_t)((1 << this->bbt_erase_shift) - 1));
 			len = 1 << this->bbt_erase_shift;
-			res = mtd->read(mtd, to, len, &retlen, buf);
+			res = mtd_read(mtd, to, len, &retlen, buf);
 			if (res < 0) {
 				if (retlen != len) {
 					pr_info("nand_bbt: error reading block "
@@ -769,7 +769,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 			/* Read oob data */
 			ops.ooblen = (len >> this->page_shift) * mtd->oobsize;
 			ops.oobbuf = &buf[len];
-			res = mtd->read_oob(mtd, to + mtd->writesize, &ops);
+			res = mtd_read_oob(mtd, to + mtd->writesize, &ops);
 			if (res < 0 || ops.oobretlen != ops.ooblen)
 				goto outerr;
 
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index 00cf1b0d6053..af4fe8ca7b5e 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -73,11 +73,12 @@ struct nand_flash_dev nand_flash_ids[] = {
 #define LP_OPTIONS (NAND_SAMSUNG_LP_OPTIONS | NAND_NO_READRDY | NAND_NO_AUTOINCR)
 #define LP_OPTIONS16 (LP_OPTIONS | NAND_BUSWIDTH_16)
 
-	/*512 Megabit */
+	/* 512 Megabit */
 	{"NAND 64MiB 1,8V 8-bit",	0xA2, 0,  64, 0, LP_OPTIONS},
 	{"NAND 64MiB 1,8V 8-bit",	0xA0, 0,  64, 0, LP_OPTIONS},
 	{"NAND 64MiB 3,3V 8-bit",	0xF2, 0,  64, 0, LP_OPTIONS},
 	{"NAND 64MiB 3,3V 8-bit",	0xD0, 0,  64, 0, LP_OPTIONS},
+	{"NAND 64MiB 3,3V 8-bit",	0xF0, 0,  64, 0, LP_OPTIONS},
 	{"NAND 64MiB 1,8V 16-bit",	0xB2, 0,  64, 0, LP_OPTIONS16},
 	{"NAND 64MiB 1,8V 16-bit",	0xB0, 0,  64, 0, LP_OPTIONS16},
 	{"NAND 64MiB 3,3V 16-bit",	0xC2, 0,  64, 0, LP_OPTIONS16},
@@ -176,6 +177,7 @@ struct nand_manufacturers nand_manuf_ids[] = {
 	{NAND_MFR_HYNIX, "Hynix"},
 	{NAND_MFR_MICRON, "Micron"},
 	{NAND_MFR_AMD, "AMD"},
+	{NAND_MFR_MACRONIX, "Macronix"},
 	{0x0, "Unknown"}
 };
 
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index 34c03be77301..261f478f8cc3 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -737,7 +737,7 @@ static int parse_badblocks(struct nandsim *ns, struct mtd_info *mtd)
 			return -EINVAL;
 		}
 		offset = erase_block_no * ns->geom.secsz;
-		if (mtd->block_markbad(mtd, offset)) {
+		if (mtd_block_markbad(mtd, offset)) {
 			NS_ERR("invalid badblocks.\n");
 			return -EINVAL;
 		}
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index f8aacf48ecdd..ec688548c880 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -294,18 +294,7 @@ static struct platform_driver ndfc_driver = {
 	.remove = __devexit_p(ndfc_remove),
 };
 
-static int __init ndfc_nand_init(void)
-{
-	return platform_driver_register(&ndfc_driver);
-}
-
-static void __exit ndfc_nand_exit(void)
-{
-	platform_driver_unregister(&ndfc_driver);
-}
-
-module_init(ndfc_nand_init);
-module_exit(ndfc_nand_exit);
+module_platform_driver(ndfc_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Thomas Gleixner <tglx@linutronix.de>");
diff --git a/drivers/mtd/nand/nomadik_nand.c b/drivers/mtd/nand/nomadik_nand.c
index b463ecfb4c1a..a86aa812ca13 100644
--- a/drivers/mtd/nand/nomadik_nand.c
+++ b/drivers/mtd/nand/nomadik_nand.c
@@ -201,7 +201,7 @@ static int nomadik_nand_suspend(struct device *dev)
 	struct nomadik_nand_host *host = dev_get_drvdata(dev);
 	int ret = 0;
 	if (host)
-		ret = host->mtd.suspend(&host->mtd);
+		ret = mtd_suspend(&host->mtd);
 	return ret;
 }
 
@@ -209,7 +209,7 @@ static int nomadik_nand_resume(struct device *dev)
 {
 	struct nomadik_nand_host *host = dev_get_drvdata(dev);
 	if (host)
-		host->mtd.resume(&host->mtd);
+		mtd_resume(&host->mtd);
 	return 0;
 }
 
@@ -228,19 +228,7 @@ static struct platform_driver nomadik_nand_driver = {
 	},
 };
 
-static int __init nand_nomadik_init(void)
-{
-	pr_info("Nomadik NAND driver\n");
-	return platform_driver_register(&nomadik_nand_driver);
-}
-
-static void __exit nand_nomadik_exit(void)
-{
-	platform_driver_unregister(&nomadik_nand_driver);
-}
-
-module_init(nand_nomadik_init);
-module_exit(nand_nomadik_exit);
+module_platform_driver(nomadik_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("ST Microelectronics (sachin.verma@st.com)");
diff --git a/drivers/mtd/nand/nuc900_nand.c b/drivers/mtd/nand/nuc900_nand.c
index fa8faedfad6e..8febe46e1105 100644
--- a/drivers/mtd/nand/nuc900_nand.c
+++ b/drivers/mtd/nand/nuc900_nand.c
@@ -364,18 +364,7 @@ static struct platform_driver nuc900_nand_driver = {
 	},
 };
 
-static int __init nuc900_nand_init(void)
-{
-	return platform_driver_register(&nuc900_nand_driver);
-}
-
-static void __exit nuc900_nand_exit(void)
-{
-	platform_driver_unregister(&nuc900_nand_driver);
-}
-
-module_init(nuc900_nand_init);
-module_exit(nuc900_nand_exit);
+module_platform_driver(nuc900_nand_driver);
 
 MODULE_AUTHOR("Wan ZongShun <mcuos.com@gmail.com>");
 MODULE_DESCRIPTION("w90p910/NUC9xx nand driver!");
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index f745f00f3167..b3a883e2a22f 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -1145,20 +1145,7 @@ static struct platform_driver omap_nand_driver = {
 	},
 };
 
-static int __init omap_nand_init(void)
-{
-	pr_info("%s driver initializing\n", DRIVER_NAME);
-
-	return platform_driver_register(&omap_nand_driver);
-}
-
-static void __exit omap_nand_exit(void)
-{
-	platform_driver_unregister(&omap_nand_driver);
-}
-
-module_init(omap_nand_init);
-module_exit(omap_nand_exit);
+module_platform_driver(omap_nand_driver);
 
 MODULE_ALIAS("platform:" DRIVER_NAME);
 MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/nand/pasemi_nand.c b/drivers/mtd/nand/pasemi_nand.c
index a97264ececdb..974dbf8251c9 100644
--- a/drivers/mtd/nand/pasemi_nand.c
+++ b/drivers/mtd/nand/pasemi_nand.c
@@ -230,17 +230,7 @@ static struct platform_driver pasemi_nand_driver =
 	.remove		= pasemi_nand_remove,
 };
 
-static int __init pasemi_nand_init(void)
-{
-	return platform_driver_register(&pasemi_nand_driver);
-}
-module_init(pasemi_nand_init);
-
-static void __exit pasemi_nand_exit(void)
-{
-	platform_driver_unregister(&pasemi_nand_driver);
-}
-module_exit(pasemi_nand_exit);
+module_platform_driver(pasemi_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Egor Martovetsky <egor@pasemi.com>");
diff --git a/drivers/mtd/nand/plat_nand.c b/drivers/mtd/nand/plat_nand.c
index ea8e1234e0e2..7f2da6953357 100644
--- a/drivers/mtd/nand/plat_nand.c
+++ b/drivers/mtd/nand/plat_nand.c
@@ -148,18 +148,7 @@ static struct platform_driver plat_nand_driver = {
 	},
 };
 
-static int __init plat_nand_init(void)
-{
-	return platform_driver_register(&plat_nand_driver);
-}
-
-static void __exit plat_nand_exit(void)
-{
-	platform_driver_unregister(&plat_nand_driver);
-}
-
-module_init(plat_nand_init);
-module_exit(plat_nand_exit);
+module_platform_driver(plat_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Vitaly Wool");
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index 9eb7f879969e..8544d6bf50a0 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -1258,7 +1258,7 @@ static int pxa3xx_nand_suspend(struct platform_device *pdev, pm_message_t state)
 
 	for (cs = 0; cs < pdata->num_cs; cs++) {
 		mtd = info->host[cs]->mtd;
-		mtd->suspend(mtd);
+		mtd_suspend(mtd);
 	}
 
 	return 0;
@@ -1291,7 +1291,7 @@ static int pxa3xx_nand_resume(struct platform_device *pdev)
 	nand_writel(info, NDSR, NDSR_MASK);
 	for (cs = 0; cs < pdata->num_cs; cs++) {
 		mtd = info->host[cs]->mtd;
-		mtd->resume(mtd);
+		mtd_resume(mtd);
 	}
 
 	return 0;
@@ -1311,17 +1311,7 @@ static struct platform_driver pxa3xx_nand_driver = {
 	.resume		= pxa3xx_nand_resume,
 };
 
-static int __init pxa3xx_nand_init(void)
-{
-	return platform_driver_register(&pxa3xx_nand_driver);
-}
-module_init(pxa3xx_nand_init);
-
-static void __exit pxa3xx_nand_exit(void)
-{
-	platform_driver_unregister(&pxa3xx_nand_driver);
-}
-module_exit(pxa3xx_nand_exit);
+module_platform_driver(pxa3xx_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("PXA3xx NAND controller driver");
diff --git a/drivers/mtd/nand/sharpsl.c b/drivers/mtd/nand/sharpsl.c
index 619d2a504788..b175c0fd8b93 100644
--- a/drivers/mtd/nand/sharpsl.c
+++ b/drivers/mtd/nand/sharpsl.c
@@ -230,17 +230,7 @@ static struct platform_driver sharpsl_nand_driver = {
 	.remove		= __devexit_p(sharpsl_nand_remove),
 };
 
-static int __init sharpsl_nand_init(void)
-{
-	return platform_driver_register(&sharpsl_nand_driver);
-}
-module_init(sharpsl_nand_init);
-
-static void __exit sharpsl_nand_exit(void)
-{
-	platform_driver_unregister(&sharpsl_nand_driver);
-}
-module_exit(sharpsl_nand_exit);
+module_platform_driver(sharpsl_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Richard Purdie <rpurdie@rpsys.net>");
diff --git a/drivers/mtd/nand/sm_common.c b/drivers/mtd/nand/sm_common.c
index 32ae5af7444f..774c3c266713 100644
--- a/drivers/mtd/nand/sm_common.c
+++ b/drivers/mtd/nand/sm_common.c
@@ -55,7 +55,7 @@ static int sm_block_markbad(struct mtd_info *mtd, loff_t ofs)
 	ops.datbuf = NULL;
 
 
-	ret = mtd->write_oob(mtd, ofs, &ops);
+	ret = mtd_write_oob(mtd, ofs, &ops);
 	if (ret < 0 || ops.oobretlen != SM_OOB_SIZE) {
 		printk(KERN_NOTICE
 			"sm_common: can't mark sector at %i as bad\n",
diff --git a/drivers/mtd/nand/socrates_nand.c b/drivers/mtd/nand/socrates_nand.c
index 0fb24f9c2327..e02b08bcf0c0 100644
--- a/drivers/mtd/nand/socrates_nand.c
+++ b/drivers/mtd/nand/socrates_nand.c
@@ -273,18 +273,7 @@ static struct platform_driver socrates_nand_driver = {
 	.remove		= __devexit_p(socrates_nand_remove),
 };
 
-static int __init socrates_nand_init(void)
-{
-	return platform_driver_register(&socrates_nand_driver);
-}
-
-static void __exit socrates_nand_exit(void)
-{
-	platform_driver_unregister(&socrates_nand_driver);
-}
-
-module_init(socrates_nand_init);
-module_exit(socrates_nand_exit);
+module_platform_driver(socrates_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Ilya Yanok");
diff --git a/drivers/mtd/nand/tmio_nand.c b/drivers/mtd/nand/tmio_nand.c
index beebd95f7690..6caa0cd9d6a7 100644
--- a/drivers/mtd/nand/tmio_nand.c
+++ b/drivers/mtd/nand/tmio_nand.c
@@ -533,18 +533,7 @@ static struct platform_driver tmio_driver = {
 	.resume		= tmio_resume,
 };
 
-static int __init tmio_init(void)
-{
-	return platform_driver_register(&tmio_driver);
-}
-
-static void __exit tmio_exit(void)
-{
-	platform_driver_unregister(&tmio_driver);
-}
-
-module_init(tmio_init);
-module_exit(tmio_exit);
+module_platform_driver(tmio_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Ian Molton, Dirk Opfer, Chris Humbert, Dmitry Baryshkov");
diff --git a/drivers/mtd/nand/txx9ndfmc.c b/drivers/mtd/nand/txx9ndfmc.c
index ace46fdaef58..c7c4f1d11c77 100644
--- a/drivers/mtd/nand/txx9ndfmc.c
+++ b/drivers/mtd/nand/txx9ndfmc.c
@@ -298,11 +298,7 @@ static int __init txx9ndfmc_probe(struct platform_device *dev)
 	drvdata = devm_kzalloc(&dev->dev, sizeof(*drvdata), GFP_KERNEL);
 	if (!drvdata)
 		return -ENOMEM;
-	if (!devm_request_mem_region(&dev->dev, res->start,
-				     resource_size(res), dev_name(&dev->dev)))
-		return -EBUSY;
-	drvdata->base = devm_ioremap(&dev->dev, res->start,
-				     resource_size(res));
+	drvdata->base = devm_request_and_ioremap(&dev->dev, res);
 	if (!drvdata->base)
 		return -EBUSY;
 
diff --git a/drivers/mtd/nftlcore.c b/drivers/mtd/nftlcore.c
index cda77b562ad4..a75382aff5f6 100644
--- a/drivers/mtd/nftlcore.c
+++ b/drivers/mtd/nftlcore.c
@@ -56,7 +56,7 @@ static void nftl_add_mtd(struct mtd_blktrans_ops *tr, struct mtd_info *mtd)
 	if (memcmp(mtd->name, "DiskOnChip", 10))
 		return;
 
-	if (!mtd->block_isbad) {
+	if (!mtd_can_have_bb(mtd)) {
 		printk(KERN_ERR
 "NFTL no longer supports the old DiskOnChip drivers loaded via docprobe.\n"
 "Please use the new diskonchip driver under the NAND subsystem.\n");
@@ -153,7 +153,7 @@ int nftl_read_oob(struct mtd_info *mtd, loff_t offs, size_t len,
 	ops.oobbuf = buf;
 	ops.datbuf = NULL;
 
-	res = mtd->read_oob(mtd, offs & ~mask, &ops);
+	res = mtd_read_oob(mtd, offs & ~mask, &ops);
 	*retlen = ops.oobretlen;
 	return res;
 }
@@ -174,7 +174,7 @@ int nftl_write_oob(struct mtd_info *mtd, loff_t offs, size_t len,
 	ops.oobbuf = buf;
 	ops.datbuf = NULL;
 
-	res = mtd->write_oob(mtd, offs & ~mask, &ops);
+	res = mtd_write_oob(mtd, offs & ~mask, &ops);
 	*retlen = ops.oobretlen;
 	return res;
 }
@@ -198,7 +198,7 @@ static int nftl_write(struct mtd_info *mtd, loff_t offs, size_t len,
 	ops.datbuf = buf;
 	ops.len = len;
 
-	res = mtd->write_oob(mtd, offs & ~mask, &ops);
+	res = mtd_write_oob(mtd, offs & ~mask, &ops);
 	*retlen = ops.retlen;
 	return res;
 }
@@ -423,12 +423,17 @@ static u16 NFTL_foldchain (struct NFTLrecord *nftl, unsigned thisVUC, unsigned p
 		if (BlockMap[block] == BLOCK_NIL)
 			continue;
 
-		ret = mtd->read(mtd, (nftl->EraseSize * BlockMap[block]) + (block * 512),
-				512, &retlen, movebuf);
+		ret = mtd_read(mtd,
+			       (nftl->EraseSize * BlockMap[block]) + (block * 512),
+			       512,
+			       &retlen,
+			       movebuf);
 		if (ret < 0 && !mtd_is_bitflip(ret)) {
-			ret = mtd->read(mtd, (nftl->EraseSize * BlockMap[block])
-					+ (block * 512), 512, &retlen,
-					movebuf);
+			ret = mtd_read(mtd,
+				       (nftl->EraseSize * BlockMap[block]) + (block * 512),
+				       512,
+				       &retlen,
+				       movebuf);
 			if (ret != -EIO)
 				printk("Error went away on retry.\n");
 		}
@@ -771,7 +776,7 @@ static int nftl_readblock(struct mtd_blktrans_dev *mbd, unsigned long block,
 	} else {
 		loff_t ptr = (lastgoodEUN * nftl->EraseSize) + blockofs;
 		size_t retlen;
-		int res = mtd->read(mtd, ptr, 512, &retlen, buffer);
+		int res = mtd_read(mtd, ptr, 512, &retlen, buffer);
 
 		if (res < 0 && !mtd_is_bitflip(res))
 			return -EIO;
diff --git a/drivers/mtd/nftlmount.c b/drivers/mtd/nftlmount.c
index ac4092591aea..51b9d6af307f 100644
--- a/drivers/mtd/nftlmount.c
+++ b/drivers/mtd/nftlmount.c
@@ -63,8 +63,8 @@ static int find_boot_record(struct NFTLrecord *nftl)
 
 		/* Check for ANAND header first. Then can whinge if it's found but later
 		   checks fail */
-		ret = mtd->read(mtd, block * nftl->EraseSize, SECTORSIZE,
-				&retlen, buf);
+		ret = mtd_read(mtd, block * nftl->EraseSize, SECTORSIZE,
+			       &retlen, buf);
 		/* We ignore ret in case the ECC of the MediaHeader is invalid
 		   (which is apparently acceptable) */
 		if (retlen != SECTORSIZE) {
@@ -242,7 +242,8 @@ The new DiskOnChip driver already scanned the bad block table.  Just query it.
 			if (buf[i & (SECTORSIZE - 1)] != 0xff)
 				nftl->ReplUnitTable[i] = BLOCK_RESERVED;
 #endif
-			if (nftl->mbd.mtd->block_isbad(nftl->mbd.mtd, i * nftl->EraseSize))
+			if (mtd_block_isbad(nftl->mbd.mtd,
+					    i * nftl->EraseSize))
 				nftl->ReplUnitTable[i] = BLOCK_RESERVED;
 		}
 
@@ -274,7 +275,7 @@ static int check_free_sectors(struct NFTLrecord *nftl, unsigned int address, int
 	int i;
 
 	for (i = 0; i < len; i += SECTORSIZE) {
-		if (mtd->read(mtd, address, SECTORSIZE, &retlen, buf))
+		if (mtd_read(mtd, address, SECTORSIZE, &retlen, buf))
 			return -1;
 		if (memcmpb(buf, 0xff, SECTORSIZE) != 0)
 			return -1;
@@ -326,7 +327,7 @@ int NFTL_formatblock(struct NFTLrecord *nftl, int block)
 	instr->mtd = nftl->mbd.mtd;
 	instr->addr = block * nftl->EraseSize;
 	instr->len = nftl->EraseSize;
-	mtd->erase(mtd, instr);
+	mtd_erase(mtd, instr);
 
 	if (instr->state == MTD_ERASE_FAILED) {
 		printk("Error while formatting block %d\n", block);
@@ -355,7 +356,7 @@ int NFTL_formatblock(struct NFTLrecord *nftl, int block)
 fail:
 	/* could not format, update the bad block table (caller is responsible
 	   for setting the ReplUnitTable to BLOCK_RESERVED on failure) */
-	nftl->mbd.mtd->block_markbad(nftl->mbd.mtd, instr->addr);
+	mtd_block_markbad(nftl->mbd.mtd, instr->addr);
 	return -1;
 }
 
diff --git a/drivers/mtd/onenand/generic.c b/drivers/mtd/onenand/generic.c
index 7813095264a5..0ccd5bff2544 100644
--- a/drivers/mtd/onenand/generic.c
+++ b/drivers/mtd/onenand/generic.c
@@ -115,21 +115,9 @@ static struct platform_driver generic_onenand_driver = {
 	.remove		= __devexit_p(generic_onenand_remove),
 };
 
-MODULE_ALIAS("platform:" DRIVER_NAME);
-
-static int __init generic_onenand_init(void)
-{
-	return platform_driver_register(&generic_onenand_driver);
-}
-
-static void __exit generic_onenand_exit(void)
-{
-	platform_driver_unregister(&generic_onenand_driver);
-}
-
-module_init(generic_onenand_init);
-module_exit(generic_onenand_exit);
+module_platform_driver(generic_onenand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Kyungmin Park <kyungmin.park@samsung.com>");
 MODULE_DESCRIPTION("Glue layer for OneNAND flash on generic boards");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/mtd/onenand/onenand_base.c b/drivers/mtd/onenand/onenand_base.c
index a8394730b4b6..a061bc163da2 100644
--- a/drivers/mtd/onenand/onenand_base.c
+++ b/drivers/mtd/onenand/onenand_base.c
@@ -2633,7 +2633,6 @@ static int onenand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
  */
 static int onenand_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
-	struct onenand_chip *this = mtd->priv;
 	int ret;
 
 	ret = onenand_block_isbad(mtd, ofs);
@@ -2645,7 +2644,7 @@ static int onenand_block_markbad(struct mtd_info *mtd, loff_t ofs)
 	}
 
 	onenand_get_device(mtd, FL_WRITING);
-	ret = this->block_markbad(mtd, ofs);
+	ret = mtd_block_markbad(mtd, ofs);
 	onenand_release_device(mtd);
 	return ret;
 }
diff --git a/drivers/mtd/onenand/samsung.c b/drivers/mtd/onenand/samsung.c
index 5474547eafc2..fa1ee43f735b 100644
--- a/drivers/mtd/onenand/samsung.c
+++ b/drivers/mtd/onenand/samsung.c
@@ -1133,18 +1133,7 @@ static struct platform_driver s3c_onenand_driver = {
 	.remove         = __devexit_p(s3c_onenand_remove),
 };
 
-static int __init s3c_onenand_init(void)
-{
-	return platform_driver_register(&s3c_onenand_driver);
-}
-
-static void __exit s3c_onenand_exit(void)
-{
-	platform_driver_unregister(&s3c_onenand_driver);
-}
-
-module_init(s3c_onenand_init);
-module_exit(s3c_onenand_exit);
+module_platform_driver(s3c_onenand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Kyungmin Park <kyungmin.park@samsung.com>");
diff --git a/drivers/mtd/redboot.c b/drivers/mtd/redboot.c
index e366b1d84ead..48970c14beff 100644
--- a/drivers/mtd/redboot.c
+++ b/drivers/mtd/redboot.c
@@ -78,8 +78,8 @@ static int parse_redboot_partitions(struct mtd_info *master,
 
 	if ( directory < 0 ) {
 		offset = master->size + directory * master->erasesize;
-		while (master->block_isbad && 
-		       master->block_isbad(master, offset)) {
+		while (mtd_can_have_bb(master) &&
+		       mtd_block_isbad(master, offset)) {
 			if (!offset) {
 			nogood:
 				printk(KERN_NOTICE "Failed to find a non-bad block to check for RedBoot partition table\n");
@@ -89,8 +89,8 @@ static int parse_redboot_partitions(struct mtd_info *master,
 		}
 	} else {
 		offset = directory * master->erasesize;
-		while (master->block_isbad && 
-		       master->block_isbad(master, offset)) {
+		while (mtd_can_have_bb(master) &&
+		       mtd_block_isbad(master, offset)) {
 			offset += master->erasesize;
 			if (offset == master->size)
 				goto nogood;
@@ -104,8 +104,8 @@ static int parse_redboot_partitions(struct mtd_info *master,
 	printk(KERN_NOTICE "Searching for RedBoot partition table in %s at offset 0x%lx\n",
 	       master->name, offset);
 
-	ret = master->read(master, offset,
-			   master->erasesize, &retlen, (void *)buf);
+	ret = mtd_read(master, offset, master->erasesize, &retlen,
+		       (void *)buf);
 
 	if (ret)
 		goto out;
diff --git a/drivers/mtd/rfd_ftl.c b/drivers/mtd/rfd_ftl.c
index 73ae217a4252..233b946e5d66 100644
--- a/drivers/mtd/rfd_ftl.c
+++ b/drivers/mtd/rfd_ftl.c
@@ -200,9 +200,9 @@ static int scan_header(struct partition *part)
 		part->sector_map[i] = -1;
 
 	for (i=0, blocks_found=0; i<part->total_blocks; i++) {
-		rc = part->mbd.mtd->read(part->mbd.mtd,
-				i * part->block_size, part->header_size,
-				&retlen, (u_char*)part->header_cache);
+		rc = mtd_read(part->mbd.mtd, i * part->block_size,
+			      part->header_size, &retlen,
+			      (u_char *)part->header_cache);
 
 		if (!rc && retlen != part->header_size)
 			rc = -EIO;
@@ -250,8 +250,8 @@ static int rfd_ftl_readsect(struct mtd_blktrans_dev *dev, u_long sector, char *b
 
 	addr = part->sector_map[sector];
 	if (addr != -1) {
-		rc = part->mbd.mtd->read(part->mbd.mtd, addr, SECTOR_SIZE,
-						&retlen, (u_char*)buf);
+		rc = mtd_read(part->mbd.mtd, addr, SECTOR_SIZE, &retlen,
+			      (u_char *)buf);
 		if (!rc && retlen != SECTOR_SIZE)
 			rc = -EIO;
 
@@ -304,9 +304,8 @@ static void erase_callback(struct erase_info *erase)
 	part->blocks[i].used_sectors = 0;
 	part->blocks[i].erases++;
 
-	rc = part->mbd.mtd->write(part->mbd.mtd,
-		part->blocks[i].offset, sizeof(magic), &retlen,
-		(u_char*)&magic);
+	rc = mtd_write(part->mbd.mtd, part->blocks[i].offset, sizeof(magic),
+		       &retlen, (u_char *)&magic);
 
 	if (!rc && retlen != sizeof(magic))
 		rc = -EIO;
@@ -342,7 +341,7 @@ static int erase_block(struct partition *part, int block)
 	part->blocks[block].state = BLOCK_ERASING;
 	part->blocks[block].free_sectors = 0;
 
-	rc = part->mbd.mtd->erase(part->mbd.mtd, erase);
+	rc = mtd_erase(part->mbd.mtd, erase);
 
 	if (rc) {
 		printk(KERN_ERR PREFIX "erase of region %llx,%llx on '%s' "
@@ -372,9 +371,8 @@ static int move_block_contents(struct partition *part, int block_no, u_long *old
 	if (!map)
 		goto err2;
 
-	rc = part->mbd.mtd->read(part->mbd.mtd,
-		part->blocks[block_no].offset, part->header_size,
-		&retlen, (u_char*)map);
+	rc = mtd_read(part->mbd.mtd, part->blocks[block_no].offset,
+		      part->header_size, &retlen, (u_char *)map);
 
 	if (!rc && retlen != part->header_size)
 		rc = -EIO;
@@ -413,8 +411,8 @@ static int move_block_contents(struct partition *part, int block_no, u_long *old
 			}
 			continue;
 		}
-		rc = part->mbd.mtd->read(part->mbd.mtd, addr,
-			SECTOR_SIZE, &retlen, sector_data);
+		rc = mtd_read(part->mbd.mtd, addr, SECTOR_SIZE, &retlen,
+			      sector_data);
 
 		if (!rc && retlen != SECTOR_SIZE)
 			rc = -EIO;
@@ -450,8 +448,7 @@ static int reclaim_block(struct partition *part, u_long *old_sector)
 	int rc;
 
 	/* we have a race if sync doesn't exist */
-	if (part->mbd.mtd->sync)
-		part->mbd.mtd->sync(part->mbd.mtd);
+	mtd_sync(part->mbd.mtd);
 
 	score = 0x7fffffff; /* MAX_INT */
 	best_block = -1;
@@ -563,8 +560,9 @@ static int find_writable_block(struct partition *part, u_long *old_sector)
 		}
 	}
 
-	rc = part->mbd.mtd->read(part->mbd.mtd, part->blocks[block].offset,
-		part->header_size, &retlen, (u_char*)part->header_cache);
+	rc = mtd_read(part->mbd.mtd, part->blocks[block].offset,
+		      part->header_size, &retlen,
+		      (u_char *)part->header_cache);
 
 	if (!rc && retlen != part->header_size)
 		rc = -EIO;
@@ -595,8 +593,8 @@ static int mark_sector_deleted(struct partition *part, u_long old_addr)
 
 	addr = part->blocks[block].offset +
 			(HEADER_MAP_OFFSET + offset) * sizeof(u16);
-	rc = part->mbd.mtd->write(part->mbd.mtd, addr,
-		sizeof(del), &retlen, (u_char*)&del);
+	rc = mtd_write(part->mbd.mtd, addr, sizeof(del), &retlen,
+		       (u_char *)&del);
 
 	if (!rc && retlen != sizeof(del))
 		rc = -EIO;
@@ -668,8 +666,8 @@ static int do_writesect(struct mtd_blktrans_dev *dev, u_long sector, char *buf,
 
 	addr = (i + part->header_sectors_per_block) * SECTOR_SIZE +
 		block->offset;
-	rc = part->mbd.mtd->write(part->mbd.mtd,
-		addr, SECTOR_SIZE, &retlen, (u_char*)buf);
+	rc = mtd_write(part->mbd.mtd, addr, SECTOR_SIZE, &retlen,
+		       (u_char *)buf);
 
 	if (!rc && retlen != SECTOR_SIZE)
 		rc = -EIO;
@@ -688,8 +686,8 @@ static int do_writesect(struct mtd_blktrans_dev *dev, u_long sector, char *buf,
 	part->header_cache[i + HEADER_MAP_OFFSET] = entry;
 
 	addr = block->offset + (HEADER_MAP_OFFSET + i) * sizeof(u16);
-	rc = part->mbd.mtd->write(part->mbd.mtd, addr,
-			sizeof(entry), &retlen, (u_char*)&entry);
+	rc = mtd_write(part->mbd.mtd, addr, sizeof(entry), &retlen,
+		       (u_char *)&entry);
 
 	if (!rc && retlen != sizeof(entry))
 		rc = -EIO;
diff --git a/drivers/mtd/sm_ftl.c b/drivers/mtd/sm_ftl.c
index fddb714e323c..072ed5970e2f 100644
--- a/drivers/mtd/sm_ftl.c
+++ b/drivers/mtd/sm_ftl.c
@@ -25,7 +25,7 @@
 struct workqueue_struct *cache_flush_workqueue;
 
 static int cache_timeout = 1000;
-module_param(cache_timeout, bool, S_IRUGO);
+module_param(cache_timeout, int, S_IRUGO);
 MODULE_PARM_DESC(cache_timeout,
 	"Timeout (in ms) for cache flush (1000 ms default");
 
@@ -278,7 +278,7 @@ again:
 
 	/* Unfortunately, oob read will _always_ succeed,
 		despite card removal..... */
-	ret = mtd->read_oob(mtd, sm_mkoffset(ftl, zone, block, boffset), &ops);
+	ret = mtd_read_oob(mtd, sm_mkoffset(ftl, zone, block, boffset), &ops);
 
 	/* Test for unknown errors */
 	if (ret != 0 && !mtd_is_bitflip_or_eccerr(ret)) {
@@ -343,7 +343,7 @@ static int sm_write_sector(struct sm_ftl *ftl,
 	ops.ooblen = SM_OOB_SIZE;
 	ops.oobbuf = (void *)oob;
 
-	ret = mtd->write_oob(mtd, sm_mkoffset(ftl, zone, block, boffset), &ops);
+	ret = mtd_write_oob(mtd, sm_mkoffset(ftl, zone, block, boffset), &ops);
 
 	/* Now we assume that hardware will catch write bitflip errors */
 	/* If you are paranoid, use CONFIG_MTD_NAND_VERIFY_WRITE */
@@ -479,7 +479,7 @@ static int sm_erase_block(struct sm_ftl *ftl, int zone_num, uint16_t block,
 		return -EIO;
 	}
 
-	if (mtd->erase(mtd, &erase)) {
+	if (mtd_erase(mtd, &erase)) {
 		sm_printk("erase of block %d in zone %d failed",
 							block, zone_num);
 		goto error;
@@ -645,8 +645,8 @@ int sm_get_media_info(struct sm_ftl *ftl, struct mtd_info *mtd)
 	if (!ftl->smallpagenand && mtd->oobsize < SM_OOB_SIZE)
 		return -ENODEV;
 
-	/* We use these functions for IO */
-	if (!mtd->read_oob || !mtd->write_oob)
+	/* We use OOB */
+	if (!mtd_has_oob(mtd))
 		return -ENODEV;
 
 	/* Find geometry information */
diff --git a/drivers/mtd/ssfdc.c b/drivers/mtd/ssfdc.c
index 976e3d28b962..ab2a52a039c3 100644
--- a/drivers/mtd/ssfdc.c
+++ b/drivers/mtd/ssfdc.c
@@ -122,9 +122,9 @@ static int get_valid_cis_sector(struct mtd_info *mtd)
 	 * is not SSFDC formatted
 	 */
 	for (k = 0, offset = 0; k < 4; k++, offset += mtd->erasesize) {
-		if (!mtd->block_isbad(mtd, offset)) {
-			ret = mtd->read(mtd, offset, SECTOR_SIZE, &retlen,
-				sect_buf);
+		if (mtd_block_isbad(mtd, offset)) {
+			ret = mtd_read(mtd, offset, SECTOR_SIZE, &retlen,
+				       sect_buf);
 
 			/* CIS pattern match on the sector buffer */
 			if (ret < 0 || retlen != SECTOR_SIZE) {
@@ -156,7 +156,7 @@ static int read_physical_sector(struct mtd_info *mtd, uint8_t *sect_buf,
 	size_t retlen;
 	loff_t offset = (loff_t)sect_no << SECTOR_SHIFT;
 
-	ret = mtd->read(mtd, offset, SECTOR_SIZE, &retlen, sect_buf);
+	ret = mtd_read(mtd, offset, SECTOR_SIZE, &retlen, sect_buf);
 	if (ret < 0 || retlen != SECTOR_SIZE)
 		return -1;
 
@@ -175,7 +175,7 @@ static int read_raw_oob(struct mtd_info *mtd, loff_t offs, uint8_t *buf)
 	ops.oobbuf = buf;
 	ops.datbuf = NULL;
 
-	ret = mtd->read_oob(mtd, offs, &ops);
+	ret = mtd_read_oob(mtd, offs, &ops);
 	if (ret < 0 || ops.oobretlen != OOB_SIZE)
 		return -1;
 
@@ -255,7 +255,7 @@ static int build_logical_block_map(struct ssfdcr_record *ssfdc)
 	for (phys_block = ssfdc->cis_block + 1; phys_block < ssfdc->map_len;
 			phys_block++) {
 		offset = (unsigned long)phys_block * ssfdc->erase_size;
-		if (mtd->block_isbad(mtd, offset))
+		if (mtd_block_isbad(mtd, offset))
 			continue;	/* skip bad blocks */
 
 		ret = read_raw_oob(mtd, offset, oob_buf);
diff --git a/drivers/mtd/tests/mtd_oobtest.c b/drivers/mtd/tests/mtd_oobtest.c
index 933f7e5f32d3..ed9b62827f1b 100644
--- a/drivers/mtd/tests/mtd_oobtest.c
+++ b/drivers/mtd/tests/mtd_oobtest.c
@@ -78,7 +78,7 @@ static int erase_eraseblock(int ebnum)
 	ei.addr = addr;
 	ei.len  = mtd->erasesize;
 
-	err = mtd->erase(mtd, &ei);
+	err = mtd_erase(mtd, &ei);
 	if (err) {
 		printk(PRINT_PREF "error %d while erasing EB %d\n", err, ebnum);
 		return err;
@@ -139,7 +139,7 @@ static int write_eraseblock(int ebnum)
 		ops.ooboffs   = use_offset;
 		ops.datbuf    = NULL;
 		ops.oobbuf    = writebuf;
-		err = mtd->write_oob(mtd, addr, &ops);
+		err = mtd_write_oob(mtd, addr, &ops);
 		if (err || ops.oobretlen != use_len) {
 			printk(PRINT_PREF "error: writeoob failed at %#llx\n",
 			       (long long)addr);
@@ -192,7 +192,7 @@ static int verify_eraseblock(int ebnum)
 		ops.ooboffs   = use_offset;
 		ops.datbuf    = NULL;
 		ops.oobbuf    = readbuf;
-		err = mtd->read_oob(mtd, addr, &ops);
+		err = mtd_read_oob(mtd, addr, &ops);
 		if (err || ops.oobretlen != use_len) {
 			printk(PRINT_PREF "error: readoob failed at %#llx\n",
 			       (long long)addr);
@@ -219,7 +219,7 @@ static int verify_eraseblock(int ebnum)
 			ops.ooboffs   = 0;
 			ops.datbuf    = NULL;
 			ops.oobbuf    = readbuf;
-			err = mtd->read_oob(mtd, addr, &ops);
+			err = mtd_read_oob(mtd, addr, &ops);
 			if (err || ops.oobretlen != mtd->ecclayout->oobavail) {
 				printk(PRINT_PREF "error: readoob failed at "
 				       "%#llx\n", (long long)addr);
@@ -284,7 +284,7 @@ static int verify_eraseblock_in_one_go(int ebnum)
 	ops.ooboffs   = 0;
 	ops.datbuf    = NULL;
 	ops.oobbuf    = readbuf;
-	err = mtd->read_oob(mtd, addr, &ops);
+	err = mtd_read_oob(mtd, addr, &ops);
 	if (err || ops.oobretlen != len) {
 		printk(PRINT_PREF "error: readoob failed at %#llx\n",
 		       (long long)addr);
@@ -329,7 +329,7 @@ static int is_block_bad(int ebnum)
 	int ret;
 	loff_t addr = ebnum * mtd->erasesize;
 
-	ret = mtd->block_isbad(mtd, addr);
+	ret = mtd_block_isbad(mtd, addr);
 	if (ret)
 		printk(PRINT_PREF "block %d is bad\n", ebnum);
 	return ret;
@@ -524,7 +524,7 @@ static int __init mtd_oobtest_init(void)
 	ops.oobbuf    = writebuf;
 	printk(PRINT_PREF "attempting to start write past end of OOB\n");
 	printk(PRINT_PREF "an error is expected...\n");
-	err = mtd->write_oob(mtd, addr0, &ops);
+	err = mtd_write_oob(mtd, addr0, &ops);
 	if (err) {
 		printk(PRINT_PREF "error occurred as expected\n");
 		err = 0;
@@ -544,7 +544,7 @@ static int __init mtd_oobtest_init(void)
 	ops.oobbuf    = readbuf;
 	printk(PRINT_PREF "attempting to start read past end of OOB\n");
 	printk(PRINT_PREF "an error is expected...\n");
-	err = mtd->read_oob(mtd, addr0, &ops);
+	err = mtd_read_oob(mtd, addr0, &ops);
 	if (err) {
 		printk(PRINT_PREF "error occurred as expected\n");
 		err = 0;
@@ -568,7 +568,7 @@ static int __init mtd_oobtest_init(void)
 		ops.oobbuf    = writebuf;
 		printk(PRINT_PREF "attempting to write past end of device\n");
 		printk(PRINT_PREF "an error is expected...\n");
-		err = mtd->write_oob(mtd, mtd->size - mtd->writesize, &ops);
+		err = mtd_write_oob(mtd, mtd->size - mtd->writesize, &ops);
 		if (err) {
 			printk(PRINT_PREF "error occurred as expected\n");
 			err = 0;
@@ -588,7 +588,7 @@ static int __init mtd_oobtest_init(void)
 		ops.oobbuf    = readbuf;
 		printk(PRINT_PREF "attempting to read past end of device\n");
 		printk(PRINT_PREF "an error is expected...\n");
-		err = mtd->read_oob(mtd, mtd->size - mtd->writesize, &ops);
+		err = mtd_read_oob(mtd, mtd->size - mtd->writesize, &ops);
 		if (err) {
 			printk(PRINT_PREF "error occurred as expected\n");
 			err = 0;
@@ -612,7 +612,7 @@ static int __init mtd_oobtest_init(void)
 		ops.oobbuf    = writebuf;
 		printk(PRINT_PREF "attempting to write past end of device\n");
 		printk(PRINT_PREF "an error is expected...\n");
-		err = mtd->write_oob(mtd, mtd->size - mtd->writesize, &ops);
+		err = mtd_write_oob(mtd, mtd->size - mtd->writesize, &ops);
 		if (err) {
 			printk(PRINT_PREF "error occurred as expected\n");
 			err = 0;
@@ -632,7 +632,7 @@ static int __init mtd_oobtest_init(void)
 		ops.oobbuf    = readbuf;
 		printk(PRINT_PREF "attempting to read past end of device\n");
 		printk(PRINT_PREF "an error is expected...\n");
-		err = mtd->read_oob(mtd, mtd->size - mtd->writesize, &ops);
+		err = mtd_read_oob(mtd, mtd->size - mtd->writesize, &ops);
 		if (err) {
 			printk(PRINT_PREF "error occurred as expected\n");
 			err = 0;
@@ -670,7 +670,7 @@ static int __init mtd_oobtest_init(void)
 			ops.ooboffs   = 0;
 			ops.datbuf    = NULL;
 			ops.oobbuf    = writebuf;
-			err = mtd->write_oob(mtd, addr, &ops);
+			err = mtd_write_oob(mtd, addr, &ops);
 			if (err)
 				goto out;
 			if (i % 256 == 0)
@@ -698,7 +698,7 @@ static int __init mtd_oobtest_init(void)
 		ops.ooboffs   = 0;
 		ops.datbuf    = NULL;
 		ops.oobbuf    = readbuf;
-		err = mtd->read_oob(mtd, addr, &ops);
+		err = mtd_read_oob(mtd, addr, &ops);
 		if (err)
 			goto out;
 		if (memcmp(readbuf, writebuf, mtd->ecclayout->oobavail * 2)) {
diff --git a/drivers/mtd/tests/mtd_pagetest.c b/drivers/mtd/tests/mtd_pagetest.c
index afafb6935fd0..252ddb092fb2 100644
--- a/drivers/mtd/tests/mtd_pagetest.c
+++ b/drivers/mtd/tests/mtd_pagetest.c
@@ -77,7 +77,7 @@ static int erase_eraseblock(int ebnum)
 	ei.addr = addr;
 	ei.len  = mtd->erasesize;
 
-	err = mtd->erase(mtd, &ei);
+	err = mtd_erase(mtd, &ei);
 	if (err) {
 		printk(PRINT_PREF "error %d while erasing EB %d\n", err, ebnum);
 		return err;
@@ -95,12 +95,12 @@ static int erase_eraseblock(int ebnum)
 static int write_eraseblock(int ebnum)
 {
 	int err = 0;
-	size_t written = 0;
+	size_t written;
 	loff_t addr = ebnum * mtd->erasesize;
 
 	set_random_data(writebuf, mtd->erasesize);
 	cond_resched();
-	err = mtd->write(mtd, addr, mtd->erasesize, &written, writebuf);
+	err = mtd_write(mtd, addr, mtd->erasesize, &written, writebuf);
 	if (err || written != mtd->erasesize)
 		printk(PRINT_PREF "error: write failed at %#llx\n",
 		       (long long)addr);
@@ -111,7 +111,7 @@ static int write_eraseblock(int ebnum)
 static int verify_eraseblock(int ebnum)
 {
 	uint32_t j;
-	size_t read = 0;
+	size_t read;
 	int err = 0, i;
 	loff_t addr0, addrn;
 	loff_t addr = ebnum * mtd->erasesize;
@@ -127,7 +127,7 @@ static int verify_eraseblock(int ebnum)
 	set_random_data(writebuf, mtd->erasesize);
 	for (j = 0; j < pgcnt - 1; ++j, addr += pgsize) {
 		/* Do a read to set the internal dataRAMs to different data */
-		err = mtd->read(mtd, addr0, bufsize, &read, twopages);
+		err = mtd_read(mtd, addr0, bufsize, &read, twopages);
 		if (mtd_is_bitflip(err))
 			err = 0;
 		if (err || read != bufsize) {
@@ -135,7 +135,7 @@ static int verify_eraseblock(int ebnum)
 			       (long long)addr0);
 			return err;
 		}
-		err = mtd->read(mtd, addrn - bufsize, bufsize, &read, twopages);
+		err = mtd_read(mtd, addrn - bufsize, bufsize, &read, twopages);
 		if (mtd_is_bitflip(err))
 			err = 0;
 		if (err || read != bufsize) {
@@ -144,8 +144,7 @@ static int verify_eraseblock(int ebnum)
 			return err;
 		}
 		memset(twopages, 0, bufsize);
-		read = 0;
-		err = mtd->read(mtd, addr, bufsize, &read, twopages);
+		err = mtd_read(mtd, addr, bufsize, &read, twopages);
 		if (mtd_is_bitflip(err))
 			err = 0;
 		if (err || read != bufsize) {
@@ -163,7 +162,7 @@ static int verify_eraseblock(int ebnum)
 	if (addr <= addrn - pgsize - pgsize && !bbt[ebnum + 1]) {
 		unsigned long oldnext = next;
 		/* Do a read to set the internal dataRAMs to different data */
-		err = mtd->read(mtd, addr0, bufsize, &read, twopages);
+		err = mtd_read(mtd, addr0, bufsize, &read, twopages);
 		if (mtd_is_bitflip(err))
 			err = 0;
 		if (err || read != bufsize) {
@@ -171,7 +170,7 @@ static int verify_eraseblock(int ebnum)
 			       (long long)addr0);
 			return err;
 		}
-		err = mtd->read(mtd, addrn - bufsize, bufsize, &read, twopages);
+		err = mtd_read(mtd, addrn - bufsize, bufsize, &read, twopages);
 		if (mtd_is_bitflip(err))
 			err = 0;
 		if (err || read != bufsize) {
@@ -180,8 +179,7 @@ static int verify_eraseblock(int ebnum)
 			return err;
 		}
 		memset(twopages, 0, bufsize);
-		read = 0;
-		err = mtd->read(mtd, addr, bufsize, &read, twopages);
+		err = mtd_read(mtd, addr, bufsize, &read, twopages);
 		if (mtd_is_bitflip(err))
 			err = 0;
 		if (err || read != bufsize) {
@@ -203,7 +201,7 @@ static int verify_eraseblock(int ebnum)
 
 static int crosstest(void)
 {
-	size_t read = 0;
+	size_t read;
 	int err = 0, i;
 	loff_t addr, addr0, addrn;
 	unsigned char *pp1, *pp2, *pp3, *pp4;
@@ -228,9 +226,8 @@ static int crosstest(void)
 		addrn -= mtd->erasesize;
 
 	/* Read 2nd-to-last page to pp1 */
-	read = 0;
 	addr = addrn - pgsize - pgsize;
-	err = mtd->read(mtd, addr, pgsize, &read, pp1);
+	err = mtd_read(mtd, addr, pgsize, &read, pp1);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (err || read != pgsize) {
@@ -241,9 +238,8 @@ static int crosstest(void)
 	}
 
 	/* Read 3rd-to-last page to pp1 */
-	read = 0;
 	addr = addrn - pgsize - pgsize - pgsize;
-	err = mtd->read(mtd, addr, pgsize, &read, pp1);
+	err = mtd_read(mtd, addr, pgsize, &read, pp1);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (err || read != pgsize) {
@@ -254,10 +250,9 @@ static int crosstest(void)
 	}
 
 	/* Read first page to pp2 */
-	read = 0;
 	addr = addr0;
 	printk(PRINT_PREF "reading page at %#llx\n", (long long)addr);
-	err = mtd->read(mtd, addr, pgsize, &read, pp2);
+	err = mtd_read(mtd, addr, pgsize, &read, pp2);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (err || read != pgsize) {
@@ -268,10 +263,9 @@ static int crosstest(void)
 	}
 
 	/* Read last page to pp3 */
-	read = 0;
 	addr = addrn - pgsize;
 	printk(PRINT_PREF "reading page at %#llx\n", (long long)addr);
-	err = mtd->read(mtd, addr, pgsize, &read, pp3);
+	err = mtd_read(mtd, addr, pgsize, &read, pp3);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (err || read != pgsize) {
@@ -282,10 +276,9 @@ static int crosstest(void)
 	}
 
 	/* Read first page again to pp4 */
-	read = 0;
 	addr = addr0;
 	printk(PRINT_PREF "reading page at %#llx\n", (long long)addr);
-	err = mtd->read(mtd, addr, pgsize, &read, pp4);
+	err = mtd_read(mtd, addr, pgsize, &read, pp4);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (err || read != pgsize) {
@@ -309,7 +302,7 @@ static int crosstest(void)
 
 static int erasecrosstest(void)
 {
-	size_t read = 0, written = 0;
+	size_t read, written;
 	int err = 0, i, ebnum, ebnum2;
 	loff_t addr0;
 	char *readbuf = twopages;
@@ -335,7 +328,7 @@ static int erasecrosstest(void)
 	printk(PRINT_PREF "writing 1st page of block %d\n", ebnum);
 	set_random_data(writebuf, pgsize);
 	strcpy(writebuf, "There is no data like this!");
-	err = mtd->write(mtd, addr0, pgsize, &written, writebuf);
+	err = mtd_write(mtd, addr0, pgsize, &written, writebuf);
 	if (err || written != pgsize) {
 		printk(PRINT_PREF "error: write failed at %#llx\n",
 		       (long long)addr0);
@@ -344,7 +337,7 @@ static int erasecrosstest(void)
 
 	printk(PRINT_PREF "reading 1st page of block %d\n", ebnum);
 	memset(readbuf, 0, pgsize);
-	err = mtd->read(mtd, addr0, pgsize, &read, readbuf);
+	err = mtd_read(mtd, addr0, pgsize, &read, readbuf);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (err || read != pgsize) {
@@ -368,7 +361,7 @@ static int erasecrosstest(void)
 	printk(PRINT_PREF "writing 1st page of block %d\n", ebnum);
 	set_random_data(writebuf, pgsize);
 	strcpy(writebuf, "There is no data like this!");
-	err = mtd->write(mtd, addr0, pgsize, &written, writebuf);
+	err = mtd_write(mtd, addr0, pgsize, &written, writebuf);
 	if (err || written != pgsize) {
 		printk(PRINT_PREF "error: write failed at %#llx\n",
 		       (long long)addr0);
@@ -382,7 +375,7 @@ static int erasecrosstest(void)
 
 	printk(PRINT_PREF "reading 1st page of block %d\n", ebnum);
 	memset(readbuf, 0, pgsize);
-	err = mtd->read(mtd, addr0, pgsize, &read, readbuf);
+	err = mtd_read(mtd, addr0, pgsize, &read, readbuf);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (err || read != pgsize) {
@@ -405,7 +398,7 @@ static int erasecrosstest(void)
 
 static int erasetest(void)
 {
-	size_t read = 0, written = 0;
+	size_t read, written;
 	int err = 0, i, ebnum, ok = 1;
 	loff_t addr0;
 
@@ -425,7 +418,7 @@ static int erasetest(void)
 
 	printk(PRINT_PREF "writing 1st page of block %d\n", ebnum);
 	set_random_data(writebuf, pgsize);
-	err = mtd->write(mtd, addr0, pgsize, &written, writebuf);
+	err = mtd_write(mtd, addr0, pgsize, &written, writebuf);
 	if (err || written != pgsize) {
 		printk(PRINT_PREF "error: write failed at %#llx\n",
 		       (long long)addr0);
@@ -438,7 +431,7 @@ static int erasetest(void)
 		return err;
 
 	printk(PRINT_PREF "reading 1st page of block %d\n", ebnum);
-	err = mtd->read(mtd, addr0, pgsize, &read, twopages);
+	err = mtd_read(mtd, addr0, pgsize, &read, twopages);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (err || read != pgsize) {
@@ -469,7 +462,7 @@ static int is_block_bad(int ebnum)
 	loff_t addr = ebnum * mtd->erasesize;
 	int ret;
 
-	ret = mtd->block_isbad(mtd, addr);
+	ret = mtd_block_isbad(mtd, addr);
 	if (ret)
 		printk(PRINT_PREF "block %d is bad\n", ebnum);
 	return ret;
diff --git a/drivers/mtd/tests/mtd_readtest.c b/drivers/mtd/tests/mtd_readtest.c
index 550fe51225a7..121aba189cec 100644
--- a/drivers/mtd/tests/mtd_readtest.c
+++ b/drivers/mtd/tests/mtd_readtest.c
@@ -44,7 +44,7 @@ static int pgcnt;
 
 static int read_eraseblock_by_page(int ebnum)
 {
-	size_t read = 0;
+	size_t read;
 	int i, ret, err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 	void *buf = iobuf;
@@ -52,7 +52,7 @@ static int read_eraseblock_by_page(int ebnum)
 
 	for (i = 0; i < pgcnt; i++) {
 		memset(buf, 0 , pgcnt);
-		ret = mtd->read(mtd, addr, pgsize, &read, buf);
+		ret = mtd_read(mtd, addr, pgsize, &read, buf);
 		if (ret == -EUCLEAN)
 			ret = 0;
 		if (ret || read != pgsize) {
@@ -74,7 +74,7 @@ static int read_eraseblock_by_page(int ebnum)
 			ops.ooboffs   = 0;
 			ops.datbuf    = NULL;
 			ops.oobbuf    = oobbuf;
-			ret = mtd->read_oob(mtd, addr, &ops);
+			ret = mtd_read_oob(mtd, addr, &ops);
 			if ((ret && !mtd_is_bitflip(ret)) ||
 					ops.oobretlen != mtd->oobsize) {
 				printk(PRINT_PREF "error: read oob failed at "
@@ -132,7 +132,7 @@ static int is_block_bad(int ebnum)
 	loff_t addr = ebnum * mtd->erasesize;
 	int ret;
 
-	ret = mtd->block_isbad(mtd, addr);
+	ret = mtd_block_isbad(mtd, addr);
 	if (ret)
 		printk(PRINT_PREF "block %d is bad\n", ebnum);
 	return ret;
@@ -148,8 +148,7 @@ static int scan_for_bad_eraseblocks(void)
 		return -ENOMEM;
 	}
 
-	/* NOR flash does not implement block_isbad */
-	if (mtd->block_isbad == NULL)
+	if (!mtd_can_have_bb(mtd))
 		return 0;
 
 	printk(PRINT_PREF "scanning for bad eraseblocks\n");
diff --git a/drivers/mtd/tests/mtd_speedtest.c b/drivers/mtd/tests/mtd_speedtest.c
index 493b367bdd35..2aec4f3b72be 100644
--- a/drivers/mtd/tests/mtd_speedtest.c
+++ b/drivers/mtd/tests/mtd_speedtest.c
@@ -79,7 +79,7 @@ static int erase_eraseblock(int ebnum)
 	ei.addr = addr;
 	ei.len  = mtd->erasesize;
 
-	err = mtd->erase(mtd, &ei);
+	err = mtd_erase(mtd, &ei);
 	if (err) {
 		printk(PRINT_PREF "error %d while erasing EB %d\n", err, ebnum);
 		return err;
@@ -105,7 +105,7 @@ static int multiblock_erase(int ebnum, int blocks)
 	ei.addr = addr;
 	ei.len  = mtd->erasesize * blocks;
 
-	err = mtd->erase(mtd, &ei);
+	err = mtd_erase(mtd, &ei);
 	if (err) {
 		printk(PRINT_PREF "error %d while erasing EB %d, blocks %d\n",
 		       err, ebnum, blocks);
@@ -139,11 +139,11 @@ static int erase_whole_device(void)
 
 static int write_eraseblock(int ebnum)
 {
-	size_t written = 0;
+	size_t written;
 	int err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 
-	err = mtd->write(mtd, addr, mtd->erasesize, &written, iobuf);
+	err = mtd_write(mtd, addr, mtd->erasesize, &written, iobuf);
 	if (err || written != mtd->erasesize) {
 		printk(PRINT_PREF "error: write failed at %#llx\n", addr);
 		if (!err)
@@ -155,13 +155,13 @@ static int write_eraseblock(int ebnum)
 
 static int write_eraseblock_by_page(int ebnum)
 {
-	size_t written = 0;
+	size_t written;
 	int i, err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 	void *buf = iobuf;
 
 	for (i = 0; i < pgcnt; i++) {
-		err = mtd->write(mtd, addr, pgsize, &written, buf);
+		err = mtd_write(mtd, addr, pgsize, &written, buf);
 		if (err || written != pgsize) {
 			printk(PRINT_PREF "error: write failed at %#llx\n",
 			       addr);
@@ -178,13 +178,13 @@ static int write_eraseblock_by_page(int ebnum)
 
 static int write_eraseblock_by_2pages(int ebnum)
 {
-	size_t written = 0, sz = pgsize * 2;
+	size_t written, sz = pgsize * 2;
 	int i, n = pgcnt / 2, err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 	void *buf = iobuf;
 
 	for (i = 0; i < n; i++) {
-		err = mtd->write(mtd, addr, sz, &written, buf);
+		err = mtd_write(mtd, addr, sz, &written, buf);
 		if (err || written != sz) {
 			printk(PRINT_PREF "error: write failed at %#llx\n",
 			       addr);
@@ -196,7 +196,7 @@ static int write_eraseblock_by_2pages(int ebnum)
 		buf += sz;
 	}
 	if (pgcnt % 2) {
-		err = mtd->write(mtd, addr, pgsize, &written, buf);
+		err = mtd_write(mtd, addr, pgsize, &written, buf);
 		if (err || written != pgsize) {
 			printk(PRINT_PREF "error: write failed at %#llx\n",
 			       addr);
@@ -210,11 +210,11 @@ static int write_eraseblock_by_2pages(int ebnum)
 
 static int read_eraseblock(int ebnum)
 {
-	size_t read = 0;
+	size_t read;
 	int err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 
-	err = mtd->read(mtd, addr, mtd->erasesize, &read, iobuf);
+	err = mtd_read(mtd, addr, mtd->erasesize, &read, iobuf);
 	/* Ignore corrected ECC errors */
 	if (mtd_is_bitflip(err))
 		err = 0;
@@ -229,13 +229,13 @@ static int read_eraseblock(int ebnum)
 
 static int read_eraseblock_by_page(int ebnum)
 {
-	size_t read = 0;
+	size_t read;
 	int i, err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 	void *buf = iobuf;
 
 	for (i = 0; i < pgcnt; i++) {
-		err = mtd->read(mtd, addr, pgsize, &read, buf);
+		err = mtd_read(mtd, addr, pgsize, &read, buf);
 		/* Ignore corrected ECC errors */
 		if (mtd_is_bitflip(err))
 			err = 0;
@@ -255,13 +255,13 @@ static int read_eraseblock_by_page(int ebnum)
 
 static int read_eraseblock_by_2pages(int ebnum)
 {
-	size_t read = 0, sz = pgsize * 2;
+	size_t read, sz = pgsize * 2;
 	int i, n = pgcnt / 2, err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 	void *buf = iobuf;
 
 	for (i = 0; i < n; i++) {
-		err = mtd->read(mtd, addr, sz, &read, buf);
+		err = mtd_read(mtd, addr, sz, &read, buf);
 		/* Ignore corrected ECC errors */
 		if (mtd_is_bitflip(err))
 			err = 0;
@@ -276,7 +276,7 @@ static int read_eraseblock_by_2pages(int ebnum)
 		buf += sz;
 	}
 	if (pgcnt % 2) {
-		err = mtd->read(mtd, addr, pgsize, &read, buf);
+		err = mtd_read(mtd, addr, pgsize, &read, buf);
 		/* Ignore corrected ECC errors */
 		if (mtd_is_bitflip(err))
 			err = 0;
@@ -296,7 +296,7 @@ static int is_block_bad(int ebnum)
 	loff_t addr = ebnum * mtd->erasesize;
 	int ret;
 
-	ret = mtd->block_isbad(mtd, addr);
+	ret = mtd_block_isbad(mtd, addr);
 	if (ret)
 		printk(PRINT_PREF "block %d is bad\n", ebnum);
 	return ret;
@@ -336,8 +336,7 @@ static int scan_for_bad_eraseblocks(void)
 		return -ENOMEM;
 	}
 
-	/* NOR flash does not implement block_isbad */
-	if (mtd->block_isbad == NULL)
+	if (!mtd_can_have_bb(mtd))
 		goto out;
 
 	printk(PRINT_PREF "scanning for bad eraseblocks\n");
diff --git a/drivers/mtd/tests/mtd_stresstest.c b/drivers/mtd/tests/mtd_stresstest.c
index 52ffd9120e0d..7b33f22d0b58 100644
--- a/drivers/mtd/tests/mtd_stresstest.c
+++ b/drivers/mtd/tests/mtd_stresstest.c
@@ -112,7 +112,7 @@ static int erase_eraseblock(int ebnum)
 	ei.addr = addr;
 	ei.len  = mtd->erasesize;
 
-	err = mtd->erase(mtd, &ei);
+	err = mtd_erase(mtd, &ei);
 	if (unlikely(err)) {
 		printk(PRINT_PREF "error %d while erasing EB %d\n", err, ebnum);
 		return err;
@@ -132,7 +132,7 @@ static int is_block_bad(int ebnum)
 	loff_t addr = ebnum * mtd->erasesize;
 	int ret;
 
-	ret = mtd->block_isbad(mtd, addr);
+	ret = mtd_block_isbad(mtd, addr);
 	if (ret)
 		printk(PRINT_PREF "block %d is bad\n", ebnum);
 	return ret;
@@ -140,7 +140,7 @@ static int is_block_bad(int ebnum)
 
 static int do_read(void)
 {
-	size_t read = 0;
+	size_t read;
 	int eb = rand_eb();
 	int offs = rand_offs();
 	int len = rand_len(offs), err;
@@ -153,7 +153,7 @@ static int do_read(void)
 			len = mtd->erasesize - offs;
 	}
 	addr = eb * mtd->erasesize + offs;
-	err = mtd->read(mtd, addr, len, &read, readbuf);
+	err = mtd_read(mtd, addr, len, &read, readbuf);
 	if (mtd_is_bitflip(err))
 		err = 0;
 	if (unlikely(err || read != len)) {
@@ -169,7 +169,7 @@ static int do_read(void)
 static int do_write(void)
 {
 	int eb = rand_eb(), offs, err, len;
-	size_t written = 0;
+	size_t written;
 	loff_t addr;
 
 	offs = offsets[eb];
@@ -192,7 +192,7 @@ static int do_write(void)
 		}
 	}
 	addr = eb * mtd->erasesize + offs;
-	err = mtd->write(mtd, addr, len, &written, writebuf);
+	err = mtd_write(mtd, addr, len, &written, writebuf);
 	if (unlikely(err || written != len)) {
 		printk(PRINT_PREF "error: write failed at 0x%llx\n",
 		       (long long)addr);
@@ -227,8 +227,7 @@ static int scan_for_bad_eraseblocks(void)
 		return -ENOMEM;
 	}
 
-	/* NOR flash does not implement block_isbad */
-	if (mtd->block_isbad == NULL)
+	if (!mtd_can_have_bb(mtd))
 		return 0;
 
 	printk(PRINT_PREF "scanning for bad eraseblocks\n");
@@ -284,6 +283,12 @@ static int __init mtd_stresstest_init(void)
 	       (unsigned long long)mtd->size, mtd->erasesize,
 	       pgsize, ebcnt, pgcnt, mtd->oobsize);
 
+	if (ebcnt < 2) {
+		printk(PRINT_PREF "error: need at least 2 eraseblocks\n");
+		err = -ENOSPC;
+		goto out_put_mtd;
+	}
+
 	/* Read or write up 2 eraseblocks at a time */
 	bufsize = mtd->erasesize * 2;
 
@@ -322,6 +327,7 @@ out:
 	kfree(bbt);
 	vfree(writebuf);
 	vfree(readbuf);
+out_put_mtd:
 	put_mtd_device(mtd);
 	if (err)
 		printk(PRINT_PREF "error %d occurred\n", err);
diff --git a/drivers/mtd/tests/mtd_subpagetest.c b/drivers/mtd/tests/mtd_subpagetest.c
index 1a05bfac4eee..9667bf535282 100644
--- a/drivers/mtd/tests/mtd_subpagetest.c
+++ b/drivers/mtd/tests/mtd_subpagetest.c
@@ -80,7 +80,7 @@ static int erase_eraseblock(int ebnum)
 	ei.addr = addr;
 	ei.len  = mtd->erasesize;
 
-	err = mtd->erase(mtd, &ei);
+	err = mtd_erase(mtd, &ei);
 	if (err) {
 		printk(PRINT_PREF "error %d while erasing EB %d\n", err, ebnum);
 		return err;
@@ -115,12 +115,12 @@ static int erase_whole_device(void)
 
 static int write_eraseblock(int ebnum)
 {
-	size_t written = 0;
+	size_t written;
 	int err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 
 	set_random_data(writebuf, subpgsize);
-	err = mtd->write(mtd, addr, subpgsize, &written, writebuf);
+	err = mtd_write(mtd, addr, subpgsize, &written, writebuf);
 	if (unlikely(err || written != subpgsize)) {
 		printk(PRINT_PREF "error: write failed at %#llx\n",
 		       (long long)addr);
@@ -134,7 +134,7 @@ static int write_eraseblock(int ebnum)
 	addr += subpgsize;
 
 	set_random_data(writebuf, subpgsize);
-	err = mtd->write(mtd, addr, subpgsize, &written, writebuf);
+	err = mtd_write(mtd, addr, subpgsize, &written, writebuf);
 	if (unlikely(err || written != subpgsize)) {
 		printk(PRINT_PREF "error: write failed at %#llx\n",
 		       (long long)addr);
@@ -150,7 +150,7 @@ static int write_eraseblock(int ebnum)
 
 static int write_eraseblock2(int ebnum)
 {
-	size_t written = 0;
+	size_t written;
 	int err = 0, k;
 	loff_t addr = ebnum * mtd->erasesize;
 
@@ -158,7 +158,7 @@ static int write_eraseblock2(int ebnum)
 		if (addr + (subpgsize * k) > (ebnum + 1) * mtd->erasesize)
 			break;
 		set_random_data(writebuf, subpgsize * k);
-		err = mtd->write(mtd, addr, subpgsize * k, &written, writebuf);
+		err = mtd_write(mtd, addr, subpgsize * k, &written, writebuf);
 		if (unlikely(err || written != subpgsize * k)) {
 			printk(PRINT_PREF "error: write failed at %#llx\n",
 			       (long long)addr);
@@ -189,14 +189,13 @@ static void print_subpage(unsigned char *p)
 
 static int verify_eraseblock(int ebnum)
 {
-	size_t read = 0;
+	size_t read;
 	int err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 
 	set_random_data(writebuf, subpgsize);
 	clear_data(readbuf, subpgsize);
-	read = 0;
-	err = mtd->read(mtd, addr, subpgsize, &read, readbuf);
+	err = mtd_read(mtd, addr, subpgsize, &read, readbuf);
 	if (unlikely(err || read != subpgsize)) {
 		if (mtd_is_bitflip(err) && read == subpgsize) {
 			printk(PRINT_PREF "ECC correction at %#llx\n",
@@ -223,8 +222,7 @@ static int verify_eraseblock(int ebnum)
 
 	set_random_data(writebuf, subpgsize);
 	clear_data(readbuf, subpgsize);
-	read = 0;
-	err = mtd->read(mtd, addr, subpgsize, &read, readbuf);
+	err = mtd_read(mtd, addr, subpgsize, &read, readbuf);
 	if (unlikely(err || read != subpgsize)) {
 		if (mtd_is_bitflip(err) && read == subpgsize) {
 			printk(PRINT_PREF "ECC correction at %#llx\n",
@@ -252,7 +250,7 @@ static int verify_eraseblock(int ebnum)
 
 static int verify_eraseblock2(int ebnum)
 {
-	size_t read = 0;
+	size_t read;
 	int err = 0, k;
 	loff_t addr = ebnum * mtd->erasesize;
 
@@ -261,8 +259,7 @@ static int verify_eraseblock2(int ebnum)
 			break;
 		set_random_data(writebuf, subpgsize * k);
 		clear_data(readbuf, subpgsize * k);
-		read = 0;
-		err = mtd->read(mtd, addr, subpgsize * k, &read, readbuf);
+		err = mtd_read(mtd, addr, subpgsize * k, &read, readbuf);
 		if (unlikely(err || read != subpgsize * k)) {
 			if (mtd_is_bitflip(err) && read == subpgsize * k) {
 				printk(PRINT_PREF "ECC correction at %#llx\n",
@@ -288,15 +285,14 @@ static int verify_eraseblock2(int ebnum)
 static int verify_eraseblock_ff(int ebnum)
 {
 	uint32_t j;
-	size_t read = 0;
+	size_t read;
 	int err = 0;
 	loff_t addr = ebnum * mtd->erasesize;
 
 	memset(writebuf, 0xff, subpgsize);
 	for (j = 0; j < mtd->erasesize / subpgsize; ++j) {
 		clear_data(readbuf, subpgsize);
-		read = 0;
-		err = mtd->read(mtd, addr, subpgsize, &read, readbuf);
+		err = mtd_read(mtd, addr, subpgsize, &read, readbuf);
 		if (unlikely(err || read != subpgsize)) {
 			if (mtd_is_bitflip(err) && read == subpgsize) {
 				printk(PRINT_PREF "ECC correction at %#llx\n",
@@ -344,7 +340,7 @@ static int is_block_bad(int ebnum)
 	loff_t addr = ebnum * mtd->erasesize;
 	int ret;
 
-	ret = mtd->block_isbad(mtd, addr);
+	ret = mtd_block_isbad(mtd, addr);
 	if (ret)
 		printk(PRINT_PREF "block %d is bad\n", ebnum);
 	return ret;
diff --git a/drivers/mtd/tests/mtd_torturetest.c b/drivers/mtd/tests/mtd_torturetest.c
index 03ab649a6964..b65861bc7b8e 100644
--- a/drivers/mtd/tests/mtd_torturetest.c
+++ b/drivers/mtd/tests/mtd_torturetest.c
@@ -105,7 +105,7 @@ static inline int erase_eraseblock(int ebnum)
 	ei.addr = addr;
 	ei.len  = mtd->erasesize;
 
-	err = mtd->erase(mtd, &ei);
+	err = mtd_erase(mtd, &ei);
 	if (err) {
 		printk(PRINT_PREF "error %d while erasing EB %d\n", err, ebnum);
 		return err;
@@ -127,7 +127,7 @@ static inline int erase_eraseblock(int ebnum)
 static inline int check_eraseblock(int ebnum, unsigned char *buf)
 {
 	int err, retries = 0;
-	size_t read = 0;
+	size_t read;
 	loff_t addr = ebnum * mtd->erasesize;
 	size_t len = mtd->erasesize;
 
@@ -137,7 +137,7 @@ static inline int check_eraseblock(int ebnum, unsigned char *buf)
 	}
 
 retry:
-	err = mtd->read(mtd, addr, len, &read, check_buf);
+	err = mtd_read(mtd, addr, len, &read, check_buf);
 	if (mtd_is_bitflip(err))
 		printk(PRINT_PREF "single bit flip occurred at EB %d "
 		       "MTD reported that it was fixed.\n", ebnum);
@@ -181,7 +181,7 @@ retry:
 static inline int write_pattern(int ebnum, void *buf)
 {
 	int err;
-	size_t written = 0;
+	size_t written;
 	loff_t addr = ebnum * mtd->erasesize;
 	size_t len = mtd->erasesize;
 
@@ -189,7 +189,7 @@ static inline int write_pattern(int ebnum, void *buf)
 		addr = (ebnum + 1) * mtd->erasesize - pgcnt * pgsize;
 		len = pgcnt * pgsize;
 	}
-	err = mtd->write(mtd, addr, len, &written, buf);
+	err = mtd_write(mtd, addr, len, &written, buf);
 	if (err) {
 		printk(PRINT_PREF "error %d while writing EB %d, written %zd"
 		      " bytes\n", err, ebnum, written);
@@ -290,10 +290,9 @@ static int __init tort_init(void)
 	 * Check if there is a bad eraseblock among those we are going to test.
 	 */
 	memset(&bad_ebs[0], 0, sizeof(int) * ebcnt);
-	if (mtd->block_isbad) {
+	if (mtd_can_have_bb(mtd)) {
 		for (i = eb; i < eb + ebcnt; i++) {
-			err = mtd->block_isbad(mtd,
-					       (loff_t)i * mtd->erasesize);
+			err = mtd_block_isbad(mtd, (loff_t)i * mtd->erasesize);
 
 			if (err < 0) {
 				printk(PRINT_PREF "block_isbad() returned %d "
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index 6c3fb5ab20f5..115749f20f9e 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -664,7 +664,7 @@ static int io_init(struct ubi_device *ubi)
 	ubi->peb_count  = mtd_div_by_eb(ubi->mtd->size, ubi->mtd);
 	ubi->flash_size = ubi->mtd->size;
 
-	if (ubi->mtd->block_isbad && ubi->mtd->block_markbad)
+	if (mtd_can_have_bb(ubi->mtd))
 		ubi->bad_allowed = 1;
 
 	if (ubi->mtd->type == MTD_NORFLASH) {
diff --git a/drivers/mtd/ubi/debug.c b/drivers/mtd/ubi/debug.c
index ab80c0debac8..e2cdebf40840 100644
--- a/drivers/mtd/ubi/debug.c
+++ b/drivers/mtd/ubi/debug.c
@@ -216,7 +216,7 @@ void ubi_dbg_dump_flash(struct ubi_device *ubi, int pnum, int offset, int len)
 	buf = vmalloc(len);
 	if (!buf)
 		return;
-	err = ubi->mtd->read(ubi->mtd, addr, len, &read, buf);
+	err = mtd_read(ubi->mtd, addr, len, &read, buf);
 	if (err && err != -EUCLEAN) {
 		ubi_err("error %d while reading %d bytes from PEB %d:%d, "
 			"read %zd bytes", err, len, pnum, offset, read);
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index fb7f19b62d91..cd26da8ad225 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -1028,12 +1028,14 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 	 * 'ubi_wl_put_peb()' function on the @ubi->move_mutex. In turn, we are
 	 * holding @ubi->move_mutex and go sleep on the LEB lock. So, if the
 	 * LEB is already locked, we just do not move it and return
-	 * %MOVE_CANCEL_RACE, which means that UBI will re-try, but later.
+	 * %MOVE_RETRY. Note, we do not return %MOVE_CANCEL_RACE here because
+	 * we do not know the reasons of the contention - it may be just a
+	 * normal I/O on this LEB, so we want to re-try.
 	 */
 	err = leb_write_trylock(ubi, vol_id, lnum);
 	if (err) {
 		dbg_wl("contention on LEB %d:%d, cancel", vol_id, lnum);
-		return MOVE_CANCEL_RACE;
+		return MOVE_RETRY;
 	}
 
 	/*
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index f20b6f22f240..5cde4e5ca3e5 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -170,7 +170,7 @@ int ubi_io_read(const struct ubi_device *ubi, void *buf, int pnum, int offset,
 
 	addr = (loff_t)pnum * ubi->peb_size + offset;
 retry:
-	err = ubi->mtd->read(ubi->mtd, addr, len, &read, buf);
+	err = mtd_read(ubi->mtd, addr, len, &read, buf);
 	if (err) {
 		const char *errstr = mtd_is_eccerr(err) ? " (ECC error)" : "";
 
@@ -289,7 +289,7 @@ int ubi_io_write(struct ubi_device *ubi, const void *buf, int pnum, int offset,
 	}
 
 	addr = (loff_t)pnum * ubi->peb_size + offset;
-	err = ubi->mtd->write(ubi->mtd, addr, len, &written, buf);
+	err = mtd_write(ubi->mtd, addr, len, &written, buf);
 	if (err) {
 		ubi_err("error %d while writing %d bytes to PEB %d:%d, written "
 			"%zd bytes", err, len, pnum, offset, written);
@@ -361,7 +361,7 @@ retry:
 	ei.callback = erase_callback;
 	ei.priv     = (unsigned long)&wq;
 
-	err = ubi->mtd->erase(ubi->mtd, &ei);
+	err = mtd_erase(ubi->mtd, &ei);
 	if (err) {
 		if (retries++ < UBI_IO_RETRIES) {
 			dbg_io("error %d while erasing PEB %d, retry",
@@ -525,11 +525,10 @@ static int nor_erase_prepare(struct ubi_device *ubi, int pnum)
 	 * the header comment in scan.c for more information).
 	 */
 	addr = (loff_t)pnum * ubi->peb_size;
-	err = ubi->mtd->write(ubi->mtd, addr, 4, &written, (void *)&data);
+	err = mtd_write(ubi->mtd, addr, 4, &written, (void *)&data);
 	if (!err) {
 		addr += ubi->vid_hdr_aloffset;
-		err = ubi->mtd->write(ubi->mtd, addr, 4, &written,
-				      (void *)&data);
+		err = mtd_write(ubi->mtd, addr, 4, &written, (void *)&data);
 		if (!err)
 			return 0;
 	}
@@ -635,7 +634,7 @@ int ubi_io_is_bad(const struct ubi_device *ubi, int pnum)
 	if (ubi->bad_allowed) {
 		int ret;
 
-		ret = mtd->block_isbad(mtd, (loff_t)pnum * ubi->peb_size);
+		ret = mtd_block_isbad(mtd, (loff_t)pnum * ubi->peb_size);
 		if (ret < 0)
 			ubi_err("error %d while checking if PEB %d is bad",
 				ret, pnum);
@@ -670,7 +669,7 @@ int ubi_io_mark_bad(const struct ubi_device *ubi, int pnum)
 	if (!ubi->bad_allowed)
 		return 0;
 
-	err = mtd->block_markbad(mtd, (loff_t)pnum * ubi->peb_size);
+	err = mtd_block_markbad(mtd, (loff_t)pnum * ubi->peb_size);
 	if (err)
 		ubi_err("cannot mark PEB %d bad, error %d", pnum, err);
 	return err;
@@ -1357,7 +1356,7 @@ int ubi_dbg_check_write(struct ubi_device *ubi, const void *buf, int pnum,
 		return 0;
 	}
 
-	err = ubi->mtd->read(ubi->mtd, addr, len, &read, buf1);
+	err = mtd_read(ubi->mtd, addr, len, &read, buf1);
 	if (err && !mtd_is_bitflip(err))
 		goto out_free;
 
@@ -1421,7 +1420,7 @@ int ubi_dbg_check_all_ff(struct ubi_device *ubi, int pnum, int offset, int len)
 		return 0;
 	}
 
-	err = ubi->mtd->read(ubi->mtd, addr, len, &read, buf);
+	err = mtd_read(ubi->mtd, addr, len, &read, buf);
 	if (err && !mtd_is_bitflip(err)) {
 		ubi_err("error %d while reading %d bytes from PEB %d:%d, "
 			"read %zd bytes", err, len, pnum, offset, read);
diff --git a/drivers/mtd/ubi/kapi.c b/drivers/mtd/ubi/kapi.c
index 1a35fc5e3b40..9fdb35367fe0 100644
--- a/drivers/mtd/ubi/kapi.c
+++ b/drivers/mtd/ubi/kapi.c
@@ -714,9 +714,7 @@ int ubi_sync(int ubi_num)
 	if (!ubi)
 		return -ENODEV;
 
-	if (ubi->mtd->sync)
-		ubi->mtd->sync(ubi->mtd);
-
+	mtd_sync(ubi->mtd);
 	ubi_put_device(ubi);
 	return 0;
 }
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index dc64c767fd21..d51d75d34446 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -120,6 +120,7 @@ enum {
  *                     PEB
  * MOVE_CANCEL_BITFLIPS: canceled because a bit-flip was detected in the
  *                       target PEB
+ * MOVE_RETRY: retry scrubbing the PEB
  */
 enum {
 	MOVE_CANCEL_RACE = 1,
@@ -127,6 +128,7 @@ enum {
 	MOVE_TARGET_RD_ERR,
 	MOVE_TARGET_WR_ERR,
 	MOVE_CANCEL_BITFLIPS,
+	MOVE_RETRY,
 };
 
 /**
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 42c684cf3688..0696e36b0539 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -795,7 +795,10 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 			protect = 1;
 			goto out_not_moved;
 		}
-
+		if (err == MOVE_RETRY) {
+			scrubbing = 1;
+			goto out_not_moved;
+		}
 		if (err == MOVE_CANCEL_BITFLIPS || err == MOVE_TARGET_WR_ERR ||
 		    err == MOVE_TARGET_RD_ERR) {
 			/*
@@ -1049,7 +1052,6 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 
 	ubi_err("failed to erase PEB %d, error %d", pnum, err);
 	kfree(wl_wrk);
-	kmem_cache_free(ubi_wl_entry_slab, e);
 
 	if (err == -EINTR || err == -ENOMEM || err == -EAGAIN ||
 	    err == -EBUSY) {
@@ -1062,14 +1064,16 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 			goto out_ro;
 		}
 		return err;
-	} else if (err != -EIO) {
+	}
+
+	kmem_cache_free(ubi_wl_entry_slab, e);
+	if (err != -EIO)
 		/*
 		 * If this is not %-EIO, we have no idea what to do. Scheduling
 		 * this physical eraseblock for erasure again would cause
 		 * errors again and again. Well, lets switch to R/O mode.
 		 */
 		goto out_ro;
-	}
 
 	/* It is %-EIO, the PEB went bad */
 
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 9845afb37cc8..b98285446a5a 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -342,4 +342,6 @@ config VMXNET3
 	  To compile this driver as a module, choose M here: the
 	  module will be called vmxnet3.
 
+source "drivers/net/hyperv/Kconfig"
+
 endif # NETDEVICES
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index 1988881853ab..a6b8ce11a22f 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -68,3 +68,5 @@ obj-$(CONFIG_USB_USBNET)        += usb/
 obj-$(CONFIG_USB_ZD1201)        += usb/
 obj-$(CONFIG_USB_IPHETH)        += usb/
 obj-$(CONFIG_USB_CDC_PHONET)   += usb/
+
+obj-$(CONFIG_HYPERV_NET) += hyperv/
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index 165a4c798025..7fd8089946fb 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -802,7 +802,7 @@ static int flexcan_open(struct net_device *dev)
 	struct flexcan_priv *priv = netdev_priv(dev);
 	int err;
 
-	clk_enable(priv->clk);
+	clk_prepare_enable(priv->clk);
 
 	err = open_candev(dev);
 	if (err)
@@ -824,7 +824,7 @@ static int flexcan_open(struct net_device *dev)
  out_close:
 	close_candev(dev);
  out:
-	clk_disable(priv->clk);
+	clk_disable_unprepare(priv->clk);
 
 	return err;
 }
@@ -838,7 +838,7 @@ static int flexcan_close(struct net_device *dev)
 	flexcan_chip_stop(dev);
 
 	free_irq(dev->irq, dev);
-	clk_disable(priv->clk);
+	clk_disable_unprepare(priv->clk);
 
 	close_candev(dev);
 
@@ -877,7 +877,7 @@ static int __devinit register_flexcandev(struct net_device *dev)
 	struct flexcan_regs __iomem *regs = priv->base;
 	u32 reg, err;
 
-	clk_enable(priv->clk);
+	clk_prepare_enable(priv->clk);
 
 	/* select "bus clock", chip must be disabled */
 	flexcan_chip_disable(priv);
@@ -911,7 +911,7 @@ static int __devinit register_flexcandev(struct net_device *dev)
  out:
 	/* disable core and turn off clocks */
 	flexcan_chip_disable(priv);
-	clk_disable(priv->clk);
+	clk_disable_unprepare(priv->clk);
 
 	return err;
 }
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index cd6d69a6a7d2..08d5f0388877 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -10,7 +10,7 @@ obj-$(CONFIG_NET_VENDOR_ALTEON) += alteon/
 obj-$(CONFIG_NET_VENDOR_AMD) += amd/
 obj-$(CONFIG_NET_VENDOR_APPLE) += apple/
 obj-$(CONFIG_NET_VENDOR_ATHEROS) += atheros/
-obj-$(CONFIG_NET_ATMEL) += cadence/
+obj-$(CONFIG_NET_CADENCE) += cadence/
 obj-$(CONFIG_NET_BFIN) += adi/
 obj-$(CONFIG_NET_VENDOR_BROADCOM) += broadcom/
 obj-$(CONFIG_NET_VENDOR_BROCADE) += brocade/
diff --git a/drivers/net/ethernet/broadcom/bcm63xx_enet.c b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
index a11a8ad94226..d44331eb07fe 100644
--- a/drivers/net/ethernet/broadcom/bcm63xx_enet.c
+++ b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
@@ -1469,7 +1469,7 @@ static int bcm_enet_set_pauseparam(struct net_device *dev,
 	return 0;
 }
 
-static struct ethtool_ops bcm_enet_ethtool_ops = {
+static const struct ethtool_ops bcm_enet_ethtool_ops = {
 	.get_strings		= bcm_enet_get_strings,
 	.get_sset_count		= bcm_enet_get_sset_count,
 	.get_ethtool_stats      = bcm_enet_get_ethtool_stats,
diff --git a/drivers/net/ethernet/cadence/Kconfig b/drivers/net/ethernet/cadence/Kconfig
index b48378a41e49..db931916da08 100644
--- a/drivers/net/ethernet/cadence/Kconfig
+++ b/drivers/net/ethernet/cadence/Kconfig
@@ -5,8 +5,8 @@
 config HAVE_NET_MACB
 	bool
 
-config NET_ATMEL
-	bool "Atmel devices"
+config NET_CADENCE
+	bool "Cadence devices"
 	default y
 	depends on HAVE_NET_MACB || (ARM && ARCH_AT91RM9200)
 	---help---
@@ -21,7 +21,7 @@ config NET_ATMEL
 	  the remaining Atmel network card questions. If you say Y, you will be
 	  asked for your specific card in the following questions.
 
-if NET_ATMEL
+if NET_CADENCE
 
 config ARM_AT91_ETHER
 	tristate "AT91RM9200 Ethernet support"
@@ -33,14 +33,16 @@ config ARM_AT91_ETHER
 	  ethernet support, then you should always answer Y to this.
 
 config MACB
-	tristate "Atmel MACB support"
+	tristate "Cadence MACB/GEM support"
 	depends on HAVE_NET_MACB
 	select PHYLIB
 	---help---
-	  The Atmel MACB ethernet interface is found on many AT32 and AT91
-	  parts. Say Y to include support for the MACB chip.
+	  The Cadence MACB ethernet interface is found on many Atmel AT32 and
+	  AT91 parts.  This driver also supports the Cadence GEM (Gigabit
+	  Ethernet MAC found in some ARM SoC devices).  Note: the Gigabit mode
+	  is not yet supported.  Say Y to include support for the MACB/GEM chip.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called macb.
 
-endif # NET_ATMEL
+endif # NET_CADENCE
diff --git a/drivers/net/ethernet/cadence/at91_ether.c b/drivers/net/ethernet/cadence/at91_ether.c
index 56624d303487..1a5b6efa0120 100644
--- a/drivers/net/ethernet/cadence/at91_ether.c
+++ b/drivers/net/ethernet/cadence/at91_ether.c
@@ -26,6 +26,7 @@
 #include <linux/skbuff.h>
 #include <linux/dma-mapping.h>
 #include <linux/ethtool.h>
+#include <linux/platform_data/macb.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/gfp.h>
@@ -255,8 +256,7 @@ static void enable_phyirq(struct net_device *dev)
 	unsigned int dsintr, irq_number;
 	int status;
 
-	irq_number = lp->board_data.phy_irq_pin;
-	if (!irq_number) {
+	if (!gpio_is_valid(lp->board_data.phy_irq_pin)) {
 		/*
 		 * PHY doesn't have an IRQ pin (RTL8201, DP83847, AC101L),
 		 * or board does not have it connected.
@@ -265,6 +265,7 @@ static void enable_phyirq(struct net_device *dev)
 		return;
 	}
 
+	irq_number = lp->board_data.phy_irq_pin;
 	status = request_irq(irq_number, at91ether_phy_interrupt, 0, dev->name, dev);
 	if (status) {
 		printk(KERN_ERR "at91_ether: PHY IRQ %d request failed - status %d!\n", irq_number, status);
@@ -319,8 +320,7 @@ static void disable_phyirq(struct net_device *dev)
 	unsigned int dsintr;
 	unsigned int irq_number;
 
-	irq_number = lp->board_data.phy_irq_pin;
-	if (!irq_number) {
+	if (!gpio_is_valid(lp->board_data.phy_irq_pin)) {
 		del_timer_sync(&lp->check_timer);
 		return;
 	}
@@ -365,6 +365,7 @@ static void disable_phyirq(struct net_device *dev)
 	disable_mdi();
 	spin_unlock_irq(&lp->lock);
 
+	irq_number = lp->board_data.phy_irq_pin;
 	free_irq(irq_number, dev);			/* Free interrupt handler */
 }
 
@@ -984,7 +985,7 @@ static const struct net_device_ops at91ether_netdev_ops = {
 static int __init at91ether_setup(unsigned long phy_type, unsigned short phy_address,
 			struct platform_device *pdev, struct clk *ether_clk)
 {
-	struct at91_eth_data *board_data = pdev->dev.platform_data;
+	struct macb_platform_data *board_data = pdev->dev.platform_data;
 	struct net_device *dev;
 	struct at91_private *lp;
 	unsigned int val;
@@ -1077,7 +1078,7 @@ static int __init at91ether_setup(unsigned long phy_type, unsigned short phy_add
 	netif_carrier_off(dev);		/* will be enabled in open() */
 
 	/* If board has no PHY IRQ, use a timer to poll the PHY */
-	if (!lp->board_data.phy_irq_pin) {
+	if (!gpio_is_valid(lp->board_data.phy_irq_pin)) {
 		init_timer(&lp->check_timer);
 		lp->check_timer.data = (unsigned long)dev;
 		lp->check_timer.function = at91ether_check_link;
@@ -1169,7 +1170,8 @@ static int __devexit at91ether_remove(struct platform_device *pdev)
 	struct net_device *dev = platform_get_drvdata(pdev);
 	struct at91_private *lp = netdev_priv(dev);
 
-	if (lp->board_data.phy_irq_pin >= 32)
+	if (gpio_is_valid(lp->board_data.phy_irq_pin) &&
+	    lp->board_data.phy_irq_pin >= 32)
 		gpio_free(lp->board_data.phy_irq_pin);
 
 	unregister_netdev(dev);
@@ -1188,11 +1190,12 @@ static int at91ether_suspend(struct platform_device *pdev, pm_message_t mesg)
 {
 	struct net_device *net_dev = platform_get_drvdata(pdev);
 	struct at91_private *lp = netdev_priv(net_dev);
-	int phy_irq = lp->board_data.phy_irq_pin;
 
 	if (netif_running(net_dev)) {
-		if (phy_irq)
+		if (gpio_is_valid(lp->board_data.phy_irq_pin)) {
+			int phy_irq = lp->board_data.phy_irq_pin;
 			disable_irq(phy_irq);
+		}
 
 		netif_stop_queue(net_dev);
 		netif_device_detach(net_dev);
@@ -1206,7 +1209,6 @@ static int at91ether_resume(struct platform_device *pdev)
 {
 	struct net_device *net_dev = platform_get_drvdata(pdev);
 	struct at91_private *lp = netdev_priv(net_dev);
-	int phy_irq = lp->board_data.phy_irq_pin;
 
 	if (netif_running(net_dev)) {
 		clk_enable(lp->ether_clk);
@@ -1214,8 +1216,10 @@ static int at91ether_resume(struct platform_device *pdev)
 		netif_device_attach(net_dev);
 		netif_start_queue(net_dev);
 
-		if (phy_irq)
+		if (gpio_is_valid(lp->board_data.phy_irq_pin)) {
+			int phy_irq = lp->board_data.phy_irq_pin;
 			enable_irq(phy_irq);
+		}
 	}
 	return 0;
 }
diff --git a/drivers/net/ethernet/cadence/at91_ether.h b/drivers/net/ethernet/cadence/at91_ether.h
index 353f4dab62be..3725fbb0defe 100644
--- a/drivers/net/ethernet/cadence/at91_ether.h
+++ b/drivers/net/ethernet/cadence/at91_ether.h
@@ -85,7 +85,9 @@ struct recv_desc_bufs
 struct at91_private
 {
 	struct mii_if_info mii;			/* ethtool support */
-	struct at91_eth_data board_data;	/* board-specific configuration */
+	struct macb_platform_data board_data;	/* board-specific
+						 * configuration (shared with
+						 * macb for common data */
 	struct clk *ether_clk;			/* clock */
 
 	/* PHY */
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index a437b46e5490..f3d5c65d99cf 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -1,5 +1,5 @@
 /*
- * Atmel MACB Ethernet Controller driver
+ * Cadence MACB/GEM Ethernet Controller driver
  *
  * Copyright (C) 2004-2006 Atmel Corporation
  *
@@ -8,6 +8,7 @@
  * published by the Free Software Foundation.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 #include <linux/clk.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
@@ -19,11 +20,12 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/dma-mapping.h>
+#include <linux/platform_data/macb.h>
 #include <linux/platform_device.h>
 #include <linux/phy.h>
-
-#include <mach/board.h>
-#include <mach/cpu.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_net.h>
 
 #include "macb.h"
 
@@ -60,9 +62,9 @@ static void __macb_set_hwaddr(struct macb *bp)
 	u16 top;
 
 	bottom = cpu_to_le32(*((u32 *)bp->dev->dev_addr));
-	macb_writel(bp, SA1B, bottom);
+	macb_or_gem_writel(bp, SA1B, bottom);
 	top = cpu_to_le16(*((u16 *)(bp->dev->dev_addr + 4)));
-	macb_writel(bp, SA1T, top);
+	macb_or_gem_writel(bp, SA1T, top);
 }
 
 static void __init macb_get_hwaddr(struct macb *bp)
@@ -71,8 +73,8 @@ static void __init macb_get_hwaddr(struct macb *bp)
 	u16 top;
 	u8 addr[6];
 
-	bottom = macb_readl(bp, SA1B);
-	top = macb_readl(bp, SA1T);
+	bottom = macb_or_gem_readl(bp, SA1B);
+	top = macb_or_gem_readl(bp, SA1T);
 
 	addr[0] = bottom & 0xff;
 	addr[1] = (bottom >> 8) & 0xff;
@@ -84,7 +86,7 @@ static void __init macb_get_hwaddr(struct macb *bp)
 	if (is_valid_ether_addr(addr)) {
 		memcpy(bp->dev->dev_addr, addr, sizeof(addr));
 	} else {
-		dev_info(&bp->pdev->dev, "invalid hw address, using random\n");
+		netdev_info(bp->dev, "invalid hw address, using random\n");
 		random_ether_addr(bp->dev->dev_addr);
 	}
 }
@@ -178,11 +180,12 @@ static void macb_handle_link_change(struct net_device *dev)
 
 	if (status_change) {
 		if (phydev->link)
-			printk(KERN_INFO "%s: link up (%d/%s)\n",
-			       dev->name, phydev->speed,
-			       DUPLEX_FULL == phydev->duplex ? "Full":"Half");
+			netdev_info(dev, "link up (%d/%s)\n",
+				    phydev->speed,
+				    phydev->duplex == DUPLEX_FULL ?
+				    "Full" : "Half");
 		else
-			printk(KERN_INFO "%s: link down\n", dev->name);
+			netdev_info(dev, "link down\n");
 	}
 }
 
@@ -191,25 +194,21 @@ static int macb_mii_probe(struct net_device *dev)
 {
 	struct macb *bp = netdev_priv(dev);
 	struct phy_device *phydev;
-	struct eth_platform_data *pdata;
 	int ret;
 
 	phydev = phy_find_first(bp->mii_bus);
 	if (!phydev) {
-		printk (KERN_ERR "%s: no PHY found\n", dev->name);
+		netdev_err(dev, "no PHY found\n");
 		return -1;
 	}
 
-	pdata = bp->pdev->dev.platform_data;
 	/* TODO : add pin_irq */
 
 	/* attach the mac to the phy */
 	ret = phy_connect_direct(dev, phydev, &macb_handle_link_change, 0,
-				 pdata && pdata->is_rmii ?
-				 PHY_INTERFACE_MODE_RMII :
-				 PHY_INTERFACE_MODE_MII);
+				 bp->phy_interface);
 	if (ret) {
-		printk(KERN_ERR "%s: Could not attach to PHY\n", dev->name);
+		netdev_err(dev, "Could not attach to PHY\n");
 		return ret;
 	}
 
@@ -228,7 +227,7 @@ static int macb_mii_probe(struct net_device *dev)
 
 static int macb_mii_init(struct macb *bp)
 {
-	struct eth_platform_data *pdata;
+	struct macb_platform_data *pdata;
 	int err = -ENXIO, i;
 
 	/* Enable management port */
@@ -285,8 +284,8 @@ err_out:
 static void macb_update_stats(struct macb *bp)
 {
 	u32 __iomem *reg = bp->regs + MACB_PFR;
-	u32 *p = &bp->hw_stats.rx_pause_frames;
-	u32 *end = &bp->hw_stats.tx_pause_frames + 1;
+	u32 *p = &bp->hw_stats.macb.rx_pause_frames;
+	u32 *end = &bp->hw_stats.macb.tx_pause_frames + 1;
 
 	WARN_ON((unsigned long)(end - p - 1) != (MACB_TPF - MACB_PFR) / 4);
 
@@ -303,14 +302,13 @@ static void macb_tx(struct macb *bp)
 	status = macb_readl(bp, TSR);
 	macb_writel(bp, TSR, status);
 
-	dev_dbg(&bp->pdev->dev, "macb_tx status = %02lx\n",
-		(unsigned long)status);
+	netdev_dbg(bp->dev, "macb_tx status = %02lx\n", (unsigned long)status);
 
 	if (status & (MACB_BIT(UND) | MACB_BIT(TSR_RLE))) {
 		int i;
-		printk(KERN_ERR "%s: TX %s, resetting buffers\n",
-			bp->dev->name, status & MACB_BIT(UND) ?
-			"underrun" : "retry limit exceeded");
+		netdev_err(bp->dev, "TX %s, resetting buffers\n",
+			   status & MACB_BIT(UND) ?
+			   "underrun" : "retry limit exceeded");
 
 		/* Transfer ongoing, disable transmitter, to avoid confusion */
 		if (status & MACB_BIT(TGO))
@@ -369,8 +367,8 @@ static void macb_tx(struct macb *bp)
 		if (!(bufstat & MACB_BIT(TX_USED)))
 			break;
 
-		dev_dbg(&bp->pdev->dev, "skb %u (data %p) TX complete\n",
-			tail, skb->data);
+		netdev_dbg(bp->dev, "skb %u (data %p) TX complete\n",
+			   tail, skb->data);
 		dma_unmap_single(&bp->pdev->dev, rp->mapping, skb->len,
 				 DMA_TO_DEVICE);
 		bp->stats.tx_packets++;
@@ -395,8 +393,8 @@ static int macb_rx_frame(struct macb *bp, unsigned int first_frag,
 
 	len = MACB_BFEXT(RX_FRMLEN, bp->rx_ring[last_frag].ctrl);
 
-	dev_dbg(&bp->pdev->dev, "macb_rx_frame frags %u - %u (len %u)\n",
-		first_frag, last_frag, len);
+	netdev_dbg(bp->dev, "macb_rx_frame frags %u - %u (len %u)\n",
+		   first_frag, last_frag, len);
 
 	skb = dev_alloc_skb(len + RX_OFFSET);
 	if (!skb) {
@@ -437,8 +435,8 @@ static int macb_rx_frame(struct macb *bp, unsigned int first_frag,
 
 	bp->stats.rx_packets++;
 	bp->stats.rx_bytes += len;
-	dev_dbg(&bp->pdev->dev, "received skb of length %u, csum: %08x\n",
-		skb->len, skb->csum);
+	netdev_dbg(bp->dev, "received skb of length %u, csum: %08x\n",
+		   skb->len, skb->csum);
 	netif_receive_skb(skb);
 
 	return 0;
@@ -515,8 +513,8 @@ static int macb_poll(struct napi_struct *napi, int budget)
 
 	work_done = 0;
 
-	dev_dbg(&bp->pdev->dev, "poll: status = %08lx, budget = %d\n",
-		(unsigned long)status, budget);
+	netdev_dbg(bp->dev, "poll: status = %08lx, budget = %d\n",
+		   (unsigned long)status, budget);
 
 	work_done = macb_rx(bp, budget);
 	if (work_done < budget) {
@@ -565,8 +563,7 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 			macb_writel(bp, IDR, MACB_RX_INT_FLAGS);
 
 			if (napi_schedule_prep(&bp->napi)) {
-				dev_dbg(&bp->pdev->dev,
-					"scheduling RX softirq\n");
+				netdev_dbg(bp->dev, "scheduling RX softirq\n");
 				__napi_schedule(&bp->napi);
 			}
 		}
@@ -582,16 +579,19 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 
 		if (status & MACB_BIT(ISR_ROVR)) {
 			/* We missed at least one packet */
-			bp->hw_stats.rx_overruns++;
+			if (macb_is_gem(bp))
+				bp->hw_stats.gem.rx_overruns++;
+			else
+				bp->hw_stats.macb.rx_overruns++;
 		}
 
 		if (status & MACB_BIT(HRESP)) {
 			/*
-			 * TODO: Reset the hardware, and maybe move the printk
-			 * to a lower-priority context as well (work queue?)
+			 * TODO: Reset the hardware, and maybe move the
+			 * netdev_err to a lower-priority context as well
+			 * (work queue?)
 			 */
-			printk(KERN_ERR "%s: DMA bus error: HRESP not OK\n",
-			       dev->name);
+			netdev_err(dev, "DMA bus error: HRESP not OK\n");
 		}
 
 		status = macb_readl(bp, ISR);
@@ -626,16 +626,12 @@ static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	unsigned long flags;
 
 #ifdef DEBUG
-	int i;
-	dev_dbg(&bp->pdev->dev,
-		"start_xmit: len %u head %p data %p tail %p end %p\n",
-		skb->len, skb->head, skb->data,
-		skb_tail_pointer(skb), skb_end_pointer(skb));
-	dev_dbg(&bp->pdev->dev,
-		"data:");
-	for (i = 0; i < 16; i++)
-		printk(" %02x", (unsigned int)skb->data[i]);
-	printk("\n");
+	netdev_dbg(bp->dev,
+		   "start_xmit: len %u head %p data %p tail %p end %p\n",
+		   skb->len, skb->head, skb->data,
+		   skb_tail_pointer(skb), skb_end_pointer(skb));
+	print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_OFFSET, 16, 1,
+		       skb->data, 16, true);
 #endif
 
 	len = skb->len;
@@ -645,21 +641,20 @@ static int macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (TX_BUFFS_AVAIL(bp) < 1) {
 		netif_stop_queue(dev);
 		spin_unlock_irqrestore(&bp->lock, flags);
-		dev_err(&bp->pdev->dev,
-			"BUG! Tx Ring full when queue awake!\n");
-		dev_dbg(&bp->pdev->dev, "tx_head = %u, tx_tail = %u\n",
-			bp->tx_head, bp->tx_tail);
+		netdev_err(bp->dev, "BUG! Tx Ring full when queue awake!\n");
+		netdev_dbg(bp->dev, "tx_head = %u, tx_tail = %u\n",
+			   bp->tx_head, bp->tx_tail);
 		return NETDEV_TX_BUSY;
 	}
 
 	entry = bp->tx_head;
-	dev_dbg(&bp->pdev->dev, "Allocated ring entry %u\n", entry);
+	netdev_dbg(bp->dev, "Allocated ring entry %u\n", entry);
 	mapping = dma_map_single(&bp->pdev->dev, skb->data,
 				 len, DMA_TO_DEVICE);
 	bp->tx_skb[entry].skb = skb;
 	bp->tx_skb[entry].mapping = mapping;
-	dev_dbg(&bp->pdev->dev, "Mapped skb data %p to DMA addr %08lx\n",
-		skb->data, (unsigned long)mapping);
+	netdev_dbg(bp->dev, "Mapped skb data %p to DMA addr %08lx\n",
+		   skb->data, (unsigned long)mapping);
 
 	ctrl = MACB_BF(TX_FRMLEN, len);
 	ctrl |= MACB_BIT(TX_LAST);
@@ -723,27 +718,27 @@ static int macb_alloc_consistent(struct macb *bp)
 					 &bp->rx_ring_dma, GFP_KERNEL);
 	if (!bp->rx_ring)
 		goto out_err;
-	dev_dbg(&bp->pdev->dev,
-		"Allocated RX ring of %d bytes at %08lx (mapped %p)\n",
-		size, (unsigned long)bp->rx_ring_dma, bp->rx_ring);
+	netdev_dbg(bp->dev,
+		   "Allocated RX ring of %d bytes at %08lx (mapped %p)\n",
+		   size, (unsigned long)bp->rx_ring_dma, bp->rx_ring);
 
 	size = TX_RING_BYTES;
 	bp->tx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
 					 &bp->tx_ring_dma, GFP_KERNEL);
 	if (!bp->tx_ring)
 		goto out_err;
-	dev_dbg(&bp->pdev->dev,
-		"Allocated TX ring of %d bytes at %08lx (mapped %p)\n",
-		size, (unsigned long)bp->tx_ring_dma, bp->tx_ring);
+	netdev_dbg(bp->dev,
+		   "Allocated TX ring of %d bytes at %08lx (mapped %p)\n",
+		   size, (unsigned long)bp->tx_ring_dma, bp->tx_ring);
 
 	size = RX_RING_SIZE * RX_BUFFER_SIZE;
 	bp->rx_buffers = dma_alloc_coherent(&bp->pdev->dev, size,
 					    &bp->rx_buffers_dma, GFP_KERNEL);
 	if (!bp->rx_buffers)
 		goto out_err;
-	dev_dbg(&bp->pdev->dev,
-		"Allocated RX buffers of %d bytes at %08lx (mapped %p)\n",
-		size, (unsigned long)bp->rx_buffers_dma, bp->rx_buffers);
+	netdev_dbg(bp->dev,
+		   "Allocated RX buffers of %d bytes at %08lx (mapped %p)\n",
+		   size, (unsigned long)bp->rx_buffers_dma, bp->rx_buffers);
 
 	return 0;
 
@@ -797,6 +792,84 @@ static void macb_reset_hw(struct macb *bp)
 	macb_readl(bp, ISR);
 }
 
+static u32 gem_mdc_clk_div(struct macb *bp)
+{
+	u32 config;
+	unsigned long pclk_hz = clk_get_rate(bp->pclk);
+
+	if (pclk_hz <= 20000000)
+		config = GEM_BF(CLK, GEM_CLK_DIV8);
+	else if (pclk_hz <= 40000000)
+		config = GEM_BF(CLK, GEM_CLK_DIV16);
+	else if (pclk_hz <= 80000000)
+		config = GEM_BF(CLK, GEM_CLK_DIV32);
+	else if (pclk_hz <= 120000000)
+		config = GEM_BF(CLK, GEM_CLK_DIV48);
+	else if (pclk_hz <= 160000000)
+		config = GEM_BF(CLK, GEM_CLK_DIV64);
+	else
+		config = GEM_BF(CLK, GEM_CLK_DIV96);
+
+	return config;
+}
+
+static u32 macb_mdc_clk_div(struct macb *bp)
+{
+	u32 config;
+	unsigned long pclk_hz;
+
+	if (macb_is_gem(bp))
+		return gem_mdc_clk_div(bp);
+
+	pclk_hz = clk_get_rate(bp->pclk);
+	if (pclk_hz <= 20000000)
+		config = MACB_BF(CLK, MACB_CLK_DIV8);
+	else if (pclk_hz <= 40000000)
+		config = MACB_BF(CLK, MACB_CLK_DIV16);
+	else if (pclk_hz <= 80000000)
+		config = MACB_BF(CLK, MACB_CLK_DIV32);
+	else
+		config = MACB_BF(CLK, MACB_CLK_DIV64);
+
+	return config;
+}
+
+/*
+ * Get the DMA bus width field of the network configuration register that we
+ * should program.  We find the width from decoding the design configuration
+ * register to find the maximum supported data bus width.
+ */
+static u32 macb_dbw(struct macb *bp)
+{
+	if (!macb_is_gem(bp))
+		return 0;
+
+	switch (GEM_BFEXT(DBWDEF, gem_readl(bp, DCFG1))) {
+	case 4:
+		return GEM_BF(DBW, GEM_DBW128);
+	case 2:
+		return GEM_BF(DBW, GEM_DBW64);
+	case 1:
+	default:
+		return GEM_BF(DBW, GEM_DBW32);
+	}
+}
+
+/*
+ * Configure the receive DMA engine to use the correct receive buffer size.
+ * This is a configurable parameter for GEM.
+ */
+static void macb_configure_dma(struct macb *bp)
+{
+	u32 dmacfg;
+
+	if (macb_is_gem(bp)) {
+		dmacfg = gem_readl(bp, DMACFG) & ~GEM_BF(RXBS, -1L);
+		dmacfg |= GEM_BF(RXBS, RX_BUFFER_SIZE / 64);
+		gem_writel(bp, DMACFG, dmacfg);
+	}
+}
+
 static void macb_init_hw(struct macb *bp)
 {
 	u32 config;
@@ -804,7 +877,7 @@ static void macb_init_hw(struct macb *bp)
 	macb_reset_hw(bp);
 	__macb_set_hwaddr(bp);
 
-	config = macb_readl(bp, NCFGR) & MACB_BF(CLK, -1L);
+	config = macb_mdc_clk_div(bp);
 	config |= MACB_BIT(PAE);		/* PAuse Enable */
 	config |= MACB_BIT(DRFCS);		/* Discard Rx FCS */
 	config |= MACB_BIT(BIG);		/* Receive oversized frames */
@@ -812,8 +885,11 @@ static void macb_init_hw(struct macb *bp)
 		config |= MACB_BIT(CAF);	/* Copy All Frames */
 	if (!(bp->dev->flags & IFF_BROADCAST))
 		config |= MACB_BIT(NBC);	/* No BroadCast */
+	config |= macb_dbw(bp);
 	macb_writel(bp, NCFGR, config);
 
+	macb_configure_dma(bp);
+
 	/* Initialize TX and RX buffers */
 	macb_writel(bp, RBQP, bp->rx_ring_dma);
 	macb_writel(bp, TBQP, bp->tx_ring_dma);
@@ -909,8 +985,8 @@ static void macb_sethashtable(struct net_device *dev)
 		mc_filter[bitnr >> 5] |= 1 << (bitnr & 31);
 	}
 
-	macb_writel(bp, HRB, mc_filter[0]);
-	macb_writel(bp, HRT, mc_filter[1]);
+	macb_or_gem_writel(bp, HRB, mc_filter[0]);
+	macb_or_gem_writel(bp, HRT, mc_filter[1]);
 }
 
 /*
@@ -932,8 +1008,8 @@ static void macb_set_rx_mode(struct net_device *dev)
 
 	if (dev->flags & IFF_ALLMULTI) {
 		/* Enable all multicast mode */
-		macb_writel(bp, HRB, -1);
-		macb_writel(bp, HRT, -1);
+		macb_or_gem_writel(bp, HRB, -1);
+		macb_or_gem_writel(bp, HRT, -1);
 		cfg |= MACB_BIT(NCFGR_MTI);
 	} else if (!netdev_mc_empty(dev)) {
 		/* Enable specific multicasts */
@@ -941,8 +1017,8 @@ static void macb_set_rx_mode(struct net_device *dev)
 		cfg |= MACB_BIT(NCFGR_MTI);
 	} else if (dev->flags & (~IFF_ALLMULTI)) {
 		/* Disable all multicast mode */
-		macb_writel(bp, HRB, 0);
-		macb_writel(bp, HRT, 0);
+		macb_or_gem_writel(bp, HRB, 0);
+		macb_or_gem_writel(bp, HRT, 0);
 		cfg &= ~MACB_BIT(NCFGR_MTI);
 	}
 
@@ -954,7 +1030,7 @@ static int macb_open(struct net_device *dev)
 	struct macb *bp = netdev_priv(dev);
 	int err;
 
-	dev_dbg(&bp->pdev->dev, "open\n");
+	netdev_dbg(bp->dev, "open\n");
 
 	/* if the phy is not yet register, retry later*/
 	if (!bp->phy_dev)
@@ -965,9 +1041,8 @@ static int macb_open(struct net_device *dev)
 
 	err = macb_alloc_consistent(bp);
 	if (err) {
-		printk(KERN_ERR
-		       "%s: Unable to allocate DMA memory (error %d)\n",
-		       dev->name, err);
+		netdev_err(dev, "Unable to allocate DMA memory (error %d)\n",
+			   err);
 		return err;
 	}
 
@@ -1005,11 +1080,62 @@ static int macb_close(struct net_device *dev)
 	return 0;
 }
 
+static void gem_update_stats(struct macb *bp)
+{
+	u32 __iomem *reg = bp->regs + GEM_OTX;
+	u32 *p = &bp->hw_stats.gem.tx_octets_31_0;
+	u32 *end = &bp->hw_stats.gem.rx_udp_checksum_errors + 1;
+
+	for (; p < end; p++, reg++)
+		*p += __raw_readl(reg);
+}
+
+static struct net_device_stats *gem_get_stats(struct macb *bp)
+{
+	struct gem_stats *hwstat = &bp->hw_stats.gem;
+	struct net_device_stats *nstat = &bp->stats;
+
+	gem_update_stats(bp);
+
+	nstat->rx_errors = (hwstat->rx_frame_check_sequence_errors +
+			    hwstat->rx_alignment_errors +
+			    hwstat->rx_resource_errors +
+			    hwstat->rx_overruns +
+			    hwstat->rx_oversize_frames +
+			    hwstat->rx_jabbers +
+			    hwstat->rx_undersized_frames +
+			    hwstat->rx_length_field_frame_errors);
+	nstat->tx_errors = (hwstat->tx_late_collisions +
+			    hwstat->tx_excessive_collisions +
+			    hwstat->tx_underrun +
+			    hwstat->tx_carrier_sense_errors);
+	nstat->multicast = hwstat->rx_multicast_frames;
+	nstat->collisions = (hwstat->tx_single_collision_frames +
+			     hwstat->tx_multiple_collision_frames +
+			     hwstat->tx_excessive_collisions);
+	nstat->rx_length_errors = (hwstat->rx_oversize_frames +
+				   hwstat->rx_jabbers +
+				   hwstat->rx_undersized_frames +
+				   hwstat->rx_length_field_frame_errors);
+	nstat->rx_over_errors = hwstat->rx_resource_errors;
+	nstat->rx_crc_errors = hwstat->rx_frame_check_sequence_errors;
+	nstat->rx_frame_errors = hwstat->rx_alignment_errors;
+	nstat->rx_fifo_errors = hwstat->rx_overruns;
+	nstat->tx_aborted_errors = hwstat->tx_excessive_collisions;
+	nstat->tx_carrier_errors = hwstat->tx_carrier_sense_errors;
+	nstat->tx_fifo_errors = hwstat->tx_underrun;
+
+	return nstat;
+}
+
 static struct net_device_stats *macb_get_stats(struct net_device *dev)
 {
 	struct macb *bp = netdev_priv(dev);
 	struct net_device_stats *nstat = &bp->stats;
-	struct macb_stats *hwstat = &bp->hw_stats;
+	struct macb_stats *hwstat = &bp->hw_stats.macb;
+
+	if (macb_is_gem(bp))
+		return gem_get_stats(bp);
 
 	/* read stats from hardware */
 	macb_update_stats(bp);
@@ -1117,14 +1243,59 @@ static const struct net_device_ops macb_netdev_ops = {
 #endif
 };
 
+#if defined(CONFIG_OF)
+static const struct of_device_id macb_dt_ids[] = {
+	{ .compatible = "cdns,at32ap7000-macb" },
+	{ .compatible = "cdns,at91sam9260-macb" },
+	{ .compatible = "cdns,macb" },
+	{ .compatible = "cdns,pc302-gem" },
+	{ .compatible = "cdns,gem" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(of, macb_dt_ids);
+
+static int __devinit macb_get_phy_mode_dt(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+
+	if (np)
+		return of_get_phy_mode(np);
+
+	return -ENODEV;
+}
+
+static int __devinit macb_get_hwaddr_dt(struct macb *bp)
+{
+	struct device_node *np = bp->pdev->dev.of_node;
+	if (np) {
+		const char *mac = of_get_mac_address(np);
+		if (mac) {
+			memcpy(bp->dev->dev_addr, mac, ETH_ALEN);
+			return 0;
+		}
+	}
+
+	return -ENODEV;
+}
+#else
+static int __devinit macb_get_phy_mode_dt(struct platform_device *pdev)
+{
+	return -ENODEV;
+}
+static int __devinit macb_get_hwaddr_dt(struct macb *bp)
+{
+	return -ENODEV;
+}
+#endif
+
 static int __init macb_probe(struct platform_device *pdev)
 {
-	struct eth_platform_data *pdata;
+	struct macb_platform_data *pdata;
 	struct resource *regs;
 	struct net_device *dev;
 	struct macb *bp;
 	struct phy_device *phydev;
-	unsigned long pclk_hz;
 	u32 config;
 	int err = -ENXIO;
 
@@ -1152,28 +1323,19 @@ static int __init macb_probe(struct platform_device *pdev)
 
 	spin_lock_init(&bp->lock);
 
-#if defined(CONFIG_ARCH_AT91)
-	bp->pclk = clk_get(&pdev->dev, "macb_clk");
+	bp->pclk = clk_get(&pdev->dev, "pclk");
 	if (IS_ERR(bp->pclk)) {
 		dev_err(&pdev->dev, "failed to get macb_clk\n");
 		goto err_out_free_dev;
 	}
 	clk_enable(bp->pclk);
-#else
-	bp->pclk = clk_get(&pdev->dev, "pclk");
-	if (IS_ERR(bp->pclk)) {
-		dev_err(&pdev->dev, "failed to get pclk\n");
-		goto err_out_free_dev;
-	}
+
 	bp->hclk = clk_get(&pdev->dev, "hclk");
 	if (IS_ERR(bp->hclk)) {
 		dev_err(&pdev->dev, "failed to get hclk\n");
 		goto err_out_put_pclk;
 	}
-
-	clk_enable(bp->pclk);
 	clk_enable(bp->hclk);
-#endif
 
 	bp->regs = ioremap(regs->start, resource_size(regs));
 	if (!bp->regs) {
@@ -1185,9 +1347,8 @@ static int __init macb_probe(struct platform_device *pdev)
 	dev->irq = platform_get_irq(pdev, 0);
 	err = request_irq(dev->irq, macb_interrupt, 0, dev->name, dev);
 	if (err) {
-		printk(KERN_ERR
-		       "%s: Unable to request IRQ %d (error %d)\n",
-		       dev->name, dev->irq, err);
+		dev_err(&pdev->dev, "Unable to request IRQ %d (error %d)\n",
+			dev->irq, err);
 		goto err_out_iounmap;
 	}
 
@@ -1198,31 +1359,37 @@ static int __init macb_probe(struct platform_device *pdev)
 	dev->base_addr = regs->start;
 
 	/* Set MII management clock divider */
-	pclk_hz = clk_get_rate(bp->pclk);
-	if (pclk_hz <= 20000000)
-		config = MACB_BF(CLK, MACB_CLK_DIV8);
-	else if (pclk_hz <= 40000000)
-		config = MACB_BF(CLK, MACB_CLK_DIV16);
-	else if (pclk_hz <= 80000000)
-		config = MACB_BF(CLK, MACB_CLK_DIV32);
-	else
-		config = MACB_BF(CLK, MACB_CLK_DIV64);
+	config = macb_mdc_clk_div(bp);
+	config |= macb_dbw(bp);
 	macb_writel(bp, NCFGR, config);
 
-	macb_get_hwaddr(bp);
-	pdata = pdev->dev.platform_data;
+	err = macb_get_hwaddr_dt(bp);
+	if (err < 0)
+		macb_get_hwaddr(bp);
+
+	err = macb_get_phy_mode_dt(pdev);
+	if (err < 0) {
+		pdata = pdev->dev.platform_data;
+		if (pdata && pdata->is_rmii)
+			bp->phy_interface = PHY_INTERFACE_MODE_RMII;
+		else
+			bp->phy_interface = PHY_INTERFACE_MODE_MII;
+	} else {
+		bp->phy_interface = err;
+	}
 
-	if (pdata && pdata->is_rmii)
+	if (bp->phy_interface == PHY_INTERFACE_MODE_RMII)
 #if defined(CONFIG_ARCH_AT91)
-		macb_writel(bp, USRIO, (MACB_BIT(RMII) | MACB_BIT(CLKEN)) );
+		macb_or_gem_writel(bp, USRIO, (MACB_BIT(RMII) |
+					       MACB_BIT(CLKEN)));
 #else
-		macb_writel(bp, USRIO, 0);
+		macb_or_gem_writel(bp, USRIO, 0);
 #endif
 	else
 #if defined(CONFIG_ARCH_AT91)
-		macb_writel(bp, USRIO, MACB_BIT(CLKEN));
+		macb_or_gem_writel(bp, USRIO, MACB_BIT(CLKEN));
 #else
-		macb_writel(bp, USRIO, MACB_BIT(MII));
+		macb_or_gem_writel(bp, USRIO, MACB_BIT(MII));
 #endif
 
 	bp->tx_pending = DEF_TX_RING_PENDING;
@@ -1239,13 +1406,13 @@ static int __init macb_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, dev);
 
-	printk(KERN_INFO "%s: Atmel MACB at 0x%08lx irq %d (%pM)\n",
-	       dev->name, dev->base_addr, dev->irq, dev->dev_addr);
+	netdev_info(dev, "Cadence %s at 0x%08lx irq %d (%pM)\n",
+		    macb_is_gem(bp) ? "GEM" : "MACB", dev->base_addr,
+		    dev->irq, dev->dev_addr);
 
 	phydev = bp->phy_dev;
-	printk(KERN_INFO "%s: attached PHY driver [%s] "
-		"(mii_bus:phy_addr=%s, irq=%d)\n", dev->name,
-		phydev->drv->name, dev_name(&phydev->dev), phydev->irq);
+	netdev_info(dev, "attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
+		    phydev->drv->name, dev_name(&phydev->dev), phydev->irq);
 
 	return 0;
 
@@ -1256,14 +1423,10 @@ err_out_free_irq:
 err_out_iounmap:
 	iounmap(bp->regs);
 err_out_disable_clocks:
-#ifndef CONFIG_ARCH_AT91
 	clk_disable(bp->hclk);
 	clk_put(bp->hclk);
-#endif
 	clk_disable(bp->pclk);
-#ifndef CONFIG_ARCH_AT91
 err_out_put_pclk:
-#endif
 	clk_put(bp->pclk);
 err_out_free_dev:
 	free_netdev(dev);
@@ -1289,10 +1452,8 @@ static int __exit macb_remove(struct platform_device *pdev)
 		unregister_netdev(dev);
 		free_irq(dev->irq, dev);
 		iounmap(bp->regs);
-#ifndef CONFIG_ARCH_AT91
 		clk_disable(bp->hclk);
 		clk_put(bp->hclk);
-#endif
 		clk_disable(bp->pclk);
 		clk_put(bp->pclk);
 		free_netdev(dev);
@@ -1310,9 +1471,7 @@ static int macb_suspend(struct platform_device *pdev, pm_message_t state)
 
 	netif_device_detach(netdev);
 
-#ifndef CONFIG_ARCH_AT91
 	clk_disable(bp->hclk);
-#endif
 	clk_disable(bp->pclk);
 
 	return 0;
@@ -1324,9 +1483,7 @@ static int macb_resume(struct platform_device *pdev)
 	struct macb *bp = netdev_priv(netdev);
 
 	clk_enable(bp->pclk);
-#ifndef CONFIG_ARCH_AT91
 	clk_enable(bp->hclk);
-#endif
 
 	netif_device_attach(netdev);
 
@@ -1344,6 +1501,7 @@ static struct platform_driver macb_driver = {
 	.driver		= {
 		.name		= "macb",
 		.owner	= THIS_MODULE,
+		.of_match_table	= of_match_ptr(macb_dt_ids),
 	},
 };
 
@@ -1361,6 +1519,6 @@ module_init(macb_init);
 module_exit(macb_exit);
 
 MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Atmel MACB Ethernet driver");
+MODULE_DESCRIPTION("Cadence MACB/GEM Ethernet driver");
 MODULE_AUTHOR("Haavard Skinnemoen (Atmel)");
 MODULE_ALIAS("platform:macb");
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index d3212f6db703..335e288f5314 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -59,6 +59,24 @@
 #define MACB_TPQ				0x00bc
 #define MACB_USRIO				0x00c0
 #define MACB_WOL				0x00c4
+#define MACB_MID				0x00fc
+
+/* GEM register offsets. */
+#define GEM_NCFGR				0x0004
+#define GEM_USRIO				0x000c
+#define GEM_DMACFG				0x0010
+#define GEM_HRB					0x0080
+#define GEM_HRT					0x0084
+#define GEM_SA1B				0x0088
+#define GEM_SA1T				0x008C
+#define GEM_OTX					0x0100
+#define GEM_DCFG1				0x0280
+#define GEM_DCFG2				0x0284
+#define GEM_DCFG3				0x0288
+#define GEM_DCFG4				0x028c
+#define GEM_DCFG5				0x0290
+#define GEM_DCFG6				0x0294
+#define GEM_DCFG7				0x0298
 
 /* Bitfields in NCR */
 #define MACB_LB_OFFSET				0
@@ -126,6 +144,21 @@
 #define MACB_IRXFCS_OFFSET			19
 #define MACB_IRXFCS_SIZE			1
 
+/* GEM specific NCFGR bitfields. */
+#define GEM_CLK_OFFSET				18
+#define GEM_CLK_SIZE				3
+#define GEM_DBW_OFFSET				21
+#define GEM_DBW_SIZE				2
+
+/* Constants for data bus width. */
+#define GEM_DBW32				0
+#define GEM_DBW64				1
+#define GEM_DBW128				2
+
+/* Bitfields in DMACFG. */
+#define GEM_RXBS_OFFSET				16
+#define GEM_RXBS_SIZE				8
+
 /* Bitfields in NSR */
 #define MACB_NSR_LINK_OFFSET			0
 #define MACB_NSR_LINK_SIZE			1
@@ -228,12 +261,30 @@
 #define MACB_WOL_MTI_OFFSET			19
 #define MACB_WOL_MTI_SIZE			1
 
+/* Bitfields in MID */
+#define MACB_IDNUM_OFFSET			16
+#define MACB_IDNUM_SIZE				16
+#define MACB_REV_OFFSET				0
+#define MACB_REV_SIZE				16
+
+/* Bitfields in DCFG1. */
+#define GEM_DBWDEF_OFFSET			25
+#define GEM_DBWDEF_SIZE				3
+
 /* Constants for CLK */
 #define MACB_CLK_DIV8				0
 #define MACB_CLK_DIV16				1
 #define MACB_CLK_DIV32				2
 #define MACB_CLK_DIV64				3
 
+/* GEM specific constants for CLK. */
+#define GEM_CLK_DIV8				0
+#define GEM_CLK_DIV16				1
+#define GEM_CLK_DIV32				2
+#define GEM_CLK_DIV48				3
+#define GEM_CLK_DIV64				4
+#define GEM_CLK_DIV96				5
+
 /* Constants for MAN register */
 #define MACB_MAN_SOF				1
 #define MACB_MAN_WRITE				1
@@ -254,11 +305,52 @@
 		    << MACB_##name##_OFFSET))		\
 	 | MACB_BF(name,value))
 
+#define GEM_BIT(name)					\
+	(1 << GEM_##name##_OFFSET)
+#define GEM_BF(name, value)				\
+	(((value) & ((1 << GEM_##name##_SIZE) - 1))	\
+	 << GEM_##name##_OFFSET)
+#define GEM_BFEXT(name, value)\
+	(((value) >> GEM_##name##_OFFSET)		\
+	 & ((1 << GEM_##name##_SIZE) - 1))
+#define GEM_BFINS(name, value, old)			\
+	(((old) & ~(((1 << GEM_##name##_SIZE) - 1)	\
+		    << GEM_##name##_OFFSET))		\
+	 | GEM_BF(name, value))
+
 /* Register access macros */
 #define macb_readl(port,reg)				\
 	__raw_readl((port)->regs + MACB_##reg)
 #define macb_writel(port,reg,value)			\
 	__raw_writel((value), (port)->regs + MACB_##reg)
+#define gem_readl(port, reg)				\
+	__raw_readl((port)->regs + GEM_##reg)
+#define gem_writel(port, reg, value)			\
+	__raw_writel((value), (port)->regs + GEM_##reg)
+
+/*
+ * Conditional GEM/MACB macros.  These perform the operation to the correct
+ * register dependent on whether the device is a GEM or a MACB.  For registers
+ * and bitfields that are common across both devices, use macb_{read,write}l
+ * to avoid the cost of the conditional.
+ */
+#define macb_or_gem_writel(__bp, __reg, __value) \
+	({ \
+		if (macb_is_gem((__bp))) \
+			gem_writel((__bp), __reg, __value); \
+		else \
+			macb_writel((__bp), __reg, __value); \
+	})
+
+#define macb_or_gem_readl(__bp, __reg) \
+	({ \
+		u32 __v; \
+		if (macb_is_gem((__bp))) \
+			__v = gem_readl((__bp), __reg); \
+		else \
+			__v = macb_readl((__bp), __reg); \
+		__v; \
+	})
 
 struct dma_desc {
 	u32	addr;
@@ -358,6 +450,54 @@ struct macb_stats {
 	u32	tx_pause_frames;
 };
 
+struct gem_stats {
+	u32	tx_octets_31_0;
+	u32	tx_octets_47_32;
+	u32	tx_frames;
+	u32	tx_broadcast_frames;
+	u32	tx_multicast_frames;
+	u32	tx_pause_frames;
+	u32	tx_64_byte_frames;
+	u32	tx_65_127_byte_frames;
+	u32	tx_128_255_byte_frames;
+	u32	tx_256_511_byte_frames;
+	u32	tx_512_1023_byte_frames;
+	u32	tx_1024_1518_byte_frames;
+	u32	tx_greater_than_1518_byte_frames;
+	u32	tx_underrun;
+	u32	tx_single_collision_frames;
+	u32	tx_multiple_collision_frames;
+	u32	tx_excessive_collisions;
+	u32	tx_late_collisions;
+	u32	tx_deferred_frames;
+	u32	tx_carrier_sense_errors;
+	u32	rx_octets_31_0;
+	u32	rx_octets_47_32;
+	u32	rx_frames;
+	u32	rx_broadcast_frames;
+	u32	rx_multicast_frames;
+	u32	rx_pause_frames;
+	u32	rx_64_byte_frames;
+	u32	rx_65_127_byte_frames;
+	u32	rx_128_255_byte_frames;
+	u32	rx_256_511_byte_frames;
+	u32	rx_512_1023_byte_frames;
+	u32	rx_1024_1518_byte_frames;
+	u32	rx_greater_than_1518_byte_frames;
+	u32	rx_undersized_frames;
+	u32	rx_oversize_frames;
+	u32	rx_jabbers;
+	u32	rx_frame_check_sequence_errors;
+	u32	rx_length_field_frame_errors;
+	u32	rx_symbol_errors;
+	u32	rx_alignment_errors;
+	u32	rx_resource_errors;
+	u32	rx_overruns;
+	u32	rx_ip_header_checksum_errors;
+	u32	rx_tcp_checksum_errors;
+	u32	rx_udp_checksum_errors;
+};
+
 struct macb {
 	void __iomem		*regs;
 
@@ -376,7 +516,10 @@ struct macb {
 	struct net_device	*dev;
 	struct napi_struct	napi;
 	struct net_device_stats	stats;
-	struct macb_stats	hw_stats;
+	union {
+		struct macb_stats	macb;
+		struct gem_stats	gem;
+	}			hw_stats;
 
 	dma_addr_t		rx_ring_dma;
 	dma_addr_t		tx_ring_dma;
@@ -389,6 +532,13 @@ struct macb {
 	unsigned int 		link;
 	unsigned int 		speed;
 	unsigned int 		duplex;
+
+	phy_interface_t		phy_interface;
 };
 
+static inline bool macb_is_gem(struct macb *bp)
+{
+	return MACB_BFEXT(IDNUM, macb_readl(bp, MID)) == 0x2;
+}
+
 #endif /* _MACB_H */
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 6c46753aeb43..a6bcdb5cd2be 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -3080,7 +3080,7 @@ fw_exit:
 	return status;
 }
 
-static struct net_device_ops be_netdev_ops = {
+static const struct net_device_ops be_netdev_ops = {
 	.ndo_open		= be_open,
 	.ndo_stop		= be_close,
 	.ndo_start_xmit		= be_xmit,
diff --git a/drivers/net/ethernet/freescale/fec.c b/drivers/net/ethernet/freescale/fec.c
index 20c2e3f3e18a..ddcbbb34d1b9 100644
--- a/drivers/net/ethernet/freescale/fec.c
+++ b/drivers/net/ethernet/freescale/fec.c
@@ -1610,7 +1610,7 @@ fec_probe(struct platform_device *pdev)
 		ret = PTR_ERR(fep->clk);
 		goto failed_clk;
 	}
-	clk_enable(fep->clk);
+	clk_prepare_enable(fep->clk);
 
 	ret = fec_enet_init(ndev);
 	if (ret)
@@ -1633,7 +1633,7 @@ failed_register:
 	fec_enet_mii_remove(fep);
 failed_mii_init:
 failed_init:
-	clk_disable(fep->clk);
+	clk_disable_unprepare(fep->clk);
 	clk_put(fep->clk);
 failed_clk:
 	for (i = 0; i < FEC_IRQ_NUM; i++) {
@@ -1666,7 +1666,7 @@ fec_drv_remove(struct platform_device *pdev)
 		if (irq > 0)
 			free_irq(irq, ndev);
 	}
-	clk_disable(fep->clk);
+	clk_disable_unprepare(fep->clk);
 	clk_put(fep->clk);
 	iounmap(fep->hwp);
 	free_netdev(ndev);
@@ -1691,7 +1691,7 @@ fec_suspend(struct device *dev)
 		fec_stop(ndev);
 		netif_device_detach(ndev);
 	}
-	clk_disable(fep->clk);
+	clk_disable_unprepare(fep->clk);
 
 	return 0;
 }
@@ -1702,7 +1702,7 @@ fec_resume(struct device *dev)
 	struct net_device *ndev = dev_get_drvdata(dev);
 	struct fec_enet_private *fep = netdev_priv(ndev);
 
-	clk_enable(fep->clk);
+	clk_prepare_enable(fep->clk);
 	if (netif_running(ndev)) {
 		fec_restart(ndev, fep->full_duplex);
 		netif_device_attach(ndev);
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index e87847e32ddb..80aab4e5d695 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -2511,7 +2511,7 @@ static void mv643xx_eth_netpoll(struct net_device *dev)
 /* platform glue ************************************************************/
 static void
 mv643xx_eth_conf_mbus_windows(struct mv643xx_eth_shared_private *msp,
-			      struct mbus_dram_target_info *dram)
+			      const struct mbus_dram_target_info *dram)
 {
 	void __iomem *base = msp->base;
 	u32 win_enable;
@@ -2529,7 +2529,7 @@ mv643xx_eth_conf_mbus_windows(struct mv643xx_eth_shared_private *msp,
 	win_protect = 0;
 
 	for (i = 0; i < dram->num_cs; i++) {
-		struct mbus_dram_window *cs = dram->cs + i;
+		const struct mbus_dram_window *cs = dram->cs + i;
 
 		writel((cs->base & 0xffff0000) |
 			(cs->mbus_attr << 8) |
@@ -2579,6 +2579,7 @@ static int mv643xx_eth_shared_probe(struct platform_device *pdev)
 	static int mv643xx_eth_version_printed;
 	struct mv643xx_eth_shared_platform_data *pd = pdev->dev.platform_data;
 	struct mv643xx_eth_shared_private *msp;
+	const struct mbus_dram_target_info *dram;
 	struct resource *res;
 	int ret;
 
@@ -2643,8 +2644,9 @@ static int mv643xx_eth_shared_probe(struct platform_device *pdev)
 	/*
 	 * (Re-)program MBUS remapping windows if we are asked to.
 	 */
-	if (pd != NULL && pd->dram != NULL)
-		mv643xx_eth_conf_mbus_windows(msp, pd->dram);
+	dram = mv_mbus_dram_info();
+	if (dram)
+		mv643xx_eth_conf_mbus_windows(msp, dram);
 
 	/*
 	 * Detect hardware parameters.
diff --git a/drivers/net/ethernet/rdc/r6040.c b/drivers/net/ethernet/rdc/r6040.c
index 87aa43935070..cb0eca807852 100644
--- a/drivers/net/ethernet/rdc/r6040.c
+++ b/drivers/net/ethernet/rdc/r6040.c
@@ -1160,7 +1160,7 @@ static int __devinit r6040_init_one(struct pci_dev *pdev,
 	lp->dev = dev;
 
 	/* Init RDC private data */
-	lp->mcr0 = MCR0_XMTEN | MCR0;
+	lp->mcr0 = MCR0_XMTEN | MCR0_RCVEN;
 
 	/* The RDC-specific entries in the device structure. */
 	dev->netdev_ops = &r6040_netdev_ops;
diff --git a/drivers/net/ethernet/realtek/8139cp.c b/drivers/net/ethernet/realtek/8139cp.c
index cc6b391479ca..abc79076f867 100644
--- a/drivers/net/ethernet/realtek/8139cp.c
+++ b/drivers/net/ethernet/realtek/8139cp.c
@@ -563,6 +563,7 @@ rx_next:
 		if (cpr16(IntrStatus) & cp_rx_intr_mask)
 			goto rx_status_loop;
 
+		napi_gro_flush(napi);
 		spin_lock_irqsave(&cp->lock, flags);
 		__napi_complete(napi);
 		cpw16_f(IntrMask, cp_intr_mask);
diff --git a/drivers/net/ethernet/smsc/smsc911x.h b/drivers/net/ethernet/smsc/smsc911x.h
index 938ecf290813..9ad5e5d39a03 100644
--- a/drivers/net/ethernet/smsc/smsc911x.h
+++ b/drivers/net/ethernet/smsc/smsc911x.h
@@ -401,8 +401,6 @@
 #include <asm/smsc911x.h>
 #endif
 
-#ifdef CONFIG_SMSC_PHY
 #include <linux/smscphy.h>
-#endif
 
 #endif				/* __SMSC911X_H__ */
diff --git a/drivers/net/ethernet/tile/tilepro.c b/drivers/net/ethernet/tile/tilepro.c
index 6b75063988ec..d9951afb9269 100644
--- a/drivers/net/ethernet/tile/tilepro.c
+++ b/drivers/net/ethernet/tile/tilepro.c
@@ -2260,8 +2260,7 @@ static int tile_net_get_mac(struct net_device *dev)
 	return 0;
 }
 
-
-static struct net_device_ops tile_net_ops = {
+static const struct net_device_ops tile_net_ops = {
 	.ndo_open = tile_net_open,
 	.ndo_stop = tile_net_stop,
 	.ndo_start_xmit = tile_net_tx,
diff --git a/drivers/net/hyperv/Kconfig b/drivers/net/hyperv/Kconfig
new file mode 100644
index 000000000000..936968d23559
--- /dev/null
+++ b/drivers/net/hyperv/Kconfig
@@ -0,0 +1,5 @@
+config HYPERV_NET
+	tristate "Microsoft Hyper-V virtual network driver"
+	depends on HYPERV
+	help
+	  Select this option to enable the Hyper-V virtual network driver.
diff --git a/drivers/net/hyperv/Makefile b/drivers/net/hyperv/Makefile
new file mode 100644
index 000000000000..c8a66827100c
--- /dev/null
+++ b/drivers/net/hyperv/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_HYPERV_NET) += hv_netvsc.o
+
+hv_netvsc-y := netvsc_drv.o netvsc.o rndis_filter.o
diff --git a/drivers/staging/hv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index ac1ec8405124..dec5836ae075 100644
--- a/drivers/staging/hv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -39,9 +39,6 @@ struct xferpage_packet {
 	u32 count;
 };
 
-/* The number of pages which are enough to cover jumbo frame buffer. */
-#define NETVSC_PACKET_MAXPAGE		4
-
 /*
  * Represent netvsc packet which contains 1 RNDIS and 1 ethernet frame
  * within the RNDIS
@@ -77,8 +74,9 @@ struct hv_netvsc_packet {
 
 	u32 total_data_buflen;
 	/* Points to the send/receive buffer where the ethernet frame is */
+	void *data;
 	u32 page_buf_cnt;
-	struct hv_page_buffer page_buf[NETVSC_PACKET_MAXPAGE];
+	struct hv_page_buffer page_buf[0];
 };
 
 struct netvsc_device_info {
@@ -87,6 +85,27 @@ struct netvsc_device_info {
 	int  ring_size;
 };
 
+enum rndis_device_state {
+	RNDIS_DEV_UNINITIALIZED = 0,
+	RNDIS_DEV_INITIALIZING,
+	RNDIS_DEV_INITIALIZED,
+	RNDIS_DEV_DATAINITIALIZED,
+};
+
+struct rndis_device {
+	struct netvsc_device *net_dev;
+
+	enum rndis_device_state state;
+	bool link_state;
+	atomic_t new_req_id;
+
+	spinlock_t request_lock;
+	struct list_head req_list;
+
+	unsigned char hw_mac_adr[ETH_ALEN];
+};
+
+
 /* Interface */
 int netvsc_device_add(struct hv_device *device, void *additional_info);
 int netvsc_device_remove(struct hv_device *device);
@@ -109,11 +128,13 @@ int rndis_filter_receive(struct hv_device *dev,
 int rndis_filter_send(struct hv_device *dev,
 			struct hv_netvsc_packet *pkt);
 
+int rndis_filter_set_packet_filter(struct rndis_device *dev, u32 new_filter);
+
+
 #define NVSP_INVALID_PROTOCOL_VERSION	((u32)0xFFFFFFFF)
 
 #define NVSP_PROTOCOL_VERSION_1		2
-#define NVSP_MIN_PROTOCOL_VERSION	NVSP_PROTOCOL_VERSION_1
-#define NVSP_MAX_PROTOCOL_VERSION	NVSP_PROTOCOL_VERSION_1
+#define NVSP_PROTOCOL_VERSION_2		0x30002
 
 enum {
 	NVSP_MSG_TYPE_NONE = 0,
@@ -138,11 +159,36 @@ enum {
 	NVSP_MSG1_TYPE_SEND_RNDIS_PKT,
 	NVSP_MSG1_TYPE_SEND_RNDIS_PKT_COMPLETE,
 
-	/*
-	 * This should be set to the number of messages for the version with
-	 * the maximum number of messages.
-	 */
-	NVSP_NUM_MSG_PER_VERSION		= 9,
+	/* Version 2 messages */
+	NVSP_MSG2_TYPE_SEND_CHIMNEY_DELEGATED_BUF,
+	NVSP_MSG2_TYPE_SEND_CHIMNEY_DELEGATED_BUF_COMP,
+	NVSP_MSG2_TYPE_REVOKE_CHIMNEY_DELEGATED_BUF,
+
+	NVSP_MSG2_TYPE_RESUME_CHIMNEY_RX_INDICATION,
+
+	NVSP_MSG2_TYPE_TERMINATE_CHIMNEY,
+	NVSP_MSG2_TYPE_TERMINATE_CHIMNEY_COMP,
+
+	NVSP_MSG2_TYPE_INDICATE_CHIMNEY_EVENT,
+
+	NVSP_MSG2_TYPE_SEND_CHIMNEY_PKT,
+	NVSP_MSG2_TYPE_SEND_CHIMNEY_PKT_COMP,
+
+	NVSP_MSG2_TYPE_POST_CHIMNEY_RECV_REQ,
+	NVSP_MSG2_TYPE_POST_CHIMNEY_RECV_REQ_COMP,
+
+	NVSP_MSG2_TYPE_ALLOC_RXBUF,
+	NVSP_MSG2_TYPE_ALLOC_RXBUF_COMP,
+
+	NVSP_MSG2_TYPE_FREE_RXBUF,
+
+	NVSP_MSG2_TYPE_SEND_VMQ_RNDIS_PKT,
+	NVSP_MSG2_TYPE_SEND_VMQ_RNDIS_PKT_COMP,
+
+	NVSP_MSG2_TYPE_SEND_NDIS_CONFIG,
+
+	NVSP_MSG2_TYPE_ALLOC_CHIMNEY_HANDLE,
+	NVSP_MSG2_TYPE_ALLOC_CHIMNEY_HANDLE_COMP,
 };
 
 enum {
@@ -153,6 +199,7 @@ enum {
 	NVSP_STAT_PROTOCOL_TOO_OLD,
 	NVSP_STAT_INVALID_RNDIS_PKT,
 	NVSP_STAT_BUSY,
+	NVSP_STAT_PROTOCOL_UNSUPPORTED,
 	NVSP_STAT_MAX,
 };
 
@@ -337,9 +384,69 @@ union nvsp_1_message_uber {
 						send_rndis_pkt_complete;
 } __packed;
 
+
+/*
+ * Network VSP protocol version 2 messages:
+ */
+struct nvsp_2_vsc_capability {
+	union {
+		u64 data;
+		struct {
+			u64 vmq:1;
+			u64 chimney:1;
+			u64 sriov:1;
+			u64 ieee8021q:1;
+			u64 correlation_id:1;
+		};
+	};
+} __packed;
+
+struct nvsp_2_send_ndis_config {
+	u32 mtu;
+	u32 reserved;
+	struct nvsp_2_vsc_capability capability;
+} __packed;
+
+/* Allocate receive buffer */
+struct nvsp_2_alloc_rxbuf {
+	/* Allocation ID to match the allocation request and response */
+	u32 alloc_id;
+
+	/* Length of the VM shared memory receive buffer that needs to
+	 * be allocated
+	 */
+	u32 len;
+} __packed;
+
+/* Allocate receive buffer complete */
+struct nvsp_2_alloc_rxbuf_comp {
+	/* The NDIS_STATUS code for buffer allocation */
+	u32 status;
+
+	u32 alloc_id;
+
+	/* GPADL handle for the allocated receive buffer */
+	u32 gpadl_handle;
+
+	/* Receive buffer ID */
+	u64 recv_buf_id;
+} __packed;
+
+struct nvsp_2_free_rxbuf {
+	u64 recv_buf_id;
+} __packed;
+
+union nvsp_2_message_uber {
+	struct nvsp_2_send_ndis_config send_ndis_config;
+	struct nvsp_2_alloc_rxbuf alloc_rxbuf;
+	struct nvsp_2_alloc_rxbuf_comp alloc_rxbuf_comp;
+	struct nvsp_2_free_rxbuf free_rxbuf;
+} __packed;
+
 union nvsp_all_messages {
 	union nvsp_message_init_uber init_msg;
 	union nvsp_1_message_uber v1_msg;
+	union nvsp_2_message_uber v2_msg;
 } __packed;
 
 /* ALL Messages */
@@ -349,12 +456,9 @@ struct nvsp_message {
 } __packed;
 
 
+#define NETVSC_MTU 65536
 
-
-/* #define NVSC_MIN_PROTOCOL_VERSION		1 */
-/* #define NVSC_MAX_PROTOCOL_VERSION		1 */
-
-#define NETVSC_RECEIVE_BUFFER_SIZE		(1024*1024)	/* 1MB */
+#define NETVSC_RECEIVE_BUFFER_SIZE		(1024*1024*2)	/* 2MB */
 
 #define NETVSC_RECEIVE_BUFFER_ID		0xcafe
 
@@ -369,7 +473,10 @@ struct nvsp_message {
 struct netvsc_device {
 	struct hv_device *dev;
 
+	u32 nvsp_version;
+
 	atomic_t num_outstanding_sends;
+	bool start_remove;
 	bool destroy;
 	/*
 	 * List of free preallocated hv_netvsc_packet to represent receive
diff --git a/drivers/staging/hv/netvsc.c b/drivers/net/hyperv/netvsc.c
index b902579c7fe6..8965b45ce5a5 100644
--- a/drivers/staging/hv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -28,6 +28,7 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/netdevice.h>
+#include <linux/if_ether.h>
 
 #include "hyperv_net.h"
 
@@ -41,7 +42,7 @@ static struct netvsc_device *alloc_net_device(struct hv_device *device)
 	if (!net_device)
 		return NULL;
 
-
+	net_device->start_remove = false;
 	net_device->destroy = false;
 	net_device->dev = device;
 	net_device->ndev = ndev;
@@ -230,19 +231,16 @@ static int netvsc_init_recv_buf(struct hv_device *device)
 	net_device->recv_section_cnt = init_packet->msg.
 		v1_msg.send_recv_buf_complete.num_sections;
 
-	net_device->recv_section = kmalloc(net_device->recv_section_cnt
-		* sizeof(struct nvsp_1_receive_buffer_section), GFP_KERNEL);
+	net_device->recv_section = kmemdup(
+		init_packet->msg.v1_msg.send_recv_buf_complete.sections,
+		net_device->recv_section_cnt *
+		sizeof(struct nvsp_1_receive_buffer_section),
+		GFP_KERNEL);
 	if (net_device->recv_section == NULL) {
 		ret = -EINVAL;
 		goto cleanup;
 	}
 
-	memcpy(net_device->recv_section,
-		init_packet->msg.v1_msg.
-	       send_recv_buf_complete.sections,
-		net_device->recv_section_cnt *
-	       sizeof(struct nvsp_1_receive_buffer_section));
-
 	/*
 	 * For 1st release, there should only be 1 section that represents the
 	 * entire receive buffer
@@ -263,27 +261,18 @@ exit:
 }
 
 
-static int netvsc_connect_vsp(struct hv_device *device)
+/* Negotiate NVSP protocol version */
+static int negotiate_nvsp_ver(struct hv_device *device,
+			      struct netvsc_device *net_device,
+			      struct nvsp_message *init_packet,
+			      u32 nvsp_ver)
 {
 	int ret, t;
-	struct netvsc_device *net_device;
-	struct nvsp_message *init_packet;
-	int ndis_version;
-	struct net_device *ndev;
-
-	net_device = get_outbound_net_device(device);
-	if (!net_device)
-		return -ENODEV;
-	ndev = net_device->ndev;
-
-	init_packet = &net_device->channel_init_pkt;
 
 	memset(init_packet, 0, sizeof(struct nvsp_message));
 	init_packet->hdr.msg_type = NVSP_MSG_TYPE_INIT;
-	init_packet->msg.init_msg.init.min_protocol_ver =
-		NVSP_MIN_PROTOCOL_VERSION;
-	init_packet->msg.init_msg.init.max_protocol_ver =
-		NVSP_MAX_PROTOCOL_VERSION;
+	init_packet->msg.init_msg.init.min_protocol_ver = nvsp_ver;
+	init_packet->msg.init_msg.init.max_protocol_ver = nvsp_ver;
 
 	/* Send the init request */
 	ret = vmbus_sendpacket(device->channel, init_packet,
@@ -293,26 +282,62 @@ static int netvsc_connect_vsp(struct hv_device *device)
 			       VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
 
 	if (ret != 0)
-		goto cleanup;
+		return ret;
 
 	t = wait_for_completion_timeout(&net_device->channel_init_wait, 5*HZ);
 
-	if (t == 0) {
-		ret = -ETIMEDOUT;
-		goto cleanup;
-	}
+	if (t == 0)
+		return -ETIMEDOUT;
 
 	if (init_packet->msg.init_msg.init_complete.status !=
-	    NVSP_STAT_SUCCESS) {
-		ret = -EINVAL;
-		goto cleanup;
-	}
+	    NVSP_STAT_SUCCESS)
+		return -EINVAL;
 
-	if (init_packet->msg.init_msg.init_complete.
-	    negotiated_protocol_ver != NVSP_PROTOCOL_VERSION_1) {
+	if (nvsp_ver != NVSP_PROTOCOL_VERSION_2)
+		return 0;
+
+	/* NVSPv2 only: Send NDIS config */
+	memset(init_packet, 0, sizeof(struct nvsp_message));
+	init_packet->hdr.msg_type = NVSP_MSG2_TYPE_SEND_NDIS_CONFIG;
+	init_packet->msg.v2_msg.send_ndis_config.mtu = net_device->ndev->mtu;
+
+	ret = vmbus_sendpacket(device->channel, init_packet,
+				sizeof(struct nvsp_message),
+				(unsigned long)init_packet,
+				VM_PKT_DATA_INBAND, 0);
+
+	return ret;
+}
+
+static int netvsc_connect_vsp(struct hv_device *device)
+{
+	int ret;
+	struct netvsc_device *net_device;
+	struct nvsp_message *init_packet;
+	int ndis_version;
+	struct net_device *ndev;
+
+	net_device = get_outbound_net_device(device);
+	if (!net_device)
+		return -ENODEV;
+	ndev = net_device->ndev;
+
+	init_packet = &net_device->channel_init_pkt;
+
+	/* Negotiate the latest NVSP protocol supported */
+	if (negotiate_nvsp_ver(device, net_device, init_packet,
+			       NVSP_PROTOCOL_VERSION_2) == 0) {
+		net_device->nvsp_version = NVSP_PROTOCOL_VERSION_2;
+	} else if (negotiate_nvsp_ver(device, net_device, init_packet,
+				    NVSP_PROTOCOL_VERSION_1) == 0) {
+		net_device->nvsp_version = NVSP_PROTOCOL_VERSION_1;
+	} else {
 		ret = -EPROTO;
 		goto cleanup;
 	}
+
+	pr_debug("Negotiated NVSP version:%x\n", net_device->nvsp_version);
+
 	/* Send the ndis version */
 	memset(init_packet, 0, sizeof(struct nvsp_message));
 
@@ -438,6 +463,9 @@ static void netvsc_send_completion(struct hv_device *device,
 			nvsc_packet->completion.send.send_completion_ctx);
 
 		atomic_dec(&net_device->num_outstanding_sends);
+
+		if (netif_queue_stopped(ndev) && !net_device->start_remove)
+			netif_wake_queue(ndev);
 	} else {
 		netdev_err(ndev, "Unknown send completion packet type- "
 			   "%d received!!\n", nvsp_packet->hdr.msg_type);
@@ -488,11 +516,16 @@ int netvsc_send(struct hv_device *device,
 
 	}
 
-	if (ret != 0)
+	if (ret == 0) {
+		atomic_inc(&net_device->num_outstanding_sends);
+	} else if (ret == -EAGAIN) {
+		netif_stop_queue(ndev);
+		if (atomic_read(&net_device->num_outstanding_sends) < 1)
+			netif_wake_queue(ndev);
+	} else {
 		netdev_err(ndev, "Unable to send packet %p ret %d\n",
 			   packet, ret);
-	else
-		atomic_inc(&net_device->num_outstanding_sends);
+	}
 
 	return ret;
 }
@@ -598,12 +631,10 @@ static void netvsc_receive(struct hv_device *device,
 	struct vmtransfer_page_packet_header *vmxferpage_packet;
 	struct nvsp_message *nvsp_packet;
 	struct hv_netvsc_packet *netvsc_packet = NULL;
-	unsigned long start;
-	unsigned long end, end_virtual;
 	/* struct netvsc_driver *netvscDriver; */
 	struct xferpage_packet *xferpage_packet = NULL;
-	int i, j;
-	int count = 0, bytes_remain = 0;
+	int i;
+	int count = 0;
 	unsigned long flags;
 	struct net_device *ndev;
 
@@ -712,53 +743,10 @@ static void netvsc_receive(struct hv_device *device,
 		netvsc_packet->completion.recv.recv_completion_tid =
 					vmxferpage_packet->d.trans_id;
 
+		netvsc_packet->data = (void *)((unsigned long)net_device->
+			recv_buf + vmxferpage_packet->ranges[i].byte_offset);
 		netvsc_packet->total_data_buflen =
 					vmxferpage_packet->ranges[i].byte_count;
-		netvsc_packet->page_buf_cnt = 1;
-
-		netvsc_packet->page_buf[0].len =
-					vmxferpage_packet->ranges[i].byte_count;
-
-		start = virt_to_phys((void *)((unsigned long)net_device->
-		recv_buf + vmxferpage_packet->ranges[i].byte_offset));
-
-		netvsc_packet->page_buf[0].pfn = start >> PAGE_SHIFT;
-		end_virtual = (unsigned long)net_device->recv_buf
-		    + vmxferpage_packet->ranges[i].byte_offset
-		    + vmxferpage_packet->ranges[i].byte_count - 1;
-		end = virt_to_phys((void *)end_virtual);
-
-		/* Calculate the page relative offset */
-		netvsc_packet->page_buf[0].offset =
-			vmxferpage_packet->ranges[i].byte_offset &
-			(PAGE_SIZE - 1);
-		if ((end >> PAGE_SHIFT) != (start >> PAGE_SHIFT)) {
-			/* Handle frame across multiple pages: */
-			netvsc_packet->page_buf[0].len =
-				(netvsc_packet->page_buf[0].pfn <<
-				 PAGE_SHIFT)
-				+ PAGE_SIZE - start;
-			bytes_remain = netvsc_packet->total_data_buflen -
-					netvsc_packet->page_buf[0].len;
-			for (j = 1; j < NETVSC_PACKET_MAXPAGE; j++) {
-				netvsc_packet->page_buf[j].offset = 0;
-				if (bytes_remain <= PAGE_SIZE) {
-					netvsc_packet->page_buf[j].len =
-						bytes_remain;
-					bytes_remain = 0;
-				} else {
-					netvsc_packet->page_buf[j].len =
-						PAGE_SIZE;
-					bytes_remain -= PAGE_SIZE;
-				}
-				netvsc_packet->page_buf[j].pfn =
-				    virt_to_phys((void *)(end_virtual -
-						bytes_remain)) >> PAGE_SHIFT;
-				netvsc_packet->page_buf_cnt++;
-				if (bytes_remain == 0)
-					break;
-			}
-		}
 
 		/* Pass it to the upper layer */
 		rndis_filter_receive(device, netvsc_packet);
diff --git a/drivers/staging/hv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 93b0e91cbf98..462d05f05e84 100644
--- a/drivers/staging/hv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -43,24 +43,59 @@
 struct net_device_context {
 	/* point back to our device context */
 	struct hv_device *device_ctx;
-	atomic_t avail;
 	struct delayed_work dwork;
 };
 
 
-#define PACKET_PAGES_LOWATER  8
-/* Need this many pages to handle worst case fragmented packet */
-#define PACKET_PAGES_HIWATER  (MAX_SKB_FRAGS + 2)
-
 static int ring_size = 128;
 module_param(ring_size, int, S_IRUGO);
 MODULE_PARM_DESC(ring_size, "Ring buffer size (# of pages)");
 
-/* no-op so the netdev core doesn't return -EINVAL when modifying the the
- * multicast address list in SIOCADDMULTI. hv is setup to get all multicast
- * when it calls RndisFilterOnOpen() */
+struct set_multicast_work {
+	struct work_struct work;
+	struct net_device *net;
+};
+
+static void do_set_multicast(struct work_struct *w)
+{
+	struct set_multicast_work *swk =
+		container_of(w, struct set_multicast_work, work);
+	struct net_device *net = swk->net;
+
+	struct net_device_context *ndevctx = netdev_priv(net);
+	struct netvsc_device *nvdev;
+	struct rndis_device *rdev;
+
+	nvdev = hv_get_drvdata(ndevctx->device_ctx);
+	if (nvdev == NULL)
+		return;
+
+	rdev = nvdev->extension;
+	if (rdev == NULL)
+		return;
+
+	if (net->flags & IFF_PROMISC)
+		rndis_filter_set_packet_filter(rdev,
+			NDIS_PACKET_TYPE_PROMISCUOUS);
+	else
+		rndis_filter_set_packet_filter(rdev,
+			NDIS_PACKET_TYPE_BROADCAST |
+			NDIS_PACKET_TYPE_ALL_MULTICAST |
+			NDIS_PACKET_TYPE_DIRECTED);
+
+	kfree(w);
+}
+
 static void netvsc_set_multicast_list(struct net_device *net)
 {
+	struct set_multicast_work *swk =
+		kmalloc(sizeof(struct set_multicast_work), GFP_ATOMIC);
+	if (swk == NULL)
+		return;
+
+	swk->net = net;
+	INIT_WORK(&swk->work, do_set_multicast);
+	schedule_work(&swk->work);
 }
 
 static int netvsc_open(struct net_device *net)
@@ -104,18 +139,8 @@ static void netvsc_xmit_completion(void *context)
 
 	kfree(packet);
 
-	if (skb) {
-		struct net_device *net = skb->dev;
-		struct net_device_context *net_device_ctx = netdev_priv(net);
-		unsigned int num_pages = skb_shinfo(skb)->nr_frags + 2;
-
+	if (skb)
 		dev_kfree_skb_any(skb);
-
-		atomic_add(num_pages, &net_device_ctx->avail);
-		if (atomic_read(&net_device_ctx->avail) >=
-				PACKET_PAGES_HIWATER)
-			netif_wake_queue(net);
-	}
 }
 
 static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
@@ -123,12 +148,12 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	struct net_device_context *net_device_ctx = netdev_priv(net);
 	struct hv_netvsc_packet *packet;
 	int ret;
-	unsigned int i, num_pages;
+	unsigned int i, num_pages, npg_data;
 
-	/* Add 1 for skb->data and additional one for RNDIS */
-	num_pages = skb_shinfo(skb)->nr_frags + 1 + 1;
-	if (num_pages > atomic_read(&net_device_ctx->avail))
-		return NETDEV_TX_BUSY;
+	/* Add multipage for skb->data and additional one for RNDIS */
+	npg_data = (((unsigned long)skb->data + skb_headlen(skb) - 1)
+		>> PAGE_SHIFT) - ((unsigned long)skb->data >> PAGE_SHIFT) + 1;
+	num_pages = skb_shinfo(skb)->nr_frags + npg_data + 1;
 
 	/* Allocate a netvsc packet based on # of frags. */
 	packet = kzalloc(sizeof(struct hv_netvsc_packet) +
@@ -151,21 +176,36 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	packet->page_buf_cnt = num_pages;
 
 	/* Initialize it from the skb */
-	packet->total_data_buflen	= skb->len;
+	packet->total_data_buflen = skb->len;
 
 	/* Start filling in the page buffers starting after RNDIS buffer. */
 	packet->page_buf[1].pfn = virt_to_phys(skb->data) >> PAGE_SHIFT;
 	packet->page_buf[1].offset
 		= (unsigned long)skb->data & (PAGE_SIZE - 1);
-	packet->page_buf[1].len = skb_headlen(skb);
+	if (npg_data == 1)
+		packet->page_buf[1].len = skb_headlen(skb);
+	else
+		packet->page_buf[1].len = PAGE_SIZE
+			- packet->page_buf[1].offset;
+
+	for (i = 2; i <= npg_data; i++) {
+		packet->page_buf[i].pfn = virt_to_phys(skb->data
+			+ PAGE_SIZE * (i-1)) >> PAGE_SHIFT;
+		packet->page_buf[i].offset = 0;
+		packet->page_buf[i].len = PAGE_SIZE;
+	}
+	if (npg_data > 1)
+		packet->page_buf[npg_data].len = (((unsigned long)skb->data
+			+ skb_headlen(skb) - 1) & (PAGE_SIZE - 1)) + 1;
 
 	/* Additional fragments are after SKB data */
 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 		const skb_frag_t *f = &skb_shinfo(skb)->frags[i];
 
-		packet->page_buf[i+2].pfn = page_to_pfn(skb_frag_page(f));
-		packet->page_buf[i+2].offset = f->page_offset;
-		packet->page_buf[i+2].len = skb_frag_size(f);
+		packet->page_buf[i+npg_data+1].pfn =
+			page_to_pfn(skb_frag_page(f));
+		packet->page_buf[i+npg_data+1].offset = f->page_offset;
+		packet->page_buf[i+npg_data+1].len = skb_frag_size(f);
 	}
 
 	/* Set the completion routine */
@@ -178,10 +218,6 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	if (ret == 0) {
 		net->stats.tx_bytes += skb->len;
 		net->stats.tx_packets++;
-
-		atomic_sub(num_pages, &net_device_ctx->avail);
-		if (atomic_read(&net_device_ctx->avail) < PACKET_PAGES_LOWATER)
-			netif_stop_queue(net);
 	} else {
 		/* we are shutting down or bus overloaded, just drop packet */
 		net->stats.tx_dropped++;
@@ -232,9 +268,6 @@ int netvsc_recv_callback(struct hv_device *device_obj,
 {
 	struct net_device *net = dev_get_drvdata(&device_obj->device);
 	struct sk_buff *skb;
-	void *data;
-	int i;
-	unsigned long flags;
 	struct netvsc_device *net_device;
 
 	net_device = hv_get_drvdata(device_obj);
@@ -253,27 +286,12 @@ int netvsc_recv_callback(struct hv_device *device_obj,
 		return 0;
 	}
 
-	/* for kmap_atomic */
-	local_irq_save(flags);
-
 	/*
 	 * Copy to skb. This copy is needed here since the memory pointed by
 	 * hv_netvsc_packet cannot be deallocated
 	 */
-	for (i = 0; i < packet->page_buf_cnt; i++) {
-		data = kmap_atomic(pfn_to_page(packet->page_buf[i].pfn),
-					       KM_IRQ1);
-		data = (void *)(unsigned long)data +
-				packet->page_buf[i].offset;
-
-		memcpy(skb_put(skb, packet->page_buf[i].len), data,
-		       packet->page_buf[i].len);
-
-		kunmap_atomic((void *)((unsigned long)data -
-				       packet->page_buf[i].offset), KM_IRQ1);
-	}
-
-	local_irq_restore(flags);
+	memcpy(skb_put(skb, packet->total_data_buflen), packet->data,
+		packet->total_data_buflen);
 
 	skb->protocol = eth_type_trans(skb, net);
 	skb->ip_summed = CHECKSUM_NONE;
@@ -299,6 +317,39 @@ static void netvsc_get_drvinfo(struct net_device *net,
 	strcpy(info->fw_version, "N/A");
 }
 
+static int netvsc_change_mtu(struct net_device *ndev, int mtu)
+{
+	struct net_device_context *ndevctx = netdev_priv(ndev);
+	struct hv_device *hdev =  ndevctx->device_ctx;
+	struct netvsc_device *nvdev = hv_get_drvdata(hdev);
+	struct netvsc_device_info device_info;
+	int limit = ETH_DATA_LEN;
+
+	if (nvdev == NULL || nvdev->destroy)
+		return -ENODEV;
+
+	if (nvdev->nvsp_version == NVSP_PROTOCOL_VERSION_2)
+		limit = NETVSC_MTU;
+
+	if (mtu < 68 || mtu > limit)
+		return -EINVAL;
+
+	nvdev->start_remove = true;
+	cancel_delayed_work_sync(&ndevctx->dwork);
+	netif_stop_queue(ndev);
+	rndis_filter_device_remove(hdev);
+
+	ndev->mtu = mtu;
+
+	ndevctx->device_ctx = hdev;
+	hv_set_drvdata(hdev, ndev);
+	device_info.ring_size = ring_size;
+	rndis_filter_device_add(hdev, &device_info);
+	netif_wake_queue(ndev);
+
+	return 0;
+}
+
 static const struct ethtool_ops ethtool_ops = {
 	.get_drvinfo	= netvsc_get_drvinfo,
 	.get_link	= ethtool_op_get_link,
@@ -309,7 +360,7 @@ static const struct net_device_ops device_ops = {
 	.ndo_stop =			netvsc_close,
 	.ndo_start_xmit =		netvsc_start_xmit,
 	.ndo_set_rx_mode =		netvsc_set_multicast_list,
-	.ndo_change_mtu =		eth_change_mtu,
+	.ndo_change_mtu =		netvsc_change_mtu,
 	.ndo_validate_addr =		eth_validate_addr,
 	.ndo_set_mac_address =		eth_mac_addr,
 };
@@ -351,7 +402,6 @@ static int netvsc_probe(struct hv_device *dev,
 
 	net_device_ctx = netdev_priv(net);
 	net_device_ctx->device_ctx = dev;
-	atomic_set(&net_device_ctx->avail, ring_size);
 	hv_set_drvdata(dev, net);
 	INIT_DELAYED_WORK(&net_device_ctx->dwork, netvsc_send_garp);
 
@@ -403,6 +453,8 @@ static int netvsc_remove(struct hv_device *dev)
 		return 0;
 	}
 
+	net_device->start_remove = true;
+
 	ndev_ctx = netdev_priv(net);
 	cancel_delayed_work_sync(&ndev_ctx->dwork);
 
diff --git a/drivers/staging/hv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index bafccb360041..da181f9a49d1 100644
--- a/drivers/staging/hv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -30,26 +30,6 @@
 #include "hyperv_net.h"
 
 
-enum rndis_device_state {
-	RNDIS_DEV_UNINITIALIZED = 0,
-	RNDIS_DEV_INITIALIZING,
-	RNDIS_DEV_INITIALIZED,
-	RNDIS_DEV_DATAINITIALIZED,
-};
-
-struct rndis_device {
-	struct netvsc_device *net_dev;
-
-	enum rndis_device_state state;
-	bool link_state;
-	atomic_t new_req_id;
-
-	spinlock_t request_lock;
-	struct list_head req_list;
-
-	unsigned char hw_mac_adr[ETH_ALEN];
-};
-
 struct rndis_request {
 	struct list_head list_ent;
 	struct completion  wait_event;
@@ -329,7 +309,6 @@ static void rndis_filter_receive_data(struct rndis_device *dev,
 {
 	struct rndis_packet *rndis_pkt;
 	u32 data_offset;
-	int i;
 
 	rndis_pkt = &msg->msg.pkt;
 
@@ -342,17 +321,7 @@ static void rndis_filter_receive_data(struct rndis_device *dev,
 	data_offset = RNDIS_HEADER_SIZE + rndis_pkt->data_offset;
 
 	pkt->total_data_buflen -= data_offset;
-	pkt->page_buf[0].offset += data_offset;
-	pkt->page_buf[0].len -= data_offset;
-
-	/* Drop the 0th page, if rndis data go beyond page boundary */
-	if (pkt->page_buf[0].offset >= PAGE_SIZE) {
-		pkt->page_buf[1].offset = pkt->page_buf[0].offset - PAGE_SIZE;
-		pkt->page_buf[1].len -= pkt->page_buf[1].offset;
-		pkt->page_buf_cnt--;
-		for (i = 0; i < pkt->page_buf_cnt; i++)
-			pkt->page_buf[i] = pkt->page_buf[i+1];
-	}
+	pkt->data = (void *)((unsigned long)pkt->data + data_offset);
 
 	pkt->is_data_pkt = true;
 
@@ -387,11 +356,7 @@ int rndis_filter_receive(struct hv_device *dev,
 		return -ENODEV;
 	}
 
-	rndis_hdr = (struct rndis_message *)kmap_atomic(
-			pfn_to_page(pkt->page_buf[0].pfn), KM_IRQ0);
-
-	rndis_hdr = (void *)((unsigned long)rndis_hdr +
-			pkt->page_buf[0].offset);
+	rndis_hdr = pkt->data;
 
 	/* Make sure we got a valid rndis message */
 	if ((rndis_hdr->ndis_msg_type != REMOTE_NDIS_PACKET_MSG) &&
@@ -407,8 +372,6 @@ int rndis_filter_receive(struct hv_device *dev,
 			sizeof(struct rndis_message) :
 			rndis_hdr->msg_len);
 
-	kunmap_atomic(rndis_hdr - pkt->page_buf[0].offset, KM_IRQ0);
-
 	dump_rndis_message(dev, &rndis_msg);
 
 	switch (rndis_msg.ndis_msg_type) {
@@ -522,8 +485,7 @@ static int rndis_filter_query_device_link_status(struct rndis_device *dev)
 	return ret;
 }
 
-static int rndis_filter_set_packet_filter(struct rndis_device *dev,
-				      u32 new_filter)
+int rndis_filter_set_packet_filter(struct rndis_device *dev, u32 new_filter)
 {
 	struct rndis_request *request;
 	struct rndis_set_request *set;
diff --git a/drivers/net/usb/asix.c b/drivers/net/usb/asix.c
index 1ff7163bc348..d0937c4634c9 100644
--- a/drivers/net/usb/asix.c
+++ b/drivers/net/usb/asix.c
@@ -376,7 +376,7 @@ static int asix_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 
 		skb_pull(skb, (size + 1) & 0xfffe);
 
-		if (skb->len == 0)
+		if (skb->len < sizeof(header))
 			break;
 
 		head = (u8 *) skb->data;
@@ -1152,7 +1152,7 @@ static int ax88772_bind(struct usbnet *dev, struct usb_interface *intf)
 	return 0;
 }
 
-static struct ethtool_ops ax88178_ethtool_ops = {
+static const struct ethtool_ops ax88178_ethtool_ops = {
 	.get_drvinfo		= asix_get_drvinfo,
 	.get_link		= asix_get_link,
 	.get_msglevel		= usbnet_get_msglevel,
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index cb8e595c5021..3a539a9cac54 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -138,7 +138,7 @@ struct cdc_ncm_ctx {
 static void cdc_ncm_tx_timeout(unsigned long arg);
 static const struct driver_info cdc_ncm_info;
 static struct usb_driver cdc_ncm_driver;
-static struct ethtool_ops cdc_ncm_ethtool_ops;
+static const struct ethtool_ops cdc_ncm_ethtool_ops;
 
 static const struct usb_device_id cdc_devs[] = {
 	{ USB_INTERFACE_INFO(USB_CLASS_COMM,
@@ -1220,7 +1220,7 @@ static struct usb_driver cdc_ncm_driver = {
 	.supports_autosuspend = 1,
 };
 
-static struct ethtool_ops cdc_ncm_ethtool_ops = {
+static const struct ethtool_ops cdc_ncm_ethtool_ops = {
 	.get_drvinfo = cdc_ncm_get_drvinfo,
 	.get_link = usbnet_get_link,
 	.get_msglevel = usbnet_get_msglevel,
diff --git a/drivers/net/usb/ipheth.c b/drivers/net/usb/ipheth.c
index 08a4df238796..e84662db51cc 100644
--- a/drivers/net/usb/ipheth.c
+++ b/drivers/net/usb/ipheth.c
@@ -420,7 +420,7 @@ static u32 ipheth_ethtool_op_get_link(struct net_device *net)
 	return netif_carrier_ok(dev->net);
 }
 
-static struct ethtool_ops ops = {
+static const struct ethtool_ops ops = {
 	.get_link = ipheth_ethtool_op_get_link
 };
 
diff --git a/drivers/net/usb/sierra_net.c b/drivers/net/usb/sierra_net.c
index e45dfdcb8718..b59cf20c7817 100644
--- a/drivers/net/usb/sierra_net.c
+++ b/drivers/net/usb/sierra_net.c
@@ -618,7 +618,7 @@ static u32 sierra_net_get_link(struct net_device *net)
 	return sierra_net_get_private(dev)->link_up && netif_running(net);
 }
 
-static struct ethtool_ops sierra_net_ethtool_ops = {
+static const struct ethtool_ops sierra_net_ethtool_ops = {
 	.get_drvinfo = sierra_net_get_drvinfo,
 	.get_link = sierra_net_get_link,
 	.get_msglevel = usbnet_get_msglevel,
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 639cf8ab62ba..59effac15f36 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -1634,7 +1634,7 @@ static int __init netback_init(void)
 	int rc = 0;
 	int group;
 
-	if (!xen_pv_domain())
+	if (!xen_domain())
 		return -ENODEV;
 
 	xen_netbk_group_nr = num_online_cpus();
diff --git a/drivers/net/xen-netback/xenbus.c b/drivers/net/xen-netback/xenbus.c
index 1ce729d6af75..410018c4c528 100644
--- a/drivers/net/xen-netback/xenbus.c
+++ b/drivers/net/xen-netback/xenbus.c
@@ -474,17 +474,14 @@ static const struct xenbus_device_id netback_ids[] = {
 };
 
 
-static struct xenbus_driver netback = {
-	.name = "vif",
-	.owner = THIS_MODULE,
-	.ids = netback_ids,
+static DEFINE_XENBUS_DRIVER(netback, ,
 	.probe = netback_probe,
 	.remove = netback_remove,
 	.uevent = netback_uevent,
 	.otherend_changed = frontend_changed,
-};
+);
 
 int xenvif_xenbus_init(void)
 {
-	return xenbus_register_backend(&netback);
+	return xenbus_register_backend(&netback_driver);
 }
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index 0a59c57864f5..fa679057630f 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -1914,7 +1914,7 @@ static void xennet_sysfs_delif(struct net_device *netdev)
 
 #endif /* CONFIG_SYSFS */
 
-static struct xenbus_device_id netfront_ids[] = {
+static const struct xenbus_device_id netfront_ids[] = {
 	{ "vif" },
 	{ "" }
 };
@@ -1941,15 +1941,12 @@ static int __devexit xennet_remove(struct xenbus_device *dev)
 	return 0;
 }
 
-static struct xenbus_driver netfront_driver = {
-	.name = "vif",
-	.owner = THIS_MODULE,
-	.ids = netfront_ids,
+static DEFINE_XENBUS_DRIVER(netfront, ,
 	.probe = netfront_probe,
 	.remove = __devexit_p(xennet_remove),
 	.resume = netfront_resume,
 	.otherend_changed = netback_changed,
-};
+);
 
 static int __init netif_init(void)
 {
diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
index 91a375fb6ae6..ea2bd1be2640 100644
--- a/drivers/of/fdt.c
+++ b/drivers/of/fdt.c
@@ -23,6 +23,7 @@
 #include <asm/machdep.h>
 #endif /* CONFIG_PPC */
 
+#include <asm/setup.h>
 #include <asm/page.h>
 
 char *of_fdt_get_string(struct boot_param_header *blob, u32 offset)
diff --git a/drivers/parport/parport_ax88796.c b/drivers/parport/parport_ax88796.c
index 844f6137970a..7c5d86696eed 100644
--- a/drivers/parport/parport_ax88796.c
+++ b/drivers/parport/parport_ax88796.c
@@ -420,18 +420,7 @@ static struct platform_driver axdrv = {
 	.resume		= parport_ax88796_resume,
 };
 
-static int __init parport_ax88796_init(void)
-{
-	return platform_driver_register(&axdrv);
-}
-
-static void __exit parport_ax88796_exit(void)
-{
-	platform_driver_unregister(&axdrv);
-}
-
-module_init(parport_ax88796_init)
-module_exit(parport_ax88796_exit)
+module_platform_driver(axdrv);
 
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
 MODULE_DESCRIPTION("AX88796 Parport parallel port driver");
diff --git a/drivers/parport/parport_sunbpp.c b/drivers/parport/parport_sunbpp.c
index 910c5a26e347..9390a534a2b2 100644
--- a/drivers/parport/parport_sunbpp.c
+++ b/drivers/parport/parport_sunbpp.c
@@ -391,21 +391,10 @@ static struct platform_driver bpp_sbus_driver = {
 	.remove		= __devexit_p(bpp_remove),
 };
 
-static int __init parport_sunbpp_init(void)
-{
-	return platform_driver_register(&bpp_sbus_driver);
-}
-
-static void __exit parport_sunbpp_exit(void)
-{
-	platform_driver_unregister(&bpp_sbus_driver);
-}
+module_platform_driver(bpp_sbus_driver);
 
 MODULE_AUTHOR("Derrick J Brashear");
 MODULE_DESCRIPTION("Parport Driver for Sparc bidirectional Port");
 MODULE_SUPPORTED_DEVICE("Sparc Bidirectional Parallel Port");
 MODULE_VERSION("2.0");
 MODULE_LICENSE("GPL");
-
-module_init(parport_sunbpp_init)
-module_exit(parport_sunbpp_exit)
diff --git a/drivers/pci/ats.c b/drivers/pci/ats.c
index b0dd08e6a9da..9dd90b30f91a 100644
--- a/drivers/pci/ats.c
+++ b/drivers/pci/ats.c
@@ -175,21 +175,22 @@ int pci_enable_pri(struct pci_dev *pdev, u32 reqs)
 	u32 max_requests;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PRI_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
 	if (!pos)
 		return -EINVAL;
 
-	pci_read_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, &control);
-	pci_read_config_word(pdev, pos + PCI_PRI_STATUS_OFF,  &status);
-	if ((control & PCI_PRI_ENABLE) || !(status & PCI_PRI_STATUS_STOPPED))
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+	pci_read_config_word(pdev, pos + PCI_PRI_STATUS, &status);
+	if ((control & PCI_PRI_CTRL_ENABLE) ||
+	    !(status & PCI_PRI_STATUS_STOPPED))
 		return -EBUSY;
 
-	pci_read_config_dword(pdev, pos + PCI_PRI_MAX_REQ_OFF, &max_requests);
+	pci_read_config_dword(pdev, pos + PCI_PRI_MAX_REQ, &max_requests);
 	reqs = min(max_requests, reqs);
-	pci_write_config_dword(pdev, pos + PCI_PRI_ALLOC_REQ_OFF, reqs);
+	pci_write_config_dword(pdev, pos + PCI_PRI_ALLOC_REQ, reqs);
 
-	control |= PCI_PRI_ENABLE;
-	pci_write_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, control);
+	control |= PCI_PRI_CTRL_ENABLE;
+	pci_write_config_word(pdev, pos + PCI_PRI_CTRL, control);
 
 	return 0;
 }
@@ -206,13 +207,13 @@ void pci_disable_pri(struct pci_dev *pdev)
 	u16 control;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PRI_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
 	if (!pos)
 		return;
 
-	pci_read_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, &control);
-	control &= ~PCI_PRI_ENABLE;
-	pci_write_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, control);
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+	control &= ~PCI_PRI_CTRL_ENABLE;
+	pci_write_config_word(pdev, pos + PCI_PRI_CTRL, control);
 }
 EXPORT_SYMBOL_GPL(pci_disable_pri);
 
@@ -227,13 +228,13 @@ bool pci_pri_enabled(struct pci_dev *pdev)
 	u16 control;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PRI_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
 	if (!pos)
 		return false;
 
-	pci_read_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, &control);
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
 
-	return (control & PCI_PRI_ENABLE) ? true : false;
+	return (control & PCI_PRI_CTRL_ENABLE) ? true : false;
 }
 EXPORT_SYMBOL_GPL(pci_pri_enabled);
 
@@ -249,17 +250,17 @@ int pci_reset_pri(struct pci_dev *pdev)
 	u16 control;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PRI_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
 	if (!pos)
 		return -EINVAL;
 
-	pci_read_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, &control);
-	if (control & PCI_PRI_ENABLE)
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+	if (control & PCI_PRI_CTRL_ENABLE)
 		return -EBUSY;
 
-	control |= PCI_PRI_RESET;
+	control |= PCI_PRI_CTRL_RESET;
 
-	pci_write_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, control);
+	pci_write_config_word(pdev, pos + PCI_PRI_CTRL, control);
 
 	return 0;
 }
@@ -282,14 +283,14 @@ bool pci_pri_stopped(struct pci_dev *pdev)
 	u16 control, status;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PRI_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
 	if (!pos)
 		return true;
 
-	pci_read_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, &control);
-	pci_read_config_word(pdev, pos + PCI_PRI_STATUS_OFF,  &status);
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+	pci_read_config_word(pdev, pos + PCI_PRI_STATUS, &status);
 
-	if (control & PCI_PRI_ENABLE)
+	if (control & PCI_PRI_CTRL_ENABLE)
 		return false;
 
 	return (status & PCI_PRI_STATUS_STOPPED) ? true : false;
@@ -311,15 +312,15 @@ int pci_pri_status(struct pci_dev *pdev)
 	u16 status, control;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PRI_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
 	if (!pos)
 		return -EINVAL;
 
-	pci_read_config_word(pdev, pos + PCI_PRI_CONTROL_OFF, &control);
-	pci_read_config_word(pdev, pos + PCI_PRI_STATUS_OFF,  &status);
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+	pci_read_config_word(pdev, pos + PCI_PRI_STATUS, &status);
 
 	/* Stopped bit is undefined when enable == 1, so clear it */
-	if (control & PCI_PRI_ENABLE)
+	if (control & PCI_PRI_CTRL_ENABLE)
 		status &= ~PCI_PRI_STATUS_STOPPED;
 
 	return status;
@@ -342,25 +343,25 @@ int pci_enable_pasid(struct pci_dev *pdev, int features)
 	u16 control, supported;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PASID_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PASID);
 	if (!pos)
 		return -EINVAL;
 
-	pci_read_config_word(pdev, pos + PCI_PASID_CONTROL_OFF, &control);
-	pci_read_config_word(pdev, pos + PCI_PASID_CAP_OFF,     &supported);
+	pci_read_config_word(pdev, pos + PCI_PASID_CTRL, &control);
+	pci_read_config_word(pdev, pos + PCI_PASID_CAP, &supported);
 
-	if (!(supported & PCI_PASID_ENABLE))
+	if (control & PCI_PASID_CTRL_ENABLE)
 		return -EINVAL;
 
-	supported &= PCI_PASID_EXEC | PCI_PASID_PRIV;
+	supported &= PCI_PASID_CAP_EXEC | PCI_PASID_CAP_PRIV;
 
 	/* User wants to enable anything unsupported? */
 	if ((supported & features) != features)
 		return -EINVAL;
 
-	control = PCI_PASID_ENABLE | features;
+	control = PCI_PASID_CTRL_ENABLE | features;
 
-	pci_write_config_word(pdev, pos + PCI_PASID_CONTROL_OFF, control);
+	pci_write_config_word(pdev, pos + PCI_PASID_CTRL, control);
 
 	return 0;
 }
@@ -376,11 +377,11 @@ void pci_disable_pasid(struct pci_dev *pdev)
 	u16 control = 0;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PASID_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PASID);
 	if (!pos)
 		return;
 
-	pci_write_config_word(pdev, pos + PCI_PASID_CONTROL_OFF, control);
+	pci_write_config_word(pdev, pos + PCI_PASID_CTRL, control);
 }
 EXPORT_SYMBOL_GPL(pci_disable_pasid);
 
@@ -391,22 +392,21 @@ EXPORT_SYMBOL_GPL(pci_disable_pasid);
  * Returns a negative value when no PASI capability is present.
  * Otherwise is returns a bitmask with supported features. Current
  * features reported are:
- * PCI_PASID_ENABLE - PASID capability can be enabled
- * PCI_PASID_EXEC - Execute permission supported
- * PCI_PASID_PRIV - Priviledged mode supported
+ * PCI_PASID_CAP_EXEC - Execute permission supported
+ * PCI_PASID_CAP_PRIV - Priviledged mode supported
  */
 int pci_pasid_features(struct pci_dev *pdev)
 {
 	u16 supported;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PASID_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PASID);
 	if (!pos)
 		return -EINVAL;
 
-	pci_read_config_word(pdev, pos + PCI_PASID_CAP_OFF, &supported);
+	pci_read_config_word(pdev, pos + PCI_PASID_CAP, &supported);
 
-	supported &= PCI_PASID_ENABLE | PCI_PASID_EXEC | PCI_PASID_PRIV;
+	supported &= PCI_PASID_CAP_EXEC | PCI_PASID_CAP_PRIV;
 
 	return supported;
 }
@@ -426,11 +426,11 @@ int pci_max_pasids(struct pci_dev *pdev)
 	u16 supported;
 	int pos;
 
-	pos = pci_find_ext_capability(pdev, PCI_PASID_CAP);
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PASID);
 	if (!pos)
 		return -EINVAL;
 
-	pci_read_config_word(pdev, pos + PCI_PASID_CAP_OFF, &supported);
+	pci_read_config_word(pdev, pos + PCI_PASID_CAP, &supported);
 
 	supported = (supported & PASID_NUMBER_MASK) >> PASID_NUMBER_SHIFT;
 
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index 838f571027b7..9a33fdde2d16 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -45,7 +45,6 @@ extern int pciehp_poll_time;
 extern int pciehp_debug;
 extern int pciehp_force;
 extern struct workqueue_struct *pciehp_wq;
-extern struct workqueue_struct *pciehp_ordered_wq;
 
 #define dbg(format, arg...)						\
 do {									\
diff --git a/drivers/pci/hotplug/pciehp_core.c b/drivers/pci/hotplug/pciehp_core.c
index 7ac8358df8fd..b8c99d35ac97 100644
--- a/drivers/pci/hotplug/pciehp_core.c
+++ b/drivers/pci/hotplug/pciehp_core.c
@@ -43,7 +43,6 @@ int pciehp_poll_mode;
 int pciehp_poll_time;
 int pciehp_force;
 struct workqueue_struct *pciehp_wq;
-struct workqueue_struct *pciehp_ordered_wq;
 
 #define DRIVER_VERSION	"0.4"
 #define DRIVER_AUTHOR	"Dan Zink <dan.zink@compaq.com>, Greg Kroah-Hartman <greg@kroah.com>, Dely Sy <dely.l.sy@intel.com>"
@@ -345,18 +344,11 @@ static int __init pcied_init(void)
 	if (!pciehp_wq)
 		return -ENOMEM;
 
-	pciehp_ordered_wq = alloc_ordered_workqueue("pciehp_ordered", 0);
-	if (!pciehp_ordered_wq) {
-		destroy_workqueue(pciehp_wq);
-		return -ENOMEM;
-	}
-
 	pciehp_firmware_init();
 	retval = pcie_port_service_register(&hpdriver_portdrv);
  	dbg("pcie_port_service_register = %d\n", retval);
   	info(DRIVER_DESC " version: " DRIVER_VERSION "\n");
  	if (retval) {
-		destroy_workqueue(pciehp_ordered_wq);
 		destroy_workqueue(pciehp_wq);
 		dbg("Failure to register service\n");
 	}
@@ -366,9 +358,8 @@ static int __init pcied_init(void)
 static void __exit pcied_cleanup(void)
 {
 	dbg("unload_pciehpd()\n");
-	destroy_workqueue(pciehp_ordered_wq);
-	destroy_workqueue(pciehp_wq);
 	pcie_port_service_unregister(&hpdriver_portdrv);
+	destroy_workqueue(pciehp_wq);
 	info(DRIVER_DESC " version: " DRIVER_VERSION " unloaded\n");
 }
 
diff --git a/drivers/pci/hotplug/pciehp_ctrl.c b/drivers/pci/hotplug/pciehp_ctrl.c
index 085dbb5fc168..27f44295a657 100644
--- a/drivers/pci/hotplug/pciehp_ctrl.c
+++ b/drivers/pci/hotplug/pciehp_ctrl.c
@@ -344,7 +344,7 @@ void pciehp_queue_pushbutton_work(struct work_struct *work)
 		kfree(info);
 		goto out;
 	}
-	queue_work(pciehp_ordered_wq, &info->work);
+	queue_work(pciehp_wq, &info->work);
  out:
 	mutex_unlock(&p_slot->lock);
 }
@@ -439,7 +439,7 @@ static void handle_surprise_event(struct slot *p_slot)
 	else
 		p_slot->state = POWERON_STATE;
 
-	queue_work(pciehp_ordered_wq, &info->work);
+	queue_work(pciehp_wq, &info->work);
 }
 
 static void interrupt_event_handler(struct work_struct *work)
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index 7b1414810ae3..bcdbb1643621 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -806,7 +806,6 @@ static void pcie_cleanup_slot(struct controller *ctrl)
 	struct slot *slot = ctrl->slot;
 	cancel_delayed_work(&slot->work);
 	flush_workqueue(pciehp_wq);
-	flush_workqueue(pciehp_ordered_wq);
 	kfree(slot);
 }
 
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 0e6d04d7ba4f..337e16ab4a92 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -323,6 +323,8 @@ static void free_msi_irqs(struct pci_dev *dev)
 			if (list_is_last(&entry->list, &dev->msi_list))
 				iounmap(entry->mask_base);
 		}
+		kobject_del(&entry->kobj);
+		kobject_put(&entry->kobj);
 		list_del(&entry->list);
 		kfree(entry);
 	}
@@ -403,6 +405,98 @@ void pci_restore_msi_state(struct pci_dev *dev)
 }
 EXPORT_SYMBOL_GPL(pci_restore_msi_state);
 
+
+#define to_msi_attr(obj) container_of(obj, struct msi_attribute, attr)
+#define to_msi_desc(obj) container_of(obj, struct msi_desc, kobj)
+
+struct msi_attribute {
+	struct attribute        attr;
+	ssize_t (*show)(struct msi_desc *entry, struct msi_attribute *attr,
+			char *buf);
+	ssize_t (*store)(struct msi_desc *entry, struct msi_attribute *attr,
+			 const char *buf, size_t count);
+};
+
+static ssize_t show_msi_mode(struct msi_desc *entry, struct msi_attribute *atr,
+			     char *buf)
+{
+	return sprintf(buf, "%s\n", entry->msi_attrib.is_msix ? "msix" : "msi");
+}
+
+static ssize_t msi_irq_attr_show(struct kobject *kobj,
+				 struct attribute *attr, char *buf)
+{
+	struct msi_attribute *attribute = to_msi_attr(attr);
+	struct msi_desc *entry = to_msi_desc(kobj);
+
+	if (!attribute->show)
+		return -EIO;
+
+	return attribute->show(entry, attribute, buf);
+}
+
+static const struct sysfs_ops msi_irq_sysfs_ops = {
+	.show = msi_irq_attr_show,
+};
+
+static struct msi_attribute mode_attribute =
+	__ATTR(mode, S_IRUGO, show_msi_mode, NULL);
+
+
+struct attribute *msi_irq_default_attrs[] = {
+	&mode_attribute.attr,
+	NULL
+};
+
+void msi_kobj_release(struct kobject *kobj)
+{
+	struct msi_desc *entry = to_msi_desc(kobj);
+
+	pci_dev_put(entry->dev);
+}
+
+static struct kobj_type msi_irq_ktype = {
+	.release = msi_kobj_release,
+	.sysfs_ops = &msi_irq_sysfs_ops,
+	.default_attrs = msi_irq_default_attrs,
+};
+
+static int populate_msi_sysfs(struct pci_dev *pdev)
+{
+	struct msi_desc *entry;
+	struct kobject *kobj;
+	int ret;
+	int count = 0;
+
+	pdev->msi_kset = kset_create_and_add("msi_irqs", NULL, &pdev->dev.kobj);
+	if (!pdev->msi_kset)
+		return -ENOMEM;
+
+	list_for_each_entry(entry, &pdev->msi_list, list) {
+		kobj = &entry->kobj;
+		kobj->kset = pdev->msi_kset;
+		pci_dev_get(pdev);
+		ret = kobject_init_and_add(kobj, &msi_irq_ktype, NULL,
+				     "%u", entry->irq);
+		if (ret)
+			goto out_unroll;
+
+		count++;
+	}
+
+	return 0;
+
+out_unroll:
+	list_for_each_entry(entry, &pdev->msi_list, list) {
+		if (!count)
+			break;
+		kobject_del(&entry->kobj);
+		kobject_put(&entry->kobj);
+		count--;
+	}
+	return ret;
+}
+
 /**
  * msi_capability_init - configure device's MSI capability structure
  * @dev: pointer to the pci_dev data structure of MSI device function
@@ -454,6 +548,13 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
 		return ret;
 	}
 
+	ret = populate_msi_sysfs(dev);
+	if (ret) {
+		msi_mask_irq(entry, mask, ~mask);
+		free_msi_irqs(dev);
+		return ret;
+	}
+
 	/* Set MSI enabled bits	 */
 	pci_intx_for_msi(dev, 0);
 	msi_set_enable(dev, pos, 1);
@@ -574,6 +675,12 @@ static int msix_capability_init(struct pci_dev *dev,
 
 	msix_program_entries(dev, entries);
 
+	ret = populate_msi_sysfs(dev);
+	if (ret) {
+		ret = 0;
+		goto error;
+	}
+
 	/* Set MSI-X enabled bits and unmask the function */
 	pci_intx_for_msi(dev, 0);
 	dev->msix_enabled = 1;
@@ -732,6 +839,8 @@ void pci_disable_msi(struct pci_dev *dev)
 
 	pci_msi_shutdown(dev);
 	free_msi_irqs(dev);
+	kset_unregister(dev->msi_kset);
+	dev->msi_kset = NULL;
 }
 EXPORT_SYMBOL(pci_disable_msi);
 
@@ -830,6 +939,8 @@ void pci_disable_msix(struct pci_dev *dev)
 
 	pci_msix_shutdown(dev);
 	free_msi_irqs(dev);
+	kset_unregister(dev->msi_kset);
+	dev->msi_kset = NULL;
 }
 EXPORT_SYMBOL(pci_disable_msix);
 
@@ -870,5 +981,15 @@ EXPORT_SYMBOL(pci_msi_enabled);
 
 void pci_msi_init_pci_dev(struct pci_dev *dev)
 {
+	int pos;
 	INIT_LIST_HEAD(&dev->msi_list);
+
+	/* Disable the msi hardware to avoid screaming interrupts
+	 * during boot.  This is the power on reset default so
+	 * usually this should be a noop.
+	 */
+	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
+	if (pos)
+		msi_set_enable(dev, pos, 0);
+	msix_set_enable(dev, 0);
 }
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index 4ecb6408b0d6..060fd22a1103 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -45,16 +45,20 @@ static void pci_acpi_wake_dev(acpi_handle handle, u32 event, void *context)
 {
 	struct pci_dev *pci_dev = context;
 
-	if (event == ACPI_NOTIFY_DEVICE_WAKE && pci_dev) {
+	if (event != ACPI_NOTIFY_DEVICE_WAKE || !pci_dev)
+		return;
+
+	if (!pci_dev->pm_cap || !pci_dev->pme_support
+	     || pci_check_pme_status(pci_dev)) {
 		if (pci_dev->pme_poll)
 			pci_dev->pme_poll = false;
 
 		pci_wakeup_event(pci_dev);
-		pci_check_pme_status(pci_dev);
 		pm_runtime_resume(&pci_dev->dev);
-		if (pci_dev->subordinate)
-			pci_pme_wakeup_bus(pci_dev->subordinate);
 	}
+
+	if (pci_dev->subordinate)
+		pci_pme_wakeup_bus(pci_dev->subordinate);
 }
 
 /**
@@ -395,7 +399,6 @@ static int __init acpi_pci_init(void)
 
 	if (acpi_gbl_FADT.boot_flags & ACPI_FADT_NO_ASPM) {
 		printk(KERN_INFO"ACPI FADT declares the system doesn't support PCIe ASPM, so disable it\n");
-		pcie_clear_aspm();
 		pcie_no_aspm();
 	}
 
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index cbfbab18be91..1cfbf228fbb1 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -68,7 +68,7 @@ struct pcie_link_state {
 	struct aspm_latency acceptable[8];
 };
 
-static int aspm_disabled, aspm_force, aspm_clear_state;
+static int aspm_disabled, aspm_force;
 static bool aspm_support_enabled = true;
 static DEFINE_MUTEX(aspm_lock);
 static LIST_HEAD(link_list);
@@ -500,9 +500,6 @@ static int pcie_aspm_sanity_check(struct pci_dev *pdev)
 	int pos;
 	u32 reg32;
 
-	if (aspm_clear_state)
-		return -EINVAL;
-
 	/*
 	 * Some functions in a slot might not all be PCIe functions,
 	 * very strange. Disable ASPM for the whole slot
@@ -574,9 +571,6 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev)
 	    pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM)
 		return;
 
-	if (aspm_disabled && !aspm_clear_state)
-		return;
-
 	/* VIA has a strange chipset, root port is under a bridge */
 	if (pdev->pcie_type == PCI_EXP_TYPE_ROOT_PORT &&
 	    pdev->bus->self)
@@ -608,7 +602,7 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev)
 	 * the BIOS's expectation, we'll do so once pci_enable_device() is
 	 * called.
 	 */
-	if (aspm_policy != POLICY_POWERSAVE || aspm_clear_state) {
+	if (aspm_policy != POLICY_POWERSAVE) {
 		pcie_config_aspm_path(link);
 		pcie_set_clkpm(link, policy_to_clkpm_state(link));
 	}
@@ -649,8 +643,7 @@ void pcie_aspm_exit_link_state(struct pci_dev *pdev)
 	struct pci_dev *parent = pdev->bus->self;
 	struct pcie_link_state *link, *root, *parent_link;
 
-	if ((aspm_disabled && !aspm_clear_state) || !pci_is_pcie(pdev) ||
-	    !parent || !parent->link_state)
+	if (!pci_is_pcie(pdev) || !parent || !parent->link_state)
 		return;
 	if ((parent->pcie_type != PCI_EXP_TYPE_ROOT_PORT) &&
 	    (parent->pcie_type != PCI_EXP_TYPE_DOWNSTREAM))
@@ -734,13 +727,18 @@ void pcie_aspm_powersave_config_link(struct pci_dev *pdev)
  * pci_disable_link_state - disable pci device's link state, so the link will
  * never enter specific states
  */
-static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem)
+static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem,
+				     bool force)
 {
 	struct pci_dev *parent = pdev->bus->self;
 	struct pcie_link_state *link;
 
-	if (aspm_disabled || !pci_is_pcie(pdev))
+	if (aspm_disabled && !force)
+		return;
+
+	if (!pci_is_pcie(pdev))
 		return;
+
 	if (pdev->pcie_type == PCI_EXP_TYPE_ROOT_PORT ||
 	    pdev->pcie_type == PCI_EXP_TYPE_DOWNSTREAM)
 		parent = pdev;
@@ -768,16 +766,31 @@ static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem)
 
 void pci_disable_link_state_locked(struct pci_dev *pdev, int state)
 {
-	__pci_disable_link_state(pdev, state, false);
+	__pci_disable_link_state(pdev, state, false, false);
 }
 EXPORT_SYMBOL(pci_disable_link_state_locked);
 
 void pci_disable_link_state(struct pci_dev *pdev, int state)
 {
-	__pci_disable_link_state(pdev, state, true);
+	__pci_disable_link_state(pdev, state, true, false);
 }
 EXPORT_SYMBOL(pci_disable_link_state);
 
+void pcie_clear_aspm(struct pci_bus *bus)
+{
+	struct pci_dev *child;
+
+	/*
+	 * Clear any ASPM setup that the firmware has carried out on this bus
+	 */
+	list_for_each_entry(child, &bus->devices, bus_list) {
+		__pci_disable_link_state(child, PCIE_LINK_STATE_L0S |
+					 PCIE_LINK_STATE_L1 |
+					 PCIE_LINK_STATE_CLKPM,
+					 false, true);
+	}
+}
+
 static int pcie_aspm_set_policy(const char *val, struct kernel_param *kp)
 {
 	int i;
@@ -935,6 +948,7 @@ void pcie_aspm_remove_sysfs_dev_files(struct pci_dev *pdev)
 static int __init pcie_aspm_disable(char *str)
 {
 	if (!strcmp(str, "off")) {
+		aspm_policy = POLICY_DEFAULT;
 		aspm_disabled = 1;
 		aspm_support_enabled = false;
 		printk(KERN_INFO "PCIe ASPM is disabled\n");
@@ -947,16 +961,18 @@ static int __init pcie_aspm_disable(char *str)
 
 __setup("pcie_aspm=", pcie_aspm_disable);
 
-void pcie_clear_aspm(void)
-{
-	if (!aspm_force)
-		aspm_clear_state = 1;
-}
-
 void pcie_no_aspm(void)
 {
-	if (!aspm_force)
+	/*
+	 * Disabling ASPM is intended to prevent the kernel from modifying
+	 * existing hardware state, not to clear existing state. To that end:
+	 * (a) set policy to POLICY_DEFAULT in order to avoid changing state
+	 * (b) prevent userspace from changing policy
+	 */
+	if (!aspm_force) {
+		aspm_policy = POLICY_DEFAULT;
 		aspm_disabled = 1;
+	}
 }
 
 /**
diff --git a/drivers/pci/xen-pcifront.c b/drivers/pci/xen-pcifront.c
index 90832a955991..7cf3d2fcf56a 100644
--- a/drivers/pci/xen-pcifront.c
+++ b/drivers/pci/xen-pcifront.c
@@ -1126,14 +1126,11 @@ static const struct xenbus_device_id xenpci_ids[] = {
 	{""},
 };
 
-static struct xenbus_driver xenbus_pcifront_driver = {
-	.name			= "pcifront",
-	.owner			= THIS_MODULE,
-	.ids			= xenpci_ids,
+static DEFINE_XENBUS_DRIVER(xenpci, "pcifront",
 	.probe			= pcifront_xenbus_probe,
 	.remove			= pcifront_xenbus_remove,
 	.otherend_changed	= pcifront_backend_changed,
-};
+);
 
 static int __init pcifront_init(void)
 {
@@ -1142,12 +1139,12 @@ static int __init pcifront_init(void)
 
 	pci_frontend_registrar(1 /* enable */);
 
-	return xenbus_register_frontend(&xenbus_pcifront_driver);
+	return xenbus_register_frontend(&xenpci_driver);
 }
 
 static void __exit pcifront_cleanup(void)
 {
-	xenbus_unregister_driver(&xenbus_pcifront_driver);
+	xenbus_unregister_driver(&xenpci_driver);
 	pci_frontend_registrar(0 /* disable */);
 }
 module_init(pcifront_init);
diff --git a/drivers/pcmcia/pxa2xx_cm_x255.c b/drivers/pcmcia/pxa2xx_cm_x255.c
index 0b4f946cf13a..31ab6ddf52c9 100644
--- a/drivers/pcmcia/pxa2xx_cm_x255.c
+++ b/drivers/pcmcia/pxa2xx_cm_x255.c
@@ -16,8 +16,6 @@
 #include <linux/gpio.h>
 #include <linux/export.h>
 
-#include <asm/mach-types.h>
-
 #include "soc_common.h"
 
 #define GPIO_PCMCIA_SKTSEL	(54)
@@ -27,15 +25,15 @@
 #define GPIO_PCMCIA_S1_RDYINT	(8)
 #define GPIO_PCMCIA_RESET	(9)
 
-#define PCMCIA_S0_CD_VALID	IRQ_GPIO(GPIO_PCMCIA_S0_CD_VALID)
-#define PCMCIA_S1_CD_VALID	IRQ_GPIO(GPIO_PCMCIA_S1_CD_VALID)
-#define PCMCIA_S0_RDYINT	IRQ_GPIO(GPIO_PCMCIA_S0_RDYINT)
-#define PCMCIA_S1_RDYINT	IRQ_GPIO(GPIO_PCMCIA_S1_RDYINT)
+#define PCMCIA_S0_CD_VALID	gpio_to_irq(GPIO_PCMCIA_S0_CD_VALID)
+#define PCMCIA_S1_CD_VALID	gpio_to_irq(GPIO_PCMCIA_S1_CD_VALID)
+#define PCMCIA_S0_RDYINT	gpio_to_irq(GPIO_PCMCIA_S0_RDYINT)
+#define PCMCIA_S1_RDYINT	gpio_to_irq(GPIO_PCMCIA_S1_RDYINT)
 
 
 static struct pcmcia_irqs irqs[] = {
-	{ 0, PCMCIA_S0_CD_VALID, "PCMCIA0 CD" },
-	{ 1, PCMCIA_S1_CD_VALID, "PCMCIA1 CD" },
+	{ .sock = 0, .str = "PCMCIA0 CD" },
+	{ .sock = 1, .str = "PCMCIA1 CD" },
 };
 
 static int cmx255_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
@@ -46,6 +44,8 @@ static int cmx255_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 	gpio_direction_output(GPIO_PCMCIA_RESET, 0);
 
 	skt->socket.pci_irq = skt->nr == 0 ? PCMCIA_S0_RDYINT : PCMCIA_S1_RDYINT;
+	irqs[0].irq = PCMCIA_S0_CD_VALID;
+	irqs[1].irq = PCMCIA_S1_CD_VALID;
 	ret = soc_pcmcia_request_irqs(skt, irqs, ARRAY_SIZE(irqs));
 	if (!ret)
 		gpio_free(GPIO_PCMCIA_RESET);
diff --git a/drivers/pcmcia/pxa2xx_cm_x270.c b/drivers/pcmcia/pxa2xx_cm_x270.c
index 923f315926ef..3dc7621a0767 100644
--- a/drivers/pcmcia/pxa2xx_cm_x270.c
+++ b/drivers/pcmcia/pxa2xx_cm_x270.c
@@ -16,20 +16,18 @@
 #include <linux/gpio.h>
 #include <linux/export.h>
 
-#include <asm/mach-types.h>
-
 #include "soc_common.h"
 
 #define GPIO_PCMCIA_S0_CD_VALID	(84)
 #define GPIO_PCMCIA_S0_RDYINT	(82)
 #define GPIO_PCMCIA_RESET	(53)
 
-#define PCMCIA_S0_CD_VALID	IRQ_GPIO(GPIO_PCMCIA_S0_CD_VALID)
-#define PCMCIA_S0_RDYINT	IRQ_GPIO(GPIO_PCMCIA_S0_RDYINT)
+#define PCMCIA_S0_CD_VALID	gpio_to_irq(GPIO_PCMCIA_S0_CD_VALID)
+#define PCMCIA_S0_RDYINT	gpio_to_irq(GPIO_PCMCIA_S0_RDYINT)
 
 
 static struct pcmcia_irqs irqs[] = {
-	{ 0, PCMCIA_S0_CD_VALID, "PCMCIA0 CD" },
+	{ .sock = 0, .str = "PCMCIA0 CD" },
 };
 
 static int cmx270_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
@@ -40,6 +38,7 @@ static int cmx270_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 	gpio_direction_output(GPIO_PCMCIA_RESET, 0);
 
 	skt->socket.pci_irq = PCMCIA_S0_RDYINT;
+	irqs[0].irq = PCMCIA_S0_CD_VALID;
 	ret = soc_pcmcia_request_irqs(skt, irqs, ARRAY_SIZE(irqs));
 	if (!ret)
 		gpio_free(GPIO_PCMCIA_RESET);
diff --git a/drivers/pcmcia/pxa2xx_e740.c b/drivers/pcmcia/pxa2xx_e740.c
index 8bfbd4dca131..17cd2ce7428f 100644
--- a/drivers/pcmcia/pxa2xx_e740.c
+++ b/drivers/pcmcia/pxa2xx_e740.c
@@ -26,20 +26,23 @@
 static struct pcmcia_irqs cd_irqs[] = {
 	{
 		.sock = 0,
-		.irq  = IRQ_GPIO(GPIO_E740_PCMCIA_CD0),
 		.str  = "CF card detect"
 	},
 	{
 		.sock = 1,
-		.irq  = IRQ_GPIO(GPIO_E740_PCMCIA_CD1),
 		.str  = "Wifi switch"
 	},
 };
 
 static int e740_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 {
-	skt->socket.pci_irq = skt->nr == 0 ? IRQ_GPIO(GPIO_E740_PCMCIA_RDY0) :
-				IRQ_GPIO(GPIO_E740_PCMCIA_RDY1);
+	if (skt->nr == 0)
+		skt->socket.pci_irq = gpio_to_irq(GPIO_E740_PCMCIA_RDY0);
+	else
+		skt->socket.pci_irq = gpio_to_irq(GPIO_E740_PCMCIA_RDY1);
+
+	cd_irqs[0].irq = gpio_to_irq(GPIO_E740_PCMCIA_CD0);
+	cd_irqs[1].irq = gpio_to_irq(GPIO_E740_PCMCIA_CD1);
 
 	return soc_pcmcia_request_irqs(skt, &cd_irqs[skt->nr], 1);
 }
diff --git a/drivers/pcmcia/pxa2xx_palmld.c b/drivers/pcmcia/pxa2xx_palmld.c
index d589ad1dcd4c..6a8e011a8c13 100644
--- a/drivers/pcmcia/pxa2xx_palmld.c
+++ b/drivers/pcmcia/pxa2xx_palmld.c
@@ -33,7 +33,7 @@ static int palmld_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 	ret = gpio_request_array(palmld_pcmcia_gpios,
 				ARRAY_SIZE(palmld_pcmcia_gpios));
 
-	skt->socket.pci_irq = IRQ_GPIO(GPIO_NR_PALMLD_PCMCIA_READY);
+	skt->socket.pci_irq = gpio_to_irq(GPIO_NR_PALMLD_PCMCIA_READY);
 
 	return ret;
 }
diff --git a/drivers/pcmcia/pxa2xx_palmtc.c b/drivers/pcmcia/pxa2xx_palmtc.c
index 9c6a04b2f71b..9e38de769ba3 100644
--- a/drivers/pcmcia/pxa2xx_palmtc.c
+++ b/drivers/pcmcia/pxa2xx_palmtc.c
@@ -37,7 +37,7 @@ static int palmtc_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 	ret = gpio_request_array(palmtc_pcmcia_gpios,
 				ARRAY_SIZE(palmtc_pcmcia_gpios));
 
-	skt->socket.pci_irq = IRQ_GPIO(GPIO_NR_PALMTC_PCMCIA_READY);
+	skt->socket.pci_irq = gpio_to_irq(GPIO_NR_PALMTC_PCMCIA_READY);
 
 	return ret;
 }
diff --git a/drivers/pcmcia/pxa2xx_stargate2.c b/drivers/pcmcia/pxa2xx_stargate2.c
index 939622251dfb..6c2366b74a35 100644
--- a/drivers/pcmcia/pxa2xx_stargate2.c
+++ b/drivers/pcmcia/pxa2xx_stargate2.c
@@ -34,7 +34,7 @@
 #define SG2_S0_GPIO_READY	81
 
 static struct pcmcia_irqs irqs[] = {
-	{ 0, IRQ_GPIO(SG2_S0_GPIO_DETECT), "PCMCIA0 CD" },
+	{.sock = 0, .str = "PCMCIA0 CD" },
 };
 
 static struct gpio sg2_pcmcia_gpios[] = {
@@ -44,7 +44,9 @@ static struct gpio sg2_pcmcia_gpios[] = {
 
 static int sg2_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 {
-	skt->socket.pci_irq = IRQ_GPIO(SG2_S0_GPIO_READY);
+	skt->socket.pci_irq = gpio_to_irq(SG2_S0_GPIO_READY);
+	irqs[0].irq = gpio_to_irq(SG2_S0_GPIO_DETECT);
+
 	return soc_pcmcia_request_irqs(skt, irqs, ARRAY_SIZE(irqs));
 }
 
diff --git a/drivers/pcmcia/pxa2xx_trizeps4.c b/drivers/pcmcia/pxa2xx_trizeps4.c
index 57ddb969d888..7c33f898135a 100644
--- a/drivers/pcmcia/pxa2xx_trizeps4.c
+++ b/drivers/pcmcia/pxa2xx_trizeps4.c
@@ -30,7 +30,7 @@
 extern void board_pcmcia_power(int power);
 
 static struct pcmcia_irqs irqs[] = {
-	{ 0, IRQ_GPIO(GPIO_PCD), "cs0_cd" }
+	{ .sock = 0, .str = "cs0_cd" }
 	/* on other baseboards we can have more inputs */
 };
 
@@ -53,7 +53,8 @@ static int trizeps_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 			gpio_free(GPIO_PRDY);
 			return -EINVAL;
 		}
-		skt->socket.pci_irq = IRQ_GPIO(GPIO_PRDY);
+		skt->socket.pci_irq = gpio_to_irq(GPIO_PRDY);
+		irqs[0].irq = gpio_to_irq(GPIO_PCD);
 		break;
 	default:
 		break;
diff --git a/drivers/pcmcia/pxa2xx_vpac270.c b/drivers/pcmcia/pxa2xx_vpac270.c
index 66ab92cf3105..61b17d235dbe 100644
--- a/drivers/pcmcia/pxa2xx_vpac270.c
+++ b/drivers/pcmcia/pxa2xx_vpac270.c
@@ -38,12 +38,10 @@ static struct gpio vpac270_cf_gpios[] = {
 static struct pcmcia_irqs cd_irqs[] = {
 	{
 		.sock = 0,
-		.irq  = IRQ_GPIO(GPIO84_VPAC270_PCMCIA_CD),
 		.str  = "PCMCIA CD"
 	},
 	{
 		.sock = 1,
-		.irq  = IRQ_GPIO(GPIO17_VPAC270_CF_CD),
 		.str  = "CF CD"
 	},
 };
@@ -57,6 +55,7 @@ static int vpac270_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 				ARRAY_SIZE(vpac270_pcmcia_gpios));
 
 		skt->socket.pci_irq = gpio_to_irq(GPIO35_VPAC270_PCMCIA_RDY);
+		cd_irqs[0].irq = gpio_to_irq(GPIO84_VPAC270_PCMCIA_CD);
 
 		if (!ret)
 			ret = soc_pcmcia_request_irqs(skt, &cd_irqs[0], 1);
@@ -65,6 +64,7 @@ static int vpac270_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 				ARRAY_SIZE(vpac270_cf_gpios));
 
 		skt->socket.pci_irq = gpio_to_irq(GPIO12_VPAC270_CF_RDY);
+		cd_irqs[1].irq = gpio_to_irq(GPIO17_VPAC270_CF_CD);
 
 		if (!ret)
 			ret = soc_pcmcia_request_irqs(skt, &cd_irqs[1], 1);
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index e17e2f8001d2..afaf88558125 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -12,7 +12,10 @@ menu "Pin controllers"
 	depends on PINCTRL
 
 config PINMUX
-	bool "Support pinmux controllers"
+	bool "Support pin multiplexing controllers"
+
+config PINCONF
+	bool "Support pin configuration controllers"
 
 config DEBUG_PINCTRL
 	bool "Debug PINCTRL calls"
@@ -20,16 +23,25 @@ config DEBUG_PINCTRL
 	help
 	  Say Y here to add some extra checks and diagnostics to PINCTRL calls.
 
-config PINMUX_SIRF
-	bool "CSR SiRFprimaII pinmux driver"
+config PINCTRL_SIRF
+	bool "CSR SiRFprimaII pin controller driver"
 	depends on ARCH_PRIMA2
 	select PINMUX
 
-config PINMUX_U300
-	bool "U300 pinmux driver"
+config PINCTRL_U300
+	bool "U300 pin controller driver"
 	depends on ARCH_U300
 	select PINMUX
 
+config PINCTRL_COH901
+	bool "ST-Ericsson U300 COH 901 335/571 GPIO"
+	depends on GPIOLIB && ARCH_U300 && PINMUX_U300
+	help
+	  Say yes here to support GPIO interface on ST-Ericsson U300.
+	  The names of the two IP block variants supported are
+	  COH 901 335 and COH 901 571/3. They contain 3, 5 or 7
+	  ports of 8 GPIO pins each.
+
 endmenu
 
 endif
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index bdc548a6b7e9..827601cc68f6 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -1,8 +1,10 @@
 # generic pinmux support
 
-ccflags-$(CONFIG_DEBUG_PINMUX)	+= -DDEBUG
+ccflags-$(CONFIG_DEBUG_PINCTRL)	+= -DDEBUG
 
 obj-$(CONFIG_PINCTRL)		+= core.o
 obj-$(CONFIG_PINMUX)		+= pinmux.o
-obj-$(CONFIG_PINMUX_SIRF)	+= pinmux-sirf.o
-obj-$(CONFIG_PINMUX_U300)	+= pinmux-u300.o
+obj-$(CONFIG_PINCONF)		+= pinconf.o
+obj-$(CONFIG_PINCTRL_SIRF)	+= pinctrl-sirf.o
+obj-$(CONFIG_PINCTRL_U300)	+= pinctrl-u300.o
+obj-$(CONFIG_PINCTRL_COH901)	+= pinctrl-coh901.o
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index eadef9e191ea..569bdb3ef104 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -28,17 +28,12 @@
 #include <linux/pinctrl/machine.h>
 #include "core.h"
 #include "pinmux.h"
+#include "pinconf.h"
 
 /* Global list of pin control devices */
 static DEFINE_MUTEX(pinctrldev_list_mutex);
 static LIST_HEAD(pinctrldev_list);
 
-static void pinctrl_dev_release(struct device *dev)
-{
-	struct pinctrl_dev *pctldev = dev_get_drvdata(dev);
-	kfree(pctldev);
-}
-
 const char *pinctrl_dev_get_name(struct pinctrl_dev *pctldev)
 {
 	/* We're not allowed to register devices without name */
@@ -70,14 +65,14 @@ struct pinctrl_dev *get_pinctrl_dev_from_dev(struct device *dev,
 
 	mutex_lock(&pinctrldev_list_mutex);
 	list_for_each_entry(pctldev, &pinctrldev_list, node) {
-		if (dev &&  &pctldev->dev == dev) {
+		if (dev && pctldev->dev == dev) {
 			/* Matched on device pointer */
 			found = true;
 			break;
 		}
 
 		if (devname &&
-		    !strcmp(dev_name(&pctldev->dev), devname)) {
+		    !strcmp(dev_name(pctldev->dev), devname)) {
 			/* Matched on device name */
 			found = true;
 			break;
@@ -88,7 +83,7 @@ struct pinctrl_dev *get_pinctrl_dev_from_dev(struct device *dev,
 	return found ? pctldev : NULL;
 }
 
-struct pin_desc *pin_desc_get(struct pinctrl_dev *pctldev, int pin)
+struct pin_desc *pin_desc_get(struct pinctrl_dev *pctldev, unsigned int pin)
 {
 	struct pin_desc *pindesc;
 	unsigned long flags;
@@ -101,6 +96,31 @@ struct pin_desc *pin_desc_get(struct pinctrl_dev *pctldev, int pin)
 }
 
 /**
+ * pin_get_from_name() - look up a pin number from a name
+ * @pctldev: the pin control device to lookup the pin on
+ * @name: the name of the pin to look up
+ */
+int pin_get_from_name(struct pinctrl_dev *pctldev, const char *name)
+{
+	unsigned i, pin;
+
+	/* The pin number can be retrived from the pin controller descriptor */
+	for (i = 0; i < pctldev->desc->npins; i++) {
+		struct pin_desc *desc;
+
+		pin = pctldev->desc->pins[i].number;
+		desc = pin_desc_get(pctldev, pin);
+		/* Pin space may be sparse */
+		if (desc == NULL)
+			continue;
+		if (desc->name && !strcmp(name, desc->name))
+			return pin;
+	}
+
+	return -EINVAL;
+}
+
+/**
  * pin_is_valid() - check if pin exists on controller
  * @pctldev: the pin control device to check the pin on
  * @pin: pin to check, use the local pin controller index number
@@ -139,6 +159,8 @@ static void pinctrl_free_pindescs(struct pinctrl_dev *pctldev,
 		if (pindesc != NULL) {
 			radix_tree_delete(&pctldev->pin_desc_tree,
 					  pins[i].number);
+			if (pindesc->dynamic_name)
+				kfree(pindesc->name);
 		}
 		kfree(pindesc);
 	}
@@ -160,19 +182,27 @@ static int pinctrl_register_one_pin(struct pinctrl_dev *pctldev,
 	pindesc = kzalloc(sizeof(*pindesc), GFP_KERNEL);
 	if (pindesc == NULL)
 		return -ENOMEM;
+
 	spin_lock_init(&pindesc->lock);
 
 	/* Set owner */
 	pindesc->pctldev = pctldev;
 
 	/* Copy basic pin info */
-	pindesc->name = name;
+	if (pindesc->name) {
+		pindesc->name = name;
+	} else {
+		pindesc->name = kasprintf(GFP_KERNEL, "PIN%u", number);
+		if (pindesc->name == NULL)
+			return -ENOMEM;
+		pindesc->dynamic_name = true;
+	}
 
 	spin_lock(&pctldev->pin_desc_tree_lock);
 	radix_tree_insert(&pctldev->pin_desc_tree, number, pindesc);
 	spin_unlock(&pctldev->pin_desc_tree_lock);
 	pr_debug("registered pin %d (%s) on %s\n",
-		 number, name ? name : "(unnamed)", pctldev->desc->name);
+		 number, pindesc->name, pctldev->desc->name);
 	return 0;
 }
 
@@ -284,21 +314,52 @@ void pinctrl_remove_gpio_range(struct pinctrl_dev *pctldev,
 	mutex_unlock(&pctldev->gpio_ranges_lock);
 }
 
+/**
+ * pinctrl_get_group_selector() - returns the group selector for a group
+ * @pctldev: the pin controller handling the group
+ * @pin_group: the pin group to look up
+ */
+int pinctrl_get_group_selector(struct pinctrl_dev *pctldev,
+			       const char *pin_group)
+{
+	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
+	unsigned group_selector = 0;
+
+	while (pctlops->list_groups(pctldev, group_selector) >= 0) {
+		const char *gname = pctlops->get_group_name(pctldev,
+							    group_selector);
+		if (!strcmp(gname, pin_group)) {
+			dev_dbg(pctldev->dev,
+				"found group selector %u for %s\n",
+				group_selector,
+				pin_group);
+			return group_selector;
+		}
+
+		group_selector++;
+	}
+
+	dev_err(pctldev->dev, "does not have pin group %s\n",
+		pin_group);
+
+	return -EINVAL;
+}
+
 #ifdef CONFIG_DEBUG_FS
 
 static int pinctrl_pins_show(struct seq_file *s, void *what)
 {
 	struct pinctrl_dev *pctldev = s->private;
 	const struct pinctrl_ops *ops = pctldev->desc->pctlops;
-	unsigned pin;
+	unsigned i, pin;
 
 	seq_printf(s, "registered pins: %d\n", pctldev->desc->npins);
-	seq_printf(s, "max pin number: %d\n", pctldev->desc->maxpin);
 
-	/* The highest pin number need to be included in the loop, thus <= */
-	for (pin = 0; pin <= pctldev->desc->maxpin; pin++) {
+	/* The pin number can be retrived from the pin controller descriptor */
+	for (i = 0; i < pctldev->desc->npins; i++) {
 		struct pin_desc *desc;
 
+		pin = pctldev->desc->pins[i].number;
 		desc = pin_desc_get(pctldev, pin);
 		/* Pin space may be sparse */
 		if (desc == NULL)
@@ -363,8 +424,11 @@ static int pinctrl_gpioranges_show(struct seq_file *s, void *what)
 	/* Loop over the ranges */
 	mutex_lock(&pctldev->gpio_ranges_lock);
 	list_for_each_entry(range, &pctldev->gpio_ranges, node) {
-		seq_printf(s, "%u: %s [%u - %u]\n", range->id, range->name,
-			   range->base, (range->base + range->npins - 1));
+		seq_printf(s, "%u: %s GPIOS [%u - %u] PINS [%u - %u]\n",
+			   range->id, range->name,
+			   range->base, (range->base + range->npins - 1),
+			   range->pin_base,
+			   (range->pin_base + range->npins - 1));
 	}
 	mutex_unlock(&pctldev->gpio_ranges_lock);
 
@@ -375,11 +439,15 @@ static int pinctrl_devices_show(struct seq_file *s, void *what)
 {
 	struct pinctrl_dev *pctldev;
 
-	seq_puts(s, "name [pinmux]\n");
+	seq_puts(s, "name [pinmux] [pinconf]\n");
 	mutex_lock(&pinctrldev_list_mutex);
 	list_for_each_entry(pctldev, &pinctrldev_list, node) {
 		seq_printf(s, "%s ", pctldev->desc->name);
 		if (pctldev->desc->pmxops)
+			seq_puts(s, "yes ");
+		else
+			seq_puts(s, "no ");
+		if (pctldev->desc->confops)
 			seq_puts(s, "yes");
 		else
 			seq_puts(s, "no");
@@ -444,11 +512,11 @@ static void pinctrl_init_device_debugfs(struct pinctrl_dev *pctldev)
 {
 	static struct dentry *device_root;
 
-	device_root = debugfs_create_dir(dev_name(&pctldev->dev),
+	device_root = debugfs_create_dir(dev_name(pctldev->dev),
 					 debugfs_root);
 	if (IS_ERR(device_root) || !device_root) {
 		pr_warn("failed to create debugfs directory for %s\n",
-			dev_name(&pctldev->dev));
+			dev_name(pctldev->dev));
 		return;
 	}
 	debugfs_create_file("pins", S_IFREG | S_IRUGO,
@@ -458,6 +526,7 @@ static void pinctrl_init_device_debugfs(struct pinctrl_dev *pctldev)
 	debugfs_create_file("gpio-ranges", S_IFREG | S_IRUGO,
 			    device_root, pctldev, &pinctrl_gpioranges_ops);
 	pinmux_init_device_debugfs(device_root, pctldev);
+	pinconf_init_device_debugfs(device_root, pctldev);
 }
 
 static void pinctrl_init_debugfs(void)
@@ -495,7 +564,6 @@ static void pinctrl_init_debugfs(void)
 struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 				    struct device *dev, void *driver_data)
 {
-	static atomic_t pinmux_no = ATOMIC_INIT(0);
 	struct pinctrl_dev *pctldev;
 	int ret;
 
@@ -514,6 +582,16 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 		}
 	}
 
+	/* If we're implementing pinconfig, check the ops for sanity */
+	if (pctldesc->confops) {
+		ret = pinconf_check_ops(pctldesc->confops);
+		if (ret) {
+			pr_err("%s pin config ops lacks necessary functions\n",
+			       pctldesc->name);
+			return NULL;
+		}
+	}
+
 	pctldev = kzalloc(sizeof(struct pinctrl_dev), GFP_KERNEL);
 	if (pctldev == NULL)
 		return NULL;
@@ -526,18 +604,7 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 	spin_lock_init(&pctldev->pin_desc_tree_lock);
 	INIT_LIST_HEAD(&pctldev->gpio_ranges);
 	mutex_init(&pctldev->gpio_ranges_lock);
-
-	/* Register device */
-	pctldev->dev.parent = dev;
-	dev_set_name(&pctldev->dev, "pinctrl.%d",
-		     atomic_inc_return(&pinmux_no) - 1);
-	pctldev->dev.release = pinctrl_dev_release;
-	ret = device_register(&pctldev->dev);
-	if (ret != 0) {
-		pr_err("error in device registration\n");
-		goto out_reg_dev_err;
-	}
-	dev_set_drvdata(&pctldev->dev, pctldev);
+	pctldev->dev = dev;
 
 	/* Register all the pins */
 	pr_debug("try to register %d pins on %s...\n",
@@ -547,7 +614,7 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 		pr_err("error during pin registration\n");
 		pinctrl_free_pindescs(pctldev, pctldesc->pins,
 				      pctldesc->npins);
-		goto out_reg_pins_err;
+		goto out_err;
 	}
 
 	pinctrl_init_device_debugfs(pctldev);
@@ -557,10 +624,8 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 	pinmux_hog_maps(pctldev);
 	return pctldev;
 
-out_reg_pins_err:
-	device_del(&pctldev->dev);
-out_reg_dev_err:
-	put_device(&pctldev->dev);
+out_err:
+	kfree(pctldev);
 	return NULL;
 }
 EXPORT_SYMBOL_GPL(pinctrl_register);
@@ -584,7 +649,7 @@ void pinctrl_unregister(struct pinctrl_dev *pctldev)
 	/* Destroy descriptor tree */
 	pinctrl_free_pindescs(pctldev, pctldev->desc->pins,
 			      pctldev->desc->npins);
-	device_unregister(&pctldev->dev);
+	kfree(pctldev);
 }
 EXPORT_SYMBOL_GPL(pinctrl_unregister);
 
diff --git a/drivers/pinctrl/core.h b/drivers/pinctrl/core.h
index 472fa1341cc0..177a3310547f 100644
--- a/drivers/pinctrl/core.h
+++ b/drivers/pinctrl/core.h
@@ -9,6 +9,10 @@
  * License terms: GNU General Public License (GPL) version 2
  */
 
+#include <linux/pinctrl/pinconf.h>
+
+struct pinctrl_gpio_range;
+
 /**
  * struct pinctrl_dev - pin control class device
  * @node: node to include this pin controller in the global pin controller list
@@ -34,7 +38,7 @@ struct pinctrl_dev {
 	spinlock_t pin_desc_tree_lock;
 	struct list_head gpio_ranges;
 	struct mutex gpio_ranges_lock;
-	struct device dev;
+	struct device *dev;
 	struct module *owner;
 	void *driver_data;
 #ifdef CONFIG_PINMUX
@@ -48,6 +52,7 @@ struct pinctrl_dev {
  * @pctldev: corresponding pin control device
  * @name: a name for the pin, e.g. the name of the pin/pad/finger on a
  *	datasheet or such
+ * @dynamic_name: if the name of this pin was dynamically allocated
  * @lock: a lock to protect the descriptor structure
  * @mux_requested: whether the pin is already requested by pinmux or not
  * @mux_function: a named muxing function for the pin that will be passed to
@@ -56,6 +61,7 @@ struct pinctrl_dev {
 struct pin_desc {
 	struct pinctrl_dev *pctldev;
 	const char *name;
+	bool dynamic_name;
 	spinlock_t lock;
 	/* These fields only added when supporting pinmux drivers */
 #ifdef CONFIG_PINMUX
@@ -65,7 +71,10 @@ struct pin_desc {
 
 struct pinctrl_dev *get_pinctrl_dev_from_dev(struct device *dev,
 					     const char *dev_name);
-struct pin_desc *pin_desc_get(struct pinctrl_dev *pctldev, int pin);
+struct pin_desc *pin_desc_get(struct pinctrl_dev *pctldev, unsigned int pin);
+int pin_get_from_name(struct pinctrl_dev *pctldev, const char *name);
 int pinctrl_get_device_gpio_range(unsigned gpio,
 				  struct pinctrl_dev **outdev,
 				  struct pinctrl_gpio_range **outrange);
+int pinctrl_get_group_selector(struct pinctrl_dev *pctldev,
+			       const char *pin_group);
diff --git a/drivers/pinctrl/pinconf.c b/drivers/pinctrl/pinconf.c
new file mode 100644
index 000000000000..1259872b0a1d
--- /dev/null
+++ b/drivers/pinctrl/pinconf.c
@@ -0,0 +1,326 @@
+/*
+ * Core driver for the pin config portions of the pin control subsystem
+ *
+ * Copyright (C) 2011 ST-Ericsson SA
+ * Written on behalf of Linaro for ST-Ericsson
+ *
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#define pr_fmt(fmt) "pinconfig core: " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/pinctrl/machine.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinconf.h>
+#include "core.h"
+#include "pinconf.h"
+
+int pin_config_get_for_pin(struct pinctrl_dev *pctldev, unsigned pin,
+			   unsigned long *config)
+{
+	const struct pinconf_ops *ops = pctldev->desc->confops;
+
+	if (!ops || !ops->pin_config_get) {
+		dev_err(pctldev->dev, "cannot get pin configuration, missing "
+			"pin_config_get() function in driver\n");
+		return -EINVAL;
+	}
+
+	return ops->pin_config_get(pctldev, pin, config);
+}
+
+/**
+ * pin_config_get() - get the configuration of a single pin parameter
+ * @dev_name: name of the pin controller device for this pin
+ * @name: name of the pin to get the config for
+ * @config: the config pointed to by this argument will be filled in with the
+ *	current pin state, it can be used directly by drivers as a numeral, or
+ *	it can be dereferenced to any struct.
+ */
+int pin_config_get(const char *dev_name, const char *name,
+			  unsigned long *config)
+{
+	struct pinctrl_dev *pctldev;
+	int pin;
+
+	pctldev = get_pinctrl_dev_from_dev(NULL, dev_name);
+	if (!pctldev)
+		return -EINVAL;
+
+	pin = pin_get_from_name(pctldev, name);
+	if (pin < 0)
+		return pin;
+
+	return pin_config_get_for_pin(pctldev, pin, config);
+}
+EXPORT_SYMBOL(pin_config_get);
+
+int pin_config_set_for_pin(struct pinctrl_dev *pctldev, unsigned pin,
+			   unsigned long config)
+{
+	const struct pinconf_ops *ops = pctldev->desc->confops;
+	int ret;
+
+	if (!ops || !ops->pin_config_set) {
+		dev_err(pctldev->dev, "cannot configure pin, missing "
+			"config function in driver\n");
+		return -EINVAL;
+	}
+
+	ret = ops->pin_config_set(pctldev, pin, config);
+	if (ret) {
+		dev_err(pctldev->dev,
+			"unable to set pin configuration on pin %d\n", pin);
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * pin_config_set() - set the configuration of a single pin parameter
+ * @dev_name: name of pin controller device for this pin
+ * @name: name of the pin to set the config for
+ * @config: the config in this argument will contain the desired pin state, it
+ *	can be used directly by drivers as a numeral, or it can be dereferenced
+ *	to any struct.
+ */
+int pin_config_set(const char *dev_name, const char *name,
+		   unsigned long config)
+{
+	struct pinctrl_dev *pctldev;
+	int pin;
+
+	pctldev = get_pinctrl_dev_from_dev(NULL, dev_name);
+	if (!pctldev)
+		return -EINVAL;
+
+	pin = pin_get_from_name(pctldev, name);
+	if (pin < 0)
+		return pin;
+
+	return pin_config_set_for_pin(pctldev, pin, config);
+}
+EXPORT_SYMBOL(pin_config_set);
+
+int pin_config_group_get(const char *dev_name, const char *pin_group,
+			 unsigned long *config)
+{
+	struct pinctrl_dev *pctldev;
+	const struct pinconf_ops *ops;
+	int selector;
+
+	pctldev = get_pinctrl_dev_from_dev(NULL, dev_name);
+	if (!pctldev)
+		return -EINVAL;
+	ops = pctldev->desc->confops;
+
+	if (!ops || !ops->pin_config_group_get) {
+		dev_err(pctldev->dev, "cannot get configuration for pin "
+			"group, missing group config get function in "
+			"driver\n");
+		return -EINVAL;
+	}
+
+	selector = pinctrl_get_group_selector(pctldev, pin_group);
+	if (selector < 0)
+		return selector;
+
+	return ops->pin_config_group_get(pctldev, selector, config);
+}
+EXPORT_SYMBOL(pin_config_group_get);
+
+
+int pin_config_group_set(const char *dev_name, const char *pin_group,
+			 unsigned long config)
+{
+	struct pinctrl_dev *pctldev;
+	const struct pinconf_ops *ops;
+	const struct pinctrl_ops *pctlops;
+	int selector;
+	const unsigned *pins;
+	unsigned num_pins;
+	int ret;
+	int i;
+
+	pctldev = get_pinctrl_dev_from_dev(NULL, dev_name);
+	if (!pctldev)
+		return -EINVAL;
+	ops = pctldev->desc->confops;
+	pctlops = pctldev->desc->pctlops;
+
+	if (!ops || (!ops->pin_config_group_set && !ops->pin_config_set)) {
+		dev_err(pctldev->dev, "cannot configure pin group, missing "
+			"config function in driver\n");
+		return -EINVAL;
+	}
+
+	selector = pinctrl_get_group_selector(pctldev, pin_group);
+	if (selector < 0)
+		return selector;
+
+	ret = pctlops->get_group_pins(pctldev, selector, &pins, &num_pins);
+	if (ret) {
+		dev_err(pctldev->dev, "cannot configure pin group, error "
+			"getting pins\n");
+		return ret;
+	}
+
+	/*
+	 * If the pin controller supports handling entire groups we use that
+	 * capability.
+	 */
+	if (ops->pin_config_group_set) {
+		ret = ops->pin_config_group_set(pctldev, selector, config);
+		/*
+		 * If the pin controller prefer that a certain group be handled
+		 * pin-by-pin as well, it returns -EAGAIN.
+		 */
+		if (ret != -EAGAIN)
+			return ret;
+	}
+
+	/*
+	 * If the controller cannot handle entire groups, we configure each pin
+	 * individually.
+	 */
+	if (!ops->pin_config_set)
+		return 0;
+
+	for (i = 0; i < num_pins; i++) {
+		ret = ops->pin_config_set(pctldev, pins[i], config);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(pin_config_group_set);
+
+int pinconf_check_ops(const struct pinconf_ops *ops)
+{
+	/* We must be able to read out pin status */
+	if (!ops->pin_config_get && !ops->pin_config_group_get)
+		return -EINVAL;
+	/* We have to be able to config the pins in SOME way */
+	if (!ops->pin_config_set && !ops->pin_config_group_set)
+		return -EINVAL;
+	return 0;
+}
+
+#ifdef CONFIG_DEBUG_FS
+
+static void pinconf_dump_pin(struct pinctrl_dev *pctldev,
+			     struct seq_file *s, int pin)
+{
+	const struct pinconf_ops *ops = pctldev->desc->confops;
+
+	if (ops && ops->pin_config_dbg_show)
+		ops->pin_config_dbg_show(pctldev, s, pin);
+}
+
+static int pinconf_pins_show(struct seq_file *s, void *what)
+{
+	struct pinctrl_dev *pctldev = s->private;
+	unsigned i, pin;
+
+	seq_puts(s, "Pin config settings per pin\n");
+	seq_puts(s, "Format: pin (name): pinmux setting array\n");
+
+	/* The pin number can be retrived from the pin controller descriptor */
+	for (i = 0; pin < pctldev->desc->npins; i++) {
+		struct pin_desc *desc;
+
+		pin = pctldev->desc->pins[i].number;
+		desc = pin_desc_get(pctldev, pin);
+		/* Skip if we cannot search the pin */
+		if (desc == NULL)
+			continue;
+
+		seq_printf(s, "pin %d (%s):", pin,
+			   desc->name ? desc->name : "unnamed");
+
+		pinconf_dump_pin(pctldev, s, pin);
+
+		seq_printf(s, "\n");
+	}
+
+	return 0;
+}
+
+static void pinconf_dump_group(struct pinctrl_dev *pctldev,
+			       struct seq_file *s, unsigned selector,
+			       const char *gname)
+{
+	const struct pinconf_ops *ops = pctldev->desc->confops;
+
+	if (ops && ops->pin_config_group_dbg_show)
+		ops->pin_config_group_dbg_show(pctldev, s, selector);
+}
+
+static int pinconf_groups_show(struct seq_file *s, void *what)
+{
+	struct pinctrl_dev *pctldev = s->private;
+	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
+	const struct pinconf_ops *ops = pctldev->desc->confops;
+	unsigned selector = 0;
+
+	if (!ops || !ops->pin_config_group_get)
+		return 0;
+
+	seq_puts(s, "Pin config settings per pin group\n");
+	seq_puts(s, "Format: group (name): pinmux setting array\n");
+
+	while (pctlops->list_groups(pctldev, selector) >= 0) {
+		const char *gname = pctlops->get_group_name(pctldev, selector);
+
+		seq_printf(s, "%u (%s):", selector, gname);
+		pinconf_dump_group(pctldev, s, selector, gname);
+		selector++;
+	}
+
+	return 0;
+}
+
+static int pinconf_pins_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, pinconf_pins_show, inode->i_private);
+}
+
+static int pinconf_groups_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, pinconf_groups_show, inode->i_private);
+}
+
+static const struct file_operations pinconf_pins_ops = {
+	.open		= pinconf_pins_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const struct file_operations pinconf_groups_ops = {
+	.open		= pinconf_groups_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+void pinconf_init_device_debugfs(struct dentry *devroot,
+			 struct pinctrl_dev *pctldev)
+{
+	debugfs_create_file("pinconf-pins", S_IFREG | S_IRUGO,
+			    devroot, pctldev, &pinconf_pins_ops);
+	debugfs_create_file("pinconf-groups", S_IFREG | S_IRUGO,
+			    devroot, pctldev, &pinconf_groups_ops);
+}
+
+#endif
diff --git a/drivers/pinctrl/pinconf.h b/drivers/pinctrl/pinconf.h
new file mode 100644
index 000000000000..e7dc6165032a
--- /dev/null
+++ b/drivers/pinctrl/pinconf.h
@@ -0,0 +1,36 @@
+/*
+ * Internal interface between the core pin control system and the
+ * pin config portions
+ *
+ * Copyright (C) 2011 ST-Ericsson SA
+ * Written on behalf of Linaro for ST-Ericsson
+ * Based on bits of regulator core, gpio core and clk core
+ *
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifdef CONFIG_PINCONF
+
+int pinconf_check_ops(const struct pinconf_ops *ops);
+void pinconf_init_device_debugfs(struct dentry *devroot,
+				 struct pinctrl_dev *pctldev);
+int pin_config_get_for_pin(struct pinctrl_dev *pctldev, unsigned pin,
+			   unsigned long *config);
+int pin_config_set_for_pin(struct pinctrl_dev *pctldev, unsigned pin,
+			   unsigned long config);
+
+#else
+
+static inline int pinconf_check_ops(const struct pinconf_ops *ops)
+{
+	return 0;
+}
+
+static inline void pinconf_init_device_debugfs(struct dentry *devroot,
+					       struct pinctrl_dev *pctldev)
+{
+}
+
+#endif
diff --git a/drivers/gpio/gpio-u300.c b/drivers/pinctrl/pinctrl-coh901.c
index 4035778852b0..69fb7072a23e 100644
--- a/drivers/gpio/gpio-u300.c
+++ b/drivers/pinctrl/pinctrl-coh901.c
@@ -22,6 +22,7 @@
 #include <linux/gpio.h>
 #include <linux/list.h>
 #include <linux/slab.h>
+#include <linux/pinctrl/pinmux.h>
 #include <mach/gpio-u300.h>
 
 /*
@@ -351,6 +352,24 @@ static inline struct u300_gpio *to_u300_gpio(struct gpio_chip *chip)
 	return container_of(chip, struct u300_gpio, chip);
 }
 
+static int u300_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	/*
+	 * Map back to global GPIO space and request muxing, the direction
+	 * parameter does not matter for this controller.
+	 */
+	int gpio = chip->base + offset;
+
+	return pinmux_request_gpio(gpio);
+}
+
+static void u300_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	int gpio = chip->base + offset;
+
+	pinmux_free_gpio(gpio);
+}
+
 static int u300_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct u300_gpio *gpio = to_u300_gpio(chip);
@@ -483,6 +502,8 @@ static int u300_gpio_config(struct gpio_chip *chip, unsigned offset,
 static struct gpio_chip u300_gpio_chip = {
 	.label			= "u300-gpio-chip",
 	.owner			= THIS_MODULE,
+	.request		= u300_gpio_request,
+	.free			= u300_gpio_free,
 	.get			= u300_gpio_get,
 	.set			= u300_gpio_set,
 	.direction_input	= u300_gpio_direction_input,
diff --git a/drivers/pinctrl/pinmux-sirf.c b/drivers/pinctrl/pinctrl-sirf.c
index d76cae620956..6b3534cc051a 100644
--- a/drivers/pinctrl/pinmux-sirf.c
+++ b/drivers/pinctrl/pinctrl-sirf.c
@@ -463,7 +463,7 @@ static const struct sirfsoc_padmux spi1_padmux = {
 	.funcval = BIT(8),
 };
 
-static const unsigned spi1_pins[] = { 33, 34, 35, 36 };
+static const unsigned spi1_pins[] = { 43, 44, 45, 46 };
 
 static const struct sirfsoc_muxmask sdmmc1_muxmask[] = {
 	{
@@ -1067,7 +1067,7 @@ static int sirfsoc_pinmux_request_gpio(struct pinctrl_dev *pmxdev,
 	spmx = pinctrl_dev_get_drvdata(pmxdev);
 
 	muxval = readl(spmx->gpio_virtbase + SIRFSOC_GPIO_PAD_EN(group));
-	muxval = muxval | (1 << offset);
+	muxval = muxval | (1 << (offset - range->pin_base));
 	writel(muxval, spmx->gpio_virtbase + SIRFSOC_GPIO_PAD_EN(group));
 
 	return 0;
@@ -1086,7 +1086,6 @@ static struct pinctrl_desc sirfsoc_pinmux_desc = {
 	.name = DRIVER_NAME,
 	.pins = sirfsoc_pads,
 	.npins = ARRAY_SIZE(sirfsoc_pads),
-	.maxpin = SIRFSOC_NUM_PADS - 1,
 	.pctlops = &sirfsoc_pctrl_ops,
 	.pmxops = &sirfsoc_pinmux_ops,
 	.owner = THIS_MODULE,
@@ -1100,21 +1099,25 @@ static struct pinctrl_gpio_range sirfsoc_gpio_ranges[] = {
 		.name = "sirfsoc-gpio*",
 		.id = 0,
 		.base = 0,
+		.pin_base = 0,
 		.npins = 32,
 	}, {
 		.name = "sirfsoc-gpio*",
 		.id = 1,
 		.base = 32,
+		.pin_base = 32,
 		.npins = 32,
 	}, {
 		.name = "sirfsoc-gpio*",
 		.id = 2,
 		.base = 64,
+		.pin_base = 64,
 		.npins = 32,
 	}, {
 		.name = "sirfsoc-gpio*",
 		.id = 3,
 		.base = 96,
+		.pin_base = 96,
 		.npins = 19,
 	},
 };
diff --git a/drivers/pinctrl/pinmux-u300.c b/drivers/pinctrl/pinctrl-u300.c
index 4858a64131f8..c8d02f1c2b5e 100644
--- a/drivers/pinctrl/pinmux-u300.c
+++ b/drivers/pinctrl/pinctrl-u300.c
@@ -940,20 +940,23 @@ static void u300_pmx_endisable(struct u300_pmx *upmx, unsigned selector,
 {
 	u16 regval, val, mask;
 	int i;
+	const struct u300_pmx_mask *upmx_mask;
 
+	upmx_mask = u300_pmx_functions[selector].mask;
 	for (i = 0; i < ARRAY_SIZE(u300_pmx_registers); i++) {
 		if (enable)
-			val = u300_pmx_functions[selector].mask->bits;
+			val = upmx_mask->bits;
 		else
 			val = 0;
 
-		mask = u300_pmx_functions[selector].mask->mask;
+		mask = upmx_mask->mask;
 		if (mask != 0) {
 			regval = readw(upmx->virtbase + u300_pmx_registers[i]);
 			regval &= ~mask;
 			regval |= val;
 			writew(regval, upmx->virtbase + u300_pmx_registers[i]);
 		}
+		upmx_mask++;
 	}
 }
 
@@ -1016,21 +1019,35 @@ static struct pinmux_ops u300_pmx_ops = {
 };
 
 /*
- * FIXME: this will be set to sane values as this driver engulfs
- * drivers/gpio/gpio-u300.c and we really know this stuff.
+ * GPIO ranges handled by the application-side COH901XXX GPIO controller
+ * Very many pins can be converted into GPIO pins, but we only list those
+ * that are useful in practice to cut down on tables.
  */
-static struct pinctrl_gpio_range u300_gpio_range = {
-	.name = "COH901*",
-	.id = 0,
-	.base = 0,
-	.npins = 64,
+#define U300_GPIO_RANGE(a, b, c) { .name = "COH901XXX", .id = a, .base= a, \
+			.pin_base = b, .npins = c }
+
+static struct pinctrl_gpio_range u300_gpio_ranges[] = {
+	U300_GPIO_RANGE(10, 426, 1),
+	U300_GPIO_RANGE(11, 180, 1),
+	U300_GPIO_RANGE(12, 165, 1), /* MS/MMC card insertion */
+	U300_GPIO_RANGE(13, 179, 1),
+	U300_GPIO_RANGE(14, 178, 1),
+	U300_GPIO_RANGE(16, 194, 1),
+	U300_GPIO_RANGE(17, 193, 1),
+	U300_GPIO_RANGE(18, 192, 1),
+	U300_GPIO_RANGE(19, 191, 1),
+	U300_GPIO_RANGE(20, 186, 1),
+	U300_GPIO_RANGE(21, 185, 1),
+	U300_GPIO_RANGE(22, 184, 1),
+	U300_GPIO_RANGE(23, 183, 1),
+	U300_GPIO_RANGE(24, 182, 1),
+	U300_GPIO_RANGE(25, 181, 1),
 };
 
 static struct pinctrl_desc u300_pmx_desc = {
 	.name = DRIVER_NAME,
 	.pins = u300_pads,
 	.npins = ARRAY_SIZE(u300_pads),
-	.maxpin = U300_NUM_PADS-1,
 	.pctlops = &u300_pctrl_ops,
 	.pmxops = &u300_pmx_ops,
 	.owner = THIS_MODULE,
@@ -1038,9 +1055,10 @@ static struct pinctrl_desc u300_pmx_desc = {
 
 static int __init u300_pmx_probe(struct platform_device *pdev)
 {
-	int ret;
 	struct u300_pmx *upmx;
 	struct resource *res;
+	int ret;
+	int i;
 
 	/* Create state holders etc for this driver */
 	upmx = devm_kzalloc(&pdev->dev, sizeof(*upmx), GFP_KERNEL);
@@ -1077,7 +1095,8 @@ static int __init u300_pmx_probe(struct platform_device *pdev)
 	}
 
 	/* We will handle a range of GPIO pins */
-	pinctrl_add_gpio_range(upmx->pctl, &u300_gpio_range);
+	for (i = 0; i < ARRAY_SIZE(u300_gpio_ranges); i++)
+		pinctrl_add_gpio_range(upmx->pctl, &u300_gpio_ranges[i]);
 
 	platform_set_drvdata(pdev, upmx);
 
@@ -1099,8 +1118,10 @@ out_no_resource:
 static int __exit u300_pmx_remove(struct platform_device *pdev)
 {
 	struct u300_pmx *upmx = platform_get_drvdata(pdev);
+	int i;
 
-	pinctrl_remove_gpio_range(upmx->pctl, &u300_gpio_range);
+	for (i = 0; i < ARRAY_SIZE(u300_gpio_ranges); i++)
+		pinctrl_remove_gpio_range(upmx->pctl, &u300_gpio_ranges[i]);
 	pinctrl_unregister(upmx->pctl);
 	iounmap(upmx->virtbase);
 	release_mem_region(upmx->phybase, upmx->physize);
diff --git a/drivers/pinctrl/pinmux.c b/drivers/pinctrl/pinmux.c
index a5467f8709e9..a76a348321bb 100644
--- a/drivers/pinctrl/pinmux.c
+++ b/drivers/pinctrl/pinmux.c
@@ -21,6 +21,7 @@
 #include <linux/list.h>
 #include <linux/mutex.h>
 #include <linux/spinlock.h>
+#include <linux/string.h>
 #include <linux/sysfs.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
@@ -32,12 +33,8 @@
 static DEFINE_MUTEX(pinmux_list_mutex);
 static LIST_HEAD(pinmux_list);
 
-/* List of pinmux hogs */
-static DEFINE_MUTEX(pinmux_hoglist_mutex);
-static LIST_HEAD(pinmux_hoglist);
-
-/* Global pinmux maps, we allow one set only */
-static struct pinmux_map const *pinmux_maps;
+/* Global pinmux maps */
+static struct pinmux_map *pinmux_maps;
 static unsigned pinmux_maps_num;
 
 /**
@@ -98,41 +95,35 @@ struct pinmux_hog {
  * @function: a functional name to give to this pin, passed to the driver
  *	so it knows what function to mux in, e.g. the string "gpioNN"
  *	means that you want to mux in the pin for use as GPIO number NN
- * @gpio: if this request concerns a single GPIO pin
  * @gpio_range: the range matching the GPIO pin if this is a request for a
  *	single GPIO pin
  */
 static int pin_request(struct pinctrl_dev *pctldev,
-		       int pin, const char *function, bool gpio,
+		       int pin, const char *function,
 		       struct pinctrl_gpio_range *gpio_range)
 {
 	struct pin_desc *desc;
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
 	int status = -EINVAL;
 
-	dev_dbg(&pctldev->dev, "request pin %d for %s\n", pin, function);
-
-	if (!pin_is_valid(pctldev, pin)) {
-		dev_err(&pctldev->dev, "pin is invalid\n");
-		return -EINVAL;
-	}
-
-	if (!function) {
-		dev_err(&pctldev->dev, "no function name given\n");
-		return -EINVAL;
-	}
+	dev_dbg(pctldev->dev, "request pin %d for %s\n", pin, function);
 
 	desc = pin_desc_get(pctldev, pin);
 	if (desc == NULL) {
-		dev_err(&pctldev->dev,
+		dev_err(pctldev->dev,
 			"pin is not registered so it cannot be requested\n");
 		goto out;
 	}
 
+	if (!function) {
+		dev_err(pctldev->dev, "no function name given\n");
+		return -EINVAL;
+	}
+
 	spin_lock(&desc->lock);
 	if (desc->mux_function) {
 		spin_unlock(&desc->lock);
-		dev_err(&pctldev->dev,
+		dev_err(pctldev->dev,
 			"pin already requested\n");
 		goto out;
 	}
@@ -141,7 +132,7 @@ static int pin_request(struct pinctrl_dev *pctldev,
 
 	/* Let each pin increase references to this module */
 	if (!try_module_get(pctldev->owner)) {
-		dev_err(&pctldev->dev,
+		dev_err(pctldev->dev,
 			"could not increase module refcount for pin %d\n",
 			pin);
 		status = -EINVAL;
@@ -152,7 +143,7 @@ static int pin_request(struct pinctrl_dev *pctldev,
 	 * If there is no kind of request function for the pin we just assume
 	 * we got it by default and proceed.
 	 */
-	if (gpio && ops->gpio_request_enable)
+	if (gpio_range && ops->gpio_request_enable)
 		/* This requests and enables a single GPIO pin */
 		status = ops->gpio_request_enable(pctldev, gpio_range, pin);
 	else if (ops->request)
@@ -161,7 +152,7 @@ static int pin_request(struct pinctrl_dev *pctldev,
 		status = 0;
 
 	if (status)
-		dev_err(&pctldev->dev, "->request on device %s failed "
+		dev_err(pctldev->dev, "->request on device %s failed "
 		       "for pin %d\n",
 		       pctldev->desc->name, pin);
 out_free_pin:
@@ -172,7 +163,7 @@ out_free_pin:
 	}
 out:
 	if (status)
-		dev_err(&pctldev->dev, "pin-%d (%s) status %d\n",
+		dev_err(pctldev->dev, "pin-%d (%s) status %d\n",
 		       pin, function ? : "?", status);
 
 	return status;
@@ -182,34 +173,52 @@ out:
  * pin_free() - release a single muxed in pin so something else can be muxed
  * @pctldev: pin controller device handling this pin
  * @pin: the pin to free
- * @free_func: whether to free the pin's assigned function name string
+ * @gpio_range: the range matching the GPIO pin if this is a request for a
+ *	single GPIO pin
+ *
+ * This function returns a pointer to the function name in use. This is used
+ * for callers that dynamically allocate a function name so it can be freed
+ * once the pin is free. This is done for GPIO request functions.
  */
-static void pin_free(struct pinctrl_dev *pctldev, int pin, int free_func)
+static const char *pin_free(struct pinctrl_dev *pctldev, int pin,
+			    struct pinctrl_gpio_range *gpio_range)
 {
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
 	struct pin_desc *desc;
+	const char *func;
 
 	desc = pin_desc_get(pctldev, pin);
 	if (desc == NULL) {
-		dev_err(&pctldev->dev,
+		dev_err(pctldev->dev,
 			"pin is not registered so it cannot be freed\n");
-		return;
+		return NULL;
 	}
 
-	if (ops->free)
+	/*
+	 * If there is no kind of request function for the pin we just assume
+	 * we got it by default and proceed.
+	 */
+	if (gpio_range && ops->gpio_disable_free)
+		ops->gpio_disable_free(pctldev, gpio_range, pin);
+	else if (ops->free)
 		ops->free(pctldev, pin);
 
 	spin_lock(&desc->lock);
-	if (free_func)
-		kfree(desc->mux_function);
+	func = desc->mux_function;
 	desc->mux_function = NULL;
 	spin_unlock(&desc->lock);
 	module_put(pctldev->owner);
+
+	return func;
 }
 
 /**
  * pinmux_request_gpio() - request a single pin to be muxed in as GPIO
  * @gpio: the GPIO pin number from the GPIO subsystem number space
+ *
+ * This function should *ONLY* be used from gpiolib-based GPIO drivers,
+ * as part of their gpio_request() semantics, platforms and individual drivers
+ * shall *NOT* request GPIO pins to be muxed in.
  */
 int pinmux_request_gpio(unsigned gpio)
 {
@@ -225,7 +234,7 @@ int pinmux_request_gpio(unsigned gpio)
 		return -EINVAL;
 
 	/* Convert to the pin controllers number space */
-	pin = gpio - range->base;
+	pin = gpio - range->base + range->pin_base;
 
 	/* Conjure some name stating what chip and pin this is taken by */
 	snprintf(gpiostr, 15, "%s:%d", range->name, gpio);
@@ -234,7 +243,7 @@ int pinmux_request_gpio(unsigned gpio)
 	if (!function)
 		return -EINVAL;
 
-	ret = pin_request(pctldev, pin, function, true, range);
+	ret = pin_request(pctldev, pin, function, range);
 	if (ret < 0)
 		kfree(function);
 
@@ -245,6 +254,10 @@ EXPORT_SYMBOL_GPL(pinmux_request_gpio);
 /**
  * pinmux_free_gpio() - free a single pin, currently used as GPIO
  * @gpio: the GPIO pin number from the GPIO subsystem number space
+ *
+ * This function should *ONLY* be used from gpiolib-based GPIO drivers,
+ * as part of their gpio_free() semantics, platforms and individual drivers
+ * shall *NOT* request GPIO pins to be muxed out.
  */
 void pinmux_free_gpio(unsigned gpio)
 {
@@ -252,53 +265,108 @@ void pinmux_free_gpio(unsigned gpio)
 	struct pinctrl_gpio_range *range;
 	int ret;
 	int pin;
+	const char *func;
 
 	ret = pinctrl_get_device_gpio_range(gpio, &pctldev, &range);
 	if (ret)
 		return;
 
 	/* Convert to the pin controllers number space */
-	pin = gpio - range->base;
+	pin = gpio - range->base + range->pin_base;
 
-	pin_free(pctldev, pin, true);
+	func = pin_free(pctldev, pin, range);
+	kfree(func);
 }
 EXPORT_SYMBOL_GPL(pinmux_free_gpio);
 
+static int pinmux_gpio_direction(unsigned gpio, bool input)
+{
+	struct pinctrl_dev *pctldev;
+	struct pinctrl_gpio_range *range;
+	const struct pinmux_ops *ops;
+	int ret;
+	int pin;
+
+	ret = pinctrl_get_device_gpio_range(gpio, &pctldev, &range);
+	if (ret)
+		return ret;
+
+	ops = pctldev->desc->pmxops;
+
+	/* Convert to the pin controllers number space */
+	pin = gpio - range->base + range->pin_base;
+
+	if (ops->gpio_set_direction)
+		ret = ops->gpio_set_direction(pctldev, range, pin, input);
+	else
+		ret = 0;
+
+	return ret;
+}
+
+/**
+ * pinmux_gpio_direction_input() - request a GPIO pin to go into input mode
+ * @gpio: the GPIO pin number from the GPIO subsystem number space
+ *
+ * This function should *ONLY* be used from gpiolib-based GPIO drivers,
+ * as part of their gpio_direction_input() semantics, platforms and individual
+ * drivers shall *NOT* touch pinmux GPIO calls.
+ */
+int pinmux_gpio_direction_input(unsigned gpio)
+{
+	return pinmux_gpio_direction(gpio, true);
+}
+EXPORT_SYMBOL_GPL(pinmux_gpio_direction_input);
+
+/**
+ * pinmux_gpio_direction_output() - request a GPIO pin to go into output mode
+ * @gpio: the GPIO pin number from the GPIO subsystem number space
+ *
+ * This function should *ONLY* be used from gpiolib-based GPIO drivers,
+ * as part of their gpio_direction_output() semantics, platforms and individual
+ * drivers shall *NOT* touch pinmux GPIO calls.
+ */
+int pinmux_gpio_direction_output(unsigned gpio)
+{
+	return pinmux_gpio_direction(gpio, false);
+}
+EXPORT_SYMBOL_GPL(pinmux_gpio_direction_output);
+
 /**
  * pinmux_register_mappings() - register a set of pinmux mappings
- * @maps: the pinmux mappings table to register
+ * @maps: the pinmux mappings table to register, this should be marked with
+ *	__initdata so it can be discarded after boot, this function will
+ *	perform a shallow copy for the mapping entries.
  * @num_maps: the number of maps in the mapping table
  *
  * Only call this once during initialization of your machine, the function is
  * tagged as __init and won't be callable after init has completed. The map
  * passed into this function will be owned by the pinmux core and cannot be
- * free:d.
+ * freed.
  */
 int __init pinmux_register_mappings(struct pinmux_map const *maps,
 				    unsigned num_maps)
 {
+	void *tmp_maps;
 	int i;
 
-	if (pinmux_maps != NULL) {
-		pr_err("pinmux mappings already registered, you can only "
-		       "register one set of maps\n");
-		return -EINVAL;
-	}
-
 	pr_debug("add %d pinmux maps\n", num_maps);
+
+	/* First sanity check the new mapping */
 	for (i = 0; i < num_maps; i++) {
-		/* Sanity check the mapping */
 		if (!maps[i].name) {
 			pr_err("failed to register map %d: "
 			       "no map name given\n", i);
 			return -EINVAL;
 		}
+
 		if (!maps[i].ctrl_dev && !maps[i].ctrl_dev_name) {
 			pr_err("failed to register map %s (%d): "
 			       "no pin control device given\n",
 			       maps[i].name, i);
 			return -EINVAL;
 		}
+
 		if (!maps[i].function) {
 			pr_err("failed to register map %s (%d): "
 			       "no function ID given\n", maps[i].name, i);
@@ -315,9 +383,30 @@ int __init pinmux_register_mappings(struct pinmux_map const *maps,
 				 maps[i].function);
 	}
 
-	pinmux_maps = maps;
-	pinmux_maps_num = num_maps;
+	/*
+	 * Make a copy of the map array - string pointers will end up in the
+	 * kernel const section anyway so these do not need to be deep copied.
+	 */
+	if (!pinmux_maps_num) {
+		/* On first call, just copy them */
+		tmp_maps = kmemdup(maps,
+				   sizeof(struct pinmux_map) * num_maps,
+				   GFP_KERNEL);
+		if (!tmp_maps)
+			return -ENOMEM;
+	} else {
+		/* Subsequent calls, reallocate array to new size */
+		size_t oldsize = sizeof(struct pinmux_map) * pinmux_maps_num;
+		size_t newsize = sizeof(struct pinmux_map) * num_maps;
+
+		tmp_maps = krealloc(pinmux_maps, oldsize + newsize, GFP_KERNEL);
+		if (!tmp_maps)
+			return -ENOMEM;
+		memcpy((tmp_maps + oldsize), maps, newsize);
+	}
 
+	pinmux_maps = tmp_maps;
+	pinmux_maps_num += num_maps;
 	return 0;
 }
 
@@ -345,14 +434,14 @@ static int acquire_pins(struct pinctrl_dev *pctldev,
 	if (ret)
 		return ret;
 
-	dev_dbg(&pctldev->dev, "requesting the %u pins from group %u\n",
+	dev_dbg(pctldev->dev, "requesting the %u pins from group %u\n",
 		num_pins, group_selector);
 
 	/* Try to allocate all pins in this group, one by one */
 	for (i = 0; i < num_pins; i++) {
-		ret = pin_request(pctldev, pins[i], func, false, NULL);
+		ret = pin_request(pctldev, pins[i], func, NULL);
 		if (ret) {
-			dev_err(&pctldev->dev,
+			dev_err(pctldev->dev,
 				"could not get pin %d for function %s "
 				"on device %s - conflicting mux mappings?\n",
 				pins[i], func ? : "(undefined)",
@@ -360,7 +449,7 @@ static int acquire_pins(struct pinctrl_dev *pctldev,
 			/* On error release all taken pins */
 			i--; /* this pin just failed */
 			for (; i >= 0; i--)
-				pin_free(pctldev, pins[i], false);
+				pin_free(pctldev, pins[i], NULL);
 			return -ENODEV;
 		}
 	}
@@ -384,44 +473,13 @@ static void release_pins(struct pinctrl_dev *pctldev,
 	ret = pctlops->get_group_pins(pctldev, group_selector,
 				      &pins, &num_pins);
 	if (ret) {
-		dev_err(&pctldev->dev, "could not get pins to release for "
+		dev_err(pctldev->dev, "could not get pins to release for "
 			"group selector %d\n",
 			group_selector);
 		return;
 	}
 	for (i = 0; i < num_pins; i++)
-		pin_free(pctldev, pins[i], false);
-}
-
-/**
- * pinmux_get_group_selector() - returns the group selector for a group
- * @pctldev: the pin controller handling the group
- * @pin_group: the pin group to look up
- */
-static int pinmux_get_group_selector(struct pinctrl_dev *pctldev,
-				     const char *pin_group)
-{
-	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
-	unsigned group_selector = 0;
-
-	while (pctlops->list_groups(pctldev, group_selector) >= 0) {
-		const char *gname = pctlops->get_group_name(pctldev,
-							    group_selector);
-		if (!strcmp(gname, pin_group)) {
-			dev_dbg(&pctldev->dev,
-				"found group selector %u for %s\n",
-				group_selector,
-				pin_group);
-			return group_selector;
-		}
-
-		group_selector++;
-	}
-
-	dev_err(&pctldev->dev, "does not have pin group %s\n",
-		pin_group);
-
-	return -EINVAL;
+		pin_free(pctldev, pins[i], NULL);
 }
 
 /**
@@ -465,9 +523,9 @@ static int pinmux_check_pin_group(struct pinctrl_dev *pctldev,
 			return ret;
 		if (num_groups < 1)
 			return -EINVAL;
-		ret = pinmux_get_group_selector(pctldev, groups[0]);
+		ret = pinctrl_get_group_selector(pctldev, groups[0]);
 		if (ret < 0) {
-			dev_err(&pctldev->dev,
+			dev_err(pctldev->dev,
 				"function %s wants group %s but the pin "
 				"controller does not seem to have that group\n",
 				pmxops->get_function_name(pctldev, func_selector),
@@ -476,7 +534,7 @@ static int pinmux_check_pin_group(struct pinctrl_dev *pctldev,
 		}
 
 		if (num_groups > 1)
-			dev_dbg(&pctldev->dev,
+			dev_dbg(pctldev->dev,
 				"function %s support more than one group, "
 				"default-selecting first group %s (%d)\n",
 				pmxops->get_function_name(pctldev, func_selector),
@@ -486,13 +544,13 @@ static int pinmux_check_pin_group(struct pinctrl_dev *pctldev,
 		return ret;
 	}
 
-	dev_dbg(&pctldev->dev,
+	dev_dbg(pctldev->dev,
 		"check if we have pin group %s on controller %s\n",
 		pin_group, pinctrl_dev_get_name(pctldev));
 
-	ret = pinmux_get_group_selector(pctldev, pin_group);
+	ret = pinctrl_get_group_selector(pctldev, pin_group);
 	if (ret < 0) {
-		dev_dbg(&pctldev->dev,
+		dev_dbg(pctldev->dev,
 			"%s does not support pin group %s with function %s\n",
 			pinctrl_dev_get_name(pctldev),
 			pin_group,
@@ -569,7 +627,7 @@ static int pinmux_enable_muxmap(struct pinctrl_dev *pctldev,
 	 */
 
 	if (pmx->pctldev && pmx->pctldev != pctldev) {
-		dev_err(&pctldev->dev,
+		dev_err(pctldev->dev,
 			"different pin control devices given for device %s, "
 			"function %s\n",
 			devname,
@@ -592,7 +650,7 @@ static int pinmux_enable_muxmap(struct pinctrl_dev *pctldev,
 	 */
 	if (pmx->func_selector != UINT_MAX &&
 	    pmx->func_selector != func_selector) {
-		dev_err(&pctldev->dev,
+		dev_err(pctldev->dev,
 			"dual function defines in the map for device %s\n",
 		       devname);
 		return -EINVAL;
@@ -698,7 +756,7 @@ struct pinmux *pinmux_get(struct device *dev, const char *name)
 		}
 
 		pr_debug("in map, found pctldev %s to handle function %s",
-			 dev_name(&pctldev->dev), map->function);
+			 dev_name(pctldev->dev), map->function);
 
 
 		/*
@@ -874,7 +932,7 @@ static int pinmux_hog_map(struct pinctrl_dev *pctldev,
 		 * without any problems, so then we can hog pinmuxes for
 		 * all devices that just want a static pin mux at this point.
 		 */
-		dev_err(&pctldev->dev, "map %s wants to hog a non-system "
+		dev_err(pctldev->dev, "map %s wants to hog a non-system "
 			"pinmux, this is not going to work\n", map->name);
 		return -EINVAL;
 	}
@@ -886,7 +944,7 @@ static int pinmux_hog_map(struct pinctrl_dev *pctldev,
 	pmx = pinmux_get(NULL, map->name);
 	if (IS_ERR(pmx)) {
 		kfree(hog);
-		dev_err(&pctldev->dev,
+		dev_err(pctldev->dev,
 			"could not get the %s pinmux mapping for hogging\n",
 			map->name);
 		return PTR_ERR(pmx);
@@ -896,7 +954,7 @@ static int pinmux_hog_map(struct pinctrl_dev *pctldev,
 	if (ret) {
 		pinmux_put(pmx);
 		kfree(hog);
-		dev_err(&pctldev->dev,
+		dev_err(pctldev->dev,
 			"could not enable the %s pinmux mapping for hogging\n",
 			map->name);
 		return ret;
@@ -905,7 +963,7 @@ static int pinmux_hog_map(struct pinctrl_dev *pctldev,
 	hog->map = map;
 	hog->pmx = pmx;
 
-	dev_info(&pctldev->dev, "hogged map %s, function %s\n", map->name,
+	dev_info(pctldev->dev, "hogged map %s, function %s\n", map->name,
 		 map->function);
 	mutex_lock(&pctldev->pinmux_hogs_lock);
 	list_add(&hog->node, &pctldev->pinmux_hogs);
@@ -924,7 +982,7 @@ static int pinmux_hog_map(struct pinctrl_dev *pctldev,
  */
 int pinmux_hog_maps(struct pinctrl_dev *pctldev)
 {
-	struct device *dev = &pctldev->dev;
+	struct device *dev = pctldev->dev;
 	const char *devname = dev_name(dev);
 	int ret;
 	int i;
@@ -948,7 +1006,7 @@ int pinmux_hog_maps(struct pinctrl_dev *pctldev)
 }
 
 /**
- * pinmux_hog_maps() - unhog specific map entries on controller device
+ * pinmux_unhog_maps() - unhog specific map entries on controller device
  * @pctldev: the pin control device to unhog entries on
  */
 void pinmux_unhog_maps(struct pinctrl_dev *pctldev)
@@ -1005,18 +1063,19 @@ static int pinmux_functions_show(struct seq_file *s, void *what)
 static int pinmux_pins_show(struct seq_file *s, void *what)
 {
 	struct pinctrl_dev *pctldev = s->private;
-	unsigned pin;
+	unsigned i, pin;
 
 	seq_puts(s, "Pinmux settings per pin\n");
 	seq_puts(s, "Format: pin (name): pinmuxfunction\n");
 
-	/* The highest pin number need to be included in the loop, thus <= */
-	for (pin = 0; pin <= pctldev->desc->maxpin; pin++) {
+	/* The pin number can be retrived from the pin controller descriptor */
+	for (i = 0; i < pctldev->desc->npins; i++) {
 
 		struct pin_desc *desc;
 
+		pin = pctldev->desc->pins[i].number;
 		desc = pin_desc_get(pctldev, pin);
-		/* Pin space may be sparse */
+		/* Skip if we cannot search the pin */
 		if (desc == NULL)
 			continue;
 
diff --git a/drivers/regulator/88pm8607.c b/drivers/regulator/88pm8607.c
index ca0d608f8248..df33530cec4a 100644
--- a/drivers/regulator/88pm8607.c
+++ b/drivers/regulator/88pm8607.c
@@ -427,7 +427,7 @@ static int __devinit pm8607_regulator_probe(struct platform_device *pdev)
 
 	/* replace driver_data with info */
 	info->regulator = regulator_register(&info->desc, &pdev->dev,
-					     pdata, info);
+					     pdata, info, NULL);
 	if (IS_ERR(info->regulator)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 			info->desc.name);
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 9713b1b860cb..7a61b17ddd04 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -93,6 +93,7 @@ config REGULATOR_MAX1586
 config REGULATOR_MAX8649
 	tristate "Maxim 8649 voltage regulator"
 	depends on I2C
+	select REGMAP_I2C
 	help
 	  This driver controls a Maxim 8649 voltage output regulator via
 	  I2C bus.
@@ -177,6 +178,13 @@ config REGULATOR_DA903X
 	  Say y here to support the BUCKs and LDOs regulators found on
 	  Dialog Semiconductor DA9030/DA9034 PMIC.
 
+config REGULATOR_DA9052
+	tristate "Dialog DA9052/DA9053 regulators"
+	depends on PMIC_DA9052
+	help
+	  This driver supports the voltage regulators of DA9052-BC and
+	  DA9053-AA/Bx PMIC.
+
 config REGULATOR_PCF50633
 	tristate "PCF50633 regulator driver"
         depends on MFD_PCF50633
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 93a6318f5328..503bac87715e 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -4,6 +4,7 @@
 
 
 obj-$(CONFIG_REGULATOR) += core.o dummy.o
+obj-$(CONFIG_OF) += of_regulator.o
 obj-$(CONFIG_REGULATOR_FIXED_VOLTAGE) += fixed.o
 obj-$(CONFIG_REGULATOR_VIRTUAL_CONSUMER) += virtual.o
 obj-$(CONFIG_REGULATOR_USERSPACE_CONSUMER) += userspace-consumer.o
@@ -29,6 +30,7 @@ obj-$(CONFIG_REGULATOR_WM8400) += wm8400-regulator.o
 obj-$(CONFIG_REGULATOR_WM8994) += wm8994-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6586X) += tps6586x-regulator.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
+obj-$(CONFIG_REGULATOR_DA9052)	+= da9052-regulator.o
 obj-$(CONFIG_REGULATOR_PCF50633) += pcf50633-regulator.o
 obj-$(CONFIG_REGULATOR_PCAP) += pcap-regulator.o
 obj-$(CONFIG_REGULATOR_MC13783) += mc13783-regulator.o
diff --git a/drivers/regulator/aat2870-regulator.c b/drivers/regulator/aat2870-regulator.c
index 298c6c6a2795..685ad43b0749 100644
--- a/drivers/regulator/aat2870-regulator.c
+++ b/drivers/regulator/aat2870-regulator.c
@@ -63,7 +63,7 @@ static int aat2870_ldo_set_voltage_sel(struct regulator_dev *rdev,
 	struct aat2870_data *aat2870 = dev_get_drvdata(ri->pdev->dev.parent);
 
 	return aat2870->update(aat2870, ri->voltage_addr, ri->voltage_mask,
-			(selector << ri->voltage_shift) & ri->voltage_mask);
+			       selector << ri->voltage_shift);
 }
 
 static int aat2870_ldo_get_voltage_sel(struct regulator_dev *rdev)
@@ -188,7 +188,7 @@ static int aat2870_regulator_probe(struct platform_device *pdev)
 	ri->pdev = pdev;
 
 	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdev->dev.platform_data, ri);
+				  pdev->dev.platform_data, ri, NULL);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "Failed to register regulator %s\n",
 			ri->desc.name);
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index 585e4946fe0a..042271aace6a 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -634,7 +634,7 @@ static int __devinit ab3100_regulators_probe(struct platform_device *pdev)
 		rdev = regulator_register(&ab3100_regulator_desc[i],
 					  &pdev->dev,
 					  &plfdata->reg_constraints[i],
-					  reg);
+					  reg, NULL);
 
 		if (IS_ERR(rdev)) {
 			err = PTR_ERR(rdev);
diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index 6e1ae69646b3..e91b8ddc2793 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -822,7 +822,7 @@ static __devinit int ab8500_regulator_probe(struct platform_device *pdev)
 
 		/* register regulator with framework */
 		info->regulator = regulator_register(&info->desc, &pdev->dev,
-				&pdata->regulator[i], info);
+				&pdata->regulator[i], info, NULL);
 		if (IS_ERR(info->regulator)) {
 			err = PTR_ERR(info->regulator);
 			dev_err(&pdev->dev, "failed to register regulator %s\n",
diff --git a/drivers/regulator/ad5398.c b/drivers/regulator/ad5398.c
index a4be41614eeb..483c80930852 100644
--- a/drivers/regulator/ad5398.c
+++ b/drivers/regulator/ad5398.c
@@ -233,7 +233,7 @@ static int __devinit ad5398_probe(struct i2c_client *client,
 	chip->current_mask = (chip->current_level - 1) << chip->current_offset;
 
 	chip->rdev = regulator_register(&ad5398_reg, &client->dev,
-					init_data, chip);
+					init_data, chip, NULL);
 	if (IS_ERR(chip->rdev)) {
 		ret = PTR_ERR(chip->rdev);
 		dev_err(&client->dev, "failed to register %s %s\n",
diff --git a/drivers/regulator/bq24022.c b/drivers/regulator/bq24022.c
index e24d1b7d97a8..9fab6d1bbe80 100644
--- a/drivers/regulator/bq24022.c
+++ b/drivers/regulator/bq24022.c
@@ -107,7 +107,7 @@ static int __init bq24022_probe(struct platform_device *pdev)
 	ret = gpio_direction_output(pdata->gpio_nce, 1);
 
 	bq24022 = regulator_register(&bq24022_desc, &pdev->dev,
-				     pdata->init_data, pdata);
+				     pdata->init_data, pdata, NULL);
 	if (IS_ERR(bq24022)) {
 		dev_dbg(&pdev->dev, "couldn't register regulator\n");
 		ret = PTR_ERR(bq24022);
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 938398f3e869..ca86f39a0fdc 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -25,6 +25,8 @@
 #include <linux/mutex.h>
 #include <linux/suspend.h>
 #include <linux/delay.h>
+#include <linux/of.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/regulator/consumer.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
@@ -132,6 +134,33 @@ static struct regulator *get_device_regulator(struct device *dev)
 	return NULL;
 }
 
+/**
+ * of_get_regulator - get a regulator device node based on supply name
+ * @dev: Device pointer for the consumer (of regulator) device
+ * @supply: regulator supply name
+ *
+ * Extract the regulator device node corresponding to the supply name.
+ * retruns the device node corresponding to the regulator if found, else
+ * returns NULL.
+ */
+static struct device_node *of_get_regulator(struct device *dev, const char *supply)
+{
+	struct device_node *regnode = NULL;
+	char prop_name[32]; /* 32 is max size of property name */
+
+	dev_dbg(dev, "Looking up %s-supply from device tree\n", supply);
+
+	snprintf(prop_name, 32, "%s-supply", supply);
+	regnode = of_parse_phandle(dev->of_node, prop_name, 0);
+
+	if (!regnode) {
+		dev_warn(dev, "%s property in node %s references invalid phandle",
+				prop_name, dev->of_node->full_name);
+		return NULL;
+	}
+	return regnode;
+}
+
 /* Platform voltage constraint check */
 static int regulator_check_voltage(struct regulator_dev *rdev,
 				   int *min_uV, int *max_uV)
@@ -883,8 +912,12 @@ static int set_machine_constraints(struct regulator_dev *rdev,
 	int ret = 0;
 	struct regulator_ops *ops = rdev->desc->ops;
 
-	rdev->constraints = kmemdup(constraints, sizeof(*constraints),
-				    GFP_KERNEL);
+	if (constraints)
+		rdev->constraints = kmemdup(constraints, sizeof(*constraints),
+					    GFP_KERNEL);
+	else
+		rdev->constraints = kzalloc(sizeof(*constraints),
+					    GFP_KERNEL);
 	if (!rdev->constraints)
 		return -ENOMEM;
 
@@ -893,7 +926,7 @@ static int set_machine_constraints(struct regulator_dev *rdev,
 		goto out;
 
 	/* do we need to setup our suspend state */
-	if (constraints->initial_state) {
+	if (rdev->constraints->initial_state) {
 		ret = suspend_prepare(rdev, rdev->constraints->initial_state);
 		if (ret < 0) {
 			rdev_err(rdev, "failed to set suspend state\n");
@@ -901,7 +934,7 @@ static int set_machine_constraints(struct regulator_dev *rdev,
 		}
 	}
 
-	if (constraints->initial_mode) {
+	if (rdev->constraints->initial_mode) {
 		if (!ops->set_mode) {
 			rdev_err(rdev, "no set_mode operation\n");
 			ret = -EINVAL;
@@ -952,9 +985,8 @@ static int set_supply(struct regulator_dev *rdev,
 	rdev_info(rdev, "supplied by %s\n", rdev_get_name(supply_rdev));
 
 	rdev->supply = create_regulator(supply_rdev, &rdev->dev, "SUPPLY");
-	if (IS_ERR(rdev->supply)) {
-		err = PTR_ERR(rdev->supply);
-		rdev->supply = NULL;
+	if (rdev->supply == NULL) {
+		err = -ENOMEM;
 		return err;
 	}
 
@@ -1148,6 +1180,30 @@ static int _regulator_get_enable_time(struct regulator_dev *rdev)
 	return rdev->desc->ops->enable_time(rdev);
 }
 
+static struct regulator_dev *regulator_dev_lookup(struct device *dev,
+							 const char *supply)
+{
+	struct regulator_dev *r;
+	struct device_node *node;
+
+	/* first do a dt based lookup */
+	if (dev && dev->of_node) {
+		node = of_get_regulator(dev, supply);
+		if (node)
+			list_for_each_entry(r, &regulator_list, list)
+				if (r->dev.parent &&
+					node == r->dev.of_node)
+					return r;
+	}
+
+	/* if not found, try doing it non-dt way */
+	list_for_each_entry(r, &regulator_list, list)
+		if (strcmp(rdev_get_name(r), supply) == 0)
+			return r;
+
+	return NULL;
+}
+
 /* Internal regulator request function */
 static struct regulator *_regulator_get(struct device *dev, const char *id,
 					int exclusive)
@@ -1168,6 +1224,10 @@ static struct regulator *_regulator_get(struct device *dev, const char *id,
 
 	mutex_lock(&regulator_list_mutex);
 
+	rdev = regulator_dev_lookup(dev, id);
+	if (rdev)
+		goto found;
+
 	list_for_each_entry(map, &regulator_map_list, list) {
 		/* If the mapping has a device set up it must match */
 		if (map->dev_name &&
@@ -1221,6 +1281,7 @@ found:
 	if (regulator == NULL) {
 		regulator = ERR_PTR(-ENOMEM);
 		module_put(rdev->owner);
+		goto out;
 	}
 
 	rdev->open_count++;
@@ -1726,6 +1787,7 @@ int regulator_is_supported_voltage(struct regulator *regulator,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(regulator_is_supported_voltage);
 
 static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 				     int min_uV, int max_uV)
@@ -2429,6 +2491,43 @@ err:
 EXPORT_SYMBOL_GPL(regulator_bulk_disable);
 
 /**
+ * regulator_bulk_force_disable - force disable multiple regulator consumers
+ *
+ * @num_consumers: Number of consumers
+ * @consumers:     Consumer data; clients are stored here.
+ * @return         0 on success, an errno on failure
+ *
+ * This convenience API allows consumers to forcibly disable multiple regulator
+ * clients in a single API call.
+ * NOTE: This should be used for situations when device damage will
+ * likely occur if the regulators are not disabled (e.g. over temp).
+ * Although regulator_force_disable function call for some consumers can
+ * return error numbers, the function is called for all consumers.
+ */
+int regulator_bulk_force_disable(int num_consumers,
+			   struct regulator_bulk_data *consumers)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < num_consumers; i++)
+		consumers[i].ret =
+			    regulator_force_disable(consumers[i].consumer);
+
+	for (i = 0; i < num_consumers; i++) {
+		if (consumers[i].ret != 0) {
+			ret = consumers[i].ret;
+			goto out;
+		}
+	}
+
+	return 0;
+out:
+	return ret;
+}
+EXPORT_SYMBOL_GPL(regulator_bulk_force_disable);
+
+/**
  * regulator_bulk_free - free multiple regulator consumers
  *
  * @num_consumers: Number of consumers
@@ -2503,7 +2602,8 @@ static int add_regulator_attributes(struct regulator_dev *rdev)
 	int			status = 0;
 
 	/* some attributes need specific methods to be displayed */
-	if (ops->get_voltage || ops->get_voltage_sel) {
+	if ((ops->get_voltage && ops->get_voltage(rdev) >= 0) ||
+	    (ops->get_voltage_sel && ops->get_voltage_sel(rdev) >= 0)) {
 		status = device_create_file(dev, &dev_attr_microvolts);
 		if (status < 0)
 			return status;
@@ -2637,11 +2737,13 @@ static void rdev_init_debugfs(struct regulator_dev *rdev)
  */
 struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	struct device *dev, const struct regulator_init_data *init_data,
-	void *driver_data)
+	void *driver_data, struct device_node *of_node)
 {
+	const struct regulation_constraints *constraints = NULL;
 	static atomic_t regulator_no = ATOMIC_INIT(0);
 	struct regulator_dev *rdev;
 	int ret, i;
+	const char *supply = NULL;
 
 	if (regulator_desc == NULL)
 		return ERR_PTR(-EINVAL);
@@ -2653,9 +2755,6 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	    regulator_desc->type != REGULATOR_CURRENT)
 		return ERR_PTR(-EINVAL);
 
-	if (!init_data)
-		return ERR_PTR(-EINVAL);
-
 	/* Only one of each should be implemented */
 	WARN_ON(regulator_desc->ops->get_voltage &&
 		regulator_desc->ops->get_voltage_sel);
@@ -2688,7 +2787,7 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	INIT_DELAYED_WORK(&rdev->disable_work, regulator_disable_work);
 
 	/* preform any regulator specific init */
-	if (init_data->regulator_init) {
+	if (init_data && init_data->regulator_init) {
 		ret = init_data->regulator_init(rdev->reg_data);
 		if (ret < 0)
 			goto clean;
@@ -2696,6 +2795,7 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 
 	/* register with sysfs */
 	rdev->dev.class = &regulator_class;
+	rdev->dev.of_node = of_node;
 	rdev->dev.parent = dev;
 	dev_set_name(&rdev->dev, "regulator.%d",
 		     atomic_inc_return(&regulator_no) - 1);
@@ -2708,7 +2808,10 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	dev_set_drvdata(&rdev->dev, rdev);
 
 	/* set regulator constraints */
-	ret = set_machine_constraints(rdev, &init_data->constraints);
+	if (init_data)
+		constraints = &init_data->constraints;
+
+	ret = set_machine_constraints(rdev, constraints);
 	if (ret < 0)
 		goto scrub;
 
@@ -2717,21 +2820,18 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	if (ret < 0)
 		goto scrub;
 
-	if (init_data->supply_regulator) {
+	if (init_data && init_data->supply_regulator)
+		supply = init_data->supply_regulator;
+	else if (regulator_desc->supply_name)
+		supply = regulator_desc->supply_name;
+
+	if (supply) {
 		struct regulator_dev *r;
-		int found = 0;
 
-		list_for_each_entry(r, &regulator_list, list) {
-			if (strcmp(rdev_get_name(r),
-				   init_data->supply_regulator) == 0) {
-				found = 1;
-				break;
-			}
-		}
+		r = regulator_dev_lookup(dev, supply);
 
-		if (!found) {
-			dev_err(dev, "Failed to find supply %s\n",
-				init_data->supply_regulator);
+		if (!r) {
+			dev_err(dev, "Failed to find supply %s\n", supply);
 			ret = -ENODEV;
 			goto scrub;
 		}
@@ -2739,18 +2839,28 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 		ret = set_supply(rdev, r);
 		if (ret < 0)
 			goto scrub;
+
+		/* Enable supply if rail is enabled */
+		if (rdev->desc->ops->is_enabled &&
+				rdev->desc->ops->is_enabled(rdev)) {
+			ret = regulator_enable(rdev->supply);
+			if (ret < 0)
+				goto scrub;
+		}
 	}
 
 	/* add consumers devices */
-	for (i = 0; i < init_data->num_consumer_supplies; i++) {
-		ret = set_consumer_device_supply(rdev,
-			init_data->consumer_supplies[i].dev,
-			init_data->consumer_supplies[i].dev_name,
-			init_data->consumer_supplies[i].supply);
-		if (ret < 0) {
-			dev_err(dev, "Failed to set supply %s\n",
+	if (init_data) {
+		for (i = 0; i < init_data->num_consumer_supplies; i++) {
+			ret = set_consumer_device_supply(rdev,
+				init_data->consumer_supplies[i].dev,
+				init_data->consumer_supplies[i].dev_name,
 				init_data->consumer_supplies[i].supply);
-			goto unset_supplies;
+			if (ret < 0) {
+				dev_err(dev, "Failed to set supply %s\n",
+					init_data->consumer_supplies[i].supply);
+				goto unset_supplies;
+			}
 		}
 	}
 
diff --git a/drivers/regulator/da903x.c b/drivers/regulator/da903x.c
index e23ddfa8b2c6..8dbc54da7d70 100644
--- a/drivers/regulator/da903x.c
+++ b/drivers/regulator/da903x.c
@@ -537,7 +537,7 @@ static int __devinit da903x_regulator_probe(struct platform_device *pdev)
 		ri->desc.ops = &da9030_regulator_ldo1_15_ops;
 
 	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdev->dev.platform_data, ri);
+				  pdev->dev.platform_data, ri, NULL);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 				ri->desc.name);
diff --git a/drivers/regulator/da9052-regulator.c b/drivers/regulator/da9052-regulator.c
new file mode 100644
index 000000000000..3767364452fd
--- /dev/null
+++ b/drivers/regulator/da9052-regulator.c
@@ -0,0 +1,606 @@
+/*
+* da9052-regulator.c: Regulator driver for DA9052
+*
+* Copyright(c) 2011 Dialog Semiconductor Ltd.
+*
+* Author: David Dajun Chen <dchen@diasemi.com>
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation; either version 2 of the License, or
+* (at your option) any later version.
+*
+*/
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+
+#include <linux/mfd/da9052/da9052.h>
+#include <linux/mfd/da9052/reg.h>
+#include <linux/mfd/da9052/pdata.h>
+
+/* Buck step size */
+#define DA9052_BUCK_PERI_3uV_STEP		100000
+#define DA9052_BUCK_PERI_REG_MAP_UPTO_3uV	24
+#define DA9052_CONST_3uV			3000000
+
+#define DA9052_MIN_UA		0
+#define DA9052_MAX_UA		3
+#define DA9052_CURRENT_RANGE	4
+
+/* Bit masks */
+#define DA9052_BUCK_ILIM_MASK_EVEN	0x0c
+#define DA9052_BUCK_ILIM_MASK_ODD	0xc0
+
+static const u32 da9052_current_limits[3][4] = {
+	{700000, 800000, 1000000, 1200000},	/* DA9052-BC BUCKs */
+	{1600000, 2000000, 2400000, 3000000},	/* DA9053-AA/Bx BUCK-CORE */
+	{800000, 1000000, 1200000, 1500000},	/* DA9053-AA/Bx BUCK-PRO,
+						 * BUCK-MEM and BUCK-PERI
+						*/
+};
+
+struct da9052_regulator_info {
+	struct regulator_desc reg_desc;
+	int step_uV;
+	int min_uV;
+	int max_uV;
+	unsigned char volt_shift;
+	unsigned char en_bit;
+	unsigned char activate_bit;
+};
+
+struct da9052_regulator {
+	struct da9052 *da9052;
+	struct da9052_regulator_info *info;
+	struct regulator_dev *rdev;
+};
+
+static int verify_range(struct da9052_regulator_info *info,
+			 int min_uV, int max_uV)
+{
+	if (min_uV > info->max_uV || max_uV < info->min_uV)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int da9052_regulator_enable(struct regulator_dev *rdev)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int offset = rdev_get_id(rdev);
+
+	return da9052_reg_update(regulator->da9052,
+				 DA9052_BUCKCORE_REG + offset,
+				 1 << info->en_bit, 1 << info->en_bit);
+}
+
+static int da9052_regulator_disable(struct regulator_dev *rdev)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int offset = rdev_get_id(rdev);
+
+	return da9052_reg_update(regulator->da9052,
+				 DA9052_BUCKCORE_REG + offset,
+				 1 << info->en_bit, 0);
+}
+
+static int da9052_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int offset = rdev_get_id(rdev);
+	int ret;
+
+	ret = da9052_reg_read(regulator->da9052, DA9052_BUCKCORE_REG + offset);
+	if (ret < 0)
+		return ret;
+
+	return ret & (1 << info->en_bit);
+}
+
+static int da9052_dcdc_get_current_limit(struct regulator_dev *rdev)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	int offset = rdev_get_id(rdev);
+	int ret, row = 2;
+
+	ret = da9052_reg_read(regulator->da9052, DA9052_BUCKA_REG + offset/2);
+	if (ret < 0)
+		return ret;
+
+	/* Determine the even or odd position of the buck current limit
+	 * register field
+	*/
+	if (offset % 2 == 0)
+		ret = (ret & DA9052_BUCK_ILIM_MASK_EVEN) >> 2;
+	else
+		ret = (ret & DA9052_BUCK_ILIM_MASK_ODD) >> 6;
+
+	/* Select the appropriate current limit range */
+	if (regulator->da9052->chip_id == DA9052)
+		row = 0;
+	else if (offset == 0)
+		row = 1;
+
+	return da9052_current_limits[row][ret];
+}
+
+static int da9052_dcdc_set_current_limit(struct regulator_dev *rdev, int min_uA,
+					  int max_uA)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	int offset = rdev_get_id(rdev);
+	int reg_val = 0;
+	int i, row = 2;
+
+	/* Select the appropriate current limit range */
+	if (regulator->da9052->chip_id == DA9052)
+		row = 0;
+	else if (offset == 0)
+		row = 1;
+
+	if (min_uA > da9052_current_limits[row][DA9052_MAX_UA] ||
+	    max_uA < da9052_current_limits[row][DA9052_MIN_UA])
+		return -EINVAL;
+
+	for (i = 0; i < DA9052_CURRENT_RANGE; i++) {
+		if (min_uA <= da9052_current_limits[row][i]) {
+			reg_val = i;
+			break;
+		}
+	}
+
+	/* Determine the even or odd position of the buck current limit
+	 * register field
+	*/
+	if (offset % 2 == 0)
+		return da9052_reg_update(regulator->da9052,
+					 DA9052_BUCKA_REG + offset/2,
+					 DA9052_BUCK_ILIM_MASK_EVEN,
+					 reg_val << 2);
+	else
+		return da9052_reg_update(regulator->da9052,
+					 DA9052_BUCKA_REG + offset/2,
+					 DA9052_BUCK_ILIM_MASK_ODD,
+					 reg_val << 6);
+}
+
+static int da9052_list_buckperi_voltage(struct regulator_dev *rdev,
+					 unsigned int selector)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int volt_uV;
+
+	if ((regulator->da9052->chip_id == DA9052) &&
+	    (selector >= DA9052_BUCK_PERI_REG_MAP_UPTO_3uV)) {
+		volt_uV = ((DA9052_BUCK_PERI_REG_MAP_UPTO_3uV * info->step_uV)
+			    + info->min_uV);
+		volt_uV += (selector - DA9052_BUCK_PERI_REG_MAP_UPTO_3uV)
+			    * (DA9052_BUCK_PERI_3uV_STEP);
+	} else
+			volt_uV = (selector * info->step_uV) + info->min_uV;
+
+	if (volt_uV > info->max_uV)
+		return -EINVAL;
+
+	return volt_uV;
+}
+
+static int da9052_list_voltage(struct regulator_dev *rdev,
+				unsigned int selector)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int volt_uV;
+
+	volt_uV = info->min_uV + info->step_uV * selector;
+
+	if (volt_uV > info->max_uV)
+		return -EINVAL;
+
+	return volt_uV;
+}
+
+static int da9052_regulator_set_voltage_int(struct regulator_dev *rdev,
+					     int min_uV, int max_uV,
+					     unsigned int *selector)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int offset = rdev_get_id(rdev);
+	int ret;
+
+	ret = verify_range(info, min_uV, max_uV);
+	if (ret < 0)
+		return ret;
+
+	if (min_uV < info->min_uV)
+		min_uV = info->min_uV;
+
+	*selector = (min_uV - info->min_uV) / info->step_uV;
+
+	ret = da9052_list_voltage(rdev, *selector);
+	if (ret < 0)
+		return ret;
+
+	return da9052_reg_update(regulator->da9052,
+				 DA9052_BUCKCORE_REG + offset,
+				 (1 << info->volt_shift) - 1, *selector);
+}
+
+static int da9052_set_ldo_voltage(struct regulator_dev *rdev,
+				   int min_uV, int max_uV,
+				   unsigned int *selector)
+{
+	return da9052_regulator_set_voltage_int(rdev, min_uV, max_uV, selector);
+}
+
+static int da9052_set_ldo5_6_voltage(struct regulator_dev *rdev,
+				      int min_uV, int max_uV,
+				      unsigned int *selector)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int ret;
+
+	ret = da9052_regulator_set_voltage_int(rdev, min_uV, max_uV, selector);
+	if (ret < 0)
+		return ret;
+
+	/* Some LDOs are DVC controlled which requires enabling of
+	 * the LDO activate bit to implment the changes on the
+	 * LDO output.
+	*/
+	return da9052_reg_update(regulator->da9052, DA9052_SUPPLY_REG, 0,
+				 info->activate_bit);
+}
+
+static int da9052_set_dcdc_voltage(struct regulator_dev *rdev,
+				    int min_uV, int max_uV,
+				    unsigned int *selector)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int ret;
+
+	ret = da9052_regulator_set_voltage_int(rdev, min_uV, max_uV, selector);
+	if (ret < 0)
+		return ret;
+
+	/* Some DCDCs are DVC controlled which requires enabling of
+	 * the DCDC activate bit to implment the changes on the
+	 * DCDC output.
+	*/
+	return da9052_reg_update(regulator->da9052, DA9052_SUPPLY_REG, 0,
+				 info->activate_bit);
+}
+
+static int da9052_get_regulator_voltage_sel(struct regulator_dev *rdev)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int offset = rdev_get_id(rdev);
+	int ret;
+
+	ret = da9052_reg_read(regulator->da9052, DA9052_BUCKCORE_REG + offset);
+	if (ret < 0)
+		return ret;
+
+	ret &= ((1 << info->volt_shift) - 1);
+
+	return ret;
+}
+
+static int da9052_set_buckperi_voltage(struct regulator_dev *rdev, int min_uV,
+					int max_uV, unsigned int *selector)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int offset = rdev_get_id(rdev);
+	int ret;
+
+	ret = verify_range(info, min_uV, max_uV);
+	if (ret < 0)
+		return ret;
+
+	if (min_uV < info->min_uV)
+		min_uV = info->min_uV;
+
+	if ((regulator->da9052->chip_id == DA9052) &&
+	    (min_uV >= DA9052_CONST_3uV))
+		*selector = DA9052_BUCK_PERI_REG_MAP_UPTO_3uV +
+			    ((min_uV - DA9052_CONST_3uV) /
+			    (DA9052_BUCK_PERI_3uV_STEP));
+	else
+		*selector = (min_uV - info->min_uV) / info->step_uV;
+
+	ret = da9052_list_buckperi_voltage(rdev, *selector);
+	if (ret < 0)
+		return ret;
+
+	return da9052_reg_update(regulator->da9052,
+				 DA9052_BUCKCORE_REG + offset,
+				 (1 << info->volt_shift) - 1, *selector);
+}
+
+static int da9052_get_buckperi_voltage_sel(struct regulator_dev *rdev)
+{
+	struct da9052_regulator *regulator = rdev_get_drvdata(rdev);
+	struct da9052_regulator_info *info = regulator->info;
+	int offset = rdev_get_id(rdev);
+	int ret;
+
+	ret = da9052_reg_read(regulator->da9052, DA9052_BUCKCORE_REG + offset);
+	if (ret < 0)
+		return ret;
+
+	ret &= ((1 << info->volt_shift) - 1);
+
+	return ret;
+}
+
+static struct regulator_ops da9052_buckperi_ops = {
+	.list_voltage = da9052_list_buckperi_voltage,
+	.get_voltage_sel = da9052_get_buckperi_voltage_sel,
+	.set_voltage = da9052_set_buckperi_voltage,
+
+	.get_current_limit = da9052_dcdc_get_current_limit,
+	.set_current_limit = da9052_dcdc_set_current_limit,
+
+	.is_enabled = da9052_regulator_is_enabled,
+	.enable = da9052_regulator_enable,
+	.disable = da9052_regulator_disable,
+};
+
+static struct regulator_ops da9052_dcdc_ops = {
+	.set_voltage = da9052_set_dcdc_voltage,
+	.get_current_limit = da9052_dcdc_get_current_limit,
+	.set_current_limit = da9052_dcdc_set_current_limit,
+
+	.list_voltage = da9052_list_voltage,
+	.get_voltage_sel = da9052_get_regulator_voltage_sel,
+	.is_enabled = da9052_regulator_is_enabled,
+	.enable = da9052_regulator_enable,
+	.disable = da9052_regulator_disable,
+};
+
+static struct regulator_ops da9052_ldo5_6_ops = {
+	.set_voltage = da9052_set_ldo5_6_voltage,
+
+	.list_voltage = da9052_list_voltage,
+	.get_voltage_sel = da9052_get_regulator_voltage_sel,
+	.is_enabled = da9052_regulator_is_enabled,
+	.enable = da9052_regulator_enable,
+	.disable = da9052_regulator_disable,
+};
+
+static struct regulator_ops da9052_ldo_ops = {
+	.set_voltage = da9052_set_ldo_voltage,
+
+	.list_voltage = da9052_list_voltage,
+	.get_voltage_sel = da9052_get_regulator_voltage_sel,
+	.is_enabled = da9052_regulator_is_enabled,
+	.enable = da9052_regulator_enable,
+	.disable = da9052_regulator_disable,
+};
+
+#define DA9052_LDO5_6(_id, step, min, max, sbits, ebits, abits) \
+{\
+	.reg_desc = {\
+		.name = "LDO" #_id,\
+		.ops = &da9052_ldo5_6_ops,\
+		.type = REGULATOR_VOLTAGE,\
+		.id = _id,\
+		.owner = THIS_MODULE,\
+	},\
+	.min_uV = (min) * 1000,\
+	.max_uV = (max) * 1000,\
+	.step_uV = (step) * 1000,\
+	.volt_shift = (sbits),\
+	.en_bit = (ebits),\
+	.activate_bit = (abits),\
+}
+
+#define DA9052_LDO(_id, step, min, max, sbits, ebits, abits) \
+{\
+	.reg_desc = {\
+		.name = "LDO" #_id,\
+		.ops = &da9052_ldo_ops,\
+		.type = REGULATOR_VOLTAGE,\
+		.id = _id,\
+		.owner = THIS_MODULE,\
+	},\
+	.min_uV = (min) * 1000,\
+	.max_uV = (max) * 1000,\
+	.step_uV = (step) * 1000,\
+	.volt_shift = (sbits),\
+	.en_bit = (ebits),\
+	.activate_bit = (abits),\
+}
+
+#define DA9052_DCDC(_id, step, min, max, sbits, ebits, abits) \
+{\
+	.reg_desc = {\
+		.name = "BUCK" #_id,\
+		.ops = &da9052_dcdc_ops,\
+		.type = REGULATOR_VOLTAGE,\
+		.id = _id,\
+		.owner = THIS_MODULE,\
+	},\
+	.min_uV = (min) * 1000,\
+	.max_uV = (max) * 1000,\
+	.step_uV = (step) * 1000,\
+	.volt_shift = (sbits),\
+	.en_bit = (ebits),\
+	.activate_bit = (abits),\
+}
+
+#define DA9052_BUCKPERI(_id, step, min, max, sbits, ebits, abits) \
+{\
+	.reg_desc = {\
+		.name = "BUCK" #_id,\
+		.ops = &da9052_buckperi_ops,\
+		.type = REGULATOR_VOLTAGE,\
+		.id = _id,\
+		.owner = THIS_MODULE,\
+	},\
+	.min_uV = (min) * 1000,\
+	.max_uV = (max) * 1000,\
+	.step_uV = (step) * 1000,\
+	.volt_shift = (sbits),\
+	.en_bit = (ebits),\
+	.activate_bit = (abits),\
+}
+
+static struct da9052_regulator_info da9052_regulator_info[] = {
+	/* Buck1 - 4 */
+	DA9052_DCDC(0, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBCOREGO),
+	DA9052_DCDC(1, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBPROGO),
+	DA9052_DCDC(2, 25, 925, 2500, 6, 6, DA9052_SUPPLY_VBMEMGO),
+	DA9052_BUCKPERI(3, 50, 1800, 3600, 5, 6, 0),
+	/* LD01 - LDO10 */
+	DA9052_LDO(4, 50, 600, 1800, 5, 6, 0),
+	DA9052_LDO5_6(5, 25, 600, 1800, 6, 6, DA9052_SUPPLY_VLDO2GO),
+	DA9052_LDO5_6(6, 25, 1725, 3300, 6, 6, DA9052_SUPPLY_VLDO3GO),
+	DA9052_LDO(7, 25, 1725, 3300, 6, 6, 0),
+	DA9052_LDO(8, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(9, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(10, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(11, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(12, 50, 1250, 3650, 6, 6, 0),
+	DA9052_LDO(13, 50, 1200, 3600, 6, 6, 0),
+};
+
+static struct da9052_regulator_info da9053_regulator_info[] = {
+	/* Buck1 - 4 */
+	DA9052_DCDC(0, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBCOREGO),
+	DA9052_DCDC(1, 25, 500, 2075, 6, 6, DA9052_SUPPLY_VBPROGO),
+	DA9052_DCDC(2, 25, 925, 2500, 6, 6, DA9052_SUPPLY_VBMEMGO),
+	DA9052_BUCKPERI(3, 25, 925, 2500, 6, 6, 0),
+	/* LD01 - LDO10 */
+	DA9052_LDO(4, 50, 600, 1800, 5, 6, 0),
+	DA9052_LDO5_6(5, 25, 600, 1800, 6, 6, DA9052_SUPPLY_VLDO2GO),
+	DA9052_LDO5_6(6, 25, 1725, 3300, 6, 6, DA9052_SUPPLY_VLDO3GO),
+	DA9052_LDO(7, 25, 1725, 3300, 6, 6, 0),
+	DA9052_LDO(8, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(9, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(10, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(11, 50, 1200, 3600, 6, 6, 0),
+	DA9052_LDO(12, 50, 1250, 3650, 6, 6, 0),
+	DA9052_LDO(13, 50, 1200, 3600, 6, 6, 0),
+};
+
+static inline struct da9052_regulator_info *find_regulator_info(u8 chip_id,
+								 int id)
+{
+	struct da9052_regulator_info *info;
+	int i;
+
+	switch (chip_id) {
+	case DA9052:
+		for (i = 0; i < ARRAY_SIZE(da9052_regulator_info); i++) {
+			info = &da9052_regulator_info[i];
+			if (info->reg_desc.id == id)
+				return info;
+		}
+		break;
+	case DA9053_AA:
+	case DA9053_BA:
+	case DA9053_BB:
+		for (i = 0; i < ARRAY_SIZE(da9053_regulator_info); i++) {
+			info = &da9053_regulator_info[i];
+			if (info->reg_desc.id == id)
+				return info;
+		}
+		break;
+	}
+
+	return NULL;
+}
+
+static int __devinit da9052_regulator_probe(struct platform_device *pdev)
+{
+	struct da9052_regulator *regulator;
+	struct da9052 *da9052;
+	struct da9052_pdata *pdata;
+	int ret;
+
+	regulator = devm_kzalloc(&pdev->dev, sizeof(struct da9052_regulator),
+				 GFP_KERNEL);
+	if (!regulator)
+		return -ENOMEM;
+
+	da9052 = dev_get_drvdata(pdev->dev.parent);
+	pdata = da9052->dev->platform_data;
+	regulator->da9052 = da9052;
+
+	regulator->info = find_regulator_info(regulator->da9052->chip_id,
+					      pdev->id);
+	if (regulator->info == NULL) {
+		dev_err(&pdev->dev, "invalid regulator ID specified\n");
+		ret = -EINVAL;
+		goto err;
+	}
+	regulator->rdev = regulator_register(&regulator->info->reg_desc,
+					     &pdev->dev,
+					     pdata->regulators[pdev->id],
+					     regulator, NULL);
+	if (IS_ERR(regulator->rdev)) {
+		dev_err(&pdev->dev, "failed to register regulator %s\n",
+			regulator->info->reg_desc.name);
+		ret = PTR_ERR(regulator->rdev);
+		goto err;
+	}
+
+	platform_set_drvdata(pdev, regulator);
+
+	return 0;
+err:
+	devm_kfree(&pdev->dev, regulator);
+	return ret;
+}
+
+static int __devexit da9052_regulator_remove(struct platform_device *pdev)
+{
+	struct da9052_regulator *regulator = platform_get_drvdata(pdev);
+
+	regulator_unregister(regulator->rdev);
+	devm_kfree(&pdev->dev, regulator);
+
+	return 0;
+}
+
+static struct platform_driver da9052_regulator_driver = {
+	.probe = da9052_regulator_probe,
+	.remove = __devexit_p(da9052_regulator_remove),
+	.driver = {
+		.name = "da9052-regulator",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init da9052_regulator_init(void)
+{
+	return platform_driver_register(&da9052_regulator_driver);
+}
+subsys_initcall(da9052_regulator_init);
+
+static void __exit da9052_regulator_exit(void)
+{
+	platform_driver_unregister(&da9052_regulator_driver);
+}
+module_exit(da9052_regulator_exit);
+
+MODULE_AUTHOR("David Dajun Chen <dchen@diasemi.com>");
+MODULE_DESCRIPTION("Power Regulator driver for Dialog DA9052 PMIC");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:da9052-regulator");
diff --git a/drivers/regulator/db8500-prcmu.c b/drivers/regulator/db8500-prcmu.c
index 78329751af54..515443fcd26b 100644
--- a/drivers/regulator/db8500-prcmu.c
+++ b/drivers/regulator/db8500-prcmu.c
@@ -486,7 +486,7 @@ static int __devinit db8500_regulator_probe(struct platform_device *pdev)
 
 		/* register with the regulator framework */
 		info->rdev = regulator_register(&info->desc, &pdev->dev,
-				init_data, info);
+				init_data, info, NULL);
 		if (IS_ERR(info->rdev)) {
 			err = PTR_ERR(info->rdev);
 			dev_err(&pdev->dev, "failed to register %s: err %i\n",
diff --git a/drivers/regulator/dummy.c b/drivers/regulator/dummy.c
index b8f520513ce7..0ee00de4be72 100644
--- a/drivers/regulator/dummy.c
+++ b/drivers/regulator/dummy.c
@@ -42,7 +42,7 @@ static int __devinit dummy_regulator_probe(struct platform_device *pdev)
 	int ret;
 
 	dummy_regulator_rdev = regulator_register(&dummy_desc, NULL,
-						  &dummy_initdata, NULL);
+						  &dummy_initdata, NULL, NULL);
 	if (IS_ERR(dummy_regulator_rdev)) {
 		ret = PTR_ERR(dummy_regulator_rdev);
 		pr_err("Failed to register regulator: %d\n", ret);
diff --git a/drivers/regulator/fixed.c b/drivers/regulator/fixed.c
index 21ecf212a522..e24e3a174c4b 100644
--- a/drivers/regulator/fixed.c
+++ b/drivers/regulator/fixed.c
@@ -27,6 +27,10 @@
 #include <linux/gpio.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/regulator/machine.h>
 
 struct fixed_voltage_data {
 	struct regulator_desc desc;
@@ -38,6 +42,58 @@ struct fixed_voltage_data {
 	bool is_enabled;
 };
 
+
+/**
+ * of_get_fixed_voltage_config - extract fixed_voltage_config structure info
+ * @dev: device requesting for fixed_voltage_config
+ *
+ * Populates fixed_voltage_config structure by extracting data from device
+ * tree node, returns a pointer to the populated structure of NULL if memory
+ * alloc fails.
+ */
+static struct fixed_voltage_config *
+of_get_fixed_voltage_config(struct device *dev)
+{
+	struct fixed_voltage_config *config;
+	struct device_node *np = dev->of_node;
+	const __be32 *delay;
+	struct regulator_init_data *init_data;
+
+	config = devm_kzalloc(dev, sizeof(struct fixed_voltage_config),
+								 GFP_KERNEL);
+	if (!config)
+		return NULL;
+
+	config->init_data = of_get_regulator_init_data(dev, dev->of_node);
+	if (!config->init_data)
+		return NULL;
+
+	init_data = config->init_data;
+	init_data->constraints.apply_uV = 0;
+
+	config->supply_name = init_data->constraints.name;
+	if (init_data->constraints.min_uV == init_data->constraints.max_uV) {
+		config->microvolts = init_data->constraints.min_uV;
+	} else {
+		dev_err(dev,
+			 "Fixed regulator specified with variable voltages\n");
+		return NULL;
+	}
+
+	if (init_data->constraints.boot_on)
+		config->enabled_at_boot = true;
+
+	config->gpio = of_get_named_gpio(np, "gpio", 0);
+	delay = of_get_property(np, "startup-delay-us", NULL);
+	if (delay)
+		config->startup_delay = be32_to_cpu(*delay);
+
+	if (of_find_property(np, "enable-active-high", NULL))
+		config->enable_high = true;
+
+	return config;
+}
+
 static int fixed_voltage_is_enabled(struct regulator_dev *dev)
 {
 	struct fixed_voltage_data *data = rdev_get_drvdata(dev);
@@ -80,7 +136,10 @@ static int fixed_voltage_get_voltage(struct regulator_dev *dev)
 {
 	struct fixed_voltage_data *data = rdev_get_drvdata(dev);
 
-	return data->microvolts;
+	if (data->microvolts)
+		return data->microvolts;
+	else
+		return -EINVAL;
 }
 
 static int fixed_voltage_list_voltage(struct regulator_dev *dev,
@@ -105,10 +164,18 @@ static struct regulator_ops fixed_voltage_ops = {
 
 static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 {
-	struct fixed_voltage_config *config = pdev->dev.platform_data;
+	struct fixed_voltage_config *config;
 	struct fixed_voltage_data *drvdata;
 	int ret;
 
+	if (pdev->dev.of_node)
+		config = of_get_fixed_voltage_config(&pdev->dev);
+	else
+		config = pdev->dev.platform_data;
+
+	if (!config)
+		return -ENOMEM;
+
 	drvdata = kzalloc(sizeof(struct fixed_voltage_data), GFP_KERNEL);
 	if (drvdata == NULL) {
 		dev_err(&pdev->dev, "Failed to allocate device data\n");
@@ -180,7 +247,8 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 	}
 
 	drvdata->dev = regulator_register(&drvdata->desc, &pdev->dev,
-					  config->init_data, drvdata);
+					  config->init_data, drvdata,
+					  pdev->dev.of_node);
 	if (IS_ERR(drvdata->dev)) {
 		ret = PTR_ERR(drvdata->dev);
 		dev_err(&pdev->dev, "Failed to register regulator: %d\n", ret);
@@ -217,12 +285,23 @@ static int __devexit reg_fixed_voltage_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#if defined(CONFIG_OF)
+static const struct of_device_id fixed_of_match[] __devinitconst = {
+	{ .compatible = "regulator-fixed", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, fixed_of_match);
+#else
+#define fixed_of_match NULL
+#endif
+
 static struct platform_driver regulator_fixed_voltage_driver = {
 	.probe		= reg_fixed_voltage_probe,
 	.remove		= __devexit_p(reg_fixed_voltage_remove),
 	.driver		= {
 		.name		= "reg-fixed-voltage",
 		.owner		= THIS_MODULE,
+		.of_match_table = fixed_of_match,
 	},
 };
 
diff --git a/drivers/regulator/gpio-regulator.c b/drivers/regulator/gpio-regulator.c
index f0acf52498bd..42e1cb1835e5 100644
--- a/drivers/regulator/gpio-regulator.c
+++ b/drivers/regulator/gpio-regulator.c
@@ -284,7 +284,7 @@ static int __devinit gpio_regulator_probe(struct platform_device *pdev)
 	drvdata->state = state;
 
 	drvdata->dev = regulator_register(&drvdata->desc, &pdev->dev,
-					  config->init_data, drvdata);
+					  config->init_data, drvdata, NULL);
 	if (IS_ERR(drvdata->dev)) {
 		ret = PTR_ERR(drvdata->dev);
 		dev_err(&pdev->dev, "Failed to register regulator: %d\n", ret);
diff --git a/drivers/regulator/isl6271a-regulator.c b/drivers/regulator/isl6271a-regulator.c
index e4b3592e8176..c1a456c4257c 100644
--- a/drivers/regulator/isl6271a-regulator.c
+++ b/drivers/regulator/isl6271a-regulator.c
@@ -170,7 +170,7 @@ static int __devinit isl6271a_probe(struct i2c_client *i2c,
 
 	for (i = 0; i < 3; i++) {
 		pmic->rdev[i] = regulator_register(&isl_rd[i], &i2c->dev,
-						init_data, pmic);
+						init_data, pmic, NULL);
 		if (IS_ERR(pmic->rdev[i])) {
 			dev_err(&i2c->dev, "failed to register %s\n", id->name);
 			err = PTR_ERR(pmic->rdev[i]);
diff --git a/drivers/regulator/lp3971.c b/drivers/regulator/lp3971.c
index 72b16b5f3db6..0cfabd318a59 100644
--- a/drivers/regulator/lp3971.c
+++ b/drivers/regulator/lp3971.c
@@ -451,7 +451,7 @@ static int __devinit setup_regulators(struct lp3971 *lp3971,
 	for (i = 0; i < pdata->num_regulators; i++) {
 		struct lp3971_regulator_subdev *reg = &pdata->regulators[i];
 		lp3971->rdev[i] = regulator_register(&regulators[reg->id],
-					lp3971->dev, reg->initdata, lp3971);
+				lp3971->dev, reg->initdata, lp3971, NULL);
 
 		if (IS_ERR(lp3971->rdev[i])) {
 			err = PTR_ERR(lp3971->rdev[i]);
diff --git a/drivers/regulator/lp3972.c b/drivers/regulator/lp3972.c
index fbc5e3741bef..49a15eefe5fe 100644
--- a/drivers/regulator/lp3972.c
+++ b/drivers/regulator/lp3972.c
@@ -555,7 +555,7 @@ static int __devinit setup_regulators(struct lp3972 *lp3972,
 	for (i = 0; i < pdata->num_regulators; i++) {
 		struct lp3972_regulator_subdev *reg = &pdata->regulators[i];
 		lp3972->rdev[i] = regulator_register(&regulators[reg->id],
-					lp3972->dev, reg->initdata, lp3972);
+				lp3972->dev, reg->initdata, lp3972, NULL);
 
 		if (IS_ERR(lp3972->rdev[i])) {
 			err = PTR_ERR(lp3972->rdev[i]);
diff --git a/drivers/regulator/max1586.c b/drivers/regulator/max1586.c
index 3f49512c5134..40e7a4db2853 100644
--- a/drivers/regulator/max1586.c
+++ b/drivers/regulator/max1586.c
@@ -214,7 +214,7 @@ static int __devinit max1586_pmic_probe(struct i2c_client *client,
 		}
 		rdev[i] = regulator_register(&max1586_reg[id], &client->dev,
 					     pdata->subdevs[i].platform_data,
-					     max1586);
+					     max1586, NULL);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(&client->dev, "failed to register %s\n",
diff --git a/drivers/regulator/max8649.c b/drivers/regulator/max8649.c
index 1062cf9f02dc..b06a2399587c 100644
--- a/drivers/regulator/max8649.c
+++ b/drivers/regulator/max8649.c
@@ -16,6 +16,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/slab.h>
 #include <linux/regulator/max8649.h>
+#include <linux/regmap.h>
 
 #define MAX8649_DCDC_VMIN	750000		/* uV */
 #define MAX8649_DCDC_VMAX	1380000		/* uV */
@@ -49,9 +50,8 @@
 
 struct max8649_regulator_info {
 	struct regulator_dev	*regulator;
-	struct i2c_client	*i2c;
 	struct device		*dev;
-	struct mutex		io_lock;
+	struct regmap		*regmap;
 
 	int		vol_reg;
 	unsigned	mode:2;	/* bit[1:0] = VID1, VID0 */
@@ -63,71 +63,6 @@ struct max8649_regulator_info {
 
 /* I2C operations */
 
-static inline int max8649_read_device(struct i2c_client *i2c,
-				      int reg, int bytes, void *dest)
-{
-	unsigned char data;
-	int ret;
-
-	data = (unsigned char)reg;
-	ret = i2c_master_send(i2c, &data, 1);
-	if (ret < 0)
-		return ret;
-	ret = i2c_master_recv(i2c, dest, bytes);
-	if (ret < 0)
-		return ret;
-	return 0;
-}
-
-static inline int max8649_write_device(struct i2c_client *i2c,
-				       int reg, int bytes, void *src)
-{
-	unsigned char buf[bytes + 1];
-	int ret;
-
-	buf[0] = (unsigned char)reg;
-	memcpy(&buf[1], src, bytes);
-
-	ret = i2c_master_send(i2c, buf, bytes + 1);
-	if (ret < 0)
-		return ret;
-	return 0;
-}
-
-static int max8649_reg_read(struct i2c_client *i2c, int reg)
-{
-	struct max8649_regulator_info *info = i2c_get_clientdata(i2c);
-	unsigned char data;
-	int ret;
-
-	mutex_lock(&info->io_lock);
-	ret = max8649_read_device(i2c, reg, 1, &data);
-	mutex_unlock(&info->io_lock);
-
-	if (ret < 0)
-		return ret;
-	return (int)data;
-}
-
-static int max8649_set_bits(struct i2c_client *i2c, int reg,
-			    unsigned char mask, unsigned char data)
-{
-	struct max8649_regulator_info *info = i2c_get_clientdata(i2c);
-	unsigned char value;
-	int ret;
-
-	mutex_lock(&info->io_lock);
-	ret = max8649_read_device(i2c, reg, 1, &value);
-	if (ret < 0)
-		goto out;
-	value &= ~mask;
-	value |= data;
-	ret = max8649_write_device(i2c, reg, 1, &value);
-out:
-	mutex_unlock(&info->io_lock);
-	return ret;
-}
-
 static inline int check_range(int min_uV, int max_uV)
 {
 	if ((min_uV < MAX8649_DCDC_VMIN) || (max_uV > MAX8649_DCDC_VMAX)
@@ -144,13 +79,14 @@ static int max8649_list_voltage(struct regulator_dev *rdev, unsigned index)
 static int max8649_get_voltage(struct regulator_dev *rdev)
 {
 	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
+	unsigned int val;
 	unsigned char data;
 	int ret;
 
-	ret = max8649_reg_read(info->i2c, info->vol_reg);
-	if (ret < 0)
+	ret = regmap_read(info->regmap, info->vol_reg, &val);
+	if (ret != 0)
 		return ret;
-	data = (unsigned char)ret & MAX8649_VOL_MASK;
+	data = (unsigned char)val & MAX8649_VOL_MASK;
 	return max8649_list_voltage(rdev, data);
 }
 
@@ -170,14 +106,14 @@ static int max8649_set_voltage(struct regulator_dev *rdev,
 	mask = MAX8649_VOL_MASK;
 	*selector = data & mask;
 
-	return max8649_set_bits(info->i2c, info->vol_reg, mask, data);
+	return regmap_update_bits(info->regmap, info->vol_reg, mask, data);
 }
 
 /* EN_PD means pulldown on EN input */
 static int max8649_enable(struct regulator_dev *rdev)
 {
 	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
-	return max8649_set_bits(info->i2c, MAX8649_CONTROL, MAX8649_EN_PD, 0);
+	return regmap_update_bits(info->regmap, MAX8649_CONTROL, MAX8649_EN_PD, 0);
 }
 
 /*
@@ -187,38 +123,40 @@ static int max8649_enable(struct regulator_dev *rdev)
 static int max8649_disable(struct regulator_dev *rdev)
 {
 	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
-	return max8649_set_bits(info->i2c, MAX8649_CONTROL, MAX8649_EN_PD,
+	return regmap_update_bits(info->regmap, MAX8649_CONTROL, MAX8649_EN_PD,
 				MAX8649_EN_PD);
 }
 
 static int max8649_is_enabled(struct regulator_dev *rdev)
 {
 	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
+	unsigned int val;
 	int ret;
 
-	ret = max8649_reg_read(info->i2c, MAX8649_CONTROL);
-	if (ret < 0)
+	ret = regmap_read(info->regmap, MAX8649_CONTROL, &val);
+	if (ret != 0)
 		return ret;
-	return !((unsigned char)ret & MAX8649_EN_PD);
+	return !((unsigned char)val & MAX8649_EN_PD);
 }
 
 static int max8649_enable_time(struct regulator_dev *rdev)
 {
 	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
 	int voltage, rate, ret;
+	unsigned int val;
 
 	/* get voltage */
-	ret = max8649_reg_read(info->i2c, info->vol_reg);
-	if (ret < 0)
+	ret = regmap_read(info->regmap, info->vol_reg, &val);
+	if (ret != 0)
 		return ret;
-	ret &= MAX8649_VOL_MASK;
+	val &= MAX8649_VOL_MASK;
 	voltage = max8649_list_voltage(rdev, (unsigned char)ret); /* uV */
 
 	/* get rate */
-	ret = max8649_reg_read(info->i2c, MAX8649_RAMP);
-	if (ret < 0)
+	ret = regmap_read(info->regmap, MAX8649_RAMP, &val);
+	if (ret != 0)
 		return ret;
-	ret = (ret & MAX8649_RAMP_MASK) >> 5;
+	ret = (val & MAX8649_RAMP_MASK) >> 5;
 	rate = (32 * 1000) >> ret;	/* uV/uS */
 
 	return DIV_ROUND_UP(voltage, rate);
@@ -230,12 +168,12 @@ static int max8649_set_mode(struct regulator_dev *rdev, unsigned int mode)
 
 	switch (mode) {
 	case REGULATOR_MODE_FAST:
-		max8649_set_bits(info->i2c, info->vol_reg, MAX8649_FORCE_PWM,
-				 MAX8649_FORCE_PWM);
+		regmap_update_bits(info->regmap, info->vol_reg, MAX8649_FORCE_PWM,
+				   MAX8649_FORCE_PWM);
 		break;
 	case REGULATOR_MODE_NORMAL:
-		max8649_set_bits(info->i2c, info->vol_reg,
-				 MAX8649_FORCE_PWM, 0);
+		regmap_update_bits(info->regmap, info->vol_reg,
+				   MAX8649_FORCE_PWM, 0);
 		break;
 	default:
 		return -EINVAL;
@@ -246,10 +184,13 @@ static int max8649_set_mode(struct regulator_dev *rdev, unsigned int mode)
 static unsigned int max8649_get_mode(struct regulator_dev *rdev)
 {
 	struct max8649_regulator_info *info = rdev_get_drvdata(rdev);
+	unsigned int val;
 	int ret;
 
-	ret = max8649_reg_read(info->i2c, info->vol_reg);
-	if (ret & MAX8649_FORCE_PWM)
+	ret = regmap_read(info->regmap, info->vol_reg, &val);
+	if (ret != 0)
+		return ret;
+	if (val & MAX8649_FORCE_PWM)
 		return REGULATOR_MODE_FAST;
 	return REGULATOR_MODE_NORMAL;
 }
@@ -275,11 +216,17 @@ static struct regulator_desc dcdc_desc = {
 	.owner		= THIS_MODULE,
 };
 
+static struct regmap_config max8649_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
 static int __devinit max8649_regulator_probe(struct i2c_client *client,
 					     const struct i2c_device_id *id)
 {
 	struct max8649_platform_data *pdata = client->dev.platform_data;
 	struct max8649_regulator_info *info = NULL;
+	unsigned int val;
 	unsigned char data;
 	int ret;
 
@@ -289,9 +236,14 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 		return -ENOMEM;
 	}
 
-	info->i2c = client;
+	info->regmap = regmap_init_i2c(client, &max8649_regmap_config);
+	if (IS_ERR(info->regmap)) {
+		ret = PTR_ERR(info->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n", ret);
+		goto fail;
+	}
+
 	info->dev = &client->dev;
-	mutex_init(&info->io_lock);
 	i2c_set_clientdata(client, info);
 
 	info->mode = pdata->mode;
@@ -312,8 +264,8 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 		break;
 	}
 
-	ret = max8649_reg_read(info->i2c, MAX8649_CHIP_ID1);
-	if (ret < 0) {
+	ret = regmap_read(info->regmap, MAX8649_CHIP_ID1, &val);
+	if (ret != 0) {
 		dev_err(info->dev, "Failed to detect ID of MAX8649:%d\n",
 			ret);
 		goto out;
@@ -321,33 +273,33 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 	dev_info(info->dev, "Detected MAX8649 (ID:%x)\n", ret);
 
 	/* enable VID0 & VID1 */
-	max8649_set_bits(info->i2c, MAX8649_CONTROL, MAX8649_VID_MASK, 0);
+	regmap_update_bits(info->regmap, MAX8649_CONTROL, MAX8649_VID_MASK, 0);
 
 	/* enable/disable external clock synchronization */
 	info->extclk = pdata->extclk;
 	data = (info->extclk) ? MAX8649_SYNC_EXTCLK : 0;
-	max8649_set_bits(info->i2c, info->vol_reg, MAX8649_SYNC_EXTCLK, data);
+	regmap_update_bits(info->regmap, info->vol_reg, MAX8649_SYNC_EXTCLK, data);
 	if (info->extclk) {
 		/* set external clock frequency */
 		info->extclk_freq = pdata->extclk_freq;
-		max8649_set_bits(info->i2c, MAX8649_SYNC, MAX8649_EXT_MASK,
-				 info->extclk_freq << 6);
+		regmap_update_bits(info->regmap, MAX8649_SYNC, MAX8649_EXT_MASK,
+				   info->extclk_freq << 6);
 	}
 
 	if (pdata->ramp_timing) {
 		info->ramp_timing = pdata->ramp_timing;
-		max8649_set_bits(info->i2c, MAX8649_RAMP, MAX8649_RAMP_MASK,
-				 info->ramp_timing << 5);
+		regmap_update_bits(info->regmap, MAX8649_RAMP, MAX8649_RAMP_MASK,
+				   info->ramp_timing << 5);
 	}
 
 	info->ramp_down = pdata->ramp_down;
 	if (info->ramp_down) {
-		max8649_set_bits(info->i2c, MAX8649_RAMP, MAX8649_RAMP_DOWN,
-				 MAX8649_RAMP_DOWN);
+		regmap_update_bits(info->regmap, MAX8649_RAMP, MAX8649_RAMP_DOWN,
+				   MAX8649_RAMP_DOWN);
 	}
 
 	info->regulator = regulator_register(&dcdc_desc, &client->dev,
-					     pdata->regulator, info);
+					     pdata->regulator, info, NULL);
 	if (IS_ERR(info->regulator)) {
 		dev_err(info->dev, "failed to register regulator %s\n",
 			dcdc_desc.name);
@@ -358,6 +310,8 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
 	dev_info(info->dev, "Max8649 regulator device is detected.\n");
 	return 0;
 out:
+	regmap_exit(info->regmap);
+fail:
 	kfree(info);
 	return ret;
 }
@@ -369,6 +323,7 @@ static int __devexit max8649_regulator_remove(struct i2c_client *client)
 	if (info) {
 		if (info->regulator)
 			regulator_unregister(info->regulator);
+		regmap_exit(info->regmap);
 		kfree(info);
 	}
 
diff --git a/drivers/regulator/max8660.c b/drivers/regulator/max8660.c
index 33f5d9a492ef..a838e664569f 100644
--- a/drivers/regulator/max8660.c
+++ b/drivers/regulator/max8660.c
@@ -449,7 +449,7 @@ static int __devinit max8660_probe(struct i2c_client *client,
 
 		rdev[i] = regulator_register(&max8660_reg[id], &client->dev,
 					     pdata->subdevs[i].platform_data,
-					     max8660);
+					     max8660, NULL);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(&client->dev, "failed to register %s\n",
diff --git a/drivers/regulator/max8925-regulator.c b/drivers/regulator/max8925-regulator.c
index cc9ec0e03271..cc290d37c463 100644
--- a/drivers/regulator/max8925-regulator.c
+++ b/drivers/regulator/max8925-regulator.c
@@ -24,9 +24,13 @@
 #define SD1_DVM_SHIFT		5		/* SDCTL1 bit5 */
 #define SD1_DVM_EN		6		/* SDV1 bit 6 */
 
-/* bit definitions in SD & LDO control registers */
-#define OUT_ENABLE   		0x1f		/* Power U/D sequence as I2C */
-#define OUT_DISABLE		0x1e		/* Power U/D sequence as I2C */
+/* bit definitions in LDO control registers */
+#define LDO_SEQ_I2C		0x7		/* Power U/D by i2c */
+#define LDO_SEQ_MASK		0x7		/* Power U/D sequence mask */
+#define LDO_SEQ_SHIFT		2		/* Power U/D sequence offset */
+#define LDO_I2C_EN		0x1		/* Enable by i2c */
+#define LDO_I2C_EN_MASK		0x1		/* Enable mask by i2c */
+#define LDO_I2C_EN_SHIFT	0		/* Enable offset by i2c */
 
 struct max8925_regulator_info {
 	struct regulator_desc	desc;
@@ -40,7 +44,6 @@ struct max8925_regulator_info {
 	int	vol_reg;
 	int	vol_shift;
 	int	vol_nbits;
-	int	enable_bit;
 	int	enable_reg;
 };
 
@@ -98,8 +101,10 @@ static int max8925_enable(struct regulator_dev *rdev)
 	struct max8925_regulator_info *info = rdev_get_drvdata(rdev);
 
 	return max8925_set_bits(info->i2c, info->enable_reg,
-				OUT_ENABLE << info->enable_bit,
-				OUT_ENABLE << info->enable_bit);
+				LDO_SEQ_MASK << LDO_SEQ_SHIFT |
+				LDO_I2C_EN_MASK << LDO_I2C_EN_SHIFT,
+				LDO_SEQ_I2C << LDO_SEQ_SHIFT |
+				LDO_I2C_EN << LDO_I2C_EN_SHIFT);
 }
 
 static int max8925_disable(struct regulator_dev *rdev)
@@ -107,20 +112,24 @@ static int max8925_disable(struct regulator_dev *rdev)
 	struct max8925_regulator_info *info = rdev_get_drvdata(rdev);
 
 	return max8925_set_bits(info->i2c, info->enable_reg,
-				OUT_ENABLE << info->enable_bit,
-				OUT_DISABLE << info->enable_bit);
+				LDO_SEQ_MASK << LDO_SEQ_SHIFT |
+				LDO_I2C_EN_MASK << LDO_I2C_EN_SHIFT,
+				LDO_SEQ_I2C << LDO_SEQ_SHIFT);
 }
 
 static int max8925_is_enabled(struct regulator_dev *rdev)
 {
 	struct max8925_regulator_info *info = rdev_get_drvdata(rdev);
-	int ret;
+	int ldo_seq, ret;
 
 	ret = max8925_reg_read(info->i2c, info->enable_reg);
 	if (ret < 0)
 		return ret;
-
-	return ret & (1 << info->enable_bit);
+	ldo_seq = (ret >> LDO_SEQ_SHIFT) & LDO_SEQ_MASK;
+	if (ldo_seq != LDO_SEQ_I2C)
+		return 1;
+	else
+		return ret & (LDO_I2C_EN_MASK << LDO_I2C_EN_SHIFT);
 }
 
 static int max8925_set_dvm_voltage(struct regulator_dev *rdev, int uV)
@@ -188,7 +197,6 @@ static struct regulator_ops max8925_regulator_ldo_ops = {
 	.vol_shift	= 0,					\
 	.vol_nbits	= 6,					\
 	.enable_reg	= MAX8925_SDCTL##_id,			\
-	.enable_bit	= 0,					\
 }
 
 #define MAX8925_LDO(_id, min, max, step)			\
@@ -207,7 +215,6 @@ static struct regulator_ops max8925_regulator_ldo_ops = {
 	.vol_shift	= 0,					\
 	.vol_nbits	= 6,					\
 	.enable_reg	= MAX8925_LDOCTL##_id,			\
-	.enable_bit	= 0,					\
 }
 
 static struct max8925_regulator_info max8925_regulator_info[] = {
@@ -266,7 +273,7 @@ static int __devinit max8925_regulator_probe(struct platform_device *pdev)
 	ri->chip = chip;
 
 	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdata->regulator[pdev->id], ri);
+				  pdata->regulator[pdev->id], ri, NULL);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 				ri->desc.name);
diff --git a/drivers/regulator/max8952.c b/drivers/regulator/max8952.c
index 3883d85c5b88..75d89400c123 100644
--- a/drivers/regulator/max8952.c
+++ b/drivers/regulator/max8952.c
@@ -208,7 +208,7 @@ static int __devinit max8952_pmic_probe(struct i2c_client *client,
 	max8952->pdata = pdata;
 
 	max8952->rdev = regulator_register(&regulator, max8952->dev,
-			&pdata->reg_data, max8952);
+			&pdata->reg_data, max8952, NULL);
 
 	if (IS_ERR(max8952->rdev)) {
 		ret = PTR_ERR(max8952->rdev);
diff --git a/drivers/regulator/max8997.c b/drivers/regulator/max8997.c
index 6176129a27e5..d26e8646277b 100644
--- a/drivers/regulator/max8997.c
+++ b/drivers/regulator/max8997.c
@@ -1146,7 +1146,7 @@ static __devinit int max8997_pmic_probe(struct platform_device *pdev)
 			regulators[id].n_voltages = 16;
 
 		rdev[i] = regulator_register(&regulators[id], max8997->dev,
-				pdata->regulators[i].initdata, max8997);
+				pdata->regulators[i].initdata, max8997, NULL);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(max8997->dev, "regulator init failed for %d\n",
diff --git a/drivers/regulator/max8998.c b/drivers/regulator/max8998.c
index 41a1495eec2b..2d38c2493a07 100644
--- a/drivers/regulator/max8998.c
+++ b/drivers/regulator/max8998.c
@@ -847,7 +847,7 @@ static __devinit int max8998_pmic_probe(struct platform_device *pdev)
 			regulators[index].n_voltages = count;
 		}
 		rdev[i] = regulator_register(&regulators[index], max8998->dev,
-				pdata->regulators[i].initdata, max8998);
+				pdata->regulators[i].initdata, max8998, NULL);
 		if (IS_ERR(rdev[i])) {
 			ret = PTR_ERR(rdev[i]);
 			dev_err(max8998->dev, "regulator init failed\n");
diff --git a/drivers/regulator/mc13783-regulator.c b/drivers/regulator/mc13783-regulator.c
index 8479082e1aea..8e9b90ad88ae 100644
--- a/drivers/regulator/mc13783-regulator.c
+++ b/drivers/regulator/mc13783-regulator.c
@@ -344,7 +344,7 @@ static int __devinit mc13783_regulator_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "%s id %d\n", __func__, pdev->id);
 
-	priv = kzalloc(sizeof(*priv) +
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv) +
 			pdata->num_regulators * sizeof(priv->regulators[0]),
 			GFP_KERNEL);
 	if (!priv)
@@ -357,7 +357,7 @@ static int __devinit mc13783_regulator_probe(struct platform_device *pdev)
 		init_data = &pdata->regulators[i];
 		priv->regulators[i] = regulator_register(
 				&mc13783_regulators[init_data->id].desc,
-				&pdev->dev, init_data->init_data, priv);
+				&pdev->dev, init_data->init_data, priv, NULL);
 
 		if (IS_ERR(priv->regulators[i])) {
 			dev_err(&pdev->dev, "failed to register regulator %s\n",
@@ -374,8 +374,6 @@ err:
 	while (--i >= 0)
 		regulator_unregister(priv->regulators[i]);
 
-	kfree(priv);
-
 	return ret;
 }
 
@@ -391,7 +389,6 @@ static int __devexit mc13783_regulator_remove(struct platform_device *pdev)
 	for (i = 0; i < pdata->num_regulators; i++)
 		regulator_unregister(priv->regulators[i]);
 
-	kfree(priv);
 	return 0;
 }
 
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index 023d17d022cf..e8cfc99dd8f0 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -527,18 +527,27 @@ static int __devinit mc13892_regulator_probe(struct platform_device *pdev)
 	struct mc13xxx *mc13892 = dev_get_drvdata(pdev->dev.parent);
 	struct mc13xxx_regulator_platform_data *pdata =
 		dev_get_platdata(&pdev->dev);
-	struct mc13xxx_regulator_init_data *init_data;
+	struct mc13xxx_regulator_init_data *mc13xxx_data;
 	int i, ret;
+	int num_regulators = 0;
 	u32 val;
 
-	priv = kzalloc(sizeof(*priv) +
-		pdata->num_regulators * sizeof(priv->regulators[0]),
+	num_regulators = mc13xxx_get_num_regulators_dt(pdev);
+	if (num_regulators <= 0 && pdata)
+		num_regulators = pdata->num_regulators;
+	if (num_regulators <= 0)
+		return -EINVAL;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv) +
+		num_regulators * sizeof(priv->regulators[0]),
 		GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
+	priv->num_regulators = num_regulators;
 	priv->mc13xxx_regulators = mc13892_regulators;
 	priv->mc13xxx = mc13892;
+	platform_set_drvdata(pdev, priv);
 
 	mc13xxx_lock(mc13892);
 	ret = mc13xxx_reg_read(mc13892, MC13892_REVISION, &val);
@@ -569,11 +578,27 @@ static int __devinit mc13892_regulator_probe(struct platform_device *pdev)
 		= mc13892_vcam_set_mode;
 	mc13892_regulators[MC13892_VCAM].desc.ops->get_mode
 		= mc13892_vcam_get_mode;
-	for (i = 0; i < pdata->num_regulators; i++) {
-		init_data = &pdata->regulators[i];
+
+	mc13xxx_data = mc13xxx_parse_regulators_dt(pdev, mc13892_regulators,
+					ARRAY_SIZE(mc13892_regulators));
+	for (i = 0; i < num_regulators; i++) {
+		struct regulator_init_data *init_data;
+		struct regulator_desc *desc;
+		struct device_node *node = NULL;
+		int id;
+
+		if (mc13xxx_data) {
+			id = mc13xxx_data[i].id;
+			init_data = mc13xxx_data[i].init_data;
+			node = mc13xxx_data[i].node;
+		} else {
+			id = pdata->regulators[i].id;
+			init_data = pdata->regulators[i].init_data;
+		}
+		desc = &mc13892_regulators[id].desc;
+
 		priv->regulators[i] = regulator_register(
-			&mc13892_regulators[init_data->id].desc,
-			&pdev->dev, init_data->init_data, priv);
+			desc, &pdev->dev, init_data, priv, node);
 
 		if (IS_ERR(priv->regulators[i])) {
 			dev_err(&pdev->dev, "failed to register regulator %s\n",
@@ -583,8 +608,6 @@ static int __devinit mc13892_regulator_probe(struct platform_device *pdev)
 		}
 	}
 
-	platform_set_drvdata(pdev, priv);
-
 	return 0;
 err:
 	while (--i >= 0)
@@ -592,7 +615,6 @@ err:
 
 err_free:
 	mc13xxx_unlock(mc13892);
-	kfree(priv);
 
 	return ret;
 }
@@ -600,16 +622,13 @@ err_free:
 static int __devexit mc13892_regulator_remove(struct platform_device *pdev)
 {
 	struct mc13xxx_regulator_priv *priv = platform_get_drvdata(pdev);
-	struct mc13xxx_regulator_platform_data *pdata =
-		dev_get_platdata(&pdev->dev);
 	int i;
 
 	platform_set_drvdata(pdev, NULL);
 
-	for (i = 0; i < pdata->num_regulators; i++)
+	for (i = 0; i < priv->num_regulators; i++)
 		regulator_unregister(priv->regulators[i]);
 
-	kfree(priv);
 	return 0;
 }
 
diff --git a/drivers/regulator/mc13xxx-regulator-core.c b/drivers/regulator/mc13xxx-regulator-core.c
index 6532853a6ef5..80ecafef1bc3 100644
--- a/drivers/regulator/mc13xxx-regulator-core.c
+++ b/drivers/regulator/mc13xxx-regulator-core.c
@@ -18,12 +18,14 @@
 #include <linux/mfd/mc13xxx.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/platform_device.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/err.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include "mc13xxx.h"
 
 static int mc13xxx_regulator_enable(struct regulator_dev *rdev)
@@ -236,6 +238,61 @@ int mc13xxx_sw_regulator_is_enabled(struct regulator_dev *rdev)
 }
 EXPORT_SYMBOL_GPL(mc13xxx_sw_regulator_is_enabled);
 
+#ifdef CONFIG_OF
+int __devinit mc13xxx_get_num_regulators_dt(struct platform_device *pdev)
+{
+	struct device_node *parent, *child;
+	int num = 0;
+
+	of_node_get(pdev->dev.parent->of_node);
+	parent = of_find_node_by_name(pdev->dev.parent->of_node, "regulators");
+	if (!parent)
+		return -ENODEV;
+
+	for_each_child_of_node(parent, child)
+		num++;
+
+	return num;
+}
+
+struct mc13xxx_regulator_init_data * __devinit mc13xxx_parse_regulators_dt(
+	struct platform_device *pdev, struct mc13xxx_regulator *regulators,
+	int num_regulators)
+{
+	struct mc13xxx_regulator_priv *priv = platform_get_drvdata(pdev);
+	struct mc13xxx_regulator_init_data *data, *p;
+	struct device_node *parent, *child;
+	int i;
+
+	of_node_get(pdev->dev.parent->of_node);
+	parent = of_find_node_by_name(pdev->dev.parent->of_node, "regulators");
+	if (!parent)
+		return NULL;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data) * priv->num_regulators,
+			    GFP_KERNEL);
+	if (!data)
+		return NULL;
+	p = data;
+
+	for_each_child_of_node(parent, child) {
+		for (i = 0; i < num_regulators; i++) {
+			if (!of_node_cmp(child->name,
+					 regulators[i].desc.name)) {
+				p->id = i;
+				p->init_data = of_get_regulator_init_data(
+							&pdev->dev, child);
+				p->node = child;
+				p++;
+				break;
+			}
+		}
+	}
+
+	return data;
+}
+#endif
+
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Yong Shen <yong.shen@linaro.org>");
 MODULE_DESCRIPTION("Regulator Driver for Freescale MC13xxx PMIC");
diff --git a/drivers/regulator/mc13xxx.h b/drivers/regulator/mc13xxx.h
index 27758267e122..b3961c658b05 100644
--- a/drivers/regulator/mc13xxx.h
+++ b/drivers/regulator/mc13xxx.h
@@ -29,6 +29,7 @@ struct mc13xxx_regulator_priv {
 	struct mc13xxx *mc13xxx;
 	u32 powermisc_pwgt_state;
 	struct mc13xxx_regulator *mc13xxx_regulators;
+	int num_regulators;
 	struct regulator_dev *regulators[];
 };
 
@@ -42,13 +43,32 @@ extern int mc13xxx_fixed_regulator_set_voltage(struct regulator_dev *rdev,
 		int min_uV, int max_uV, unsigned *selector);
 extern int mc13xxx_fixed_regulator_get_voltage(struct regulator_dev *rdev);
 
+#ifdef CONFIG_OF
+extern int mc13xxx_get_num_regulators_dt(struct platform_device *pdev);
+extern struct mc13xxx_regulator_init_data *mc13xxx_parse_regulators_dt(
+	struct platform_device *pdev, struct mc13xxx_regulator *regulators,
+	int num_regulators);
+#else
+static inline int mc13xxx_get_num_regulators_dt(struct platform_device *pdev)
+{
+	return -ENODEV;
+}
+
+static inline struct mc13xxx_regulator_init_data *mc13xxx_parse_regulators_dt(
+	struct platform_device *pdev, struct mc13xxx_regulator *regulators,
+	int num_regulators)
+{
+	return NULL;
+}
+#endif
+
 extern struct regulator_ops mc13xxx_regulator_ops;
 extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 
 #define MC13xxx_DEFINE(prefix, _name, _reg, _vsel_reg, _voltages, _ops)	\
 	[prefix ## _name] = {				\
 		.desc = {						\
-			.name = #prefix "_" #_name,			\
+			.name = #_name,					\
 			.n_voltages = ARRAY_SIZE(_voltages),		\
 			.ops = &_ops,			\
 			.type = REGULATOR_VOLTAGE,			\
@@ -66,7 +86,7 @@ extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 #define MC13xxx_FIXED_DEFINE(prefix, _name, _reg, _voltages, _ops)	\
 	[prefix ## _name] = {				\
 		.desc = {						\
-			.name = #prefix "_" #_name,			\
+			.name = #_name,					\
 			.n_voltages = ARRAY_SIZE(_voltages),		\
 			.ops = &_ops,		\
 			.type = REGULATOR_VOLTAGE,			\
@@ -81,7 +101,7 @@ extern struct regulator_ops mc13xxx_fixed_regulator_ops;
 #define MC13xxx_GPO_DEFINE(prefix, _name, _reg,  _voltages, _ops)	\
 	[prefix ## _name] = {				\
 		.desc = {						\
-			.name = #prefix "_" #_name,			\
+			.name = #_name,					\
 			.n_voltages = ARRAY_SIZE(_voltages),		\
 			.ops = &_ops,		\
 			.type = REGULATOR_VOLTAGE,			\
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
new file mode 100644
index 000000000000..f1651eb69648
--- /dev/null
+++ b/drivers/regulator/of_regulator.c
@@ -0,0 +1,87 @@
+/*
+ * OF helpers for regulator framework
+ *
+ * Copyright (C) 2011 Texas Instruments, Inc.
+ * Rajendra Nayak <rnayak@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/regulator/machine.h>
+
+static void of_get_regulation_constraints(struct device_node *np,
+					struct regulator_init_data **init_data)
+{
+	const __be32 *min_uV, *max_uV, *uV_offset;
+	const __be32 *min_uA, *max_uA;
+	struct regulation_constraints *constraints = &(*init_data)->constraints;
+
+	constraints->name = of_get_property(np, "regulator-name", NULL);
+
+	min_uV = of_get_property(np, "regulator-min-microvolt", NULL);
+	if (min_uV)
+		constraints->min_uV = be32_to_cpu(*min_uV);
+	max_uV = of_get_property(np, "regulator-max-microvolt", NULL);
+	if (max_uV)
+		constraints->max_uV = be32_to_cpu(*max_uV);
+
+	/* Voltage change possible? */
+	if (constraints->min_uV != constraints->max_uV)
+		constraints->valid_ops_mask |= REGULATOR_CHANGE_VOLTAGE;
+	/* Only one voltage?  Then make sure it's set. */
+	if (constraints->min_uV == constraints->max_uV)
+		constraints->apply_uV = true;
+
+	uV_offset = of_get_property(np, "regulator-microvolt-offset", NULL);
+	if (uV_offset)
+		constraints->uV_offset = be32_to_cpu(*uV_offset);
+	min_uA = of_get_property(np, "regulator-min-microamp", NULL);
+	if (min_uA)
+		constraints->min_uA = be32_to_cpu(*min_uA);
+	max_uA = of_get_property(np, "regulator-max-microamp", NULL);
+	if (max_uA)
+		constraints->max_uA = be32_to_cpu(*max_uA);
+
+	/* Current change possible? */
+	if (constraints->min_uA != constraints->max_uA)
+		constraints->valid_ops_mask |= REGULATOR_CHANGE_CURRENT;
+
+	if (of_find_property(np, "regulator-boot-on", NULL))
+		constraints->boot_on = true;
+
+	if (of_find_property(np, "regulator-always-on", NULL))
+		constraints->always_on = true;
+	else /* status change should be possible if not always on. */
+		constraints->valid_ops_mask |= REGULATOR_CHANGE_STATUS;
+}
+
+/**
+ * of_get_regulator_init_data - extract regulator_init_data structure info
+ * @dev: device requesting for regulator_init_data
+ *
+ * Populates regulator_init_data structure by extracting data from device
+ * tree node, returns a pointer to the populated struture or NULL if memory
+ * alloc fails.
+ */
+struct regulator_init_data *of_get_regulator_init_data(struct device *dev,
+						struct device_node *node)
+{
+	struct regulator_init_data *init_data;
+
+	if (!node)
+		return NULL;
+
+	init_data = devm_kzalloc(dev, sizeof(*init_data), GFP_KERNEL);
+	if (!init_data)
+		return NULL; /* Out of memory? */
+
+	of_get_regulation_constraints(node, &init_data);
+	return init_data;
+}
+EXPORT_SYMBOL_GPL(of_get_regulator_init_data);
diff --git a/drivers/regulator/pcap-regulator.c b/drivers/regulator/pcap-regulator.c
index 31f6e11a7f16..a5aab1b08bcf 100644
--- a/drivers/regulator/pcap-regulator.c
+++ b/drivers/regulator/pcap-regulator.c
@@ -277,7 +277,7 @@ static int __devinit pcap_regulator_probe(struct platform_device *pdev)
 	void *pcap = dev_get_drvdata(pdev->dev.parent);
 
 	rdev = regulator_register(&pcap_regulators[pdev->id], &pdev->dev,
-				pdev->dev.platform_data, pcap);
+				pdev->dev.platform_data, pcap, NULL);
 	if (IS_ERR(rdev))
 		return PTR_ERR(rdev);
 
diff --git a/drivers/regulator/pcf50633-regulator.c b/drivers/regulator/pcf50633-regulator.c
index 69a11d9dd87f..1d1c31056297 100644
--- a/drivers/regulator/pcf50633-regulator.c
+++ b/drivers/regulator/pcf50633-regulator.c
@@ -320,7 +320,7 @@ static int __devinit pcf50633_regulator_probe(struct platform_device *pdev)
 	pcf = dev_to_pcf50633(pdev->dev.parent);
 
 	rdev = regulator_register(&regulators[pdev->id], &pdev->dev,
-				  pdev->dev.platform_data, pcf);
+				  pdev->dev.platform_data, pcf, NULL);
 	if (IS_ERR(rdev))
 		return PTR_ERR(rdev);
 
diff --git a/drivers/regulator/tps6105x-regulator.c b/drivers/regulator/tps6105x-regulator.c
index 1011873896dc..d9278da18a9e 100644
--- a/drivers/regulator/tps6105x-regulator.c
+++ b/drivers/regulator/tps6105x-regulator.c
@@ -151,7 +151,8 @@ static int __devinit tps6105x_regulator_probe(struct platform_device *pdev)
 	/* Register regulator with framework */
 	tps6105x->regulator = regulator_register(&tps6105x_regulator_desc,
 					     &tps6105x->client->dev,
-					     pdata->regulator_data, tps6105x);
+					     pdata->regulator_data, tps6105x,
+					     NULL);
 	if (IS_ERR(tps6105x->regulator)) {
 		ret = PTR_ERR(tps6105x->regulator);
 		dev_err(&tps6105x->client->dev,
diff --git a/drivers/regulator/tps65023-regulator.c b/drivers/regulator/tps65023-regulator.c
index 9fb4c7b81753..18d61a0529a9 100644
--- a/drivers/regulator/tps65023-regulator.c
+++ b/drivers/regulator/tps65023-regulator.c
@@ -152,48 +152,21 @@ struct tps_driver_data {
 	u8 core_regulator;
 };
 
-static int tps_65023_set_bits(struct tps_pmic *tps, u8 reg, u8 mask)
-{
-	return regmap_update_bits(tps->regmap, reg, mask, mask);
-}
-
-static int tps_65023_clear_bits(struct tps_pmic *tps, u8 reg, u8 mask)
-{
-	return regmap_update_bits(tps->regmap, reg, mask, 0);
-}
-
-static int tps_65023_reg_read(struct tps_pmic *tps, u8 reg)
-{
-	unsigned int val;
-	int ret;
-
-	ret = regmap_read(tps->regmap, reg, &val);
-
-	if (ret != 0)
-		return ret;
-	else
-		return val;
-}
-
-static int tps_65023_reg_write(struct tps_pmic *tps, u8 reg, u8 val)
-{
-	return regmap_write(tps->regmap, reg, val);
-}
-
 static int tps65023_dcdc_is_enabled(struct regulator_dev *dev)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
 	int data, dcdc = rdev_get_id(dev);
+	int ret;
 	u8 shift;
 
 	if (dcdc < TPS65023_DCDC_1 || dcdc > TPS65023_DCDC_3)
 		return -EINVAL;
 
 	shift = TPS65023_NUM_REGULATOR - dcdc;
-	data = tps_65023_reg_read(tps, TPS65023_REG_REG_CTRL);
+	ret = regmap_read(tps->regmap, TPS65023_REG_REG_CTRL, &data);
 
-	if (data < 0)
-		return data;
+	if (ret != 0)
+		return ret;
 	else
 		return (data & 1<<shift) ? 1 : 0;
 }
@@ -202,16 +175,17 @@ static int tps65023_ldo_is_enabled(struct regulator_dev *dev)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
 	int data, ldo = rdev_get_id(dev);
+	int ret;
 	u8 shift;
 
 	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
 		return -EINVAL;
 
 	shift = (ldo == TPS65023_LDO_1 ? 1 : 2);
-	data = tps_65023_reg_read(tps, TPS65023_REG_REG_CTRL);
+	ret = regmap_read(tps->regmap, TPS65023_REG_REG_CTRL, &data);
 
-	if (data < 0)
-		return data;
+	if (ret != 0)
+		return ret;
 	else
 		return (data & 1<<shift) ? 1 : 0;
 }
@@ -226,7 +200,7 @@ static int tps65023_dcdc_enable(struct regulator_dev *dev)
 		return -EINVAL;
 
 	shift = TPS65023_NUM_REGULATOR - dcdc;
-	return tps_65023_set_bits(tps, TPS65023_REG_REG_CTRL, 1 << shift);
+	return regmap_update_bits(tps->regmap, TPS65023_REG_REG_CTRL, 1 << shift, 1 << shift);
 }
 
 static int tps65023_dcdc_disable(struct regulator_dev *dev)
@@ -239,7 +213,7 @@ static int tps65023_dcdc_disable(struct regulator_dev *dev)
 		return -EINVAL;
 
 	shift = TPS65023_NUM_REGULATOR - dcdc;
-	return tps_65023_clear_bits(tps, TPS65023_REG_REG_CTRL, 1 << shift);
+	return regmap_update_bits(tps->regmap, TPS65023_REG_REG_CTRL, 1 << shift, 0);
 }
 
 static int tps65023_ldo_enable(struct regulator_dev *dev)
@@ -252,7 +226,7 @@ static int tps65023_ldo_enable(struct regulator_dev *dev)
 		return -EINVAL;
 
 	shift = (ldo == TPS65023_LDO_1 ? 1 : 2);
-	return tps_65023_set_bits(tps, TPS65023_REG_REG_CTRL, 1 << shift);
+	return regmap_update_bits(tps->regmap, TPS65023_REG_REG_CTRL, 1 << shift, 1 << shift);
 }
 
 static int tps65023_ldo_disable(struct regulator_dev *dev)
@@ -265,21 +239,22 @@ static int tps65023_ldo_disable(struct regulator_dev *dev)
 		return -EINVAL;
 
 	shift = (ldo == TPS65023_LDO_1 ? 1 : 2);
-	return tps_65023_clear_bits(tps, TPS65023_REG_REG_CTRL, 1 << shift);
+	return regmap_update_bits(tps->regmap, TPS65023_REG_REG_CTRL, 1 << shift, 0);
 }
 
 static int tps65023_dcdc_get_voltage(struct regulator_dev *dev)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
+	int ret;
 	int data, dcdc = rdev_get_id(dev);
 
 	if (dcdc < TPS65023_DCDC_1 || dcdc > TPS65023_DCDC_3)
 		return -EINVAL;
 
 	if (dcdc == tps->core_regulator) {
-		data = tps_65023_reg_read(tps, TPS65023_REG_DEF_CORE);
-		if (data < 0)
-			return data;
+		ret = regmap_read(tps->regmap, TPS65023_REG_DEF_CORE, &data);
+		if (ret != 0)
+			return ret;
 		data &= (tps->info[dcdc]->table_len - 1);
 		return tps->info[dcdc]->table[data] * 1000;
 	} else
@@ -318,13 +293,13 @@ static int tps65023_dcdc_set_voltage(struct regulator_dev *dev,
 	if (vsel == tps->info[dcdc]->table_len)
 		goto failed;
 
-	ret = tps_65023_reg_write(tps, TPS65023_REG_DEF_CORE, vsel);
+	ret = regmap_write(tps->regmap, TPS65023_REG_DEF_CORE, vsel);
 
 	/* Tell the chip that we have changed the value in DEFCORE
 	 * and its time to update the core voltage
 	 */
-	tps_65023_set_bits(tps, TPS65023_REG_CON_CTRL2,
-						TPS65023_REG_CTRL2_GO);
+	regmap_update_bits(tps->regmap, TPS65023_REG_CON_CTRL2,
+			TPS65023_REG_CTRL2_GO, TPS65023_REG_CTRL2_GO);
 
 	return ret;
 
@@ -336,13 +311,14 @@ static int tps65023_ldo_get_voltage(struct regulator_dev *dev)
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
 	int data, ldo = rdev_get_id(dev);
+	int ret;
 
 	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
 		return -EINVAL;
 
-	data = tps_65023_reg_read(tps, TPS65023_REG_LDO_CTRL);
-	if (data < 0)
-		return data;
+	ret = regmap_read(tps->regmap, TPS65023_REG_LDO_CTRL, &data);
+	if (ret != 0)
+		return ret;
 
 	data >>= (TPS65023_LDO_CTRL_LDOx_SHIFT(ldo - TPS65023_LDO_1));
 	data &= (tps->info[ldo]->table_len - 1);
@@ -354,6 +330,7 @@ static int tps65023_ldo_set_voltage(struct regulator_dev *dev,
 {
 	struct tps_pmic *tps = rdev_get_drvdata(dev);
 	int data, vsel, ldo = rdev_get_id(dev);
+	int ret;
 
 	if (ldo < TPS65023_LDO_1 || ldo > TPS65023_LDO_2)
 		return -EINVAL;
@@ -377,13 +354,13 @@ static int tps65023_ldo_set_voltage(struct regulator_dev *dev,
 
 	*selector = vsel;
 
-	data = tps_65023_reg_read(tps, TPS65023_REG_LDO_CTRL);
-	if (data < 0)
-		return data;
+	ret = regmap_read(tps->regmap, TPS65023_REG_LDO_CTRL, &data);
+	if (ret != 0)
+		return ret;
 
 	data &= TPS65023_LDO_CTRL_LDOx_MASK(ldo - TPS65023_LDO_1);
 	data |= (vsel << (TPS65023_LDO_CTRL_LDOx_SHIFT(ldo - TPS65023_LDO_1)));
-	return tps_65023_reg_write(tps, TPS65023_REG_LDO_CTRL, data);
+	return regmap_write(tps->regmap, TPS65023_REG_LDO_CTRL, data);
 }
 
 static int tps65023_dcdc_list_voltage(struct regulator_dev *dev,
@@ -496,7 +473,7 @@ static int __devinit tps_65023_probe(struct i2c_client *client,
 
 		/* Register the regulators */
 		rdev = regulator_register(&tps->desc[i], &client->dev,
-					  init_data, tps);
+					  init_data, tps, NULL);
 		if (IS_ERR(rdev)) {
 			dev_err(&client->dev, "failed to register %s\n",
 				id->name);
@@ -511,12 +488,12 @@ static int __devinit tps_65023_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, tps);
 
 	/* Enable setting output voltage by I2C */
-	tps_65023_clear_bits(tps, TPS65023_REG_CON_CTRL2,
-						TPS65023_REG_CTRL2_CORE_ADJ);
+	regmap_update_bits(tps->regmap, TPS65023_REG_CON_CTRL2,
+			TPS65023_REG_CTRL2_CORE_ADJ, TPS65023_REG_CTRL2_CORE_ADJ);
 
 	/* Enable setting output voltage by I2C */
-	tps_65023_clear_bits(tps, TPS65023_REG_CON_CTRL2,
-						TPS65023_REG_CTRL2_CORE_ADJ);
+	regmap_update_bits(tps->regmap, TPS65023_REG_CON_CTRL2,
+			TPS65023_REG_CTRL2_CORE_ADJ, TPS65023_REG_CTRL2_CORE_ADJ);
 
 	return 0;
 
diff --git a/drivers/regulator/tps6507x-regulator.c b/drivers/regulator/tps6507x-regulator.c
index bdef70365f52..0b63ef71a5fe 100644
--- a/drivers/regulator/tps6507x-regulator.c
+++ b/drivers/regulator/tps6507x-regulator.c
@@ -599,7 +599,7 @@ int tps6507x_pmic_probe(struct platform_device *pdev)
 		tps->desc[i].owner = THIS_MODULE;
 
 		rdev = regulator_register(&tps->desc[i],
-					tps6507x_dev->dev, init_data, tps);
+					tps6507x_dev->dev, init_data, tps, NULL);
 		if (IS_ERR(rdev)) {
 			dev_err(tps6507x_dev->dev,
 				"failed to register %s regulator\n",
diff --git a/drivers/regulator/tps6524x-regulator.c b/drivers/regulator/tps6524x-regulator.c
index 9166aa0a9df7..70b7b1f4f000 100644
--- a/drivers/regulator/tps6524x-regulator.c
+++ b/drivers/regulator/tps6524x-regulator.c
@@ -651,7 +651,7 @@ static int __devinit pmic_probe(struct spi_device *spi)
 			hw->desc[i].n_voltages = 1;
 
 		hw->rdev[i] = regulator_register(&hw->desc[i], dev,
-						 init_data, hw);
+						 init_data, hw, NULL);
 		if (IS_ERR(hw->rdev[i])) {
 			ret = PTR_ERR(hw->rdev[i]);
 			hw->rdev[i] = NULL;
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index 14b9389dd52a..c75fb20faa57 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -396,7 +396,7 @@ static int __devinit tps6586x_regulator_probe(struct platform_device *pdev)
 		return err;
 
 	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdev->dev.platform_data, ri);
+				  pdev->dev.platform_data, ri, NULL);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "failed to register regulator %s\n",
 				ri->desc.name);
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index b552aae55b41..5c15ba01e9c7 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -25,30 +25,6 @@
 #include <linux/gpio.h>
 #include <linux/mfd/tps65910.h>
 
-#define TPS65910_REG_VRTC		0
-#define TPS65910_REG_VIO		1
-#define TPS65910_REG_VDD1		2
-#define TPS65910_REG_VDD2		3
-#define TPS65910_REG_VDD3		4
-#define TPS65910_REG_VDIG1		5
-#define TPS65910_REG_VDIG2		6
-#define TPS65910_REG_VPLL		7
-#define TPS65910_REG_VDAC		8
-#define TPS65910_REG_VAUX1		9
-#define TPS65910_REG_VAUX2		10
-#define TPS65910_REG_VAUX33		11
-#define TPS65910_REG_VMMC		12
-
-#define TPS65911_REG_VDDCTRL		4
-#define TPS65911_REG_LDO1		5
-#define TPS65911_REG_LDO2		6
-#define TPS65911_REG_LDO3		7
-#define TPS65911_REG_LDO4		8
-#define TPS65911_REG_LDO5		9
-#define TPS65911_REG_LDO6		10
-#define TPS65911_REG_LDO7		11
-#define TPS65911_REG_LDO8		12
-
 #define TPS65910_SUPPLY_STATE_ENABLED	0x1
 
 /* supported VIO voltages in milivolts */
@@ -885,8 +861,6 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 	if (!pmic_plat_data)
 		return -EINVAL;
 
-	reg_data = pmic_plat_data->tps65910_pmic_init_data;
-
 	pmic = kzalloc(sizeof(*pmic), GFP_KERNEL);
 	if (!pmic)
 		return -ENOMEM;
@@ -937,7 +911,16 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 		goto err_free_info;
 	}
 
-	for (i = 0; i < pmic->num_regulators; i++, info++, reg_data++) {
+	for (i = 0; i < pmic->num_regulators && i < TPS65910_NUM_REGS;
+			i++, info++) {
+
+		reg_data = pmic_plat_data->tps65910_pmic_init_data[i];
+
+		/* Regulator API handles empty constraints but not NULL
+		 * constraints */
+		if (!reg_data)
+			continue;
+
 		/* Register the regulators */
 		pmic->info[i] = info;
 
@@ -965,7 +948,7 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 		pmic->desc[i].owner = THIS_MODULE;
 
 		rdev = regulator_register(&pmic->desc[i],
-				tps65910->dev, reg_data, pmic);
+				tps65910->dev, reg_data, pmic, NULL);
 		if (IS_ERR(rdev)) {
 			dev_err(tps65910->dev,
 				"failed to register %s regulator\n",
diff --git a/drivers/regulator/tps65912-regulator.c b/drivers/regulator/tps65912-regulator.c
index 39d4a1749e71..da00d88f94b7 100644
--- a/drivers/regulator/tps65912-regulator.c
+++ b/drivers/regulator/tps65912-regulator.c
@@ -727,7 +727,7 @@ static __devinit int tps65912_probe(struct platform_device *pdev)
 		pmic->desc[i].owner = THIS_MODULE;
 		range = tps65912_get_range(pmic, i);
 		rdev = regulator_register(&pmic->desc[i],
-					tps65912->dev, reg_data, pmic);
+					tps65912->dev, reg_data, pmic, NULL);
 		if (IS_ERR(rdev)) {
 			dev_err(tps65912->dev,
 				"failed to register %s regulator\n",
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index 11cc308d66e9..181a2cfe180c 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -1112,7 +1112,7 @@ static int __devinit twlreg_probe(struct platform_device *pdev)
 		break;
 	}
 
-	rdev = regulator_register(&info->desc, &pdev->dev, initdata, info);
+	rdev = regulator_register(&info->desc, &pdev->dev, initdata, info, NULL);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "can't register %s, %ld\n",
 				info->desc.name, PTR_ERR(rdev));
diff --git a/drivers/regulator/userspace-consumer.c b/drivers/regulator/userspace-consumer.c
index fc6655146999..518667ef9a0d 100644
--- a/drivers/regulator/userspace-consumer.c
+++ b/drivers/regulator/userspace-consumer.c
@@ -185,18 +185,7 @@ static struct platform_driver regulator_userspace_consumer_driver = {
 	},
 };
 
-
-static int __init regulator_userspace_consumer_init(void)
-{
-	return platform_driver_register(&regulator_userspace_consumer_driver);
-}
-module_init(regulator_userspace_consumer_init);
-
-static void __exit regulator_userspace_consumer_exit(void)
-{
-	platform_driver_unregister(&regulator_userspace_consumer_driver);
-}
-module_exit(regulator_userspace_consumer_exit);
+module_platform_driver(regulator_userspace_consumer_driver);
 
 MODULE_AUTHOR("Mike Rapoport <mike@compulab.co.il>");
 MODULE_DESCRIPTION("Userspace consumer for voltage and current regulators");
diff --git a/drivers/regulator/virtual.c b/drivers/regulator/virtual.c
index 858c1f861ba5..ee0b161c998f 100644
--- a/drivers/regulator/virtual.c
+++ b/drivers/regulator/virtual.c
@@ -352,17 +352,7 @@ static struct platform_driver regulator_virtual_consumer_driver = {
 	},
 };
 
-static int __init regulator_virtual_consumer_init(void)
-{
-	return platform_driver_register(&regulator_virtual_consumer_driver);
-}
-module_init(regulator_virtual_consumer_init);
-
-static void __exit regulator_virtual_consumer_exit(void)
-{
-	platform_driver_unregister(&regulator_virtual_consumer_driver);
-}
-module_exit(regulator_virtual_consumer_exit);
+module_platform_driver(regulator_virtual_consumer_driver);
 
 MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
 MODULE_DESCRIPTION("Virtual regulator consumer");
diff --git a/drivers/regulator/wm831x-dcdc.c b/drivers/regulator/wm831x-dcdc.c
index bd3531d8b2ac..4904a40b0d46 100644
--- a/drivers/regulator/wm831x-dcdc.c
+++ b/drivers/regulator/wm831x-dcdc.c
@@ -511,7 +511,8 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 	if (pdata == NULL || pdata->dcdc[id] == NULL)
 		return -ENODEV;
 
-	dcdc = kzalloc(sizeof(struct wm831x_dcdc), GFP_KERNEL);
+	dcdc = devm_kzalloc(&pdev->dev,  sizeof(struct wm831x_dcdc),
+			    GFP_KERNEL);
 	if (dcdc == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
 		return -ENOMEM;
@@ -553,7 +554,7 @@ static __devinit int wm831x_buckv_probe(struct platform_device *pdev)
 		wm831x_buckv_dvs_init(dcdc, pdata->dcdc[id]->driver_data);
 
 	dcdc->regulator = regulator_register(&dcdc->desc, &pdev->dev,
-					     pdata->dcdc[id], dcdc);
+					     pdata->dcdc[id], dcdc, NULL);
 	if (IS_ERR(dcdc->regulator)) {
 		ret = PTR_ERR(dcdc->regulator);
 		dev_err(wm831x->dev, "Failed to register DCDC%d: %d\n",
@@ -590,7 +591,6 @@ err_regulator:
 err:
 	if (dcdc->dvs_gpio)
 		gpio_free(dcdc->dvs_gpio);
-	kfree(dcdc);
 	return ret;
 }
 
@@ -605,7 +605,6 @@ static __devexit int wm831x_buckv_remove(struct platform_device *pdev)
 	regulator_unregister(dcdc->regulator);
 	if (dcdc->dvs_gpio)
 		gpio_free(dcdc->dvs_gpio);
-	kfree(dcdc);
 
 	return 0;
 }
@@ -722,7 +721,8 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 	if (pdata == NULL || pdata->dcdc[id] == NULL)
 		return -ENODEV;
 
-	dcdc = kzalloc(sizeof(struct wm831x_dcdc), GFP_KERNEL);
+	dcdc = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_dcdc),
+			    GFP_KERNEL);
 	if (dcdc == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
 		return -ENOMEM;
@@ -747,7 +747,7 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 	dcdc->desc.owner = THIS_MODULE;
 
 	dcdc->regulator = regulator_register(&dcdc->desc, &pdev->dev,
-					     pdata->dcdc[id], dcdc);
+					     pdata->dcdc[id], dcdc, NULL);
 	if (IS_ERR(dcdc->regulator)) {
 		ret = PTR_ERR(dcdc->regulator);
 		dev_err(wm831x->dev, "Failed to register DCDC%d: %d\n",
@@ -771,7 +771,6 @@ static __devinit int wm831x_buckp_probe(struct platform_device *pdev)
 err_regulator:
 	regulator_unregister(dcdc->regulator);
 err:
-	kfree(dcdc);
 	return ret;
 }
 
@@ -783,7 +782,6 @@ static __devexit int wm831x_buckp_remove(struct platform_device *pdev)
 
 	free_irq(platform_get_irq_byname(pdev, "UV"), dcdc);
 	regulator_unregister(dcdc->regulator);
-	kfree(dcdc);
 
 	return 0;
 }
@@ -874,7 +872,7 @@ static __devinit int wm831x_boostp_probe(struct platform_device *pdev)
 	dcdc->desc.owner = THIS_MODULE;
 
 	dcdc->regulator = regulator_register(&dcdc->desc, &pdev->dev,
-					     pdata->dcdc[id], dcdc);
+					     pdata->dcdc[id], dcdc, NULL);
 	if (IS_ERR(dcdc->regulator)) {
 		ret = PTR_ERR(dcdc->regulator);
 		dev_err(wm831x->dev, "Failed to register DCDC%d: %d\n",
@@ -973,7 +971,7 @@ static __devinit int wm831x_epe_probe(struct platform_device *pdev)
 	dcdc->desc.owner = THIS_MODULE;
 
 	dcdc->regulator = regulator_register(&dcdc->desc, &pdev->dev,
-					     pdata->epe[id], dcdc);
+					     pdata->epe[id], dcdc, NULL);
 	if (IS_ERR(dcdc->regulator)) {
 		ret = PTR_ERR(dcdc->regulator);
 		dev_err(wm831x->dev, "Failed to register EPE%d: %d\n",
diff --git a/drivers/regulator/wm831x-isink.c b/drivers/regulator/wm831x-isink.c
index 01f27c7f4236..634aac3f2d5f 100644
--- a/drivers/regulator/wm831x-isink.c
+++ b/drivers/regulator/wm831x-isink.c
@@ -162,7 +162,8 @@ static __devinit int wm831x_isink_probe(struct platform_device *pdev)
 	if (pdata == NULL || pdata->isink[id] == NULL)
 		return -ENODEV;
 
-	isink = kzalloc(sizeof(struct wm831x_isink), GFP_KERNEL);
+	isink = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_isink),
+			     GFP_KERNEL);
 	if (isink == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
 		return -ENOMEM;
@@ -189,7 +190,7 @@ static __devinit int wm831x_isink_probe(struct platform_device *pdev)
 	isink->desc.owner = THIS_MODULE;
 
 	isink->regulator = regulator_register(&isink->desc, &pdev->dev,
-					     pdata->isink[id], isink);
+					     pdata->isink[id], isink, NULL);
 	if (IS_ERR(isink->regulator)) {
 		ret = PTR_ERR(isink->regulator);
 		dev_err(wm831x->dev, "Failed to register ISINK%d: %d\n",
@@ -213,7 +214,6 @@ static __devinit int wm831x_isink_probe(struct platform_device *pdev)
 err_regulator:
 	regulator_unregister(isink->regulator);
 err:
-	kfree(isink);
 	return ret;
 }
 
@@ -226,7 +226,6 @@ static __devexit int wm831x_isink_remove(struct platform_device *pdev)
 	free_irq(platform_get_irq(pdev, 0), isink);
 
 	regulator_unregister(isink->regulator);
-	kfree(isink);
 
 	return 0;
 }
diff --git a/drivers/regulator/wm831x-ldo.c b/drivers/regulator/wm831x-ldo.c
index 6709710a059e..f1e4ab0f9fda 100644
--- a/drivers/regulator/wm831x-ldo.c
+++ b/drivers/regulator/wm831x-ldo.c
@@ -326,7 +326,7 @@ static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 	if (pdata == NULL || pdata->ldo[id] == NULL)
 		return -ENODEV;
 
-	ldo = kzalloc(sizeof(struct wm831x_ldo), GFP_KERNEL);
+	ldo = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_ldo), GFP_KERNEL);
 	if (ldo == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
 		return -ENOMEM;
@@ -351,7 +351,7 @@ static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 	ldo->desc.owner = THIS_MODULE;
 
 	ldo->regulator = regulator_register(&ldo->desc, &pdev->dev,
-					     pdata->ldo[id], ldo);
+					     pdata->ldo[id], ldo, NULL);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm831x->dev, "Failed to register LDO%d: %d\n",
@@ -376,7 +376,6 @@ static __devinit int wm831x_gp_ldo_probe(struct platform_device *pdev)
 err_regulator:
 	regulator_unregister(ldo->regulator);
 err:
-	kfree(ldo);
 	return ret;
 }
 
@@ -388,7 +387,6 @@ static __devexit int wm831x_gp_ldo_remove(struct platform_device *pdev)
 
 	free_irq(platform_get_irq_byname(pdev, "UV"), ldo);
 	regulator_unregister(ldo->regulator);
-	kfree(ldo);
 
 	return 0;
 }
@@ -596,7 +594,7 @@ static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 	if (pdata == NULL || pdata->ldo[id] == NULL)
 		return -ENODEV;
 
-	ldo = kzalloc(sizeof(struct wm831x_ldo), GFP_KERNEL);
+	ldo = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_ldo), GFP_KERNEL);
 	if (ldo == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
 		return -ENOMEM;
@@ -621,7 +619,7 @@ static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 	ldo->desc.owner = THIS_MODULE;
 
 	ldo->regulator = regulator_register(&ldo->desc, &pdev->dev,
-					     pdata->ldo[id], ldo);
+					     pdata->ldo[id], ldo, NULL);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm831x->dev, "Failed to register LDO%d: %d\n",
@@ -645,7 +643,6 @@ static __devinit int wm831x_aldo_probe(struct platform_device *pdev)
 err_regulator:
 	regulator_unregister(ldo->regulator);
 err:
-	kfree(ldo);
 	return ret;
 }
 
@@ -655,7 +652,6 @@ static __devexit int wm831x_aldo_remove(struct platform_device *pdev)
 
 	free_irq(platform_get_irq_byname(pdev, "UV"), ldo);
 	regulator_unregister(ldo->regulator);
-	kfree(ldo);
 
 	return 0;
 }
@@ -793,7 +789,7 @@ static __devinit int wm831x_alive_ldo_probe(struct platform_device *pdev)
 	if (pdata == NULL || pdata->ldo[id] == NULL)
 		return -ENODEV;
 
-	ldo = kzalloc(sizeof(struct wm831x_ldo), GFP_KERNEL);
+	ldo = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_ldo), GFP_KERNEL);
 	if (ldo == NULL) {
 		dev_err(&pdev->dev, "Unable to allocate private data\n");
 		return -ENOMEM;
@@ -818,7 +814,7 @@ static __devinit int wm831x_alive_ldo_probe(struct platform_device *pdev)
 	ldo->desc.owner = THIS_MODULE;
 
 	ldo->regulator = regulator_register(&ldo->desc, &pdev->dev,
-					     pdata->ldo[id], ldo);
+					     pdata->ldo[id], ldo, NULL);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm831x->dev, "Failed to register LDO%d: %d\n",
@@ -831,7 +827,6 @@ static __devinit int wm831x_alive_ldo_probe(struct platform_device *pdev)
 	return 0;
 
 err:
-	kfree(ldo);
 	return ret;
 }
 
@@ -840,7 +835,6 @@ static __devexit int wm831x_alive_ldo_remove(struct platform_device *pdev)
 	struct wm831x_ldo *ldo = platform_get_drvdata(pdev);
 
 	regulator_unregister(ldo->regulator);
-	kfree(ldo);
 
 	return 0;
 }
diff --git a/drivers/regulator/wm8350-regulator.c b/drivers/regulator/wm8350-regulator.c
index 1bcb22c44095..6894009d815a 100644
--- a/drivers/regulator/wm8350-regulator.c
+++ b/drivers/regulator/wm8350-regulator.c
@@ -1428,7 +1428,7 @@ static int wm8350_regulator_probe(struct platform_device *pdev)
 	/* register regulator */
 	rdev = regulator_register(&wm8350_reg[pdev->id], &pdev->dev,
 				  pdev->dev.platform_data,
-				  dev_get_drvdata(&pdev->dev));
+				  dev_get_drvdata(&pdev->dev), NULL);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "failed to register %s\n",
 			wm8350_reg[pdev->id].name);
diff --git a/drivers/regulator/wm8400-regulator.c b/drivers/regulator/wm8400-regulator.c
index 71632ddc3781..706f39563a7b 100644
--- a/drivers/regulator/wm8400-regulator.c
+++ b/drivers/regulator/wm8400-regulator.c
@@ -326,7 +326,7 @@ static int __devinit wm8400_regulator_probe(struct platform_device *pdev)
 	struct regulator_dev *rdev;
 
 	rdev = regulator_register(&regulators[pdev->id], &pdev->dev,
-				  pdev->dev.platform_data, wm8400);
+				  pdev->dev.platform_data, wm8400, NULL);
 
 	if (IS_ERR(rdev))
 		return PTR_ERR(rdev);
diff --git a/drivers/regulator/wm8994-regulator.c b/drivers/regulator/wm8994-regulator.c
index b87bf5c841f8..435e335d6e67 100644
--- a/drivers/regulator/wm8994-regulator.c
+++ b/drivers/regulator/wm8994-regulator.c
@@ -269,7 +269,7 @@ static __devinit int wm8994_ldo_probe(struct platform_device *pdev)
 		ldo->is_enabled = true;
 
 	ldo->regulator = regulator_register(&wm8994_ldo_desc[id], &pdev->dev,
-					     pdata->ldo[id].init_data, ldo);
+					     pdata->ldo[id].init_data, ldo, NULL);
 	if (IS_ERR(ldo->regulator)) {
 		ret = PTR_ERR(ldo->regulator);
 		dev_err(wm8994->dev, "Failed to register LDO%d: %d\n",
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 53eb4e55b289..877cf6fdcf24 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -774,7 +774,7 @@ config RTC_DRV_EP93XX
 
 config RTC_DRV_SA1100
 	tristate "SA11x0/PXA2xx"
-	depends on ARCH_SA1100 || ARCH_PXA
+	depends on ARCH_SA1100 || ARCH_PXA || ARCH_MMP
 	help
 	  If you say Y here you will get access to the real time clock
 	  built into your SA11x0 or PXA2xx CPU.
diff --git a/drivers/rtc/interface.c b/drivers/rtc/interface.c
index 8e286259a007..8a1c031391d6 100644
--- a/drivers/rtc/interface.c
+++ b/drivers/rtc/interface.c
@@ -228,11 +228,11 @@ int __rtc_read_alarm(struct rtc_device *rtc, struct rtc_wkalrm *alarm)
 		alarm->time.tm_hour = now.tm_hour;
 
 	/* For simplicity, only support date rollover for now */
-	if (alarm->time.tm_mday == -1) {
+	if (alarm->time.tm_mday < 1 || alarm->time.tm_mday > 31) {
 		alarm->time.tm_mday = now.tm_mday;
 		missing = day;
 	}
-	if (alarm->time.tm_mon == -1) {
+	if ((unsigned)alarm->time.tm_mon >= 12) {
 		alarm->time.tm_mon = now.tm_mon;
 		if (missing == none)
 			missing = month;
diff --git a/drivers/rtc/rtc-88pm860x.c b/drivers/rtc/rtc-88pm860x.c
index 64b847b7f970..f04761e6622d 100644
--- a/drivers/rtc/rtc-88pm860x.c
+++ b/drivers/rtc/rtc-88pm860x.c
@@ -410,17 +410,7 @@ static struct platform_driver pm860x_rtc_driver = {
 	.remove		= __devexit_p(pm860x_rtc_remove),
 };
 
-static int __init pm860x_rtc_init(void)
-{
-	return platform_driver_register(&pm860x_rtc_driver);
-}
-module_init(pm860x_rtc_init);
-
-static void __exit pm860x_rtc_exit(void)
-{
-	platform_driver_unregister(&pm860x_rtc_driver);
-}
-module_exit(pm860x_rtc_exit);
+module_platform_driver(pm860x_rtc_driver);
 
 MODULE_DESCRIPTION("Marvell 88PM860x RTC driver");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
diff --git a/drivers/rtc/rtc-ab8500.c b/drivers/rtc/rtc-ab8500.c
index e346705aae92..a0a9810adf0b 100644
--- a/drivers/rtc/rtc-ab8500.c
+++ b/drivers/rtc/rtc-ab8500.c
@@ -90,7 +90,7 @@ static int ab8500_rtc_read_time(struct device *dev, struct rtc_time *tm)
 
 	/* Early AB8500 chips will not clear the rtc read request bit */
 	if (abx500_get_chip_id(dev) == 0) {
-		msleep(1);
+		usleep_range(1000, 1000);
 	} else {
 		/* Wait for some cycles after enabling the rtc read in ab8500 */
 		while (time_before(jiffies, timeout)) {
@@ -102,7 +102,7 @@ static int ab8500_rtc_read_time(struct device *dev, struct rtc_time *tm)
 			if (!(value & RTC_READ_REQUEST))
 				break;
 
-			msleep(1);
+			usleep_range(1000, 5000);
 		}
 	}
 
@@ -258,6 +258,109 @@ static int ab8500_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 	return ab8500_rtc_irq_enable(dev, alarm->enabled);
 }
 
+
+static int ab8500_rtc_set_calibration(struct device *dev, int calibration)
+{
+	int retval;
+	u8  rtccal = 0;
+
+	/*
+	 * Check that the calibration value (which is in units of 0.5
+	 * parts-per-million) is in the AB8500's range for RtcCalibration
+	 * register. -128 (0x80) is not permitted because the AB8500 uses
+	 * a sign-bit rather than two's complement, so 0x80 is just another
+	 * representation of zero.
+	 */
+	if ((calibration < -127) || (calibration > 127)) {
+		dev_err(dev, "RtcCalibration value outside permitted range\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * The AB8500 uses sign (in bit7) and magnitude (in bits0-7)
+	 * so need to convert to this sort of representation before writing
+	 * into RtcCalibration register...
+	 */
+	if (calibration >= 0)
+		rtccal = 0x7F & calibration;
+	else
+		rtccal = ~(calibration - 1) | 0x80;
+
+	retval = abx500_set_register_interruptible(dev, AB8500_RTC,
+			AB8500_RTC_CALIB_REG, rtccal);
+
+	return retval;
+}
+
+static int ab8500_rtc_get_calibration(struct device *dev, int *calibration)
+{
+	int retval;
+	u8  rtccal = 0;
+
+	retval =  abx500_get_register_interruptible(dev, AB8500_RTC,
+			AB8500_RTC_CALIB_REG, &rtccal);
+	if (retval >= 0) {
+		/*
+		 * The AB8500 uses sign (in bit7) and magnitude (in bits0-7)
+		 * so need to convert value from RtcCalibration register into
+		 * a two's complement signed value...
+		 */
+		if (rtccal & 0x80)
+			*calibration = 0 - (rtccal & 0x7F);
+		else
+			*calibration = 0x7F & rtccal;
+	}
+
+	return retval;
+}
+
+static ssize_t ab8500_sysfs_store_rtc_calibration(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	int retval;
+	int calibration = 0;
+
+	if (sscanf(buf, " %i ", &calibration) != 1) {
+		dev_err(dev, "Failed to store RTC calibration attribute\n");
+		return -EINVAL;
+	}
+
+	retval = ab8500_rtc_set_calibration(dev, calibration);
+
+	return retval ? retval : count;
+}
+
+static ssize_t ab8500_sysfs_show_rtc_calibration(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int  retval = 0;
+	int  calibration = 0;
+
+	retval = ab8500_rtc_get_calibration(dev, &calibration);
+	if (retval < 0) {
+		dev_err(dev, "Failed to read RTC calibration attribute\n");
+		sprintf(buf, "0\n");
+		return retval;
+	}
+
+	return sprintf(buf, "%d\n", calibration);
+}
+
+static DEVICE_ATTR(rtc_calibration, S_IRUGO | S_IWUSR,
+		   ab8500_sysfs_show_rtc_calibration,
+		   ab8500_sysfs_store_rtc_calibration);
+
+static int ab8500_sysfs_rtc_register(struct device *dev)
+{
+	return device_create_file(dev, &dev_attr_rtc_calibration);
+}
+
+static void ab8500_sysfs_rtc_unregister(struct device *dev)
+{
+	device_remove_file(dev, &dev_attr_rtc_calibration);
+}
+
 static irqreturn_t rtc_alarm_handler(int irq, void *data)
 {
 	struct rtc_device *rtc = data;
@@ -295,7 +398,7 @@ static int __devinit ab8500_rtc_probe(struct platform_device *pdev)
 		return err;
 
 	/* Wait for reset by the PorRtc */
-	msleep(1);
+	usleep_range(1000, 5000);
 
 	err = abx500_get_register_interruptible(&pdev->dev, AB8500_RTC,
 		AB8500_RTC_STAT_REG, &rtc_ctrl);
@@ -308,6 +411,8 @@ static int __devinit ab8500_rtc_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
+	device_init_wakeup(&pdev->dev, true);
+
 	rtc = rtc_device_register("ab8500-rtc", &pdev->dev, &ab8500_rtc_ops,
 			THIS_MODULE);
 	if (IS_ERR(rtc)) {
@@ -316,8 +421,8 @@ static int __devinit ab8500_rtc_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	err = request_threaded_irq(irq, NULL, rtc_alarm_handler, 0,
-				   "ab8500-rtc", rtc);
+	err = request_threaded_irq(irq, NULL, rtc_alarm_handler,
+		IRQF_NO_SUSPEND, "ab8500-rtc", rtc);
 	if (err < 0) {
 		rtc_device_unregister(rtc);
 		return err;
@@ -325,6 +430,13 @@ static int __devinit ab8500_rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, rtc);
 
+
+	err = ab8500_sysfs_rtc_register(&pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "sysfs RTC failed to register\n");
+		return err;
+	}
+
 	return 0;
 }
 
@@ -333,6 +445,8 @@ static int __devexit ab8500_rtc_remove(struct platform_device *pdev)
 	struct rtc_device *rtc = platform_get_drvdata(pdev);
 	int irq = platform_get_irq_byname(pdev, "ALARM");
 
+	ab8500_sysfs_rtc_unregister(&pdev->dev);
+
 	free_irq(irq, rtc);
 	rtc_device_unregister(rtc);
 	platform_set_drvdata(pdev, NULL);
@@ -349,18 +463,8 @@ static struct platform_driver ab8500_rtc_driver = {
 	.remove = __devexit_p(ab8500_rtc_remove),
 };
 
-static int __init ab8500_rtc_init(void)
-{
-	return platform_driver_register(&ab8500_rtc_driver);
-}
-
-static void __exit ab8500_rtc_exit(void)
-{
-	platform_driver_unregister(&ab8500_rtc_driver);
-}
+module_platform_driver(ab8500_rtc_driver);
 
-module_init(ab8500_rtc_init);
-module_exit(ab8500_rtc_exit);
 MODULE_AUTHOR("Virupax Sadashivpetimath <virupax.sadashivpetimath@stericsson.com>");
 MODULE_DESCRIPTION("AB8500 RTC Driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/rtc/rtc-at91rm9200.c b/drivers/rtc/rtc-at91rm9200.c
index e39b77a4609a..dc474bc6522d 100644
--- a/drivers/rtc/rtc-at91rm9200.c
+++ b/drivers/rtc/rtc-at91rm9200.c
@@ -32,11 +32,17 @@
 
 #include <mach/at91_rtc.h>
 
+#define at91_rtc_read(field) \
+	__raw_readl(at91_rtc_regs + field)
+#define at91_rtc_write(field, val) \
+	__raw_writel((val), at91_rtc_regs + field)
 
 #define AT91_RTC_EPOCH		1900UL	/* just like arch/arm/common/rtctime.c */
 
 static DECLARE_COMPLETION(at91_rtc_updated);
 static unsigned int at91_alarm_year = AT91_RTC_EPOCH;
+static void __iomem *at91_rtc_regs;
+static int irq;
 
 /*
  * Decode time/date into rtc_time structure
@@ -48,10 +54,10 @@ static void at91_rtc_decodetime(unsigned int timereg, unsigned int calreg,
 
 	/* must read twice in case it changes */
 	do {
-		time = at91_sys_read(timereg);
-		date = at91_sys_read(calreg);
-	} while ((time != at91_sys_read(timereg)) ||
-			(date != at91_sys_read(calreg)));
+		time = at91_rtc_read(timereg);
+		date = at91_rtc_read(calreg);
+	} while ((time != at91_rtc_read(timereg)) ||
+			(date != at91_rtc_read(calreg)));
 
 	tm->tm_sec  = bcd2bin((time & AT91_RTC_SEC) >> 0);
 	tm->tm_min  = bcd2bin((time & AT91_RTC_MIN) >> 8);
@@ -98,19 +104,19 @@ static int at91_rtc_settime(struct device *dev, struct rtc_time *tm)
 		tm->tm_hour, tm->tm_min, tm->tm_sec);
 
 	/* Stop Time/Calendar from counting */
-	cr = at91_sys_read(AT91_RTC_CR);
-	at91_sys_write(AT91_RTC_CR, cr | AT91_RTC_UPDCAL | AT91_RTC_UPDTIM);
+	cr = at91_rtc_read(AT91_RTC_CR);
+	at91_rtc_write(AT91_RTC_CR, cr | AT91_RTC_UPDCAL | AT91_RTC_UPDTIM);
 
-	at91_sys_write(AT91_RTC_IER, AT91_RTC_ACKUPD);
+	at91_rtc_write(AT91_RTC_IER, AT91_RTC_ACKUPD);
 	wait_for_completion(&at91_rtc_updated);	/* wait for ACKUPD interrupt */
-	at91_sys_write(AT91_RTC_IDR, AT91_RTC_ACKUPD);
+	at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ACKUPD);
 
-	at91_sys_write(AT91_RTC_TIMR,
+	at91_rtc_write(AT91_RTC_TIMR,
 			  bin2bcd(tm->tm_sec) << 0
 			| bin2bcd(tm->tm_min) << 8
 			| bin2bcd(tm->tm_hour) << 16);
 
-	at91_sys_write(AT91_RTC_CALR,
+	at91_rtc_write(AT91_RTC_CALR,
 			  bin2bcd((tm->tm_year + 1900) / 100)	/* century */
 			| bin2bcd(tm->tm_year % 100) << 8	/* year */
 			| bin2bcd(tm->tm_mon + 1) << 16		/* tm_mon starts at zero */
@@ -118,8 +124,8 @@ static int at91_rtc_settime(struct device *dev, struct rtc_time *tm)
 			| bin2bcd(tm->tm_mday) << 24);
 
 	/* Restart Time/Calendar */
-	cr = at91_sys_read(AT91_RTC_CR);
-	at91_sys_write(AT91_RTC_CR, cr & ~(AT91_RTC_UPDCAL | AT91_RTC_UPDTIM));
+	cr = at91_rtc_read(AT91_RTC_CR);
+	at91_rtc_write(AT91_RTC_CR, cr & ~(AT91_RTC_UPDCAL | AT91_RTC_UPDTIM));
 
 	return 0;
 }
@@ -135,7 +141,7 @@ static int at91_rtc_readalarm(struct device *dev, struct rtc_wkalrm *alrm)
 	tm->tm_yday = rtc_year_days(tm->tm_mday, tm->tm_mon, tm->tm_year);
 	tm->tm_year = at91_alarm_year - 1900;
 
-	alrm->enabled = (at91_sys_read(AT91_RTC_IMR) & AT91_RTC_ALARM)
+	alrm->enabled = (at91_rtc_read(AT91_RTC_IMR) & AT91_RTC_ALARM)
 			? 1 : 0;
 
 	pr_debug("%s(): %4d-%02d-%02d %02d:%02d:%02d\n", __func__,
@@ -160,20 +166,20 @@ static int at91_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
 	tm.tm_min = alrm->time.tm_min;
 	tm.tm_sec = alrm->time.tm_sec;
 
-	at91_sys_write(AT91_RTC_IDR, AT91_RTC_ALARM);
-	at91_sys_write(AT91_RTC_TIMALR,
+	at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ALARM);
+	at91_rtc_write(AT91_RTC_TIMALR,
 		  bin2bcd(tm.tm_sec) << 0
 		| bin2bcd(tm.tm_min) << 8
 		| bin2bcd(tm.tm_hour) << 16
 		| AT91_RTC_HOUREN | AT91_RTC_MINEN | AT91_RTC_SECEN);
-	at91_sys_write(AT91_RTC_CALALR,
+	at91_rtc_write(AT91_RTC_CALALR,
 		  bin2bcd(tm.tm_mon + 1) << 16		/* tm_mon starts at zero */
 		| bin2bcd(tm.tm_mday) << 24
 		| AT91_RTC_DATEEN | AT91_RTC_MTHEN);
 
 	if (alrm->enabled) {
-		at91_sys_write(AT91_RTC_SCCR, AT91_RTC_ALARM);
-		at91_sys_write(AT91_RTC_IER, AT91_RTC_ALARM);
+		at91_rtc_write(AT91_RTC_SCCR, AT91_RTC_ALARM);
+		at91_rtc_write(AT91_RTC_IER, AT91_RTC_ALARM);
 	}
 
 	pr_debug("%s(): %4d-%02d-%02d %02d:%02d:%02d\n", __func__,
@@ -188,10 +194,10 @@ static int at91_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
 	pr_debug("%s(): cmd=%08x\n", __func__, enabled);
 
 	if (enabled) {
-		at91_sys_write(AT91_RTC_SCCR, AT91_RTC_ALARM);
-		at91_sys_write(AT91_RTC_IER, AT91_RTC_ALARM);
+		at91_rtc_write(AT91_RTC_SCCR, AT91_RTC_ALARM);
+		at91_rtc_write(AT91_RTC_IER, AT91_RTC_ALARM);
 	} else
-		at91_sys_write(AT91_RTC_IDR, AT91_RTC_ALARM);
+		at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ALARM);
 
 	return 0;
 }
@@ -200,7 +206,7 @@ static int at91_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
  */
 static int at91_rtc_proc(struct device *dev, struct seq_file *seq)
 {
-	unsigned long imr = at91_sys_read(AT91_RTC_IMR);
+	unsigned long imr = at91_rtc_read(AT91_RTC_IMR);
 
 	seq_printf(seq, "update_IRQ\t: %s\n",
 			(imr & AT91_RTC_ACKUPD) ? "yes" : "no");
@@ -220,7 +226,7 @@ static irqreturn_t at91_rtc_interrupt(int irq, void *dev_id)
 	unsigned int rtsr;
 	unsigned long events = 0;
 
-	rtsr = at91_sys_read(AT91_RTC_SR) & at91_sys_read(AT91_RTC_IMR);
+	rtsr = at91_rtc_read(AT91_RTC_SR) & at91_rtc_read(AT91_RTC_IMR);
 	if (rtsr) {		/* this interrupt is shared!  Is it ours? */
 		if (rtsr & AT91_RTC_ALARM)
 			events |= (RTC_AF | RTC_IRQF);
@@ -229,7 +235,7 @@ static irqreturn_t at91_rtc_interrupt(int irq, void *dev_id)
 		if (rtsr & AT91_RTC_ACKUPD)
 			complete(&at91_rtc_updated);
 
-		at91_sys_write(AT91_RTC_SCCR, rtsr);	/* clear status reg */
+		at91_rtc_write(AT91_RTC_SCCR, rtsr);	/* clear status reg */
 
 		rtc_update_irq(rtc, 1, events);
 
@@ -256,22 +262,41 @@ static const struct rtc_class_ops at91_rtc_ops = {
 static int __init at91_rtc_probe(struct platform_device *pdev)
 {
 	struct rtc_device *rtc;
-	int ret;
+	struct resource *regs;
+	int ret = 0;
 
-	at91_sys_write(AT91_RTC_CR, 0);
-	at91_sys_write(AT91_RTC_MR, 0);		/* 24 hour mode */
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!regs) {
+		dev_err(&pdev->dev, "no mmio resource defined\n");
+		return -ENXIO;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "no irq resource defined\n");
+		return -ENXIO;
+	}
+
+	at91_rtc_regs = ioremap(regs->start, resource_size(regs));
+	if (!at91_rtc_regs) {
+		dev_err(&pdev->dev, "failed to map registers, aborting.\n");
+		return -ENOMEM;
+	}
+
+	at91_rtc_write(AT91_RTC_CR, 0);
+	at91_rtc_write(AT91_RTC_MR, 0);		/* 24 hour mode */
 
 	/* Disable all interrupts */
-	at91_sys_write(AT91_RTC_IDR, AT91_RTC_ACKUPD | AT91_RTC_ALARM |
+	at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ACKUPD | AT91_RTC_ALARM |
 					AT91_RTC_SECEV | AT91_RTC_TIMEV |
 					AT91_RTC_CALEV);
 
-	ret = request_irq(AT91_ID_SYS, at91_rtc_interrupt,
+	ret = request_irq(irq, at91_rtc_interrupt,
 				IRQF_SHARED,
 				"at91_rtc", pdev);
 	if (ret) {
 		printk(KERN_ERR "at91_rtc: IRQ %d already in use.\n",
-				AT91_ID_SYS);
+				irq);
 		return ret;
 	}
 
@@ -284,7 +309,7 @@ static int __init at91_rtc_probe(struct platform_device *pdev)
 	rtc = rtc_device_register(pdev->name, &pdev->dev,
 				&at91_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc)) {
-		free_irq(AT91_ID_SYS, pdev);
+		free_irq(irq, pdev);
 		return PTR_ERR(rtc);
 	}
 	platform_set_drvdata(pdev, rtc);
@@ -301,10 +326,10 @@ static int __exit at91_rtc_remove(struct platform_device *pdev)
 	struct rtc_device *rtc = platform_get_drvdata(pdev);
 
 	/* Disable all interrupts */
-	at91_sys_write(AT91_RTC_IDR, AT91_RTC_ACKUPD | AT91_RTC_ALARM |
+	at91_rtc_write(AT91_RTC_IDR, AT91_RTC_ACKUPD | AT91_RTC_ALARM |
 					AT91_RTC_SECEV | AT91_RTC_TIMEV |
 					AT91_RTC_CALEV);
-	free_irq(AT91_ID_SYS, pdev);
+	free_irq(irq, pdev);
 
 	rtc_device_unregister(rtc);
 	platform_set_drvdata(pdev, NULL);
@@ -323,13 +348,13 @@ static int at91_rtc_suspend(struct device *dev)
 	/* this IRQ is shared with DBGU and other hardware which isn't
 	 * necessarily doing PM like we are...
 	 */
-	at91_rtc_imr = at91_sys_read(AT91_RTC_IMR)
+	at91_rtc_imr = at91_rtc_read(AT91_RTC_IMR)
 			& (AT91_RTC_ALARM|AT91_RTC_SECEV);
 	if (at91_rtc_imr) {
 		if (device_may_wakeup(dev))
-			enable_irq_wake(AT91_ID_SYS);
+			enable_irq_wake(irq);
 		else
-			at91_sys_write(AT91_RTC_IDR, at91_rtc_imr);
+			at91_rtc_write(AT91_RTC_IDR, at91_rtc_imr);
 	}
 	return 0;
 }
@@ -338,9 +363,9 @@ static int at91_rtc_resume(struct device *dev)
 {
 	if (at91_rtc_imr) {
 		if (device_may_wakeup(dev))
-			disable_irq_wake(AT91_ID_SYS);
+			disable_irq_wake(irq);
 		else
-			at91_sys_write(AT91_RTC_IER, at91_rtc_imr);
+			at91_rtc_write(AT91_RTC_IER, at91_rtc_imr);
 	}
 	return 0;
 }
diff --git a/drivers/rtc/rtc-bfin.c b/drivers/rtc/rtc-bfin.c
index 90d866272c8e..abfc1a0c07d9 100644
--- a/drivers/rtc/rtc-bfin.c
+++ b/drivers/rtc/rtc-bfin.c
@@ -456,18 +456,7 @@ static struct platform_driver bfin_rtc_driver = {
 	.resume		= bfin_rtc_resume,
 };
 
-static int __init bfin_rtc_init(void)
-{
-	return platform_driver_register(&bfin_rtc_driver);
-}
-
-static void __exit bfin_rtc_exit(void)
-{
-	platform_driver_unregister(&bfin_rtc_driver);
-}
-
-module_init(bfin_rtc_init);
-module_exit(bfin_rtc_exit);
+module_platform_driver(bfin_rtc_driver);
 
 MODULE_DESCRIPTION("Blackfin On-Chip Real Time Clock Driver");
 MODULE_AUTHOR("Mike Frysinger <vapier@gentoo.org>");
diff --git a/drivers/rtc/rtc-bq4802.c b/drivers/rtc/rtc-bq4802.c
index 128270ce355d..bf612ef22941 100644
--- a/drivers/rtc/rtc-bq4802.c
+++ b/drivers/rtc/rtc-bq4802.c
@@ -218,15 +218,4 @@ static struct platform_driver bq4802_driver = {
 	.remove		= __devexit_p(bq4802_remove),
 };
 
-static int __init bq4802_init(void)
-{
-	return platform_driver_register(&bq4802_driver);
-}
-
-static void __exit bq4802_exit(void)
-{
-	platform_driver_unregister(&bq4802_driver);
-}
-
-module_init(bq4802_init);
-module_exit(bq4802_exit);
+module_platform_driver(bq4802_driver);
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index 05beb6c1ca79..d7782aa09943 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -164,7 +164,7 @@ static inline unsigned char cmos_read_bank2(unsigned char addr)
 static inline void cmos_write_bank2(unsigned char val, unsigned char addr)
 {
 	outb(addr, RTC_PORT(2));
-	outb(val, RTC_PORT(2));
+	outb(val, RTC_PORT(3));
 }
 
 #else
diff --git a/drivers/rtc/rtc-dm355evm.c b/drivers/rtc/rtc-dm355evm.c
index 2322c43af201..d4457afcba89 100644
--- a/drivers/rtc/rtc-dm355evm.c
+++ b/drivers/rtc/rtc-dm355evm.c
@@ -161,16 +161,6 @@ static struct platform_driver rtc_dm355evm_driver = {
 	},
 };
 
-static int __init dm355evm_rtc_init(void)
-{
-	return platform_driver_register(&rtc_dm355evm_driver);
-}
-module_init(dm355evm_rtc_init);
-
-static void __exit dm355evm_rtc_exit(void)
-{
-	platform_driver_unregister(&rtc_dm355evm_driver);
-}
-module_exit(dm355evm_rtc_exit);
+module_platform_driver(rtc_dm355evm_driver);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-ds1286.c b/drivers/rtc/rtc-ds1286.c
index 68e6caf25496..990c3ff489bf 100644
--- a/drivers/rtc/rtc-ds1286.c
+++ b/drivers/rtc/rtc-ds1286.c
@@ -396,21 +396,10 @@ static struct platform_driver ds1286_platform_driver = {
 	.remove		= __devexit_p(ds1286_remove),
 };
 
-static int __init ds1286_init(void)
-{
-	return platform_driver_register(&ds1286_platform_driver);
-}
-
-static void __exit ds1286_exit(void)
-{
-	platform_driver_unregister(&ds1286_platform_driver);
-}
+module_platform_driver(ds1286_platform_driver);
 
 MODULE_AUTHOR("Thomas Bogendoerfer <tsbogend@alpha.franken.de>");
 MODULE_DESCRIPTION("DS1286 RTC driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 MODULE_ALIAS("platform:rtc-ds1286");
-
-module_init(ds1286_init);
-module_exit(ds1286_exit);
diff --git a/drivers/rtc/rtc-ds1511.c b/drivers/rtc/rtc-ds1511.c
index 586c244a05d8..761f36bc83a9 100644
--- a/drivers/rtc/rtc-ds1511.c
+++ b/drivers/rtc/rtc-ds1511.c
@@ -580,20 +580,7 @@ static struct platform_driver ds1511_rtc_driver = {
 	},
 };
 
- static int __init
-ds1511_rtc_init(void)
-{
-	return platform_driver_register(&ds1511_rtc_driver);
-}
-
- static void __exit
-ds1511_rtc_exit(void)
-{
-	platform_driver_unregister(&ds1511_rtc_driver);
-}
-
-module_init(ds1511_rtc_init);
-module_exit(ds1511_rtc_exit);
+module_platform_driver(ds1511_rtc_driver);
 
 MODULE_AUTHOR("Andrew Sharp <andy.sharp@lsi.com>");
 MODULE_DESCRIPTION("Dallas DS1511 RTC driver");
diff --git a/drivers/rtc/rtc-ds1553.c b/drivers/rtc/rtc-ds1553.c
index 1350029044e6..6f0a1b530f2e 100644
--- a/drivers/rtc/rtc-ds1553.c
+++ b/drivers/rtc/rtc-ds1553.c
@@ -361,18 +361,7 @@ static struct platform_driver ds1553_rtc_driver = {
 	},
 };
 
-static __init int ds1553_init(void)
-{
-	return platform_driver_register(&ds1553_rtc_driver);
-}
-
-static __exit void ds1553_exit(void)
-{
-	platform_driver_unregister(&ds1553_rtc_driver);
-}
-
-module_init(ds1553_init);
-module_exit(ds1553_exit);
+module_platform_driver(ds1553_rtc_driver);
 
 MODULE_AUTHOR("Atsushi Nemoto <anemo@mba.ocn.ne.jp>");
 MODULE_DESCRIPTION("Dallas DS1553 RTC driver");
diff --git a/drivers/rtc/rtc-ds1742.c b/drivers/rtc/rtc-ds1742.c
index e3e0f92b60f0..76112667c507 100644
--- a/drivers/rtc/rtc-ds1742.c
+++ b/drivers/rtc/rtc-ds1742.c
@@ -240,18 +240,7 @@ static struct platform_driver ds1742_rtc_driver = {
 	},
 };
 
-static __init int ds1742_init(void)
-{
-	return platform_driver_register(&ds1742_rtc_driver);
-}
-
-static __exit void ds1742_exit(void)
-{
-	platform_driver_unregister(&ds1742_rtc_driver);
-}
-
-module_init(ds1742_init);
-module_exit(ds1742_exit);
+module_platform_driver(ds1742_rtc_driver);
 
 MODULE_AUTHOR("Atsushi Nemoto <anemo@mba.ocn.ne.jp>");
 MODULE_DESCRIPTION("Dallas DS1742 RTC driver");
diff --git a/drivers/rtc/rtc-jz4740.c b/drivers/rtc/rtc-jz4740.c
index b6473631d182..05ab227eeff7 100644
--- a/drivers/rtc/rtc-jz4740.c
+++ b/drivers/rtc/rtc-jz4740.c
@@ -345,7 +345,7 @@ static const struct dev_pm_ops jz4740_pm_ops = {
 #define JZ4740_RTC_PM_OPS NULL
 #endif  /* CONFIG_PM */
 
-struct platform_driver jz4740_rtc_driver = {
+static struct platform_driver jz4740_rtc_driver = {
 	.probe	 = jz4740_rtc_probe,
 	.remove	 = __devexit_p(jz4740_rtc_remove),
 	.driver	 = {
@@ -355,17 +355,7 @@ struct platform_driver jz4740_rtc_driver = {
 	},
 };
 
-static int __init jz4740_rtc_init(void)
-{
-	return platform_driver_register(&jz4740_rtc_driver);
-}
-module_init(jz4740_rtc_init);
-
-static void __exit jz4740_rtc_exit(void)
-{
-	platform_driver_unregister(&jz4740_rtc_driver);
-}
-module_exit(jz4740_rtc_exit);
+module_platform_driver(jz4740_rtc_driver);
 
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-lpc32xx.c b/drivers/rtc/rtc-lpc32xx.c
index ae16250c762f..ecc1713b2b4f 100644
--- a/drivers/rtc/rtc-lpc32xx.c
+++ b/drivers/rtc/rtc-lpc32xx.c
@@ -396,17 +396,7 @@ static struct platform_driver lpc32xx_rtc_driver = {
 	},
 };
 
-static int __init lpc32xx_rtc_init(void)
-{
-	return platform_driver_register(&lpc32xx_rtc_driver);
-}
-module_init(lpc32xx_rtc_init);
-
-static void __exit lpc32xx_rtc_exit(void)
-{
-	platform_driver_unregister(&lpc32xx_rtc_driver);
-}
-module_exit(lpc32xx_rtc_exit);
+module_platform_driver(lpc32xx_rtc_driver);
 
 MODULE_AUTHOR("Kevin Wells <wellsk40@gmail.com");
 MODULE_DESCRIPTION("RTC driver for the LPC32xx SoC");
diff --git a/drivers/rtc/rtc-m41t93.c b/drivers/rtc/rtc-m41t93.c
index 7317d3b9a3d5..ef71132ff205 100644
--- a/drivers/rtc/rtc-m41t93.c
+++ b/drivers/rtc/rtc-m41t93.c
@@ -200,7 +200,6 @@ static int __devexit m41t93_remove(struct spi_device *spi)
 static struct spi_driver m41t93_driver = {
 	.driver = {
 		.name	= "rtc-m41t93",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe	= m41t93_probe,
diff --git a/drivers/rtc/rtc-m41t94.c b/drivers/rtc/rtc-m41t94.c
index e259ed76ae85..2a4721f61797 100644
--- a/drivers/rtc/rtc-m41t94.c
+++ b/drivers/rtc/rtc-m41t94.c
@@ -147,7 +147,6 @@ static int __devexit m41t94_remove(struct spi_device *spi)
 static struct spi_driver m41t94_driver = {
 	.driver = {
 		.name	= "rtc-m41t94",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe	= m41t94_probe,
diff --git a/drivers/rtc/rtc-m48t35.c b/drivers/rtc/rtc-m48t35.c
index 8e2a24e33ed6..f9e3b3583733 100644
--- a/drivers/rtc/rtc-m48t35.c
+++ b/drivers/rtc/rtc-m48t35.c
@@ -216,21 +216,10 @@ static struct platform_driver m48t35_platform_driver = {
 	.remove		= __devexit_p(m48t35_remove),
 };
 
-static int __init m48t35_init(void)
-{
-	return platform_driver_register(&m48t35_platform_driver);
-}
-
-static void __exit m48t35_exit(void)
-{
-	platform_driver_unregister(&m48t35_platform_driver);
-}
+module_platform_driver(m48t35_platform_driver);
 
 MODULE_AUTHOR("Thomas Bogendoerfer <tsbogend@alpha.franken.de>");
 MODULE_DESCRIPTION("M48T35 RTC driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 MODULE_ALIAS("platform:rtc-m48t35");
-
-module_init(m48t35_init);
-module_exit(m48t35_exit);
diff --git a/drivers/rtc/rtc-m48t59.c b/drivers/rtc/rtc-m48t59.c
index 28365388fb6c..30ebfec9fd2b 100644
--- a/drivers/rtc/rtc-m48t59.c
+++ b/drivers/rtc/rtc-m48t59.c
@@ -530,18 +530,7 @@ static struct platform_driver m48t59_rtc_driver = {
 	.remove		= __devexit_p(m48t59_rtc_remove),
 };
 
-static int __init m48t59_rtc_init(void)
-{
-	return platform_driver_register(&m48t59_rtc_driver);
-}
-
-static void __exit m48t59_rtc_exit(void)
-{
-	platform_driver_unregister(&m48t59_rtc_driver);
-}
-
-module_init(m48t59_rtc_init);
-module_exit(m48t59_rtc_exit);
+module_platform_driver(m48t59_rtc_driver);
 
 MODULE_AUTHOR("Mark Zhan <rongkai.zhan@windriver.com>");
 MODULE_DESCRIPTION("M48T59/M48T02/M48T08 RTC driver");
diff --git a/drivers/rtc/rtc-m48t86.c b/drivers/rtc/rtc-m48t86.c
index f981287d582b..863fb3363aa6 100644
--- a/drivers/rtc/rtc-m48t86.c
+++ b/drivers/rtc/rtc-m48t86.c
@@ -185,21 +185,10 @@ static struct platform_driver m48t86_rtc_platform_driver = {
 	.remove		= __devexit_p(m48t86_rtc_remove),
 };
 
-static int __init m48t86_rtc_init(void)
-{
-	return platform_driver_register(&m48t86_rtc_platform_driver);
-}
-
-static void __exit m48t86_rtc_exit(void)
-{
-	platform_driver_unregister(&m48t86_rtc_platform_driver);
-}
+module_platform_driver(m48t86_rtc_platform_driver);
 
 MODULE_AUTHOR("Alessandro Zummo <a.zummo@towertech.it>");
 MODULE_DESCRIPTION("M48T86 RTC driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 MODULE_ALIAS("platform:rtc-m48t86");
-
-module_init(m48t86_rtc_init);
-module_exit(m48t86_rtc_exit);
diff --git a/drivers/rtc/rtc-max6902.c b/drivers/rtc/rtc-max6902.c
index 0ec3f588a255..1f6b3cc58e8a 100644
--- a/drivers/rtc/rtc-max6902.c
+++ b/drivers/rtc/rtc-max6902.c
@@ -154,7 +154,6 @@ static int __devexit max6902_remove(struct spi_device *spi)
 static struct spi_driver max6902_driver = {
 	.driver = {
 		.name	= "rtc-max6902",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe	= max6902_probe,
diff --git a/drivers/rtc/rtc-max8925.c b/drivers/rtc/rtc-max8925.c
index 3bc046f427e0..4a5529346b47 100644
--- a/drivers/rtc/rtc-max8925.c
+++ b/drivers/rtc/rtc-max8925.c
@@ -299,17 +299,7 @@ static struct platform_driver max8925_rtc_driver = {
 	.remove		= __devexit_p(max8925_rtc_remove),
 };
 
-static int __init max8925_rtc_init(void)
-{
-	return platform_driver_register(&max8925_rtc_driver);
-}
-module_init(max8925_rtc_init);
-
-static void __exit max8925_rtc_exit(void)
-{
-	platform_driver_unregister(&max8925_rtc_driver);
-}
-module_exit(max8925_rtc_exit);
+module_platform_driver(max8925_rtc_driver);
 
 MODULE_DESCRIPTION("Maxim MAX8925 RTC driver");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
diff --git a/drivers/rtc/rtc-max8998.c b/drivers/rtc/rtc-max8998.c
index 2e48aa604273..7196f438c089 100644
--- a/drivers/rtc/rtc-max8998.c
+++ b/drivers/rtc/rtc-max8998.c
@@ -327,17 +327,7 @@ static struct platform_driver max8998_rtc_driver = {
 	.id_table	= max8998_rtc_id,
 };
 
-static int __init max8998_rtc_init(void)
-{
-	return platform_driver_register(&max8998_rtc_driver);
-}
-module_init(max8998_rtc_init);
-
-static void __exit max8998_rtc_exit(void)
-{
-	platform_driver_unregister(&max8998_rtc_driver);
-}
-module_exit(max8998_rtc_exit);
+module_platform_driver(max8998_rtc_driver);
 
 MODULE_AUTHOR("Minkyu Kang <mk7.kang@samsung.com>");
 MODULE_AUTHOR("Joonyoung Shim <jy0922.shim@samsung.com>");
diff --git a/drivers/rtc/rtc-mc13xxx.c b/drivers/rtc/rtc-mc13xxx.c
index 9d0c3b478d55..546f6850bffb 100644
--- a/drivers/rtc/rtc-mc13xxx.c
+++ b/drivers/rtc/rtc-mc13xxx.c
@@ -399,7 +399,7 @@ static int __exit mc13xxx_rtc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-const struct platform_device_id mc13xxx_rtc_idtable[] = {
+static const struct platform_device_id mc13xxx_rtc_idtable[] = {
 	{
 		.name = "mc13783-rtc",
 	}, {
diff --git a/drivers/rtc/rtc-mpc5121.c b/drivers/rtc/rtc-mpc5121.c
index da60915818b6..9d3caccfc250 100644
--- a/drivers/rtc/rtc-mpc5121.c
+++ b/drivers/rtc/rtc-mpc5121.c
@@ -418,17 +418,7 @@ static struct platform_driver mpc5121_rtc_driver = {
 	.remove = __devexit_p(mpc5121_rtc_remove),
 };
 
-static int __init mpc5121_rtc_init(void)
-{
-	return platform_driver_register(&mpc5121_rtc_driver);
-}
-module_init(mpc5121_rtc_init);
-
-static void __exit mpc5121_rtc_exit(void)
-{
-	platform_driver_unregister(&mpc5121_rtc_driver);
-}
-module_exit(mpc5121_rtc_exit);
+module_platform_driver(mpc5121_rtc_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("John Rigby <jcrigby@gmail.com>");
diff --git a/drivers/rtc/rtc-mrst.c b/drivers/rtc/rtc-mrst.c
index bb21f443fb70..6cd6c7235344 100644
--- a/drivers/rtc/rtc-mrst.c
+++ b/drivers/rtc/rtc-mrst.c
@@ -537,18 +537,7 @@ static struct platform_driver vrtc_mrst_platform_driver = {
 	}
 };
 
-static int __init vrtc_mrst_init(void)
-{
-	return platform_driver_register(&vrtc_mrst_platform_driver);
-}
-
-static void __exit vrtc_mrst_exit(void)
-{
-	platform_driver_unregister(&vrtc_mrst_platform_driver);
-}
-
-module_init(vrtc_mrst_init);
-module_exit(vrtc_mrst_exit);
+module_platform_driver(vrtc_mrst_platform_driver);
 
 MODULE_AUTHOR("Jacob Pan; Feng Tang");
 MODULE_DESCRIPTION("Driver for Moorestown virtual RTC");
diff --git a/drivers/rtc/rtc-mxc.c b/drivers/rtc/rtc-mxc.c
index 39e41fbdf08b..5e1d64ee5228 100644
--- a/drivers/rtc/rtc-mxc.c
+++ b/drivers/rtc/rtc-mxc.c
@@ -155,7 +155,6 @@ static int rtc_update_alarm(struct device *dev, struct rtc_time *alrm)
 {
 	struct rtc_time alarm_tm, now_tm;
 	unsigned long now, time;
-	int ret;
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 	void __iomem *ioaddr = pdata->ioaddr;
@@ -168,21 +167,33 @@ static int rtc_update_alarm(struct device *dev, struct rtc_time *alrm)
 	alarm_tm.tm_hour = alrm->tm_hour;
 	alarm_tm.tm_min = alrm->tm_min;
 	alarm_tm.tm_sec = alrm->tm_sec;
-	rtc_tm_to_time(&now_tm, &now);
 	rtc_tm_to_time(&alarm_tm, &time);
 
-	if (time < now) {
-		time += 60 * 60 * 24;
-		rtc_time_to_tm(time, &alarm_tm);
-	}
-
-	ret = rtc_tm_to_time(&alarm_tm, &time);
-
 	/* clear all the interrupt status bits */
 	writew(readw(ioaddr + RTC_RTCISR), ioaddr + RTC_RTCISR);
 	set_alarm_or_time(dev, MXC_RTC_ALARM, time);
 
-	return ret;
+	return 0;
+}
+
+static void mxc_rtc_irq_enable(struct device *dev, unsigned int bit,
+				unsigned int enabled)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	u32 reg;
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	reg = readw(ioaddr + RTC_RTCIENR);
+
+	if (enabled)
+		reg |= bit;
+	else
+		reg &= ~bit;
+
+	writew(reg, ioaddr + RTC_RTCIENR);
+	spin_unlock_irq(&pdata->rtc->irq_lock);
 }
 
 /* This function is the RTC interrupt service routine. */
@@ -199,13 +210,12 @@ static irqreturn_t mxc_rtc_interrupt(int irq, void *dev_id)
 	/* clear interrupt sources */
 	writew(status, ioaddr + RTC_RTCISR);
 
-	/* clear alarm interrupt if it has occurred */
-	if (status & RTC_ALM_BIT)
-		status &= ~RTC_ALM_BIT;
-
 	/* update irq data & counter */
-	if (status & RTC_ALM_BIT)
+	if (status & RTC_ALM_BIT) {
 		events |= (RTC_AF | RTC_IRQF);
+		/* RTC alarm should be one-shot */
+		mxc_rtc_irq_enable(&pdev->dev, RTC_ALM_BIT, 0);
+	}
 
 	if (status & RTC_1HZ_BIT)
 		events |= (RTC_UF | RTC_IRQF);
@@ -213,9 +223,6 @@ static irqreturn_t mxc_rtc_interrupt(int irq, void *dev_id)
 	if (status & PIT_ALL_ON)
 		events |= (RTC_PF | RTC_IRQF);
 
-	if ((status & RTC_ALM_BIT) && rtc_valid_tm(&pdata->g_rtc_alarm))
-		rtc_update_alarm(&pdev->dev, &pdata->g_rtc_alarm);
-
 	rtc_update_irq(pdata->rtc, 1, events);
 	spin_unlock_irq(&pdata->rtc->irq_lock);
 
@@ -242,26 +249,6 @@ static void mxc_rtc_release(struct device *dev)
 	spin_unlock_irq(&pdata->rtc->irq_lock);
 }
 
-static void mxc_rtc_irq_enable(struct device *dev, unsigned int bit,
-				unsigned int enabled)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
-	void __iomem *ioaddr = pdata->ioaddr;
-	u32 reg;
-
-	spin_lock_irq(&pdata->rtc->irq_lock);
-	reg = readw(ioaddr + RTC_RTCIENR);
-
-	if (enabled)
-		reg |= bit;
-	else
-		reg &= ~bit;
-
-	writew(reg, ioaddr + RTC_RTCIENR);
-	spin_unlock_irq(&pdata->rtc->irq_lock);
-}
-
 static int mxc_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
 {
 	mxc_rtc_irq_enable(dev, RTC_ALM_BIT, enabled);
@@ -290,6 +277,17 @@ static int mxc_rtc_read_time(struct device *dev, struct rtc_time *tm)
  */
 static int mxc_rtc_set_mmss(struct device *dev, unsigned long time)
 {
+	/*
+	 * TTC_DAYR register is 9-bit in MX1 SoC, save time and day of year only
+	 */
+	if (cpu_is_mx1()) {
+		struct rtc_time tm;
+
+		rtc_time_to_tm(time, &tm);
+		tm.tm_year = 70;
+		rtc_tm_to_time(&tm, &time);
+	}
+
 	/* Avoid roll-over from reading the different registers */
 	do {
 		set_alarm_or_time(dev, MXC_RTC_TIME, time);
@@ -324,21 +322,7 @@ static int mxc_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 	int ret;
 
-	if (rtc_valid_tm(&alrm->time)) {
-		if (alrm->time.tm_sec > 59 ||
-		    alrm->time.tm_hour > 23 ||
-		    alrm->time.tm_min > 59)
-			return -EINVAL;
-
-		ret = rtc_update_alarm(dev, &alrm->time);
-	} else {
-		ret = rtc_valid_tm(&alrm->time);
-		if (ret)
-			return ret;
-
-		ret = rtc_update_alarm(dev, &alrm->time);
-	}
-
+	ret = rtc_update_alarm(dev, &alrm->time);
 	if (ret)
 		return ret;
 
@@ -424,6 +408,9 @@ static int __init mxc_rtc_probe(struct platform_device *pdev)
 		pdata->irq = -1;
 	}
 
+	if (pdata->irq >=0)
+		device_init_wakeup(&pdev->dev, 1);
+
 	rtc = rtc_device_register(pdev->name, &pdev->dev, &mxc_rtc_ops,
 				  THIS_MODULE);
 	if (IS_ERR(rtc)) {
@@ -459,9 +446,39 @@ static int __exit mxc_rtc_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int mxc_rtc_suspend(struct device *dev)
+{
+	struct rtc_plat_data *pdata = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(pdata->irq);
+
+	return 0;
+}
+
+static int mxc_rtc_resume(struct device *dev)
+{
+	struct rtc_plat_data *pdata = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(pdata->irq);
+
+	return 0;
+}
+
+static struct dev_pm_ops mxc_rtc_pm_ops = {
+	.suspend	= mxc_rtc_suspend,
+	.resume		= mxc_rtc_resume,
+};
+#endif
+
 static struct platform_driver mxc_rtc_driver = {
 	.driver = {
 		   .name	= "mxc_rtc",
+#ifdef CONFIG_PM
+		   .pm		= &mxc_rtc_pm_ops,
+#endif
 		   .owner	= THIS_MODULE,
 	},
 	.remove		= __exit_p(mxc_rtc_remove),
diff --git a/drivers/rtc/rtc-pcf2123.c b/drivers/rtc/rtc-pcf2123.c
index 2ee3bbf7e5ea..b46c4004d8fe 100644
--- a/drivers/rtc/rtc-pcf2123.c
+++ b/drivers/rtc/rtc-pcf2123.c
@@ -340,7 +340,6 @@ static int __devexit pcf2123_remove(struct spi_device *spi)
 static struct spi_driver pcf2123_driver = {
 	.driver	= {
 			.name	= "rtc-pcf2123",
-			.bus	= &spi_bus_type,
 			.owner	= THIS_MODULE,
 	},
 	.probe	= pcf2123_probe,
diff --git a/drivers/rtc/rtc-pcf50633.c b/drivers/rtc/rtc-pcf50633.c
index 0c423892923c..a20202f9ee57 100644
--- a/drivers/rtc/rtc-pcf50633.c
+++ b/drivers/rtc/rtc-pcf50633.c
@@ -294,17 +294,7 @@ static struct platform_driver pcf50633_rtc_driver = {
 	.remove = __devexit_p(pcf50633_rtc_remove),
 };
 
-static int __init pcf50633_rtc_init(void)
-{
-	return platform_driver_register(&pcf50633_rtc_driver);
-}
-module_init(pcf50633_rtc_init);
-
-static void __exit pcf50633_rtc_exit(void)
-{
-	platform_driver_unregister(&pcf50633_rtc_driver);
-}
-module_exit(pcf50633_rtc_exit);
+module_platform_driver(pcf50633_rtc_driver);
 
 MODULE_DESCRIPTION("PCF50633 RTC driver");
 MODULE_AUTHOR("Balaji Rao <balajirrao@openmoko.org>");
diff --git a/drivers/rtc/rtc-pm8xxx.c b/drivers/rtc/rtc-pm8xxx.c
index d420e9d877e8..9f1d6bcbdf6c 100644
--- a/drivers/rtc/rtc-pm8xxx.c
+++ b/drivers/rtc/rtc-pm8xxx.c
@@ -532,17 +532,7 @@ static struct platform_driver pm8xxx_rtc_driver = {
 	},
 };
 
-static int __init pm8xxx_rtc_init(void)
-{
-	return platform_driver_register(&pm8xxx_rtc_driver);
-}
-module_init(pm8xxx_rtc_init);
-
-static void __exit pm8xxx_rtc_exit(void)
-{
-	platform_driver_unregister(&pm8xxx_rtc_driver);
-}
-module_exit(pm8xxx_rtc_exit);
+module_platform_driver(pm8xxx_rtc_driver);
 
 MODULE_ALIAS("platform:rtc-pm8xxx");
 MODULE_DESCRIPTION("PMIC8xxx RTC driver");
diff --git a/drivers/rtc/rtc-puv3.c b/drivers/rtc/rtc-puv3.c
index e4b6880aabd0..ab0acaeb2371 100644
--- a/drivers/rtc/rtc-puv3.c
+++ b/drivers/rtc/rtc-puv3.c
@@ -164,7 +164,7 @@ static int puv3_rtc_open(struct device *dev)
 	int ret;
 
 	ret = request_irq(puv3_rtc_alarmno, puv3_rtc_alarmirq,
-			  IRQF_DISABLED,  "pkunity-rtc alarm", rtc_dev);
+			0, "pkunity-rtc alarm", rtc_dev);
 
 	if (ret) {
 		dev_err(dev, "IRQ%d error %d\n", puv3_rtc_alarmno, ret);
@@ -172,7 +172,7 @@ static int puv3_rtc_open(struct device *dev)
 	}
 
 	ret = request_irq(puv3_rtc_tickno, puv3_rtc_tickirq,
-			  IRQF_DISABLED,  "pkunity-rtc tick", rtc_dev);
+			0, "pkunity-rtc tick", rtc_dev);
 
 	if (ret) {
 		dev_err(dev, "IRQ%d error %d\n", puv3_rtc_tickno, ret);
@@ -326,7 +326,7 @@ static int puv3_rtc_resume(struct platform_device *pdev)
 #define puv3_rtc_resume  NULL
 #endif
 
-static struct platform_driver puv3_rtcdrv = {
+static struct platform_driver puv3_rtc_driver = {
 	.probe		= puv3_rtc_probe,
 	.remove		= __devexit_p(puv3_rtc_remove),
 	.suspend	= puv3_rtc_suspend,
@@ -337,21 +337,7 @@ static struct platform_driver puv3_rtcdrv = {
 	}
 };
 
-static char __initdata banner[] = "PKUnity-v3 RTC, (c) 2009 PKUnity Co.\n";
-
-static int __init puv3_rtc_init(void)
-{
-	printk(banner);
-	return platform_driver_register(&puv3_rtcdrv);
-}
-
-static void __exit puv3_rtc_exit(void)
-{
-	platform_driver_unregister(&puv3_rtcdrv);
-}
-
-module_init(puv3_rtc_init);
-module_exit(puv3_rtc_exit);
+module_platform_driver(puv3_rtc_driver);
 
 MODULE_DESCRIPTION("RTC Driver for the PKUnity v3 chip");
 MODULE_AUTHOR("Hu Dongliang");
diff --git a/drivers/rtc/rtc-rs5c348.c b/drivers/rtc/rtc-rs5c348.c
index 971bc8e08da6..ce2ca8523ddd 100644
--- a/drivers/rtc/rtc-rs5c348.c
+++ b/drivers/rtc/rtc-rs5c348.c
@@ -229,7 +229,6 @@ static int __devexit rs5c348_remove(struct spi_device *spi)
 static struct spi_driver rs5c348_driver = {
 	.driver = {
 		.name	= "rtc-rs5c348",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe	= rs5c348_probe,
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 5b979d9cc332..aef40bd2957b 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -25,6 +25,7 @@
 #include <linux/clk.h>
 #include <linux/log2.h>
 #include <linux/slab.h>
+#include <linux/of.h>
 
 #include <mach/hardware.h>
 #include <asm/uaccess.h>
@@ -507,7 +508,13 @@ static int __devinit s3c_rtc_probe(struct platform_device *pdev)
 		goto err_nortc;
 	}
 
-	s3c_rtc_cpu_type = platform_get_device_id(pdev)->driver_data;
+#ifdef CONFIG_OF
+	if (pdev->dev.of_node)
+		s3c_rtc_cpu_type = of_device_is_compatible(pdev->dev.of_node,
+			"samsung,s3c6410-rtc") ? TYPE_S3C64XX : TYPE_S3C2410;
+	else
+#endif
+		s3c_rtc_cpu_type = platform_get_device_id(pdev)->driver_data;
 
 	/* Check RTC Time */
 
@@ -629,6 +636,17 @@ static int s3c_rtc_resume(struct platform_device *pdev)
 #define s3c_rtc_resume  NULL
 #endif
 
+#ifdef CONFIG_OF
+static const struct of_device_id s3c_rtc_dt_match[] = {
+	{ .compatible = "samsung,s3c2410-rtc" },
+	{ .compatible = "samsung,s3c6410-rtc" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, s3c_rtc_dt_match);
+#else
+#define s3c_rtc_dt_match NULL
+#endif
+
 static struct platform_device_id s3c_rtc_driver_ids[] = {
 	{
 		.name		= "s3c2410-rtc",
@@ -651,24 +669,11 @@ static struct platform_driver s3c_rtc_driver = {
 	.driver		= {
 		.name	= "s3c-rtc",
 		.owner	= THIS_MODULE,
+		.of_match_table	= s3c_rtc_dt_match,
 	},
 };
 
-static char __initdata banner[] = "S3C24XX RTC, (c) 2004,2006 Simtec Electronics\n";
-
-static int __init s3c_rtc_init(void)
-{
-	printk(banner);
-	return platform_driver_register(&s3c_rtc_driver);
-}
-
-static void __exit s3c_rtc_exit(void)
-{
-	platform_driver_unregister(&s3c_rtc_driver);
-}
-
-module_init(s3c_rtc_init);
-module_exit(s3c_rtc_exit);
+module_platform_driver(s3c_rtc_driver);
 
 MODULE_DESCRIPTION("Samsung S3C RTC Driver");
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
diff --git a/drivers/rtc/rtc-sa1100.c b/drivers/rtc/rtc-sa1100.c
index 0b40bb88a884..4595d3e645a7 100644
--- a/drivers/rtc/rtc-sa1100.c
+++ b/drivers/rtc/rtc-sa1100.c
@@ -27,35 +27,42 @@
 #include <linux/init.h>
 #include <linux/fs.h>
 #include <linux/interrupt.h>
-#include <linux/string.h>
 #include <linux/pm.h>
-#include <linux/bitops.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/io.h>
 
 #include <mach/hardware.h>
 #include <asm/irq.h>
 
-#ifdef CONFIG_ARCH_PXA
-#include <mach/regs-rtc.h>
-#include <mach/regs-ost.h>
-#endif
-
 #define RTC_DEF_DIVIDER		(32768 - 1)
 #define RTC_DEF_TRIM		0
-
-static const unsigned long RTC_FREQ = 1024;
-static struct rtc_time rtc_alarm;
-static DEFINE_SPINLOCK(sa1100_rtc_lock);
-
-static inline int rtc_periodic_alarm(struct rtc_time *tm)
-{
-	return  (tm->tm_year == -1) ||
-		((unsigned)tm->tm_mon >= 12) ||
-		((unsigned)(tm->tm_mday - 1) >= 31) ||
-		((unsigned)tm->tm_hour > 23) ||
-		((unsigned)tm->tm_min > 59) ||
-		((unsigned)tm->tm_sec > 59);
-}
-
+#define RTC_FREQ		1024
+
+#define RCNR		0x00	/* RTC Count Register */
+#define RTAR		0x04	/* RTC Alarm Register */
+#define RTSR		0x08	/* RTC Status Register */
+#define RTTR		0x0c	/* RTC Timer Trim Register */
+
+#define RTSR_HZE	(1 << 3)	/* HZ interrupt enable */
+#define RTSR_ALE	(1 << 2)	/* RTC alarm interrupt enable */
+#define RTSR_HZ		(1 << 1)	/* HZ rising-edge detected */
+#define RTSR_AL		(1 << 0)	/* RTC alarm detected */
+
+#define rtc_readl(sa1100_rtc, reg)	\
+	readl_relaxed((sa1100_rtc)->base + (reg))
+#define rtc_writel(sa1100_rtc, reg, value)	\
+	writel_relaxed((value), (sa1100_rtc)->base + (reg))
+
+struct sa1100_rtc {
+	struct resource		*ress;
+	void __iomem		*base;
+	struct clk		*clk;
+	int			irq_1Hz;
+	int			irq_Alrm;
+	struct rtc_device	*rtc;
+	spinlock_t		lock;		/* Protects this structure */
+};
 /*
  * Calculate the next alarm time given the requested alarm time mask
  * and the current time.
@@ -83,46 +90,26 @@ static void rtc_next_alarm_time(struct rtc_time *next, struct rtc_time *now,
 	}
 }
 
-static int rtc_update_alarm(struct rtc_time *alrm)
-{
-	struct rtc_time alarm_tm, now_tm;
-	unsigned long now, time;
-	int ret;
-
-	do {
-		now = RCNR;
-		rtc_time_to_tm(now, &now_tm);
-		rtc_next_alarm_time(&alarm_tm, &now_tm, alrm);
-		ret = rtc_tm_to_time(&alarm_tm, &time);
-		if (ret != 0)
-			break;
-
-		RTSR = RTSR & (RTSR_HZE|RTSR_ALE|RTSR_AL);
-		RTAR = time;
-	} while (now != RCNR);
-
-	return ret;
-}
-
 static irqreturn_t sa1100_rtc_interrupt(int irq, void *dev_id)
 {
 	struct platform_device *pdev = to_platform_device(dev_id);
-	struct rtc_device *rtc = platform_get_drvdata(pdev);
+	struct sa1100_rtc *sa1100_rtc = platform_get_drvdata(pdev);
 	unsigned int rtsr;
 	unsigned long events = 0;
 
-	spin_lock(&sa1100_rtc_lock);
+	spin_lock(&sa1100_rtc->lock);
 
-	rtsr = RTSR;
 	/* clear interrupt sources */
-	RTSR = 0;
+	rtsr = rtc_readl(sa1100_rtc, RTSR);
+	rtc_writel(sa1100_rtc, RTSR, 0);
+
 	/* Fix for a nasty initialization problem the in SA11xx RTSR register.
 	 * See also the comments in sa1100_rtc_probe(). */
 	if (rtsr & (RTSR_ALE | RTSR_HZE)) {
 		/* This is the original code, before there was the if test
 		 * above. This code does not clear interrupts that were not
 		 * enabled. */
-		RTSR = (RTSR_AL | RTSR_HZ) & (rtsr >> 2);
+		rtc_writel(sa1100_rtc, RTSR, (RTSR_AL | RTSR_HZ) & (rtsr >> 2));
 	} else {
 		/* For some reason, it is possible to enter this routine
 		 * without interruptions enabled, it has been tested with
@@ -131,13 +118,13 @@ static irqreturn_t sa1100_rtc_interrupt(int irq, void *dev_id)
 		 * This situation leads to an infinite "loop" of interrupt
 		 * routine calling and as a result the processor seems to
 		 * lock on its first call to open(). */
-		RTSR = RTSR_AL | RTSR_HZ;
+		rtc_writel(sa1100_rtc, RTSR, (RTSR_AL | RTSR_HZ));
 	}
 
 	/* clear alarm interrupt if it has occurred */
 	if (rtsr & RTSR_AL)
 		rtsr &= ~RTSR_ALE;
-	RTSR = rtsr & (RTSR_ALE | RTSR_HZE);
+	rtc_writel(sa1100_rtc, RTSR, rtsr & (RTSR_ALE | RTSR_HZE));
 
 	/* update irq data & counter */
 	if (rtsr & RTSR_AL)
@@ -145,91 +132,100 @@ static irqreturn_t sa1100_rtc_interrupt(int irq, void *dev_id)
 	if (rtsr & RTSR_HZ)
 		events |= RTC_UF | RTC_IRQF;
 
-	rtc_update_irq(rtc, 1, events);
+	rtc_update_irq(sa1100_rtc->rtc, 1, events);
 
-	if (rtsr & RTSR_AL && rtc_periodic_alarm(&rtc_alarm))
-		rtc_update_alarm(&rtc_alarm);
-
-	spin_unlock(&sa1100_rtc_lock);
+	spin_unlock(&sa1100_rtc->lock);
 
 	return IRQ_HANDLED;
 }
 
 static int sa1100_rtc_open(struct device *dev)
 {
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
 	int ret;
-	struct platform_device *plat_dev = to_platform_device(dev);
-	struct rtc_device *rtc = platform_get_drvdata(plat_dev);
 
-	ret = request_irq(IRQ_RTC1Hz, sa1100_rtc_interrupt, IRQF_DISABLED,
-		"rtc 1Hz", dev);
+	ret = request_irq(sa1100_rtc->irq_1Hz, sa1100_rtc_interrupt,
+				IRQF_DISABLED, "rtc 1Hz", dev);
 	if (ret) {
-		dev_err(dev, "IRQ %d already in use.\n", IRQ_RTC1Hz);
+		dev_err(dev, "IRQ %d already in use.\n", sa1100_rtc->irq_1Hz);
 		goto fail_ui;
 	}
-	ret = request_irq(IRQ_RTCAlrm, sa1100_rtc_interrupt, IRQF_DISABLED,
-		"rtc Alrm", dev);
+	ret = request_irq(sa1100_rtc->irq_Alrm, sa1100_rtc_interrupt,
+				IRQF_DISABLED, "rtc Alrm", dev);
 	if (ret) {
-		dev_err(dev, "IRQ %d already in use.\n", IRQ_RTCAlrm);
+		dev_err(dev, "IRQ %d already in use.\n", sa1100_rtc->irq_Alrm);
 		goto fail_ai;
 	}
-	rtc->max_user_freq = RTC_FREQ;
-	rtc_irq_set_freq(rtc, NULL, RTC_FREQ);
+	sa1100_rtc->rtc->max_user_freq = RTC_FREQ;
+	rtc_irq_set_freq(sa1100_rtc->rtc, NULL, RTC_FREQ);
 
 	return 0;
 
  fail_ai:
-	free_irq(IRQ_RTC1Hz, dev);
+	free_irq(sa1100_rtc->irq_1Hz, dev);
  fail_ui:
 	return ret;
 }
 
 static void sa1100_rtc_release(struct device *dev)
 {
-	spin_lock_irq(&sa1100_rtc_lock);
-	RTSR = 0;
-	OIER &= ~OIER_E1;
-	OSSR = OSSR_M1;
-	spin_unlock_irq(&sa1100_rtc_lock);
-
-	free_irq(IRQ_RTCAlrm, dev);
-	free_irq(IRQ_RTC1Hz, dev);
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
+
+	spin_lock_irq(&sa1100_rtc->lock);
+	rtc_writel(sa1100_rtc, RTSR, 0);
+	spin_unlock_irq(&sa1100_rtc->lock);
+
+	free_irq(sa1100_rtc->irq_Alrm, dev);
+	free_irq(sa1100_rtc->irq_1Hz, dev);
 }
 
 static int sa1100_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
 {
-	spin_lock_irq(&sa1100_rtc_lock);
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
+	unsigned int rtsr;
+
+	spin_lock_irq(&sa1100_rtc->lock);
+
+	rtsr = rtc_readl(sa1100_rtc, RTSR);
 	if (enabled)
-		RTSR |= RTSR_ALE;
+		rtsr |= RTSR_ALE;
 	else
-		RTSR &= ~RTSR_ALE;
-	spin_unlock_irq(&sa1100_rtc_lock);
+		rtsr &= ~RTSR_ALE;
+	rtc_writel(sa1100_rtc, RTSR, rtsr);
+
+	spin_unlock_irq(&sa1100_rtc->lock);
 	return 0;
 }
 
 static int sa1100_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
-	rtc_time_to_tm(RCNR, tm);
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
+
+	rtc_time_to_tm(rtc_readl(sa1100_rtc, RCNR), tm);
 	return 0;
 }
 
 static int sa1100_rtc_set_time(struct device *dev, struct rtc_time *tm)
 {
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
 	unsigned long time;
 	int ret;
 
 	ret = rtc_tm_to_time(tm, &time);
 	if (ret == 0)
-		RCNR = time;
+		rtc_writel(sa1100_rtc, RCNR, time);
 	return ret;
 }
 
 static int sa1100_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 {
-	u32	rtsr;
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
+	unsigned long time;
+	unsigned int rtsr;
 
-	memcpy(&alrm->time, &rtc_alarm, sizeof(struct rtc_time));
-	rtsr = RTSR;
+	time = rtc_readl(sa1100_rtc, RCNR);
+	rtc_time_to_tm(time, &alrm->time);
+	rtsr = rtc_readl(sa1100_rtc, RTSR);
 	alrm->enabled = (rtsr & RTSR_ALE) ? 1 : 0;
 	alrm->pending = (rtsr & RTSR_AL) ? 1 : 0;
 	return 0;
@@ -237,26 +233,39 @@ static int sa1100_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 
 static int sa1100_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 {
-	int ret;
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
+	struct rtc_time now_tm, alarm_tm;
+	unsigned long time, alarm;
+	unsigned int rtsr;
 
-	spin_lock_irq(&sa1100_rtc_lock);
-	ret = rtc_update_alarm(&alrm->time);
-	if (ret == 0) {
-		if (alrm->enabled)
-			RTSR |= RTSR_ALE;
-		else
-			RTSR &= ~RTSR_ALE;
-	}
-	spin_unlock_irq(&sa1100_rtc_lock);
+	spin_lock_irq(&sa1100_rtc->lock);
 
-	return ret;
+	time = rtc_readl(sa1100_rtc, RCNR);
+	rtc_time_to_tm(time, &now_tm);
+	rtc_next_alarm_time(&alarm_tm, &now_tm, &alrm->time);
+	rtc_tm_to_time(&alarm_tm, &alarm);
+	rtc_writel(sa1100_rtc, RTAR, alarm);
+
+	rtsr = rtc_readl(sa1100_rtc, RTSR);
+	if (alrm->enabled)
+		rtsr |= RTSR_ALE;
+	else
+		rtsr &= ~RTSR_ALE;
+	rtc_writel(sa1100_rtc, RTSR, rtsr);
+
+	spin_unlock_irq(&sa1100_rtc->lock);
+
+	return 0;
 }
 
 static int sa1100_rtc_proc(struct device *dev, struct seq_file *seq)
 {
-	seq_printf(seq, "trim/divider\t\t: 0x%08x\n", (u32) RTTR);
-	seq_printf(seq, "RTSR\t\t\t: 0x%08x\n", (u32)RTSR);
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
 
+	seq_printf(seq, "trim/divider\t\t: 0x%08x\n",
+			rtc_readl(sa1100_rtc, RTTR));
+	seq_printf(seq, "RTSR\t\t\t: 0x%08x\n",
+			rtc_readl(sa1100_rtc, RTSR));
 	return 0;
 }
 
@@ -273,7 +282,51 @@ static const struct rtc_class_ops sa1100_rtc_ops = {
 
 static int sa1100_rtc_probe(struct platform_device *pdev)
 {
-	struct rtc_device *rtc;
+	struct sa1100_rtc *sa1100_rtc;
+	unsigned int rttr;
+	int ret;
+
+	sa1100_rtc = kzalloc(sizeof(struct sa1100_rtc), GFP_KERNEL);
+	if (!sa1100_rtc)
+		return -ENOMEM;
+
+	spin_lock_init(&sa1100_rtc->lock);
+	platform_set_drvdata(pdev, sa1100_rtc);
+
+	ret = -ENXIO;
+	sa1100_rtc->ress = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!sa1100_rtc->ress) {
+		dev_err(&pdev->dev, "No I/O memory resource defined\n");
+		goto err_ress;
+	}
+
+	sa1100_rtc->irq_1Hz = platform_get_irq(pdev, 0);
+	if (sa1100_rtc->irq_1Hz < 0) {
+		dev_err(&pdev->dev, "No 1Hz IRQ resource defined\n");
+		goto err_ress;
+	}
+	sa1100_rtc->irq_Alrm = platform_get_irq(pdev, 1);
+	if (sa1100_rtc->irq_Alrm < 0) {
+		dev_err(&pdev->dev, "No alarm IRQ resource defined\n");
+		goto err_ress;
+	}
+
+	ret = -ENOMEM;
+	sa1100_rtc->base = ioremap(sa1100_rtc->ress->start,
+				resource_size(sa1100_rtc->ress));
+	if (!sa1100_rtc->base) {
+		dev_err(&pdev->dev, "Unable to map pxa RTC I/O memory\n");
+		goto err_map;
+	}
+
+	sa1100_rtc->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(sa1100_rtc->clk)) {
+		dev_err(&pdev->dev, "failed to find rtc clock source\n");
+		ret = PTR_ERR(sa1100_rtc->clk);
+		goto err_clk;
+	}
+	clk_prepare(sa1100_rtc->clk);
+	clk_enable(sa1100_rtc->clk);
 
 	/*
 	 * According to the manual we should be able to let RTTR be zero
@@ -282,24 +335,24 @@ static int sa1100_rtc_probe(struct platform_device *pdev)
 	 * If the clock divider is uninitialized then reset it to the
 	 * default value to get the 1Hz clock.
 	 */
-	if (RTTR == 0) {
-		RTTR = RTC_DEF_DIVIDER + (RTC_DEF_TRIM << 16);
-		dev_warn(&pdev->dev, "warning: "
-			"initializing default clock divider/trim value\n");
+	if (rtc_readl(sa1100_rtc, RTTR) == 0) {
+		rttr = RTC_DEF_DIVIDER + (RTC_DEF_TRIM << 16);
+		rtc_writel(sa1100_rtc, RTTR, rttr);
+		dev_warn(&pdev->dev, "warning: initializing default clock"
+			 " divider/trim value\n");
 		/* The current RTC value probably doesn't make sense either */
-		RCNR = 0;
+		rtc_writel(sa1100_rtc, RCNR, 0);
 	}
 
 	device_init_wakeup(&pdev->dev, 1);
 
-	rtc = rtc_device_register(pdev->name, &pdev->dev, &sa1100_rtc_ops,
-		THIS_MODULE);
-
-	if (IS_ERR(rtc))
-		return PTR_ERR(rtc);
-
-	platform_set_drvdata(pdev, rtc);
-
+	sa1100_rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+						&sa1100_rtc_ops, THIS_MODULE);
+	if (IS_ERR(sa1100_rtc->rtc)) {
+		dev_err(&pdev->dev, "Failed to register RTC device -> %d\n",
+			ret);
+		goto err_rtc_reg;
+	}
 	/* Fix for a nasty initialization problem the in SA11xx RTSR register.
 	 * See also the comments in sa1100_rtc_interrupt().
 	 *
@@ -322,33 +375,46 @@ static int sa1100_rtc_probe(struct platform_device *pdev)
 	 *
 	 * Notice that clearing bit 1 and 0 is accomplished by writting ONES to
 	 * the corresponding bits in RTSR. */
-	RTSR = RTSR_AL | RTSR_HZ;
+	rtc_writel(sa1100_rtc, RTSR, (RTSR_AL | RTSR_HZ));
 
 	return 0;
+
+err_rtc_reg:
+err_clk:
+	iounmap(sa1100_rtc->base);
+err_ress:
+err_map:
+	kfree(sa1100_rtc);
+	return ret;
 }
 
 static int sa1100_rtc_remove(struct platform_device *pdev)
 {
-	struct rtc_device *rtc = platform_get_drvdata(pdev);
-
-	if (rtc)
-		rtc_device_unregister(rtc);
+	struct sa1100_rtc *sa1100_rtc = platform_get_drvdata(pdev);
 
+	rtc_device_unregister(sa1100_rtc->rtc);
+	clk_disable(sa1100_rtc->clk);
+	clk_unprepare(sa1100_rtc->clk);
+	iounmap(sa1100_rtc->base);
 	return 0;
 }
 
 #ifdef CONFIG_PM
 static int sa1100_rtc_suspend(struct device *dev)
 {
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
+
 	if (device_may_wakeup(dev))
-		enable_irq_wake(IRQ_RTCAlrm);
+		enable_irq_wake(sa1100_rtc->irq_Alrm);
 	return 0;
 }
 
 static int sa1100_rtc_resume(struct device *dev)
 {
+	struct sa1100_rtc *sa1100_rtc = dev_get_drvdata(dev);
+
 	if (device_may_wakeup(dev))
-		disable_irq_wake(IRQ_RTCAlrm);
+		disable_irq_wake(sa1100_rtc->irq_Alrm);
 	return 0;
 }
 
@@ -369,18 +435,7 @@ static struct platform_driver sa1100_rtc_driver = {
 	},
 };
 
-static int __init sa1100_rtc_init(void)
-{
-	return platform_driver_register(&sa1100_rtc_driver);
-}
-
-static void __exit sa1100_rtc_exit(void)
-{
-	platform_driver_unregister(&sa1100_rtc_driver);
-}
-
-module_init(sa1100_rtc_init);
-module_exit(sa1100_rtc_exit);
+module_platform_driver(sa1100_rtc_driver);
 
 MODULE_AUTHOR("Richard Purdie <rpurdie@rpsys.net>");
 MODULE_DESCRIPTION("SA11x0/PXA2xx Realtime Clock Driver (RTC)");
diff --git a/drivers/rtc/rtc-spear.c b/drivers/rtc/rtc-spear.c
index 893bac2bb21b..19a28a671a8e 100644
--- a/drivers/rtc/rtc-spear.c
+++ b/drivers/rtc/rtc-spear.c
@@ -516,17 +516,7 @@ static struct platform_driver spear_rtc_driver = {
 	},
 };
 
-static int __init rtc_init(void)
-{
-	return platform_driver_register(&spear_rtc_driver);
-}
-module_init(rtc_init);
-
-static void __exit rtc_exit(void)
-{
-	platform_driver_unregister(&spear_rtc_driver);
-}
-module_exit(rtc_exit);
+module_platform_driver(spear_rtc_driver);
 
 MODULE_ALIAS("platform:rtc-spear");
 MODULE_AUTHOR("Rajeev Kumar <rajeev-dlh.kumar@st.com>");
diff --git a/drivers/rtc/rtc-stk17ta8.c b/drivers/rtc/rtc-stk17ta8.c
index ed3e9b599031..7621116bd20d 100644
--- a/drivers/rtc/rtc-stk17ta8.c
+++ b/drivers/rtc/rtc-stk17ta8.c
@@ -370,18 +370,7 @@ static struct platform_driver stk17ta8_rtc_driver = {
 	},
 };
 
-static __init int stk17ta8_init(void)
-{
-	return platform_driver_register(&stk17ta8_rtc_driver);
-}
-
-static __exit void stk17ta8_exit(void)
-{
-	platform_driver_unregister(&stk17ta8_rtc_driver);
-}
-
-module_init(stk17ta8_init);
-module_exit(stk17ta8_exit);
+module_platform_driver(stk17ta8_rtc_driver);
 
 MODULE_AUTHOR("Thomas Hommel <thomas.hommel@ge.com>");
 MODULE_DESCRIPTION("Simtek STK17TA8 RTC driver");
diff --git a/drivers/rtc/rtc-stmp3xxx.c b/drivers/rtc/rtc-stmp3xxx.c
index 7315068daa59..10287865e330 100644
--- a/drivers/rtc/rtc-stmp3xxx.c
+++ b/drivers/rtc/rtc-stmp3xxx.c
@@ -276,18 +276,7 @@ static struct platform_driver stmp3xxx_rtcdrv = {
 	},
 };
 
-static int __init stmp3xxx_rtc_init(void)
-{
-	return platform_driver_register(&stmp3xxx_rtcdrv);
-}
-
-static void __exit stmp3xxx_rtc_exit(void)
-{
-	platform_driver_unregister(&stmp3xxx_rtcdrv);
-}
-
-module_init(stmp3xxx_rtc_init);
-module_exit(stmp3xxx_rtc_exit);
+module_platform_driver(stmp3xxx_rtcdrv);
 
 MODULE_DESCRIPTION("STMP3xxx RTC Driver");
 MODULE_AUTHOR("dmitry pervushin <dpervushin@embeddedalley.com> and "
diff --git a/drivers/rtc/rtc-twl.c b/drivers/rtc/rtc-twl.c
index 20687d55e7a7..d43b4f6eb4e4 100644
--- a/drivers/rtc/rtc-twl.c
+++ b/drivers/rtc/rtc-twl.c
@@ -550,6 +550,11 @@ static int twl_rtc_resume(struct platform_device *pdev)
 #define twl_rtc_resume  NULL
 #endif
 
+static const struct of_device_id twl_rtc_of_match[] = {
+	{.compatible = "ti,twl4030-rtc", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, twl_rtc_of_match);
 MODULE_ALIAS("platform:twl_rtc");
 
 static struct platform_driver twl4030rtc_driver = {
@@ -559,8 +564,9 @@ static struct platform_driver twl4030rtc_driver = {
 	.suspend	= twl_rtc_suspend,
 	.resume		= twl_rtc_resume,
 	.driver		= {
-		.owner	= THIS_MODULE,
-		.name	= "twl_rtc",
+		.owner		= THIS_MODULE,
+		.name		= "twl_rtc",
+		.of_match_table = twl_rtc_of_match,
 	},
 };
 
diff --git a/drivers/rtc/rtc-v3020.c b/drivers/rtc/rtc-v3020.c
index f71c3ce18036..bca5d677bc85 100644
--- a/drivers/rtc/rtc-v3020.c
+++ b/drivers/rtc/rtc-v3020.c
@@ -393,18 +393,7 @@ static struct platform_driver rtc_device_driver = {
 	},
 };
 
-static __init int v3020_init(void)
-{
-	return platform_driver_register(&rtc_device_driver);
-}
-
-static __exit void v3020_exit(void)
-{
-	platform_driver_unregister(&rtc_device_driver);
-}
-
-module_init(v3020_init);
-module_exit(v3020_exit);
+module_platform_driver(rtc_device_driver);
 
 MODULE_DESCRIPTION("V3020 RTC");
 MODULE_AUTHOR("Raphael Assenat");
diff --git a/drivers/rtc/rtc-vr41xx.c b/drivers/rtc/rtc-vr41xx.c
index c5698cda366a..fcbfdda2993b 100644
--- a/drivers/rtc/rtc-vr41xx.c
+++ b/drivers/rtc/rtc-vr41xx.c
@@ -405,15 +405,4 @@ static struct platform_driver rtc_platform_driver = {
 	},
 };
 
-static int __init vr41xx_rtc_init(void)
-{
-	return platform_driver_register(&rtc_platform_driver);
-}
-
-static void __exit vr41xx_rtc_exit(void)
-{
-	platform_driver_unregister(&rtc_platform_driver);
-}
-
-module_init(vr41xx_rtc_init);
-module_exit(vr41xx_rtc_exit);
+module_platform_driver(rtc_platform_driver);
diff --git a/drivers/rtc/rtc-vt8500.c b/drivers/rtc/rtc-vt8500.c
index f93f412423c6..9e94fb147c26 100644
--- a/drivers/rtc/rtc-vt8500.c
+++ b/drivers/rtc/rtc-vt8500.c
@@ -311,17 +311,7 @@ static struct platform_driver vt8500_rtc_driver = {
 	},
 };
 
-static int __init vt8500_rtc_init(void)
-{
-	return platform_driver_register(&vt8500_rtc_driver);
-}
-module_init(vt8500_rtc_init);
-
-static void __exit vt8500_rtc_exit(void)
-{
-	platform_driver_unregister(&vt8500_rtc_driver);
-}
-module_exit(vt8500_rtc_exit);
+module_platform_driver(vt8500_rtc_driver);
 
 MODULE_AUTHOR("Alexey Charkov <alchark@gmail.com>");
 MODULE_DESCRIPTION("VIA VT8500 SoC Realtime Clock Driver (RTC)");
diff --git a/drivers/rtc/rtc-wm831x.c b/drivers/rtc/rtc-wm831x.c
index bdc909bd56da..3b6e6a67e765 100644
--- a/drivers/rtc/rtc-wm831x.c
+++ b/drivers/rtc/rtc-wm831x.c
@@ -324,15 +324,6 @@ static irqreturn_t wm831x_alm_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t wm831x_per_irq(int irq, void *data)
-{
-	struct wm831x_rtc *wm831x_rtc = data;
-
-	rtc_update_irq(wm831x_rtc->rtc, 1, RTC_IRQF | RTC_UF);
-
-	return IRQ_HANDLED;
-}
-
 static const struct rtc_class_ops wm831x_rtc_ops = {
 	.read_time = wm831x_rtc_readtime,
 	.set_mmss = wm831x_rtc_set_mmss,
@@ -405,11 +396,10 @@ static int wm831x_rtc_probe(struct platform_device *pdev)
 {
 	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
 	struct wm831x_rtc *wm831x_rtc;
-	int per_irq = platform_get_irq_byname(pdev, "PER");
 	int alm_irq = platform_get_irq_byname(pdev, "ALM");
 	int ret = 0;
 
-	wm831x_rtc = kzalloc(sizeof(*wm831x_rtc), GFP_KERNEL);
+	wm831x_rtc = devm_kzalloc(&pdev->dev, sizeof(*wm831x_rtc), GFP_KERNEL);
 	if (wm831x_rtc == NULL)
 		return -ENOMEM;
 
@@ -433,14 +423,6 @@ static int wm831x_rtc_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	ret = request_threaded_irq(per_irq, NULL, wm831x_per_irq,
-				   IRQF_TRIGGER_RISING, "RTC period",
-				   wm831x_rtc);
-	if (ret != 0) {
-		dev_err(&pdev->dev, "Failed to request periodic IRQ %d: %d\n",
-			per_irq, ret);
-	}
-
 	ret = request_threaded_irq(alm_irq, NULL, wm831x_alm_irq,
 				   IRQF_TRIGGER_RISING, "RTC alarm",
 				   wm831x_rtc);
@@ -452,20 +434,16 @@ static int wm831x_rtc_probe(struct platform_device *pdev)
 	return 0;
 
 err:
-	kfree(wm831x_rtc);
 	return ret;
 }
 
 static int __devexit wm831x_rtc_remove(struct platform_device *pdev)
 {
 	struct wm831x_rtc *wm831x_rtc = platform_get_drvdata(pdev);
-	int per_irq = platform_get_irq_byname(pdev, "PER");
 	int alm_irq = platform_get_irq_byname(pdev, "ALM");
 
 	free_irq(alm_irq, wm831x_rtc);
-	free_irq(per_irq, wm831x_rtc);
 	rtc_device_unregister(wm831x_rtc->rtc);
-	kfree(wm831x_rtc);
 
 	return 0;
 }
@@ -490,17 +468,7 @@ static struct platform_driver wm831x_rtc_driver = {
 	},
 };
 
-static int __init wm831x_rtc_init(void)
-{
-	return platform_driver_register(&wm831x_rtc_driver);
-}
-module_init(wm831x_rtc_init);
-
-static void __exit wm831x_rtc_exit(void)
-{
-	platform_driver_unregister(&wm831x_rtc_driver);
-}
-module_exit(wm831x_rtc_exit);
+module_platform_driver(wm831x_rtc_driver);
 
 MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
 MODULE_DESCRIPTION("RTC driver for the WM831x series PMICs");
diff --git a/drivers/rtc/rtc-wm8350.c b/drivers/rtc/rtc-wm8350.c
index 66421426e404..c2e52d15abb2 100644
--- a/drivers/rtc/rtc-wm8350.c
+++ b/drivers/rtc/rtc-wm8350.c
@@ -486,17 +486,7 @@ static struct platform_driver wm8350_rtc_driver = {
 	},
 };
 
-static int __init wm8350_rtc_init(void)
-{
-	return platform_driver_register(&wm8350_rtc_driver);
-}
-module_init(wm8350_rtc_init);
-
-static void __exit wm8350_rtc_exit(void)
-{
-	platform_driver_unregister(&wm8350_rtc_driver);
-}
-module_exit(wm8350_rtc_exit);
+module_platform_driver(wm8350_rtc_driver);
 
 MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com>");
 MODULE_DESCRIPTION("RTC driver for the WM8350");
diff --git a/drivers/s390/block/dasd_3990_erp.c b/drivers/s390/block/dasd_3990_erp.c
index 87a0cf160fe5..0326571e7ffa 100644
--- a/drivers/s390/block/dasd_3990_erp.c
+++ b/drivers/s390/block/dasd_3990_erp.c
@@ -1718,7 +1718,7 @@ dasd_3990_erp_action_1B_32(struct dasd_ccw_req * default_erp, char *sense)
 	erp->startdev = device;
 	erp->memdev = device;
 	erp->magic = default_erp->magic;
-	erp->expires = 0;
+	erp->expires = default_erp->expires;
 	erp->retries = 256;
 	erp->buildclk = get_clock();
 	erp->status = DASD_CQR_FILLED;
@@ -2363,7 +2363,7 @@ static struct dasd_ccw_req *dasd_3990_erp_add_erp(struct dasd_ccw_req *cqr)
 	erp->memdev   = device;
 	erp->block    = cqr->block;
 	erp->magic    = cqr->magic;
-	erp->expires  = 0;
+	erp->expires  = cqr->expires;
 	erp->retries  = 256;
 	erp->buildclk = get_clock();
 	erp->status = DASD_CQR_FILLED;
diff --git a/drivers/s390/block/dasd_alias.c b/drivers/s390/block/dasd_alias.c
index c388eda1e2b1..553b3c5abb0a 100644
--- a/drivers/s390/block/dasd_alias.c
+++ b/drivers/s390/block/dasd_alias.c
@@ -705,6 +705,16 @@ struct dasd_device *dasd_alias_get_start_dev(struct dasd_device *base_device)
 	if (lcu->pav == NO_PAV ||
 	    lcu->flags & (NEED_UAC_UPDATE | UPDATE_PENDING))
 		return NULL;
+	if (unlikely(!(private->features.feature[8] & 0x01))) {
+		/*
+		 * PAV enabled but prefix not, very unlikely
+		 * seems to be a lost pathgroup
+		 * use base device to do IO
+		 */
+		DBF_DEV_EVENT(DBF_ERR, base_device, "%s",
+			      "Prefix not enabled with PAV enabled\n");
+		return NULL;
+	}
 
 	spin_lock_irqsave(&lcu->lock, flags);
 	alias_device = group->next;
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index 6ab29680586a..bbcd5e9206ee 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -752,24 +752,13 @@ dasd_eckd_cdl_reclen(int recid)
 		return sizes_trk0[recid];
 	return LABEL_SIZE;
 }
-
-/*
- * Generate device unique id that specifies the physical device.
- */
-static int dasd_eckd_generate_uid(struct dasd_device *device)
+/* create unique id from private structure. */
+static void create_uid(struct dasd_eckd_private *private)
 {
-	struct dasd_eckd_private *private;
-	struct dasd_uid *uid;
 	int count;
-	unsigned long flags;
+	struct dasd_uid *uid;
 
-	private = (struct dasd_eckd_private *) device->private;
-	if (!private)
-		return -ENODEV;
-	if (!private->ned || !private->gneq)
-		return -ENODEV;
 	uid = &private->uid;
-	spin_lock_irqsave(get_ccwdev_lock(device->cdev), flags);
 	memset(uid, 0, sizeof(struct dasd_uid));
 	memcpy(uid->vendor, private->ned->HDA_manufacturer,
 	       sizeof(uid->vendor) - 1);
@@ -792,6 +781,23 @@ static int dasd_eckd_generate_uid(struct dasd_device *device)
 				private->vdsneq->uit[count]);
 		}
 	}
+}
+
+/*
+ * Generate device unique id that specifies the physical device.
+ */
+static int dasd_eckd_generate_uid(struct dasd_device *device)
+{
+	struct dasd_eckd_private *private;
+	unsigned long flags;
+
+	private = (struct dasd_eckd_private *) device->private;
+	if (!private)
+		return -ENODEV;
+	if (!private->ned || !private->gneq)
+		return -ENODEV;
+	spin_lock_irqsave(get_ccwdev_lock(device->cdev), flags);
+	create_uid(private);
 	spin_unlock_irqrestore(get_ccwdev_lock(device->cdev), flags);
 	return 0;
 }
@@ -811,6 +817,21 @@ static int dasd_eckd_get_uid(struct dasd_device *device, struct dasd_uid *uid)
 	return -EINVAL;
 }
 
+/*
+ * compare device UID with data of a given dasd_eckd_private structure
+ * return 0 for match
+ */
+static int dasd_eckd_compare_path_uid(struct dasd_device *device,
+				      struct dasd_eckd_private *private)
+{
+	struct dasd_uid device_uid;
+
+	create_uid(private);
+	dasd_eckd_get_uid(device, &device_uid);
+
+	return memcmp(&device_uid, &private->uid, sizeof(struct dasd_uid));
+}
+
 static void dasd_eckd_fill_rcd_cqr(struct dasd_device *device,
 				   struct dasd_ccw_req *cqr,
 				   __u8 *rcd_buffer,
@@ -1005,59 +1026,120 @@ static int dasd_eckd_read_conf(struct dasd_device *device)
 	int conf_len, conf_data_saved;
 	int rc;
 	__u8 lpm, opm;
-	struct dasd_eckd_private *private;
+	struct dasd_eckd_private *private, path_private;
 	struct dasd_path *path_data;
+	struct dasd_uid *uid;
+	char print_path_uid[60], print_device_uid[60];
 
 	private = (struct dasd_eckd_private *) device->private;
 	path_data = &device->path_data;
 	opm = ccw_device_get_path_mask(device->cdev);
-	lpm = 0x80;
 	conf_data_saved = 0;
 	/* get configuration data per operational path */
 	for (lpm = 0x80; lpm; lpm>>= 1) {
-		if (lpm & opm) {
-			rc = dasd_eckd_read_conf_lpm(device, &conf_data,
-						     &conf_len, lpm);
-			if (rc && rc != -EOPNOTSUPP) {	/* -EOPNOTSUPP is ok */
-				DBF_EVENT_DEVID(DBF_WARNING, device->cdev,
-					  "Read configuration data returned "
-					  "error %d", rc);
-				return rc;
-			}
-			if (conf_data == NULL) {
-				DBF_EVENT_DEVID(DBF_WARNING, device->cdev, "%s",
-						"No configuration data "
-						"retrieved");
-				/* no further analysis possible */
-				path_data->opm |= lpm;
-				continue;	/* no error */
+		if (!(lpm & opm))
+			continue;
+		rc = dasd_eckd_read_conf_lpm(device, &conf_data,
+					     &conf_len, lpm);
+		if (rc && rc != -EOPNOTSUPP) {	/* -EOPNOTSUPP is ok */
+			DBF_EVENT_DEVID(DBF_WARNING, device->cdev,
+					"Read configuration data returned "
+					"error %d", rc);
+			return rc;
+		}
+		if (conf_data == NULL) {
+			DBF_EVENT_DEVID(DBF_WARNING, device->cdev, "%s",
+					"No configuration data "
+					"retrieved");
+			/* no further analysis possible */
+			path_data->opm |= lpm;
+			continue;	/* no error */
+		}
+		/* save first valid configuration data */
+		if (!conf_data_saved) {
+			kfree(private->conf_data);
+			private->conf_data = conf_data;
+			private->conf_len = conf_len;
+			if (dasd_eckd_identify_conf_parts(private)) {
+				private->conf_data = NULL;
+				private->conf_len = 0;
+				kfree(conf_data);
+				continue;
 			}
-			/* save first valid configuration data */
-			if (!conf_data_saved) {
-				kfree(private->conf_data);
-				private->conf_data = conf_data;
-				private->conf_len = conf_len;
-				if (dasd_eckd_identify_conf_parts(private)) {
-					private->conf_data = NULL;
-					private->conf_len = 0;
-					kfree(conf_data);
-					continue;
-				}
-				conf_data_saved++;
+			/*
+			 * build device UID that other path data
+			 * can be compared to it
+			 */
+			dasd_eckd_generate_uid(device);
+			conf_data_saved++;
+		} else {
+			path_private.conf_data = conf_data;
+			path_private.conf_len = DASD_ECKD_RCD_DATA_SIZE;
+			if (dasd_eckd_identify_conf_parts(
+				    &path_private)) {
+				path_private.conf_data = NULL;
+				path_private.conf_len = 0;
+				kfree(conf_data);
+				continue;
 			}
-			switch (dasd_eckd_path_access(conf_data, conf_len)) {
-			case 0x02:
-				path_data->npm |= lpm;
-				break;
-			case 0x03:
-				path_data->ppm |= lpm;
-				break;
+
+			if (dasd_eckd_compare_path_uid(
+				    device, &path_private)) {
+				uid = &path_private.uid;
+				if (strlen(uid->vduit) > 0)
+					snprintf(print_path_uid,
+						 sizeof(print_path_uid),
+						 "%s.%s.%04x.%02x.%s",
+						 uid->vendor, uid->serial,
+						 uid->ssid, uid->real_unit_addr,
+						 uid->vduit);
+				else
+					snprintf(print_path_uid,
+						 sizeof(print_path_uid),
+						 "%s.%s.%04x.%02x",
+						 uid->vendor, uid->serial,
+						 uid->ssid,
+						 uid->real_unit_addr);
+				uid = &private->uid;
+				if (strlen(uid->vduit) > 0)
+					snprintf(print_device_uid,
+						 sizeof(print_device_uid),
+						 "%s.%s.%04x.%02x.%s",
+						 uid->vendor, uid->serial,
+						 uid->ssid, uid->real_unit_addr,
+						 uid->vduit);
+				else
+					snprintf(print_device_uid,
+						 sizeof(print_device_uid),
+						 "%s.%s.%04x.%02x",
+						 uid->vendor, uid->serial,
+						 uid->ssid,
+						 uid->real_unit_addr);
+				dev_err(&device->cdev->dev,
+					"Not all channel paths lead to "
+					"the same device, path %02X leads to "
+					"device %s instead of %s\n", lpm,
+					print_path_uid, print_device_uid);
+				return -EINVAL;
 			}
-			path_data->opm |= lpm;
-			if (conf_data != private->conf_data)
-				kfree(conf_data);
+
+			path_private.conf_data = NULL;
+			path_private.conf_len = 0;
 		}
+		switch (dasd_eckd_path_access(conf_data, conf_len)) {
+		case 0x02:
+			path_data->npm |= lpm;
+			break;
+		case 0x03:
+			path_data->ppm |= lpm;
+			break;
+		}
+		path_data->opm |= lpm;
+
+		if (conf_data != private->conf_data)
+			kfree(conf_data);
 	}
+
 	return 0;
 }
 
@@ -1090,12 +1172,61 @@ static int verify_fcx_max_data(struct dasd_device *device, __u8 lpm)
 	return 0;
 }
 
+static int rebuild_device_uid(struct dasd_device *device,
+			      struct path_verification_work_data *data)
+{
+	struct dasd_eckd_private *private;
+	struct dasd_path *path_data;
+	__u8 lpm, opm;
+	int rc;
+
+	rc = -ENODEV;
+	private = (struct dasd_eckd_private *) device->private;
+	path_data = &device->path_data;
+	opm = device->path_data.opm;
+
+	for (lpm = 0x80; lpm; lpm >>= 1) {
+		if (!(lpm & opm))
+			continue;
+		memset(&data->rcd_buffer, 0, sizeof(data->rcd_buffer));
+		memset(&data->cqr, 0, sizeof(data->cqr));
+		data->cqr.cpaddr = &data->ccw;
+		rc = dasd_eckd_read_conf_immediately(device, &data->cqr,
+						     data->rcd_buffer,
+						     lpm);
+
+		if (rc) {
+			if (rc == -EOPNOTSUPP) /* -EOPNOTSUPP is ok */
+				continue;
+			DBF_EVENT_DEVID(DBF_WARNING, device->cdev,
+					"Read configuration data "
+					"returned error %d", rc);
+			break;
+		}
+		memcpy(private->conf_data, data->rcd_buffer,
+		       DASD_ECKD_RCD_DATA_SIZE);
+		if (dasd_eckd_identify_conf_parts(private)) {
+			rc = -ENODEV;
+		} else /* first valid path is enough */
+			break;
+	}
+
+	if (!rc)
+		rc = dasd_eckd_generate_uid(device);
+
+	return rc;
+}
+
 static void do_path_verification_work(struct work_struct *work)
 {
 	struct path_verification_work_data *data;
 	struct dasd_device *device;
+	struct dasd_eckd_private path_private;
+	struct dasd_uid *uid;
+	__u8 path_rcd_buf[DASD_ECKD_RCD_DATA_SIZE];
 	__u8 lpm, opm, npm, ppm, epm;
 	unsigned long flags;
+	char print_uid[60];
 	int rc;
 
 	data = container_of(work, struct path_verification_work_data, worker);
@@ -1112,64 +1243,129 @@ static void do_path_verification_work(struct work_struct *work)
 	ppm = 0;
 	epm = 0;
 	for (lpm = 0x80; lpm; lpm >>= 1) {
-		if (lpm & data->tbvpm) {
-			memset(data->rcd_buffer, 0, sizeof(data->rcd_buffer));
-			memset(&data->cqr, 0, sizeof(data->cqr));
-			data->cqr.cpaddr = &data->ccw;
-			rc = dasd_eckd_read_conf_immediately(device, &data->cqr,
-							     data->rcd_buffer,
-							     lpm);
-			if (!rc) {
-				switch (dasd_eckd_path_access(data->rcd_buffer,
-						     DASD_ECKD_RCD_DATA_SIZE)) {
-				case 0x02:
-					npm |= lpm;
-					break;
-				case 0x03:
-					ppm |= lpm;
-					break;
-				}
-				opm |= lpm;
-			} else if (rc == -EOPNOTSUPP) {
-				DBF_EVENT_DEVID(DBF_WARNING, device->cdev, "%s",
-				       "path verification: No configuration "
-				       "data retrieved");
-				opm |= lpm;
-			} else if (rc == -EAGAIN) {
-				DBF_EVENT_DEVID(DBF_WARNING, device->cdev, "%s",
+		if (!(lpm & data->tbvpm))
+			continue;
+		memset(&data->rcd_buffer, 0, sizeof(data->rcd_buffer));
+		memset(&data->cqr, 0, sizeof(data->cqr));
+		data->cqr.cpaddr = &data->ccw;
+		rc = dasd_eckd_read_conf_immediately(device, &data->cqr,
+						     data->rcd_buffer,
+						     lpm);
+		if (!rc) {
+			switch (dasd_eckd_path_access(data->rcd_buffer,
+						      DASD_ECKD_RCD_DATA_SIZE)
+				) {
+			case 0x02:
+				npm |= lpm;
+				break;
+			case 0x03:
+				ppm |= lpm;
+				break;
+			}
+			opm |= lpm;
+		} else if (rc == -EOPNOTSUPP) {
+			DBF_EVENT_DEVID(DBF_WARNING, device->cdev, "%s",
+					"path verification: No configuration "
+					"data retrieved");
+			opm |= lpm;
+		} else if (rc == -EAGAIN) {
+			DBF_EVENT_DEVID(DBF_WARNING, device->cdev, "%s",
 					"path verification: device is stopped,"
 					" try again later");
-				epm |= lpm;
-			} else {
-				dev_warn(&device->cdev->dev,
-					 "Reading device feature codes failed "
-					 "(rc=%d) for new path %x\n", rc, lpm);
-				continue;
-			}
-			if (verify_fcx_max_data(device, lpm)) {
+			epm |= lpm;
+		} else {
+			dev_warn(&device->cdev->dev,
+				 "Reading device feature codes failed "
+				 "(rc=%d) for new path %x\n", rc, lpm);
+			continue;
+		}
+		if (verify_fcx_max_data(device, lpm)) {
+			opm &= ~lpm;
+			npm &= ~lpm;
+			ppm &= ~lpm;
+			continue;
+		}
+
+		/*
+		 * save conf_data for comparison after
+		 * rebuild_device_uid may have changed
+		 * the original data
+		 */
+		memcpy(&path_rcd_buf, data->rcd_buffer,
+		       DASD_ECKD_RCD_DATA_SIZE);
+		path_private.conf_data = (void *) &path_rcd_buf;
+		path_private.conf_len = DASD_ECKD_RCD_DATA_SIZE;
+		if (dasd_eckd_identify_conf_parts(&path_private)) {
+			path_private.conf_data = NULL;
+			path_private.conf_len = 0;
+			continue;
+		}
+
+		/*
+		 * compare path UID with device UID only if at least
+		 * one valid path is left
+		 * in other case the device UID may have changed and
+		 * the first working path UID will be used as device UID
+		 */
+		if (device->path_data.opm &&
+		    dasd_eckd_compare_path_uid(device, &path_private)) {
+			/*
+			 * the comparison was not successful
+			 * rebuild the device UID with at least one
+			 * known path in case a z/VM hyperswap command
+			 * has changed the device
+			 *
+			 * after this compare again
+			 *
+			 * if either the rebuild or the recompare fails
+			 * the path can not be used
+			 */
+			if (rebuild_device_uid(device, data) ||
+			    dasd_eckd_compare_path_uid(
+				    device, &path_private)) {
+				uid = &path_private.uid;
+				if (strlen(uid->vduit) > 0)
+					snprintf(print_uid, sizeof(print_uid),
+						 "%s.%s.%04x.%02x.%s",
+						 uid->vendor, uid->serial,
+						 uid->ssid, uid->real_unit_addr,
+						 uid->vduit);
+				else
+					snprintf(print_uid, sizeof(print_uid),
+						 "%s.%s.%04x.%02x",
+						 uid->vendor, uid->serial,
+						 uid->ssid,
+						 uid->real_unit_addr);
+				dev_err(&device->cdev->dev,
+					"The newly added channel path %02X "
+					"will not be used because it leads "
+					"to a different device %s\n",
+					lpm, print_uid);
 				opm &= ~lpm;
 				npm &= ~lpm;
 				ppm &= ~lpm;
+				continue;
 			}
 		}
+
+		/*
+		 * There is a small chance that a path is lost again between
+		 * above path verification and the following modification of
+		 * the device opm mask. We could avoid that race here by using
+		 * yet another path mask, but we rather deal with this unlikely
+		 * situation in dasd_start_IO.
+		 */
+		spin_lock_irqsave(get_ccwdev_lock(device->cdev), flags);
+		if (!device->path_data.opm && opm) {
+			device->path_data.opm = opm;
+			dasd_generic_path_operational(device);
+		} else
+			device->path_data.opm |= opm;
+		device->path_data.npm |= npm;
+		device->path_data.ppm |= ppm;
+		device->path_data.tbvpm |= epm;
+		spin_unlock_irqrestore(get_ccwdev_lock(device->cdev), flags);
 	}
-	/*
-	 * There is a small chance that a path is lost again between
-	 * above path verification and the following modification of
-	 * the device opm mask. We could avoid that race here by using
-	 * yet another path mask, but we rather deal with this unlikely
-	 * situation in dasd_start_IO.
-	 */
-	spin_lock_irqsave(get_ccwdev_lock(device->cdev), flags);
-	if (!device->path_data.opm && opm) {
-		device->path_data.opm = opm;
-		dasd_generic_path_operational(device);
-	} else
-		device->path_data.opm |= opm;
-	device->path_data.npm |= npm;
-	device->path_data.ppm |= ppm;
-	device->path_data.tbvpm |= epm;
-	spin_unlock_irqrestore(get_ccwdev_lock(device->cdev), flags);
 
 	dasd_put_device(device);
 	if (data->isglobal)
@@ -1441,11 +1637,6 @@ dasd_eckd_check_characteristics(struct dasd_device *device)
 			device->default_expires = value;
 	}
 
-	/* Generate device unique id */
-	rc = dasd_eckd_generate_uid(device);
-	if (rc)
-		goto out_err1;
-
 	dasd_eckd_get_uid(device, &temp_uid);
 	if (temp_uid.type == UA_BASE_DEVICE) {
 		block = dasd_alloc_block();
@@ -2206,7 +2397,7 @@ static struct dasd_ccw_req *dasd_eckd_build_cp_cmd_single(
 					   sizeof(struct PFX_eckd_data));
 	} else {
 		if (define_extent(ccw++, cqr->data, first_trk,
-				  last_trk, cmd, startdev) == -EAGAIN) {
+				  last_trk, cmd, basedev) == -EAGAIN) {
 			/* Clock not in sync and XRC is enabled.
 			 * Try again later.
 			 */
diff --git a/drivers/s390/char/tape_class.h b/drivers/s390/char/tape_class.h
index 9e32780c317f..ba2092f741d5 100644
--- a/drivers/s390/char/tape_class.h
+++ b/drivers/s390/char/tape_class.h
@@ -14,7 +14,6 @@
 #include <linux/module.h>
 #include <linux/fs.h>
 #include <linux/major.h>
-#include <linux/kobj_map.h>
 #include <linux/cdev.h>
 
 #include <linux/device.h>
diff --git a/drivers/s390/cio/qdio_setup.c b/drivers/s390/cio/qdio_setup.c
index 2acc01f90a6a..452989a7ec13 100644
--- a/drivers/s390/cio/qdio_setup.c
+++ b/drivers/s390/cio/qdio_setup.c
@@ -22,12 +22,9 @@
 static struct kmem_cache *qdio_q_cache;
 static struct kmem_cache *qdio_aob_cache;
 
-struct qaob *qdio_allocate_aob()
+struct qaob *qdio_allocate_aob(void)
 {
-	struct qaob *aob;
-
-	aob = kmem_cache_zalloc(qdio_aob_cache, GFP_ATOMIC);
-	return aob;
+	return kmem_cache_zalloc(qdio_aob_cache, GFP_ATOMIC);
 }
 EXPORT_SYMBOL_GPL(qdio_allocate_aob);
 
@@ -180,7 +177,8 @@ static void setup_queues(struct qdio_irq *irq_ptr,
 		setup_queues_misc(q, irq_ptr, qdio_init->input_handler, i);
 
 		q->is_input_q = 1;
-		q->u.in.queue_start_poll = qdio_init->queue_start_poll[i];
+		q->u.in.queue_start_poll = qdio_init->queue_start_poll_array ?
+				qdio_init->queue_start_poll_array[i] : NULL;
 
 		setup_storage_lists(q, irq_ptr, input_sbal_array, i);
 		input_sbal_array += QDIO_MAX_BUFFERS_PER_Q;
diff --git a/drivers/s390/crypto/zcrypt_pcixcc.c b/drivers/s390/crypto/zcrypt_pcixcc.c
index dd4737808e06..077b7d109fde 100644
--- a/drivers/s390/crypto/zcrypt_pcixcc.c
+++ b/drivers/s390/crypto/zcrypt_pcixcc.c
@@ -56,11 +56,6 @@
 #define PCIXCC_MAX_ICA_RESPONSE_SIZE 0x77c /* max size type86 v2 reply	    */
 
 #define PCIXCC_MAX_XCRB_MESSAGE_SIZE (12*1024)
-#define PCIXCC_MAX_XCRB_RESPONSE_SIZE PCIXCC_MAX_XCRB_MESSAGE_SIZE
-#define PCIXCC_MAX_XCRB_DATA_SIZE (11*1024)
-#define PCIXCC_MAX_XCRB_REPLY_SIZE (5*1024)
-
-#define PCIXCC_MAX_RESPONSE_SIZE PCIXCC_MAX_XCRB_RESPONSE_SIZE
 
 #define PCIXCC_CLEANUP_TIME	(15*HZ)
 
@@ -265,7 +260,7 @@ static int ICACRT_msg_to_type6CRT_msgX(struct zcrypt_device *zdev,
  * @ap_msg: pointer to AP message
  * @xcRB: pointer to user input data
  *
- * Returns 0 on success or -EFAULT.
+ * Returns 0 on success or -EFAULT, -EINVAL.
  */
 struct type86_fmt2_msg {
 	struct type86_hdr hdr;
@@ -295,19 +290,12 @@ static int XCRB_msg_to_type6CPRB_msgX(struct zcrypt_device *zdev,
 		CEIL4(xcRB->request_control_blk_length) +
 		xcRB->request_data_length;
 	if (ap_msg->length > PCIXCC_MAX_XCRB_MESSAGE_SIZE)
-		return -EFAULT;
-	if (CEIL4(xcRB->reply_control_blk_length) > PCIXCC_MAX_XCRB_REPLY_SIZE)
-		return -EFAULT;
-	if (CEIL4(xcRB->reply_data_length) > PCIXCC_MAX_XCRB_DATA_SIZE)
-		return -EFAULT;
-	replylen = CEIL4(xcRB->reply_control_blk_length) +
-		CEIL4(xcRB->reply_data_length) +
-		sizeof(struct type86_fmt2_msg);
-	if (replylen > PCIXCC_MAX_XCRB_RESPONSE_SIZE) {
-		xcRB->reply_control_blk_length = PCIXCC_MAX_XCRB_RESPONSE_SIZE -
-			(sizeof(struct type86_fmt2_msg) +
-			    CEIL4(xcRB->reply_data_length));
-	}
+		return -EINVAL;
+	replylen = sizeof(struct type86_fmt2_msg) +
+		CEIL4(xcRB->reply_control_blk_length) +
+		xcRB->reply_data_length;
+	if (replylen > PCIXCC_MAX_XCRB_MESSAGE_SIZE)
+		return -EINVAL;
 
 	/* prepare type6 header */
 	msg->hdr = static_type6_hdrX;
@@ -326,7 +314,7 @@ static int XCRB_msg_to_type6CPRB_msgX(struct zcrypt_device *zdev,
 		return -EFAULT;
 	if (msg->cprbx.cprb_len + sizeof(msg->hdr.function_code) >
 	    xcRB->request_control_blk_length)
-		return -EFAULT;
+		return -EINVAL;
 	function_code = ((unsigned char *)&msg->cprbx) + msg->cprbx.cprb_len;
 	memcpy(msg->hdr.function_code, function_code, sizeof(msg->hdr.function_code));
 
@@ -678,7 +666,7 @@ static void zcrypt_pcixcc_receive(struct ap_device *ap_dev,
 			break;
 		case PCIXCC_RESPONSE_TYPE_XCRB:
 			length = t86r->fmt2.offset2 + t86r->fmt2.count2;
-			length = min(PCIXCC_MAX_XCRB_RESPONSE_SIZE, length);
+			length = min(PCIXCC_MAX_XCRB_MESSAGE_SIZE, length);
 			memcpy(msg->message, reply->message, length);
 			break;
 		default:
@@ -1043,7 +1031,7 @@ static int zcrypt_pcixcc_probe(struct ap_device *ap_dev)
 	struct zcrypt_device *zdev;
 	int rc = 0;
 
-	zdev = zcrypt_device_alloc(PCIXCC_MAX_RESPONSE_SIZE);
+	zdev = zcrypt_device_alloc(PCIXCC_MAX_XCRB_MESSAGE_SIZE);
 	if (!zdev)
 		return -ENOMEM;
 	zdev->ap_dev = ap_dev;
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 4fae1dc19951..9c3f38da4c01 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -4552,7 +4552,7 @@ static int qeth_qdio_establish(struct qeth_card *card)
 	init_data.no_output_qs           = card->qdio.no_out_queues;
 	init_data.input_handler          = card->discipline.input_handler;
 	init_data.output_handler         = card->discipline.output_handler;
-	init_data.queue_start_poll       = queue_start_poll;
+	init_data.queue_start_poll_array = queue_start_poll;
 	init_data.int_parm               = (unsigned long) card;
 	init_data.input_sbal_addr_array  = (void **) in_sbal_ptrs;
 	init_data.output_sbal_addr_array = (void **) out_sbal_ptrs;
diff --git a/drivers/scsi/be2iscsi/be_main.c b/drivers/scsi/be2iscsi/be_main.c
index 797a43994b55..375756fa95cf 100644
--- a/drivers/scsi/be2iscsi/be_main.c
+++ b/drivers/scsi/be2iscsi/be_main.c
@@ -1105,7 +1105,6 @@ be_complete_io(struct beiscsi_conn *beiscsi_conn,
 	struct be_status_bhs *sts_bhs =
 				(struct be_status_bhs *)io_task->cmd_bhs;
 	struct iscsi_conn *conn = beiscsi_conn->conn;
-	unsigned int sense_len;
 	unsigned char *sense;
 	u32 resid = 0, exp_cmdsn, max_cmdsn;
 	u8 rsp, status, flags;
@@ -1153,9 +1152,11 @@ be_complete_io(struct beiscsi_conn *beiscsi_conn,
 	}
 
 	if (status == SAM_STAT_CHECK_CONDITION) {
+		u16 sense_len;
 		unsigned short *slen = (unsigned short *)sts_bhs->sense_info;
+
 		sense = sts_bhs->sense_info + sizeof(unsigned short);
-		sense_len =  cpu_to_be16(*slen);
+		sense_len = be16_to_cpu(*slen);
 		memcpy(task->sc->sense_buffer, sense,
 		       min_t(u16, sense_len, SCSI_SENSE_BUFFERSIZE));
 	}
diff --git a/drivers/scsi/bfa/bfa_defs.h b/drivers/scsi/bfa/bfa_defs.h
index 7b3d235d20b4..b5a1595cc0a5 100644
--- a/drivers/scsi/bfa/bfa_defs.h
+++ b/drivers/scsi/bfa/bfa_defs.h
@@ -902,7 +902,7 @@ struct sfp_mem_s {
 union sfp_xcvr_e10g_code_u {
 	u8		b;
 	struct {
-#ifdef __BIGENDIAN
+#ifdef __BIG_ENDIAN
 		u8	e10g_unall:1;   /* 10G Ethernet compliance */
 		u8	e10g_lrm:1;
 		u8	e10g_lr:1;
@@ -982,7 +982,7 @@ union sfp_xcvr_fc2_code_u {
 union sfp_xcvr_fc3_code_u {
 	u8		b;
 	struct {
-#ifdef __BIGENDIAN
+#ifdef __BIG_ENDIAN
 		u8	rsv4:1;
 		u8	mb800:1;    /* 800 Mbytes/sec */
 		u8	mb1600:1;   /* 1600 Mbytes/sec */
diff --git a/drivers/scsi/bfa/bfa_defs_svc.h b/drivers/scsi/bfa/bfa_defs_svc.h
index 863c6ba7d5eb..78963be2c4fb 100644
--- a/drivers/scsi/bfa/bfa_defs_svc.h
+++ b/drivers/scsi/bfa/bfa_defs_svc.h
@@ -34,22 +34,22 @@
 struct bfa_iocfc_intr_attr_s {
 	u8		coalesce;	/*  enable/disable coalescing */
 	u8		rsvd[3];
-	__be16	latency;	/*  latency in microseconds   */
-	__be16	delay;		/*  delay in microseconds     */
+	__be16		latency;	/*  latency in microseconds   */
+	__be16		delay;		/*  delay in microseconds     */
 };
 
 /*
  * IOC firmware configuraton
  */
 struct bfa_iocfc_fwcfg_s {
-	u16        num_fabrics;	/*  number of fabrics		*/
-	u16        num_lports;	/*  number of local lports	*/
-	u16        num_rports;	/*  number of remote ports	*/
-	u16        num_ioim_reqs;	/*  number of IO reqs		*/
-	u16        num_tskim_reqs;	/*  task management requests	*/
-	u16	   num_fwtio_reqs;	/* number of TM IO reqs in FW */
-	u16        num_fcxp_reqs;	/*  unassisted FC exchanges	*/
-	u16        num_uf_bufs;	/*  unsolicited recv buffers	*/
+	u16		num_fabrics;	/*  number of fabrics		*/
+	u16		num_lports;	/*  number of local lports	*/
+	u16		num_rports;	/*  number of remote ports	*/
+	u16		num_ioim_reqs;	/*  number of IO reqs		*/
+	u16		num_tskim_reqs;	/*  task management requests	*/
+	u16		num_fwtio_reqs;	/* number of TM IO reqs in FW   */
+	u16		num_fcxp_reqs;	/*  unassisted FC exchanges	*/
+	u16		num_uf_bufs;	/*  unsolicited recv buffers	*/
 	u8		num_cqs;
 	u8		fw_tick_res;	/*  FW clock resolution in ms */
 	u8		rsvd[2];
@@ -57,19 +57,19 @@ struct bfa_iocfc_fwcfg_s {
 #pragma pack()
 
 struct bfa_iocfc_drvcfg_s {
-	u16        num_reqq_elems;	/*  number of req queue elements */
-	u16        num_rspq_elems;	/*  number of rsp queue elements */
-	u16        num_sgpgs;	/*  number of total SG pages	  */
-	u16        num_sboot_tgts;	/*  number of SAN boot targets	  */
-	u16        num_sboot_luns;	/*  number of SAN boot luns	  */
-	u16	    ioc_recover;	/*  IOC recovery mode		  */
-	u16	    min_cfg;	/*  minimum configuration	  */
-	u16        path_tov;	/*  device path timeout	  */
-	u16		num_tio_reqs;   /*!< number of TM IO reqs	*/
+	u16		num_reqq_elems;	/*  number of req queue elements */
+	u16		num_rspq_elems;	/*  number of rsp queue elements */
+	u16		num_sgpgs;	/*  number of total SG pages	 */
+	u16		num_sboot_tgts;	/*  number of SAN boot targets	 */
+	u16		num_sboot_luns;	/*  number of SAN boot luns	 */
+	u16		ioc_recover;	/*  IOC recovery mode		 */
+	u16		min_cfg;	/*  minimum configuration	 */
+	u16		path_tov;	/*  device path timeout		*/
+	u16		num_tio_reqs;	/* number of TM IO reqs	*/
 	u8		port_mode;
 	u8		rsvd_a;
-	bfa_boolean_t   delay_comp; /*  delay completion of
-							failed inflight IOs */
+	bfa_boolean_t	delay_comp;	/* delay completion of failed
+					 * inflight IOs */
 	u16		num_ttsk_reqs;	 /* TM task management requests */
 	u32		rsvd;
 };
@@ -101,8 +101,8 @@ struct bfa_fw_ioim_stats_s {
 	u32	fw_frm_drop;		/*  f/w drop the frame */
 
 	u32	rec_timeout;		/*  FW rec timed out */
-	u32	error_rec;			/*  FW sending rec on
-							* an error condition*/
+	u32	error_rec;		/*  FW sending rec on
+					 *  an error condition*/
 	u32	wait_for_si;		/*  FW wait for SI */
 	u32	rec_rsp_inval;		/*  REC rsp invalid */
 	u32	seqr_io_abort;		/*  target does not know cmd so abort */
@@ -124,9 +124,9 @@ struct bfa_fw_ioim_stats_s {
 	u32	unexp_fcp_rsp;		/*  fcp response in wrong state */
 
 	u32	fcp_rsp_under_run;	/*  fcp rsp IO underrun */
-	u32        fcp_rsp_under_run_wr;   /*  fcp rsp IO underrun for write */
+	u32     fcp_rsp_under_run_wr;   /*  fcp rsp IO underrun for write */
 	u32	fcp_rsp_under_run_err;	/*  fcp rsp IO underrun error */
-	u32        fcp_rsp_resid_inval;    /*  invalid residue */
+	u32     fcp_rsp_resid_inval;    /*  invalid residue */
 	u32	fcp_rsp_over_run;	/*  fcp rsp IO overrun */
 	u32	fcp_rsp_over_run_err;	/*  fcp rsp IO overrun error */
 	u32	fcp_rsp_proto_err;	/*  protocol error in fcp rsp */
@@ -142,21 +142,20 @@ struct bfa_fw_ioim_stats_s {
 	u32	ioh_hit_class2_event;	/*  IOH hit class2 */
 	u32	ioh_miss_other_event;	/*  IOH miss other */
 	u32	ioh_seq_cnt_err_event;	/*  IOH seq cnt error */
-	u32	ioh_len_err_event;		/*  IOH len error - fcp_dl !=
-							* bytes xfered */
+	u32	ioh_len_err_event;	/*  IOH len error - fcp_dl !=
+					 *  bytes xfered */
 	u32	ioh_seq_len_err_event;	/*  IOH seq len error */
 	u32	ioh_data_oor_event;	/*  Data out of range */
 	u32	ioh_ro_ooo_event;	/*  Relative offset out of range */
 	u32	ioh_cpu_owned_event;	/*  IOH hit -iost owned by f/w */
 	u32	ioh_unexp_frame_event;	/*  unexpected frame received
-						 *   count */
+					 *  count */
 	u32	ioh_err_int;		/*  IOH error int during data-phase
-						 *   for scsi write
-						 */
+					 *  for scsi write */
 };
 
 struct bfa_fw_tio_stats_s {
-	u32	tio_conf_proc;	/* TIO CONF processed */
+	u32	tio_conf_proc;	    /* TIO CONF processed */
 	u32	tio_conf_drop;      /* TIO CONF dropped */
 	u32	tio_cleanup_req;    /* TIO cleanup requested */
 	u32	tio_cleanup_comp;   /* TIO cleanup completed */
@@ -164,34 +163,36 @@ struct bfa_fw_tio_stats_s {
 	u32	tio_abort_rsp_comp; /* TIO abort rsp completed */
 	u32	tio_abts_req;       /* TIO ABTS requested */
 	u32	tio_abts_ack;       /* TIO ABTS ack-ed */
-	u32	tio_abts_ack_nocomp; /* TIO ABTS ack-ed but not completed */
+	u32	tio_abts_ack_nocomp;/* TIO ABTS ack-ed but not completed */
 	u32	tio_abts_tmo;       /* TIO ABTS timeout */
 	u32	tio_snsdata_dma;    /* TIO sense data DMA */
-	u32	tio_rxwchan_wait; /* TIO waiting for RX wait channel */
-	u32	tio_rxwchan_avail; /* TIO RX wait channel available */
+	u32	tio_rxwchan_wait;   /* TIO waiting for RX wait channel */
+	u32	tio_rxwchan_avail;  /* TIO RX wait channel available */
 	u32	tio_hit_bls;        /* TIO IOH BLS event */
 	u32	tio_uf_recv;        /* TIO received UF */
-	u32	tio_rd_invalid_sm; /* TIO read reqst in wrong state machine */
-	u32	tio_wr_invalid_sm;/* TIO write reqst in wrong state machine */
+	u32	tio_rd_invalid_sm;  /* TIO read reqst in wrong state machine */
+	u32	tio_wr_invalid_sm;  /* TIO write reqst in wrong state machine */
 
-	u32	ds_rxwchan_wait; /* DS waiting for RX wait channel */
-	u32	ds_rxwchan_avail; /* DS RX wait channel available */
+	u32	ds_rxwchan_wait;    /* DS waiting for RX wait channel */
+	u32	ds_rxwchan_avail;   /* DS RX wait channel available */
 	u32	ds_unaligned_rd;    /* DS unaligned read */
-	u32	ds_rdcomp_invalid_sm; /* DS read completed in wrong state machine */
-	u32	ds_wrcomp_invalid_sm; /* DS write completed in wrong state machine */
+	u32	ds_rdcomp_invalid_sm; /* DS read completed in wrong state
+				       * machine */
+	u32	ds_wrcomp_invalid_sm; /* DS write completed in wrong state
+				       * machine */
 	u32	ds_flush_req;       /* DS flush requested */
 	u32	ds_flush_comp;      /* DS flush completed */
 	u32	ds_xfrdy_exp;       /* DS XFER_RDY expired */
 	u32	ds_seq_cnt_err;     /* DS seq cnt error */
 	u32	ds_seq_len_err;     /* DS seq len error */
 	u32	ds_data_oor;        /* DS data out of order */
-	u32	ds_hit_bls;     /* DS hit BLS */
+	u32	ds_hit_bls;	    /* DS hit BLS */
 	u32	ds_edtov_timer_exp; /* DS edtov expired */
 	u32	ds_cpu_owned;       /* DS cpu owned */
 	u32	ds_hit_class2;      /* DS hit class2 */
 	u32	ds_length_err;      /* DS length error */
 	u32	ds_ro_ooo_err;      /* DS relative offset out-of-order error */
-	u32	ds_rectov_timer_exp;    /* DS rectov expired */
+	u32	ds_rectov_timer_exp;/* DS rectov expired */
 	u32	ds_unexp_fr_err;    /* DS unexp frame error */
 };
 
@@ -208,119 +209,119 @@ struct bfa_fw_io_stats_s {
  */
 
 struct bfa_fw_port_fpg_stats_s {
-    u32    intr_evt;
-    u32    intr;
-    u32    intr_excess;
-    u32    intr_cause0;
-    u32    intr_other;
-    u32    intr_other_ign;
-    u32    sig_lost;
-    u32    sig_regained;
-    u32    sync_lost;
-    u32    sync_to;
-    u32    sync_regained;
-    u32    div2_overflow;
-    u32    div2_underflow;
-    u32    efifo_overflow;
-    u32    efifo_underflow;
-    u32    idle_rx;
-    u32    lrr_rx;
-    u32    lr_rx;
-    u32    ols_rx;
-    u32    nos_rx;
-    u32    lip_rx;
-    u32    arbf0_rx;
-    u32    arb_rx;
-    u32    mrk_rx;
-    u32    const_mrk_rx;
-    u32    prim_unknown;
+	u32    intr_evt;
+	u32    intr;
+	u32    intr_excess;
+	u32    intr_cause0;
+	u32    intr_other;
+	u32    intr_other_ign;
+	u32    sig_lost;
+	u32    sig_regained;
+	u32    sync_lost;
+	u32    sync_to;
+	u32    sync_regained;
+	u32    div2_overflow;
+	u32    div2_underflow;
+	u32    efifo_overflow;
+	u32    efifo_underflow;
+	u32    idle_rx;
+	u32    lrr_rx;
+	u32    lr_rx;
+	u32    ols_rx;
+	u32    nos_rx;
+	u32    lip_rx;
+	u32    arbf0_rx;
+	u32    arb_rx;
+	u32    mrk_rx;
+	u32    const_mrk_rx;
+	u32    prim_unknown;
 };
 
 
 struct bfa_fw_port_lksm_stats_s {
-    u32    hwsm_success;       /*  hwsm state machine success          */
-    u32    hwsm_fails;         /*  hwsm fails                          */
-    u32    hwsm_wdtov;         /*  hwsm timed out                      */
-    u32    swsm_success;       /*  swsm success                        */
-    u32    swsm_fails;         /*  swsm fails                          */
-    u32    swsm_wdtov;         /*  swsm timed out                      */
-    u32    busybufs;           /*  link init failed due to busybuf     */
-    u32    buf_waits;          /*  bufwait state entries               */
-    u32    link_fails;         /*  link failures                       */
-    u32    psp_errors;         /*  primitive sequence protocol errors  */
-    u32    lr_unexp;           /*  No. of times LR rx-ed unexpectedly  */
-    u32    lrr_unexp;          /*  No. of times LRR rx-ed unexpectedly */
-    u32    lr_tx;              /*  No. of times LR tx started          */
-    u32    lrr_tx;             /*  No. of times LRR tx started         */
-    u32    ols_tx;             /*  No. of times OLS tx started         */
-    u32    nos_tx;             /*  No. of times NOS tx started         */
-    u32    hwsm_lrr_rx;        /*  No. of times LRR rx-ed by HWSM      */
-    u32    hwsm_lr_rx;         /*  No. of times LR rx-ed by HWSM      */
-	u32	bbsc_lr;	/* LKSM LR tx for credit recovery	*/
+	u32    hwsm_success;       /*  hwsm state machine success          */
+	u32    hwsm_fails;         /*  hwsm fails                          */
+	u32    hwsm_wdtov;         /*  hwsm timed out                      */
+	u32    swsm_success;       /*  swsm success                        */
+	u32    swsm_fails;         /*  swsm fails                          */
+	u32    swsm_wdtov;         /*  swsm timed out                      */
+	u32    busybufs;           /*  link init failed due to busybuf     */
+	u32    buf_waits;          /*  bufwait state entries               */
+	u32    link_fails;         /*  link failures                       */
+	u32    psp_errors;         /*  primitive sequence protocol errors  */
+	u32    lr_unexp;           /*  No. of times LR rx-ed unexpectedly  */
+	u32    lrr_unexp;          /*  No. of times LRR rx-ed unexpectedly */
+	u32    lr_tx;              /*  No. of times LR tx started          */
+	u32    lrr_tx;             /*  No. of times LRR tx started         */
+	u32    ols_tx;             /*  No. of times OLS tx started         */
+	u32    nos_tx;             /*  No. of times NOS tx started         */
+	u32    hwsm_lrr_rx;        /*  No. of times LRR rx-ed by HWSM      */
+	u32    hwsm_lr_rx;         /*  No. of times LR rx-ed by HWSM       */
+	u32    bbsc_lr;		   /* LKSM LR tx for credit recovery       */
 };
 
 struct bfa_fw_port_snsm_stats_s {
-    u32    hwsm_success;       /*  Successful hwsm terminations        */
-    u32    hwsm_fails;         /*  hwsm fail count                     */
-    u32    hwsm_wdtov;         /*  hwsm timed out                      */
-    u32    swsm_success;       /*  swsm success                        */
-    u32    swsm_wdtov;         /*  swsm timed out                      */
-    u32    error_resets;       /*  error resets initiated by upsm      */
-    u32    sync_lost;          /*  Sync loss count                     */
-    u32    sig_lost;           /*  Signal loss count                   */
-	u32	asn8g_attempts;	/* SNSM HWSM at 8Gbps attempts */
+	u32    hwsm_success;       /*  Successful hwsm terminations        */
+	u32    hwsm_fails;         /*  hwsm fail count                     */
+	u32    hwsm_wdtov;         /*  hwsm timed out                      */
+	u32    swsm_success;       /*  swsm success                        */
+	u32    swsm_wdtov;         /*  swsm timed out                      */
+	u32    error_resets;       /*  error resets initiated by upsm      */
+	u32    sync_lost;          /*  Sync loss count                     */
+	u32    sig_lost;           /*  Signal loss count                   */
+	u32    asn8g_attempts;	   /* SNSM HWSM at 8Gbps attempts	   */
 };
 
 struct bfa_fw_port_physm_stats_s {
-    u32    module_inserts;     /*  Module insert count                 */
-    u32    module_xtracts;     /*  Module extracts count               */
-    u32    module_invalids;    /*  Invalid module inserted count       */
-    u32    module_read_ign;    /*  Module validation status ignored    */
-    u32    laser_faults;       /*  Laser fault count                   */
-    u32    rsvd;
+	u32    module_inserts;     /*  Module insert count                 */
+	u32    module_xtracts;     /*  Module extracts count               */
+	u32    module_invalids;    /*  Invalid module inserted count       */
+	u32    module_read_ign;    /*  Module validation status ignored    */
+	u32    laser_faults;       /*  Laser fault count                   */
+	u32    rsvd;
 };
 
 struct bfa_fw_fip_stats_s {
-    u32    vlan_req;           /*  vlan discovery requests             */
-    u32    vlan_notify;        /*  vlan notifications                  */
-    u32    vlan_err;           /*  vlan response error                 */
-    u32    vlan_timeouts;      /*  vlan disvoery timeouts              */
-    u32    vlan_invalids;      /*  invalid vlan in discovery advert.   */
-    u32    disc_req;           /*  Discovery solicit requests          */
-    u32    disc_rsp;           /*  Discovery solicit response          */
-    u32    disc_err;           /*  Discovery advt. parse errors        */
-    u32    disc_unsol;         /*  Discovery unsolicited               */
-    u32    disc_timeouts;      /*  Discovery timeouts                  */
-    u32    disc_fcf_unavail;   /*  Discovery FCF Not Avail.            */
-    u32    linksvc_unsupp;     /*  Unsupported link service req        */
-    u32    linksvc_err;        /*  Parse error in link service req     */
-    u32    logo_req;           /*  FIP logos received                  */
-    u32    clrvlink_req;       /*  Clear virtual link req              */
-    u32    op_unsupp;          /*  Unsupported FIP operation           */
-    u32    untagged;           /*  Untagged frames (ignored)           */
-    u32    invalid_version;    /*  Invalid FIP version                 */
+	u32    vlan_req;           /*  vlan discovery requests             */
+	u32    vlan_notify;        /*  vlan notifications                  */
+	u32    vlan_err;           /*  vlan response error                 */
+	u32    vlan_timeouts;      /*  vlan disvoery timeouts              */
+	u32    vlan_invalids;      /*  invalid vlan in discovery advert.   */
+	u32    disc_req;           /*  Discovery solicit requests          */
+	u32    disc_rsp;           /*  Discovery solicit response          */
+	u32    disc_err;           /*  Discovery advt. parse errors        */
+	u32    disc_unsol;         /*  Discovery unsolicited               */
+	u32    disc_timeouts;      /*  Discovery timeouts                  */
+	u32    disc_fcf_unavail;   /*  Discovery FCF Not Avail.            */
+	u32    linksvc_unsupp;     /*  Unsupported link service req        */
+	u32    linksvc_err;        /*  Parse error in link service req     */
+	u32    logo_req;           /*  FIP logos received                  */
+	u32    clrvlink_req;       /*  Clear virtual link req              */
+	u32    op_unsupp;          /*  Unsupported FIP operation           */
+	u32    untagged;           /*  Untagged frames (ignored)           */
+	u32    invalid_version;    /*  Invalid FIP version                 */
 };
 
 struct bfa_fw_lps_stats_s {
-    u32    mac_invalids;       /*  Invalid mac assigned                */
-    u32    rsvd;
+	u32    mac_invalids;       /*  Invalid mac assigned                */
+	u32    rsvd;
 };
 
 struct bfa_fw_fcoe_stats_s {
-    u32    cee_linkups;        /*  CEE link up count                   */
-    u32    cee_linkdns;        /*  CEE link down count                 */
-    u32    fip_linkups;        /*  FIP link up count                   */
-    u32    fip_linkdns;        /*  FIP link up count                   */
-    u32    fip_fails;          /*  FIP fail count                      */
-    u32    mac_invalids;       /*  Invalid mac assigned                */
+	u32    cee_linkups;        /*  CEE link up count                   */
+	u32    cee_linkdns;        /*  CEE link down count                 */
+	u32    fip_linkups;        /*  FIP link up count                   */
+	u32    fip_linkdns;        /*  FIP link up count                   */
+	u32    fip_fails;          /*  FIP fail count                      */
+	u32    mac_invalids;       /*  Invalid mac assigned                */
 };
 
 /*
  * IOC firmware FCoE port stats
  */
 struct bfa_fw_fcoe_port_stats_s {
-	struct bfa_fw_fcoe_stats_s  fcoe_stats;
-	struct bfa_fw_fip_stats_s   fip_stats;
+	struct bfa_fw_fcoe_stats_s		fcoe_stats;
+	struct bfa_fw_fip_stats_s		fip_stats;
 };
 
 /*
@@ -335,8 +336,8 @@ struct bfa_fw_fc_uport_stats_s {
  * IOC firmware FC port stats
  */
 union bfa_fw_fc_port_stats_s {
-	struct bfa_fw_fc_uport_stats_s	fc_stats;
-	struct bfa_fw_fcoe_port_stats_s	fcoe_stats;
+	struct bfa_fw_fc_uport_stats_s		fc_stats;
+	struct bfa_fw_fcoe_port_stats_s		fcoe_stats;
 };
 
 /*
@@ -366,25 +367,25 @@ struct bfa_fw_lpsm_stats_s {
  */
 struct bfa_fw_trunk_stats_s {
 	u32 emt_recvd;		/*  Trunk EMT received		*/
-	u32 emt_accepted;		/*  Trunk EMT Accepted		*/
-	u32 emt_rejected;		/*  Trunk EMT rejected		*/
+	u32 emt_accepted;	/*  Trunk EMT Accepted		*/
+	u32 emt_rejected;	/*  Trunk EMT rejected		*/
 	u32 etp_recvd;		/*  Trunk ETP received		*/
-	u32 etp_accepted;		/*  Trunk ETP Accepted		*/
-	u32 etp_rejected;		/*  Trunk ETP rejected		*/
+	u32 etp_accepted;	/*  Trunk ETP Accepted		*/
+	u32 etp_rejected;	/*  Trunk ETP rejected		*/
 	u32 lr_recvd;		/*  Trunk LR received		*/
-	u32 rsvd;			/*  padding for 64 bit alignment */
+	u32 rsvd;		/*  padding for 64 bit alignment */
 };
 
 struct bfa_fw_advsm_stats_s {
 	u32 flogi_sent;		/*  Flogi sent			*/
 	u32 flogi_acc_recvd;	/*  Flogi Acc received		*/
 	u32 flogi_rjt_recvd;	/*  Flogi rejects received	*/
-	u32 flogi_retries;		/*  Flogi retries		*/
+	u32 flogi_retries;	/*  Flogi retries		*/
 
 	u32 elp_recvd;		/*  ELP received		*/
-	u32 elp_accepted;		/*  ELP Accepted		*/
-	u32 elp_rejected;		/*  ELP rejected		*/
-	u32 elp_dropped;		/*  ELP dropped		*/
+	u32 elp_accepted;	/*  ELP Accepted		*/
+	u32 elp_rejected;	/*  ELP rejected		*/
+	u32 elp_dropped;	/*  ELP dropped			*/
 };
 
 /*
@@ -521,7 +522,7 @@ struct bfa_qos_vc_attr_s {
 	u16  total_vc_count;                    /*  Total VC Count */
 	u16  shared_credit;
 	u32  elp_opmode_flags;
-	struct bfa_qos_vc_info_s vc_info[BFA_QOS_MAX_VC];  /*   as many as
+	struct bfa_qos_vc_info_s vc_info[BFA_QOS_MAX_VC];  /* as many as
 							    * total_vc_count */
 };
 
@@ -531,16 +532,16 @@ struct bfa_qos_vc_attr_s {
 struct bfa_qos_stats_s {
 	u32	flogi_sent;		/*  QoS Flogi sent */
 	u32	flogi_acc_recvd;	/*  QoS Flogi Acc received */
-	u32	flogi_rjt_recvd; /*  QoS Flogi rejects received */
+	u32	flogi_rjt_recvd;	/*  QoS Flogi rejects received */
 	u32	flogi_retries;		/*  QoS Flogi retries */
 
 	u32	elp_recvd;		/*  QoS ELP received */
 	u32	elp_accepted;		/*  QoS ELP Accepted */
-	u32	elp_rejected;       /*  QoS ELP rejected */
-	u32	elp_dropped;        /*  QoS ELP dropped  */
+	u32	elp_rejected;		/*  QoS ELP rejected */
+	u32	elp_dropped;		/*  QoS ELP dropped  */
 
-	u32	qos_rscn_recvd;     /*  QoS RSCN received */
-	u32	rsvd;		    /* padding for 64 bit alignment */
+	u32	qos_rscn_recvd;		/*  QoS RSCN received */
+	u32	rsvd;			/* padding for 64 bit alignment */
 };
 
 /*
@@ -548,9 +549,9 @@ struct bfa_qos_stats_s {
  */
 struct bfa_fcoe_stats_s {
 	u64	secs_reset;	/*  Seconds since stats reset	     */
-	u64	cee_linkups;	/*  CEE link up		     */
+	u64	cee_linkups;	/*  CEE link up			     */
 	u64	cee_linkdns;	/*  CEE link down		     */
-	u64	fip_linkups;	/*  FIP link up		     */
+	u64	fip_linkups;	/*  FIP link up			     */
 	u64	fip_linkdns;	/*  FIP link down		     */
 	u64	fip_fails;	/*  FIP failures		     */
 	u64	mac_invalids;	/*  Invalid mac assignments	     */
@@ -560,38 +561,38 @@ struct bfa_fcoe_stats_s {
 	u64	vlan_timeouts;	/*  Vlan request timeouts	     */
 	u64	vlan_invalids;	/*  Vlan invalids		     */
 	u64	disc_req;	/*  Discovery requests		     */
-	u64	disc_rsp;	/*  Discovery responses	     */
+	u64	disc_rsp;	/*  Discovery responses		     */
 	u64	disc_err;	/*  Discovery error frames	     */
 	u64	disc_unsol;	/*  Discovery unsolicited	     */
 	u64	disc_timeouts;	/*  Discovery timeouts		     */
 	u64	disc_fcf_unavail; /*  Discovery FCF not avail	     */
-	u64	linksvc_unsupp;	/*  FIP link service req unsupp.    */
-	u64	linksvc_err;	/*  FIP link service req errors     */
+	u64	linksvc_unsupp;	/*  FIP link service req unsupp	     */
+	u64	linksvc_err;	/*  FIP link service req errors	     */
 	u64	logo_req;	/*  FIP logos received		     */
-	u64	clrvlink_req;	/*  Clear virtual link requests     */
+	u64	clrvlink_req;	/*  Clear virtual link requests	     */
 	u64	op_unsupp;	/*  FIP operation unsupp.	     */
-	u64	untagged;	/*  FIP untagged frames	     */
+	u64	untagged;	/*  FIP untagged frames		     */
 	u64	txf_ucast;	/*  Tx FCoE unicast frames	     */
-	u64	txf_ucast_vlan;	/*  Tx FCoE unicast vlan frames     */
+	u64	txf_ucast_vlan;	/*  Tx FCoE unicast vlan frames      */
 	u64	txf_ucast_octets; /*  Tx FCoE unicast octets	     */
 	u64	txf_mcast;	/*  Tx FCoE multicast frames	     */
-	u64	txf_mcast_vlan;	/*  Tx FCoE multicast vlan frames   */
+	u64	txf_mcast_vlan;	/*  Tx FCoE multicast vlan frames    */
 	u64	txf_mcast_octets; /*  Tx FCoE multicast octets	     */
 	u64	txf_bcast;	/*  Tx FCoE broadcast frames	     */
-	u64	txf_bcast_vlan;	/*  Tx FCoE broadcast vlan frames   */
+	u64	txf_bcast_vlan;	/*  Tx FCoE broadcast vlan frames    */
 	u64	txf_bcast_octets; /*  Tx FCoE broadcast octets	     */
-	u64	txf_timeout;	/*  Tx timeouts		     */
+	u64	txf_timeout;	  /*  Tx timeouts		     */
 	u64	txf_parity_errors; /*  Transmit parity err	     */
-	u64	txf_fid_parity_errors; /*  Transmit FID parity err  */
+	u64	txf_fid_parity_errors; /*  Transmit FID parity err   */
 	u64	rxf_ucast_octets; /*  Rx FCoE unicast octets	     */
 	u64	rxf_ucast;	/*  Rx FCoE unicast frames	     */
-	u64	rxf_ucast_vlan;	/*  Rx FCoE unicast vlan frames     */
+	u64	rxf_ucast_vlan;	/*  Rx FCoE unicast vlan frames	     */
 	u64	rxf_mcast_octets; /*  Rx FCoE multicast octets	     */
 	u64	rxf_mcast;	/*  Rx FCoE multicast frames	     */
-	u64	rxf_mcast_vlan;	/*  Rx FCoE multicast vlan frames   */
+	u64	rxf_mcast_vlan;	/*  Rx FCoE multicast vlan frames    */
 	u64	rxf_bcast_octets; /*  Rx FCoE broadcast octets	     */
 	u64	rxf_bcast;	/*  Rx FCoE broadcast frames	     */
-	u64	rxf_bcast_vlan;	/*  Rx FCoE broadcast vlan frames   */
+	u64	rxf_bcast_vlan;	/*  Rx FCoE broadcast vlan frames    */
 };
 
 /*
@@ -852,12 +853,12 @@ struct bfa_port_cfg_s {
 	u8	 tx_bbcredit;	/*  transmit buffer credits	*/
 	u8	 ratelimit;	/*  ratelimit enabled or not	*/
 	u8	 trl_def_speed;	/*  ratelimit default speed	*/
-	u8	bb_scn;		/*  BB_SCN value from FLOGI Exchg */
-	u8	bb_scn_state;	/*  Config state of BB_SCN */
-	u8	faa_state;	/*  FAA enabled/disabled        */
-	u8	rsvd[1];
-	u16 path_tov;	/*  device path timeout	*/
-	u16 q_depth;	/*  SCSI Queue depth		*/
+	u8	 bb_scn;	/*  BB_SCN value from FLOGI Exchg */
+	u8	 bb_scn_state;	/*  Config state of BB_SCN */
+	u8	 faa_state;	/*  FAA enabled/disabled        */
+	u8	 rsvd[1];
+	u16	 path_tov;	/*  device path timeout	*/
+	u16	 q_depth;	/*  SCSI Queue depth		*/
 };
 #pragma pack()
 
@@ -868,20 +869,21 @@ struct bfa_port_attr_s {
 	/*
 	 * Static fields
 	 */
-	wwn_t	   nwwn;		/*  node wwn */
-	wwn_t	   pwwn;		/*  port wwn */
-	wwn_t	   factorynwwn;	/*  factory node wwn */
-	wwn_t	   factorypwwn;	/*  factory port wwn */
-	enum fc_cos	cos_supported;	/*  supported class of services */
-	u32	rsvd;
+	wwn_t			nwwn;		/*  node wwn */
+	wwn_t			pwwn;		/*  port wwn */
+	wwn_t			factorynwwn;	/*  factory node wwn */
+	wwn_t			factorypwwn;	/*  factory port wwn */
+	enum fc_cos		cos_supported;	/*  supported class of
+						 *  services */
+	u32			rsvd;
 	struct fc_symname_s	port_symname;	/*  port symbolic name */
-	enum bfa_port_speed speed_supported; /*  supported speeds */
-	bfa_boolean_t   pbind_enabled;
+	enum bfa_port_speed	speed_supported; /* supported speeds */
+	bfa_boolean_t		pbind_enabled;
 
 	/*
 	 * Configured values
 	 */
-	struct bfa_port_cfg_s pport_cfg;	/*  pport cfg */
+	struct bfa_port_cfg_s	pport_cfg;	/*  pport cfg */
 
 	/*
 	 * Dynamic field - info from BFA
@@ -890,19 +892,20 @@ struct bfa_port_attr_s {
 	enum bfa_port_speed	speed;		/*  current speed */
 	enum bfa_port_topology	topology;	/*  current topology */
 	bfa_boolean_t		beacon;		/*  current beacon status */
-	bfa_boolean_t		link_e2e_beacon; /*  link beacon is on */
-	bfa_boolean_t	bbsc_op_status;	/* fc credit recovery oper state */
+	bfa_boolean_t		link_e2e_beacon; /* link beacon is on */
+	bfa_boolean_t		bbsc_op_status;	/* fc credit recovery oper
+						 * state */
 
 	/*
 	 * Dynamic field - info from FCS
 	 */
-	u32		pid;		/*  port ID */
+	u32			pid;		/*  port ID */
 	enum bfa_port_type	port_type;	/*  current topology */
-	u32		loopback;	/*  external loopback */
-	u32		authfail;	/*  auth fail state */
+	u32			loopback;	/*  external loopback */
+	u32			authfail;	/*  auth fail state */
 
 	/* FCoE specific  */
-	u16		fcoe_vlan;
+	u16			fcoe_vlan;
 	u8			rsvd1[2];
 };
 
@@ -910,48 +913,48 @@ struct bfa_port_attr_s {
  *	      Port FCP mappings.
  */
 struct bfa_port_fcpmap_s {
-	char		osdevname[256];
+	char	osdevname[256];
 	u32	bus;
 	u32	target;
 	u32	oslun;
 	u32	fcid;
-	wwn_t	   nwwn;
-	wwn_t	   pwwn;
+	wwn_t	nwwn;
+	wwn_t	pwwn;
 	u64	fcplun;
-	char		luid[256];
+	char	luid[256];
 };
 
 /*
  *	      Port RNID info.
  */
 struct bfa_port_rnid_s {
-	wwn_t	     wwn;
+	wwn_t	  wwn;
 	u32	  unittype;
 	u32	  portid;
 	u32	  attached_nodes_num;
 	u16	  ip_version;
 	u16	  udp_port;
-	u8	   ipaddr[16];
+	u8	  ipaddr[16];
 	u16	  rsvd;
 	u16	  topologydiscoveryflags;
 };
 
 #pragma pack(1)
 struct bfa_fcport_fcf_s {
-	wwn_t	   name;	   /*  FCF name		 */
-	wwn_t	   fabric_name;    /*  Fabric Name	      */
-	u8		fipenabled;	/*  FIP enabled or not */
-	u8		fipfailed;	/*  FIP failed or not	*/
-	u8		resv[2];
-	u8	 pri;	    /*  FCF priority	     */
-	u8	 version;	/*  FIP version used	 */
-	u8	 available;      /*  Available  for  login    */
-	u8	 fka_disabled;   /*  FKA is disabled	  */
-	u8	 maxsz_verified; /*  FCoE max size verified   */
-	u8	 fc_map[3];      /*  FC map		   */
-	__be16	 vlan;	   /*  FCoE vlan tag/priority   */
-	u32	fka_adv_per;    /*  FIP  ka advert. period   */
-	mac_t	   mac;	    /*  FCF mac		  */
+	wwn_t	name;		/*  FCF name		   */
+	wwn_t	fabric_name;    /*  Fabric Name		   */
+	u8	fipenabled;	/*  FIP enabled or not	   */
+	u8	fipfailed;	/*  FIP failed or not	   */
+	u8	resv[2];
+	u8	pri;		/*  FCF priority	   */
+	u8	version;	/*  FIP version used	   */
+	u8	available;      /*  Available for login    */
+	u8	fka_disabled;   /*  FKA is disabled	   */
+	u8	maxsz_verified; /*  FCoE max size verified */
+	u8	fc_map[3];      /*  FC map		   */
+	__be16	vlan;		/*  FCoE vlan tag/priority */
+	u32	fka_adv_per;    /*  FIP  ka advert. period */
+	mac_t	mac;		/*  FCF mac		   */
 };
 
 /*
@@ -981,7 +984,7 @@ struct bfa_port_link_s {
 	u8	 linkstate_rsn;	/*  bfa_port_linkstate_rsn_t */
 	u8	 topology;	/*  P2P/LOOP bfa_port_topology */
 	u8	 speed;		/*  Link speed (1/2/4/8 G) */
-	u32	linkstate_opt;  /*  Linkstate optional data (debug) */
+	u32	 linkstate_opt; /*  Linkstate optional data (debug) */
 	u8	 trunked;	/*  Trunked or not (1 or 0) */
 	u8	 resvd[3];
 	struct bfa_qos_attr_s  qos_attr;   /* QoS Attributes */
@@ -1035,7 +1038,7 @@ struct bfa_rport_hal_stats_s {
 	u32        sm_fwc_del;	    /*  fw create: delete events   */
 	u32        sm_fwc_off;	    /*  fw create: offline events  */
 	u32        sm_fwc_hwf;	    /*  fw create: IOC down        */
-	u32        sm_fwc_unexp;	    /*  fw create: exception events*/
+	u32        sm_fwc_unexp;    /*  fw create: exception events*/
 	u32        sm_on_off;	    /*  online: offline events     */
 	u32        sm_on_del;	    /*  online: delete events      */
 	u32        sm_on_hwf;	    /*  online: IOC down events    */
@@ -1043,25 +1046,25 @@ struct bfa_rport_hal_stats_s {
 	u32        sm_fwd_rsp;	    /*  fw delete: fw responses    */
 	u32        sm_fwd_del;	    /*  fw delete: delete events   */
 	u32        sm_fwd_hwf;	    /*  fw delete: IOC down events */
-	u32        sm_fwd_unexp;	    /*  fw delete: exception events*/
+	u32        sm_fwd_unexp;    /*  fw delete: exception events*/
 	u32        sm_off_del;	    /*  offline: delete events     */
 	u32        sm_off_on;	    /*  offline: online events     */
 	u32        sm_off_hwf;	    /*  offline: IOC down events   */
-	u32        sm_off_unexp;	    /*  offline: exception events  */
-	u32        sm_del_fwrsp;	    /*  delete: fw responses       */
+	u32        sm_off_unexp;    /*  offline: exception events  */
+	u32        sm_del_fwrsp;    /*  delete: fw responses       */
 	u32        sm_del_hwf;	    /*  delete: IOC down events    */
-	u32        sm_del_unexp;	    /*  delete: exception events   */
-	u32        sm_delp_fwrsp;	    /*  delete pend: fw responses  */
+	u32        sm_del_unexp;    /*  delete: exception events   */
+	u32        sm_delp_fwrsp;   /*  delete pend: fw responses  */
 	u32        sm_delp_hwf;	    /*  delete pend: IOC downs     */
-	u32        sm_delp_unexp;	    /*  delete pend: exceptions    */
-	u32        sm_offp_fwrsp;	    /*  off-pending: fw responses  */
+	u32        sm_delp_unexp;   /*  delete pend: exceptions    */
+	u32        sm_offp_fwrsp;   /*  off-pending: fw responses  */
 	u32        sm_offp_del;	    /*  off-pending: deletes       */
 	u32        sm_offp_hwf;	    /*  off-pending: IOC downs     */
-	u32        sm_offp_unexp;	    /*  off-pending: exceptions    */
+	u32        sm_offp_unexp;   /*  off-pending: exceptions    */
 	u32        sm_iocd_off;	    /*  IOC down: offline events   */
 	u32        sm_iocd_del;	    /*  IOC down: delete events    */
 	u32        sm_iocd_on;	    /*  IOC down: online events    */
-	u32        sm_iocd_unexp;	    /*  IOC down: exceptions       */
+	u32        sm_iocd_unexp;   /*  IOC down: exceptions       */
 	u32        rsvd;
 };
 #pragma pack(1)
@@ -1069,9 +1072,9 @@ struct bfa_rport_hal_stats_s {
  *  Rport's QoS attributes
  */
 struct bfa_rport_qos_attr_s {
-	u8			qos_priority;  /*  rport's QoS priority   */
-	u8			rsvd[3];
-	u32	       qos_flow_id;	  /*  QoS flow Id	 */
+	u8		qos_priority;	/*  rport's QoS priority   */
+	u8		rsvd[3];
+	u32		qos_flow_id;	/*  QoS flow Id	 */
 };
 #pragma pack()
 
diff --git a/drivers/scsi/bfa/bfa_ioc.c b/drivers/scsi/bfa/bfa_ioc.c
index 1ac5aecf25a6..eca7ab78085b 100644
--- a/drivers/scsi/bfa/bfa_ioc.c
+++ b/drivers/scsi/bfa/bfa_ioc.c
@@ -3727,11 +3727,11 @@ bfa_sfp_media_get(struct bfa_sfp_s *sfp)
 			 (xmtr_tech & SFP_XMTR_TECH_SA))
 			*media = BFA_SFP_MEDIA_SW;
 		/* Check 10G Ethernet Compilance code */
-		else if (e10g.b & 0x10)
+		else if (e10g.r.e10g_sr)
 			*media = BFA_SFP_MEDIA_SW;
-		else if (e10g.b & 0x60)
+		else if (e10g.r.e10g_lrm && e10g.r.e10g_lr)
 			*media = BFA_SFP_MEDIA_LW;
-		else if (e10g.r.e10g_unall & 0x80)
+		else if (e10g.r.e10g_unall)
 			*media = BFA_SFP_MEDIA_UNKNOWN;
 		else
 			bfa_trc(sfp, 0);
diff --git a/drivers/scsi/bfa/bfad_debugfs.c b/drivers/scsi/bfa/bfad_debugfs.c
index caca9b7c8309..439c012be763 100644
--- a/drivers/scsi/bfa/bfad_debugfs.c
+++ b/drivers/scsi/bfa/bfad_debugfs.c
@@ -557,8 +557,7 @@ bfad_debugfs_exit(struct bfad_port_s *port)
 		}
 	}
 
-	/*
-	 * Remove the pci_dev debugfs directory for the port */
+	/* Remove the pci_dev debugfs directory for the port */
 	if (port->port_debugfs_root) {
 		debugfs_remove(port->port_debugfs_root);
 		port->port_debugfs_root = NULL;
diff --git a/drivers/scsi/device_handler/scsi_dh.c b/drivers/scsi/device_handler/scsi_dh.c
index 23149b9e297c..48e46f5b77cc 100644
--- a/drivers/scsi/device_handler/scsi_dh.c
+++ b/drivers/scsi/device_handler/scsi_dh.c
@@ -28,7 +28,6 @@
 
 static DEFINE_SPINLOCK(list_lock);
 static LIST_HEAD(scsi_dh_list);
-static int scsi_dh_list_idx = 1;
 
 static struct scsi_device_handler *get_device_handler(const char *name)
 {
@@ -45,21 +44,6 @@ static struct scsi_device_handler *get_device_handler(const char *name)
 	return found;
 }
 
-static struct scsi_device_handler *get_device_handler_by_idx(int idx)
-{
-	struct scsi_device_handler *tmp, *found = NULL;
-
-	spin_lock(&list_lock);
-	list_for_each_entry(tmp, &scsi_dh_list, list) {
-		if (tmp->idx == idx) {
-			found = tmp;
-			break;
-		}
-	}
-	spin_unlock(&list_lock);
-	return found;
-}
-
 /*
  * device_handler_match_function - Match a device handler to a device
  * @sdev - SCSI device to be tested
@@ -84,23 +68,6 @@ device_handler_match_function(struct scsi_device *sdev)
 }
 
 /*
- * device_handler_match_devlist - Match a device handler to a device
- * @sdev - SCSI device to be tested
- *
- * Tests @sdev against all device_handler registered in the devlist.
- * Returns the found device handler or NULL if not found.
- */
-static struct scsi_device_handler *
-device_handler_match_devlist(struct scsi_device *sdev)
-{
-	int idx;
-
-	idx = scsi_get_device_flags_keyed(sdev, sdev->vendor, sdev->model,
-					  SCSI_DEVINFO_DH);
-	return get_device_handler_by_idx(idx);
-}
-
-/*
  * device_handler_match - Attach a device handler to a device
  * @scsi_dh - The device handler to match against or NULL
  * @sdev - SCSI device to be tested against @scsi_dh
@@ -116,8 +83,6 @@ device_handler_match(struct scsi_device_handler *scsi_dh,
 	struct scsi_device_handler *found_dh;
 
 	found_dh = device_handler_match_function(sdev);
-	if (!found_dh)
-		found_dh = device_handler_match_devlist(sdev);
 
 	if (scsi_dh && found_dh != scsi_dh)
 		found_dh = NULL;
@@ -361,25 +326,14 @@ static int scsi_dh_notifier_remove(struct device *dev, void *data)
  */
 int scsi_register_device_handler(struct scsi_device_handler *scsi_dh)
 {
-	int i;
 
 	if (get_device_handler(scsi_dh->name))
 		return -EBUSY;
 
 	spin_lock(&list_lock);
-	scsi_dh->idx = scsi_dh_list_idx++;
 	list_add(&scsi_dh->list, &scsi_dh_list);
 	spin_unlock(&list_lock);
 
-	for (i = 0; scsi_dh->devlist && scsi_dh->devlist[i].vendor; i++) {
-		scsi_dev_info_list_add_keyed(0,
-					scsi_dh->devlist[i].vendor,
-					scsi_dh->devlist[i].model,
-					NULL,
-					scsi_dh->idx,
-					SCSI_DEVINFO_DH);
-	}
-
 	bus_for_each_dev(&scsi_bus_type, NULL, scsi_dh, scsi_dh_notifier_add);
 	printk(KERN_INFO "%s: device handler registered\n", scsi_dh->name);
 
@@ -396,7 +350,6 @@ EXPORT_SYMBOL_GPL(scsi_register_device_handler);
  */
 int scsi_unregister_device_handler(struct scsi_device_handler *scsi_dh)
 {
-	int i;
 
 	if (!get_device_handler(scsi_dh->name))
 		return -ENODEV;
@@ -404,12 +357,6 @@ int scsi_unregister_device_handler(struct scsi_device_handler *scsi_dh)
 	bus_for_each_dev(&scsi_bus_type, NULL, scsi_dh,
 			 scsi_dh_notifier_remove);
 
-	for (i = 0; scsi_dh->devlist && scsi_dh->devlist[i].vendor; i++) {
-		scsi_dev_info_list_del_keyed(scsi_dh->devlist[i].vendor,
-					     scsi_dh->devlist[i].model,
-					     SCSI_DEVINFO_DH);
-	}
-
 	spin_lock(&list_lock);
 	list_del(&scsi_dh->list);
 	spin_unlock(&list_lock);
@@ -588,10 +535,6 @@ static int __init scsi_dh_init(void)
 {
 	int r;
 
-	r = scsi_dev_info_add_list(SCSI_DEVINFO_DH, "SCSI Device Handler");
-	if (r)
-		return r;
-
 	r = bus_register_notifier(&scsi_bus_type, &scsi_dh_nb);
 
 	if (!r)
@@ -606,7 +549,6 @@ static void __exit scsi_dh_exit(void)
 	bus_for_each_dev(&scsi_bus_type, NULL, NULL,
 			 scsi_dh_sysfs_attr_remove);
 	bus_unregister_notifier(&scsi_bus_type, &scsi_dh_nb);
-	scsi_dev_info_remove_list(SCSI_DEVINFO_DH);
 }
 
 module_init(scsi_dh_init);
diff --git a/drivers/scsi/device_handler/scsi_dh_emc.c b/drivers/scsi/device_handler/scsi_dh_emc.c
index 591186cf1896..e1c8be06de9d 100644
--- a/drivers/scsi/device_handler/scsi_dh_emc.c
+++ b/drivers/scsi/device_handler/scsi_dh_emc.c
@@ -629,6 +629,24 @@ static const struct scsi_dh_devlist clariion_dev_list[] = {
 	{NULL, NULL},
 };
 
+static bool clariion_match(struct scsi_device *sdev)
+{
+	int i;
+
+	if (scsi_device_tpgs(sdev))
+		return false;
+
+	for (i = 0; clariion_dev_list[i].vendor; i++) {
+		if (!strncmp(sdev->vendor, clariion_dev_list[i].vendor,
+			strlen(clariion_dev_list[i].vendor)) &&
+		    !strncmp(sdev->model, clariion_dev_list[i].model,
+			strlen(clariion_dev_list[i].model))) {
+			return true;
+		}
+	}
+	return false;
+}
+
 static int clariion_bus_attach(struct scsi_device *sdev);
 static void clariion_bus_detach(struct scsi_device *sdev);
 
@@ -642,6 +660,7 @@ static struct scsi_device_handler clariion_dh = {
 	.activate	= clariion_activate,
 	.prep_fn	= clariion_prep_fn,
 	.set_params	= clariion_set_params,
+	.match		= clariion_match,
 };
 
 static int clariion_bus_attach(struct scsi_device *sdev)
diff --git a/drivers/scsi/device_handler/scsi_dh_hp_sw.c b/drivers/scsi/device_handler/scsi_dh_hp_sw.c
index 0f86a18b157d..084062bb8ee9 100644
--- a/drivers/scsi/device_handler/scsi_dh_hp_sw.c
+++ b/drivers/scsi/device_handler/scsi_dh_hp_sw.c
@@ -320,6 +320,24 @@ static const struct scsi_dh_devlist hp_sw_dh_data_list[] = {
 	{NULL, NULL},
 };
 
+static bool hp_sw_match(struct scsi_device *sdev)
+{
+	int i;
+
+	if (scsi_device_tpgs(sdev))
+		return false;
+
+	for (i = 0; hp_sw_dh_data_list[i].vendor; i++) {
+		if (!strncmp(sdev->vendor, hp_sw_dh_data_list[i].vendor,
+			strlen(hp_sw_dh_data_list[i].vendor)) &&
+		    !strncmp(sdev->model, hp_sw_dh_data_list[i].model,
+			strlen(hp_sw_dh_data_list[i].model))) {
+			return true;
+		}
+	}
+	return false;
+}
+
 static int hp_sw_bus_attach(struct scsi_device *sdev);
 static void hp_sw_bus_detach(struct scsi_device *sdev);
 
@@ -331,6 +349,7 @@ static struct scsi_device_handler hp_sw_dh = {
 	.detach		= hp_sw_bus_detach,
 	.activate	= hp_sw_activate,
 	.prep_fn	= hp_sw_prep_fn,
+	.match		= hp_sw_match,
 };
 
 static int hp_sw_bus_attach(struct scsi_device *sdev)
diff --git a/drivers/scsi/device_handler/scsi_dh_rdac.c b/drivers/scsi/device_handler/scsi_dh_rdac.c
index 1d3127920063..841ebf4a6788 100644
--- a/drivers/scsi/device_handler/scsi_dh_rdac.c
+++ b/drivers/scsi/device_handler/scsi_dh_rdac.c
@@ -820,6 +820,24 @@ static const struct scsi_dh_devlist rdac_dev_list[] = {
 	{NULL, NULL},
 };
 
+static bool rdac_match(struct scsi_device *sdev)
+{
+	int i;
+
+	if (scsi_device_tpgs(sdev))
+		return false;
+
+	for (i = 0; rdac_dev_list[i].vendor; i++) {
+		if (!strncmp(sdev->vendor, rdac_dev_list[i].vendor,
+			strlen(rdac_dev_list[i].vendor)) &&
+		    !strncmp(sdev->model, rdac_dev_list[i].model,
+			strlen(rdac_dev_list[i].model))) {
+			return true;
+		}
+	}
+	return false;
+}
+
 static int rdac_bus_attach(struct scsi_device *sdev);
 static void rdac_bus_detach(struct scsi_device *sdev);
 
@@ -832,6 +850,7 @@ static struct scsi_device_handler rdac_dh = {
 	.attach = rdac_bus_attach,
 	.detach = rdac_bus_detach,
 	.activate = rdac_activate,
+	.match = rdac_match,
 };
 
 static int rdac_bus_attach(struct scsi_device *sdev)
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index 865d452542be..5140f5d0fd6b 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -293,12 +293,14 @@ static u32 unresettable_controller[] = {
 	0x3215103C, /* Smart Array E200i */
 	0x3237103C, /* Smart Array E500 */
 	0x323D103C, /* Smart Array P700m */
+	0x40800E11, /* Smart Array 5i */
 	0x409C0E11, /* Smart Array 6400 */
 	0x409D0E11, /* Smart Array 6400 EM */
 };
 
 /* List of controllers which cannot even be soft reset */
 static u32 soft_unresettable_controller[] = {
+	0x40800E11, /* Smart Array 5i */
 	/* Exclude 640x boards.  These are two pci devices in one slot
 	 * which share a battery backed cache module.  One controls the
 	 * cache, the other accesses the cache through the one that controls
@@ -4072,10 +4074,10 @@ static int hpsa_request_irq(struct ctlr_info *h,
 
 	if (h->msix_vector || h->msi_vector)
 		rc = request_irq(h->intr[h->intr_mode], msixhandler,
-				IRQF_DISABLED, h->devname, h);
+				0, h->devname, h);
 	else
 		rc = request_irq(h->intr[h->intr_mode], intxhandler,
-				IRQF_DISABLED, h->devname, h);
+				IRQF_SHARED, h->devname, h);
 	if (rc) {
 		dev_err(&h->pdev->dev, "unable to get irq %d for %s\n",
 		       h->intr[h->intr_mode], h->devname);
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index bb4c8e0584e2..825f9307417a 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -247,18 +247,6 @@ struct lpfc_stats {
 	uint32_t fcpLocalErr;
 };
 
-enum sysfs_mbox_state {
-	SMBOX_IDLE,
-	SMBOX_WRITING,
-	SMBOX_READING
-};
-
-struct lpfc_sysfs_mbox {
-	enum sysfs_mbox_state state;
-	size_t                offset;
-	struct lpfcMboxq *    mbox;
-};
-
 struct lpfc_hba;
 
 
@@ -783,8 +771,6 @@ struct lpfc_hba {
 	uint64_t bg_apptag_err_cnt;
 	uint64_t bg_reftag_err_cnt;
 
-	struct lpfc_sysfs_mbox sysfs_mbox;
-
 	/* fastpath list. */
 	spinlock_t scsi_buf_list_lock;
 	struct list_head lpfc_scsi_buf_list;
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index d0ebaeb7ef60..f6697cb0e216 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -351,10 +351,23 @@ lpfc_fwrev_show(struct device *dev, struct device_attribute *attr,
 	struct Scsi_Host  *shost = class_to_shost(dev);
 	struct lpfc_vport *vport = (struct lpfc_vport *) shost->hostdata;
 	struct lpfc_hba   *phba = vport->phba;
+	uint32_t if_type;
+	uint8_t sli_family;
 	char fwrev[32];
+	int len;
 
 	lpfc_decode_firmware_rev(phba, fwrev, 1);
-	return snprintf(buf, PAGE_SIZE, "%s, sli-%d\n", fwrev, phba->sli_rev);
+	if_type = phba->sli4_hba.pc_sli4_params.if_type;
+	sli_family = phba->sli4_hba.pc_sli4_params.sli_family;
+
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		len = snprintf(buf, PAGE_SIZE, "%s, sli-%d\n",
+			       fwrev, phba->sli_rev);
+	else
+		len = snprintf(buf, PAGE_SIZE, "%s, sli-%d:%d:%x\n",
+			       fwrev, phba->sli_rev, if_type, sli_family);
+
+	return len;
 }
 
 /**
@@ -488,6 +501,34 @@ lpfc_link_state_show(struct device *dev, struct device_attribute *attr,
 }
 
 /**
+ * lpfc_sli4_protocol_show - Return the fip mode of the HBA
+ * @dev: class unused variable.
+ * @attr: device attribute, not used.
+ * @buf: on return contains the module description text.
+ *
+ * Returns: size of formatted string.
+ **/
+static ssize_t
+lpfc_sli4_protocol_show(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	struct Scsi_Host *shost = class_to_shost(dev);
+	struct lpfc_vport *vport = (struct lpfc_vport *) shost->hostdata;
+	struct lpfc_hba *phba = vport->phba;
+
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		return snprintf(buf, PAGE_SIZE, "fc\n");
+
+	if (phba->sli4_hba.lnk_info.lnk_dv == LPFC_LNK_DAT_VAL) {
+		if (phba->sli4_hba.lnk_info.lnk_tp == LPFC_LNK_TYPE_GE)
+			return snprintf(buf, PAGE_SIZE, "fcoe\n");
+		if (phba->sli4_hba.lnk_info.lnk_tp == LPFC_LNK_TYPE_FC)
+			return snprintf(buf, PAGE_SIZE, "fc\n");
+	}
+	return snprintf(buf, PAGE_SIZE, "unknown\n");
+}
+
+/**
  * lpfc_link_state_store - Transition the link_state on an HBA port
  * @dev: class device that is converted into a Scsi_host.
  * @attr: device attribute, not used.
@@ -773,7 +814,12 @@ lpfc_issue_reset(struct device *dev, struct device_attribute *attr,
  * the readyness after performing a firmware reset.
  *
  * Returns:
- * zero for success
+ * zero for success, -EPERM when port does not have privilage to perform the
+ * reset, -EIO when port timeout from recovering from the reset.
+ *
+ * Note:
+ * As the caller will interpret the return code by value, be careful in making
+ * change or addition to return codes.
  **/
 int
 lpfc_sli4_pdev_status_reg_wait(struct lpfc_hba *phba)
@@ -826,9 +872,11 @@ lpfc_sli4_pdev_reg_request(struct lpfc_hba *phba, uint32_t opcode)
 {
 	struct completion online_compl;
 	struct pci_dev *pdev = phba->pcidev;
+	uint32_t before_fc_flag;
+	uint32_t sriov_nr_virtfn;
 	uint32_t reg_val;
-	int status = 0;
-	int rc;
+	int status = 0, rc = 0;
+	int job_posted = 1, sriov_err;
 
 	if (!phba->cfg_enable_hba_reset)
 		return -EACCES;
@@ -838,6 +886,10 @@ lpfc_sli4_pdev_reg_request(struct lpfc_hba *phba, uint32_t opcode)
 	     LPFC_SLI_INTF_IF_TYPE_2))
 		return -EPERM;
 
+	/* Keep state if we need to restore back */
+	before_fc_flag = phba->pport->fc_flag;
+	sriov_nr_virtfn = phba->cfg_sriov_nr_virtfn;
+
 	/* Disable SR-IOV virtual functions if enabled */
 	if (phba->cfg_sriov_nr_virtfn) {
 		pci_disable_sriov(pdev);
@@ -869,21 +921,44 @@ lpfc_sli4_pdev_reg_request(struct lpfc_hba *phba, uint32_t opcode)
 	/* delay driver action following IF_TYPE_2 reset */
 	rc = lpfc_sli4_pdev_status_reg_wait(phba);
 
-	if (rc)
+	if (rc == -EPERM) {
+		/* no privilage for reset, restore if needed */
+		if (before_fc_flag & FC_OFFLINE_MODE)
+			goto out;
+	} else if (rc == -EIO) {
+		/* reset failed, there is nothing more we can do */
 		return rc;
+	}
+
+	/* keep the original port state */
+	if (before_fc_flag & FC_OFFLINE_MODE)
+		goto out;
 
 	init_completion(&online_compl);
-	rc = lpfc_workq_post_event(phba, &status, &online_compl,
-				   LPFC_EVT_ONLINE);
-	if (rc == 0)
-		return -ENOMEM;
+	job_posted = lpfc_workq_post_event(phba, &status, &online_compl,
+					   LPFC_EVT_ONLINE);
+	if (!job_posted)
+		goto out;
 
 	wait_for_completion(&online_compl);
 
-	if (status != 0)
-		return -EIO;
+out:
+	/* in any case, restore the virtual functions enabled as before */
+	if (sriov_nr_virtfn) {
+		sriov_err =
+			lpfc_sli_probe_sriov_nr_virtfn(phba, sriov_nr_virtfn);
+		if (!sriov_err)
+			phba->cfg_sriov_nr_virtfn = sriov_nr_virtfn;
+	}
 
-	return 0;
+	/* return proper error code */
+	if (!rc) {
+		if (!job_posted)
+			rc = -ENOMEM;
+		else if (status)
+			rc = -EIO;
+	}
+	return rc;
 }
 
 /**
@@ -955,33 +1030,38 @@ lpfc_board_mode_store(struct device *dev, struct device_attribute *attr,
 	struct lpfc_vport *vport = (struct lpfc_vport *) shost->hostdata;
 	struct lpfc_hba   *phba = vport->phba;
 	struct completion online_compl;
-	int status=0;
+	char *board_mode_str = NULL;
+	int status = 0;
 	int rc;
 
-	if (!phba->cfg_enable_hba_reset)
-		return -EACCES;
+	if (!phba->cfg_enable_hba_reset) {
+		status = -EACCES;
+		goto board_mode_out;
+	}
 
 	lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
-		"3050 lpfc_board_mode set to %s\n", buf);
+			 "3050 lpfc_board_mode set to %s\n", buf);
 
 	init_completion(&online_compl);
 
 	if(strncmp(buf, "online", sizeof("online") - 1) == 0) {
 		rc = lpfc_workq_post_event(phba, &status, &online_compl,
 				      LPFC_EVT_ONLINE);
-		if (rc == 0)
-			return -ENOMEM;
+		if (rc == 0) {
+			status = -ENOMEM;
+			goto board_mode_out;
+		}
 		wait_for_completion(&online_compl);
 	} else if (strncmp(buf, "offline", sizeof("offline") - 1) == 0)
 		status = lpfc_do_offline(phba, LPFC_EVT_OFFLINE);
 	else if (strncmp(buf, "warm", sizeof("warm") - 1) == 0)
 		if (phba->sli_rev == LPFC_SLI_REV4)
-			return -EINVAL;
+			status = -EINVAL;
 		else
 			status = lpfc_do_offline(phba, LPFC_EVT_WARM_START);
 	else if (strncmp(buf, "error", sizeof("error") - 1) == 0)
 		if (phba->sli_rev == LPFC_SLI_REV4)
-			return -EINVAL;
+			status = -EINVAL;
 		else
 			status = lpfc_do_offline(phba, LPFC_EVT_KILL);
 	else if (strncmp(buf, "dump", sizeof("dump") - 1) == 0)
@@ -991,12 +1071,21 @@ lpfc_board_mode_store(struct device *dev, struct device_attribute *attr,
 	else if (strncmp(buf, "dv_reset", sizeof("dv_reset") - 1) == 0)
 		status = lpfc_sli4_pdev_reg_request(phba, LPFC_DV_RESET);
 	else
-		return -EINVAL;
+		status = -EINVAL;
 
+board_mode_out:
 	if (!status)
 		return strlen(buf);
-	else
+	else {
+		board_mode_str = strchr(buf, '\n');
+		if (board_mode_str)
+			*board_mode_str = '\0';
+		lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
+				 "3097 Failed \"%s\", status(%d), "
+				 "fc_flag(x%x)\n",
+				 buf, status, phba->pport->fc_flag);
 		return status;
+	}
 }
 
 /**
@@ -1942,6 +2031,7 @@ static DEVICE_ATTR(lpfc_fips_rev, S_IRUGO, lpfc_fips_rev_show, NULL);
 static DEVICE_ATTR(lpfc_dss, S_IRUGO, lpfc_dss_show, NULL);
 static DEVICE_ATTR(lpfc_sriov_hw_max_virtfn, S_IRUGO,
 		   lpfc_sriov_hw_max_virtfn_show, NULL);
+static DEVICE_ATTR(protocol, S_IRUGO, lpfc_sli4_protocol_show, NULL);
 
 static char *lpfc_soft_wwn_key = "C99G71SL8032A";
 
@@ -2687,6 +2777,14 @@ lpfc_topology_store(struct device *dev, struct device_attribute *attr,
 	if (val >= 0 && val <= 6) {
 		prev_val = phba->cfg_topology;
 		phba->cfg_topology = val;
+		if (phba->cfg_link_speed == LPFC_USER_LINK_SPEED_16G &&
+			val == 4) {
+			lpfc_printf_vlog(vport, KERN_ERR, LOG_INIT,
+				"3113 Loop mode not supported at speed %d\n",
+				phba->cfg_link_speed);
+			phba->cfg_topology = prev_val;
+			return -EINVAL;
+		}
 		if (nolip)
 			return strlen(buf);
 
@@ -3132,6 +3230,14 @@ lpfc_link_speed_store(struct device *dev, struct device_attribute *attr,
 				val);
 		return -EINVAL;
 	}
+	if (val == LPFC_USER_LINK_SPEED_16G &&
+		 phba->fc_topology == LPFC_TOPOLOGY_LOOP) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"3112 lpfc_link_speed attribute cannot be set "
+				"to %d. Speed is not supported in loop mode.\n",
+				val);
+		return -EINVAL;
+	}
 	if ((val >= 0) && (val <= LPFC_USER_LINK_SPEED_MAX) &&
 	    (LPFC_USER_LINK_SPEED_BITMAP & (1 << val))) {
 		prev_val = phba->cfg_link_speed;
@@ -3176,6 +3282,13 @@ lpfc_param_show(link_speed)
 static int
 lpfc_link_speed_init(struct lpfc_hba *phba, int val)
 {
+	if (val == LPFC_USER_LINK_SPEED_16G && phba->cfg_topology == 4) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+			"3111 lpfc_link_speed of %d cannot "
+			"support loop mode, setting topology to default.\n",
+			 val);
+		phba->cfg_topology = 0;
+	}
 	if ((val >= 0) && (val <= LPFC_USER_LINK_SPEED_MAX) &&
 	    (LPFC_USER_LINK_SPEED_BITMAP & (1 << val))) {
 		phba->cfg_link_speed = val;
@@ -3830,6 +3943,7 @@ struct device_attribute *lpfc_hba_attrs[] = {
 	&dev_attr_lpfc_fips_rev,
 	&dev_attr_lpfc_dss,
 	&dev_attr_lpfc_sriov_hw_max_virtfn,
+	&dev_attr_protocol,
 	NULL,
 };
 
@@ -3988,23 +4102,6 @@ static struct bin_attribute sysfs_ctlreg_attr = {
 };
 
 /**
- * sysfs_mbox_idle - frees the sysfs mailbox
- * @phba: lpfc_hba pointer
- **/
-static void
-sysfs_mbox_idle(struct lpfc_hba *phba)
-{
-	phba->sysfs_mbox.state = SMBOX_IDLE;
-	phba->sysfs_mbox.offset = 0;
-
-	if (phba->sysfs_mbox.mbox) {
-		mempool_free(phba->sysfs_mbox.mbox,
-			     phba->mbox_mem_pool);
-		phba->sysfs_mbox.mbox = NULL;
-	}
-}
-
-/**
  * sysfs_mbox_write - Write method for writing information via mbox
  * @filp: open sysfs file
  * @kobj: kernel kobject that contains the kernel class device.
@@ -4014,71 +4111,18 @@ sysfs_mbox_idle(struct lpfc_hba *phba)
  * @count: bytes to transfer.
  *
  * Description:
- * Accessed via /sys/class/scsi_host/hostxxx/mbox.
- * Uses the sysfs mbox to send buf contents to the adapter.
+ * Deprecated function. All mailbox access from user space is performed via the
+ * bsg interface.
  *
  * Returns:
- * -ERANGE off and count combo out of range
- * -EINVAL off, count or buff address invalid
- * zero if count is zero
- * -EPERM adapter is offline
- * -ENOMEM failed to allocate memory for the mail box
- * -EAGAIN offset, state or mbox is NULL
- * count number of bytes transferred
+ * -EPERM operation not permitted
  **/
 static ssize_t
 sysfs_mbox_write(struct file *filp, struct kobject *kobj,
 		 struct bin_attribute *bin_attr,
 		 char *buf, loff_t off, size_t count)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct Scsi_Host  *shost = class_to_shost(dev);
-	struct lpfc_vport *vport = (struct lpfc_vport *) shost->hostdata;
-	struct lpfc_hba   *phba = vport->phba;
-	struct lpfcMboxq  *mbox = NULL;
-
-	if ((count + off) > MAILBOX_CMD_SIZE)
-		return -ERANGE;
-
-	if (off % 4 ||  count % 4 || (unsigned long)buf % 4)
-		return -EINVAL;
-
-	if (count == 0)
-		return 0;
-
-	if (off == 0) {
-		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-		if (!mbox)
-			return -ENOMEM;
-		memset(mbox, 0, sizeof (LPFC_MBOXQ_t));
-	}
-
-	spin_lock_irq(&phba->hbalock);
-
-	if (off == 0) {
-		if (phba->sysfs_mbox.mbox)
-			mempool_free(mbox, phba->mbox_mem_pool);
-		else
-			phba->sysfs_mbox.mbox = mbox;
-		phba->sysfs_mbox.state = SMBOX_WRITING;
-	} else {
-		if (phba->sysfs_mbox.state  != SMBOX_WRITING ||
-		    phba->sysfs_mbox.offset != off           ||
-		    phba->sysfs_mbox.mbox   == NULL) {
-			sysfs_mbox_idle(phba);
-			spin_unlock_irq(&phba->hbalock);
-			return -EAGAIN;
-		}
-	}
-
-	memcpy((uint8_t *) &phba->sysfs_mbox.mbox->u.mb + off,
-	       buf, count);
-
-	phba->sysfs_mbox.offset = off + count;
-
-	spin_unlock_irq(&phba->hbalock);
-
-	return count;
+	return -EPERM;
 }
 
 /**
@@ -4091,201 +4135,18 @@ sysfs_mbox_write(struct file *filp, struct kobject *kobj,
  * @count: bytes to transfer.
  *
  * Description:
- * Accessed via /sys/class/scsi_host/hostxxx/mbox.
- * Uses the sysfs mbox to receive data from to the adapter.
+ * Deprecated function. All mailbox access from user space is performed via the
+ * bsg interface.
  *
  * Returns:
- * -ERANGE off greater than mailbox command size
- * -EINVAL off, count or buff address invalid
- * zero if off and count are zero
- * -EACCES adapter over temp
- * -EPERM garbage can value to catch a multitude of errors
- * -EAGAIN management IO not permitted, state or off error
- * -ETIME mailbox timeout
- * -ENODEV mailbox error
- * count number of bytes transferred
+ * -EPERM operation not permitted
  **/
 static ssize_t
 sysfs_mbox_read(struct file *filp, struct kobject *kobj,
 		struct bin_attribute *bin_attr,
 		char *buf, loff_t off, size_t count)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct Scsi_Host  *shost = class_to_shost(dev);
-	struct lpfc_vport *vport = (struct lpfc_vport *) shost->hostdata;
-	struct lpfc_hba   *phba = vport->phba;
-	LPFC_MBOXQ_t *mboxq;
-	MAILBOX_t *pmb;
-	uint32_t mbox_tmo;
-	int rc;
-
-	if (off > MAILBOX_CMD_SIZE)
-		return -ERANGE;
-
-	if ((count + off) > MAILBOX_CMD_SIZE)
-		count = MAILBOX_CMD_SIZE - off;
-
-	if (off % 4 ||  count % 4 || (unsigned long)buf % 4)
-		return -EINVAL;
-
-	if (off && count == 0)
-		return 0;
-
-	spin_lock_irq(&phba->hbalock);
-
-	if (phba->over_temp_state == HBA_OVER_TEMP) {
-		sysfs_mbox_idle(phba);
-		spin_unlock_irq(&phba->hbalock);
-		return  -EACCES;
-	}
-
-	if (off == 0 &&
-	    phba->sysfs_mbox.state  == SMBOX_WRITING &&
-	    phba->sysfs_mbox.offset >= 2 * sizeof(uint32_t)) {
-		mboxq = (LPFC_MBOXQ_t *)&phba->sysfs_mbox.mbox;
-		pmb = &mboxq->u.mb;
-		switch (pmb->mbxCommand) {
-			/* Offline only */
-		case MBX_INIT_LINK:
-		case MBX_DOWN_LINK:
-		case MBX_CONFIG_LINK:
-		case MBX_CONFIG_RING:
-		case MBX_RESET_RING:
-		case MBX_UNREG_LOGIN:
-		case MBX_CLEAR_LA:
-		case MBX_DUMP_CONTEXT:
-		case MBX_RUN_DIAGS:
-		case MBX_RESTART:
-		case MBX_SET_MASK:
-		case MBX_SET_DEBUG:
-			if (!(vport->fc_flag & FC_OFFLINE_MODE)) {
-				printk(KERN_WARNING "mbox_read:Command 0x%x "
-				       "is illegal in on-line state\n",
-				       pmb->mbxCommand);
-				sysfs_mbox_idle(phba);
-				spin_unlock_irq(&phba->hbalock);
-				return -EPERM;
-			}
-		case MBX_WRITE_NV:
-		case MBX_WRITE_VPARMS:
-		case MBX_LOAD_SM:
-		case MBX_READ_NV:
-		case MBX_READ_CONFIG:
-		case MBX_READ_RCONFIG:
-		case MBX_READ_STATUS:
-		case MBX_READ_XRI:
-		case MBX_READ_REV:
-		case MBX_READ_LNK_STAT:
-		case MBX_DUMP_MEMORY:
-		case MBX_DOWN_LOAD:
-		case MBX_UPDATE_CFG:
-		case MBX_KILL_BOARD:
-		case MBX_LOAD_AREA:
-		case MBX_LOAD_EXP_ROM:
-		case MBX_BEACON:
-		case MBX_DEL_LD_ENTRY:
-		case MBX_SET_VARIABLE:
-		case MBX_WRITE_WWN:
-		case MBX_PORT_CAPABILITIES:
-		case MBX_PORT_IOV_CONTROL:
-			break;
-		case MBX_SECURITY_MGMT:
-		case MBX_AUTH_PORT:
-			if (phba->pci_dev_grp == LPFC_PCI_DEV_OC) {
-				printk(KERN_WARNING "mbox_read:Command 0x%x "
-				       "is not permitted\n", pmb->mbxCommand);
-				sysfs_mbox_idle(phba);
-				spin_unlock_irq(&phba->hbalock);
-				return -EPERM;
-			}
-			break;
-		case MBX_READ_SPARM64:
-		case MBX_READ_TOPOLOGY:
-		case MBX_REG_LOGIN:
-		case MBX_REG_LOGIN64:
-		case MBX_CONFIG_PORT:
-		case MBX_RUN_BIU_DIAG:
-			printk(KERN_WARNING "mbox_read: Illegal Command 0x%x\n",
-			       pmb->mbxCommand);
-			sysfs_mbox_idle(phba);
-			spin_unlock_irq(&phba->hbalock);
-			return -EPERM;
-		default:
-			printk(KERN_WARNING "mbox_read: Unknown Command 0x%x\n",
-			       pmb->mbxCommand);
-			sysfs_mbox_idle(phba);
-			spin_unlock_irq(&phba->hbalock);
-			return -EPERM;
-		}
-
-		/* If HBA encountered an error attention, allow only DUMP
-		 * or RESTART mailbox commands until the HBA is restarted.
-		 */
-		if (phba->pport->stopped &&
-		    pmb->mbxCommand != MBX_DUMP_MEMORY &&
-		    pmb->mbxCommand != MBX_RESTART &&
-		    pmb->mbxCommand != MBX_WRITE_VPARMS &&
-		    pmb->mbxCommand != MBX_WRITE_WWN)
-			lpfc_printf_log(phba, KERN_WARNING, LOG_MBOX,
-					"1259 mbox: Issued mailbox cmd "
-					"0x%x while in stopped state.\n",
-					pmb->mbxCommand);
-
-		phba->sysfs_mbox.mbox->vport = vport;
-
-		/* Don't allow mailbox commands to be sent when blocked
-		 * or when in the middle of discovery
-		 */
-		if (phba->sli.sli_flag & LPFC_BLOCK_MGMT_IO) {
-			sysfs_mbox_idle(phba);
-			spin_unlock_irq(&phba->hbalock);
-			return  -EAGAIN;
-		}
-
-		if ((vport->fc_flag & FC_OFFLINE_MODE) ||
-		    (!(phba->sli.sli_flag & LPFC_SLI_ACTIVE))) {
-
-			spin_unlock_irq(&phba->hbalock);
-			rc = lpfc_sli_issue_mbox (phba,
-						  phba->sysfs_mbox.mbox,
-						  MBX_POLL);
-			spin_lock_irq(&phba->hbalock);
-
-		} else {
-			spin_unlock_irq(&phba->hbalock);
-			mbox_tmo = lpfc_mbox_tmo_val(phba, mboxq);
-			rc = lpfc_sli_issue_mbox_wait(phba, mboxq, mbox_tmo);
-			spin_lock_irq(&phba->hbalock);
-		}
-
-		if (rc != MBX_SUCCESS) {
-			if (rc == MBX_TIMEOUT) {
-				phba->sysfs_mbox.mbox = NULL;
-			}
-			sysfs_mbox_idle(phba);
-			spin_unlock_irq(&phba->hbalock);
-			return  (rc == MBX_TIMEOUT) ? -ETIME : -ENODEV;
-		}
-		phba->sysfs_mbox.state = SMBOX_READING;
-	}
-	else if (phba->sysfs_mbox.offset != off ||
-		 phba->sysfs_mbox.state  != SMBOX_READING) {
-		printk(KERN_WARNING  "mbox_read: Bad State\n");
-		sysfs_mbox_idle(phba);
-		spin_unlock_irq(&phba->hbalock);
-		return -EAGAIN;
-	}
-
-	memcpy(buf, (uint8_t *) &pmb + off, count);
-
-	phba->sysfs_mbox.offset = off + count;
-
-	if (phba->sysfs_mbox.offset == MAILBOX_CMD_SIZE)
-		sysfs_mbox_idle(phba);
-
-	spin_unlock_irq(&phba->hbalock);
-
-	return count;
+	return -EPERM;
 }
 
 static struct bin_attribute sysfs_mbox_attr = {
@@ -4429,8 +4290,13 @@ lpfc_get_host_port_state(struct Scsi_Host *shost)
 		case LPFC_LINK_UP:
 		case LPFC_CLEAR_LA:
 		case LPFC_HBA_READY:
-			/* Links up, beyond this port_type reports state */
-			fc_host_port_state(shost) = FC_PORTSTATE_ONLINE;
+			/* Links up, reports port state accordingly */
+			if (vport->port_state < LPFC_VPORT_READY)
+				fc_host_port_state(shost) =
+							FC_PORTSTATE_BYPASSED;
+			else
+				fc_host_port_state(shost) =
+							FC_PORTSTATE_ONLINE;
 			break;
 		case LPFC_HBA_ERROR:
 			fc_host_port_state(shost) = FC_PORTSTATE_ERROR;
diff --git a/drivers/scsi/lpfc/lpfc_bsg.c b/drivers/scsi/lpfc/lpfc_bsg.c
index 6760c69f5253..56a86baece5b 100644
--- a/drivers/scsi/lpfc/lpfc_bsg.c
+++ b/drivers/scsi/lpfc/lpfc_bsg.c
@@ -916,9 +916,11 @@ lpfc_bsg_ct_unsol_event(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 				} else {
 					switch (cmd) {
 					case ELX_LOOPBACK_DATA:
-						diag_cmd_data_free(phba,
-						(struct lpfc_dmabufext *)
-							dmabuf);
+						if (phba->sli_rev <
+						    LPFC_SLI_REV4)
+							diag_cmd_data_free(phba,
+							(struct lpfc_dmabufext
+							 *)dmabuf);
 						break;
 					case ELX_LOOPBACK_XRI_SETUP:
 						if ((phba->sli_rev ==
@@ -1000,7 +1002,8 @@ lpfc_bsg_ct_unsol_event(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 error_ct_unsol_exit:
 	if (!list_empty(&head))
 		list_del(&head);
-	if (evt_req_id == SLI_CT_ELX_LOOPBACK)
+	if ((phba->sli_rev < LPFC_SLI_REV4) &&
+	    (evt_req_id == SLI_CT_ELX_LOOPBACK))
 		return 0;
 	return 1;
 }
@@ -1566,7 +1569,7 @@ lpfc_sli3_bsg_diag_loopback_mode(struct lpfc_hba *phba, struct fc_bsg_job *job)
 	struct diag_mode_set *loopback_mode;
 	uint32_t link_flags;
 	uint32_t timeout;
-	LPFC_MBOXQ_t *pmboxq;
+	LPFC_MBOXQ_t *pmboxq  = NULL;
 	int mbxstatus = MBX_SUCCESS;
 	int i = 0;
 	int rc = 0;
@@ -1615,7 +1618,6 @@ lpfc_sli3_bsg_diag_loopback_mode(struct lpfc_hba *phba, struct fc_bsg_job *job)
 				rc = -ETIMEDOUT;
 				goto loopback_mode_exit;
 			}
-
 			msleep(10);
 		}
 
@@ -1635,7 +1637,9 @@ lpfc_sli3_bsg_diag_loopback_mode(struct lpfc_hba *phba, struct fc_bsg_job *job)
 		if ((mbxstatus != MBX_SUCCESS) || (pmboxq->u.mb.mbxStatus))
 			rc = -ENODEV;
 		else {
+			spin_lock_irq(&phba->hbalock);
 			phba->link_flag |= LS_LOOPBACK_MODE;
+			spin_unlock_irq(&phba->hbalock);
 			/* wait for the link attention interrupt */
 			msleep(100);
 
@@ -1659,7 +1663,7 @@ loopback_mode_exit:
 	/*
 	 * Let SLI layer release mboxq if mbox command completed after timeout.
 	 */
-	if (mbxstatus != MBX_TIMEOUT)
+	if (pmboxq && mbxstatus != MBX_TIMEOUT)
 		mempool_free(pmboxq, phba->mbox_mem_pool);
 
 job_error:
@@ -1700,11 +1704,16 @@ lpfc_sli4_bsg_set_link_diag_state(struct lpfc_hba *phba, uint32_t diag)
 		rc = -ENOMEM;
 		goto link_diag_state_set_out;
 	}
+	lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+			"3128 Set link to diagnostic state:x%x (x%x/x%x)\n",
+			diag, phba->sli4_hba.lnk_info.lnk_tp,
+			phba->sli4_hba.lnk_info.lnk_no);
+
 	link_diag_state = &pmboxq->u.mqe.un.link_diag_state;
 	bf_set(lpfc_mbx_set_diag_state_link_num, &link_diag_state->u.req,
-	       phba->sli4_hba.link_state.number);
+	       phba->sli4_hba.lnk_info.lnk_no);
 	bf_set(lpfc_mbx_set_diag_state_link_type, &link_diag_state->u.req,
-	       phba->sli4_hba.link_state.type);
+	       phba->sli4_hba.lnk_info.lnk_tp);
 	if (diag)
 		bf_set(lpfc_mbx_set_diag_state_diag,
 		       &link_diag_state->u.req, 1);
@@ -1727,6 +1736,79 @@ link_diag_state_set_out:
 }
 
 /**
+ * lpfc_sli4_bsg_set_internal_loopback - set sli4 internal loopback diagnostic
+ * @phba: Pointer to HBA context object.
+ *
+ * This function is responsible for issuing a sli4 mailbox command for setting
+ * up internal loopback diagnostic.
+ */
+static int
+lpfc_sli4_bsg_set_internal_loopback(struct lpfc_hba *phba)
+{
+	LPFC_MBOXQ_t *pmboxq;
+	uint32_t req_len, alloc_len;
+	struct lpfc_mbx_set_link_diag_loopback *link_diag_loopback;
+	int mbxstatus = MBX_SUCCESS, rc = 0;
+
+	pmboxq = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!pmboxq)
+		return -ENOMEM;
+	req_len = (sizeof(struct lpfc_mbx_set_link_diag_loopback) -
+		   sizeof(struct lpfc_sli4_cfg_mhdr));
+	alloc_len = lpfc_sli4_config(phba, pmboxq, LPFC_MBOX_SUBSYSTEM_FCOE,
+				LPFC_MBOX_OPCODE_FCOE_LINK_DIAG_LOOPBACK,
+				req_len, LPFC_SLI4_MBX_EMBED);
+	if (alloc_len != req_len) {
+		mempool_free(pmboxq, phba->mbox_mem_pool);
+		return -ENOMEM;
+	}
+	link_diag_loopback = &pmboxq->u.mqe.un.link_diag_loopback;
+	bf_set(lpfc_mbx_set_diag_state_link_num,
+	       &link_diag_loopback->u.req, phba->sli4_hba.lnk_info.lnk_no);
+	bf_set(lpfc_mbx_set_diag_state_link_type,
+	       &link_diag_loopback->u.req, phba->sli4_hba.lnk_info.lnk_tp);
+	bf_set(lpfc_mbx_set_diag_lpbk_type, &link_diag_loopback->u.req,
+	       LPFC_DIAG_LOOPBACK_TYPE_SERDES);
+
+	mbxstatus = lpfc_sli_issue_mbox_wait(phba, pmboxq, LPFC_MBOX_TMO);
+	if ((mbxstatus != MBX_SUCCESS) || (pmboxq->u.mb.mbxStatus)) {
+		lpfc_printf_log(phba, KERN_WARNING, LOG_LIBDFC,
+				"3127 Failed setup loopback mode mailbox "
+				"command, rc:x%x, status:x%x\n", mbxstatus,
+				pmboxq->u.mb.mbxStatus);
+		rc = -ENODEV;
+	}
+	if (pmboxq && (mbxstatus != MBX_TIMEOUT))
+		mempool_free(pmboxq, phba->mbox_mem_pool);
+	return rc;
+}
+
+/**
+ * lpfc_sli4_diag_fcport_reg_setup - setup port registrations for diagnostic
+ * @phba: Pointer to HBA context object.
+ *
+ * This function set up SLI4 FC port registrations for diagnostic run, which
+ * includes all the rpis, vfi, and also vpi.
+ */
+static int
+lpfc_sli4_diag_fcport_reg_setup(struct lpfc_hba *phba)
+{
+	int rc;
+
+	if (phba->pport->fc_flag & FC_VFI_REGISTERED) {
+		lpfc_printf_log(phba, KERN_WARNING, LOG_LIBDFC,
+				"3136 Port still had vfi registered: "
+				"mydid:x%x, fcfi:%d, vfi:%d, vpi:%d\n",
+				phba->pport->fc_myDID, phba->fcf.fcfi,
+				phba->sli4_hba.vfi_ids[phba->pport->vfi],
+				phba->vpi_ids[phba->pport->vpi]);
+		return -EINVAL;
+	}
+	rc = lpfc_issue_reg_vfi(phba->pport);
+	return rc;
+}
+
+/**
  * lpfc_sli4_bsg_diag_loopback_mode - process an sli4 bsg vendor command
  * @phba: Pointer to HBA context object.
  * @job: LPFC_BSG_VENDOR_DIAG_MODE
@@ -1738,10 +1820,8 @@ static int
 lpfc_sli4_bsg_diag_loopback_mode(struct lpfc_hba *phba, struct fc_bsg_job *job)
 {
 	struct diag_mode_set *loopback_mode;
-	uint32_t link_flags, timeout, req_len, alloc_len;
-	struct lpfc_mbx_set_link_diag_loopback *link_diag_loopback;
-	LPFC_MBOXQ_t *pmboxq = NULL;
-	int mbxstatus = MBX_SUCCESS, i, rc = 0;
+	uint32_t link_flags, timeout;
+	int i, rc = 0;
 
 	/* no data to return just the return code */
 	job->reply->reply_payload_rcv_len = 0;
@@ -1762,65 +1842,100 @@ lpfc_sli4_bsg_diag_loopback_mode(struct lpfc_hba *phba, struct fc_bsg_job *job)
 	if (rc)
 		goto job_error;
 
+	/* indicate we are in loobpack diagnostic mode */
+	spin_lock_irq(&phba->hbalock);
+	phba->link_flag |= LS_LOOPBACK_MODE;
+	spin_unlock_irq(&phba->hbalock);
+
+	/* reset port to start frome scratch */
+	rc = lpfc_selective_reset(phba);
+	if (rc)
+		goto job_error;
+
 	/* bring the link to diagnostic mode */
+	lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+			"3129 Bring link to diagnostic state.\n");
 	loopback_mode = (struct diag_mode_set *)
 		job->request->rqst_data.h_vendor.vendor_cmd;
 	link_flags = loopback_mode->type;
 	timeout = loopback_mode->timeout * 100;
 
 	rc = lpfc_sli4_bsg_set_link_diag_state(phba, 1);
-	if (rc)
+	if (rc) {
+		lpfc_printf_log(phba, KERN_WARNING, LOG_LIBDFC,
+				"3130 Failed to bring link to diagnostic "
+				"state, rc:x%x\n", rc);
 		goto loopback_mode_exit;
+	}
 
 	/* wait for link down before proceeding */
 	i = 0;
 	while (phba->link_state != LPFC_LINK_DOWN) {
 		if (i++ > timeout) {
 			rc = -ETIMEDOUT;
+			lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+					"3131 Timeout waiting for link to "
+					"diagnostic mode, timeout:%d ms\n",
+					timeout * 10);
 			goto loopback_mode_exit;
 		}
 		msleep(10);
 	}
+
 	/* set up loopback mode */
-	pmboxq = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-	if (!pmboxq) {
-		rc = -ENOMEM;
-		goto loopback_mode_exit;
-	}
-	req_len = (sizeof(struct lpfc_mbx_set_link_diag_loopback) -
-		   sizeof(struct lpfc_sli4_cfg_mhdr));
-	alloc_len = lpfc_sli4_config(phba, pmboxq, LPFC_MBOX_SUBSYSTEM_FCOE,
-				LPFC_MBOX_OPCODE_FCOE_LINK_DIAG_LOOPBACK,
-				req_len, LPFC_SLI4_MBX_EMBED);
-	if (alloc_len != req_len) {
-		rc = -ENOMEM;
+	lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+			"3132 Set up loopback mode:x%x\n", link_flags);
+
+	if (link_flags == INTERNAL_LOOP_BACK)
+		rc = lpfc_sli4_bsg_set_internal_loopback(phba);
+	else if (link_flags == EXTERNAL_LOOP_BACK)
+		rc = lpfc_hba_init_link_fc_topology(phba,
+						    FLAGS_TOPOLOGY_MODE_PT_PT,
+						    MBX_NOWAIT);
+	else {
+		rc = -EINVAL;
+		lpfc_printf_log(phba, KERN_ERR, LOG_LIBDFC,
+				"3141 Loopback mode:x%x not supported\n",
+				link_flags);
 		goto loopback_mode_exit;
 	}
-	link_diag_loopback = &pmboxq->u.mqe.un.link_diag_loopback;
-	bf_set(lpfc_mbx_set_diag_state_link_num,
-	       &link_diag_loopback->u.req, phba->sli4_hba.link_state.number);
-	bf_set(lpfc_mbx_set_diag_state_link_type,
-	       &link_diag_loopback->u.req, phba->sli4_hba.link_state.type);
-	if (link_flags == INTERNAL_LOOP_BACK)
-		bf_set(lpfc_mbx_set_diag_lpbk_type,
-		       &link_diag_loopback->u.req,
-		       LPFC_DIAG_LOOPBACK_TYPE_INTERNAL);
-	else
-		bf_set(lpfc_mbx_set_diag_lpbk_type,
-		       &link_diag_loopback->u.req,
-		       LPFC_DIAG_LOOPBACK_TYPE_EXTERNAL);
 
-	mbxstatus = lpfc_sli_issue_mbox_wait(phba, pmboxq, LPFC_MBOX_TMO);
-	if ((mbxstatus != MBX_SUCCESS) || (pmboxq->u.mb.mbxStatus))
-		rc = -ENODEV;
-	else {
-		phba->link_flag |= LS_LOOPBACK_MODE;
+	if (!rc) {
 		/* wait for the link attention interrupt */
 		msleep(100);
 		i = 0;
+		while (phba->link_state < LPFC_LINK_UP) {
+			if (i++ > timeout) {
+				rc = -ETIMEDOUT;
+				lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+					"3137 Timeout waiting for link up "
+					"in loopback mode, timeout:%d ms\n",
+					timeout * 10);
+				break;
+			}
+			msleep(10);
+		}
+	}
+
+	/* port resource registration setup for loopback diagnostic */
+	if (!rc) {
+		/* set up a none zero myDID for loopback test */
+		phba->pport->fc_myDID = 1;
+		rc = lpfc_sli4_diag_fcport_reg_setup(phba);
+	} else
+		goto loopback_mode_exit;
+
+	if (!rc) {
+		/* wait for the port ready */
+		msleep(100);
+		i = 0;
 		while (phba->link_state != LPFC_HBA_READY) {
 			if (i++ > timeout) {
 				rc = -ETIMEDOUT;
+				lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+					"3133 Timeout waiting for port "
+					"loopback mode ready, timeout:%d ms\n",
+					timeout * 10);
 				break;
 			}
 			msleep(10);
@@ -1828,14 +1943,14 @@ lpfc_sli4_bsg_diag_loopback_mode(struct lpfc_hba *phba, struct fc_bsg_job *job)
 	}
 
 loopback_mode_exit:
+	/* clear loopback diagnostic mode */
+	if (rc) {
+		spin_lock_irq(&phba->hbalock);
+		phba->link_flag &= ~LS_LOOPBACK_MODE;
+		spin_unlock_irq(&phba->hbalock);
+	}
 	lpfc_bsg_diag_mode_exit(phba);
 
-	/*
-	 * Let SLI layer release mboxq if mbox command completed after timeout.
-	 */
-	if (pmboxq && (mbxstatus != MBX_TIMEOUT))
-		mempool_free(pmboxq, phba->mbox_mem_pool);
-
 job_error:
 	/* make error code available to userspace */
 	job->reply->result = rc;
@@ -1879,7 +1994,6 @@ lpfc_bsg_diag_loopback_mode(struct fc_bsg_job *job)
 		rc = -ENODEV;
 
 	return rc;
-
 }
 
 /**
@@ -1895,7 +2009,9 @@ lpfc_sli4_bsg_diag_mode_end(struct fc_bsg_job *job)
 	struct Scsi_Host *shost;
 	struct lpfc_vport *vport;
 	struct lpfc_hba *phba;
-	int rc;
+	struct diag_mode_set *loopback_mode_end_cmd;
+	uint32_t timeout;
+	int rc, i;
 
 	shost = job->shost;
 	if (!shost)
@@ -1913,11 +2029,47 @@ lpfc_sli4_bsg_diag_mode_end(struct fc_bsg_job *job)
 	    LPFC_SLI_INTF_IF_TYPE_2)
 		return -ENODEV;
 
+	/* clear loopback diagnostic mode */
+	spin_lock_irq(&phba->hbalock);
+	phba->link_flag &= ~LS_LOOPBACK_MODE;
+	spin_unlock_irq(&phba->hbalock);
+	loopback_mode_end_cmd = (struct diag_mode_set *)
+			job->request->rqst_data.h_vendor.vendor_cmd;
+	timeout = loopback_mode_end_cmd->timeout * 100;
+
 	rc = lpfc_sli4_bsg_set_link_diag_state(phba, 0);
+	if (rc) {
+		lpfc_printf_log(phba, KERN_WARNING, LOG_LIBDFC,
+				"3139 Failed to bring link to diagnostic "
+				"state, rc:x%x\n", rc);
+		goto loopback_mode_end_exit;
+	}
 
-	if (!rc)
-		rc = phba->lpfc_hba_init_link(phba, MBX_NOWAIT);
+	/* wait for link down before proceeding */
+	i = 0;
+	while (phba->link_state != LPFC_LINK_DOWN) {
+		if (i++ > timeout) {
+			rc = -ETIMEDOUT;
+			lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+					"3140 Timeout waiting for link to "
+					"diagnostic mode_end, timeout:%d ms\n",
+					timeout * 10);
+			/* there is nothing much we can do here */
+			break;
+		}
+		msleep(10);
+	}
+
+	/* reset port resource registrations */
+	rc = lpfc_selective_reset(phba);
+	phba->pport->fc_myDID = 0;
 
+loopback_mode_end_exit:
+	/* make return code available to userspace */
+	job->reply->result = rc;
+	/* complete the job back to userspace if no error */
+	if (rc == 0)
+		job->job_done(job);
 	return rc;
 }
 
@@ -2012,9 +2164,9 @@ lpfc_sli4_bsg_link_diag_test(struct fc_bsg_job *job)
 	}
 	run_link_diag_test = &pmboxq->u.mqe.un.link_diag_test;
 	bf_set(lpfc_mbx_run_diag_test_link_num, &run_link_diag_test->u.req,
-	       phba->sli4_hba.link_state.number);
+	       phba->sli4_hba.lnk_info.lnk_no);
 	bf_set(lpfc_mbx_run_diag_test_link_type, &run_link_diag_test->u.req,
-	       phba->sli4_hba.link_state.type);
+	       phba->sli4_hba.lnk_info.lnk_tp);
 	bf_set(lpfc_mbx_run_diag_test_test_id, &run_link_diag_test->u.req,
 	       link_diag_test_cmd->test_id);
 	bf_set(lpfc_mbx_run_diag_test_loops, &run_link_diag_test->u.req,
@@ -2091,10 +2243,18 @@ static int lpfcdiag_loop_self_reg(struct lpfc_hba *phba, uint16_t *rpi)
 	if (!mbox)
 		return -ENOMEM;
 
-	if (phba->sli_rev == LPFC_SLI_REV4)
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		status = lpfc_reg_rpi(phba, 0, phba->pport->fc_myDID,
+				(uint8_t *)&phba->pport->fc_sparam,
+				mbox, *rpi);
+	else {
 		*rpi = lpfc_sli4_alloc_rpi(phba);
-	status = lpfc_reg_rpi(phba, 0, phba->pport->fc_myDID,
-			      (uint8_t *)&phba->pport->fc_sparam, mbox, *rpi);
+		status = lpfc_reg_rpi(phba, phba->pport->vpi,
+				phba->pport->fc_myDID,
+				(uint8_t *)&phba->pport->fc_sparam,
+				mbox, *rpi);
+	}
+
 	if (status) {
 		mempool_free(mbox, phba->mbox_mem_pool);
 		if (phba->sli_rev == LPFC_SLI_REV4)
@@ -2117,7 +2277,8 @@ static int lpfcdiag_loop_self_reg(struct lpfc_hba *phba, uint16_t *rpi)
 		return -ENODEV;
 	}
 
-	*rpi = mbox->u.mb.un.varWords[0];
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		*rpi = mbox->u.mb.un.varWords[0];
 
 	lpfc_mbuf_free(phba, dmabuff->virt, dmabuff->phys);
 	kfree(dmabuff);
@@ -2142,7 +2303,12 @@ static int lpfcdiag_loop_self_unreg(struct lpfc_hba *phba, uint16_t rpi)
 	if (mbox == NULL)
 		return -ENOMEM;
 
-	lpfc_unreg_login(phba, 0, rpi, mbox);
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		lpfc_unreg_login(phba, 0, rpi, mbox);
+	else
+		lpfc_unreg_login(phba, phba->pport->vpi,
+				 phba->sli4_hba.rpi_ids[rpi], mbox);
+
 	status = lpfc_sli_issue_mbox_wait(phba, mbox, LPFC_MBOX_TMO);
 
 	if ((status != MBX_SUCCESS) || (mbox->u.mb.mbxStatus)) {
@@ -2630,15 +2796,15 @@ lpfc_bsg_diag_loopback_run(struct fc_bsg_job *job)
 	uint32_t full_size;
 	size_t segment_len = 0, segment_offset = 0, current_offset = 0;
 	uint16_t rpi = 0;
-	struct lpfc_iocbq *cmdiocbq, *rspiocbq;
-	IOCB_t *cmd, *rsp;
+	struct lpfc_iocbq *cmdiocbq, *rspiocbq = NULL;
+	IOCB_t *cmd, *rsp = NULL;
 	struct lpfc_sli_ct_request *ctreq;
 	struct lpfc_dmabuf *txbmp;
 	struct ulp_bde64 *txbpl = NULL;
 	struct lpfc_dmabufext *txbuffer = NULL;
 	struct list_head head;
 	struct lpfc_dmabuf  *curr;
-	uint16_t txxri, rxxri;
+	uint16_t txxri = 0, rxxri;
 	uint32_t num_bde;
 	uint8_t *ptr = NULL, *rx_databuf = NULL;
 	int rc = 0;
@@ -2665,7 +2831,6 @@ lpfc_bsg_diag_loopback_run(struct fc_bsg_job *job)
 		rc = -EINVAL;
 		goto loopback_test_exit;
 	}
-
 	diag_mode = (struct diag_mode_test *)
 		job->request->rqst_data.h_vendor.vendor_cmd;
 
@@ -2720,18 +2885,19 @@ lpfc_bsg_diag_loopback_run(struct fc_bsg_job *job)
 	if (rc)
 		goto loopback_test_exit;
 
-	rc = lpfcdiag_loop_get_xri(phba, rpi, &txxri, &rxxri);
-	if (rc) {
-		lpfcdiag_loop_self_unreg(phba, rpi);
-		goto loopback_test_exit;
-	}
+	if (phba->sli_rev < LPFC_SLI_REV4) {
+		rc = lpfcdiag_loop_get_xri(phba, rpi, &txxri, &rxxri);
+		if (rc) {
+			lpfcdiag_loop_self_unreg(phba, rpi);
+			goto loopback_test_exit;
+		}
 
-	rc = lpfcdiag_loop_post_rxbufs(phba, rxxri, full_size);
-	if (rc) {
-		lpfcdiag_loop_self_unreg(phba, rpi);
-		goto loopback_test_exit;
+		rc = lpfcdiag_loop_post_rxbufs(phba, rxxri, full_size);
+		if (rc) {
+			lpfcdiag_loop_self_unreg(phba, rpi);
+			goto loopback_test_exit;
+		}
 	}
-
 	evt = lpfc_bsg_event_new(FC_REG_CT_EVENT, current->pid,
 				SLI_CT_ELX_LOOPBACK);
 	if (!evt) {
@@ -2746,7 +2912,8 @@ lpfc_bsg_diag_loopback_run(struct fc_bsg_job *job)
 	spin_unlock_irqrestore(&phba->ct_ev_lock, flags);
 
 	cmdiocbq = lpfc_sli_get_iocbq(phba);
-	rspiocbq = lpfc_sli_get_iocbq(phba);
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		rspiocbq = lpfc_sli_get_iocbq(phba);
 	txbmp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
 
 	if (txbmp) {
@@ -2759,14 +2926,18 @@ lpfc_bsg_diag_loopback_run(struct fc_bsg_job *job)
 		}
 	}
 
-	if (!cmdiocbq || !rspiocbq || !txbmp || !txbpl || !txbuffer ||
-		!txbmp->virt) {
+	if (!cmdiocbq || !txbmp || !txbpl || !txbuffer || !txbmp->virt) {
+		rc = -ENOMEM;
+		goto err_loopback_test_exit;
+	}
+	if ((phba->sli_rev < LPFC_SLI_REV4) && !rspiocbq) {
 		rc = -ENOMEM;
 		goto err_loopback_test_exit;
 	}
 
 	cmd = &cmdiocbq->iocb;
-	rsp = &rspiocbq->iocb;
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		rsp = &rspiocbq->iocb;
 
 	INIT_LIST_HEAD(&head);
 	list_add_tail(&head, &txbuffer->dma.list);
@@ -2796,7 +2967,6 @@ lpfc_bsg_diag_loopback_run(struct fc_bsg_job *job)
 	list_del(&head);
 
 	/* Build the XMIT_SEQUENCE iocb */
-
 	num_bde = (uint32_t)txbuffer->flag;
 
 	cmd->un.xseq64.bdl.addrHigh = putPaddrHigh(txbmp->phys);
@@ -2813,16 +2983,27 @@ lpfc_bsg_diag_loopback_run(struct fc_bsg_job *job)
 	cmd->ulpBdeCount = 1;
 	cmd->ulpLe = 1;
 	cmd->ulpClass = CLASS3;
-	cmd->ulpContext = txxri;
 
+	if (phba->sli_rev < LPFC_SLI_REV4) {
+		cmd->ulpContext = txxri;
+	} else {
+		cmd->un.xseq64.bdl.ulpIoTag32 = 0;
+		cmd->un.ulpWord[3] = phba->sli4_hba.rpi_ids[rpi];
+		cmdiocbq->context3 = txbmp;
+		cmdiocbq->sli4_xritag = NO_XRI;
+		cmd->unsli3.rcvsli3.ox_id = 0xffff;
+	}
 	cmdiocbq->iocb_flag |= LPFC_IO_LIBDFC;
 	cmdiocbq->vport = phba->pport;
-
 	iocb_stat = lpfc_sli_issue_iocb_wait(phba, LPFC_ELS_RING, cmdiocbq,
 					     rspiocbq, (phba->fc_ratov * 2) +
 					     LPFC_DRVR_TIMEOUT);
 
-	if ((iocb_stat != IOCB_SUCCESS) || (rsp->ulpStatus != IOCB_SUCCESS)) {
+	if ((iocb_stat != IOCB_SUCCESS) || ((phba->sli_rev < LPFC_SLI_REV4) &&
+					   (rsp->ulpStatus != IOCB_SUCCESS))) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_LIBDFC,
+				"3126 Failed loopback test issue iocb: "
+				"iocb_stat:x%x\n", iocb_stat);
 		rc = -EIO;
 		goto err_loopback_test_exit;
 	}
@@ -2832,9 +3013,12 @@ lpfc_bsg_diag_loopback_run(struct fc_bsg_job *job)
 		evt->wq, !list_empty(&evt->events_to_see),
 		((phba->fc_ratov * 2) + LPFC_DRVR_TIMEOUT) * HZ);
 	evt->waiting = 0;
-	if (list_empty(&evt->events_to_see))
+	if (list_empty(&evt->events_to_see)) {
 		rc = (time_left) ? -EINTR : -ETIMEDOUT;
-	else {
+		lpfc_printf_log(phba, KERN_ERR, LOG_LIBDFC,
+				"3125 Not receiving unsolicited event, "
+				"rc:x%x\n", rc);
+	} else {
 		spin_lock_irqsave(&phba->ct_ev_lock, flags);
 		list_move(evt->events_to_see.prev, &evt->events_to_get);
 		evdat = list_entry(evt->events_to_get.prev,
@@ -2891,7 +3075,7 @@ loopback_test_exit:
 	job->reply->result = rc;
 	job->dd_data = NULL;
 	/* complete the job back to userspace if no error */
-	if (rc == 0)
+	if (rc == IOCB_SUCCESS)
 		job->job_done(job);
 	return rc;
 }
@@ -3078,7 +3262,9 @@ static int lpfc_bsg_check_cmd_access(struct lpfc_hba *phba,
 			&& (mb->un.varWords[1] == 1)) {
 			phba->wait_4_mlo_maint_flg = 1;
 		} else if (mb->un.varWords[0] == SETVAR_MLORST) {
+			spin_lock_irq(&phba->hbalock);
 			phba->link_flag &= ~LS_LOOPBACK_MODE;
+			spin_unlock_irq(&phba->hbalock);
 			phba->fc_topology = LPFC_TOPOLOGY_PT_PT;
 		}
 		break;
@@ -3140,6 +3326,9 @@ lpfc_bsg_issue_mbox_ext_handle_job(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 	unsigned long flags;
 	uint32_t size;
 	int rc = 0;
+	struct lpfc_dmabuf *dmabuf;
+	struct lpfc_sli_config_mbox *sli_cfg_mbx;
+	uint8_t *pmbx;
 
 	spin_lock_irqsave(&phba->ct_ev_lock, flags);
 	dd_data = pmboxq->context1;
@@ -3156,7 +3345,19 @@ lpfc_bsg_issue_mbox_ext_handle_job(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 	 */
 	pmb = (uint8_t *)&pmboxq->u.mb;
 	pmb_buf = (uint8_t *)dd_data->context_un.mbox.mb;
+	/* Copy the byte swapped response mailbox back to the user */
 	memcpy(pmb_buf, pmb, sizeof(MAILBOX_t));
+	/* if there is any non-embedded extended data copy that too */
+	dmabuf = phba->mbox_ext_buf_ctx.mbx_dmabuf;
+	sli_cfg_mbx = (struct lpfc_sli_config_mbox *)dmabuf->virt;
+	if (!bsg_bf_get(lpfc_mbox_hdr_emb,
+	    &sli_cfg_mbx->un.sli_config_emb0_subsys.sli_config_hdr)) {
+		pmbx = (uint8_t *)dmabuf->virt;
+		/* byte swap the extended data following the mailbox command */
+		lpfc_sli_pcimem_bcopy(&pmbx[sizeof(MAILBOX_t)],
+			&pmbx[sizeof(MAILBOX_t)],
+			sli_cfg_mbx->un.sli_config_emb0_subsys.mse[0].buf_len);
+	}
 
 	job = dd_data->context_un.mbox.set_job;
 	if (job) {
@@ -3519,6 +3720,18 @@ lpfc_bsg_sli_cfg_read_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	/* state change */
 	phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_PORT;
 
+	/*
+	 * Non-embedded mailbox subcommand data gets byte swapped here because
+	 * the lower level driver code only does the first 64 mailbox words.
+	 */
+	if ((!bsg_bf_get(lpfc_mbox_hdr_emb,
+	    &sli_cfg_mbx->un.sli_config_emb0_subsys.sli_config_hdr)) &&
+		(nemb_tp == nemb_mse))
+		lpfc_sli_pcimem_bcopy(&pmbx[sizeof(MAILBOX_t)],
+			&pmbx[sizeof(MAILBOX_t)],
+				sli_cfg_mbx->un.sli_config_emb0_subsys.
+					mse[0].buf_len);
+
 	rc = lpfc_sli_issue_mbox(phba, pmboxq, MBX_NOWAIT);
 	if ((rc == MBX_SUCCESS) || (rc == MBX_BUSY)) {
 		lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
@@ -3575,7 +3788,7 @@ lpfc_bsg_sli_cfg_write_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 			&sli_cfg_mbx->un.sli_config_emb0_subsys.sli_config_hdr);
 		if (ext_buf_cnt > LPFC_MBX_SLI_CONFIG_MAX_MSE) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_LIBDFC,
-					"2953 Handled SLI_CONFIG(mse) wr, "
+					"2953 Failed SLI_CONFIG(mse) wr, "
 					"ext_buf_cnt(%d) out of range(%d)\n",
 					ext_buf_cnt,
 					LPFC_MBX_SLI_CONFIG_MAX_MSE);
@@ -3593,7 +3806,7 @@ lpfc_bsg_sli_cfg_write_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 		ext_buf_cnt = sli_cfg_mbx->un.sli_config_emb1_subsys.hbd_count;
 		if (ext_buf_cnt > LPFC_MBX_SLI_CONFIG_MAX_HBD) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_LIBDFC,
-					"2954 Handled SLI_CONFIG(hbd) wr, "
+					"2954 Failed SLI_CONFIG(hbd) wr, "
 					"ext_buf_cnt(%d) out of range(%d)\n",
 					ext_buf_cnt,
 					LPFC_MBX_SLI_CONFIG_MAX_HBD);
@@ -3687,6 +3900,7 @@ lpfc_bsg_sli_cfg_write_cmd_ext(struct lpfc_hba *phba, struct fc_bsg_job *job,
 				"2956 Failed to issue SLI_CONFIG ext-buffer "
 				"maibox command, rc:x%x\n", rc);
 		rc = -EPIPE;
+		goto job_error;
 	}
 
 	/* wait for additoinal external buffers */
@@ -3721,7 +3935,7 @@ lpfc_bsg_handle_sli_cfg_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	uint32_t opcode;
 	int rc = SLI_CONFIG_NOT_HANDLED;
 
-	/* state change */
+	/* state change on new multi-buffer pass-through mailbox command */
 	phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_HOST;
 
 	sli_cfg_mbx = (struct lpfc_sli_config_mbox *)dmabuf->virt;
@@ -3752,18 +3966,36 @@ lpfc_bsg_handle_sli_cfg_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 				break;
 			default:
 				lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
-						"2959 Not handled SLI_CONFIG "
+						"2959 Reject SLI_CONFIG "
 						"subsys_fcoe, opcode:x%x\n",
 						opcode);
-				rc = SLI_CONFIG_NOT_HANDLED;
+				rc = -EPERM;
+				break;
+			}
+		} else if (subsys == SLI_CONFIG_SUBSYS_COMN) {
+			switch (opcode) {
+			case COMN_OPCODE_GET_CNTL_ADDL_ATTRIBUTES:
+				lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+						"3106 Handled SLI_CONFIG "
+						"subsys_fcoe, opcode:x%x\n",
+						opcode);
+				rc = lpfc_bsg_sli_cfg_read_cmd_ext(phba, job,
+							nemb_mse, dmabuf);
+				break;
+			default:
+				lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
+						"3107 Reject SLI_CONFIG "
+						"subsys_fcoe, opcode:x%x\n",
+						opcode);
+				rc = -EPERM;
 				break;
 			}
 		} else {
 			lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
-					"2977 Handled SLI_CONFIG "
+					"2977 Reject SLI_CONFIG "
 					"subsys:x%d, opcode:x%x\n",
 					subsys, opcode);
-			rc = SLI_CONFIG_NOT_HANDLED;
+			rc = -EPERM;
 		}
 	} else {
 		subsys = bsg_bf_get(lpfc_emb1_subcmnd_subsys,
@@ -3799,12 +4031,17 @@ lpfc_bsg_handle_sli_cfg_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 			}
 		} else {
 			lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
-					"2978 Handled SLI_CONFIG "
+					"2978 Not handled SLI_CONFIG "
 					"subsys:x%d, opcode:x%x\n",
 					subsys, opcode);
 			rc = SLI_CONFIG_NOT_HANDLED;
 		}
 	}
+
+	/* state reset on not handled new multi-buffer mailbox command */
+	if (rc != SLI_CONFIG_HANDLED)
+		phba->mbox_ext_buf_ctx.state = LPFC_BSG_MBOX_IDLE;
+
 	return rc;
 }
 
@@ -4262,11 +4499,8 @@ lpfc_bsg_issue_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 
 	/* extended mailbox commands will need an extended buffer */
 	if (mbox_req->inExtWLen || mbox_req->outExtWLen) {
-		/* any data for the device? */
-		if (mbox_req->inExtWLen) {
-			from = pmbx;
-			ext = from + sizeof(MAILBOX_t);
-		}
+		from = pmbx;
+		ext = from + sizeof(MAILBOX_t);
 		pmboxq->context2 = ext;
 		pmboxq->in_ext_byte_len =
 			mbox_req->inExtWLen * sizeof(uint32_t);
diff --git a/drivers/scsi/lpfc/lpfc_bsg.h b/drivers/scsi/lpfc/lpfc_bsg.h
index c8c2b47ea886..edfe61fc52b1 100644
--- a/drivers/scsi/lpfc/lpfc_bsg.h
+++ b/drivers/scsi/lpfc/lpfc_bsg.h
@@ -96,7 +96,7 @@ struct get_mgmt_rev {
 };
 
 #define MANAGEMENT_MAJOR_REV   1
-#define MANAGEMENT_MINOR_REV   0
+#define MANAGEMENT_MINOR_REV   1
 
 /* the MgmtRevInfo structure */
 struct MgmtRevInfo {
@@ -248,6 +248,7 @@ struct lpfc_sli_config_emb1_subsys {
 #define COMN_OPCODE_WRITE_OBJECT	0xAC
 #define COMN_OPCODE_READ_OBJECT_LIST	0xAD
 #define COMN_OPCODE_DELETE_OBJECT	0xAE
+#define COMN_OPCODE_GET_CNTL_ADDL_ATTRIBUTES	0x79
 	uint32_t timeout;
 	uint32_t request_length;
 	uint32_t word9;
diff --git a/drivers/scsi/lpfc/lpfc_compat.h b/drivers/scsi/lpfc/lpfc_compat.h
index 75e2e569dede..c88e556ea62e 100644
--- a/drivers/scsi/lpfc/lpfc_compat.h
+++ b/drivers/scsi/lpfc/lpfc_compat.h
@@ -1,7 +1,7 @@
 /*******************************************************************
  * This file is part of the Emulex Linux Device Driver for         *
  * Fibre Channel Host Bus Adapters.                                *
- * Copyright (C) 2004-2005 Emulex.  All rights reserved.           *
+ * Copyright (C) 2004-2011 Emulex.  All rights reserved.           *
  * EMULEX and SLI are trademarks of Emulex.                        *
  * www.emulex.com                                                  *
  *                                                                 *
@@ -82,7 +82,8 @@ lpfc_memcpy_from_slim( void *dest, void __iomem *src, unsigned int bytes)
 static inline void
 lpfc_memcpy_to_slim( void __iomem *dest, void *src, unsigned int bytes)
 {
-	__iowrite32_copy(dest, src, bytes);
+	/* convert bytes in argument list to word count for copy function */
+	__iowrite32_copy(dest, src, bytes / sizeof(uint32_t));
 }
 
 static inline void
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index 60f95347babf..26924b7a6cde 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -26,7 +26,7 @@ void lpfc_sli_read_link_ste(struct lpfc_hba *);
 void lpfc_dump_mem(struct lpfc_hba *, LPFC_MBOXQ_t *, uint16_t, uint16_t);
 void lpfc_dump_wakeup_param(struct lpfc_hba *, LPFC_MBOXQ_t *);
 int lpfc_dump_static_vport(struct lpfc_hba *, LPFC_MBOXQ_t *, uint16_t);
-int lpfc_dump_fcoe_param(struct lpfc_hba *, struct lpfcMboxq *);
+int lpfc_sli4_dump_cfg_rg23(struct lpfc_hba *, struct lpfcMboxq *);
 void lpfc_read_nv(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_config_async(struct lpfc_hba *, LPFC_MBOXQ_t *, uint32_t);
 
@@ -78,6 +78,7 @@ void lpfc_mbx_cmpl_fabric_reg_login(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_mbx_cmpl_ns_reg_login(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_mbx_cmpl_fdmi_reg_login(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_mbx_cmpl_reg_vfi(struct lpfc_hba *, LPFC_MBOXQ_t *);
+void lpfc_unregister_vfi_cmpl(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_enqueue_node(struct lpfc_vport *, struct lpfc_nodelist *);
 void lpfc_dequeue_node(struct lpfc_vport *, struct lpfc_nodelist *);
 struct lpfc_nodelist *lpfc_enable_node(struct lpfc_vport *,
@@ -106,7 +107,7 @@ void lpfc_cleanup(struct lpfc_vport *);
 void lpfc_disc_timeout(unsigned long);
 
 struct lpfc_nodelist *__lpfc_findnode_rpi(struct lpfc_vport *, uint16_t);
-
+struct lpfc_nodelist *lpfc_findnode_rpi(struct lpfc_vport *, uint16_t);
 void lpfc_worker_wake_up(struct lpfc_hba *);
 int lpfc_workq_post_event(struct lpfc_hba *, void *, void *, uint32_t);
 int lpfc_do_work(void *);
@@ -453,3 +454,11 @@ int lpfc_sli_probe_sriov_nr_virtfn(struct lpfc_hba *, int);
 uint16_t lpfc_sli_sriov_nr_virtfn_get(struct lpfc_hba *);
 int lpfc_sli4_queue_create(struct lpfc_hba *);
 void lpfc_sli4_queue_destroy(struct lpfc_hba *);
+void lpfc_sli4_abts_err_handler(struct lpfc_hba *, struct lpfc_nodelist *,
+				struct sli4_wcqe_xri_aborted *);
+int lpfc_hba_init_link_fc_topology(struct lpfc_hba *, uint32_t, uint32_t);
+int lpfc_issue_reg_vfi(struct lpfc_vport *);
+int lpfc_issue_unreg_vfi(struct lpfc_vport *);
+int lpfc_selective_reset(struct lpfc_hba *);
+int lpfc_sli4_read_config(struct lpfc_hba *phba);
+int lpfc_scsi_buf_update(struct lpfc_hba *phba);
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.c b/drivers/scsi/lpfc/lpfc_debugfs.c
index 28382596fb9a..3587a3fe8fcb 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.c
+++ b/drivers/scsi/lpfc/lpfc_debugfs.c
@@ -1997,7 +1997,8 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 	/* Get slow-path event queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Slow-path EQ information:\n");
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.sp_eq) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"\tEQID[%02d], "
 			"QE-COUNT[%04d], QE-SIZE[%04d], "
 			"HOST-INDEX[%04d], PORT-INDEX[%04d]\n\n",
@@ -2006,12 +2007,17 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 			phba->sli4_hba.sp_eq->entry_size,
 			phba->sli4_hba.sp_eq->host_index,
 			phba->sli4_hba.sp_eq->hba_index);
+	}
 
 	/* Get fast-path event queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Fast-path EQ information:\n");
-	for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_eq_count; fcp_qidx++) {
-		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.fp_eq) {
+		for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_eq_count;
+		     fcp_qidx++) {
+			if (phba->sli4_hba.fp_eq[fcp_qidx]) {
+				len += snprintf(pbuffer+len,
+					LPFC_QUE_INFO_GET_BUF_SIZE-len,
 				"\tEQID[%02d], "
 				"QE-COUNT[%04d], QE-SIZE[%04d], "
 				"HOST-INDEX[%04d], PORT-INDEX[%04d]\n",
@@ -2020,16 +2026,19 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 				phba->sli4_hba.fp_eq[fcp_qidx]->entry_size,
 				phba->sli4_hba.fp_eq[fcp_qidx]->host_index,
 				phba->sli4_hba.fp_eq[fcp_qidx]->hba_index);
+			}
+		}
 	}
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len, "\n");
 
 	/* Get mailbox complete queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Slow-path MBX CQ information:\n");
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.mbx_cq) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Associated EQID[%02d]:\n",
 			phba->sli4_hba.mbx_cq->assoc_qid);
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"\tCQID[%02d], "
 			"QE-COUNT[%04d], QE-SIZE[%04d], "
 			"HOST-INDEX[%04d], PORT-INDEX[%04d]\n\n",
@@ -2038,14 +2047,16 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 			phba->sli4_hba.mbx_cq->entry_size,
 			phba->sli4_hba.mbx_cq->host_index,
 			phba->sli4_hba.mbx_cq->hba_index);
+	}
 
 	/* Get slow-path complete queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Slow-path ELS CQ information:\n");
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.els_cq) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Associated EQID[%02d]:\n",
 			phba->sli4_hba.els_cq->assoc_qid);
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"\tCQID [%02d], "
 			"QE-COUNT[%04d], QE-SIZE[%04d], "
 			"HOST-INDEX[%04d], PORT-INDEX[%04d]\n\n",
@@ -2054,16 +2065,21 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 			phba->sli4_hba.els_cq->entry_size,
 			phba->sli4_hba.els_cq->host_index,
 			phba->sli4_hba.els_cq->hba_index);
+	}
 
 	/* Get fast-path complete queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Fast-path FCP CQ information:\n");
 	fcp_qidx = 0;
-	do {
-		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.fcp_cq) {
+		do {
+			if (phba->sli4_hba.fcp_cq[fcp_qidx]) {
+				len += snprintf(pbuffer+len,
+					LPFC_QUE_INFO_GET_BUF_SIZE-len,
 				"Associated EQID[%02d]:\n",
 				phba->sli4_hba.fcp_cq[fcp_qidx]->assoc_qid);
-		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+				len += snprintf(pbuffer+len,
+					LPFC_QUE_INFO_GET_BUF_SIZE-len,
 				"\tCQID[%02d], "
 				"QE-COUNT[%04d], QE-SIZE[%04d], "
 				"HOST-INDEX[%04d], PORT-INDEX[%04d]\n",
@@ -2072,16 +2088,20 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 				phba->sli4_hba.fcp_cq[fcp_qidx]->entry_size,
 				phba->sli4_hba.fcp_cq[fcp_qidx]->host_index,
 				phba->sli4_hba.fcp_cq[fcp_qidx]->hba_index);
-	} while (++fcp_qidx < phba->cfg_fcp_eq_count);
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len, "\n");
+			}
+		} while (++fcp_qidx < phba->cfg_fcp_eq_count);
+		len += snprintf(pbuffer+len,
+				LPFC_QUE_INFO_GET_BUF_SIZE-len, "\n");
+	}
 
 	/* Get mailbox queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Slow-path MBX MQ information:\n");
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.mbx_wq) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Associated CQID[%02d]:\n",
 			phba->sli4_hba.mbx_wq->assoc_qid);
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"\tWQID[%02d], "
 			"QE-COUNT[%04d], QE-SIZE[%04d], "
 			"HOST-INDEX[%04d], PORT-INDEX[%04d]\n\n",
@@ -2090,14 +2110,16 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 			phba->sli4_hba.mbx_wq->entry_size,
 			phba->sli4_hba.mbx_wq->host_index,
 			phba->sli4_hba.mbx_wq->hba_index);
+	}
 
 	/* Get slow-path work queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Slow-path ELS WQ information:\n");
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.els_wq) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Associated CQID[%02d]:\n",
 			phba->sli4_hba.els_wq->assoc_qid);
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"\tWQID[%02d], "
 			"QE-COUNT[%04d], QE-SIZE[%04d], "
 			"HOST-INDEX[%04d], PORT-INDEX[%04d]\n\n",
@@ -2106,15 +2128,22 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 			phba->sli4_hba.els_wq->entry_size,
 			phba->sli4_hba.els_wq->host_index,
 			phba->sli4_hba.els_wq->hba_index);
+	}
 
 	/* Get fast-path work queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Fast-path FCP WQ information:\n");
-	for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_wq_count; fcp_qidx++) {
-		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.fcp_wq) {
+		for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_wq_count;
+		     fcp_qidx++) {
+			if (!phba->sli4_hba.fcp_wq[fcp_qidx])
+				continue;
+			len += snprintf(pbuffer+len,
+					LPFC_QUE_INFO_GET_BUF_SIZE-len,
 				"Associated CQID[%02d]:\n",
 				phba->sli4_hba.fcp_wq[fcp_qidx]->assoc_qid);
-		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+			len += snprintf(pbuffer+len,
+					LPFC_QUE_INFO_GET_BUF_SIZE-len,
 				"\tWQID[%02d], "
 				"QE-COUNT[%04d], WQE-SIZE[%04d], "
 				"HOST-INDEX[%04d], PORT-INDEX[%04d]\n",
@@ -2123,16 +2152,19 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 				phba->sli4_hba.fcp_wq[fcp_qidx]->entry_size,
 				phba->sli4_hba.fcp_wq[fcp_qidx]->host_index,
 				phba->sli4_hba.fcp_wq[fcp_qidx]->hba_index);
+		}
+		len += snprintf(pbuffer+len,
+				LPFC_QUE_INFO_GET_BUF_SIZE-len, "\n");
 	}
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len, "\n");
 
 	/* Get receive queue information */
 	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Slow-path RQ information:\n");
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+	if (phba->sli4_hba.hdr_rq && phba->sli4_hba.dat_rq) {
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"Associated CQID[%02d]:\n",
 			phba->sli4_hba.hdr_rq->assoc_qid);
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"\tHQID[%02d], "
 			"QE-COUNT[%04d], QE-SIZE[%04d], "
 			"HOST-INDEX[%04d], PORT-INDEX[%04d]\n",
@@ -2141,7 +2173,7 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 			phba->sli4_hba.hdr_rq->entry_size,
 			phba->sli4_hba.hdr_rq->host_index,
 			phba->sli4_hba.hdr_rq->hba_index);
-	len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
+		len += snprintf(pbuffer+len, LPFC_QUE_INFO_GET_BUF_SIZE-len,
 			"\tDQID[%02d], "
 			"QE-COUNT[%04d], QE-SIZE[%04d], "
 			"HOST-INDEX[%04d], PORT-INDEX[%04d]\n",
@@ -2150,7 +2182,7 @@ lpfc_idiag_queinfo_read(struct file *file, char __user *buf, size_t nbytes,
 			phba->sli4_hba.dat_rq->entry_size,
 			phba->sli4_hba.dat_rq->host_index,
 			phba->sli4_hba.dat_rq->hba_index);
-
+	}
 	return simple_read_from_buffer(buf, nbytes, ppos, pbuffer, len);
 }
 
@@ -2360,7 +2392,8 @@ lpfc_idiag_queacc_write(struct file *file, const char __user *buf,
 	switch (quetp) {
 	case LPFC_IDIAG_EQ:
 		/* Slow-path event queue */
-		if (phba->sli4_hba.sp_eq->queue_id == queid) {
+		if (phba->sli4_hba.sp_eq &&
+		    phba->sli4_hba.sp_eq->queue_id == queid) {
 			/* Sanity check */
 			rc = lpfc_idiag_que_param_check(
 					phba->sli4_hba.sp_eq, index, count);
@@ -2370,23 +2403,29 @@ lpfc_idiag_queacc_write(struct file *file, const char __user *buf,
 			goto pass_check;
 		}
 		/* Fast-path event queue */
-		for (qidx = 0; qidx < phba->cfg_fcp_eq_count; qidx++) {
-			if (phba->sli4_hba.fp_eq[qidx]->queue_id == queid) {
-				/* Sanity check */
-				rc = lpfc_idiag_que_param_check(
+		if (phba->sli4_hba.fp_eq) {
+			for (qidx = 0; qidx < phba->cfg_fcp_eq_count; qidx++) {
+				if (phba->sli4_hba.fp_eq[qidx] &&
+				    phba->sli4_hba.fp_eq[qidx]->queue_id ==
+				    queid) {
+					/* Sanity check */
+					rc = lpfc_idiag_que_param_check(
 						phba->sli4_hba.fp_eq[qidx],
 						index, count);
-				if (rc)
-					goto error_out;
-				idiag.ptr_private = phba->sli4_hba.fp_eq[qidx];
-				goto pass_check;
+					if (rc)
+						goto error_out;
+					idiag.ptr_private =
+						phba->sli4_hba.fp_eq[qidx];
+					goto pass_check;
+				}
 			}
 		}
 		goto error_out;
 		break;
 	case LPFC_IDIAG_CQ:
 		/* MBX complete queue */
-		if (phba->sli4_hba.mbx_cq->queue_id == queid) {
+		if (phba->sli4_hba.mbx_cq &&
+		    phba->sli4_hba.mbx_cq->queue_id == queid) {
 			/* Sanity check */
 			rc = lpfc_idiag_que_param_check(
 					phba->sli4_hba.mbx_cq, index, count);
@@ -2396,7 +2435,8 @@ lpfc_idiag_queacc_write(struct file *file, const char __user *buf,
 			goto pass_check;
 		}
 		/* ELS complete queue */
-		if (phba->sli4_hba.els_cq->queue_id == queid) {
+		if (phba->sli4_hba.els_cq &&
+		    phba->sli4_hba.els_cq->queue_id == queid) {
 			/* Sanity check */
 			rc = lpfc_idiag_que_param_check(
 					phba->sli4_hba.els_cq, index, count);
@@ -2406,25 +2446,30 @@ lpfc_idiag_queacc_write(struct file *file, const char __user *buf,
 			goto pass_check;
 		}
 		/* FCP complete queue */
-		qidx = 0;
-		do {
-			if (phba->sli4_hba.fcp_cq[qidx]->queue_id == queid) {
-				/* Sanity check */
-				rc = lpfc_idiag_que_param_check(
+		if (phba->sli4_hba.fcp_cq) {
+			qidx = 0;
+			do {
+				if (phba->sli4_hba.fcp_cq[qidx] &&
+				    phba->sli4_hba.fcp_cq[qidx]->queue_id ==
+				    queid) {
+					/* Sanity check */
+					rc = lpfc_idiag_que_param_check(
 						phba->sli4_hba.fcp_cq[qidx],
 						index, count);
-				if (rc)
-					goto error_out;
-				idiag.ptr_private =
+					if (rc)
+						goto error_out;
+					idiag.ptr_private =
 						phba->sli4_hba.fcp_cq[qidx];
-				goto pass_check;
-			}
-		} while (++qidx < phba->cfg_fcp_eq_count);
+					goto pass_check;
+				}
+			} while (++qidx < phba->cfg_fcp_eq_count);
+		}
 		goto error_out;
 		break;
 	case LPFC_IDIAG_MQ:
 		/* MBX work queue */
-		if (phba->sli4_hba.mbx_wq->queue_id == queid) {
+		if (phba->sli4_hba.mbx_wq &&
+		    phba->sli4_hba.mbx_wq->queue_id == queid) {
 			/* Sanity check */
 			rc = lpfc_idiag_que_param_check(
 					phba->sli4_hba.mbx_wq, index, count);
@@ -2433,10 +2478,12 @@ lpfc_idiag_queacc_write(struct file *file, const char __user *buf,
 			idiag.ptr_private = phba->sli4_hba.mbx_wq;
 			goto pass_check;
 		}
+		goto error_out;
 		break;
 	case LPFC_IDIAG_WQ:
 		/* ELS work queue */
-		if (phba->sli4_hba.els_wq->queue_id == queid) {
+		if (phba->sli4_hba.els_wq &&
+		    phba->sli4_hba.els_wq->queue_id == queid) {
 			/* Sanity check */
 			rc = lpfc_idiag_que_param_check(
 					phba->sli4_hba.els_wq, index, count);
@@ -2446,24 +2493,30 @@ lpfc_idiag_queacc_write(struct file *file, const char __user *buf,
 			goto pass_check;
 		}
 		/* FCP work queue */
-		for (qidx = 0; qidx < phba->cfg_fcp_wq_count; qidx++) {
-			if (phba->sli4_hba.fcp_wq[qidx]->queue_id == queid) {
-				/* Sanity check */
-				rc = lpfc_idiag_que_param_check(
+		if (phba->sli4_hba.fcp_wq) {
+			for (qidx = 0; qidx < phba->cfg_fcp_wq_count; qidx++) {
+				if (!phba->sli4_hba.fcp_wq[qidx])
+					continue;
+				if (phba->sli4_hba.fcp_wq[qidx]->queue_id ==
+				    queid) {
+					/* Sanity check */
+					rc = lpfc_idiag_que_param_check(
 						phba->sli4_hba.fcp_wq[qidx],
 						index, count);
-				if (rc)
-					goto error_out;
-				idiag.ptr_private =
-					phba->sli4_hba.fcp_wq[qidx];
-				goto pass_check;
+					if (rc)
+						goto error_out;
+					idiag.ptr_private =
+						phba->sli4_hba.fcp_wq[qidx];
+					goto pass_check;
+				}
 			}
 		}
 		goto error_out;
 		break;
 	case LPFC_IDIAG_RQ:
 		/* HDR queue */
-		if (phba->sli4_hba.hdr_rq->queue_id == queid) {
+		if (phba->sli4_hba.hdr_rq &&
+		    phba->sli4_hba.hdr_rq->queue_id == queid) {
 			/* Sanity check */
 			rc = lpfc_idiag_que_param_check(
 					phba->sli4_hba.hdr_rq, index, count);
@@ -2473,7 +2526,8 @@ lpfc_idiag_queacc_write(struct file *file, const char __user *buf,
 			goto pass_check;
 		}
 		/* DAT queue */
-		if (phba->sli4_hba.dat_rq->queue_id == queid) {
+		if (phba->sli4_hba.dat_rq &&
+		    phba->sli4_hba.dat_rq->queue_id == queid) {
 			/* Sanity check */
 			rc = lpfc_idiag_que_param_check(
 					phba->sli4_hba.dat_rq, index, count);
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index 445826a4c981..7afc757338de 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -421,13 +421,13 @@ fail:
  * @vport: pointer to a host virtual N_Port data structure.
  *
  * This routine issues a REG_VFI mailbox for the vfi, vpi, fcfi triplet for
- * the @vport. This mailbox command is necessary for FCoE only.
+ * the @vport. This mailbox command is necessary for SLI4 port only.
  *
  * Return code
  *   0 - successfully issued REG_VFI for @vport
  *   A failure code otherwise.
  **/
-static int
+int
 lpfc_issue_reg_vfi(struct lpfc_vport *vport)
 {
 	struct lpfc_hba  *phba = vport->phba;
@@ -438,10 +438,14 @@ lpfc_issue_reg_vfi(struct lpfc_vport *vport)
 	int rc = 0;
 
 	sp = &phba->fc_fabparam;
-	ndlp = lpfc_findnode_did(vport, Fabric_DID);
-	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp)) {
-		rc = -ENODEV;
-		goto fail;
+	/* move forward in case of SLI4 FC port loopback test */
+	if ((phba->sli_rev == LPFC_SLI_REV4) &&
+	    !(phba->link_flag & LS_LOOPBACK_MODE)) {
+		ndlp = lpfc_findnode_did(vport, Fabric_DID);
+		if (!ndlp || !NLP_CHK_NODE_ACT(ndlp)) {
+			rc = -ENODEV;
+			goto fail;
+		}
 	}
 
 	dmabuf = kzalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
@@ -487,6 +491,54 @@ fail:
 }
 
 /**
+ * lpfc_issue_unreg_vfi - Unregister VFI for this vport's fabric login
+ * @vport: pointer to a host virtual N_Port data structure.
+ *
+ * This routine issues a UNREG_VFI mailbox with the vfi, vpi, fcfi triplet for
+ * the @vport. This mailbox command is necessary for SLI4 port only.
+ *
+ * Return code
+ *   0 - successfully issued REG_VFI for @vport
+ *   A failure code otherwise.
+ **/
+int
+lpfc_issue_unreg_vfi(struct lpfc_vport *vport)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct Scsi_Host *shost;
+	LPFC_MBOXQ_t *mboxq;
+	int rc;
+
+	mboxq = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!mboxq) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_DISCOVERY|LOG_MBOX,
+				"2556 UNREG_VFI mbox allocation failed"
+				"HBA state x%x\n", phba->pport->port_state);
+		return -ENOMEM;
+	}
+
+	lpfc_unreg_vfi(mboxq, vport);
+	mboxq->vport = vport;
+	mboxq->mbox_cmpl = lpfc_unregister_vfi_cmpl;
+
+	rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_NOWAIT);
+	if (rc == MBX_NOT_FINISHED) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_DISCOVERY|LOG_MBOX,
+				"2557 UNREG_VFI issue mbox failed rc x%x "
+				"HBA state x%x\n",
+				rc, phba->pport->port_state);
+		mempool_free(mboxq, phba->mbox_mem_pool);
+		return -EIO;
+	}
+
+	shost = lpfc_shost_from_vport(vport);
+	spin_lock_irq(shost->host_lock);
+	vport->fc_flag &= ~FC_VFI_REGISTERED;
+	spin_unlock_irq(shost->host_lock);
+	return 0;
+}
+
+/**
  * lpfc_check_clean_addr_bit - Check whether assigned FCID is clean.
  * @vport: pointer to a host virtual N_Port data structure.
  * @sp: pointer to service parameter data structure.
@@ -615,7 +667,9 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 					 "1816 FLOGI NPIV supported, "
 					 "response data 0x%x\n",
 					 sp->cmn.response_multiple_NPort);
+			spin_lock_irq(&phba->hbalock);
 			phba->link_flag |= LS_NPIV_FAB_SUPPORTED;
+			spin_unlock_irq(&phba->hbalock);
 		} else {
 			/* Because we asked f/w for NPIV it still expects us
 			to call reg_vnpid atleast for the physcial host */
@@ -623,7 +677,9 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 					 LOG_ELS | LOG_VPORT,
 					 "1817 Fabric does not support NPIV "
 					 "- configuring single port mode.\n");
+			spin_lock_irq(&phba->hbalock);
 			phba->link_flag &= ~LS_NPIV_FAB_SUPPORTED;
+			spin_unlock_irq(&phba->hbalock);
 		}
 	}
 
@@ -686,11 +742,16 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 			lpfc_do_scr_ns_plogi(phba, vport);
 		} else if (vport->fc_flag & FC_VFI_REGISTERED)
 			lpfc_issue_init_vpi(vport);
-		else
+		else {
+			lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
+					"3135 Need register VFI: (x%x/%x)\n",
+					vport->fc_prevDID, vport->fc_myDID);
 			lpfc_issue_reg_vfi(vport);
+		}
 	}
 	return 0;
 }
+
 /**
  * lpfc_cmpl_els_flogi_nport - Completion function for flogi to an N_Port
  * @vport: pointer to a host virtual N_Port data structure.
@@ -907,17 +968,16 @@ lpfc_cmpl_els_flogi(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 		 * LPFC_MAX_DISC_THREADS (32). Scanning in the case of no
 		 * alpa map would take too long otherwise.
 		 */
-		if (phba->alpa_map[0] == 0) {
+		if (phba->alpa_map[0] == 0)
 			vport->cfg_discovery_threads = LPFC_MAX_DISC_THREADS;
-			if ((phba->sli_rev == LPFC_SLI_REV4) &&
-			    (!(vport->fc_flag & FC_VFI_REGISTERED) ||
-			     (vport->fc_prevDID != vport->fc_myDID))) {
-				if (vport->fc_flag & FC_VFI_REGISTERED)
-					lpfc_sli4_unreg_all_rpis(vport);
-				lpfc_issue_reg_vfi(vport);
-				lpfc_nlp_put(ndlp);
-				goto out;
-			}
+		if ((phba->sli_rev == LPFC_SLI_REV4) &&
+		    (!(vport->fc_flag & FC_VFI_REGISTERED) ||
+		     (vport->fc_prevDID != vport->fc_myDID))) {
+			if (vport->fc_flag & FC_VFI_REGISTERED)
+				lpfc_sli4_unreg_all_rpis(vport);
+			lpfc_issue_reg_vfi(vport);
+			lpfc_nlp_put(ndlp);
+			goto out;
 		}
 		goto flogifail;
 	}
@@ -1075,6 +1135,7 @@ lpfc_issue_els_flogi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	/* Setup CSPs accordingly for Fabric */
 	sp->cmn.e_d_tov = 0;
 	sp->cmn.w2.r_a_tov = 0;
+	sp->cmn.virtual_fabric_support = 0;
 	sp->cls1.classValid = 0;
 	sp->cls2.seqDelivery = 1;
 	sp->cls3.seqDelivery = 1;
@@ -1163,8 +1224,7 @@ lpfc_els_abort_flogi(struct lpfc_hba *phba)
 	spin_lock_irq(&phba->hbalock);
 	list_for_each_entry_safe(iocb, next_iocb, &pring->txcmplq, list) {
 		icmd = &iocb->iocb;
-		if (icmd->ulpCommand == CMD_ELS_REQUEST64_CR &&
-		    icmd->un.elsreq64.bdl.ulpIoTag32) {
+		if (icmd->ulpCommand == CMD_ELS_REQUEST64_CR) {
 			ndlp = (struct lpfc_nodelist *)(iocb->context1);
 			if (ndlp && NLP_CHK_NODE_ACT(ndlp) &&
 			    (ndlp->nlp_DID == Fabric_DID))
@@ -3066,17 +3126,22 @@ lpfc_els_retry(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 	if (did == FDMI_DID)
 		retry = 1;
 
-	if (((cmd == ELS_CMD_FLOGI) || (cmd == ELS_CMD_FDISC)) &&
+	if ((cmd == ELS_CMD_FLOGI) &&
 	    (phba->fc_topology != LPFC_TOPOLOGY_LOOP) &&
 	    !lpfc_error_lost_link(irsp)) {
 		/* FLOGI retry policy */
 		retry = 1;
-		/* retry forever */
+		/* retry FLOGI forever */
 		maxretry = 0;
 		if (cmdiocb->retry >= 100)
 			delay = 5000;
 		else if (cmdiocb->retry >= 32)
 			delay = 1000;
+	} else if ((cmd == ELS_CMD_FDISC) && !lpfc_error_lost_link(irsp)) {
+		/* retry FDISCs every second up to devloss */
+		retry = 1;
+		maxretry = vport->cfg_devloss_tmo;
+		delay = 1000;
 	}
 
 	cmdiocb->retry++;
@@ -3389,11 +3454,17 @@ lpfc_cmpl_els_logo_acc(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 
 	/*
 	 * The driver received a LOGO from the rport and has ACK'd it.
-	 * At this point, the driver is done so release the IOCB and
-	 * remove the ndlp reference.
+	 * At this point, the driver is done so release the IOCB
 	 */
 	lpfc_els_free_iocb(phba, cmdiocb);
-	lpfc_nlp_put(ndlp);
+
+	/*
+	 * Remove the ndlp reference if it's a fabric node that has
+	 * sent us an unsolicted LOGO.
+	 */
+	if (ndlp->nlp_type & NLP_FABRIC)
+		lpfc_nlp_put(ndlp);
+
 	return;
 }
 
@@ -4867,23 +4938,31 @@ lpfc_els_rcv_flogi(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
 			    sizeof(struct lpfc_name));
 
 		if (!rc) {
-			mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-			if (!mbox)
+			if (phba->sli_rev < LPFC_SLI_REV4) {
+				mbox = mempool_alloc(phba->mbox_mem_pool,
+						     GFP_KERNEL);
+				if (!mbox)
+					return 1;
+				lpfc_linkdown(phba);
+				lpfc_init_link(phba, mbox,
+					       phba->cfg_topology,
+					       phba->cfg_link_speed);
+				mbox->u.mb.un.varInitLnk.lipsr_AL_PA = 0;
+				mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
+				mbox->vport = vport;
+				rc = lpfc_sli_issue_mbox(phba, mbox,
+							 MBX_NOWAIT);
+				lpfc_set_loopback_flag(phba);
+				if (rc == MBX_NOT_FINISHED)
+					mempool_free(mbox, phba->mbox_mem_pool);
 				return 1;
-
-			lpfc_linkdown(phba);
-			lpfc_init_link(phba, mbox,
-				       phba->cfg_topology,
-				       phba->cfg_link_speed);
-			mbox->u.mb.un.varInitLnk.lipsr_AL_PA = 0;
-			mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
-			mbox->vport = vport;
-			rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
-			lpfc_set_loopback_flag(phba);
-			if (rc == MBX_NOT_FINISHED) {
-				mempool_free(mbox, phba->mbox_mem_pool);
+			} else {
+				/* abort the flogi coming back to ourselves
+				 * due to external loopback on the port.
+				 */
+				lpfc_els_abort_flogi(phba);
+				return 0;
 			}
-			return 1;
 		} else if (rc > 0) {	/* greater than */
 			spin_lock_irq(shost->host_lock);
 			vport->fc_flag |= FC_PT2PT_PLOGI;
@@ -5838,8 +5917,12 @@ lpfc_els_rcv_fan(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
 			vport->fc_myDID = vport->fc_prevDID;
 			if (phba->sli_rev < LPFC_SLI_REV4)
 				lpfc_issue_fabric_reglogin(vport);
-			else
+			else {
+				lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
+					"3138 Need register VFI: (x%x/%x)\n",
+					vport->fc_prevDID, vport->fc_myDID);
 				lpfc_issue_reg_vfi(vport);
+			}
 		}
 	}
 	return 0;
@@ -6596,56 +6679,6 @@ dropit:
 }
 
 /**
- * lpfc_find_vport_by_vpid - Find a vport on a HBA through vport identifier
- * @phba: pointer to lpfc hba data structure.
- * @vpi: host virtual N_Port identifier.
- *
- * This routine finds a vport on a HBA (referred by @phba) through a
- * @vpi. The function walks the HBA's vport list and returns the address
- * of the vport with the matching @vpi.
- *
- * Return code
- *    NULL - No vport with the matching @vpi found
- *    Otherwise - Address to the vport with the matching @vpi.
- **/
-struct lpfc_vport *
-lpfc_find_vport_by_vpid(struct lpfc_hba *phba, uint16_t vpi)
-{
-	struct lpfc_vport *vport;
-	unsigned long flags;
-	int i = 0;
-
-	/* The physical ports are always vpi 0 - translate is unnecessary. */
-	if (vpi > 0) {
-		/*
-		 * Translate the physical vpi to the logical vpi.  The
-		 * vport stores the logical vpi.
-		 */
-		for (i = 0; i < phba->max_vpi; i++) {
-			if (vpi == phba->vpi_ids[i])
-				break;
-		}
-
-		if (i >= phba->max_vpi) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_ELS,
-					 "2936 Could not find Vport mapped "
-					 "to vpi %d\n", vpi);
-			return NULL;
-		}
-	}
-
-	spin_lock_irqsave(&phba->hbalock, flags);
-	list_for_each_entry(vport, &phba->port_list, listentry) {
-		if (vport->vpi == i) {
-			spin_unlock_irqrestore(&phba->hbalock, flags);
-			return vport;
-		}
-	}
-	spin_unlock_irqrestore(&phba->hbalock, flags);
-	return NULL;
-}
-
-/**
  * lpfc_els_unsol_event - Process an unsolicited event from an els sli ring
  * @phba: pointer to lpfc hba data structure.
  * @pring: pointer to a SLI ring.
@@ -7281,6 +7314,7 @@ lpfc_issue_els_fdisc(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	/* Setup CSPs accordingly for Fabric */
 	sp->cmn.e_d_tov = 0;
 	sp->cmn.w2.r_a_tov = 0;
+	sp->cmn.virtual_fabric_support = 0;
 	sp->cls1.classValid = 0;
 	sp->cls2.seqDelivery = 1;
 	sp->cls3.seqDelivery = 1;
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index 091f68e5cb70..678a4b11059c 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -1074,6 +1074,12 @@ lpfc_mbx_cmpl_local_config_link(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 
 	mempool_free(pmb, phba->mbox_mem_pool);
 
+	/* don't perform discovery for SLI4 loopback diagnostic test */
+	if ((phba->sli_rev == LPFC_SLI_REV4) &&
+	    !(phba->hba_flag & HBA_FCOE_MODE) &&
+	    (phba->link_flag & LS_LOOPBACK_MODE))
+		return;
+
 	if (phba->fc_topology == LPFC_TOPOLOGY_LOOP &&
 	    vport->fc_flag & FC_PUBLIC_LOOP &&
 	    !(vport->fc_flag & FC_LBIT)) {
@@ -2646,9 +2652,14 @@ lpfc_init_vfi_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 {
 	struct lpfc_vport *vport = mboxq->vport;
 
-	/* VFI not supported on interface type 0, just do the flogi */
-	if (mboxq->u.mb.mbxStatus && (bf_get(lpfc_sli_intf_if_type,
-	    &phba->sli4_hba.sli_intf) != LPFC_SLI_INTF_IF_TYPE_0)) {
+	/*
+	 * VFI not supported on interface type 0, just do the flogi
+	 * Also continue if the VFI is in use - just use the same one.
+	 */
+	if (mboxq->u.mb.mbxStatus &&
+	    (bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf) !=
+			LPFC_SLI_INTF_IF_TYPE_0) &&
+	    mboxq->u.mb.mbxStatus != MBX_VFI_IN_USE) {
 		lpfc_printf_vlog(vport, KERN_ERR,
 				LOG_MBOX,
 				"2891 Init VFI mailbox failed 0x%x\n",
@@ -2842,10 +2853,10 @@ lpfc_mbx_cmpl_reg_vfi(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 			lpfc_disc_list_loopmap(vport);
 			/* Start discovery */
 			lpfc_disc_start(vport);
-			goto fail_free_mem;
+			goto out_free_mem;
 		}
 		lpfc_vport_set_state(vport, FC_VPORT_FAILED);
-		goto fail_free_mem;
+		goto out_free_mem;
 	}
 	/* The VPI is implicitly registered when the VFI is registered */
 	spin_lock_irq(shost->host_lock);
@@ -2855,10 +2866,16 @@ lpfc_mbx_cmpl_reg_vfi(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 	vport->fc_flag &= ~FC_VPORT_NEEDS_INIT_VPI;
 	spin_unlock_irq(shost->host_lock);
 
+	/* In case SLI4 FC loopback test, we are ready */
+	if ((phba->sli_rev == LPFC_SLI_REV4) &&
+	    (phba->link_flag & LS_LOOPBACK_MODE)) {
+		phba->link_state = LPFC_HBA_READY;
+		goto out_free_mem;
+	}
+
 	if (vport->port_state == LPFC_FABRIC_CFG_LINK) {
 		/* For private loop just start discovery and we are done. */
 		if ((phba->fc_topology == LPFC_TOPOLOGY_LOOP) &&
-		    (phba->alpa_map[0] == 0) &&
 		    !(vport->fc_flag & FC_PUBLIC_LOOP)) {
 			/* Use loop map to make discovery list */
 			lpfc_disc_list_loopmap(vport);
@@ -2870,7 +2887,7 @@ lpfc_mbx_cmpl_reg_vfi(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 		}
 	}
 
-fail_free_mem:
+out_free_mem:
 	mempool_free(mboxq, phba->mbox_mem_pool);
 	lpfc_mbuf_free(phba, dmabuf->virt, dmabuf->phys);
 	kfree(dmabuf);
@@ -2923,6 +2940,7 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 {
 	struct lpfc_vport *vport = phba->pport;
 	LPFC_MBOXQ_t *sparam_mbox, *cfglink_mbox = NULL;
+	struct Scsi_Host *shost;
 	int i;
 	struct lpfc_dmabuf *mp;
 	int rc;
@@ -2946,6 +2964,7 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 	phba->fc_topology = bf_get(lpfc_mbx_read_top_topology, la);
 	phba->link_flag &= ~LS_NPIV_FAB_SUPPORTED;
 
+	shost = lpfc_shost_from_vport(vport);
 	if (phba->fc_topology == LPFC_TOPOLOGY_LOOP) {
 		phba->sli3_options &= ~LPFC_SLI3_NPIV_ENABLED;
 
@@ -2957,8 +2976,11 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 				"1309 Link Up Event npiv not supported in loop "
 				"topology\n");
 				/* Get Loop Map information */
-		if (bf_get(lpfc_mbx_read_top_il, la))
+		if (bf_get(lpfc_mbx_read_top_il, la)) {
+			spin_lock_irq(shost->host_lock);
 			vport->fc_flag |= FC_LBIT;
+			spin_unlock_irq(shost->host_lock);
+		}
 
 		vport->fc_myDID = bf_get(lpfc_mbx_read_top_alpa_granted, la);
 		i = la->lilpBde64.tus.f.bdeSize;
@@ -3003,11 +3025,13 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 	} else {
 		if (!(phba->sli3_options & LPFC_SLI3_NPIV_ENABLED)) {
 			if (phba->max_vpi && phba->cfg_enable_npiv &&
-			   (phba->sli_rev == 3))
+			   (phba->sli_rev >= LPFC_SLI_REV3))
 				phba->sli3_options |= LPFC_SLI3_NPIV_ENABLED;
 		}
 		vport->fc_myDID = phba->fc_pref_DID;
+		spin_lock_irq(shost->host_lock);
 		vport->fc_flag |= FC_LBIT;
+		spin_unlock_irq(shost->host_lock);
 	}
 	spin_unlock_irq(&phba->hbalock);
 
@@ -3224,15 +3248,14 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	} else if (bf_get(lpfc_mbx_read_top_att_type, la) ==
 		   LPFC_ATT_LINK_DOWN) {
 		phba->fc_stat.LinkDown++;
-		if (phba->link_flag & LS_LOOPBACK_MODE) {
+		if (phba->link_flag & LS_LOOPBACK_MODE)
 			lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
 				"1308 Link Down Event in loop back mode "
 				"x%x received "
 				"Data: x%x x%x x%x\n",
 				la->eventTag, phba->fc_eventTag,
 				phba->pport->port_state, vport->fc_flag);
-		}
-		else {
+		else
 			lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
 				"1305 Link Down Event x%x received "
 				"Data: x%x x%x x%x x%x x%x\n",
@@ -3240,7 +3263,6 @@ lpfc_mbx_cmpl_read_topology(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 				phba->pport->port_state, vport->fc_flag,
 				bf_get(lpfc_mbx_read_top_mm, la),
 				bf_get(lpfc_mbx_read_top_fa, la));
-		}
 		lpfc_mbx_issue_link_down(phba);
 	}
 	if ((bf_get(lpfc_mbx_read_top_mm, la)) &&
@@ -3594,6 +3616,7 @@ lpfc_mbx_cmpl_fabric_reg_login(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	MAILBOX_t *mb = &pmb->u.mb;
 	struct lpfc_dmabuf *mp = (struct lpfc_dmabuf *) (pmb->context1);
 	struct lpfc_nodelist *ndlp;
+	struct Scsi_Host *shost;
 
 	ndlp = (struct lpfc_nodelist *) pmb->context2;
 	pmb->context1 = NULL;
@@ -3639,8 +3662,12 @@ lpfc_mbx_cmpl_fabric_reg_login(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 		 * vport discovery */
 		if (!(vport->fc_flag & FC_LOGO_RCVD_DID_CHNG))
 			lpfc_start_fdiscs(phba);
-		else
+		else {
+			shost = lpfc_shost_from_vport(vport);
+			spin_lock_irq(shost->host_lock);
 			vport->fc_flag &= ~FC_LOGO_RCVD_DID_CHNG ;
+			spin_unlock_irq(shost->host_lock);
+		}
 		lpfc_do_scr_ns_plogi(phba, vport);
 	}
 
@@ -5353,6 +5380,73 @@ lpfc_findnode_wwpn(struct lpfc_vport *vport, struct lpfc_name *wwpn)
 	return ndlp;
 }
 
+/*
+ * This routine looks up the ndlp lists for the given RPI. If the rpi
+ * is found, the routine returns the node element list pointer else
+ * return NULL.
+ */
+struct lpfc_nodelist *
+lpfc_findnode_rpi(struct lpfc_vport *vport, uint16_t rpi)
+{
+	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
+	struct lpfc_nodelist *ndlp;
+
+	spin_lock_irq(shost->host_lock);
+	ndlp = __lpfc_findnode_rpi(vport, rpi);
+	spin_unlock_irq(shost->host_lock);
+	return ndlp;
+}
+
+/**
+ * lpfc_find_vport_by_vpid - Find a vport on a HBA through vport identifier
+ * @phba: pointer to lpfc hba data structure.
+ * @vpi: the physical host virtual N_Port identifier.
+ *
+ * This routine finds a vport on a HBA (referred by @phba) through a
+ * @vpi. The function walks the HBA's vport list and returns the address
+ * of the vport with the matching @vpi.
+ *
+ * Return code
+ *    NULL - No vport with the matching @vpi found
+ *    Otherwise - Address to the vport with the matching @vpi.
+ **/
+struct lpfc_vport *
+lpfc_find_vport_by_vpid(struct lpfc_hba *phba, uint16_t vpi)
+{
+	struct lpfc_vport *vport;
+	unsigned long flags;
+	int i = 0;
+
+	/* The physical ports are always vpi 0 - translate is unnecessary. */
+	if (vpi > 0) {
+		/*
+		 * Translate the physical vpi to the logical vpi.  The
+		 * vport stores the logical vpi.
+		 */
+		for (i = 0; i < phba->max_vpi; i++) {
+			if (vpi == phba->vpi_ids[i])
+				break;
+		}
+
+		if (i >= phba->max_vpi) {
+			lpfc_printf_log(phba, KERN_ERR, LOG_ELS,
+					 "2936 Could not find Vport mapped "
+					 "to vpi %d\n", vpi);
+			return NULL;
+		}
+	}
+
+	spin_lock_irqsave(&phba->hbalock, flags);
+	list_for_each_entry(vport, &phba->port_list, listentry) {
+		if (vport->vpi == i) {
+			spin_unlock_irqrestore(&phba->hbalock, flags);
+			return vport;
+		}
+	}
+	spin_unlock_irqrestore(&phba->hbalock, flags);
+	return NULL;
+}
+
 void
 lpfc_nlp_init(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	      uint32_t did)
@@ -5599,7 +5693,7 @@ out:
  *
  * This function frees memory associated with the mailbox command.
  */
-static void
+void
 lpfc_unregister_vfi_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 {
 	struct lpfc_vport *vport = mboxq->vport;
@@ -5651,7 +5745,6 @@ lpfc_unregister_fcfi_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 int
 lpfc_unregister_fcf_prep(struct lpfc_hba *phba)
 {
-	LPFC_MBOXQ_t *mbox;
 	struct lpfc_vport **vports;
 	struct lpfc_nodelist *ndlp;
 	struct Scsi_Host *shost;
@@ -5687,35 +5780,9 @@ lpfc_unregister_fcf_prep(struct lpfc_hba *phba)
 	/* Cleanup any outstanding ELS commands */
 	lpfc_els_flush_all_cmd(phba);
 
-	/* Unregister VFI */
-	mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-	if (!mbox) {
-		lpfc_printf_log(phba, KERN_ERR, LOG_DISCOVERY|LOG_MBOX,
-				"2556 UNREG_VFI mbox allocation failed"
-				"HBA state x%x\n", phba->pport->port_state);
-		return -ENOMEM;
-	}
-
-	lpfc_unreg_vfi(mbox, phba->pport);
-	mbox->vport = phba->pport;
-	mbox->mbox_cmpl = lpfc_unregister_vfi_cmpl;
-
-	rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
-	if (rc == MBX_NOT_FINISHED) {
-		lpfc_printf_log(phba, KERN_ERR, LOG_DISCOVERY|LOG_MBOX,
-				"2557 UNREG_VFI issue mbox failed rc x%x "
-				"HBA state x%x\n",
-				rc, phba->pport->port_state);
-		mempool_free(mbox, phba->mbox_mem_pool);
-		return -EIO;
-	}
-
-	shost = lpfc_shost_from_vport(phba->pport);
-	spin_lock_irq(shost->host_lock);
-	phba->pport->fc_flag &= ~FC_VFI_REGISTERED;
-	spin_unlock_irq(shost->host_lock);
-
-	return 0;
+	/* Unregister the physical port VFI */
+	rc = lpfc_issue_unreg_vfi(phba->pport);
+	return rc;
 }
 
 /**
diff --git a/drivers/scsi/lpfc/lpfc_hw.h b/drivers/scsi/lpfc/lpfc_hw.h
index 046edc4ab35f..7245bead3755 100644
--- a/drivers/scsi/lpfc/lpfc_hw.h
+++ b/drivers/scsi/lpfc/lpfc_hw.h
@@ -349,6 +349,12 @@ struct csp {
  * Word 1 Bit 31 in FLOGI response is clean address bit
  */
 #define clean_address_bit request_multiple_Nport /* Word 1, bit 31 */
+/*
+ * Word 1 Bit 30 in common service parameter is overloaded.
+ * Word 1 Bit 30 in FLOGI request is Virtual Fabrics
+ * Word 1 Bit 30 in PLOGI request is random offset
+ */
+#define virtual_fabric_support randomOffset /* Word 1, bit 30 */
 #ifdef __BIG_ENDIAN_BITFIELD
 	uint16_t request_multiple_Nport:1;	/* FC Word 1, bit 31 */
 	uint16_t randomOffset:1;	/* FC Word 1, bit 30 */
@@ -1852,8 +1858,8 @@ typedef struct {
 	uint8_t fabric_AL_PA;	/* If using a Fabric Assigned AL_PA */
 #endif
 
-#define FLAGS_LOCAL_LB               0x01 /* link_flags (=1) ENDEC loopback */
 #define FLAGS_TOPOLOGY_MODE_LOOP_PT  0x00 /* Attempt loop then pt-pt */
+#define FLAGS_LOCAL_LB               0x01 /* link_flags (=1) ENDEC loopback */
 #define FLAGS_TOPOLOGY_MODE_PT_PT    0x02 /* Attempt pt-pt only */
 #define FLAGS_TOPOLOGY_MODE_LOOP     0x04 /* Attempt loop only */
 #define FLAGS_TOPOLOGY_MODE_PT_LOOP  0x06 /* Attempt pt-pt then loop */
@@ -2819,7 +2825,8 @@ typedef struct {
 #ifdef __BIG_ENDIAN_BITFIELD
 	uint32_t rsvd1     : 19;  /* Reserved                             */
 	uint32_t cdss      :  1;  /* Configure Data Security SLI          */
-	uint32_t rsvd2     :  3;  /* Reserved                             */
+	uint32_t casabt    :  1;  /* Configure async abts status notice   */
+	uint32_t rsvd2     :  2;  /* Reserved                             */
 	uint32_t cbg       :  1;  /* Configure BlockGuard                 */
 	uint32_t cmv       :  1;  /* Configure Max VPIs                   */
 	uint32_t ccrp      :  1;  /* Config Command Ring Polling          */
@@ -2839,14 +2846,16 @@ typedef struct {
 	uint32_t ccrp      :  1;  /* Config Command Ring Polling          */
 	uint32_t cmv	   :  1;  /* Configure Max VPIs                   */
 	uint32_t cbg       :  1;  /* Configure BlockGuard                 */
-	uint32_t rsvd2     :  3;  /* Reserved                             */
+	uint32_t rsvd2     :  2;  /* Reserved                             */
+	uint32_t casabt    :  1;  /* Configure async abts status notice   */
 	uint32_t cdss      :  1;  /* Configure Data Security SLI          */
 	uint32_t rsvd1     : 19;  /* Reserved                             */
 #endif
 #ifdef __BIG_ENDIAN_BITFIELD
 	uint32_t rsvd3     : 19;  /* Reserved                             */
 	uint32_t gdss      :  1;  /* Configure Data Security SLI          */
-	uint32_t rsvd4     :  3;  /* Reserved                             */
+	uint32_t gasabt    :  1;  /* Grant async abts status notice       */
+	uint32_t rsvd4     :  2;  /* Reserved                             */
 	uint32_t gbg       :  1;  /* Grant BlockGuard                     */
 	uint32_t gmv	   :  1;  /* Grant Max VPIs                       */
 	uint32_t gcrp	   :  1;  /* Grant Command Ring Polling           */
@@ -2866,7 +2875,8 @@ typedef struct {
 	uint32_t gcrp	   :  1;  /* Grant Command Ring Polling           */
 	uint32_t gmv	   :  1;  /* Grant Max VPIs                       */
 	uint32_t gbg       :  1;  /* Grant BlockGuard                     */
-	uint32_t rsvd4     :  3;  /* Reserved                             */
+	uint32_t rsvd4     :  2;  /* Reserved                             */
+	uint32_t gasabt    :  1;  /* Grant async abts status notice       */
 	uint32_t gdss      :  1;  /* Configure Data Security SLI          */
 	uint32_t rsvd3     : 19;  /* Reserved                             */
 #endif
@@ -3465,6 +3475,7 @@ typedef struct {
 } ASYNCSTAT_FIELDS;
 #define ASYNC_TEMP_WARN		0x100
 #define ASYNC_TEMP_SAFE		0x101
+#define ASYNC_STATUS_CN		0x102
 
 /* IOCB Command template for CMD_IOCB_RCV_ELS64_CX (0xB7)
    or CMD_IOCB_RCV_SEQ64_CX (0xB5) */
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 98d21521f539..e5bfa7f334e3 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -1351,11 +1351,11 @@ struct lpfc_mbx_set_link_diag_loopback {
 		struct {
 			uint32_t word0;
 #define lpfc_mbx_set_diag_lpbk_type_SHIFT	0
-#define lpfc_mbx_set_diag_lpbk_type_MASK	0x00000001
+#define lpfc_mbx_set_diag_lpbk_type_MASK	0x00000003
 #define lpfc_mbx_set_diag_lpbk_type_WORD	word0
 #define LPFC_DIAG_LOOPBACK_TYPE_DISABLE		0x0
 #define LPFC_DIAG_LOOPBACK_TYPE_INTERNAL	0x1
-#define LPFC_DIAG_LOOPBACK_TYPE_EXTERNAL	0x2
+#define LPFC_DIAG_LOOPBACK_TYPE_SERDES		0x2
 #define lpfc_mbx_set_diag_lpbk_link_num_SHIFT	16
 #define lpfc_mbx_set_diag_lpbk_link_num_MASK	0x0000003F
 #define lpfc_mbx_set_diag_lpbk_link_num_WORD	word0
@@ -1830,6 +1830,8 @@ struct lpfc_mbx_init_vfi {
 #define lpfc_init_vfi_hop_count_MASK	0x000000FF
 #define lpfc_init_vfi_hop_count_WORD	word4
 };
+#define MBX_VFI_IN_USE			0x9F02
+
 
 struct lpfc_mbx_reg_vfi {
 	uint32_t word1;
@@ -2104,6 +2106,8 @@ struct lpfc_mbx_read_config {
 #define lpfc_mbx_rd_conf_lnk_type_SHIFT		6
 #define lpfc_mbx_rd_conf_lnk_type_MASK		0x00000003
 #define lpfc_mbx_rd_conf_lnk_type_WORD		word2
+#define LPFC_LNK_TYPE_GE	0
+#define LPFC_LNK_TYPE_FC	1
 #define lpfc_mbx_rd_conf_lnk_ldv_SHIFT		8
 #define lpfc_mbx_rd_conf_lnk_ldv_MASK		0x00000001
 #define lpfc_mbx_rd_conf_lnk_ldv_WORD		word2
@@ -3320,6 +3324,9 @@ struct wqe_rctl_dfctl {
 #define wqe_la_SHIFT 3
 #define wqe_la_MASK  0x000000001
 #define wqe_la_WORD  word5
+#define wqe_xo_SHIFT	6
+#define wqe_xo_MASK	0x000000001
+#define wqe_xo_WORD	word5
 #define wqe_ls_SHIFT 7
 #define wqe_ls_MASK  0x000000001
 #define wqe_ls_WORD  word5
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 55bc4fc7376f..dfea2dada02c 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -62,7 +62,6 @@ static int lpfc_post_rcv_buf(struct lpfc_hba *);
 static int lpfc_sli4_queue_verify(struct lpfc_hba *);
 static int lpfc_create_bootstrap_mbox(struct lpfc_hba *);
 static int lpfc_setup_endian_order(struct lpfc_hba *);
-static int lpfc_sli4_read_config(struct lpfc_hba *);
 static void lpfc_destroy_bootstrap_mbox(struct lpfc_hba *);
 static void lpfc_free_sgl_list(struct lpfc_hba *);
 static int lpfc_init_sgl_list(struct lpfc_hba *);
@@ -475,27 +474,6 @@ lpfc_config_port_post(struct lpfc_hba *phba)
 	/* Get the default values for Model Name and Description */
 	lpfc_get_hba_model_desc(phba, phba->ModelName, phba->ModelDesc);
 
-	if ((phba->cfg_link_speed > LPFC_USER_LINK_SPEED_16G)
-	    || ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_1G)
-		&& !(phba->lmt & LMT_1Gb))
-	    || ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_2G)
-		&& !(phba->lmt & LMT_2Gb))
-	    || ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_4G)
-		&& !(phba->lmt & LMT_4Gb))
-	    || ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_8G)
-		&& !(phba->lmt & LMT_8Gb))
-	    || ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_10G)
-		&& !(phba->lmt & LMT_10Gb))
-	    || ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_16G)
-		&& !(phba->lmt & LMT_16Gb))) {
-		/* Reset link speed to auto */
-		lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
-			"1302 Invalid speed for this board: "
-			"Reset link speed to auto: x%x\n",
-			phba->cfg_link_speed);
-			phba->cfg_link_speed = LPFC_USER_LINK_SPEED_AUTO;
-	}
-
 	phba->link_state = LPFC_LINK_DOWN;
 
 	/* Only process IOCBs on ELS ring till hba_state is READY */
@@ -585,28 +563,10 @@ lpfc_config_port_post(struct lpfc_hba *phba)
 			return -EIO;
 		}
 	} else if (phba->cfg_suppress_link_up == LPFC_INITIALIZE_LINK) {
-		lpfc_init_link(phba, pmb, phba->cfg_topology,
-			phba->cfg_link_speed);
-		pmb->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
-		lpfc_set_loopback_flag(phba);
-		rc = lpfc_sli_issue_mbox(phba, pmb, MBX_NOWAIT);
-		if (rc != MBX_SUCCESS) {
-			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-				"0454 Adapter failed to init, mbxCmd x%x "
-				"INIT_LINK, mbxStatus x%x\n",
-				mb->mbxCommand, mb->mbxStatus);
-
-			/* Clear all interrupt enable conditions */
-			writel(0, phba->HCregaddr);
-			readl(phba->HCregaddr); /* flush */
-			/* Clear all pending interrupts */
-			writel(0xffffffff, phba->HAregaddr);
-			readl(phba->HAregaddr); /* flush */
-			phba->link_state = LPFC_HBA_ERROR;
-			if (rc != MBX_BUSY)
-				mempool_free(pmb, phba->mbox_mem_pool);
-			return -EIO;
-		}
+		mempool_free(pmb, phba->mbox_mem_pool);
+		rc = phba->lpfc_hba_init_link(phba, MBX_NOWAIT);
+		if (rc)
+			return rc;
 	}
 	/* MBOX buffer will be freed in mbox compl */
 	pmb = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
@@ -668,6 +628,28 @@ lpfc_config_port_post(struct lpfc_hba *phba)
 int
 lpfc_hba_init_link(struct lpfc_hba *phba, uint32_t flag)
 {
+	return lpfc_hba_init_link_fc_topology(phba, phba->cfg_topology, flag);
+}
+
+/**
+ * lpfc_hba_init_link_fc_topology - Initialize FC link with desired topology
+ * @phba: pointer to lpfc hba data structure.
+ * @fc_topology: desired fc topology.
+ * @flag: mailbox command issue mode - either MBX_POLL or MBX_NOWAIT
+ *
+ * This routine will issue the INIT_LINK mailbox command call.
+ * It is available to other drivers through the lpfc_hba data
+ * structure for use as a delayed link up mechanism with the
+ * module parameter lpfc_suppress_link_up.
+ *
+ * Return code
+ *              0 - success
+ *              Any other value - error
+ **/
+int
+lpfc_hba_init_link_fc_topology(struct lpfc_hba *phba, uint32_t fc_topology,
+			       uint32_t flag)
+{
 	struct lpfc_vport *vport = phba->pport;
 	LPFC_MBOXQ_t *pmb;
 	MAILBOX_t *mb;
@@ -681,9 +663,30 @@ lpfc_hba_init_link(struct lpfc_hba *phba, uint32_t flag)
 	mb = &pmb->u.mb;
 	pmb->vport = vport;
 
-	lpfc_init_link(phba, pmb, phba->cfg_topology, phba->cfg_link_speed);
+	if ((phba->cfg_link_speed > LPFC_USER_LINK_SPEED_MAX) ||
+	    ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_1G) &&
+	     !(phba->lmt & LMT_1Gb)) ||
+	    ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_2G) &&
+	     !(phba->lmt & LMT_2Gb)) ||
+	    ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_4G) &&
+	     !(phba->lmt & LMT_4Gb)) ||
+	    ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_8G) &&
+	     !(phba->lmt & LMT_8Gb)) ||
+	    ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_10G) &&
+	     !(phba->lmt & LMT_10Gb)) ||
+	    ((phba->cfg_link_speed == LPFC_USER_LINK_SPEED_16G) &&
+	     !(phba->lmt & LMT_16Gb))) {
+		/* Reset link speed to auto */
+		lpfc_printf_log(phba, KERN_ERR, LOG_LINK_EVENT,
+			"1302 Invalid speed for this board:%d "
+			"Reset link speed to auto.\n",
+			phba->cfg_link_speed);
+			phba->cfg_link_speed = LPFC_USER_LINK_SPEED_AUTO;
+	}
+	lpfc_init_link(phba, pmb, fc_topology, phba->cfg_link_speed);
 	pmb->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
-	lpfc_set_loopback_flag(phba);
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		lpfc_set_loopback_flag(phba);
 	rc = lpfc_sli_issue_mbox(phba, pmb, flag);
 	if ((rc != MBX_BUSY) && (rc != MBX_SUCCESS)) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
@@ -1437,7 +1440,10 @@ lpfc_handle_eratt_s4(struct lpfc_hba *phba)
 	uint32_t event_data;
 	struct Scsi_Host *shost;
 	uint32_t if_type;
-	struct lpfc_register portstat_reg;
+	struct lpfc_register portstat_reg = {0};
+	uint32_t reg_err1, reg_err2;
+	uint32_t uerrlo_reg, uemasklo_reg;
+	uint32_t pci_rd_rc1, pci_rd_rc2;
 	int rc;
 
 	/* If the pci channel is offline, ignore possible errors, since
@@ -1449,38 +1455,52 @@ lpfc_handle_eratt_s4(struct lpfc_hba *phba)
 	if (!phba->cfg_enable_hba_reset)
 		return;
 
-	/* Send an internal error event to mgmt application */
-	lpfc_board_errevt_to_mgmt(phba);
-
-	/* For now, the actual action for SLI4 device handling is not
-	 * specified yet, just treated it as adaptor hardware failure
-	 */
-	event_data = FC_REG_DUMP_EVENT;
-	shost = lpfc_shost_from_vport(vport);
-	fc_host_post_vendor_event(shost, fc_get_event_number(),
-				  sizeof(event_data), (char *) &event_data,
-				  SCSI_NL_VID_TYPE_PCI | PCI_VENDOR_ID_EMULEX);
-
 	if_type = bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf);
 	switch (if_type) {
 	case LPFC_SLI_INTF_IF_TYPE_0:
+		pci_rd_rc1 = lpfc_readl(
+				phba->sli4_hba.u.if_type0.UERRLOregaddr,
+				&uerrlo_reg);
+		pci_rd_rc2 = lpfc_readl(
+				phba->sli4_hba.u.if_type0.UEMASKLOregaddr,
+				&uemasklo_reg);
+		/* consider PCI bus read error as pci_channel_offline */
+		if (pci_rd_rc1 == -EIO && pci_rd_rc2 == -EIO)
+			return;
 		lpfc_sli4_offline_eratt(phba);
 		break;
 	case LPFC_SLI_INTF_IF_TYPE_2:
-		portstat_reg.word0 =
-			readl(phba->sli4_hba.u.if_type2.STATUSregaddr);
-
+		pci_rd_rc1 = lpfc_readl(
+				phba->sli4_hba.u.if_type2.STATUSregaddr,
+				&portstat_reg.word0);
+		/* consider PCI bus read error as pci_channel_offline */
+		if (pci_rd_rc1 == -EIO)
+			return;
+		reg_err1 = readl(phba->sli4_hba.u.if_type2.ERR1regaddr);
+		reg_err2 = readl(phba->sli4_hba.u.if_type2.ERR2regaddr);
 		if (bf_get(lpfc_sliport_status_oti, &portstat_reg)) {
 			/* TODO: Register for Overtemp async events. */
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"2889 Port Overtemperature event, "
-				"taking port\n");
+				"taking port offline\n");
 			spin_lock_irq(&phba->hbalock);
 			phba->over_temp_state = HBA_OVER_TEMP;
 			spin_unlock_irq(&phba->hbalock);
 			lpfc_sli4_offline_eratt(phba);
-			return;
+			break;
 		}
+		if (reg_err1 == SLIPORT_ERR1_REG_ERR_CODE_2 &&
+		    reg_err2 == SLIPORT_ERR2_REG_FW_RESTART)
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+					"3143 Port Down: Firmware Restarted\n");
+		else if (reg_err1 == SLIPORT_ERR1_REG_ERR_CODE_2 &&
+			 reg_err2 == SLIPORT_ERR2_REG_FORCED_DUMP)
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+					"3144 Port Down: Debug Dump\n");
+		else if (reg_err1 == SLIPORT_ERR1_REG_ERR_CODE_2 &&
+			 reg_err2 == SLIPORT_ERR2_REG_FUNC_PROVISON)
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+					"3145 Port Down: Provisioning\n");
 		/*
 		 * On error status condition, driver need to wait for port
 		 * ready before performing reset.
@@ -1489,14 +1509,19 @@ lpfc_handle_eratt_s4(struct lpfc_hba *phba)
 		if (!rc) {
 			/* need reset: attempt for port recovery */
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-					"2887 Port Error: Attempting "
-					"Port Recovery\n");
+					"2887 Reset Needed: Attempting Port "
+					"Recovery...\n");
 			lpfc_offline_prep(phba);
 			lpfc_offline(phba);
 			lpfc_sli_brdrestart(phba);
 			if (lpfc_online(phba) == 0) {
 				lpfc_unblock_mgmt_io(phba);
-				return;
+				/* don't report event on forced debug dump */
+				if (reg_err1 == SLIPORT_ERR1_REG_ERR_CODE_2 &&
+				    reg_err2 == SLIPORT_ERR2_REG_FORCED_DUMP)
+					return;
+				else
+					break;
 			}
 			/* fall through for not able to recover */
 		}
@@ -1506,6 +1531,16 @@ lpfc_handle_eratt_s4(struct lpfc_hba *phba)
 	default:
 		break;
 	}
+	lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
+			"3123 Report dump event to upper layer\n");
+	/* Send an internal error event to mgmt application */
+	lpfc_board_errevt_to_mgmt(phba);
+
+	event_data = FC_REG_DUMP_EVENT;
+	shost = lpfc_shost_from_vport(vport);
+	fc_host_post_vendor_event(shost, fc_get_event_number(),
+				  sizeof(event_data), (char *) &event_data,
+				  SCSI_NL_VID_TYPE_PCI | PCI_VENDOR_ID_EMULEX);
 }
 
 /**
@@ -2674,6 +2709,32 @@ lpfc_offline(struct lpfc_hba *phba)
 }
 
 /**
+ * lpfc_scsi_buf_update - Update the scsi_buffers that are already allocated.
+ * @phba: pointer to lpfc hba data structure.
+ *
+ * This routine goes through all the scsi buffers in the system and updates the
+ * Physical XRIs assigned to the SCSI buffer because these may change after any
+ * firmware reset
+ *
+ * Return codes
+ *   0 - successful (for now, it always returns 0)
+ **/
+int
+lpfc_scsi_buf_update(struct lpfc_hba *phba)
+{
+	struct lpfc_scsi_buf *sb, *sb_next;
+
+	spin_lock_irq(&phba->hbalock);
+	spin_lock(&phba->scsi_buf_list_lock);
+	list_for_each_entry_safe(sb, sb_next, &phba->lpfc_scsi_buf_list, list)
+		sb->cur_iocbq.sli4_xritag =
+			phba->sli4_hba.xri_ids[sb->cur_iocbq.sli4_lxritag];
+	spin_unlock(&phba->scsi_buf_list_lock);
+	spin_unlock_irq(&phba->hbalock);
+	return 0;
+}
+
+/**
  * lpfc_scsi_free - Free all the SCSI buffers and IOCBs from driver lists
  * @phba: pointer to lpfc hba data structure.
  *
@@ -5040,15 +5101,8 @@ lpfc_sli4_init_rpi_hdrs(struct lpfc_hba *phba)
 	struct lpfc_rpi_hdr *rpi_hdr;
 
 	INIT_LIST_HEAD(&phba->sli4_hba.lpfc_rpi_hdr_list);
-	/*
-	 * If the SLI4 port supports extents, posting the rpi header isn't
-	 * required.  Set the expected maximum count and let the actual value
-	 * get set when extents are fully allocated.
-	 */
-	if (!phba->sli4_hba.rpi_hdrs_in_use) {
-		phba->sli4_hba.next_rpi = phba->sli4_hba.max_cfg_param.max_rpi;
+	if (!phba->sli4_hba.rpi_hdrs_in_use)
 		return rc;
-	}
 	if (phba->sli4_hba.extents_in_use)
 		return -EIO;
 
@@ -5942,7 +5996,7 @@ lpfc_destroy_bootstrap_mbox(struct lpfc_hba *phba)
  * 	-ENOMEM - No available memory
  *      -EIO - The mailbox failed to complete successfully.
  **/
-static int
+int
 lpfc_sli4_read_config(struct lpfc_hba *phba)
 {
 	LPFC_MBOXQ_t *pmb;
@@ -5974,6 +6028,20 @@ lpfc_sli4_read_config(struct lpfc_hba *phba)
 		rc = -EIO;
 	} else {
 		rd_config = &pmb->u.mqe.un.rd_config;
+		if (bf_get(lpfc_mbx_rd_conf_lnk_ldv, rd_config)) {
+			phba->sli4_hba.lnk_info.lnk_dv = LPFC_LNK_DAT_VAL;
+			phba->sli4_hba.lnk_info.lnk_tp =
+				bf_get(lpfc_mbx_rd_conf_lnk_type, rd_config);
+			phba->sli4_hba.lnk_info.lnk_no =
+				bf_get(lpfc_mbx_rd_conf_lnk_numb, rd_config);
+			lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+					"3081 lnk_type:%d, lnk_numb:%d\n",
+					phba->sli4_hba.lnk_info.lnk_tp,
+					phba->sli4_hba.lnk_info.lnk_no);
+		} else
+			lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
+					"3082 Mailbox (x%x) returned ldv:x0\n",
+					bf_get(lpfc_mqe_command, &pmb->u.mqe));
 		phba->sli4_hba.extents_in_use =
 			bf_get(lpfc_mbx_rd_conf_extnts_inuse, rd_config);
 		phba->sli4_hba.max_cfg_param.max_xri =
@@ -6462,6 +6530,7 @@ out_free_fcp_wq:
 		phba->sli4_hba.fcp_wq[fcp_wqidx] = NULL;
 	}
 	kfree(phba->sli4_hba.fcp_wq);
+	phba->sli4_hba.fcp_wq = NULL;
 out_free_els_wq:
 	lpfc_sli4_queue_free(phba->sli4_hba.els_wq);
 	phba->sli4_hba.els_wq = NULL;
@@ -6474,6 +6543,7 @@ out_free_fcp_cq:
 		phba->sli4_hba.fcp_cq[fcp_cqidx] = NULL;
 	}
 	kfree(phba->sli4_hba.fcp_cq);
+	phba->sli4_hba.fcp_cq = NULL;
 out_free_els_cq:
 	lpfc_sli4_queue_free(phba->sli4_hba.els_cq);
 	phba->sli4_hba.els_cq = NULL;
@@ -6486,6 +6556,7 @@ out_free_fp_eq:
 		phba->sli4_hba.fp_eq[fcp_eqidx] = NULL;
 	}
 	kfree(phba->sli4_hba.fp_eq);
+	phba->sli4_hba.fp_eq = NULL;
 out_free_sp_eq:
 	lpfc_sli4_queue_free(phba->sli4_hba.sp_eq);
 	phba->sli4_hba.sp_eq = NULL;
@@ -6519,8 +6590,10 @@ lpfc_sli4_queue_destroy(struct lpfc_hba *phba)
 	phba->sli4_hba.els_wq = NULL;
 
 	/* Release FCP work queue */
-	for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_wq_count; fcp_qidx++)
-		lpfc_sli4_queue_free(phba->sli4_hba.fcp_wq[fcp_qidx]);
+	if (phba->sli4_hba.fcp_wq != NULL)
+		for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_wq_count;
+		     fcp_qidx++)
+			lpfc_sli4_queue_free(phba->sli4_hba.fcp_wq[fcp_qidx]);
 	kfree(phba->sli4_hba.fcp_wq);
 	phba->sli4_hba.fcp_wq = NULL;
 
@@ -6540,15 +6613,18 @@ lpfc_sli4_queue_destroy(struct lpfc_hba *phba)
 
 	/* Release FCP response complete queue */
 	fcp_qidx = 0;
-	do
-		lpfc_sli4_queue_free(phba->sli4_hba.fcp_cq[fcp_qidx]);
-	while (++fcp_qidx < phba->cfg_fcp_eq_count);
+	if (phba->sli4_hba.fcp_cq != NULL)
+		do
+			lpfc_sli4_queue_free(phba->sli4_hba.fcp_cq[fcp_qidx]);
+		while (++fcp_qidx < phba->cfg_fcp_eq_count);
 	kfree(phba->sli4_hba.fcp_cq);
 	phba->sli4_hba.fcp_cq = NULL;
 
 	/* Release fast-path event queue */
-	for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_eq_count; fcp_qidx++)
-		lpfc_sli4_queue_free(phba->sli4_hba.fp_eq[fcp_qidx]);
+	if (phba->sli4_hba.fp_eq != NULL)
+		for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_eq_count;
+		     fcp_qidx++)
+			lpfc_sli4_queue_free(phba->sli4_hba.fp_eq[fcp_qidx]);
 	kfree(phba->sli4_hba.fp_eq);
 	phba->sli4_hba.fp_eq = NULL;
 
@@ -6601,11 +6677,18 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 			phba->sli4_hba.sp_eq->queue_id);
 
 	/* Set up fast-path event queue */
+	if (phba->cfg_fcp_eq_count && !phba->sli4_hba.fp_eq) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"3147 Fast-path EQs not allocated\n");
+		rc = -ENOMEM;
+		goto out_destroy_sp_eq;
+	}
 	for (fcp_eqidx = 0; fcp_eqidx < phba->cfg_fcp_eq_count; fcp_eqidx++) {
 		if (!phba->sli4_hba.fp_eq[fcp_eqidx]) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"0522 Fast-path EQ (%d) not "
 					"allocated\n", fcp_eqidx);
+			rc = -ENOMEM;
 			goto out_destroy_fp_eq;
 		}
 		rc = lpfc_eq_create(phba, phba->sli4_hba.fp_eq[fcp_eqidx],
@@ -6630,6 +6713,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (!phba->sli4_hba.mbx_cq) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0528 Mailbox CQ not allocated\n");
+		rc = -ENOMEM;
 		goto out_destroy_fp_eq;
 	}
 	rc = lpfc_cq_create(phba, phba->sli4_hba.mbx_cq, phba->sli4_hba.sp_eq,
@@ -6649,6 +6733,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (!phba->sli4_hba.els_cq) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0530 ELS CQ not allocated\n");
+		rc = -ENOMEM;
 		goto out_destroy_mbx_cq;
 	}
 	rc = lpfc_cq_create(phba, phba->sli4_hba.els_cq, phba->sli4_hba.sp_eq,
@@ -6665,12 +6750,20 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 			phba->sli4_hba.sp_eq->queue_id);
 
 	/* Set up fast-path FCP Response Complete Queue */
+	if (!phba->sli4_hba.fcp_cq) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"3148 Fast-path FCP CQ array not "
+				"allocated\n");
+		rc = -ENOMEM;
+		goto out_destroy_els_cq;
+	}
 	fcp_cqidx = 0;
 	do {
 		if (!phba->sli4_hba.fcp_cq[fcp_cqidx]) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"0526 Fast-path FCP CQ (%d) not "
 					"allocated\n", fcp_cqidx);
+			rc = -ENOMEM;
 			goto out_destroy_fcp_cq;
 		}
 		if (phba->cfg_fcp_eq_count)
@@ -6709,6 +6802,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (!phba->sli4_hba.mbx_wq) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0538 Slow-path MQ not allocated\n");
+		rc = -ENOMEM;
 		goto out_destroy_fcp_cq;
 	}
 	rc = lpfc_mq_create(phba, phba->sli4_hba.mbx_wq,
@@ -6728,6 +6822,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (!phba->sli4_hba.els_wq) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0536 Slow-path ELS WQ not allocated\n");
+		rc = -ENOMEM;
 		goto out_destroy_mbx_wq;
 	}
 	rc = lpfc_wq_create(phba, phba->sli4_hba.els_wq,
@@ -6744,11 +6839,19 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 			phba->sli4_hba.els_cq->queue_id);
 
 	/* Set up fast-path FCP Work Queue */
+	if (!phba->sli4_hba.fcp_wq) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"3149 Fast-path FCP WQ array not "
+				"allocated\n");
+		rc = -ENOMEM;
+		goto out_destroy_els_wq;
+	}
 	for (fcp_wqidx = 0; fcp_wqidx < phba->cfg_fcp_wq_count; fcp_wqidx++) {
 		if (!phba->sli4_hba.fcp_wq[fcp_wqidx]) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"0534 Fast-path FCP WQ (%d) not "
 					"allocated\n", fcp_wqidx);
+			rc = -ENOMEM;
 			goto out_destroy_fcp_wq;
 		}
 		rc = lpfc_wq_create(phba, phba->sli4_hba.fcp_wq[fcp_wqidx],
@@ -6779,6 +6882,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (!phba->sli4_hba.hdr_rq || !phba->sli4_hba.dat_rq) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0540 Receive Queue not allocated\n");
+		rc = -ENOMEM;
 		goto out_destroy_fcp_wq;
 	}
 
@@ -6805,18 +6909,21 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 out_destroy_fcp_wq:
 	for (--fcp_wqidx; fcp_wqidx >= 0; fcp_wqidx--)
 		lpfc_wq_destroy(phba, phba->sli4_hba.fcp_wq[fcp_wqidx]);
+out_destroy_els_wq:
 	lpfc_wq_destroy(phba, phba->sli4_hba.els_wq);
 out_destroy_mbx_wq:
 	lpfc_mq_destroy(phba, phba->sli4_hba.mbx_wq);
 out_destroy_fcp_cq:
 	for (--fcp_cqidx; fcp_cqidx >= 0; fcp_cqidx--)
 		lpfc_cq_destroy(phba, phba->sli4_hba.fcp_cq[fcp_cqidx]);
+out_destroy_els_cq:
 	lpfc_cq_destroy(phba, phba->sli4_hba.els_cq);
 out_destroy_mbx_cq:
 	lpfc_cq_destroy(phba, phba->sli4_hba.mbx_cq);
 out_destroy_fp_eq:
 	for (--fcp_eqidx; fcp_eqidx >= 0; fcp_eqidx--)
 		lpfc_eq_destroy(phba, phba->sli4_hba.fp_eq[fcp_eqidx]);
+out_destroy_sp_eq:
 	lpfc_eq_destroy(phba, phba->sli4_hba.sp_eq);
 out_error:
 	return rc;
@@ -6853,13 +6960,18 @@ lpfc_sli4_queue_unset(struct lpfc_hba *phba)
 	/* Unset ELS complete queue */
 	lpfc_cq_destroy(phba, phba->sli4_hba.els_cq);
 	/* Unset FCP response complete queue */
-	fcp_qidx = 0;
-	do {
-		lpfc_cq_destroy(phba, phba->sli4_hba.fcp_cq[fcp_qidx]);
-	} while (++fcp_qidx < phba->cfg_fcp_eq_count);
+	if (phba->sli4_hba.fcp_cq) {
+		fcp_qidx = 0;
+		do {
+			lpfc_cq_destroy(phba, phba->sli4_hba.fcp_cq[fcp_qidx]);
+		} while (++fcp_qidx < phba->cfg_fcp_eq_count);
+	}
 	/* Unset fast-path event queue */
-	for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_eq_count; fcp_qidx++)
-		lpfc_eq_destroy(phba, phba->sli4_hba.fp_eq[fcp_qidx]);
+	if (phba->sli4_hba.fp_eq) {
+		for (fcp_qidx = 0; fcp_qidx < phba->cfg_fcp_eq_count;
+		     fcp_qidx++)
+			lpfc_eq_destroy(phba, phba->sli4_hba.fp_eq[fcp_qidx]);
+	}
 	/* Unset slow-path event queue */
 	lpfc_eq_destroy(phba, phba->sli4_hba.sp_eq);
 }
@@ -7398,22 +7510,25 @@ out:
 static void
 lpfc_sli4_pci_mem_unset(struct lpfc_hba *phba)
 {
-	struct pci_dev *pdev;
-
-	/* Obtain PCI device reference */
-	if (!phba->pcidev)
-		return;
-	else
-		pdev = phba->pcidev;
-
-	/* Free coherent DMA memory allocated */
-
-	/* Unmap I/O memory space */
-	iounmap(phba->sli4_hba.drbl_regs_memmap_p);
-	iounmap(phba->sli4_hba.ctrl_regs_memmap_p);
-	iounmap(phba->sli4_hba.conf_regs_memmap_p);
+	uint32_t if_type;
+	if_type = bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf);
 
-	return;
+	switch (if_type) {
+	case LPFC_SLI_INTF_IF_TYPE_0:
+		iounmap(phba->sli4_hba.drbl_regs_memmap_p);
+		iounmap(phba->sli4_hba.ctrl_regs_memmap_p);
+		iounmap(phba->sli4_hba.conf_regs_memmap_p);
+		break;
+	case LPFC_SLI_INTF_IF_TYPE_2:
+		iounmap(phba->sli4_hba.conf_regs_memmap_p);
+		break;
+	case LPFC_SLI_INTF_IF_TYPE_1:
+	default:
+		dev_printk(KERN_ERR, &phba->pcidev->dev,
+			   "FATAL - unsupported SLI4 interface type - %d\n",
+			   if_type);
+		break;
+	}
 }
 
 /**
@@ -9198,12 +9313,15 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
 	/* Perform post initialization setup */
 	lpfc_post_init_setup(phba);
 
-	/* check for firmware upgrade or downgrade */
-	snprintf(file_name, 16, "%s.grp", phba->ModelName);
-	error = request_firmware(&fw, file_name, &phba->pcidev->dev);
-	if (!error) {
-		lpfc_write_firmware(phba, fw);
-		release_firmware(fw);
+	/* check for firmware upgrade or downgrade (if_type 2 only) */
+	if (bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf) ==
+	    LPFC_SLI_INTF_IF_TYPE_2) {
+		snprintf(file_name, 16, "%s.grp", phba->ModelName);
+		error = request_firmware(&fw, file_name, &phba->pcidev->dev);
+		if (!error) {
+			lpfc_write_firmware(phba, fw);
+			release_firmware(fw);
+		}
 	}
 
 	/* Check if there are static vports to be created. */
diff --git a/drivers/scsi/lpfc/lpfc_mbox.c b/drivers/scsi/lpfc/lpfc_mbox.c
index 2ebc7d2540c0..20336f09fb3c 100644
--- a/drivers/scsi/lpfc/lpfc_mbox.c
+++ b/drivers/scsi/lpfc/lpfc_mbox.c
@@ -1293,6 +1293,10 @@ lpfc_config_port(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 		phba->sli_rev = LPFC_SLI_REV2;
 	mb->un.varCfgPort.sli_mode = phba->sli_rev;
 
+	/* If this is an SLI3 port, configure async status notification. */
+	if (phba->sli_rev == LPFC_SLI_REV3)
+		mb->un.varCfgPort.casabt = 1;
+
 	/* Now setup pcb */
 	phba->pcb->type = TYPE_NATIVE_SLI2;
 	phba->pcb->feature = FEATURE_INITIAL_SLI2;
@@ -2129,6 +2133,14 @@ lpfc_reg_vfi(struct lpfcMboxq *mbox, struct lpfc_vport *vport, dma_addr_t phys)
 	reg_vfi->bde.tus.f.bdeSize = sizeof(vport->fc_sparam);
 	reg_vfi->bde.tus.f.bdeFlags = BUFF_TYPE_BDE_64;
 	bf_set(lpfc_reg_vfi_nport_id, reg_vfi, vport->fc_myDID);
+	lpfc_printf_vlog(vport, KERN_INFO, LOG_MBOX,
+			"3134 Register VFI, mydid:x%x, fcfi:%d, "
+			" vfi:%d, vpi:%d, fc_pname:%x%x\n",
+			vport->fc_myDID,
+			vport->phba->fcf.fcfi,
+			vport->phba->sli4_hba.vfi_ids[vport->vfi],
+			vport->phba->vpi_ids[vport->vpi],
+			reg_vfi->wwn[0], reg_vfi->wwn[1]);
 }
 
 /**
@@ -2175,16 +2187,15 @@ lpfc_unreg_vfi(struct lpfcMboxq *mbox, struct lpfc_vport *vport)
 }
 
 /**
- * lpfc_dump_fcoe_param - Dump config region 23 to get FCoe parameters.
+ * lpfc_sli4_dump_cfg_rg23 - Dump sli4 port config region 23
  * @phba: pointer to the hba structure containing.
  * @mbox: pointer to lpfc mbox command to initialize.
  *
- * This function create a SLI4 dump mailbox command to dump FCoE
- * parameters stored in region 23.
+ * This function create a SLI4 dump mailbox command to dump configure
+ * region 23.
  **/
 int
-lpfc_dump_fcoe_param(struct lpfc_hba *phba,
-		struct lpfcMboxq *mbox)
+lpfc_sli4_dump_cfg_rg23(struct lpfc_hba *phba, struct lpfcMboxq *mbox)
 {
 	struct lpfc_dmabuf *mp = NULL;
 	MAILBOX_t *mb;
@@ -2198,9 +2209,9 @@ lpfc_dump_fcoe_param(struct lpfc_hba *phba,
 
 	if (!mp || !mp->virt) {
 		kfree(mp);
-		/* dump_fcoe_param failed to allocate memory */
+		/* dump config region 23 failed to allocate memory */
 		lpfc_printf_log(phba, KERN_WARNING, LOG_MBOX,
-			"2569 lpfc_dump_fcoe_param: memory"
+			"2569 lpfc dump config region 23: memory"
 			" allocation failed\n");
 		return 1;
 	}
diff --git a/drivers/scsi/lpfc/lpfc_mem.c b/drivers/scsi/lpfc/lpfc_mem.c
index 10d5b5e41499..ade763d3930a 100644
--- a/drivers/scsi/lpfc/lpfc_mem.c
+++ b/drivers/scsi/lpfc/lpfc_mem.c
@@ -389,7 +389,7 @@ lpfc_els_hbq_alloc(struct lpfc_hba *phba)
 {
 	struct hbq_dmabuf *hbqbp;
 
-	hbqbp = kmalloc(sizeof(struct hbq_dmabuf), GFP_KERNEL);
+	hbqbp = kzalloc(sizeof(struct hbq_dmabuf), GFP_KERNEL);
 	if (!hbqbp)
 		return NULL;
 
@@ -441,7 +441,7 @@ lpfc_sli4_rb_alloc(struct lpfc_hba *phba)
 {
 	struct hbq_dmabuf *dma_buf;
 
-	dma_buf = kmalloc(sizeof(struct hbq_dmabuf), GFP_KERNEL);
+	dma_buf = kzalloc(sizeof(struct hbq_dmabuf), GFP_KERNEL);
 	if (!dma_buf)
 		return NULL;
 
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index 2ddd02f7c603..e8bb00559943 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -783,6 +783,14 @@ lpfc_device_rm_unused_node(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 }
 
 static uint32_t
+lpfc_device_recov_unused_node(struct lpfc_vport *vport,
+			struct lpfc_nodelist *ndlp,
+			   void *arg, uint32_t evt)
+{
+	return ndlp->nlp_state;
+}
+
+static uint32_t
 lpfc_rcv_plogi_plogi_issue(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 			   void *arg, uint32_t evt)
 {
@@ -2147,7 +2155,7 @@ static uint32_t (*lpfc_disc_action[NLP_STE_MAX_STATE * NLP_EVT_MAX_EVENT])
 	lpfc_disc_illegal,		/* CMPL_ADISC      */
 	lpfc_disc_illegal,		/* CMPL_REG_LOGIN  */
 	lpfc_device_rm_unused_node,	/* DEVICE_RM       */
-	lpfc_disc_illegal,		/* DEVICE_RECOVERY */
+	lpfc_device_recov_unused_node,	/* DEVICE_RECOVERY */
 
 	lpfc_rcv_plogi_plogi_issue,	/* RCV_PLOGI   PLOGI_ISSUE    */
 	lpfc_rcv_prli_plogi_issue,	/* RCV_PRLI        */
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 2e1e54e5c3ae..c60f5d0b3869 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -681,8 +681,10 @@ lpfc_sli4_fcp_xri_aborted(struct lpfc_hba *phba,
 
 			rrq_empty = list_empty(&phba->active_rrq_list);
 			spin_unlock_irqrestore(&phba->hbalock, iflag);
-			if (ndlp)
+			if (ndlp) {
 				lpfc_set_rrq_active(phba, ndlp, xri, rxid, 1);
+				lpfc_sli4_abts_err_handler(phba, ndlp, axri);
+			}
 			lpfc_release_scsi_buf_s4(phba, psb);
 			if (rrq_empty)
 				lpfc_worker_wake_up(phba);
@@ -2911,8 +2913,8 @@ lpfc_scsi_prep_cmnd(struct lpfc_vport *vport, struct lpfc_scsi_buf *lpfc_cmd,
 	int_to_scsilun(lpfc_cmd->pCmd->device->lun,
 			&lpfc_cmd->fcp_cmnd->fcp_lun);
 
-	memcpy(&fcp_cmnd->fcpCdb[0], scsi_cmnd->cmnd, 16);
-
+	memset(&fcp_cmnd->fcpCdb[0], 0, LPFC_FCP_CDB_LEN);
+	memcpy(&fcp_cmnd->fcpCdb[0], scsi_cmnd->cmnd, scsi_cmnd->cmd_len);
 	if (scsi_populate_tag_msg(scsi_cmnd, tag)) {
 		switch (tag[0]) {
 		case HEAD_OF_QUEUE_TAG:
@@ -3236,6 +3238,15 @@ lpfc_queuecommand_lck(struct scsi_cmnd *cmnd, void (*done) (struct scsi_cmnd *))
 		cmnd->result = err;
 		goto out_fail_command;
 	}
+	/*
+	 * Do not let the mid-layer retry I/O too fast. If an I/O is retried
+	 * without waiting a bit then indicate that the device is busy.
+	 */
+	if (cmnd->retries &&
+	    time_before(jiffies, (cmnd->jiffies_at_alloc +
+				  msecs_to_jiffies(LPFC_RETRY_PAUSE *
+						   cmnd->retries))))
+		return SCSI_MLQUEUE_DEVICE_BUSY;
 	ndlp = rdata->pnode;
 
 	if ((scsi_get_prot_op(cmnd) != SCSI_PROT_NORMAL) &&
diff --git a/drivers/scsi/lpfc/lpfc_scsi.h b/drivers/scsi/lpfc/lpfc_scsi.h
index ce645b20a6ad..9075a08cf781 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.h
+++ b/drivers/scsi/lpfc/lpfc_scsi.h
@@ -21,6 +21,7 @@
 #include <asm/byteorder.h>
 
 struct lpfc_hba;
+#define LPFC_FCP_CDB_LEN 16
 
 #define list_remove_head(list, entry, type, member)		\
 	do {							\
@@ -102,7 +103,7 @@ struct fcp_cmnd {
 #define  WRITE_DATA      0x01	/* Bit 0 */
 #define  READ_DATA       0x02	/* Bit 1 */
 
-	uint8_t fcpCdb[16];	/* SRB cdb field is copied here */
+	uint8_t fcpCdb[LPFC_FCP_CDB_LEN]; /* SRB cdb field is copied here */
 	uint32_t fcpDl;		/* Total transfer length */
 
 };
@@ -153,5 +154,5 @@ struct lpfc_scsi_buf {
 
 #define LPFC_SCSI_DMA_EXT_SIZE 264
 #define LPFC_BPL_SIZE          1024
-
+#define LPFC_RETRY_PAUSE       300
 #define MDAC_DIRECT_CMD                  0x22
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 4d4104f38c98..23a27592388c 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -89,15 +89,20 @@ lpfc_get_iocb_from_iocbq(struct lpfc_iocbq *iocbq)
 static uint32_t
 lpfc_sli4_wq_put(struct lpfc_queue *q, union lpfc_wqe *wqe)
 {
-	union lpfc_wqe *temp_wqe = q->qe[q->host_index].wqe;
+	union lpfc_wqe *temp_wqe;
 	struct lpfc_register doorbell;
 	uint32_t host_index;
 
+	/* sanity check on queue memory */
+	if (unlikely(!q))
+		return -ENOMEM;
+	temp_wqe = q->qe[q->host_index].wqe;
+
 	/* If the host has not yet processed the next entry then we are done */
 	if (((q->host_index + 1) % q->entry_count) == q->hba_index)
 		return -ENOMEM;
 	/* set consumption flag every once in a while */
-	if (!((q->host_index + 1) % LPFC_RELEASE_NOTIFICATION_INTERVAL))
+	if (!((q->host_index + 1) % q->entry_repost))
 		bf_set(wqe_wqec, &wqe->generic.wqe_com, 1);
 	if (q->phba->sli3_options & LPFC_SLI4_PHWQ_ENABLED)
 		bf_set(wqe_wqid, &wqe->generic.wqe_com, q->queue_id);
@@ -134,6 +139,10 @@ lpfc_sli4_wq_release(struct lpfc_queue *q, uint32_t index)
 {
 	uint32_t released = 0;
 
+	/* sanity check on queue memory */
+	if (unlikely(!q))
+		return 0;
+
 	if (q->hba_index == index)
 		return 0;
 	do {
@@ -158,10 +167,15 @@ lpfc_sli4_wq_release(struct lpfc_queue *q, uint32_t index)
 static uint32_t
 lpfc_sli4_mq_put(struct lpfc_queue *q, struct lpfc_mqe *mqe)
 {
-	struct lpfc_mqe *temp_mqe = q->qe[q->host_index].mqe;
+	struct lpfc_mqe *temp_mqe;
 	struct lpfc_register doorbell;
 	uint32_t host_index;
 
+	/* sanity check on queue memory */
+	if (unlikely(!q))
+		return -ENOMEM;
+	temp_mqe = q->qe[q->host_index].mqe;
+
 	/* If the host has not yet processed the next entry then we are done */
 	if (((q->host_index + 1) % q->entry_count) == q->hba_index)
 		return -ENOMEM;
@@ -195,6 +209,10 @@ lpfc_sli4_mq_put(struct lpfc_queue *q, struct lpfc_mqe *mqe)
 static uint32_t
 lpfc_sli4_mq_release(struct lpfc_queue *q)
 {
+	/* sanity check on queue memory */
+	if (unlikely(!q))
+		return 0;
+
 	/* Clear the mailbox pointer for completion */
 	q->phba->mbox = NULL;
 	q->hba_index = ((q->hba_index + 1) % q->entry_count);
@@ -213,7 +231,12 @@ lpfc_sli4_mq_release(struct lpfc_queue *q)
 static struct lpfc_eqe *
 lpfc_sli4_eq_get(struct lpfc_queue *q)
 {
-	struct lpfc_eqe *eqe = q->qe[q->hba_index].eqe;
+	struct lpfc_eqe *eqe;
+
+	/* sanity check on queue memory */
+	if (unlikely(!q))
+		return NULL;
+	eqe = q->qe[q->hba_index].eqe;
 
 	/* If the next EQE is not valid then we are done */
 	if (!bf_get_le32(lpfc_eqe_valid, eqe))
@@ -248,6 +271,10 @@ lpfc_sli4_eq_release(struct lpfc_queue *q, bool arm)
 	struct lpfc_eqe *temp_eqe;
 	struct lpfc_register doorbell;
 
+	/* sanity check on queue memory */
+	if (unlikely(!q))
+		return 0;
+
 	/* while there are valid entries */
 	while (q->hba_index != q->host_index) {
 		temp_eqe = q->qe[q->host_index].eqe;
@@ -288,6 +315,10 @@ lpfc_sli4_cq_get(struct lpfc_queue *q)
 {
 	struct lpfc_cqe *cqe;
 
+	/* sanity check on queue memory */
+	if (unlikely(!q))
+		return NULL;
+
 	/* If the next CQE is not valid then we are done */
 	if (!bf_get_le32(lpfc_cqe_valid, q->qe[q->hba_index].cqe))
 		return NULL;
@@ -322,6 +353,9 @@ lpfc_sli4_cq_release(struct lpfc_queue *q, bool arm)
 	struct lpfc_cqe *temp_qe;
 	struct lpfc_register doorbell;
 
+	/* sanity check on queue memory */
+	if (unlikely(!q))
+		return 0;
 	/* while there are valid entries */
 	while (q->hba_index != q->host_index) {
 		temp_qe = q->qe[q->host_index].cqe;
@@ -359,11 +393,17 @@ static int
 lpfc_sli4_rq_put(struct lpfc_queue *hq, struct lpfc_queue *dq,
 		 struct lpfc_rqe *hrqe, struct lpfc_rqe *drqe)
 {
-	struct lpfc_rqe *temp_hrqe = hq->qe[hq->host_index].rqe;
-	struct lpfc_rqe *temp_drqe = dq->qe[dq->host_index].rqe;
+	struct lpfc_rqe *temp_hrqe;
+	struct lpfc_rqe *temp_drqe;
 	struct lpfc_register doorbell;
 	int put_index = hq->host_index;
 
+	/* sanity check on queue memory */
+	if (unlikely(!hq) || unlikely(!dq))
+		return -ENOMEM;
+	temp_hrqe = hq->qe[hq->host_index].rqe;
+	temp_drqe = dq->qe[dq->host_index].rqe;
+
 	if (hq->type != LPFC_HRQ || dq->type != LPFC_DRQ)
 		return -EINVAL;
 	if (hq->host_index != dq->host_index)
@@ -402,6 +442,10 @@ lpfc_sli4_rq_put(struct lpfc_queue *hq, struct lpfc_queue *dq,
 static uint32_t
 lpfc_sli4_rq_release(struct lpfc_queue *hq, struct lpfc_queue *dq)
 {
+	/* sanity check on queue memory */
+	if (unlikely(!hq) || unlikely(!dq))
+		return 0;
+
 	if ((hq->type != LPFC_HRQ) || (dq->type != LPFC_DRQ))
 		return 0;
 	hq->hba_index = ((hq->hba_index + 1) % hq->entry_count);
@@ -3575,8 +3619,8 @@ lpfc_sli_brdready(struct lpfc_hba *phba, uint32_t mask)
  * lpfc_reset_barrier - Make HBA ready for HBA reset
  * @phba: Pointer to HBA context object.
  *
- * This function is called before resetting an HBA. This
- * function requests HBA to quiesce DMAs before a reset.
+ * This function is called before resetting an HBA. This function is called
+ * with hbalock held and requests HBA to quiesce DMAs before a reset.
  **/
 void lpfc_reset_barrier(struct lpfc_hba *phba)
 {
@@ -3851,7 +3895,6 @@ lpfc_sli4_brdreset(struct lpfc_hba *phba)
 {
 	struct lpfc_sli *psli = &phba->sli;
 	uint16_t cfg_value;
-	uint8_t qindx;
 
 	/* Reset HBA */
 	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
@@ -3867,19 +3910,6 @@ lpfc_sli4_brdreset(struct lpfc_hba *phba)
 	spin_lock_irq(&phba->hbalock);
 	psli->sli_flag &= ~(LPFC_PROCESS_LA);
 	phba->fcf.fcf_flag = 0;
-	/* Clean up the child queue list for the CQs */
-	list_del_init(&phba->sli4_hba.mbx_wq->list);
-	list_del_init(&phba->sli4_hba.els_wq->list);
-	list_del_init(&phba->sli4_hba.hdr_rq->list);
-	list_del_init(&phba->sli4_hba.dat_rq->list);
-	list_del_init(&phba->sli4_hba.mbx_cq->list);
-	list_del_init(&phba->sli4_hba.els_cq->list);
-	for (qindx = 0; qindx < phba->cfg_fcp_wq_count; qindx++)
-		list_del_init(&phba->sli4_hba.fcp_wq[qindx]->list);
-	qindx = 0;
-	do
-		list_del_init(&phba->sli4_hba.fcp_cq[qindx]->list);
-	while (++qindx < phba->cfg_fcp_eq_count);
 	spin_unlock_irq(&phba->hbalock);
 
 	/* Now physically reset the device */
@@ -3892,6 +3922,7 @@ lpfc_sli4_brdreset(struct lpfc_hba *phba)
 			      ~(PCI_COMMAND_PARITY | PCI_COMMAND_SERR)));
 
 	/* Perform FCoE PCI function reset */
+	lpfc_sli4_queue_destroy(phba);
 	lpfc_pci_function_reset(phba);
 
 	/* Restore PCI cmd register */
@@ -4339,6 +4370,11 @@ lpfc_sli_config_port(struct lpfc_hba *phba, int sli_mode)
 			phba->sli.sli_flag &= ~LPFC_SLI_ASYNC_MBX_BLK;
 			spin_unlock_irq(&phba->hbalock);
 			done = 1;
+
+			if ((pmb->u.mb.un.varCfgPort.casabt == 1) &&
+			    (pmb->u.mb.un.varCfgPort.gasabt == 0))
+				lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
+					"3110 Port did not grant ASABT\n");
 		}
 	}
 	if (!done) {
@@ -4551,9 +4587,9 @@ lpfc_sli_hba_setup_error:
  * data structure.
  **/
 static int
-lpfc_sli4_read_fcoe_params(struct lpfc_hba *phba,
-		LPFC_MBOXQ_t *mboxq)
+lpfc_sli4_read_fcoe_params(struct lpfc_hba *phba)
 {
+	LPFC_MBOXQ_t *mboxq;
 	struct lpfc_dmabuf *mp;
 	struct lpfc_mqe *mqe;
 	uint32_t data_length;
@@ -4565,10 +4601,16 @@ lpfc_sli4_read_fcoe_params(struct lpfc_hba *phba,
 	phba->fc_map[1] = LPFC_FCOE_FCF_MAP1;
 	phba->fc_map[2] = LPFC_FCOE_FCF_MAP2;
 
-	mqe = &mboxq->u.mqe;
-	if (lpfc_dump_fcoe_param(phba, mboxq))
+	mboxq = (LPFC_MBOXQ_t *)mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!mboxq)
 		return -ENOMEM;
 
+	mqe = &mboxq->u.mqe;
+	if (lpfc_sli4_dump_cfg_rg23(phba, mboxq)) {
+		rc = -ENOMEM;
+		goto out_free_mboxq;
+	}
+
 	mp = (struct lpfc_dmabuf *) mboxq->context1;
 	rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_POLL);
 
@@ -4596,19 +4638,25 @@ lpfc_sli4_read_fcoe_params(struct lpfc_hba *phba,
 	if (rc) {
 		lpfc_mbuf_free(phba, mp->virt, mp->phys);
 		kfree(mp);
-		return -EIO;
+		rc = -EIO;
+		goto out_free_mboxq;
 	}
 	data_length = mqe->un.mb_words[5];
 	if (data_length > DMP_RGN23_SIZE) {
 		lpfc_mbuf_free(phba, mp->virt, mp->phys);
 		kfree(mp);
-		return -EIO;
+		rc = -EIO;
+		goto out_free_mboxq;
 	}
 
 	lpfc_parse_fcoe_conf(phba, mp->virt, data_length);
 	lpfc_mbuf_free(phba, mp->virt, mp->phys);
 	kfree(mp);
-	return 0;
+	rc = 0;
+
+out_free_mboxq:
+	mempool_free(mboxq, phba->mbox_mem_pool);
+	return rc;
 }
 
 /**
@@ -4706,7 +4754,6 @@ static int
 lpfc_sli4_retrieve_pport_name(struct lpfc_hba *phba)
 {
 	LPFC_MBOXQ_t *mboxq;
-	struct lpfc_mbx_read_config *rd_config;
 	struct lpfc_mbx_get_cntl_attributes *mbx_cntl_attr;
 	struct lpfc_controller_attribute *cntl_attr;
 	struct lpfc_mbx_get_port_name *get_port_name;
@@ -4724,33 +4771,11 @@ lpfc_sli4_retrieve_pport_name(struct lpfc_hba *phba)
 	mboxq = (LPFC_MBOXQ_t *)mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
 	if (!mboxq)
 		return -ENOMEM;
-
 	/* obtain link type and link number via READ_CONFIG */
-	lpfc_read_config(phba, mboxq);
-	rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_POLL);
-	if (rc == MBX_SUCCESS) {
-		rd_config = &mboxq->u.mqe.un.rd_config;
-		if (bf_get(lpfc_mbx_rd_conf_lnk_ldv, rd_config)) {
-			phba->sli4_hba.lnk_info.lnk_dv = LPFC_LNK_DAT_VAL;
-			phba->sli4_hba.lnk_info.lnk_tp =
-				bf_get(lpfc_mbx_rd_conf_lnk_type, rd_config);
-			phba->sli4_hba.lnk_info.lnk_no =
-				bf_get(lpfc_mbx_rd_conf_lnk_numb, rd_config);
-			lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
-					"3081 lnk_type:%d, lnk_numb:%d\n",
-					phba->sli4_hba.lnk_info.lnk_tp,
-					phba->sli4_hba.lnk_info.lnk_no);
-			goto retrieve_ppname;
-		} else
-			lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
-					"3082 Mailbox (x%x) returned ldv:x0\n",
-					bf_get(lpfc_mqe_command,
-					       &mboxq->u.mqe));
-	} else
-		lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
-				"3083 Mailbox (x%x) failed, status:x%x\n",
-				bf_get(lpfc_mqe_command, &mboxq->u.mqe),
-				bf_get(lpfc_mqe_status, &mboxq->u.mqe));
+	phba->sli4_hba.lnk_info.lnk_dv = LPFC_LNK_DAT_INVAL;
+	lpfc_sli4_read_config(phba);
+	if (phba->sli4_hba.lnk_info.lnk_dv == LPFC_LNK_DAT_VAL)
+		goto retrieve_ppname;
 
 	/* obtain link type and link number via COMMON_GET_CNTL_ATTRIBUTES */
 	reqlen = sizeof(struct lpfc_mbx_get_cntl_attributes);
@@ -4875,14 +4900,19 @@ lpfc_sli4_arm_cqeq_intr(struct lpfc_hba *phba)
 	lpfc_sli4_cq_release(phba->sli4_hba.mbx_cq, LPFC_QUEUE_REARM);
 	lpfc_sli4_cq_release(phba->sli4_hba.els_cq, LPFC_QUEUE_REARM);
 	fcp_eqidx = 0;
-	do
-		lpfc_sli4_cq_release(phba->sli4_hba.fcp_cq[fcp_eqidx],
-				     LPFC_QUEUE_REARM);
-	while (++fcp_eqidx < phba->cfg_fcp_eq_count);
+	if (phba->sli4_hba.fcp_cq) {
+		do
+			lpfc_sli4_cq_release(phba->sli4_hba.fcp_cq[fcp_eqidx],
+					     LPFC_QUEUE_REARM);
+		while (++fcp_eqidx < phba->cfg_fcp_eq_count);
+	}
 	lpfc_sli4_eq_release(phba->sli4_hba.sp_eq, LPFC_QUEUE_REARM);
-	for (fcp_eqidx = 0; fcp_eqidx < phba->cfg_fcp_eq_count; fcp_eqidx++)
-		lpfc_sli4_eq_release(phba->sli4_hba.fp_eq[fcp_eqidx],
-				     LPFC_QUEUE_REARM);
+	if (phba->sli4_hba.fp_eq) {
+		for (fcp_eqidx = 0; fcp_eqidx < phba->cfg_fcp_eq_count;
+		     fcp_eqidx++)
+			lpfc_sli4_eq_release(phba->sli4_hba.fp_eq[fcp_eqidx],
+					     LPFC_QUEUE_REARM);
+	}
 }
 
 /**
@@ -5457,6 +5487,8 @@ lpfc_sli4_alloc_resource_identifiers(struct lpfc_hba *phba)
 	uint16_t count, base;
 	unsigned long longs;
 
+	if (!phba->sli4_hba.rpi_hdrs_in_use)
+		phba->sli4_hba.next_rpi = phba->sli4_hba.max_cfg_param.max_rpi;
 	if (phba->sli4_hba.extents_in_use) {
 		/*
 		 * The port supports resource extents. The XRI, VPI, VFI, RPI
@@ -5538,9 +5570,10 @@ lpfc_sli4_alloc_resource_identifiers(struct lpfc_hba *phba)
 		 * need any action - just exit.
 		 */
 		if (bf_get(lpfc_idx_rsrc_rdy, &phba->sli4_hba.sli4_flags) ==
-		    LPFC_IDX_RSRC_RDY)
-			return 0;
-
+		    LPFC_IDX_RSRC_RDY) {
+			lpfc_sli4_dealloc_resource_identifiers(phba);
+			lpfc_sli4_remove_rpis(phba);
+		}
 		/* RPIs. */
 		count = phba->sli4_hba.max_cfg_param.max_rpi;
 		base = phba->sli4_hba.max_cfg_param.rpi_base;
@@ -5880,14 +5913,6 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 	if (!mboxq)
 		return -ENOMEM;
 
-	/*
-	 * Continue initialization with default values even if driver failed
-	 * to read FCoE param config regions
-	 */
-	if (lpfc_sli4_read_fcoe_params(phba, mboxq))
-		lpfc_printf_log(phba, KERN_WARNING, LOG_MBOX | LOG_INIT,
-			"2570 Failed to read FCoE parameters\n");
-
 	/* Issue READ_REV to collect vpd and FW information. */
 	vpd_size = SLI4_PAGE_SIZE;
 	vpd = kzalloc(vpd_size, GFP_KERNEL);
@@ -5925,6 +5950,16 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 	}
 
 	/*
+	 * Continue initialization with default values even if driver failed
+	 * to read FCoE param config regions, only read parameters if the
+	 * board is FCoE
+	 */
+	if (phba->hba_flag & HBA_FCOE_MODE &&
+	    lpfc_sli4_read_fcoe_params(phba))
+		lpfc_printf_log(phba, KERN_WARNING, LOG_MBOX | LOG_INIT,
+			"2570 Failed to read FCoE parameters\n");
+
+	/*
 	 * Retrieve sli4 device physical port name, failure of doing it
 	 * is considered as non-fatal.
 	 */
@@ -6044,6 +6079,8 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 				"rc = x%x\n", rc);
 		goto out_free_mbox;
 	}
+	/* update physical xri mappings in the scsi buffers */
+	lpfc_scsi_buf_update(phba);
 
 	/* Read the port's service parameters. */
 	rc = lpfc_read_sparam(phba, mboxq, vport->vpi);
@@ -6205,7 +6242,11 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 		rc = 0;
 		phba->fcf.fcfi = bf_get(lpfc_reg_fcfi_fcfi,
 					&mboxq->u.mqe.un.reg_fcfi);
+
+		/* Check if the port is configured to be disabled */
+		lpfc_sli_read_link_ste(phba);
 	}
+
 	/*
 	 * The port is ready, set the host's link state to LINK_DOWN
 	 * in preparation for link interrupts.
@@ -6213,10 +6254,25 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 	spin_lock_irq(&phba->hbalock);
 	phba->link_state = LPFC_LINK_DOWN;
 	spin_unlock_irq(&phba->hbalock);
-	if (phba->cfg_suppress_link_up == LPFC_INITIALIZE_LINK) {
-		rc = phba->lpfc_hba_init_link(phba, MBX_NOWAIT);
-		if (rc)
+	if (!(phba->hba_flag & HBA_FCOE_MODE) &&
+	    (phba->hba_flag & LINK_DISABLED)) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT | LOG_SLI,
+				"3103 Adapter Link is disabled.\n");
+		lpfc_down_link(phba, mboxq);
+		rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_POLL);
+		if (rc != MBX_SUCCESS) {
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT | LOG_SLI,
+					"3104 Adapter failed to issue "
+					"DOWN_LINK mbox cmd, rc:x%x\n", rc);
 			goto out_unset_queue;
+		}
+	} else if (phba->cfg_suppress_link_up == LPFC_INITIALIZE_LINK) {
+		/* don't perform init_link on SLI4 FC port loopback test */
+		if (!(phba->link_flag & LS_LOOPBACK_MODE)) {
+			rc = phba->lpfc_hba_init_link(phba, MBX_NOWAIT);
+			if (rc)
+				goto out_unset_queue;
+		}
 	}
 	mempool_free(mboxq, phba->mbox_mem_pool);
 	return rc;
@@ -7487,6 +7543,7 @@ lpfc_sli4_bpl2sgl(struct lpfc_hba *phba, struct lpfc_iocbq *piocbq,
 	struct ulp_bde64 *bpl = NULL;
 	struct ulp_bde64 bde;
 	struct sli4_sge *sgl  = NULL;
+	struct lpfc_dmabuf *dmabuf;
 	IOCB_t *icmd;
 	int numBdes = 0;
 	int i = 0;
@@ -7505,9 +7562,12 @@ lpfc_sli4_bpl2sgl(struct lpfc_hba *phba, struct lpfc_iocbq *piocbq,
 		 * have not been byteswapped yet so there is no
 		 * need to swap them back.
 		 */
-		bpl  = (struct ulp_bde64 *)
-			((struct lpfc_dmabuf *)piocbq->context3)->virt;
+		if (piocbq->context3)
+			dmabuf = (struct lpfc_dmabuf *)piocbq->context3;
+		else
+			return xritag;
 
+		bpl  = (struct ulp_bde64 *)dmabuf->virt;
 		if (!bpl)
 			return xritag;
 
@@ -7616,6 +7676,8 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 	int numBdes, i;
 	struct ulp_bde64 bde;
 	struct lpfc_nodelist *ndlp;
+	uint32_t *pcmd;
+	uint32_t if_type;
 
 	fip = phba->hba_flag & HBA_FIP_SUPPORT;
 	/* The fcp commands will set command type */
@@ -7669,6 +7731,7 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 				iocbq->iocb.ulpCommand);
 			return IOCB_ERROR;
 		}
+
 		wqe->els_req.payload_len = xmit_len;
 		/* Els_reguest64 has a TMO */
 		bf_set(wqe_tmo, &wqe->els_req.wqe_com,
@@ -7683,9 +7746,28 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 		bf_set(wqe_ct, &wqe->els_req.wqe_com, ct);
 		bf_set(wqe_pu, &wqe->els_req.wqe_com, 0);
 		/* CCP CCPE PV PRI in word10 were set in the memcpy */
-		if (command_type == ELS_COMMAND_FIP) {
+		if (command_type == ELS_COMMAND_FIP)
 			els_id = ((iocbq->iocb_flag & LPFC_FIP_ELS_ID_MASK)
 					>> LPFC_FIP_ELS_ID_SHIFT);
+		pcmd = (uint32_t *) (((struct lpfc_dmabuf *)
+					iocbq->context2)->virt);
+		if_type = bf_get(lpfc_sli_intf_if_type,
+					&phba->sli4_hba.sli_intf);
+		if (if_type == LPFC_SLI_INTF_IF_TYPE_2) {
+			if (pcmd && (*pcmd == ELS_CMD_FLOGI ||
+				*pcmd == ELS_CMD_SCR ||
+				*pcmd == ELS_CMD_PLOGI)) {
+				bf_set(els_req64_sp, &wqe->els_req, 1);
+				bf_set(els_req64_sid, &wqe->els_req,
+					iocbq->vport->fc_myDID);
+				bf_set(wqe_ct, &wqe->els_req.wqe_com, 1);
+				bf_set(wqe_ctxt_tag, &wqe->els_req.wqe_com,
+					phba->vpi_ids[phba->pport->vpi]);
+			} else if (iocbq->context1) {
+				bf_set(wqe_ct, &wqe->els_req.wqe_com, 0);
+				bf_set(wqe_ctxt_tag, &wqe->els_req.wqe_com,
+					phba->sli4_hba.rpi_ids[ndlp->nlp_rpi]);
+			}
 		}
 		bf_set(wqe_temp_rpi, &wqe->els_req.wqe_com,
 		       phba->sli4_hba.rpi_ids[ndlp->nlp_rpi]);
@@ -7704,6 +7786,8 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 		/* The entire sequence is transmitted for this IOCB */
 		xmit_len = total_len;
 		cmnd = CMD_XMIT_SEQUENCE64_CR;
+		if (phba->link_flag & LS_LOOPBACK_MODE)
+			bf_set(wqe_xo, &wqe->xmit_sequence.wge_ctl, 1);
 	case CMD_XMIT_SEQUENCE64_CR:
 		/* word3 iocb=io_tag32 wqe=reserved */
 		wqe->xmit_sequence.rsvd3 = 0;
@@ -7846,6 +7930,16 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 		bf_set(wqe_ebde_cnt, &wqe->xmit_els_rsp.wqe_com, 0);
 		bf_set(wqe_rsp_temp_rpi, &wqe->xmit_els_rsp,
 		       phba->sli4_hba.rpi_ids[ndlp->nlp_rpi]);
+		pcmd = (uint32_t *) (((struct lpfc_dmabuf *)
+					iocbq->context2)->virt);
+		if (phba->fc_topology == LPFC_TOPOLOGY_LOOP) {
+				bf_set(els_req64_sp, &wqe->els_req, 1);
+				bf_set(els_req64_sid, &wqe->els_req,
+					iocbq->vport->fc_myDID);
+				bf_set(wqe_ct, &wqe->els_req.wqe_com, 1);
+				bf_set(wqe_ctxt_tag, &wqe->els_req.wqe_com,
+					phba->vpi_ids[phba->pport->vpi]);
+		}
 		command_type = OTHER_COMMAND;
 		break;
 	case CMD_CLOSE_XRI_CN:
@@ -8037,6 +8131,8 @@ __lpfc_sli_issue_iocb_s4(struct lpfc_hba *phba, uint32_t ring_number,
 		 */
 		if (piocb->iocb_flag & LPFC_IO_FCP)
 			piocb->fcp_wqidx = lpfc_sli4_scmd_to_wqidx_distr(phba);
+		if (unlikely(!phba->sli4_hba.fcp_wq))
+			return IOCB_ERROR;
 		if (lpfc_sli4_wq_put(phba->sli4_hba.fcp_wq[piocb->fcp_wqidx],
 				     &wqe))
 			return IOCB_ERROR;
@@ -8173,6 +8269,137 @@ lpfc_extra_ring_setup( struct lpfc_hba *phba)
 	return 0;
 }
 
+/* lpfc_sli_abts_recover_port - Recover a port that failed an ABTS.
+ * @vport: pointer to virtual port object.
+ * @ndlp: nodelist pointer for the impacted rport.
+ *
+ * The driver calls this routine in response to a XRI ABORT CQE
+ * event from the port.  In this event, the driver is required to
+ * recover its login to the rport even though its login may be valid
+ * from the driver's perspective.  The failed ABTS notice from the
+ * port indicates the rport is not responding.
+ */
+static void
+lpfc_sli_abts_recover_port(struct lpfc_vport *vport,
+			   struct lpfc_nodelist *ndlp)
+{
+	struct Scsi_Host *shost;
+	struct lpfc_hba *phba;
+	unsigned long flags = 0;
+
+	shost = lpfc_shost_from_vport(vport);
+	phba = vport->phba;
+	if (ndlp->nlp_state != NLP_STE_MAPPED_NODE) {
+		lpfc_printf_log(phba, KERN_INFO,
+			LOG_SLI, "3093 No rport recovery needed. "
+			"rport in state 0x%x\n",
+			ndlp->nlp_state);
+		return;
+	}
+	lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+			"3094 Start rport recovery on shost id 0x%x "
+			"fc_id 0x%06x vpi 0x%x rpi 0x%x state 0x%x "
+			"flags 0x%x\n",
+			shost->host_no, ndlp->nlp_DID,
+			vport->vpi, ndlp->nlp_rpi, ndlp->nlp_state,
+			ndlp->nlp_flag);
+	/*
+	 * The rport is not responding.  Don't attempt ADISC recovery.
+	 * Remove the FCP-2 flag to force a PLOGI.
+	 */
+	spin_lock_irqsave(shost->host_lock, flags);
+	ndlp->nlp_fcp_info &= ~NLP_FCP_2_DEVICE;
+	spin_unlock_irqrestore(shost->host_lock, flags);
+	lpfc_disc_state_machine(vport, ndlp, NULL,
+				NLP_EVT_DEVICE_RECOVERY);
+	lpfc_cancel_retry_delay_tmo(vport, ndlp);
+	spin_lock_irqsave(shost->host_lock, flags);
+	ndlp->nlp_flag |= NLP_NPR_2B_DISC;
+	spin_unlock_irqrestore(shost->host_lock, flags);
+	lpfc_disc_start(vport);
+}
+
+/* lpfc_sli_abts_err_handler - handle a failed ABTS request from an SLI3 port.
+ * @phba: Pointer to HBA context object.
+ * @iocbq: Pointer to iocb object.
+ *
+ * The async_event handler calls this routine when it receives
+ * an ASYNC_STATUS_CN event from the port.  The port generates
+ * this event when an Abort Sequence request to an rport fails
+ * twice in succession.  The abort could be originated by the
+ * driver or by the port.  The ABTS could have been for an ELS
+ * or FCP IO.  The port only generates this event when an ABTS
+ * fails to complete after one retry.
+ */
+static void
+lpfc_sli_abts_err_handler(struct lpfc_hba *phba,
+			  struct lpfc_iocbq *iocbq)
+{
+	struct lpfc_nodelist *ndlp = NULL;
+	uint16_t rpi = 0, vpi = 0;
+	struct lpfc_vport *vport = NULL;
+
+	/* The rpi in the ulpContext is vport-sensitive. */
+	vpi = iocbq->iocb.un.asyncstat.sub_ctxt_tag;
+	rpi = iocbq->iocb.ulpContext;
+
+	lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
+			"3092 Port generated ABTS async event "
+			"on vpi %d rpi %d status 0x%x\n",
+			vpi, rpi, iocbq->iocb.ulpStatus);
+
+	vport = lpfc_find_vport_by_vpid(phba, vpi);
+	if (!vport)
+		goto err_exit;
+	ndlp = lpfc_findnode_rpi(vport, rpi);
+	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp))
+		goto err_exit;
+
+	if (iocbq->iocb.ulpStatus == IOSTAT_LOCAL_REJECT)
+		lpfc_sli_abts_recover_port(vport, ndlp);
+	return;
+
+ err_exit:
+	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+			"3095 Event Context not found, no "
+			"action on vpi %d rpi %d status 0x%x, reason 0x%x\n",
+			iocbq->iocb.ulpContext, iocbq->iocb.ulpStatus,
+			vpi, rpi);
+}
+
+/* lpfc_sli4_abts_err_handler - handle a failed ABTS request from an SLI4 port.
+ * @phba: pointer to HBA context object.
+ * @ndlp: nodelist pointer for the impacted rport.
+ * @axri: pointer to the wcqe containing the failed exchange.
+ *
+ * The driver calls this routine when it receives an ABORT_XRI_FCP CQE from the
+ * port.  The port generates this event when an abort exchange request to an
+ * rport fails twice in succession with no reply.  The abort could be originated
+ * by the driver or by the port.  The ABTS could have been for an ELS or FCP IO.
+ */
+void
+lpfc_sli4_abts_err_handler(struct lpfc_hba *phba,
+			   struct lpfc_nodelist *ndlp,
+			   struct sli4_wcqe_xri_aborted *axri)
+{
+	struct lpfc_vport *vport;
+
+	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp))
+		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+				"3115 Node Context not found, driver "
+				"ignoring abts err event\n");
+	vport = ndlp->vport;
+	lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
+			"3116 Port generated FCP XRI ABORT event on "
+			"vpi %d rpi %d xri x%x status 0x%x\n",
+			ndlp->vport->vpi, ndlp->nlp_rpi,
+			bf_get(lpfc_wcqe_xa_xri, axri),
+			bf_get(lpfc_wcqe_xa_status, axri));
+
+	if (bf_get(lpfc_wcqe_xa_status, axri) == IOSTAT_LOCAL_REJECT)
+		lpfc_sli_abts_recover_port(vport, ndlp);
+}
+
 /**
  * lpfc_sli_async_event_handler - ASYNC iocb handler function
  * @phba: Pointer to HBA context object.
@@ -8192,63 +8419,58 @@ lpfc_sli_async_event_handler(struct lpfc_hba * phba,
 {
 	IOCB_t *icmd;
 	uint16_t evt_code;
-	uint16_t temp;
 	struct temp_event temp_event_data;
 	struct Scsi_Host *shost;
 	uint32_t *iocb_w;
 
 	icmd = &iocbq->iocb;
 	evt_code = icmd->un.asyncstat.evt_code;
-	temp = icmd->ulpContext;
 
-	if ((evt_code != ASYNC_TEMP_WARN) &&
-		(evt_code != ASYNC_TEMP_SAFE)) {
+	switch (evt_code) {
+	case ASYNC_TEMP_WARN:
+	case ASYNC_TEMP_SAFE:
+		temp_event_data.data = (uint32_t) icmd->ulpContext;
+		temp_event_data.event_type = FC_REG_TEMPERATURE_EVENT;
+		if (evt_code == ASYNC_TEMP_WARN) {
+			temp_event_data.event_code = LPFC_THRESHOLD_TEMP;
+			lpfc_printf_log(phba, KERN_ERR, LOG_TEMP,
+				"0347 Adapter is very hot, please take "
+				"corrective action. temperature : %d Celsius\n",
+				(uint32_t) icmd->ulpContext);
+		} else {
+			temp_event_data.event_code = LPFC_NORMAL_TEMP;
+			lpfc_printf_log(phba, KERN_ERR, LOG_TEMP,
+				"0340 Adapter temperature is OK now. "
+				"temperature : %d Celsius\n",
+				(uint32_t) icmd->ulpContext);
+		}
+
+		/* Send temperature change event to applications */
+		shost = lpfc_shost_from_vport(phba->pport);
+		fc_host_post_vendor_event(shost, fc_get_event_number(),
+			sizeof(temp_event_data), (char *) &temp_event_data,
+			LPFC_NL_VENDOR_ID);
+		break;
+	case ASYNC_STATUS_CN:
+		lpfc_sli_abts_err_handler(phba, iocbq);
+		break;
+	default:
 		iocb_w = (uint32_t *) icmd;
-		lpfc_printf_log(phba,
-			KERN_ERR,
-			LOG_SLI,
+		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 			"0346 Ring %d handler: unexpected ASYNC_STATUS"
 			" evt_code 0x%x\n"
 			"W0  0x%08x W1  0x%08x W2  0x%08x W3  0x%08x\n"
 			"W4  0x%08x W5  0x%08x W6  0x%08x W7  0x%08x\n"
 			"W8  0x%08x W9  0x%08x W10 0x%08x W11 0x%08x\n"
 			"W12 0x%08x W13 0x%08x W14 0x%08x W15 0x%08x\n",
-			pring->ringno,
-			icmd->un.asyncstat.evt_code,
+			pring->ringno, icmd->un.asyncstat.evt_code,
 			iocb_w[0], iocb_w[1], iocb_w[2], iocb_w[3],
 			iocb_w[4], iocb_w[5], iocb_w[6], iocb_w[7],
 			iocb_w[8], iocb_w[9], iocb_w[10], iocb_w[11],
 			iocb_w[12], iocb_w[13], iocb_w[14], iocb_w[15]);
 
-		return;
-	}
-	temp_event_data.data = (uint32_t)temp;
-	temp_event_data.event_type = FC_REG_TEMPERATURE_EVENT;
-	if (evt_code == ASYNC_TEMP_WARN) {
-		temp_event_data.event_code = LPFC_THRESHOLD_TEMP;
-		lpfc_printf_log(phba,
-				KERN_ERR,
-				LOG_TEMP,
-				"0347 Adapter is very hot, please take "
-				"corrective action. temperature : %d Celsius\n",
-				temp);
-	}
-	if (evt_code == ASYNC_TEMP_SAFE) {
-		temp_event_data.event_code = LPFC_NORMAL_TEMP;
-		lpfc_printf_log(phba,
-				KERN_ERR,
-				LOG_TEMP,
-				"0340 Adapter temperature is OK now. "
-				"temperature : %d Celsius\n",
-				temp);
+		break;
 	}
-
-	/* Send temperature change event to applications */
-	shost = lpfc_shost_from_vport(phba->pport);
-	fc_host_post_vendor_event(shost, fc_get_event_number(),
-		sizeof(temp_event_data), (char *) &temp_event_data,
-		LPFC_NL_VENDOR_ID);
-
 }
 
 
@@ -8823,12 +9045,14 @@ lpfc_sli_abort_els_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 {
 	IOCB_t *irsp = &rspiocb->iocb;
 	uint16_t abort_iotag, abort_context;
-	struct lpfc_iocbq *abort_iocb;
-	struct lpfc_sli_ring *pring = &phba->sli.ring[LPFC_ELS_RING];
-
-	abort_iocb = NULL;
+	struct lpfc_iocbq *abort_iocb = NULL;
 
 	if (irsp->ulpStatus) {
+
+		/*
+		 * Assume that the port already completed and returned, or
+		 * will return the iocb. Just Log the message.
+		 */
 		abort_context = cmdiocb->iocb.un.acxri.abortContextTag;
 		abort_iotag = cmdiocb->iocb.un.acxri.abortIoTag;
 
@@ -8846,68 +9070,15 @@ lpfc_sli_abort_els_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 			 */
 			abort_iocb = phba->sli.iocbq_lookup[abort_context];
 
-		/*
-		 *  If the iocb is not found in Firmware queue the iocb
-		 *  might have completed already. Do not free it again.
-		 */
-		if (irsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
-			if (irsp->un.ulpWord[4] != IOERR_NO_XRI) {
-				spin_unlock_irq(&phba->hbalock);
-				lpfc_sli_release_iocbq(phba, cmdiocb);
-				return;
-			}
-			/* For SLI4 the ulpContext field for abort IOCB
-			 * holds the iotag of the IOCB being aborted so
-			 * the local abort_context needs to be reset to
-			 * match the aborted IOCBs ulpContext.
-			 */
-			if (abort_iocb && phba->sli_rev == LPFC_SLI_REV4)
-				abort_context = abort_iocb->iocb.ulpContext;
-		}
-
 		lpfc_printf_log(phba, KERN_WARNING, LOG_ELS | LOG_SLI,
 				"0327 Cannot abort els iocb %p "
 				"with tag %x context %x, abort status %x, "
 				"abort code %x\n",
 				abort_iocb, abort_iotag, abort_context,
 				irsp->ulpStatus, irsp->un.ulpWord[4]);
-		/*
-		 * make sure we have the right iocbq before taking it
-		 * off the txcmplq and try to call completion routine.
-		 */
-		if (!abort_iocb ||
-		    abort_iocb->iocb.ulpContext != abort_context ||
-		    (abort_iocb->iocb_flag & LPFC_DRIVER_ABORTED) == 0)
-			spin_unlock_irq(&phba->hbalock);
-		else if (phba->sli_rev < LPFC_SLI_REV4) {
-			/*
-			 * leave the SLI4 aborted command on the txcmplq
-			 * list and the command complete WCQE's XB bit
-			 * will tell whether the SGL (XRI) can be released
-			 * immediately or to the aborted SGL list for the
-			 * following abort XRI from the HBA.
-			 */
-			list_del_init(&abort_iocb->list);
-			if (abort_iocb->iocb_flag & LPFC_IO_ON_Q) {
-				abort_iocb->iocb_flag &= ~LPFC_IO_ON_Q;
-				pring->txcmplq_cnt--;
-			}
 
-			/* Firmware could still be in progress of DMAing
-			 * payload, so don't free data buffer till after
-			 * a hbeat.
-			 */
-			abort_iocb->iocb_flag |= LPFC_DELAY_MEM_FREE;
-			abort_iocb->iocb_flag &= ~LPFC_DRIVER_ABORTED;
-			spin_unlock_irq(&phba->hbalock);
-
-			abort_iocb->iocb.ulpStatus = IOSTAT_LOCAL_REJECT;
-			abort_iocb->iocb.un.ulpWord[4] = IOERR_ABORT_REQUESTED;
-			(abort_iocb->iocb_cmpl)(phba, abort_iocb, abort_iocb);
-		} else
-			spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irq(&phba->hbalock);
 	}
-
 	lpfc_sli_release_iocbq(phba, cmdiocb);
 	return;
 }
@@ -9258,6 +9429,14 @@ void
 lpfc_sli_abort_fcp_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 			struct lpfc_iocbq *rspiocb)
 {
+	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+			"3096 ABORT_XRI_CN completing on xri x%x "
+			"original iotag x%x, abort cmd iotag x%x "
+			"status 0x%x, reason 0x%x\n",
+			cmdiocb->iocb.un.acxri.abortContextTag,
+			cmdiocb->iocb.un.acxri.abortIoTag,
+			cmdiocb->iotag, rspiocb->iocb.ulpStatus,
+			rspiocb->iocb.un.ulpWord[4]);
 	lpfc_sli_release_iocbq(phba, cmdiocb);
 	return;
 }
@@ -9771,7 +9950,7 @@ lpfc_sli4_eratt_read(struct lpfc_hba *phba)
 			phba->work_status[1] =
 				readl(phba->sli4_hba.u.if_type2.ERR2regaddr);
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-					"2885 Port Error Detected: "
+					"2885 Port Status Event: "
 					"port status reg 0x%x, "
 					"port smphr reg 0x%x, "
 					"error 1=0x%x, error 2=0x%x\n",
@@ -10777,6 +10956,9 @@ static void
 lpfc_sli4_sp_handle_rel_wcqe(struct lpfc_hba *phba,
 			     struct lpfc_wcqe_release *wcqe)
 {
+	/* sanity check on queue memory */
+	if (unlikely(!phba->sli4_hba.els_wq))
+		return;
 	/* Check for the slow-path ELS work queue */
 	if (bf_get(lpfc_wcqe_r_wq_id, wcqe) == phba->sli4_hba.els_wq->queue_id)
 		lpfc_sli4_wq_release(phba->sli4_hba.els_wq,
@@ -10866,6 +11048,10 @@ lpfc_sli4_sp_handle_rcqe(struct lpfc_hba *phba, struct lpfc_rcqe *rcqe)
 	uint32_t status, rq_id;
 	unsigned long iflags;
 
+	/* sanity check on queue memory */
+	if (unlikely(!hrq) || unlikely(!drq))
+		return workposted;
+
 	if (bf_get(lpfc_cqe_code, rcqe) == CQE_CODE_RECEIVE_V1)
 		rq_id = bf_get(lpfc_rcqe_rq_id_v1, rcqe);
 	else
@@ -11000,6 +11186,9 @@ lpfc_sli4_sp_handle_eqe(struct lpfc_hba *phba, struct lpfc_eqe *eqe)
 
 	/* Search for completion queue pointer matching this cqid */
 	speq = phba->sli4_hba.sp_eq;
+	/* sanity check on queue memory */
+	if (unlikely(!speq))
+		return;
 	list_for_each_entry(childq, &speq->child_list, list) {
 		if (childq->queue_id == cqid) {
 			cq = childq;
@@ -11241,12 +11430,18 @@ lpfc_sli4_fp_handle_eqe(struct lpfc_hba *phba, struct lpfc_eqe *eqe,
 		return;
 	}
 
+	if (unlikely(!phba->sli4_hba.fcp_cq)) {
+		lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
+				"3146 Fast-path completion queues "
+				"does not exist\n");
+		return;
+	}
 	cq = phba->sli4_hba.fcp_cq[fcp_cqidx];
 	if (unlikely(!cq)) {
 		if (phba->sli.sli_flag & LPFC_SLI_ACTIVE)
 			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 					"0367 Fast-path completion queue "
-					"does not exist\n");
+					"(%d) does not exist\n", fcp_cqidx);
 		return;
 	}
 
@@ -11417,6 +11612,8 @@ lpfc_sli4_fp_intr_handler(int irq, void *dev_id)
 
 	/* Get to the EQ struct associated with this vector */
 	fpeq = phba->sli4_hba.fp_eq[fcp_eqidx];
+	if (unlikely(!fpeq))
+		return IRQ_NONE;
 
 	/* Check device state for handling interrupt */
 	if (unlikely(lpfc_intr_state_check(phba))) {
@@ -11635,6 +11832,9 @@ lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint16_t imax)
 	uint16_t dmult;
 	uint32_t hw_page_size = phba->sli4_hba.pc_sli4_params.if_page_sz;
 
+	/* sanity check on queue memory */
+	if (!eq)
+		return -ENODEV;
 	if (!phba->sli4_hba.pc_sli4_params.supported)
 		hw_page_size = SLI4_PAGE_SIZE;
 
@@ -11751,6 +11951,9 @@ lpfc_cq_create(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	union lpfc_sli4_cfg_shdr *shdr;
 	uint32_t hw_page_size = phba->sli4_hba.pc_sli4_params.if_page_sz;
 
+	/* sanity check on queue memory */
+	if (!cq || !eq)
+		return -ENODEV;
 	if (!phba->sli4_hba.pc_sli4_params.supported)
 		hw_page_size = SLI4_PAGE_SIZE;
 
@@ -11933,6 +12136,9 @@ lpfc_mq_create(struct lpfc_hba *phba, struct lpfc_queue *mq,
 	union lpfc_sli4_cfg_shdr *shdr;
 	uint32_t hw_page_size = phba->sli4_hba.pc_sli4_params.if_page_sz;
 
+	/* sanity check on queue memory */
+	if (!mq || !cq)
+		return -ENODEV;
 	if (!phba->sli4_hba.pc_sli4_params.supported)
 		hw_page_size = SLI4_PAGE_SIZE;
 
@@ -12083,6 +12289,9 @@ lpfc_wq_create(struct lpfc_hba *phba, struct lpfc_queue *wq,
 	uint32_t hw_page_size = phba->sli4_hba.pc_sli4_params.if_page_sz;
 	struct dma_address *page;
 
+	/* sanity check on queue memory */
+	if (!wq || !cq)
+		return -ENODEV;
 	if (!phba->sli4_hba.pc_sli4_params.supported)
 		hw_page_size = SLI4_PAGE_SIZE;
 
@@ -12151,6 +12360,7 @@ lpfc_wq_create(struct lpfc_hba *phba, struct lpfc_queue *wq,
 	wq->subtype = subtype;
 	wq->host_index = 0;
 	wq->hba_index = 0;
+	wq->entry_repost = LPFC_RELEASE_NOTIFICATION_INTERVAL;
 
 	/* link the wq onto the parent cq child list */
 	list_add_tail(&wq->list, &cq->child_list);
@@ -12174,6 +12384,9 @@ lpfc_rq_adjust_repost(struct lpfc_hba *phba, struct lpfc_queue *rq, int qno)
 {
 	uint32_t cnt;
 
+	/* sanity check on queue memory */
+	if (!rq)
+		return;
 	cnt = lpfc_hbq_defs[qno]->entry_count;
 
 	/* Recalc repost for RQs based on buffers initially posted */
@@ -12219,6 +12432,9 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 	union lpfc_sli4_cfg_shdr *shdr;
 	uint32_t hw_page_size = phba->sli4_hba.pc_sli4_params.if_page_sz;
 
+	/* sanity check on queue memory */
+	if (!hrq || !drq || !cq)
+		return -ENODEV;
 	if (!phba->sli4_hba.pc_sli4_params.supported)
 		hw_page_size = SLI4_PAGE_SIZE;
 
@@ -12420,6 +12636,7 @@ lpfc_eq_destroy(struct lpfc_hba *phba, struct lpfc_queue *eq)
 	uint32_t shdr_status, shdr_add_status;
 	union lpfc_sli4_cfg_shdr *shdr;
 
+	/* sanity check on queue memory */
 	if (!eq)
 		return -ENODEV;
 	mbox = mempool_alloc(eq->phba->mbox_mem_pool, GFP_KERNEL);
@@ -12475,6 +12692,7 @@ lpfc_cq_destroy(struct lpfc_hba *phba, struct lpfc_queue *cq)
 	uint32_t shdr_status, shdr_add_status;
 	union lpfc_sli4_cfg_shdr *shdr;
 
+	/* sanity check on queue memory */
 	if (!cq)
 		return -ENODEV;
 	mbox = mempool_alloc(cq->phba->mbox_mem_pool, GFP_KERNEL);
@@ -12528,6 +12746,7 @@ lpfc_mq_destroy(struct lpfc_hba *phba, struct lpfc_queue *mq)
 	uint32_t shdr_status, shdr_add_status;
 	union lpfc_sli4_cfg_shdr *shdr;
 
+	/* sanity check on queue memory */
 	if (!mq)
 		return -ENODEV;
 	mbox = mempool_alloc(mq->phba->mbox_mem_pool, GFP_KERNEL);
@@ -12581,6 +12800,7 @@ lpfc_wq_destroy(struct lpfc_hba *phba, struct lpfc_queue *wq)
 	uint32_t shdr_status, shdr_add_status;
 	union lpfc_sli4_cfg_shdr *shdr;
 
+	/* sanity check on queue memory */
 	if (!wq)
 		return -ENODEV;
 	mbox = mempool_alloc(wq->phba->mbox_mem_pool, GFP_KERNEL);
@@ -12634,6 +12854,7 @@ lpfc_rq_destroy(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 	uint32_t shdr_status, shdr_add_status;
 	union lpfc_sli4_cfg_shdr *shdr;
 
+	/* sanity check on queue memory */
 	if (!hrq || !drq)
 		return -ENODEV;
 	mbox = mempool_alloc(hrq->phba->mbox_mem_pool, GFP_KERNEL);
@@ -15252,45 +15473,42 @@ lpfc_sli4_fcf_dead_failthrough(struct lpfc_hba *phba)
 }
 
 /**
- * lpfc_sli_read_link_ste - Read region 23 to decide if link is disabled.
+ * lpfc_sli_get_config_region23 - Get sli3 port region 23 data.
  * @phba: pointer to lpfc hba data structure.
+ * @rgn23_data: pointer to configure region 23 data.
  *
- * This function read region 23 and parse TLV for port status to
- * decide if the user disaled the port. If the TLV indicates the
- * port is disabled, the hba_flag is set accordingly.
+ * This function gets SLI3 port configure region 23 data through memory dump
+ * mailbox command. When it successfully retrieves data, the size of the data
+ * will be returned, otherwise, 0 will be returned.
  **/
-void
-lpfc_sli_read_link_ste(struct lpfc_hba *phba)
+static uint32_t
+lpfc_sli_get_config_region23(struct lpfc_hba *phba, char *rgn23_data)
 {
 	LPFC_MBOXQ_t *pmb = NULL;
 	MAILBOX_t *mb;
-	uint8_t *rgn23_data = NULL;
-	uint32_t offset = 0, data_size, sub_tlv_len, tlv_offset;
+	uint32_t offset = 0;
 	int rc;
 
+	if (!rgn23_data)
+		return 0;
+
 	pmb = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
 	if (!pmb) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-			"2600 lpfc_sli_read_serdes_param failed to"
-			" allocate mailbox memory\n");
-		goto out;
+				"2600 failed to allocate mailbox memory\n");
+		return 0;
 	}
 	mb = &pmb->u.mb;
 
-	/* Get adapter Region 23 data */
-	rgn23_data = kzalloc(DMP_RGN23_SIZE, GFP_KERNEL);
-	if (!rgn23_data)
-		goto out;
-
 	do {
 		lpfc_dump_mem(phba, pmb, offset, DMP_REGION_23);
 		rc = lpfc_sli_issue_mbox(phba, pmb, MBX_POLL);
 
 		if (rc != MBX_SUCCESS) {
 			lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
-				"2601 lpfc_sli_read_link_ste failed to"
-				" read config region 23 rc 0x%x Status 0x%x\n",
-				rc, mb->mbxStatus);
+					"2601 failed to read config "
+					"region 23, rc 0x%x Status 0x%x\n",
+					rc, mb->mbxStatus);
 			mb->un.varDmp.word_cnt = 0;
 		}
 		/*
@@ -15303,13 +15521,96 @@ lpfc_sli_read_link_ste(struct lpfc_hba *phba)
 			mb->un.varDmp.word_cnt = DMP_RGN23_SIZE - offset;
 
 		lpfc_sli_pcimem_bcopy(((uint8_t *)mb) + DMP_RSP_OFFSET,
-			rgn23_data + offset,
-			mb->un.varDmp.word_cnt);
+				       rgn23_data + offset,
+				       mb->un.varDmp.word_cnt);
 		offset += mb->un.varDmp.word_cnt;
 	} while (mb->un.varDmp.word_cnt && offset < DMP_RGN23_SIZE);
 
-	data_size = offset;
-	offset = 0;
+	mempool_free(pmb, phba->mbox_mem_pool);
+	return offset;
+}
+
+/**
+ * lpfc_sli4_get_config_region23 - Get sli4 port region 23 data.
+ * @phba: pointer to lpfc hba data structure.
+ * @rgn23_data: pointer to configure region 23 data.
+ *
+ * This function gets SLI4 port configure region 23 data through memory dump
+ * mailbox command. When it successfully retrieves data, the size of the data
+ * will be returned, otherwise, 0 will be returned.
+ **/
+static uint32_t
+lpfc_sli4_get_config_region23(struct lpfc_hba *phba, char *rgn23_data)
+{
+	LPFC_MBOXQ_t *mboxq = NULL;
+	struct lpfc_dmabuf *mp = NULL;
+	struct lpfc_mqe *mqe;
+	uint32_t data_length = 0;
+	int rc;
+
+	if (!rgn23_data)
+		return 0;
+
+	mboxq = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!mboxq) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"3105 failed to allocate mailbox memory\n");
+		return 0;
+	}
+
+	if (lpfc_sli4_dump_cfg_rg23(phba, mboxq))
+		goto out;
+	mqe = &mboxq->u.mqe;
+	mp = (struct lpfc_dmabuf *) mboxq->context1;
+	rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_POLL);
+	if (rc)
+		goto out;
+	data_length = mqe->un.mb_words[5];
+	if (data_length == 0)
+		goto out;
+	if (data_length > DMP_RGN23_SIZE) {
+		data_length = 0;
+		goto out;
+	}
+	lpfc_sli_pcimem_bcopy((char *)mp->virt, rgn23_data, data_length);
+out:
+	mempool_free(mboxq, phba->mbox_mem_pool);
+	if (mp) {
+		lpfc_mbuf_free(phba, mp->virt, mp->phys);
+		kfree(mp);
+	}
+	return data_length;
+}
+
+/**
+ * lpfc_sli_read_link_ste - Read region 23 to decide if link is disabled.
+ * @phba: pointer to lpfc hba data structure.
+ *
+ * This function read region 23 and parse TLV for port status to
+ * decide if the user disaled the port. If the TLV indicates the
+ * port is disabled, the hba_flag is set accordingly.
+ **/
+void
+lpfc_sli_read_link_ste(struct lpfc_hba *phba)
+{
+	uint8_t *rgn23_data = NULL;
+	uint32_t if_type, data_size, sub_tlv_len, tlv_offset;
+	uint32_t offset = 0;
+
+	/* Get adapter Region 23 data */
+	rgn23_data = kzalloc(DMP_RGN23_SIZE, GFP_KERNEL);
+	if (!rgn23_data)
+		goto out;
+
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		data_size = lpfc_sli_get_config_region23(phba, rgn23_data);
+	else {
+		if_type = bf_get(lpfc_sli_intf_if_type,
+				 &phba->sli4_hba.sli_intf);
+		if (if_type == LPFC_SLI_INTF_IF_TYPE_0)
+			goto out;
+		data_size = lpfc_sli4_get_config_region23(phba, rgn23_data);
+	}
 
 	if (!data_size)
 		goto out;
@@ -15373,9 +15674,8 @@ lpfc_sli_read_link_ste(struct lpfc_hba *phba)
 			goto out;
 		}
 	}
+
 out:
-	if (pmb)
-		mempool_free(pmb, phba->mbox_mem_pool);
 	kfree(rgn23_data);
 	return;
 }
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index d5cffd8af340..3f266e2c54e0 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -291,7 +291,7 @@ struct lpfc_bmbx {
 #define LPFC_RQE_SIZE		8
 
 #define LPFC_EQE_DEF_COUNT	1024
-#define LPFC_CQE_DEF_COUNT      256
+#define LPFC_CQE_DEF_COUNT      1024
 #define LPFC_WQE_DEF_COUNT      256
 #define LPFC_MQE_DEF_COUNT      16
 #define LPFC_RQE_DEF_COUNT	512
@@ -420,7 +420,16 @@ struct lpfc_sli4_hba {
 			void __iomem *STATUSregaddr;
 			void __iomem *CTRLregaddr;
 			void __iomem *ERR1regaddr;
+#define SLIPORT_ERR1_REG_ERR_CODE_1		0x1
+#define SLIPORT_ERR1_REG_ERR_CODE_2		0x2
 			void __iomem *ERR2regaddr;
+#define SLIPORT_ERR2_REG_FW_RESTART		0x0
+#define SLIPORT_ERR2_REG_FUNC_PROVISON		0x1
+#define SLIPORT_ERR2_REG_FORCED_DUMP		0x2
+#define SLIPORT_ERR2_REG_FAILURE_EQ		0x3
+#define SLIPORT_ERR2_REG_FAILURE_CQ		0x4
+#define SLIPORT_ERR2_REG_FAILURE_BUS		0x5
+#define SLIPORT_ERR2_REG_FAILURE_RQ		0x6
 		} if_type2;
 	} u;
 
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index b0630e37f1ef..dd044d01a07f 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "8.3.27"
+#define LPFC_DRIVER_VERSION "8.3.28"
 #define LPFC_DRIVER_NAME		"lpfc"
 #define LPFC_SP_DRIVER_HANDLER_NAME	"lpfc:sp"
 #define LPFC_FP_DRIVER_HANDLER_NAME	"lpfc:fp"
diff --git a/drivers/scsi/lpfc/lpfc_vport.c b/drivers/scsi/lpfc/lpfc_vport.c
index cff6ca67415c..0fe188e66000 100644
--- a/drivers/scsi/lpfc/lpfc_vport.c
+++ b/drivers/scsi/lpfc/lpfc_vport.c
@@ -774,10 +774,10 @@ lpfc_create_vport_work_array(struct lpfc_hba *phba)
 		return NULL;
 	spin_lock_irq(&phba->hbalock);
 	list_for_each_entry(port_iterator, &phba->port_list, listentry) {
+		if (port_iterator->load_flag & FC_UNLOADING)
+			continue;
 		if (!scsi_host_get(lpfc_shost_from_vport(port_iterator))) {
-			if (!(port_iterator->load_flag & FC_UNLOADING))
-				lpfc_printf_vlog(port_iterator, KERN_ERR,
-					 LOG_VPORT,
+			lpfc_printf_vlog(port_iterator, KERN_ERR, LOG_VPORT,
 					 "1801 Create vport work array FAILED: "
 					 "cannot do scsi_host_get\n");
 			continue;
diff --git a/drivers/scsi/mac_scsi.c b/drivers/scsi/mac_scsi.c
index af3a6af97cc7..ea2bde206f7f 100644
--- a/drivers/scsi/mac_scsi.c
+++ b/drivers/scsi/mac_scsi.c
@@ -291,8 +291,7 @@ int __init macscsi_detect(struct scsi_host_template * tpnt)
     ((struct NCR5380_hostdata *)instance->hostdata)->ctrl = 0;
 
     if (instance->irq != SCSI_IRQ_NONE)
-	if (request_irq(instance->irq, NCR5380_intr, IRQ_FLG_SLOW, 
-			"ncr5380", instance)) {
+	if (request_irq(instance->irq, NCR5380_intr, 0, "ncr5380", instance)) {
 	    printk(KERN_WARNING "scsi%d: IRQ%d not free, interrupts disabled\n",
 		   instance->host_no, instance->irq);
 	    instance->irq = SCSI_IRQ_NONE;
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2.h b/drivers/scsi/mpt2sas/mpi/mpi2.h
index 8dc1b32918dd..a01f0aa66f20 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2.h
@@ -8,7 +8,7 @@
  *                  scatter/gather formats.
  *  Creation Date:  June 21, 2006
  *
- *  mpi2.h Version:  02.00.20
+ *  mpi2.h Version:  02.00.22
  *
  *  Version History
  *  ---------------
@@ -69,6 +69,8 @@
  *  02-23-11  02.00.19  Bumped MPI2_HEADER_VERSION_UNIT.
  *                      Added MPI2_FUNCTION_SEND_HOST_MESSAGE.
  *  03-09-11  02.00.20  Bumped MPI2_HEADER_VERSION_UNIT.
+ *  05-25-11  02.00.21  Bumped MPI2_HEADER_VERSION_UNIT.
+ *  08-24-11  02.00.22  Bumped MPI2_HEADER_VERSION_UNIT.
  *  --------------------------------------------------------------------------
  */
 
@@ -94,7 +96,7 @@
 #define MPI2_VERSION_02_00                  (0x0200)
 
 /* versioning for this MPI header set */
-#define MPI2_HEADER_VERSION_UNIT            (0x14)
+#define MPI2_HEADER_VERSION_UNIT            (0x16)
 #define MPI2_HEADER_VERSION_DEV             (0x00)
 #define MPI2_HEADER_VERSION_UNIT_MASK       (0xFF00)
 #define MPI2_HEADER_VERSION_UNIT_SHIFT      (8)
@@ -1073,8 +1075,10 @@ typedef struct _MPI2_IEEE_SGE_UNION
 #define MPI2_IEEE_SGE_FLAGS_IOCPLB_ADDR         (0x02)
 #define MPI2_IEEE_SGE_FLAGS_IOCPLBNTA_ADDR      (0x03)
 						/* IEEE Simple Element only */
-#define MPI2_IEEE_SGE_FLAGS_SYSTEMPLBCPI_ADDR   (0x03)
+#define MPI2_IEEE_SGE_FLAGS_SYSTEMPLBPCI_ADDR   (0x03)
 						/* IEEE Chain Element only */
+#define MPI2_IEEE_SGE_FLAGS_SYSTEMPLBCPI_ADDR   \
+	(MPI2_IEEE_SGE_FLAGS_SYSTEMPLBPCI_ADDR) /* typo in name */
 
 /****************************************************************************
 *  IEEE SGE operation Macros
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h b/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
index cfd95b4e3004..3a023dad77a1 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
@@ -6,7 +6,7 @@
  *          Title:  MPI Configuration messages and pages
  *  Creation Date:  November 10, 2006
  *
- *    mpi2_cnfg.h Version:  02.00.19
+ *    mpi2_cnfg.h Version:  02.00.21
  *
  *  Version History
  *  ---------------
@@ -140,6 +140,13 @@
  *                      Added SASNotifyPrimitiveMasks field to
  *                      MPI2_CONFIG_PAGE_IOC_7.
  *  03-09-11  02.00.19  Fixed IO Unit Page 10 (to match the spec).
+ *  05-25-11  02.00.20  Cleaned up a few comments.
+ *  08-24-11  02.00.21  Marked the IO Unit Page 7 PowerManagementCapabilities
+ *                      for PCIe link as obsolete.
+ *                      Added SpinupFlags field containing a Disable Spin-up
+ *                      bit to the MPI2_SAS_IOUNIT4_SPINUP_GROUP fields of
+ *                      SAS IO Unit Page 4.
+
  *  --------------------------------------------------------------------------
  */
 
@@ -904,8 +911,8 @@ typedef struct _MPI2_CONFIG_PAGE_IO_UNIT_7 {
 #define MPI2_IOUNITPAGE7_PMCAP_12_5_PCT_IOCSPEED    (0x00000400)
 #define MPI2_IOUNITPAGE7_PMCAP_25_0_PCT_IOCSPEED    (0x00000200)
 #define MPI2_IOUNITPAGE7_PMCAP_50_0_PCT_IOCSPEED    (0x00000100)
-#define MPI2_IOUNITPAGE7_PMCAP_PCIE_WIDTH_CHANGE    (0x00000008)
-#define MPI2_IOUNITPAGE7_PMCAP_PCIE_SPEED_CHANGE    (0x00000004)
+#define MPI2_IOUNITPAGE7_PMCAP_PCIE_WIDTH_CHANGE    (0x00000008) /* obsolete */
+#define MPI2_IOUNITPAGE7_PMCAP_PCIE_SPEED_CHANGE    (0x00000004) /* obsolete */
 
 /* defines for IO Unit Page 7 IOCTemperatureUnits field */
 #define MPI2_IOUNITPAGE7_IOC_TEMP_NOT_PRESENT       (0x00)
@@ -1970,10 +1977,14 @@ typedef struct _MPI2_SAS_IOUNIT4_SPINUP_GROUP
 {
     U8          MaxTargetSpinup;            /* 0x00 */
     U8          SpinupDelay;                /* 0x01 */
-    U16         Reserved1;                  /* 0x02 */
+	U8          SpinupFlags;                /* 0x02 */
+	U8          Reserved1;                  /* 0x03 */
 } MPI2_SAS_IOUNIT4_SPINUP_GROUP, MPI2_POINTER PTR_MPI2_SAS_IOUNIT4_SPINUP_GROUP,
   Mpi2SasIOUnit4SpinupGroup_t, MPI2_POINTER pMpi2SasIOUnit4SpinupGroup_t;
 
+/* defines for SAS IO Unit Page 4 SpinupFlags */
+#define MPI2_SASIOUNIT4_SPINUP_DISABLE_FLAG         (0x01)
+
 /*
  * Host code (drivers, BIOS, utilities, etc.) should leave this define set to
  * one and check the value returned for NumPhys at runtime.
@@ -2321,13 +2332,12 @@ typedef struct _MPI2_CONFIG_PAGE_EXPANDER_1
 
 /* use MPI2_SAS_NEG_LINK_RATE_ defines for the NegotiatedLinkRate field */
 
-/* use MPI2_SAS_APHYINFO_ defines for AttachedPhyInfo field */
-
 /* values for SAS Expander Page 1 DiscoveryInfo field */
 #define MPI2_SAS_EXPANDER1_DISCINFO_BAD_PHY_DISABLED    (0x04)
 #define MPI2_SAS_EXPANDER1_DISCINFO_LINK_STATUS_CHANGE  (0x02)
 #define MPI2_SAS_EXPANDER1_DISCINFO_NO_ROUTING_ENTRIES  (0x01)
 
+/* use MPI2_SAS_APHYINFO_ defines for AttachedPhyInfo field */
 
 /****************************************************************************
 *   SAS Device Config Pages
@@ -2447,6 +2457,8 @@ typedef struct _MPI2_CONFIG_PAGE_SAS_PHY_0
 
 #define MPI2_SASPHY0_PAGEVERSION            (0x03)
 
+/* use MPI2_SAS_APHYINFO_ defines for AttachedPhyInfo field */
+
 /* use MPI2_SAS_PRATE_ defines for the ProgrammedLinkRate field */
 
 /* use MPI2_SAS_HWRATE_ defines for the HwLinkRate field */
@@ -2454,12 +2466,10 @@ typedef struct _MPI2_CONFIG_PAGE_SAS_PHY_0
 /* values for SAS PHY Page 0 Flags field */
 #define MPI2_SAS_PHY0_FLAGS_SGPIO_DIRECT_ATTACH_ENC             (0x01)
 
-/* use MPI2_SAS_APHYINFO_ defines for AttachedPhyInfo field */
+/* use MPI2_SAS_PHYINFO_ for the PhyInfo field */
 
 /* use MPI2_SAS_NEG_LINK_RATE_ defines for the NegotiatedLinkRate field */
 
-/* use MPI2_SAS_PHYINFO_ for the PhyInfo field */
-
 
 /* SAS PHY Page 1 */
 
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_ioc.h b/drivers/scsi/mpt2sas/mpi/mpi2_ioc.h
index 93d9b6956d05..9a925c07a9ec 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_ioc.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_ioc.h
@@ -6,7 +6,7 @@
  *          Title:  MPI IOC, Port, Event, FW Download, and FW Upload messages
  *  Creation Date:  October 11, 2006
  *
- *  mpi2_ioc.h Version:  02.00.17
+ *  mpi2_ioc.h Version:  02.00.19
  *
  *  Version History
  *  ---------------
@@ -110,6 +110,13 @@
  *                      Added Temperature Threshold Event.
  *                      Added Host Message Event.
  *                      Added Send Host Message request and reply.
+ *  05-25-11  02.00.18  For Extended Image Header, added
+ *                      MPI2_EXT_IMAGE_TYPE_MIN_PRODUCT_SPECIFIC and
+ *                      MPI2_EXT_IMAGE_TYPE_MAX_PRODUCT_SPECIFIC defines.
+ *                      Deprecated MPI2_EXT_IMAGE_TYPE_MAX define.
+ *  08-24-11  02.00.19  Added PhysicalPort field to
+ *                      MPI2_EVENT_DATA_SAS_DEVICE_STATUS_CHANGE structure.
+ *                      Marked MPI2_PM_CONTROL_FEATURE_PCIE_LINK as obsolete.
  *  --------------------------------------------------------------------------
  */
 
@@ -578,7 +585,7 @@ typedef struct _MPI2_EVENT_DATA_SAS_DEVICE_STATUS_CHANGE
 {
     U16                     TaskTag;                        /* 0x00 */
     U8                      ReasonCode;                     /* 0x02 */
-    U8                      Reserved1;                      /* 0x03 */
+	U8                      PhysicalPort;                   /* 0x03 */
     U8                      ASC;                            /* 0x04 */
     U8                      ASCQ;                           /* 0x05 */
     U16                     DevHandle;                      /* 0x06 */
@@ -1366,16 +1373,18 @@ typedef struct _MPI2_EXT_IMAGE_HEADER
 #define MPI2_EXT_IMAGE_HEADER_SIZE              (0x40)
 
 /* defines for the ImageType field */
-#define MPI2_EXT_IMAGE_TYPE_UNSPECIFIED         (0x00)
-#define MPI2_EXT_IMAGE_TYPE_FW                  (0x01)
-#define MPI2_EXT_IMAGE_TYPE_NVDATA              (0x03)
-#define MPI2_EXT_IMAGE_TYPE_BOOTLOADER          (0x04)
-#define MPI2_EXT_IMAGE_TYPE_INITIALIZATION      (0x05)
-#define MPI2_EXT_IMAGE_TYPE_FLASH_LAYOUT        (0x06)
-#define MPI2_EXT_IMAGE_TYPE_SUPPORTED_DEVICES   (0x07)
-#define MPI2_EXT_IMAGE_TYPE_MEGARAID            (0x08)
-
-#define MPI2_EXT_IMAGE_TYPE_MAX                 (MPI2_EXT_IMAGE_TYPE_MEGARAID)
+#define MPI2_EXT_IMAGE_TYPE_UNSPECIFIED				(0x00)
+#define MPI2_EXT_IMAGE_TYPE_FW						(0x01)
+#define MPI2_EXT_IMAGE_TYPE_NVDATA					(0x03)
+#define MPI2_EXT_IMAGE_TYPE_BOOTLOADER				(0x04)
+#define MPI2_EXT_IMAGE_TYPE_INITIALIZATION			(0x05)
+#define MPI2_EXT_IMAGE_TYPE_FLASH_LAYOUT			(0x06)
+#define MPI2_EXT_IMAGE_TYPE_SUPPORTED_DEVICES		(0x07)
+#define MPI2_EXT_IMAGE_TYPE_MEGARAID				(0x08)
+#define MPI2_EXT_IMAGE_TYPE_MIN_PRODUCT_SPECIFIC    (0x80)
+#define MPI2_EXT_IMAGE_TYPE_MAX_PRODUCT_SPECIFIC    (0xFF)
+#define MPI2_EXT_IMAGE_TYPE_MAX                   \
+	(MPI2_EXT_IMAGE_TYPE_MAX_PRODUCT_SPECIFIC)	/* deprecated */
 
 
 
@@ -1568,7 +1577,7 @@ typedef struct _MPI2_PWR_MGMT_CONTROL_REQUEST {
 /* defines for the Feature field */
 #define MPI2_PM_CONTROL_FEATURE_DA_PHY_POWER_COND       (0x01)
 #define MPI2_PM_CONTROL_FEATURE_PORT_WIDTH_MODULATION   (0x02)
-#define MPI2_PM_CONTROL_FEATURE_PCIE_LINK               (0x03)
+#define MPI2_PM_CONTROL_FEATURE_PCIE_LINK               (0x03) /* obsolete */
 #define MPI2_PM_CONTROL_FEATURE_IOC_SPEED               (0x04)
 #define MPI2_PM_CONTROL_FEATURE_MIN_PRODUCT_SPECIFIC    (0x80)
 #define MPI2_PM_CONTROL_FEATURE_MAX_PRODUCT_SPECIFIC    (0xFF)
@@ -1597,14 +1606,14 @@ typedef struct _MPI2_PWR_MGMT_CONTROL_REQUEST {
 
 /* parameter usage for the MPI2_PM_CONTROL_FEATURE_PCIE_LINK Feature */
 /* Parameter1 indicates desired PCIe link speed using these defines */
-#define MPI2_PM_CONTROL_PARAM1_PCIE_2_5_GBPS            (0x00)
-#define MPI2_PM_CONTROL_PARAM1_PCIE_5_0_GBPS            (0x01)
-#define MPI2_PM_CONTROL_PARAM1_PCIE_8_0_GBPS            (0x02)
+#define MPI2_PM_CONTROL_PARAM1_PCIE_2_5_GBPS            (0x00) /* obsolete */
+#define MPI2_PM_CONTROL_PARAM1_PCIE_5_0_GBPS            (0x01) /* obsolete */
+#define MPI2_PM_CONTROL_PARAM1_PCIE_8_0_GBPS            (0x02) /* obsolete */
 /* Parameter2 indicates desired PCIe link width using these defines */
-#define MPI2_PM_CONTROL_PARAM2_WIDTH_X1                 (0x01)
-#define MPI2_PM_CONTROL_PARAM2_WIDTH_X2                 (0x02)
-#define MPI2_PM_CONTROL_PARAM2_WIDTH_X4                 (0x04)
-#define MPI2_PM_CONTROL_PARAM2_WIDTH_X8                 (0x08)
+#define MPI2_PM_CONTROL_PARAM2_WIDTH_X1                 (0x01) /* obsolete */
+#define MPI2_PM_CONTROL_PARAM2_WIDTH_X2                 (0x02) /* obsolete */
+#define MPI2_PM_CONTROL_PARAM2_WIDTH_X4                 (0x04) /* obsolete */
+#define MPI2_PM_CONTROL_PARAM2_WIDTH_X8                 (0x08) /* obsolete */
 /* Parameter3 and Parameter4 are reserved */
 
 /* parameter usage for the MPI2_PM_CONTROL_FEATURE_IOC_SPEED Feature */
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_raid.h b/drivers/scsi/mpt2sas/mpi/mpi2_raid.h
index bd61a7b60a2b..0601612b875a 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_raid.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_raid.h
@@ -6,7 +6,7 @@
  *          Title:  MPI Integrated RAID messages and structures
  *  Creation Date:  April 26, 2007
  *
- *    mpi2_raid.h Version:  02.00.05
+ *    mpi2_raid.h Version:  02.00.06
  *
  *  Version History
  *  ---------------
@@ -23,6 +23,10 @@
  *  07-30-09  02.00.04  Added proper define for the Use Default Settings bit of
  *                      VolumeCreationFlags and marked the old one as obsolete.
  *  05-12-10  02.00.05  Added MPI2_RAID_VOL_FLAGS_OP_MDC define.
+ *  08-24-10  02.00.06  Added MPI2_RAID_ACTION_COMPATIBILITY_CHECK along with
+ *                      related structures and defines.
+ *                      Added product-specific range to RAID Action values.
+
  *  --------------------------------------------------------------------------
  */
 
@@ -176,7 +180,9 @@ typedef struct _MPI2_RAID_ACTION_REQUEST
 #define MPI2_RAID_ACTION_SYSTEM_SHUTDOWN_INITIATED  (0x20)
 #define MPI2_RAID_ACTION_START_RAID_FUNCTION        (0x21)
 #define MPI2_RAID_ACTION_STOP_RAID_FUNCTION         (0x22)
-
+#define MPI2_RAID_ACTION_COMPATIBILITY_CHECK        (0x23)
+#define MPI2_RAID_ACTION_MIN_PRODUCT_SPECIFIC       (0x80)
+#define MPI2_RAID_ACTION_MAX_PRODUCT_SPECIFIC       (0xFF)
 
 /* RAID Volume Creation Structure */
 
@@ -244,6 +250,23 @@ typedef struct _MPI2_RAID_ONLINE_CAPACITY_EXPANSION
   Mpi2RaidOnlineCapacityExpansion_t,
   MPI2_POINTER pMpi2RaidOnlineCapacityExpansion_t;
 
+/* RAID Compatibility Input Structure */
+
+typedef struct _MPI2_RAID_COMPATIBILITY_INPUT_STRUCT {
+	U16                     SourceDevHandle;               /* 0x00 */
+	U16                     CandidateDevHandle;             /* 0x02 */
+	U32                     Flags;                          /* 0x04 */
+	U32                     Reserved1;                      /* 0x08 */
+	U32                     Reserved2;                      /* 0x0C */
+} MPI2_RAID_COMPATIBILITY_INPUT_STRUCT,
+MPI2_POINTER PTR_MPI2_RAID_COMPATIBILITY_INPUT_STRUCT,
+Mpi2RaidCompatibilityInputStruct_t,
+MPI2_POINTER pMpi2RaidCompatibilityInputStruct_t;
+
+/* defines for RAID Compatibility Structure Flags field */
+#define MPI2_RAID_COMPAT_SOURCE_IS_VOLUME_FLAG      (0x00000002)
+#define MPI2_RAID_COMPAT_REPORT_SOURCE_INFO_FLAG    (0x00000001)
+
 
 /* RAID Volume Indicator Structure */
 
@@ -263,15 +286,45 @@ typedef struct _MPI2_RAID_VOL_INDICATOR
 #define MPI2_RAID_VOL_FLAGS_OP_RESYNC               (0x00000003)
 #define MPI2_RAID_VOL_FLAGS_OP_MDC                  (0x00000004)
 
+/* RAID Compatibility Result Structure */
+
+typedef struct _MPI2_RAID_COMPATIBILITY_RESULT_STRUCT {
+	U8                      State;                          /* 0x00 */
+	U8                      Reserved1;                      /* 0x01 */
+	U16                     Reserved2;                      /* 0x02 */
+	U32                     GenericAttributes;              /* 0x04 */
+	U32                     OEMSpecificAttributes;          /* 0x08 */
+	U32                     Reserved3;                      /* 0x0C */
+	U32                     Reserved4;                      /* 0x10 */
+} MPI2_RAID_COMPATIBILITY_RESULT_STRUCT,
+MPI2_POINTER PTR_MPI2_RAID_COMPATIBILITY_RESULT_STRUCT,
+Mpi2RaidCompatibilityResultStruct_t,
+MPI2_POINTER pMpi2RaidCompatibilityResultStruct_t;
+
+/* defines for RAID Compatibility Result Structure State field */
+#define MPI2_RAID_COMPAT_STATE_COMPATIBLE           (0x00)
+#define MPI2_RAID_COMPAT_STATE_NOT_COMPATIBLE       (0x01)
+
+/* defines for RAID Compatibility Result Structure GenericAttributes field */
+#define MPI2_RAID_COMPAT_GENATTRIB_4K_SECTOR            (0x00000010)
+
+#define MPI2_RAID_COMPAT_GENATTRIB_MEDIA_MASK           (0x0000000C)
+#define MPI2_RAID_COMPAT_GENATTRIB_SOLID_STATE_DRIVE    (0x00000008)
+#define MPI2_RAID_COMPAT_GENATTRIB_HARD_DISK_DRIVE      (0x00000004)
+
+#define MPI2_RAID_COMPAT_GENATTRIB_PROTOCOL_MASK        (0x00000003)
+#define MPI2_RAID_COMPAT_GENATTRIB_SAS_PROTOCOL         (0x00000002)
+#define MPI2_RAID_COMPAT_GENATTRIB_SATA_PROTOCOL        (0x00000001)
 
 /* RAID Action Reply ActionData union */
 typedef union _MPI2_RAID_ACTION_REPLY_DATA
 {
-    U32                     Word[5];
-    MPI2_RAID_VOL_INDICATOR RaidVolumeIndicator;
-    U16                     VolDevHandle;
-    U8                      VolumeState;
-    U8                      PhysDiskNum;
+	U32                                     Word[5];
+	MPI2_RAID_VOL_INDICATOR                 RaidVolumeIndicator;
+	U16                                     VolDevHandle;
+	U8                                      VolumeState;
+	U8                                      PhysDiskNum;
+	MPI2_RAID_COMPATIBILITY_RESULT_STRUCT   RaidCompatibilityResult;
 } MPI2_RAID_ACTION_REPLY_DATA, MPI2_POINTER PTR_MPI2_RAID_ACTION_REPLY_DATA,
   Mpi2RaidActionReplyData_t, MPI2_POINTER pMpi2RaidActionReplyData_t;
 
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_tool.h b/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
index 2a4bceda364b..3cbe677c6886 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
@@ -6,7 +6,7 @@
  *          Title:  MPI diagnostic tool structures and definitions
  *  Creation Date:  March 26, 2007
  *
- *    mpi2_tool.h Version:  02.00.06
+ *    mpi2_tool.h Version:  02.00.07
  *
  *  Version History
  *  ---------------
@@ -25,6 +25,8 @@
  *  05-12-10  02.00.05  Added Diagnostic Data Upload tool.
  *  08-11-10  02.00.06  Added defines that were missing for Diagnostic Buffer
  *                      Post Request.
+ *  05-25-11  02.00.07  Added Flags field and related defines to
+ *                      MPI2_TOOLBOX_ISTWI_READ_WRITE_REQUEST.
  *  --------------------------------------------------------------------------
  */
 
@@ -181,7 +183,7 @@ typedef struct _MPI2_TOOLBOX_ISTWI_READ_WRITE_REQUEST {
     U8                      DevIndex;                   /* 0x14 */
     U8                      Action;                     /* 0x15 */
     U8                      SGLFlags;                   /* 0x16 */
-    U8                      Reserved7;                  /* 0x17 */
+	 U8                      Flags;                      /* 0x17 */
     U16                     TxDataLength;               /* 0x18 */
     U16                     RxDataLength;               /* 0x1A */
     U32                     Reserved8;                  /* 0x1C */
@@ -205,6 +207,9 @@ typedef struct _MPI2_TOOLBOX_ISTWI_READ_WRITE_REQUEST {
 
 /* use MPI2_SGLFLAGS_ defines from mpi2.h for the SGLFlags field */
 
+/* values for the Flags field */
+#define MPI2_TOOL_ISTWI_FLAG_AUTO_RESERVE_RELEASE   (0x80)
+#define MPI2_TOOL_ISTWI_FLAG_PAGE_ADDR_MASK         (0x07)
 
 /* Toolbox ISTWI Read Write Tool reply message */
 typedef struct _MPI2_TOOLBOX_ISTWI_REPLY {
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.c b/drivers/scsi/mpt2sas/mpt2sas_base.c
index beda04a8404b..0b2c95583660 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.c
@@ -57,6 +57,7 @@
 #include <linux/sort.h>
 #include <linux/io.h>
 #include <linux/time.h>
+#include <linux/kthread.h>
 #include <linux/aer.h>
 
 #include "mpt2sas_base.h"
@@ -65,6 +66,8 @@ static MPT_CALLBACK	mpt_callbacks[MPT_MAX_CALLBACKS];
 
 #define FAULT_POLLING_INTERVAL 1000 /* in milliseconds */
 
+#define MAX_HBA_QUEUE_DEPTH	30000
+#define MAX_CHAIN_DEPTH		100000
 static int max_queue_depth = -1;
 module_param(max_queue_depth, int, 0);
 MODULE_PARM_DESC(max_queue_depth, " max controller queue depth ");
@@ -89,19 +92,6 @@ static int disable_discovery = -1;
 module_param(disable_discovery, int, 0);
 MODULE_PARM_DESC(disable_discovery, " disable discovery ");
 
-
-/* diag_buffer_enable is bitwise
- * bit 0 set = TRACE
- * bit 1 set = SNAPSHOT
- * bit 2 set = EXTENDED
- *
- * Either bit can be set, or both
- */
-static int diag_buffer_enable;
-module_param(diag_buffer_enable, int, 0);
-MODULE_PARM_DESC(diag_buffer_enable, " post diag buffers "
-    "(TRACE=1/SNAPSHOT=2/EXTENDED=4/default=0)");
-
 /**
  * _scsih_set_fwfault_debug - global setting of ioc->fwfault_debug.
  *
@@ -120,10 +110,34 @@ _scsih_set_fwfault_debug(const char *val, struct kernel_param *kp)
 		ioc->fwfault_debug = mpt2sas_fwfault_debug;
 	return 0;
 }
+
 module_param_call(mpt2sas_fwfault_debug, _scsih_set_fwfault_debug,
     param_get_int, &mpt2sas_fwfault_debug, 0644);
 
 /**
+ *  mpt2sas_remove_dead_ioc_func - kthread context to remove dead ioc
+ * @arg: input argument, used to derive ioc
+ *
+ * Return 0 if controller is removed from pci subsystem.
+ * Return -1 for other case.
+ */
+static int mpt2sas_remove_dead_ioc_func(void *arg)
+{
+		struct MPT2SAS_ADAPTER *ioc = (struct MPT2SAS_ADAPTER *)arg;
+		struct pci_dev *pdev;
+
+		if ((ioc == NULL))
+			return -1;
+
+		pdev = ioc->pdev;
+		if ((pdev == NULL))
+			return -1;
+		pci_remove_bus_device(pdev);
+		return 0;
+}
+
+
+/**
  * _base_fault_reset_work - workq handling ioc fault conditions
  * @work: input argument, used to derive ioc
  * Context: sleep.
@@ -138,6 +152,7 @@ _base_fault_reset_work(struct work_struct *work)
 	unsigned long	 flags;
 	u32 doorbell;
 	int rc;
+	struct task_struct *p;
 
 	spin_lock_irqsave(&ioc->ioc_reset_in_progress_lock, flags);
 	if (ioc->shost_recovery)
@@ -145,6 +160,39 @@ _base_fault_reset_work(struct work_struct *work)
 	spin_unlock_irqrestore(&ioc->ioc_reset_in_progress_lock, flags);
 
 	doorbell = mpt2sas_base_get_iocstate(ioc, 0);
+	if ((doorbell & MPI2_IOC_STATE_MASK) == MPI2_IOC_STATE_MASK) {
+		printk(MPT2SAS_INFO_FMT "%s : SAS host is non-operational !!!!\n",
+			ioc->name, __func__);
+
+		/*
+		 * Call _scsih_flush_pending_cmds callback so that we flush all
+		 * pending commands back to OS. This call is required to aovid
+		 * deadlock at block layer. Dead IOC will fail to do diag reset,
+		 * and this call is safe since dead ioc will never return any
+		 * command back from HW.
+		 */
+		ioc->schedule_dead_ioc_flush_running_cmds(ioc);
+		/*
+		 * Set remove_host flag early since kernel thread will
+		 * take some time to execute.
+		 */
+		ioc->remove_host = 1;
+		/*Remove the Dead Host */
+		p = kthread_run(mpt2sas_remove_dead_ioc_func, ioc,
+		    "mpt2sas_dead_ioc_%d", ioc->id);
+		if (IS_ERR(p)) {
+			printk(MPT2SAS_ERR_FMT
+			"%s: Running mpt2sas_dead_ioc thread failed !!!!\n",
+			ioc->name, __func__);
+		} else {
+		    printk(MPT2SAS_ERR_FMT
+			"%s: Running mpt2sas_dead_ioc thread success !!!!\n",
+			ioc->name, __func__);
+		}
+
+		return; /* don't rearm timer */
+	}
+
 	if ((doorbell & MPI2_IOC_STATE_MASK) == MPI2_IOC_STATE_FAULT) {
 		rc = mpt2sas_base_hard_reset_handler(ioc, CAN_SLEEP,
 		    FORCE_BIG_HAMMER);
@@ -1346,7 +1394,7 @@ _base_enable_msix(struct MPT2SAS_ADAPTER *ioc)
 	if (_base_check_enable_msix(ioc) != 0)
 		goto try_ioapic;
 
-	ioc->reply_queue_count = min_t(u8, ioc->cpu_count,
+	ioc->reply_queue_count = min_t(int, ioc->cpu_count,
 	    ioc->msix_vector_count);
 
 	entries = kcalloc(ioc->reply_queue_count, sizeof(struct msix_entry),
@@ -1916,6 +1964,10 @@ _base_display_intel_branding(struct MPT2SAS_ADAPTER *ioc)
 			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 			    MPT2SAS_INTEL_RMS2LL040_BRANDING);
 			break;
+		case MPT2SAS_INTEL_RAMSDALE_SSDID:
+			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
+			    MPT2SAS_INTEL_RAMSDALE_BRANDING);
+			break;
 		default:
 			break;
 		}
@@ -1925,6 +1977,22 @@ _base_display_intel_branding(struct MPT2SAS_ADAPTER *ioc)
 			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 			    MPT2SAS_INTEL_RS25GB008_BRANDING);
 			break;
+		case MPT2SAS_INTEL_RMS25JB080_SSDID:
+			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
+			    MPT2SAS_INTEL_RMS25JB080_BRANDING);
+			break;
+		case MPT2SAS_INTEL_RMS25JB040_SSDID:
+			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
+			    MPT2SAS_INTEL_RMS25JB040_BRANDING);
+			break;
+		case MPT2SAS_INTEL_RMS25KB080_SSDID:
+			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
+			    MPT2SAS_INTEL_RMS25KB080_BRANDING);
+			break;
+		case MPT2SAS_INTEL_RMS25KB040_SSDID:
+			printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
+			    MPT2SAS_INTEL_RMS25KB040_BRANDING);
+			break;
 		default:
 			break;
 		}
@@ -2311,8 +2379,6 @@ _base_release_memory_pools(struct MPT2SAS_ADAPTER *ioc)
 		}
 		if (ioc->chain_dma_pool)
 			pci_pool_destroy(ioc->chain_dma_pool);
-	}
-	if (ioc->chain_lookup) {
 		free_pages((ulong)ioc->chain_lookup, ioc->chain_pages);
 		ioc->chain_lookup = NULL;
 	}
@@ -2330,9 +2396,7 @@ static int
 _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 {
 	struct mpt2sas_facts *facts;
-	u32 queue_size, queue_diff;
 	u16 max_sge_elements;
-	u16 num_of_reply_frames;
 	u16 chains_needed_per_io;
 	u32 sz, total_sz, reply_post_free_sz;
 	u32 retry_sz;
@@ -2359,7 +2423,8 @@ _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 		max_request_credit = (max_queue_depth < facts->RequestCredit)
 		    ? max_queue_depth : facts->RequestCredit;
 	else
-		max_request_credit = facts->RequestCredit;
+		max_request_credit = min_t(u16, facts->RequestCredit,
+		    MAX_HBA_QUEUE_DEPTH);
 
 	ioc->hba_queue_depth = max_request_credit;
 	ioc->hi_priority_depth = facts->HighPriorityCredit;
@@ -2400,50 +2465,25 @@ _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 	}
 	ioc->chains_needed_per_io = chains_needed_per_io;
 
-	/* reply free queue sizing - taking into account for events */
-	num_of_reply_frames = ioc->hba_queue_depth + 32;
-
-	/* number of replies frames can't be a multiple of 16 */
-	/* decrease number of reply frames by 1 */
-	if (!(num_of_reply_frames % 16))
-		num_of_reply_frames--;
-
-	/* calculate number of reply free queue entries
-	 *  (must be multiple of 16)
-	 */
-
-	/* (we know reply_free_queue_depth is not a multiple of 16) */
-	queue_size = num_of_reply_frames;
-	queue_size += 16 - (queue_size % 16);
-	ioc->reply_free_queue_depth = queue_size;
-
-	/* reply descriptor post queue sizing */
-	/* this size should be the number of request frames + number of reply
-	 * frames
-	 */
-
-	queue_size = ioc->hba_queue_depth + num_of_reply_frames + 1;
-	/* round up to 16 byte boundary */
-	if (queue_size % 16)
-		queue_size += 16 - (queue_size % 16);
-
-	/* check against IOC maximum reply post queue depth */
-	if (queue_size > facts->MaxReplyDescriptorPostQueueDepth) {
-		queue_diff = queue_size -
-		    facts->MaxReplyDescriptorPostQueueDepth;
+	/* reply free queue sizing - taking into account for 64 FW events */
+	ioc->reply_free_queue_depth = ioc->hba_queue_depth + 64;
 
-		/* round queue_diff up to multiple of 16 */
-		if (queue_diff % 16)
-			queue_diff += 16 - (queue_diff % 16);
-
-		/* adjust hba_queue_depth, reply_free_queue_depth,
-		 * and queue_size
-		 */
-		ioc->hba_queue_depth -= (queue_diff / 2);
-		ioc->reply_free_queue_depth -= (queue_diff / 2);
-		queue_size = facts->MaxReplyDescriptorPostQueueDepth;
+	/* align the reply post queue on the next 16 count boundary */
+	if (!ioc->reply_free_queue_depth % 16)
+		ioc->reply_post_queue_depth = ioc->reply_free_queue_depth + 16;
+	else
+		ioc->reply_post_queue_depth = ioc->reply_free_queue_depth +
+				32 - (ioc->reply_free_queue_depth % 16);
+	if (ioc->reply_post_queue_depth >
+	    facts->MaxReplyDescriptorPostQueueDepth) {
+		ioc->reply_post_queue_depth = min_t(u16,
+		    (facts->MaxReplyDescriptorPostQueueDepth -
+		    (facts->MaxReplyDescriptorPostQueueDepth % 16)),
+		    (ioc->hba_queue_depth - (ioc->hba_queue_depth % 16)));
+		ioc->reply_free_queue_depth = ioc->reply_post_queue_depth - 16;
+		ioc->hba_queue_depth = ioc->reply_free_queue_depth - 64;
 	}
-	ioc->reply_post_queue_depth = queue_size;
+
 
 	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "scatter gather: "
 	    "sge_in_main_msg(%d), sge_per_chain(%d), sge_per_io(%d), "
@@ -2529,15 +2569,12 @@ _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 	    "depth(%d)\n", ioc->name, ioc->request,
 	    ioc->scsiio_depth));
 
-	/* loop till the allocation succeeds */
-	do {
-		sz = ioc->chain_depth * sizeof(struct chain_tracker);
-		ioc->chain_pages = get_order(sz);
-		ioc->chain_lookup = (struct chain_tracker *)__get_free_pages(
-		    GFP_KERNEL, ioc->chain_pages);
-		if (ioc->chain_lookup == NULL)
-			ioc->chain_depth -= 100;
-	} while (ioc->chain_lookup == NULL);
+	ioc->chain_depth = min_t(u32, ioc->chain_depth, MAX_CHAIN_DEPTH);
+	sz = ioc->chain_depth * sizeof(struct chain_tracker);
+	ioc->chain_pages = get_order(sz);
+
+	ioc->chain_lookup = (struct chain_tracker *)__get_free_pages(
+	    GFP_KERNEL, ioc->chain_pages);
 	ioc->chain_dma_pool = pci_pool_create("chain pool", ioc->pdev,
 	    ioc->request_sz, 16, 0);
 	if (!ioc->chain_dma_pool) {
@@ -3136,8 +3173,8 @@ mpt2sas_base_sas_iounit_control(struct MPT2SAS_ADAPTER *ioc,
 	if (mpi_request->Operation == MPI2_SAS_OP_PHY_HARD_RESET ||
 	    mpi_request->Operation == MPI2_SAS_OP_PHY_LINK_RESET)
 		ioc->ioc_link_reset_in_progress = 1;
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->base_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->base_cmds.done,
 	    msecs_to_jiffies(10000));
 	if ((mpi_request->Operation == MPI2_SAS_OP_PHY_HARD_RESET ||
@@ -3238,8 +3275,8 @@ mpt2sas_base_scsi_enclosure_processor(struct MPT2SAS_ADAPTER *ioc,
 	request = mpt2sas_base_get_msg_frame(ioc, smid);
 	ioc->base_cmds.smid = smid;
 	memcpy(request, mpi_request, sizeof(Mpi2SepReply_t));
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->base_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->base_cmds.done,
 	    msecs_to_jiffies(10000));
 	if (!(ioc->base_cmds.status & MPT2_CMD_COMPLETE)) {
@@ -3746,8 +3783,8 @@ _base_event_notification(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	for (i = 0; i < MPI2_EVENT_NOTIFY_EVENTMASK_WORDS; i++)
 		mpi_request->EventMasks[i] =
 		    cpu_to_le32(ioc->event_masks[i]);
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->base_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->base_cmds.done, 30*HZ);
 	if (!(ioc->base_cmds.status & MPT2_CMD_COMPLETE)) {
 		printk(MPT2SAS_ERR_FMT "%s: timeout\n",
@@ -4062,7 +4099,8 @@ _base_make_ioc_operational(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 		ioc->reply_free[i] = cpu_to_le32(reply_address);
 
 	/* initialize reply queues */
-	_base_assign_reply_queues(ioc);
+	if (ioc->is_driver_loading)
+		_base_assign_reply_queues(ioc);
 
 	/* initialize Reply Post Free Queue */
 	reply_post_free = (long)ioc->reply_post_free;
@@ -4110,24 +4148,17 @@ _base_make_ioc_operational(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 
 
 	if (ioc->is_driver_loading) {
-
-
-
-		ioc->wait_for_discovery_to_complete =
-		    _base_determine_wait_on_discovery(ioc);
-		return r; /* scan_start and scan_finished support */
-	}
-
-
-	if (ioc->wait_for_discovery_to_complete && ioc->is_warpdrive) {
-		if (ioc->manu_pg10.OEMIdentifier  == 0x80) {
+		if (ioc->is_warpdrive && ioc->manu_pg10.OEMIdentifier
+		    == 0x80) {
 			hide_flag = (u8) (ioc->manu_pg10.OEMSpecificFlags0 &
 			    MFG_PAGE10_HIDE_SSDS_MASK);
 			if (hide_flag != MFG_PAGE10_HIDE_SSDS_MASK)
 				ioc->mfg_pg10_hide_flag = hide_flag;
 		}
+		ioc->wait_for_discovery_to_complete =
+		    _base_determine_wait_on_discovery(ioc);
+		return r; /* scan_start and scan_finished support */
 	}
-
 	r = _base_send_port_enable(ioc, sleep_flag);
 	if (r)
 		return r;
@@ -4206,7 +4237,7 @@ mpt2sas_base_attach(struct MPT2SAS_ADAPTER *ioc)
 
 	r = mpt2sas_base_map_resources(ioc);
 	if (r)
-		return r;
+		goto out_free_resources;
 
 	if (ioc->is_warpdrive) {
 		ioc->reply_post_host_index[0] =
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.h b/drivers/scsi/mpt2sas/mpt2sas_base.h
index 3c3babc7d260..c7459fdc06cc 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.h
@@ -69,8 +69,8 @@
 #define MPT2SAS_DRIVER_NAME		"mpt2sas"
 #define MPT2SAS_AUTHOR	"LSI Corporation <DL-MPTFusionLinux@lsi.com>"
 #define MPT2SAS_DESCRIPTION	"LSI MPT Fusion SAS 2.0 Device Driver"
-#define MPT2SAS_DRIVER_VERSION		"10.100.00.00"
-#define MPT2SAS_MAJOR_VERSION		10
+#define MPT2SAS_DRIVER_VERSION		"12.100.00.00"
+#define MPT2SAS_MAJOR_VERSION		12
 #define MPT2SAS_MINOR_VERSION		100
 #define MPT2SAS_BUILD_VERSION		00
 #define MPT2SAS_RELEASE_VERSION		00
@@ -157,20 +157,33 @@
 /*
  * Intel HBA branding
  */
+#define MPT2SAS_INTEL_RMS25JB080_BRANDING    \
+				"Intel(R) Integrated RAID Module RMS25JB080"
+#define MPT2SAS_INTEL_RMS25JB040_BRANDING    \
+				"Intel(R) Integrated RAID Module RMS25JB040"
+#define MPT2SAS_INTEL_RMS25KB080_BRANDING    \
+				"Intel(R) Integrated RAID Module RMS25KB080"
+#define MPT2SAS_INTEL_RMS25KB040_BRANDING    \
+				"Intel(R) Integrated RAID Module RMS25KB040"
 #define MPT2SAS_INTEL_RMS2LL080_BRANDING	\
 				"Intel Integrated RAID Module RMS2LL080"
 #define MPT2SAS_INTEL_RMS2LL040_BRANDING	\
 				"Intel Integrated RAID Module RMS2LL040"
 #define MPT2SAS_INTEL_RS25GB008_BRANDING       \
 				"Intel(R) RAID Controller RS25GB008"
-
+#define MPT2SAS_INTEL_RAMSDALE_BRANDING        \
+				"Intel 720 Series SSD"
 /*
  * Intel HBA SSDIDs
  */
+#define MPT2SAS_INTEL_RMS25JB080_SSDID         0x3516
+#define MPT2SAS_INTEL_RMS25JB040_SSDID         0x3517
+#define MPT2SAS_INTEL_RMS25KB080_SSDID         0x3518
+#define MPT2SAS_INTEL_RMS25KB040_SSDID         0x3519
 #define MPT2SAS_INTEL_RMS2LL080_SSDID          0x350E
 #define MPT2SAS_INTEL_RMS2LL040_SSDID          0x350F
 #define MPT2SAS_INTEL_RS25GB008_SSDID          0x3000
-
+#define MPT2SAS_INTEL_RAMSDALE_SSDID           0x3700
 
 /*
  * HP HBA branding
@@ -373,6 +386,7 @@ struct _sas_device {
  * @percent_complete: resync percent complete
  * @direct_io_enabled: Whether direct io to PDs are allowed or not
  * @stripe_exponent: X where 2powX is the stripe sz in blocks
+ * @block_exponent: X where 2powX is the block sz in bytes
  * @max_lba: Maximum number of LBA in the volume
  * @stripe_sz: Stripe Size of the volume
  * @device_info: Device info of the volume member disk
@@ -394,6 +408,7 @@ struct _raid_device {
 	u8	percent_complete;
 	u8	direct_io_enabled;
 	u8	stripe_exponent;
+	u8	block_exponent;
 	u64	max_lba;
 	u32	stripe_sz;
 	u32	device_info;
@@ -623,6 +638,7 @@ enum mutex_type {
 	TM_MUTEX_ON = 1,
 };
 
+typedef void (*MPT2SAS_FLUSH_RUNNING_CMDS)(struct MPT2SAS_ADAPTER *ioc);
 /**
  * struct MPT2SAS_ADAPTER - per adapter struct
  * @list: ioc_list
@@ -665,6 +681,7 @@ enum mutex_type {
  * @msix_vector_count: number msix vectors
  * @cpu_msix_table: table for mapping cpus to msix index
  * @cpu_msix_table_sz: table size
+ * @schedule_dead_ioc_flush_running_cmds: callback to flush pending commands
  * @scsi_io_cb_idx: shost generated commands
  * @tm_cb_idx: task management commands
  * @scsih_cb_idx: scsih internal commands
@@ -816,6 +833,7 @@ struct MPT2SAS_ADAPTER {
 	resource_size_t	**reply_post_host_index;
 	u16		cpu_msix_table_sz;
 	u32		ioc_reset_count;
+	MPT2SAS_FLUSH_RUNNING_CMDS schedule_dead_ioc_flush_running_cmds;
 
 	/* internal commands, callback index */
 	u8		scsi_io_cb_idx;
diff --git a/drivers/scsi/mpt2sas/mpt2sas_ctl.c b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
index aabcb911706e..7fceb899029e 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_ctl.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
@@ -818,6 +818,7 @@ _ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc,
 	_ctl_display_some_debug(ioc, smid, "ctl_request", NULL);
 #endif
 
+	init_completion(&ioc->ctl_cmds.done);
 	switch (mpi_request->Function) {
 	case MPI2_FUNCTION_SCSI_IO_REQUEST:
 	case MPI2_FUNCTION_RAID_SCSI_IO_PASSTHROUGH:
@@ -903,7 +904,6 @@ _ctl_do_mpt_command(struct MPT2SAS_ADAPTER *ioc,
 		timeout = MPT2_IOCTL_DEFAULT_TIMEOUT;
 	else
 		timeout = karg.timeout;
-	init_completion(&ioc->ctl_cmds.done);
 	timeleft = wait_for_completion_timeout(&ioc->ctl_cmds.done,
 	    timeout*HZ);
 	if (mpi_request->Function == MPI2_FUNCTION_SCSI_TASK_MGMT) {
@@ -1477,8 +1477,8 @@ _ctl_diag_register_2(struct MPT2SAS_ADAPTER *ioc,
 		mpi_request->ProductSpecific[i] =
 			cpu_to_le32(ioc->product_specific[buffer_type][i]);
 
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->ctl_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->ctl_cmds.done,
 	    MPT2_IOCTL_DEFAULT_TIMEOUT*HZ);
 
@@ -1821,8 +1821,8 @@ _ctl_send_release(struct MPT2SAS_ADAPTER *ioc, u8 buffer_type, u8 *issue_reset)
 	mpi_request->VF_ID = 0; /* TODO */
 	mpi_request->VP_ID = 0;
 
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->ctl_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->ctl_cmds.done,
 	    MPT2_IOCTL_DEFAULT_TIMEOUT*HZ);
 
@@ -2095,8 +2095,8 @@ _ctl_diag_read_buffer(void __user *arg, enum block_state state)
 	mpi_request->VF_ID = 0; /* TODO */
 	mpi_request->VP_ID = 0;
 
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->ctl_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->ctl_cmds.done,
 	    MPT2_IOCTL_DEFAULT_TIMEOUT*HZ);
 
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index d570573b7963..193e33e28e49 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -99,7 +99,7 @@ MODULE_PARM_DESC(logging_level, " bits for enabling additional logging info "
 
 static ushort max_sectors = 0xFFFF;
 module_param(max_sectors, ushort, 0);
-MODULE_PARM_DESC(max_sectors, "max sectors, range 64 to 8192  default=8192");
+MODULE_PARM_DESC(max_sectors, "max sectors, range 64 to 32767  default=32767");
 
 /* scsi-mid layer global parmeter is max_report_luns, which is 511 */
 #define MPT2SAS_MAX_LUN (16895)
@@ -612,13 +612,17 @@ _scsih_sas_device_add(struct MPT2SAS_ADAPTER *ioc,
 	if (!mpt2sas_transport_port_add(ioc, sas_device->handle,
 	     sas_device->sas_address_parent)) {
 		_scsih_sas_device_remove(ioc, sas_device);
-		} else if (!sas_device->starget) {
-			if (!ioc->is_driver_loading)
-				mpt2sas_transport_port_remove(ioc,
-				sas_device->sas_address,
-			    sas_device->sas_address_parent);
-			_scsih_sas_device_remove(ioc, sas_device);
-		}
+	} else if (!sas_device->starget) {
+		/* When asyn scanning is enabled, its not possible to remove
+		 * devices while scanning is turned on due to an oops in
+		 * scsi_sysfs_add_sdev()->add_device()->sysfs_addrm_start()
+		 */
+		if (!ioc->is_driver_loading)
+			mpt2sas_transport_port_remove(ioc,
+			sas_device->sas_address,
+			sas_device->sas_address_parent);
+		_scsih_sas_device_remove(ioc, sas_device);
+	}
 }
 
 /**
@@ -1007,8 +1011,8 @@ _scsih_get_chain_buffer_tracker(struct MPT2SAS_ADAPTER *ioc, u16 smid)
 	spin_lock_irqsave(&ioc->scsi_lookup_lock, flags);
 	if (list_empty(&ioc->free_chain_list)) {
 		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
-		printk(MPT2SAS_WARN_FMT "chain buffers not available\n",
-		    ioc->name);
+		dfailprintk(ioc, printk(MPT2SAS_WARN_FMT "chain buffers not "
+			"available\n", ioc->name));
 		return NULL;
 	}
 	chain_req = list_entry(ioc->free_chain_list.next,
@@ -1449,7 +1453,7 @@ _scsih_slave_destroy(struct scsi_device *sdev)
 		spin_lock_irqsave(&ioc->sas_device_lock, flags);
 		sas_device = mpt2sas_scsih_sas_device_find_by_sas_address(ioc,
 		   sas_target_priv_data->sas_address);
-		if (sas_device)
+		if (sas_device && !sas_target_priv_data->num_luns)
 			sas_device->starget = NULL;
 		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 	}
@@ -1776,11 +1780,9 @@ _scsih_init_warpdrive_properties(struct MPT2SAS_ADAPTER *ioc,
 	Mpi2ConfigReply_t mpi_reply;
 	u16 sz;
 	u8 num_pds, count;
-	u64 mb = 1024 * 1024;
-	u64 tb_2 = 2 * mb * mb;
-	u64 capacity;
-	u32 stripe_sz;
-	u8 i, stripe_exp;
+	unsigned long stripe_sz, block_sz;
+	u8 stripe_exp, block_exp;
+	u64 dev_max_lba;
 
 	if (!ioc->is_warpdrive)
 		return;
@@ -1844,51 +1846,57 @@ _scsih_init_warpdrive_properties(struct MPT2SAS_ADAPTER *ioc,
 			    vol_pg0->PhysDisk[count].PhysDiskNum);
 			goto out_error;
 		}
+		/* Disable direct I/O if member drive lba exceeds 4 bytes */
+		dev_max_lba = le64_to_cpu(pd_pg0.DeviceMaxLBA);
+		if (dev_max_lba >> 32) {
+			printk(MPT2SAS_INFO_FMT "WarpDrive : Direct IO is "
+			    "disabled for the drive with handle(0x%04x) member"
+			    "handle (0x%04x) unsupported max lba 0x%016llx\n",
+			    ioc->name, raid_device->handle,
+			    le16_to_cpu(pd_pg0.DevHandle),
+			    (unsigned long long)dev_max_lba);
+			goto out_error;
+		}
+
 		raid_device->pd_handle[count] = le16_to_cpu(pd_pg0.DevHandle);
 	}
 
 	/*
 	 * Assumption for WD: Direct I/O is not supported if the volume is
-	 * not RAID0, if the stripe size is not 64KB, if the block size is
-	 * not 512 and if the volume size is >2TB
+	 * not RAID0
 	 */
-	if (raid_device->volume_type != MPI2_RAID_VOL_TYPE_RAID0 ||
-	    le16_to_cpu(vol_pg0->BlockSize) != 512) {
+	if (raid_device->volume_type != MPI2_RAID_VOL_TYPE_RAID0) {
 		printk(MPT2SAS_INFO_FMT "WarpDrive : Direct IO is disabled "
 		    "for the drive with handle(0x%04x): type=%d, "
 		    "s_sz=%uK, blk_size=%u\n", ioc->name,
 		    raid_device->handle, raid_device->volume_type,
-		    le32_to_cpu(vol_pg0->StripeSize)/2,
+		    (le32_to_cpu(vol_pg0->StripeSize) *
+		    le16_to_cpu(vol_pg0->BlockSize)) / 1024,
 		    le16_to_cpu(vol_pg0->BlockSize));
 		goto out_error;
 	}
 
-	capacity = (u64) le16_to_cpu(vol_pg0->BlockSize) *
-	    (le64_to_cpu(vol_pg0->MaxLBA) + 1);
-
-	if (capacity > tb_2) {
+	stripe_sz = le32_to_cpu(vol_pg0->StripeSize);
+	stripe_exp = find_first_bit(&stripe_sz, 32);
+	if (stripe_exp == 32) {
 		printk(MPT2SAS_INFO_FMT "WarpDrive : Direct IO is disabled "
-		"for the drive with handle(0x%04x) since drive sz > 2TB\n",
-		ioc->name, raid_device->handle);
+		"for the drive with handle(0x%04x) invalid stripe sz %uK\n",
+		    ioc->name, raid_device->handle,
+		    (le32_to_cpu(vol_pg0->StripeSize) *
+		    le16_to_cpu(vol_pg0->BlockSize)) / 1024);
 		goto out_error;
 	}
-
-	stripe_sz = le32_to_cpu(vol_pg0->StripeSize);
-	stripe_exp = 0;
-	for (i = 0; i < 32; i++) {
-		if (stripe_sz & 1)
-			break;
-		stripe_exp++;
-		stripe_sz >>= 1;
-	}
-	if (i == 32) {
+	raid_device->stripe_exponent = stripe_exp;
+	block_sz = le16_to_cpu(vol_pg0->BlockSize);
+	block_exp = find_first_bit(&block_sz, 16);
+	if (block_exp == 16) {
 		printk(MPT2SAS_INFO_FMT "WarpDrive : Direct IO is disabled "
-		    "for the drive with handle(0x%04x) invalid stripe sz %uK\n",
+		    "for the drive with handle(0x%04x) invalid block sz %u\n",
 		    ioc->name, raid_device->handle,
-		    le32_to_cpu(vol_pg0->StripeSize)/2);
+		    le16_to_cpu(vol_pg0->BlockSize));
 		goto out_error;
 	}
-	raid_device->stripe_exponent = stripe_exp;
+	raid_device->block_exponent = block_exp;
 	raid_device->direct_io_enabled = 1;
 
 	printk(MPT2SAS_INFO_FMT "WarpDrive : Direct IO is Enabled for the drive"
@@ -3804,8 +3812,9 @@ _scsih_setup_direct_io(struct MPT2SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
 {
 	u32 v_lba, p_lba, stripe_off, stripe_unit, column, io_size;
 	u32 stripe_sz, stripe_exp;
-	u8 num_pds, *cdb_ptr, *tmp_ptr, *lba_ptr1, *lba_ptr2;
+	u8 num_pds, *cdb_ptr, i;
 	u8 cdb0 = scmd->cmnd[0];
+	u64 v_llba;
 
 	/*
 	 * Try Direct I/O to RAID memeber disks
@@ -3816,15 +3825,11 @@ _scsih_setup_direct_io(struct MPT2SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
 
 		if ((cdb0 < READ_16) || !(cdb_ptr[2] | cdb_ptr[3] | cdb_ptr[4]
 			| cdb_ptr[5])) {
-			io_size = scsi_bufflen(scmd) >> 9;
+			io_size = scsi_bufflen(scmd) >>
+			    raid_device->block_exponent;
+			i = (cdb0 < READ_16) ? 2 : 6;
 			/* get virtual lba */
-			lba_ptr1 = lba_ptr2 = (cdb0 < READ_16) ? &cdb_ptr[2] :
-			    &cdb_ptr[6];
-			tmp_ptr = (u8 *)&v_lba + 3;
-			*tmp_ptr-- = *lba_ptr1++;
-			*tmp_ptr-- = *lba_ptr1++;
-			*tmp_ptr-- = *lba_ptr1++;
-			*tmp_ptr = *lba_ptr1;
+			v_lba = be32_to_cpu(*(__be32 *)(&cdb_ptr[i]));
 
 			if (((u64)v_lba + (u64)io_size - 1) <=
 			    (u32)raid_device->max_lba) {
@@ -3843,11 +3848,39 @@ _scsih_setup_direct_io(struct MPT2SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
 					mpi_request->DevHandle =
 						cpu_to_le16(raid_device->
 						    pd_handle[column]);
-					tmp_ptr = (u8 *)&p_lba + 3;
-					*lba_ptr2++ = *tmp_ptr--;
-					*lba_ptr2++ = *tmp_ptr--;
-					*lba_ptr2++ = *tmp_ptr--;
-					*lba_ptr2 = *tmp_ptr;
+					(*(__be32 *)(&cdb_ptr[i])) =
+						cpu_to_be32(p_lba);
+					/*
+					* WD: To indicate this I/O is directI/O
+					*/
+					_scsih_scsi_direct_io_set(ioc, smid, 1);
+				}
+			}
+		} else {
+			io_size = scsi_bufflen(scmd) >>
+			    raid_device->block_exponent;
+			/* get virtual lba */
+			v_llba = be64_to_cpu(*(__be64 *)(&cdb_ptr[2]));
+
+			if ((v_llba + (u64)io_size - 1) <=
+			    raid_device->max_lba) {
+				stripe_sz = raid_device->stripe_sz;
+				stripe_exp = raid_device->stripe_exponent;
+				stripe_off = (u32) (v_llba & (stripe_sz - 1));
+
+				/* Check whether IO falls within a stripe */
+				if ((stripe_off + io_size) <= stripe_sz) {
+					num_pds = raid_device->num_pds;
+					p_lba = (u32)(v_llba >> stripe_exp);
+					stripe_unit = p_lba / num_pds;
+					column = p_lba % num_pds;
+					p_lba = (stripe_unit << stripe_exp) +
+					    stripe_off;
+					mpi_request->DevHandle =
+						cpu_to_le16(raid_device->
+						    pd_handle[column]);
+					(*(__be64 *)(&cdb_ptr[2])) =
+					    cpu_to_be64((u64)p_lba);
 					/*
 					* WD: To indicate this I/O is directI/O
 					*/
@@ -4403,11 +4436,14 @@ _scsih_io_done(struct MPT2SAS_ADAPTER *ioc, u16 smid, u8 msix_index, u32 reply)
 		scmd->result = DID_NO_CONNECT << 16;
 		goto out;
 	}
+	ioc_status = le16_to_cpu(mpi_reply->IOCStatus);
 	/*
 	 * WARPDRIVE: If direct_io is set then it is directIO,
 	 * the failed direct I/O should be redirected to volume
 	 */
-	if (_scsih_scsi_direct_io_get(ioc, smid)) {
+	if (_scsih_scsi_direct_io_get(ioc, smid) &&
+	    ((ioc_status & MPI2_IOCSTATUS_MASK)
+	    != MPI2_IOCSTATUS_SCSI_TASK_TERMINATED)) {
 		spin_lock_irqsave(&ioc->scsi_lookup_lock, flags);
 		ioc->scsi_lookup[smid - 1].scmd = scmd;
 		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
@@ -4441,7 +4477,6 @@ _scsih_io_done(struct MPT2SAS_ADAPTER *ioc, u16 smid, u8 msix_index, u32 reply)
 
 	xfer_cnt = le32_to_cpu(mpi_reply->TransferCount);
 	scsi_set_resid(scmd, scsi_bufflen(scmd) - xfer_cnt);
-	ioc_status = le16_to_cpu(mpi_reply->IOCStatus);
 	if (ioc_status & MPI2_IOCSTATUS_FLAG_LOG_INFO_AVAILABLE)
 		log_info =  le32_to_cpu(mpi_reply->IOCLogInfo);
 	else
@@ -4485,6 +4520,8 @@ _scsih_io_done(struct MPT2SAS_ADAPTER *ioc, u16 smid, u8 msix_index, u32 reply)
 			scmd->result = DID_TRANSPORT_DISRUPTED << 16;
 			goto out;
 		}
+		scmd->result = DID_SOFT_ERROR << 16;
+		break;
 	case MPI2_IOCSTATUS_SCSI_TASK_TERMINATED:
 	case MPI2_IOCSTATUS_SCSI_EXT_TERMINATED:
 		scmd->result = DID_RESET << 16;
@@ -6714,6 +6751,7 @@ _scsih_mark_responding_raid_device(struct MPT2SAS_ADAPTER *ioc, u64 wwid,
 			} else
 				sas_target_priv_data = NULL;
 			raid_device->responding = 1;
+			spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 			starget_printk(KERN_INFO, raid_device->starget,
 			    "handle(0x%04x), wwid(0x%016llx)\n", handle,
 			    (unsigned long long)raid_device->wwid);
@@ -6724,16 +6762,16 @@ _scsih_mark_responding_raid_device(struct MPT2SAS_ADAPTER *ioc, u64 wwid,
 			 */
 			_scsih_init_warpdrive_properties(ioc, raid_device);
 			if (raid_device->handle == handle)
-				goto out;
+				return;
 			printk(KERN_INFO "\thandle changed from(0x%04x)!!!\n",
 			    raid_device->handle);
 			raid_device->handle = handle;
 			if (sas_target_priv_data)
 				sas_target_priv_data->handle = handle;
-			goto out;
+			return;
 		}
 	}
- out:
+
 	spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 }
 
@@ -7418,7 +7456,7 @@ static struct scsi_host_template scsih_driver_template = {
 	.can_queue			= 1,
 	.this_id			= -1,
 	.sg_tablesize			= MPT2SAS_SG_DEPTH,
-	.max_sectors			= 8192,
+	.max_sectors			= 32767,
 	.cmd_per_lun			= 7,
 	.use_clustering			= ENABLE_CLUSTERING,
 	.shost_attrs			= mpt2sas_host_attrs,
@@ -7928,6 +7966,7 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	ioc->tm_tr_volume_cb_idx = tm_tr_volume_cb_idx;
 	ioc->tm_sas_control_cb_idx = tm_sas_control_cb_idx;
 	ioc->logging_level = logging_level;
+	ioc->schedule_dead_ioc_flush_running_cmds = &_scsih_flush_running_cmds;
 	/* misc semaphores and spin locks */
 	mutex_init(&ioc->reset_in_progress_mutex);
 	spin_lock_init(&ioc->ioc_reset_in_progress_lock);
@@ -7958,11 +7997,11 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 			printk(MPT2SAS_WARN_FMT "Invalid value %d passed "
 			    "for max_sectors, range is 64 to 8192. Assigning "
 			    "value of 64.\n", ioc->name, max_sectors);
-		} else if (max_sectors > 8192) {
-			shost->max_sectors = 8192;
+		} else if (max_sectors > 32767) {
+			shost->max_sectors = 32767;
 			printk(MPT2SAS_WARN_FMT "Invalid value %d passed "
 			    "for max_sectors, range is 64 to 8192. Assigning "
-			    "default value of 8192.\n", ioc->name,
+			    "default value of 32767.\n", ioc->name,
 			    max_sectors);
 		} else {
 			shost->max_sectors = max_sectors & 0xFFFE;
@@ -8000,7 +8039,6 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto out_attach_fail;
 	}
 
-	scsi_scan_host(shost);
 	if (ioc->is_warpdrive) {
 		if (ioc->mfg_pg10_hide_flag ==  MFG_PAGE10_EXPOSE_ALL_DISKS)
 			ioc->hide_drives = 0;
@@ -8014,8 +8052,8 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		}
 	} else
 		ioc->hide_drives = 0;
+	scsi_scan_host(shost);
 
-	_scsih_probe_devices(ioc);
 	return 0;
 
  out_attach_fail:
diff --git a/drivers/scsi/mpt2sas/mpt2sas_transport.c b/drivers/scsi/mpt2sas/mpt2sas_transport.c
index 230732241aa2..831047466a5a 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_transport.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_transport.c
@@ -398,8 +398,8 @@ _transport_expander_report_manufacture(struct MPT2SAS_ADAPTER *ioc,
 	dtransportprintk(ioc, printk(MPT2SAS_INFO_FMT "report_manufacture - "
 	    "send to sas_addr(0x%016llx)\n", ioc->name,
 	    (unsigned long long)sas_address));
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->transport_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->transport_cmds.done,
 	    10*HZ);
 
@@ -1184,8 +1184,8 @@ _transport_get_expander_phy_error_log(struct MPT2SAS_ADAPTER *ioc,
 	dtransportprintk(ioc, printk(MPT2SAS_INFO_FMT "phy_error_log - "
 	    "send to sas_addr(0x%016llx), phy(%d)\n", ioc->name,
 	    (unsigned long long)phy->identify.sas_address, phy->number));
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->transport_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->transport_cmds.done,
 	    10*HZ);
 
@@ -1509,8 +1509,9 @@ _transport_expander_phy_control(struct MPT2SAS_ADAPTER *ioc,
 	    "send to sas_addr(0x%016llx), phy(%d), opcode(%d)\n", ioc->name,
 	    (unsigned long long)phy->identify.sas_address, phy->number,
 	    phy_operation));
-	mpt2sas_base_put_smid_default(ioc, smid);
+
 	init_completion(&ioc->transport_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->transport_cmds.done,
 	    10*HZ);
 
@@ -1949,8 +1950,8 @@ _transport_smp_handler(struct Scsi_Host *shost, struct sas_rphy *rphy,
 	dtransportprintk(ioc, printk(MPT2SAS_INFO_FMT "%s - "
 	    "sending smp request\n", ioc->name, __func__));
 
-	mpt2sas_base_put_smid_default(ioc, smid);
 	init_completion(&ioc->transport_cmds.done);
+	mpt2sas_base_put_smid_default(ioc, smid);
 	timeleft = wait_for_completion_timeout(&ioc->transport_cmds.done,
 	    10*HZ);
 
diff --git a/drivers/scsi/qla2xxx/qla_attr.c b/drivers/scsi/qla2xxx/qla_attr.c
index 6465dae5883a..a2f1b3043dfb 100644
--- a/drivers/scsi/qla2xxx/qla_attr.c
+++ b/drivers/scsi/qla2xxx/qla_attr.c
@@ -107,7 +107,7 @@ qla2x00_sysfs_write_fw_dump(struct file *filp, struct kobject *kobj,
 			set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
 		break;
 	}
-	return -EINVAL;
+	return count;
 }
 
 static struct bin_attribute sysfs_fw_dump_attr = {
@@ -387,7 +387,7 @@ qla2x00_sysfs_write_optrom_ctl(struct file *filp, struct kobject *kobj,
 		break;
 	case 3:
 		if (ha->optrom_state != QLA_SWRITING)
-			return -ENOMEM;
+			return -EINVAL;
 
 		if (qla2x00_wait_for_hba_online(vha) != QLA_SUCCESS) {
 			ql_log(ql_log_warn, vha, 0x7068,
@@ -667,7 +667,7 @@ qla2x00_sysfs_write_edc(struct file *filp, struct kobject *kobj,
 	    dev, adr, len, opt);
 	if (rval != QLA_SUCCESS) {
 		ql_log(ql_log_warn, vha, 0x7074,
-		    "Unable to write EDC (%x) %02x:%04x:%02x:%02hhx\n",
+		    "Unable to write EDC (%x) %02x:%04x:%02x:%02x:%02hhx\n",
 		    rval, dev, adr, opt, len, buf[8]);
 		return -EIO;
 	}
@@ -724,7 +724,7 @@ qla2x00_sysfs_write_edc_status(struct file *filp, struct kobject *kobj,
 			dev, adr, len, opt);
 	if (rval != QLA_SUCCESS) {
 		ql_log(ql_log_info, vha, 0x7075,
-		    "Unable to write EDC status (%x) %02x:%04x:%02x.\n",
+		    "Unable to write EDC status (%x) %02x:%04x:%02x:%02x.\n",
 		    rval, dev, adr, opt, len);
 		return -EIO;
 	}
@@ -1971,8 +1971,8 @@ qla24xx_vport_delete(struct fc_vport *fc_vport)
 			    "Queue delete failed.\n");
 	}
 
-	scsi_host_put(vha->host);
 	ql_log(ql_log_info, vha, 0x7088, "VP[%d] deleted.\n", id);
+	scsi_host_put(vha->host);
 	return 0;
 }
 
diff --git a/drivers/scsi/qla2xxx/qla_bsg.c b/drivers/scsi/qla2xxx/qla_bsg.c
index 8b641a8a0c74..b1d0f936bf2d 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.c
+++ b/drivers/scsi/qla2xxx/qla_bsg.c
@@ -31,6 +31,7 @@ qla2x00_get_ctx_bsg_sp(scsi_qla_host_t *vha, fc_port_t *fcport, size_t size)
 	memset(sp, 0, sizeof(*sp));
 	sp->fcport = fcport;
 	sp->ctx = ctx;
+	ctx->iocbs = 1;
 done:
 	return sp;
 }
@@ -102,7 +103,7 @@ qla24xx_proc_fcp_prio_cfg_cmd(struct fc_bsg_job *bsg_job)
 
 	bsg_job->reply->reply_payload_rcv_len = 0;
 
-	if (!(IS_QLA24XX_TYPE(ha) || IS_QLA25XX(ha))) {
+	if (!(IS_QLA24XX_TYPE(ha) || IS_QLA25XX(ha) || IS_QLA82XX(ha))) {
 		ret = -EINVAL;
 		goto exit_fcp_prio_cfg;
 	}
@@ -389,6 +390,20 @@ done:
 	return rval;
 }
 
+inline uint16_t
+qla24xx_calc_ct_iocbs(uint16_t dsds)
+{
+	uint16_t iocbs;
+
+	iocbs = 1;
+	if (dsds > 2) {
+		iocbs += (dsds - 2) / 5;
+		if ((dsds - 2) % 5)
+			iocbs++;
+	}
+	return iocbs;
+}
+
 static int
 qla2x00_process_ct(struct fc_bsg_job *bsg_job)
 {
@@ -489,6 +504,7 @@ qla2x00_process_ct(struct fc_bsg_job *bsg_job)
 	ct = sp->ctx;
 	ct->type = SRB_CT_CMD;
 	ct->name = "bsg_ct";
+	ct->iocbs = qla24xx_calc_ct_iocbs(req_sg_cnt + rsp_sg_cnt);
 	ct->u.bsg_job = bsg_job;
 
 	ql_dbg(ql_dbg_user, vha, 0x7016,
@@ -1653,7 +1669,7 @@ qla24xx_bsg_request(struct fc_bsg_job *bsg_job)
 	}
 
 	ql_dbg(ql_dbg_user, vha, 0x7000,
-	    "Entered %s msgcode=%d.\n", __func__, bsg_job->request->msgcode);
+	    "Entered %s msgcode=0x%x.\n", __func__, bsg_job->request->msgcode);
 
 	switch (bsg_job->request->msgcode) {
 	case FC_BSG_RPT_ELS:
diff --git a/drivers/scsi/qla2xxx/qla_dbg.c b/drivers/scsi/qla2xxx/qla_dbg.c
index f3cddd5800c3..7c54624b5b13 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.c
+++ b/drivers/scsi/qla2xxx/qla_dbg.c
@@ -11,15 +11,17 @@
  * ----------------------------------------------------------------------
  * |             Level            |   Last Value Used  |     Holes	|
  * ----------------------------------------------------------------------
- * | Module Init and Probe        |       0x0116       |  		|
+ * | Module Init and Probe        |       0x0116       | 0xfa           |
  * | Mailbox commands             |       0x112b       |		|
- * | Device Discovery             |       0x2083       |		|
- * | Queue Command and IO tracing |       0x302e       |     0x3008     |
+ * | Device Discovery             |       0x2084       |		|
+ * | Queue Command and IO tracing |       0x302f       | 0x3008,0x302d, |
+ * |                              |                    | 0x302e         |
  * | DPC Thread                   |       0x401c       |		|
- * | Async Events                 |       0x5059       |		|
- * | Timer Routines               |       0x6010       | 0x600e,0x600f  |
- * | User Space Interactions      |       0x709d       |		|
- * | Task Management              |       0x8041       | 0x800b         |
+ * | Async Events                 |       0x5057       | 0x5052		|
+ * | Timer Routines               |       0x6011       | 0x600e,0x600f  |
+ * | User Space Interactions      |       0x709e       |		|
+ * | Task Management              |       0x803c       | 0x8025-0x8026  |
+ * |                              |                    | 0x800b,0x8039  |
  * | AER/EEH                      |       0x900f       |		|
  * | Virtual Port                 |       0xa007       |		|
  * | ISP82XX Specific             |       0xb052       |    		|
@@ -368,7 +370,7 @@ qla25xx_copy_fce(struct qla_hw_data *ha, void *ptr, uint32_t **last_chain)
 
 	memcpy(iter_reg, ha->fce, ntohl(fcec->size));
 
-	return iter_reg;
+	return (char *)iter_reg + ntohl(fcec->size);
 }
 
 static inline void *
@@ -1650,6 +1652,15 @@ qla81xx_fw_dump_failed:
 /****************************************************************************/
 /*                         Driver Debug Functions.                          */
 /****************************************************************************/
+
+static inline int
+ql_mask_match(uint32_t level)
+{
+	if (ql2xextended_error_logging == 1)
+		ql2xextended_error_logging = QL_DBG_DEFAULT1_MASK;
+	return (level & ql2xextended_error_logging) == level;
+}
+
 /*
  * This function is for formatting and logging debug information.
  * It is to be used when vha is available. It formats the message
@@ -1664,34 +1675,31 @@ qla81xx_fw_dump_failed:
  * msg:   The message to be displayed.
  */
 void
-ql_dbg(uint32_t level, scsi_qla_host_t *vha, int32_t id, char *msg, ...) {
-
-	char pbuf[QL_DBG_BUF_LEN];
-	va_list ap;
-	uint32_t len;
-	struct pci_dev *pdev = NULL;
-
-	memset(pbuf, 0, QL_DBG_BUF_LEN);
-
-	va_start(ap, msg);
-
-	if ((level & ql2xextended_error_logging) == level) {
-		if (vha != NULL) {
-			pdev = vha->hw->pdev;
-			/* <module-name> <pci-name> <msg-id>:<host> Message */
-			sprintf(pbuf, "%s [%s]-%04x:%ld: ", QL_MSGHDR,
-			    dev_name(&(pdev->dev)), id + ql_dbg_offset,
-			    vha->host_no);
-		} else
-			sprintf(pbuf, "%s [%s]-%04x: : ", QL_MSGHDR,
-			    "0000:00:00.0", id + ql_dbg_offset);
-
-		len = strlen(pbuf);
-		vsprintf(pbuf+len, msg, ap);
-		pr_warning("%s", pbuf);
+ql_dbg(uint32_t level, scsi_qla_host_t *vha, int32_t id, const char *fmt, ...)
+{
+	va_list va;
+	struct va_format vaf;
+
+	if (!ql_mask_match(level))
+		return;
+
+	va_start(va, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &va;
+
+	if (vha != NULL) {
+		const struct pci_dev *pdev = vha->hw->pdev;
+		/* <module-name> <pci-name> <msg-id>:<host> Message */
+		pr_warn("%s [%s]-%04x:%ld: %pV",
+			QL_MSGHDR, dev_name(&(pdev->dev)), id + ql_dbg_offset,
+			vha->host_no, &vaf);
+	} else {
+		pr_warn("%s [%s]-%04x: : %pV",
+			QL_MSGHDR, "0000:00:00.0", id + ql_dbg_offset, &vaf);
 	}
 
-	va_end(ap);
+	va_end(va);
 
 }
 
@@ -1710,31 +1718,27 @@ ql_dbg(uint32_t level, scsi_qla_host_t *vha, int32_t id, char *msg, ...) {
  * msg:   The message to be displayed.
  */
 void
-ql_dbg_pci(uint32_t level, struct pci_dev *pdev, int32_t id, char *msg, ...) {
-
-	char pbuf[QL_DBG_BUF_LEN];
-	va_list ap;
-	uint32_t len;
+ql_dbg_pci(uint32_t level, struct pci_dev *pdev, int32_t id,
+	   const char *fmt, ...)
+{
+	va_list va;
+	struct va_format vaf;
 
 	if (pdev == NULL)
 		return;
+	if (!ql_mask_match(level))
+		return;
 
-	memset(pbuf, 0, QL_DBG_BUF_LEN);
-
-	va_start(ap, msg);
-
-	if ((level & ql2xextended_error_logging) == level) {
-		/* <module-name> <dev-name>:<msg-id> Message */
-		sprintf(pbuf, "%s [%s]-%04x: : ", QL_MSGHDR,
-		    dev_name(&(pdev->dev)), id + ql_dbg_offset);
+	va_start(va, fmt);
 
-		len = strlen(pbuf);
-		vsprintf(pbuf+len, msg, ap);
-		pr_warning("%s", pbuf);
-	}
+	vaf.fmt = fmt;
+	vaf.va = &va;
 
-	va_end(ap);
+	/* <module-name> <dev-name>:<msg-id> Message */
+	pr_warn("%s [%s]-%04x: : %pV",
+		QL_MSGHDR, dev_name(&(pdev->dev)), id + ql_dbg_offset, &vaf);
 
+	va_end(va);
 }
 
 /*
@@ -1751,47 +1755,47 @@ ql_dbg_pci(uint32_t level, struct pci_dev *pdev, int32_t id, char *msg, ...) {
  * msg:   The message to be displayed.
  */
 void
-ql_log(uint32_t level, scsi_qla_host_t *vha, int32_t id, char *msg, ...) {
-
-	char pbuf[QL_DBG_BUF_LEN];
-	va_list ap;
-	uint32_t len;
-	struct pci_dev *pdev = NULL;
-
-	memset(pbuf, 0, QL_DBG_BUF_LEN);
-
-	va_start(ap, msg);
-
-	if (level <= ql_errlev) {
-		if (vha != NULL) {
-			pdev = vha->hw->pdev;
-			/* <module-name> <msg-id>:<host> Message */
-			sprintf(pbuf, "%s [%s]-%04x:%ld: ", QL_MSGHDR,
-			    dev_name(&(pdev->dev)), id, vha->host_no);
-		} else
-			sprintf(pbuf, "%s [%s]-%04x: : ", QL_MSGHDR,
-			    "0000:00:00.0", id);
+ql_log(uint32_t level, scsi_qla_host_t *vha, int32_t id, const char *fmt, ...)
+{
+	va_list va;
+	struct va_format vaf;
+	char pbuf[128];
 
-		len = strlen(pbuf);
-			vsprintf(pbuf+len, msg, ap);
+	if (level > ql_errlev)
+		return;
 
-		switch (level) {
-		case 0: /* FATAL LOG */
-			pr_crit("%s", pbuf);
-			break;
-		case 1:
-			pr_err("%s", pbuf);
-			break;
-		case 2:
-			pr_warn("%s", pbuf);
-			break;
-		default:
-			pr_info("%s", pbuf);
-			break;
-		}
+	if (vha != NULL) {
+		const struct pci_dev *pdev = vha->hw->pdev;
+		/* <module-name> <msg-id>:<host> Message */
+		snprintf(pbuf, sizeof(pbuf), "%s [%s]-%04x:%ld: ",
+			QL_MSGHDR, dev_name(&(pdev->dev)), id, vha->host_no);
+	} else {
+		snprintf(pbuf, sizeof(pbuf), "%s [%s]-%04x: : ",
+			QL_MSGHDR, "0000:00:00.0", id);
+	}
+	pbuf[sizeof(pbuf) - 1] = 0;
+
+	va_start(va, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &va;
+
+	switch (level) {
+	case 0: /* FATAL LOG */
+		pr_crit("%s%pV", pbuf, &vaf);
+		break;
+	case 1:
+		pr_err("%s%pV", pbuf, &vaf);
+		break;
+	case 2:
+		pr_warn("%s%pV", pbuf, &vaf);
+		break;
+	default:
+		pr_info("%s%pV", pbuf, &vaf);
+		break;
 	}
 
-	va_end(ap);
+	va_end(va);
 }
 
 /*
@@ -1809,43 +1813,44 @@ ql_log(uint32_t level, scsi_qla_host_t *vha, int32_t id, char *msg, ...) {
  * msg:   The message to be displayed.
  */
 void
-ql_log_pci(uint32_t level, struct pci_dev *pdev, int32_t id, char *msg, ...) {
-
-	char pbuf[QL_DBG_BUF_LEN];
-	va_list ap;
-	uint32_t len;
+ql_log_pci(uint32_t level, struct pci_dev *pdev, int32_t id,
+	   const char *fmt, ...)
+{
+	va_list va;
+	struct va_format vaf;
+	char pbuf[128];
 
 	if (pdev == NULL)
 		return;
+	if (level > ql_errlev)
+		return;
 
-	memset(pbuf, 0, QL_DBG_BUF_LEN);
-
-	va_start(ap, msg);
-
-	if (level <= ql_errlev) {
-		/* <module-name> <dev-name>:<msg-id> Message */
-		sprintf(pbuf, "%s [%s]-%04x: : ", QL_MSGHDR,
-		    dev_name(&(pdev->dev)), id);
-
-		len = strlen(pbuf);
-		vsprintf(pbuf+len, msg, ap);
-		switch (level) {
-		case 0: /* FATAL LOG */
-			pr_crit("%s", pbuf);
-			break;
-		case 1:
-			pr_err("%s", pbuf);
-			break;
-		case 2:
-			pr_warn("%s", pbuf);
-			break;
-		default:
-			pr_info("%s", pbuf);
-			break;
-		}
+	/* <module-name> <dev-name>:<msg-id> Message */
+	snprintf(pbuf, sizeof(pbuf), "%s [%s]-%04x: : ",
+		 QL_MSGHDR, dev_name(&(pdev->dev)), id);
+	pbuf[sizeof(pbuf) - 1] = 0;
+
+	va_start(va, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &va;
+
+	switch (level) {
+	case 0: /* FATAL LOG */
+		pr_crit("%s%pV", pbuf, &vaf);
+		break;
+	case 1:
+		pr_err("%s%pV", pbuf, &vaf);
+		break;
+	case 2:
+		pr_warn("%s%pV", pbuf, &vaf);
+		break;
+	default:
+		pr_info("%s%pV", pbuf, &vaf);
+		break;
 	}
 
-	va_end(ap);
+	va_end(va);
 }
 
 void
@@ -1858,20 +1863,20 @@ ql_dump_regs(uint32_t level, scsi_qla_host_t *vha, int32_t id)
 	struct device_reg_82xx __iomem *reg82 = &ha->iobase->isp82;
 	uint16_t __iomem *mbx_reg;
 
-	if ((level & ql2xextended_error_logging) == level) {
-
-		if (IS_QLA82XX(ha))
-			mbx_reg = &reg82->mailbox_in[0];
-		else if (IS_FWI2_CAPABLE(ha))
-			mbx_reg = &reg24->mailbox0;
-		else
-			mbx_reg = MAILBOX_REG(ha, reg, 0);
+	if (!ql_mask_match(level))
+		return;
 
-		ql_dbg(level, vha, id, "Mailbox registers:\n");
-		for (i = 0; i < 6; i++)
-			ql_dbg(level, vha, id,
-			    "mbox[%d] 0x%04x\n", i, RD_REG_WORD(mbx_reg++));
-	}
+	if (IS_QLA82XX(ha))
+		mbx_reg = &reg82->mailbox_in[0];
+	else if (IS_FWI2_CAPABLE(ha))
+		mbx_reg = &reg24->mailbox0;
+	else
+		mbx_reg = MAILBOX_REG(ha, reg, 0);
+
+	ql_dbg(level, vha, id, "Mailbox registers:\n");
+	for (i = 0; i < 6; i++)
+		ql_dbg(level, vha, id,
+		    "mbox[%d] 0x%04x\n", i, RD_REG_WORD(mbx_reg++));
 }
 
 
@@ -1881,24 +1886,25 @@ ql_dump_buffer(uint32_t level, scsi_qla_host_t *vha, int32_t id,
 {
 	uint32_t cnt;
 	uint8_t c;
-	if ((level & ql2xextended_error_logging) == level) {
-
-		ql_dbg(level, vha, id, " 0   1   2   3   4   5   6   7   8   "
-		    "9  Ah  Bh  Ch  Dh  Eh  Fh\n");
-		ql_dbg(level, vha, id, "----------------------------------"
-		    "----------------------------\n");
-
-		ql_dbg(level, vha, id, "");
-		for (cnt = 0; cnt < size;) {
-			c = *b++;
-			printk("%02x", (uint32_t) c);
-			cnt++;
-			if (!(cnt % 16))
-				printk("\n");
-			else
-				printk("  ");
-		}
-		if (cnt % 16)
-			ql_dbg(level, vha, id, "\n");
+
+	if (!ql_mask_match(level))
+		return;
+
+	ql_dbg(level, vha, id, " 0   1   2   3   4   5   6   7   8   "
+	    "9  Ah  Bh  Ch  Dh  Eh  Fh\n");
+	ql_dbg(level, vha, id, "----------------------------------"
+	    "----------------------------\n");
+
+	ql_dbg(level, vha, id, " ");
+	for (cnt = 0; cnt < size;) {
+		c = *b++;
+		printk("%02x", (uint32_t) c);
+		cnt++;
+		if (!(cnt % 16))
+			printk("\n");
+		else
+			printk("  ");
 	}
+	if (cnt % 16)
+		ql_dbg(level, vha, id, "\n");
 }
diff --git a/drivers/scsi/qla2xxx/qla_dbg.h b/drivers/scsi/qla2xxx/qla_dbg.h
index 98a377b99017..5f1b6d9c3dcb 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.h
+++ b/drivers/scsi/qla2xxx/qla_dbg.h
@@ -232,6 +232,7 @@ struct qla2xxx_fw_dump {
 };
 
 #define QL_MSGHDR "qla2xxx"
+#define QL_DBG_DEFAULT1_MASK    0x1e400000
 
 #define ql_log_fatal		0 /* display fatal errors */
 #define ql_log_warn		1 /* display critical errors */
@@ -244,15 +245,15 @@ struct qla2xxx_fw_dump {
 
 extern int ql_errlev;
 
-void
-ql_dbg(uint32_t, scsi_qla_host_t *vha, int32_t, char *, ...);
-void
-ql_dbg_pci(uint32_t, struct pci_dev *pdev, int32_t, char *, ...);
+void __attribute__((format (printf, 4, 5)))
+ql_dbg(uint32_t, scsi_qla_host_t *vha, int32_t, const char *fmt, ...);
+void __attribute__((format (printf, 4, 5)))
+ql_dbg_pci(uint32_t, struct pci_dev *pdev, int32_t, const char *fmt, ...);
 
-void
-ql_log(uint32_t, scsi_qla_host_t *vha, int32_t, char *, ...);
-void
-ql_log_pci(uint32_t, struct pci_dev *pdev, int32_t, char *, ...);
+void __attribute__((format (printf, 4, 5)))
+ql_log(uint32_t, scsi_qla_host_t *vha, int32_t, const char *fmt, ...);
+void __attribute__((format (printf, 4, 5)))
+ql_log_pci(uint32_t, struct pci_dev *pdev, int32_t, const char *fmt, ...);
 
 /* Debug Levels */
 /* The 0x40000000 is the max value any debug level can have
@@ -275,5 +276,3 @@ ql_log_pci(uint32_t, struct pci_dev *pdev, int32_t, char *, ...);
 #define ql_dbg_misc	0x00010000 /* For dumping everything that is not
 				    * not covered by upper categories
 				    */
-
-#define QL_DBG_BUF_LEN	512
diff --git a/drivers/scsi/qla2xxx/qla_def.h b/drivers/scsi/qla2xxx/qla_def.h
index fcf052c50bf5..a6a4eebce4a8 100644
--- a/drivers/scsi/qla2xxx/qla_def.h
+++ b/drivers/scsi/qla2xxx/qla_def.h
@@ -271,6 +271,7 @@ struct srb_iocb {
 struct srb_ctx {
 	uint16_t type;
 	char *name;
+	int iocbs;
 	union {
 		struct srb_iocb *iocb_cmd;
 		struct fc_bsg_job *bsg_job;
@@ -2244,6 +2245,7 @@ struct isp_operations {
 	int (*get_flash_version) (struct scsi_qla_host *, void *);
 	int (*start_scsi) (srb_t *);
 	int (*abort_isp) (struct scsi_qla_host *);
+	int (*iospace_config)(struct qla_hw_data*);
 };
 
 /* MSI-X Support *************************************************************/
@@ -2978,10 +2980,6 @@ typedef struct scsi_qla_host {
 	atomic_dec(&__vha->vref_count);			     \
 } while (0)
 
-
-#define qla_printk(level, ha, format, arg...) \
-	dev_printk(level , &((ha)->pdev->dev) , format , ## arg)
-
 /*
  * qla2x00 local function return status codes
  */
diff --git a/drivers/scsi/qla2xxx/qla_gbl.h b/drivers/scsi/qla2xxx/qla_gbl.h
index c0c11afb685c..408679be8fdf 100644
--- a/drivers/scsi/qla2xxx/qla_gbl.h
+++ b/drivers/scsi/qla2xxx/qla_gbl.h
@@ -572,7 +572,7 @@ extern void qla2x00_set_model_info(scsi_qla_host_t *, uint8_t *,
     size_t, char *);
 extern int qla82xx_mbx_intr_enable(scsi_qla_host_t *);
 extern int qla82xx_mbx_intr_disable(scsi_qla_host_t *);
-extern void qla82xx_start_iocbs(srb_t *);
+extern void qla82xx_start_iocbs(scsi_qla_host_t *);
 extern int qla82xx_fcoe_ctx_reset(scsi_qla_host_t *);
 extern int qla82xx_check_md_needed(scsi_qla_host_t *);
 extern void qla82xx_chip_reset_cleanup(scsi_qla_host_t *);
diff --git a/drivers/scsi/qla2xxx/qla_gs.c b/drivers/scsi/qla2xxx/qla_gs.c
index 37937aa3c3b8..4aea4ae23300 100644
--- a/drivers/scsi/qla2xxx/qla_gs.c
+++ b/drivers/scsi/qla2xxx/qla_gs.c
@@ -758,7 +758,7 @@ qla2x00_sns_ga_nxt(scsi_qla_host_t *vha, fc_port_t *fcport)
 		    "GA_NXT Send SNS failed (%d).\n", rval);
 	} else if (sns_cmd->p.gan_data[8] != 0x80 ||
 	    sns_cmd->p.gan_data[9] != 0x02) {
-		ql_dbg(ql_dbg_disc + ql_dbg_buffer, vha, 0x207d,
+		ql_dbg(ql_dbg_disc + ql_dbg_buffer, vha, 0x2084,
 		    "GA_NXT failed, rejected request ga_nxt_rsp:\n");
 		ql_dump_buffer(ql_dbg_disc + ql_dbg_buffer, vha, 0x2074,
 		    sns_cmd->p.gan_data, 16);
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index 54ea68cec4c5..1fa067e053d2 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -111,6 +111,7 @@ qla2x00_get_ctx_sp(scsi_qla_host_t *vha, fc_port_t *fcport, size_t size,
 	memset(sp, 0, sizeof(*sp));
 	sp->fcport = fcport;
 	sp->ctx = ctx;
+	ctx->iocbs = 1;
 	ctx->u.iocb_cmd = iocb;
 	iocb->free = qla2x00_ctx_sp_free;
 
@@ -154,8 +155,8 @@ qla2x00_async_iocb_timeout(srb_t *sp)
 	struct srb_ctx *ctx = sp->ctx;
 
 	ql_dbg(ql_dbg_disc, fcport->vha, 0x2071,
-	    "Async-%s timeout - portid=%02x%02x%02x.\n",
-	    ctx->name, fcport->d_id.b.domain, fcport->d_id.b.area,
+	    "Async-%s timeout - hdl=%x portid=%02x%02x%02x.\n",
+	    ctx->name, sp->handle, fcport->d_id.b.domain, fcport->d_id.b.area,
 	    fcport->d_id.b.al_pa);
 
 	fcport->flags &= ~FCF_ASYNC_SENT;
@@ -211,9 +212,10 @@ qla2x00_async_login(struct scsi_qla_host *vha, fc_port_t *fcport,
 		goto done_free_sp;
 
 	ql_dbg(ql_dbg_disc, vha, 0x2072,
-	    "Async-login - loopid=%x portid=%02x%02x%02x retries=%d.\n",
-	    fcport->loop_id, fcport->d_id.b.domain, fcport->d_id.b.area,
-	    fcport->d_id.b.al_pa, fcport->login_retry);
+	    "Async-login - hdl=%x, loopid=%x portid=%02x%02x%02x "
+	    "retries=%d.\n", sp->handle, fcport->loop_id,
+	    fcport->d_id.b.domain, fcport->d_id.b.area, fcport->d_id.b.al_pa,
+	    fcport->login_retry);
 	return rval;
 
 done_free_sp:
@@ -258,9 +260,9 @@ qla2x00_async_logout(struct scsi_qla_host *vha, fc_port_t *fcport)
 		goto done_free_sp;
 
 	ql_dbg(ql_dbg_disc, vha, 0x2070,
-	    "Async-logout - loop-id=%x portid=%02x%02x%02x.\n",
-	    fcport->loop_id, fcport->d_id.b.domain, fcport->d_id.b.area,
-	    fcport->d_id.b.al_pa);
+	    "Async-logout - hdl=%x loop-id=%x portid=%02x%02x%02x.\n",
+	    sp->handle, fcport->loop_id, fcport->d_id.b.domain,
+	    fcport->d_id.b.area, fcport->d_id.b.al_pa);
 	return rval;
 
 done_free_sp:
@@ -308,9 +310,9 @@ qla2x00_async_adisc(struct scsi_qla_host *vha, fc_port_t *fcport,
 		goto done_free_sp;
 
 	ql_dbg(ql_dbg_disc, vha, 0x206f,
-	    "Async-adisc - loopid=%x portid=%02x%02x%02x.\n",
-	    fcport->loop_id, fcport->d_id.b.domain, fcport->d_id.b.area,
-	    fcport->d_id.b.al_pa);
+	    "Async-adisc - hdl=%x loopid=%x portid=%02x%02x%02x.\n",
+	    sp->handle, fcport->loop_id, fcport->d_id.b.domain,
+	    fcport->d_id.b.area, fcport->d_id.b.al_pa);
 	return rval;
 
 done_free_sp:
@@ -360,9 +362,9 @@ qla2x00_async_tm_cmd(fc_port_t *fcport, uint32_t flags, uint32_t lun,
 		goto done_free_sp;
 
 	ql_dbg(ql_dbg_taskm, vha, 0x802f,
-	    "Async-tmf loop-id=%x portid=%02x%02x%02x.\n",
-	    fcport->loop_id, fcport->d_id.b.domain, fcport->d_id.b.area,
-	    fcport->d_id.b.al_pa);
+	    "Async-tmf hdl=%x loop-id=%x portid=%02x%02x%02x.\n",
+	    sp->handle, fcport->loop_id, fcport->d_id.b.domain,
+	    fcport->d_id.b.area, fcport->d_id.b.al_pa);
 	return rval;
 
 done_free_sp:
@@ -514,7 +516,7 @@ qla2x00_initialize_adapter(scsi_qla_host_t *vha)
 	set_bit(0, ha->req_qid_map);
 	set_bit(0, ha->rsp_qid_map);
 
-	ql_log(ql_log_info, vha, 0x0040,
+	ql_dbg(ql_dbg_init, vha, 0x0040,
 	    "Configuring PCI space...\n");
 	rval = ha->isp_ops->pci_config(vha);
 	if (rval) {
@@ -533,7 +535,7 @@ qla2x00_initialize_adapter(scsi_qla_host_t *vha)
 	}
 
 	ha->isp_ops->get_flash_version(vha, req->ring);
-	ql_log(ql_log_info, vha, 0x0061,
+	ql_dbg(ql_dbg_init, vha, 0x0061,
 	    "Configure NVRAM parameters...\n");
 
 	ha->isp_ops->nvram_config(vha);
@@ -550,7 +552,7 @@ qla2x00_initialize_adapter(scsi_qla_host_t *vha)
 		return QLA_FUNCTION_FAILED;
 	}
 
-	ql_log(ql_log_info, vha, 0x0078,
+	ql_dbg(ql_dbg_init, vha, 0x0078,
 	    "Verifying loaded RISC code...\n");
 
 	if (qla2x00_isp_firmware(vha) != QLA_SUCCESS) {
@@ -1294,7 +1296,7 @@ qla2x00_alloc_fw_dump(scsi_qla_host_t *vha)
 			ha->flags.fce_enabled = 0;
 			goto try_eft;
 		}
-		ql_log(ql_log_info, vha, 0x00c0,
+		ql_dbg(ql_dbg_init, vha, 0x00c0,
 		    "Allocate (%d KB) for FCE...\n", FCE_SIZE / 1024);
 
 		fce_size = sizeof(struct qla2xxx_fce_chain) + FCE_SIZE;
@@ -1321,7 +1323,7 @@ try_eft:
 			    tc_dma);
 			goto cont_alloc;
 		}
-		ql_log(ql_log_info, vha, 0x00c3,
+		ql_dbg(ql_dbg_init, vha, 0x00c3,
 		    "Allocated (%d KB) EFT ...\n", EFT_SIZE / 1024);
 
 		eft_size = EFT_SIZE;
@@ -1358,7 +1360,7 @@ cont_alloc:
 		}
 		return;
 	}
-	ql_log(ql_log_info, vha, 0x00c5,
+	ql_dbg(ql_dbg_init, vha, 0x00c5,
 	    "Allocated (%d KB) for firmware dump.\n", dump_size / 1024);
 
 	ha->fw_dump_len = dump_size;
@@ -1929,7 +1931,7 @@ qla2x00_fw_ready(scsi_qla_host_t *vha)
 					rval = qla84xx_init_chip(vha);
 					if (rval != QLA_SUCCESS) {
 						ql_log(ql_log_warn,
-						    vha, 0x8026,
+						    vha, 0x8007,
 						    "Init chip failed.\n");
 						break;
 					}
@@ -1938,7 +1940,7 @@ qla2x00_fw_ready(scsi_qla_host_t *vha)
 					cs84xx_time = jiffies - cs84xx_time;
 					wtime += cs84xx_time;
 					mtime += cs84xx_time;
-					ql_dbg(ql_dbg_taskm, vha, 0x8025,
+					ql_dbg(ql_dbg_taskm, vha, 0x8008,
 					    "Increasing wait time by %ld. "
 					    "New time %ld.\n", cs84xx_time,
 					    wtime);
@@ -1981,16 +1983,13 @@ qla2x00_fw_ready(scsi_qla_host_t *vha)
 
 		/* Delay for a while */
 		msleep(500);
-
-		ql_dbg(ql_dbg_taskm, vha, 0x8039,
-		    "fw_state=%x curr time=%lx.\n", state[0], jiffies);
 	} while (1);
 
 	ql_dbg(ql_dbg_taskm, vha, 0x803a,
 	    "fw_state=%x (%x, %x, %x, %x) " "curr time=%lx.\n", state[0],
 	    state[1], state[2], state[3], state[4], jiffies);
 
-	if (rval) {
+	if (rval && !(vha->device_flags & DFLG_NO_CABLE)) {
 		ql_log(ql_log_warn, vha, 0x803b,
 		    "Firmware ready **** FAILED ****.\n");
 	}
@@ -2386,7 +2385,7 @@ qla2x00_nvram_config(scsi_qla_host_t *vha)
 	 * internal driver logging.
 	 */
 	if (nv->host_p[0] & BIT_7)
-		ql2xextended_error_logging = 0x7fffffff;
+		ql2xextended_error_logging = QL_DBG_DEFAULT1_MASK;
 	ha->flags.disable_risc_code_load = ((nv->host_p[0] & BIT_4) ? 1 : 0);
 	/* Always load RISC code on non ISP2[12]00 chips. */
 	if (!IS_QLA2100(ha) && !IS_QLA2200(ha))
@@ -4188,7 +4187,8 @@ qla2x00_abort_isp(scsi_qla_host_t *vha)
 		spin_unlock_irqrestore(&ha->vport_slock, flags);
 
 	} else {
-		ql_log(ql_log_warn, vha, 0x8023, "%s **** FAILED ****.\n");
+		ql_log(ql_log_warn, vha, 0x8023, "%s **** FAILED ****.\n",
+		       __func__);
 	}
 
 	return(status);
@@ -4638,7 +4638,7 @@ qla24xx_load_risc_flash(scsi_qla_host_t *vha, uint32_t *srisc_addr,
 	struct req_que *req = ha->req_q_map[0];
 
 	ql_dbg(ql_dbg_init, vha, 0x008b,
-	    "Loading firmware from flash (%x).\n", faddr);
+	    "FW: Loading firmware from flash (%x).\n", faddr);
 
 	rval = QLA_SUCCESS;
 
@@ -4836,8 +4836,8 @@ qla24xx_load_risc_blob(scsi_qla_host_t *vha, uint32_t *srisc_addr)
 		return QLA_FUNCTION_FAILED;
 	}
 
-	ql_log(ql_log_info, vha, 0x0092,
-	    "Loading via request-firmware.\n");
+	ql_dbg(ql_dbg_init, vha, 0x0092,
+	    "FW: Loading via request-firmware.\n");
 
 	rval = QLA_SUCCESS;
 
@@ -5425,7 +5425,7 @@ qla82xx_restart_isp(scsi_qla_host_t *vha)
 		if ((vha->device_flags & DFLG_NO_CABLE))
 			status = 0;
 
-		ql_log(ql_log_info, vha, 0x803d,
+		ql_log(ql_log_info, vha, 0x8000,
 		    "Configure loop done, status = 0x%x.\n", status);
 	}
 
@@ -5458,7 +5458,7 @@ qla82xx_restart_isp(scsi_qla_host_t *vha)
 			    ha->fce_dma, ha->fce_bufs, ha->fce_mb,
 			    &ha->fce_bufs);
 			if (rval) {
-				ql_log(ql_log_warn, vha, 0x803e,
+				ql_log(ql_log_warn, vha, 0x8001,
 				    "Unable to reinitialize FCE (%d).\n",
 				    rval);
 				ha->flags.fce_enabled = 0;
@@ -5470,7 +5470,7 @@ qla82xx_restart_isp(scsi_qla_host_t *vha)
 			rval = qla2x00_enable_eft_trace(vha,
 			    ha->eft_dma, EFT_NUM_BUFFERS);
 			if (rval) {
-				ql_log(ql_log_warn, vha, 0x803f,
+				ql_log(ql_log_warn, vha, 0x8010,
 				    "Unable to reinitialize EFT (%d).\n",
 				    rval);
 			}
@@ -5478,7 +5478,7 @@ qla82xx_restart_isp(scsi_qla_host_t *vha)
 	}
 
 	if (!status) {
-		ql_dbg(ql_dbg_taskm, vha, 0x8040,
+		ql_dbg(ql_dbg_taskm, vha, 0x8011,
 		    "qla82xx_restart_isp succeeded.\n");
 
 		spin_lock_irqsave(&ha->vport_slock, flags);
@@ -5496,7 +5496,7 @@ qla82xx_restart_isp(scsi_qla_host_t *vha)
 		spin_unlock_irqrestore(&ha->vport_slock, flags);
 
 	} else {
-		ql_log(ql_log_warn, vha, 0x8041,
+		ql_log(ql_log_warn, vha, 0x8016,
 		    "qla82xx_restart_isp **** FAILED ****.\n");
 	}
 
@@ -5643,13 +5643,26 @@ qla24xx_update_fcport_fcp_prio(scsi_qla_host_t *vha, fc_port_t *fcport)
 	if (priority < 0)
 		return QLA_FUNCTION_FAILED;
 
+	if (IS_QLA82XX(vha->hw)) {
+		fcport->fcp_prio = priority & 0xf;
+		return QLA_SUCCESS;
+	}
+
 	ret = qla24xx_set_fcp_prio(vha, fcport->loop_id, priority, mb);
-	if (ret == QLA_SUCCESS)
-		fcport->fcp_prio = priority;
-	else
+	if (ret == QLA_SUCCESS) {
+		if (fcport->fcp_prio != priority)
+			ql_dbg(ql_dbg_user, vha, 0x709e,
+			    "Updated FCP_CMND priority - value=%d loop_id=%d "
+			    "port_id=%02x%02x%02x.\n", priority,
+			    fcport->loop_id, fcport->d_id.b.domain,
+			    fcport->d_id.b.area, fcport->d_id.b.al_pa);
+		fcport->fcp_prio = priority & 0xf;
+	} else
 		ql_dbg(ql_dbg_user, vha, 0x704f,
-		    "Unable to activate fcp priority, ret=0x%x.\n", ret);
-
+		    "Unable to update FCP_CMND priority - ret=0x%x for "
+		    "loop_id=%d port_id=%02x%02x%02x.\n", ret, fcport->loop_id,
+		    fcport->d_id.b.domain, fcport->d_id.b.area,
+		    fcport->d_id.b.al_pa);
 	return  ret;
 }
 
diff --git a/drivers/scsi/qla2xxx/qla_iocb.c b/drivers/scsi/qla2xxx/qla_iocb.c
index a4b267e60a35..55a96761b5a4 100644
--- a/drivers/scsi/qla2xxx/qla_iocb.c
+++ b/drivers/scsi/qla2xxx/qla_iocb.c
@@ -11,8 +11,6 @@
 
 #include <scsi/scsi_tcq.h>
 
-static void qla2x00_isp_cmd(struct scsi_qla_host *, struct req_que *);
-
 static void qla25xx_set_que(srb_t *, struct rsp_que **);
 /**
  * qla2x00_get_cmd_direction() - Determine control_flag data direction.
@@ -468,6 +466,42 @@ queuing_error:
 }
 
 /**
+ * qla2x00_start_iocbs() - Execute the IOCB command
+ */
+static void
+qla2x00_start_iocbs(struct scsi_qla_host *vha, struct req_que *req)
+{
+	struct qla_hw_data *ha = vha->hw;
+	device_reg_t __iomem *reg = ISP_QUE_REG(ha, req->id);
+	struct device_reg_2xxx __iomem *ioreg = &ha->iobase->isp;
+
+	if (IS_QLA82XX(ha)) {
+		qla82xx_start_iocbs(vha);
+	} else {
+		/* Adjust ring index. */
+		req->ring_index++;
+		if (req->ring_index == req->length) {
+			req->ring_index = 0;
+			req->ring_ptr = req->ring;
+		} else
+			req->ring_ptr++;
+
+		/* Set chip new ring index. */
+		if (ha->mqenable) {
+			WRT_REG_DWORD(&reg->isp25mq.req_q_in, req->ring_index);
+			RD_REG_DWORD(&ioreg->hccr);
+		} else if (IS_FWI2_CAPABLE(ha)) {
+			WRT_REG_DWORD(&reg->isp24.req_q_in, req->ring_index);
+			RD_REG_DWORD_RELAXED(&reg->isp24.req_q_in);
+		} else {
+			WRT_REG_WORD(ISP_REQ_Q_IN(ha, &reg->isp),
+				req->ring_index);
+			RD_REG_WORD_RELAXED(ISP_REQ_Q_IN(ha, &reg->isp));
+		}
+	}
+}
+
+/**
  * qla2x00_marker() - Send a marker IOCB to the firmware.
  * @ha: HA context
  * @loop_id: loop ID
@@ -489,6 +523,7 @@ __qla2x00_marker(struct scsi_qla_host *vha, struct req_que *req,
 	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
 
 	mrk24 = NULL;
+	req = ha->req_q_map[0];
 	mrk = (mrk_entry_t *)qla2x00_alloc_iocbs(vha, 0);
 	if (mrk == NULL) {
 		ql_log(ql_log_warn, base_vha, 0x3026,
@@ -515,7 +550,7 @@ __qla2x00_marker(struct scsi_qla_host *vha, struct req_que *req,
 	}
 	wmb();
 
-	qla2x00_isp_cmd(vha, req);
+	qla2x00_start_iocbs(vha, req);
 
 	return (QLA_SUCCESS);
 }
@@ -536,89 +571,140 @@ qla2x00_marker(struct scsi_qla_host *vha, struct req_que *req,
 }
 
 /**
- * qla2x00_isp_cmd() - Modify the request ring pointer.
- * @ha: HA context
+ * qla24xx_calc_iocbs() - Determine number of Command Type 3 and
+ * Continuation Type 1 IOCBs to allocate.
+ *
+ * @dsds: number of data segment decriptors needed
  *
- * Note: The caller must hold the hardware lock before calling this routine.
+ * Returns the number of IOCB entries needed to store @dsds.
  */
-static void
-qla2x00_isp_cmd(struct scsi_qla_host *vha, struct req_que *req)
+inline uint16_t
+qla24xx_calc_iocbs(scsi_qla_host_t *vha, uint16_t dsds)
 {
-	struct qla_hw_data *ha = vha->hw;
-	device_reg_t __iomem *reg = ISP_QUE_REG(ha, req->id);
-	struct device_reg_2xxx __iomem *ioreg = &ha->iobase->isp;
+	uint16_t iocbs;
 
-	ql_dbg(ql_dbg_io + ql_dbg_buffer, vha, 0x302d,
-	    "IOCB data:\n");
-	ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x302e,
-	    (uint8_t *)req->ring_ptr, REQUEST_ENTRY_SIZE);
+	iocbs = 1;
+	if (dsds > 1) {
+		iocbs += (dsds - 1) / 5;
+		if ((dsds - 1) % 5)
+			iocbs++;
+	}
+	return iocbs;
+}
 
-	/* Adjust ring index. */
-	req->ring_index++;
-	if (req->ring_index == req->length) {
-		req->ring_index = 0;
-		req->ring_ptr = req->ring;
-	} else
-		req->ring_ptr++;
+static inline int
+qla24xx_build_scsi_type_6_iocbs(srb_t *sp, struct cmd_type_6 *cmd_pkt,
+	uint16_t tot_dsds)
+{
+	uint32_t *cur_dsd = NULL;
+	scsi_qla_host_t	*vha;
+	struct qla_hw_data *ha;
+	struct scsi_cmnd *cmd;
+	struct	scatterlist *cur_seg;
+	uint32_t *dsd_seg;
+	void *next_dsd;
+	uint8_t avail_dsds;
+	uint8_t first_iocb = 1;
+	uint32_t dsd_list_len;
+	struct dsd_dma *dsd_ptr;
+	struct ct6_dsd *ctx;
 
-	/* Set chip new ring index. */
-	if (IS_QLA82XX(ha)) {
-		uint32_t dbval = 0x04 | (ha->portnum << 5);
+	cmd = sp->cmd;
 
-		/* write, read and verify logic */
-		dbval = dbval | (req->id << 8) | (req->ring_index << 16);
-		if (ql2xdbwr)
-			qla82xx_wr_32(ha, ha->nxdb_wr_ptr, dbval);
-		else {
-			WRT_REG_DWORD(
-				(unsigned long __iomem *)ha->nxdb_wr_ptr,
-				dbval);
-			wmb();
-			while (RD_REG_DWORD(ha->nxdb_rd_ptr) != dbval) {
-				WRT_REG_DWORD((unsigned long __iomem *)
-					ha->nxdb_wr_ptr, dbval);
-				wmb();
-			}
-		}
-	} else if (ha->mqenable) {
-		/* Set chip new ring index. */
-		WRT_REG_DWORD(&reg->isp25mq.req_q_in, req->ring_index);
-		RD_REG_DWORD(&ioreg->hccr);
-	} else {
-		if (IS_FWI2_CAPABLE(ha)) {
-			WRT_REG_DWORD(&reg->isp24.req_q_in, req->ring_index);
-			RD_REG_DWORD_RELAXED(&reg->isp24.req_q_in);
+	/* Update entry type to indicate Command Type 3 IOCB */
+	*((uint32_t *)(&cmd_pkt->entry_type)) =
+		__constant_cpu_to_le32(COMMAND_TYPE_6);
+
+	/* No data transfer */
+	if (!scsi_bufflen(cmd) || cmd->sc_data_direction == DMA_NONE) {
+		cmd_pkt->byte_count = __constant_cpu_to_le32(0);
+		return 0;
+	}
+
+	vha = sp->fcport->vha;
+	ha = vha->hw;
+
+	/* Set transfer direction */
+	if (cmd->sc_data_direction == DMA_TO_DEVICE) {
+		cmd_pkt->control_flags =
+		    __constant_cpu_to_le16(CF_WRITE_DATA);
+		ha->qla_stats.output_bytes += scsi_bufflen(cmd);
+	} else if (cmd->sc_data_direction == DMA_FROM_DEVICE) {
+		cmd_pkt->control_flags =
+		    __constant_cpu_to_le16(CF_READ_DATA);
+		ha->qla_stats.input_bytes += scsi_bufflen(cmd);
+	}
+
+	cur_seg = scsi_sglist(cmd);
+	ctx = sp->ctx;
+
+	while (tot_dsds) {
+		avail_dsds = (tot_dsds > QLA_DSDS_PER_IOCB) ?
+		    QLA_DSDS_PER_IOCB : tot_dsds;
+		tot_dsds -= avail_dsds;
+		dsd_list_len = (avail_dsds + 1) * QLA_DSD_SIZE;
+
+		dsd_ptr = list_first_entry(&ha->gbl_dsd_list,
+		    struct dsd_dma, list);
+		next_dsd = dsd_ptr->dsd_addr;
+		list_del(&dsd_ptr->list);
+		ha->gbl_dsd_avail--;
+		list_add_tail(&dsd_ptr->list, &ctx->dsd_list);
+		ctx->dsd_use_cnt++;
+		ha->gbl_dsd_inuse++;
+
+		if (first_iocb) {
+			first_iocb = 0;
+			dsd_seg = (uint32_t *)&cmd_pkt->fcp_data_dseg_address;
+			*dsd_seg++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
+			*dsd_seg++ = cpu_to_le32(MSD(dsd_ptr->dsd_list_dma));
+			cmd_pkt->fcp_data_dseg_len = cpu_to_le32(dsd_list_len);
 		} else {
-			WRT_REG_WORD(ISP_REQ_Q_IN(ha, &reg->isp),
-				req->ring_index);
-			RD_REG_WORD_RELAXED(ISP_REQ_Q_IN(ha, &reg->isp));
+			*cur_dsd++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
+			*cur_dsd++ = cpu_to_le32(MSD(dsd_ptr->dsd_list_dma));
+			*cur_dsd++ = cpu_to_le32(dsd_list_len);
+		}
+		cur_dsd = (uint32_t *)next_dsd;
+		while (avail_dsds) {
+			dma_addr_t	sle_dma;
+
+			sle_dma = sg_dma_address(cur_seg);
+			*cur_dsd++ = cpu_to_le32(LSD(sle_dma));
+			*cur_dsd++ = cpu_to_le32(MSD(sle_dma));
+			*cur_dsd++ = cpu_to_le32(sg_dma_len(cur_seg));
+			cur_seg = sg_next(cur_seg);
+			avail_dsds--;
 		}
 	}
 
+	/* Null termination */
+	*cur_dsd++ =  0;
+	*cur_dsd++ = 0;
+	*cur_dsd++ = 0;
+	cmd_pkt->control_flags |= CF_DATA_SEG_DESCR_ENABLE;
+	return 0;
 }
 
-/**
- * qla24xx_calc_iocbs() - Determine number of Command Type 3 and
- * Continuation Type 1 IOCBs to allocate.
+/*
+ * qla24xx_calc_dsd_lists() - Determine number of DSD list required
+ * for Command Type 6.
  *
  * @dsds: number of data segment decriptors needed
  *
- * Returns the number of IOCB entries needed to store @dsds.
+ * Returns the number of dsd list needed to store @dsds.
  */
 inline uint16_t
-qla24xx_calc_iocbs(scsi_qla_host_t *vha, uint16_t dsds)
+qla24xx_calc_dsd_lists(uint16_t dsds)
 {
-	uint16_t iocbs;
+	uint16_t dsd_lists = 0;
 
-	iocbs = 1;
-	if (dsds > 1) {
-		iocbs += (dsds - 1) / 5;
-		if ((dsds - 1) % 5)
-			iocbs++;
-	}
-	return iocbs;
+	dsd_lists = (dsds/QLA_DSDS_PER_IOCB);
+	if (dsds % QLA_DSDS_PER_IOCB)
+		dsd_lists++;
+	return dsd_lists;
 }
 
+
 /**
  * qla24xx_build_scsi_iocbs() - Build IOCB command utilizing Command Type 7
  * IOCB types.
@@ -945,6 +1031,7 @@ alloc_and_fill:
 	*cur_dsd++ = 0;
 	return 0;
 }
+
 static int
 qla24xx_walk_and_build_sglist(struct qla_hw_data *ha, srb_t *sp, uint32_t *dsd,
 	uint16_t tot_dsds)
@@ -1004,7 +1091,7 @@ qla24xx_walk_and_build_sglist(struct qla_hw_data *ha, srb_t *sp, uint32_t *dsd,
 		sle_dma = sg_dma_address(sg);
 		ql_dbg(ql_dbg_io, vha, 0x300a,
 		    "sg entry %d - addr=0x%x 0x%x, " "len=%d for cmd=%p.\n",
-		    cur_dsd, i, LSD(sle_dma), MSD(sle_dma), sg_dma_len(sg),
+		    i, LSD(sle_dma), MSD(sle_dma), sg_dma_len(sg),
 		    sp->cmd);
 		*cur_dsd++ = cpu_to_le32(LSD(sle_dma));
 		*cur_dsd++ = cpu_to_le32(MSD(sle_dma));
@@ -1731,6 +1818,7 @@ qla2x00_alloc_iocbs(scsi_qla_host_t *vha, srb_t *sp)
 	uint32_t index, handle;
 	request_t *pkt;
 	uint16_t cnt, req_cnt;
+	struct srb_ctx *ctx;
 
 	pkt = NULL;
 	req_cnt = 1;
@@ -1759,6 +1847,12 @@ qla2x00_alloc_iocbs(scsi_qla_host_t *vha, srb_t *sp)
 	req->outstanding_cmds[handle] = sp;
 	sp->handle = handle;
 
+	/* Adjust entry-counts as needed. */
+	if (sp->ctx) {
+		ctx = sp->ctx;
+		req_cnt = ctx->iocbs;
+	}
+
 skip_cmd_array:
 	/* Check for room on request queue. */
 	if (req->cnt < req_cnt) {
@@ -1793,42 +1887,6 @@ queuing_error:
 }
 
 static void
-qla2x00_start_iocbs(srb_t *sp)
-{
-	struct qla_hw_data *ha = sp->fcport->vha->hw;
-	struct req_que *req = ha->req_q_map[0];
-	device_reg_t __iomem *reg = ISP_QUE_REG(ha, req->id);
-	struct device_reg_2xxx __iomem *ioreg = &ha->iobase->isp;
-
-	if (IS_QLA82XX(ha)) {
-		qla82xx_start_iocbs(sp);
-	} else {
-		/* Adjust ring index. */
-		req->ring_index++;
-		if (req->ring_index == req->length) {
-			req->ring_index = 0;
-			req->ring_ptr = req->ring;
-		} else
-			req->ring_ptr++;
-
-		/* Set chip new ring index. */
-		if (ha->mqenable) {
-			WRT_REG_DWORD(&reg->isp25mq.req_q_in, req->ring_index);
-			RD_REG_DWORD(&ioreg->hccr);
-		} else if (IS_QLA82XX(ha)) {
-			qla82xx_start_iocbs(sp);
-		} else if (IS_FWI2_CAPABLE(ha)) {
-			WRT_REG_DWORD(&reg->isp24.req_q_in, req->ring_index);
-			RD_REG_DWORD_RELAXED(&reg->isp24.req_q_in);
-		} else {
-			WRT_REG_WORD(ISP_REQ_Q_IN(ha, &reg->isp),
-				req->ring_index);
-			RD_REG_WORD_RELAXED(ISP_REQ_Q_IN(ha, &reg->isp));
-		}
-	}
-}
-
-static void
 qla24xx_login_iocb(srb_t *sp, struct logio_entry_24xx *logio)
 {
 	struct srb_ctx *ctx = sp->ctx;
@@ -2160,6 +2218,381 @@ qla24xx_ct_iocb(srb_t *sp, struct ct_entry_24xx *ct_iocb)
         ct_iocb->entry_count = entry_count;
 }
 
+/*
+ * qla82xx_start_scsi() - Send a SCSI command to the ISP
+ * @sp: command to send to the ISP
+ *
+ * Returns non-zero if a failure occurred, else zero.
+ */
+int
+qla82xx_start_scsi(srb_t *sp)
+{
+	int		ret, nseg;
+	unsigned long   flags;
+	struct scsi_cmnd *cmd;
+	uint32_t	*clr_ptr;
+	uint32_t        index;
+	uint32_t	handle;
+	uint16_t	cnt;
+	uint16_t	req_cnt;
+	uint16_t	tot_dsds;
+	struct device_reg_82xx __iomem *reg;
+	uint32_t dbval;
+	uint32_t *fcp_dl;
+	uint8_t additional_cdb_len;
+	struct ct6_dsd *ctx;
+	struct scsi_qla_host *vha = sp->fcport->vha;
+	struct qla_hw_data *ha = vha->hw;
+	struct req_que *req = NULL;
+	struct rsp_que *rsp = NULL;
+	char		tag[2];
+
+	/* Setup device pointers. */
+	ret = 0;
+	reg = &ha->iobase->isp82;
+	cmd = sp->cmd;
+	req = vha->req;
+	rsp = ha->rsp_q_map[0];
+
+	/* So we know we haven't pci_map'ed anything yet */
+	tot_dsds = 0;
+
+	dbval = 0x04 | (ha->portnum << 5);
+
+	/* Send marker if required */
+	if (vha->marker_needed != 0) {
+		if (qla2x00_marker(vha, req,
+			rsp, 0, 0, MK_SYNC_ALL) != QLA_SUCCESS) {
+			ql_log(ql_log_warn, vha, 0x300c,
+			    "qla2x00_marker failed for cmd=%p.\n", cmd);
+			return QLA_FUNCTION_FAILED;
+		}
+		vha->marker_needed = 0;
+	}
+
+	/* Acquire ring specific lock */
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	/* Check for room in outstanding command list. */
+	handle = req->current_outstanding_cmd;
+	for (index = 1; index < MAX_OUTSTANDING_COMMANDS; index++) {
+		handle++;
+		if (handle == MAX_OUTSTANDING_COMMANDS)
+			handle = 1;
+		if (!req->outstanding_cmds[handle])
+			break;
+	}
+	if (index == MAX_OUTSTANDING_COMMANDS)
+		goto queuing_error;
+
+	/* Map the sg table so we have an accurate count of sg entries needed */
+	if (scsi_sg_count(cmd)) {
+		nseg = dma_map_sg(&ha->pdev->dev, scsi_sglist(cmd),
+		    scsi_sg_count(cmd), cmd->sc_data_direction);
+		if (unlikely(!nseg))
+			goto queuing_error;
+	} else
+		nseg = 0;
+
+	tot_dsds = nseg;
+
+	if (tot_dsds > ql2xshiftctondsd) {
+		struct cmd_type_6 *cmd_pkt;
+		uint16_t more_dsd_lists = 0;
+		struct dsd_dma *dsd_ptr;
+		uint16_t i;
+
+		more_dsd_lists = qla24xx_calc_dsd_lists(tot_dsds);
+		if ((more_dsd_lists + ha->gbl_dsd_inuse) >= NUM_DSD_CHAIN) {
+			ql_dbg(ql_dbg_io, vha, 0x300d,
+			    "Num of DSD list %d is than %d for cmd=%p.\n",
+			    more_dsd_lists + ha->gbl_dsd_inuse, NUM_DSD_CHAIN,
+			    cmd);
+			goto queuing_error;
+		}
+
+		if (more_dsd_lists <= ha->gbl_dsd_avail)
+			goto sufficient_dsds;
+		else
+			more_dsd_lists -= ha->gbl_dsd_avail;
+
+		for (i = 0; i < more_dsd_lists; i++) {
+			dsd_ptr = kzalloc(sizeof(struct dsd_dma), GFP_ATOMIC);
+			if (!dsd_ptr) {
+				ql_log(ql_log_fatal, vha, 0x300e,
+				    "Failed to allocate memory for dsd_dma "
+				    "for cmd=%p.\n", cmd);
+				goto queuing_error;
+			}
+
+			dsd_ptr->dsd_addr = dma_pool_alloc(ha->dl_dma_pool,
+				GFP_ATOMIC, &dsd_ptr->dsd_list_dma);
+			if (!dsd_ptr->dsd_addr) {
+				kfree(dsd_ptr);
+				ql_log(ql_log_fatal, vha, 0x300f,
+				    "Failed to allocate memory for dsd_addr "
+				    "for cmd=%p.\n", cmd);
+				goto queuing_error;
+			}
+			list_add_tail(&dsd_ptr->list, &ha->gbl_dsd_list);
+			ha->gbl_dsd_avail++;
+		}
+
+sufficient_dsds:
+		req_cnt = 1;
+
+		if (req->cnt < (req_cnt + 2)) {
+			cnt = (uint16_t)RD_REG_DWORD_RELAXED(
+				&reg->req_q_out[0]);
+			if (req->ring_index < cnt)
+				req->cnt = cnt - req->ring_index;
+			else
+				req->cnt = req->length -
+					(req->ring_index - cnt);
+		}
+
+		if (req->cnt < (req_cnt + 2))
+			goto queuing_error;
+
+		ctx = sp->ctx = mempool_alloc(ha->ctx_mempool, GFP_ATOMIC);
+		if (!sp->ctx) {
+			ql_log(ql_log_fatal, vha, 0x3010,
+			    "Failed to allocate ctx for cmd=%p.\n", cmd);
+			goto queuing_error;
+		}
+		memset(ctx, 0, sizeof(struct ct6_dsd));
+		ctx->fcp_cmnd = dma_pool_alloc(ha->fcp_cmnd_dma_pool,
+			GFP_ATOMIC, &ctx->fcp_cmnd_dma);
+		if (!ctx->fcp_cmnd) {
+			ql_log(ql_log_fatal, vha, 0x3011,
+			    "Failed to allocate fcp_cmnd for cmd=%p.\n", cmd);
+			goto queuing_error_fcp_cmnd;
+		}
+
+		/* Initialize the DSD list and dma handle */
+		INIT_LIST_HEAD(&ctx->dsd_list);
+		ctx->dsd_use_cnt = 0;
+
+		if (cmd->cmd_len > 16) {
+			additional_cdb_len = cmd->cmd_len - 16;
+			if ((cmd->cmd_len % 4) != 0) {
+				/* SCSI command bigger than 16 bytes must be
+				 * multiple of 4
+				 */
+				ql_log(ql_log_warn, vha, 0x3012,
+				    "scsi cmd len %d not multiple of 4 "
+				    "for cmd=%p.\n", cmd->cmd_len, cmd);
+				goto queuing_error_fcp_cmnd;
+			}
+			ctx->fcp_cmnd_len = 12 + cmd->cmd_len + 4;
+		} else {
+			additional_cdb_len = 0;
+			ctx->fcp_cmnd_len = 12 + 16 + 4;
+		}
+
+		cmd_pkt = (struct cmd_type_6 *)req->ring_ptr;
+		cmd_pkt->handle = MAKE_HANDLE(req->id, handle);
+
+		/* Zero out remaining portion of packet. */
+		/*    tagged queuing modifier -- default is TSK_SIMPLE (0). */
+		clr_ptr = (uint32_t *)cmd_pkt + 2;
+		memset(clr_ptr, 0, REQUEST_ENTRY_SIZE - 8);
+		cmd_pkt->dseg_count = cpu_to_le16(tot_dsds);
+
+		/* Set NPORT-ID and LUN number*/
+		cmd_pkt->nport_handle = cpu_to_le16(sp->fcport->loop_id);
+		cmd_pkt->port_id[0] = sp->fcport->d_id.b.al_pa;
+		cmd_pkt->port_id[1] = sp->fcport->d_id.b.area;
+		cmd_pkt->port_id[2] = sp->fcport->d_id.b.domain;
+		cmd_pkt->vp_index = sp->fcport->vp_idx;
+
+		/* Build IOCB segments */
+		if (qla24xx_build_scsi_type_6_iocbs(sp, cmd_pkt, tot_dsds))
+			goto queuing_error_fcp_cmnd;
+
+		int_to_scsilun(sp->cmd->device->lun, &cmd_pkt->lun);
+		host_to_fcp_swap((uint8_t *)&cmd_pkt->lun, sizeof(cmd_pkt->lun));
+
+		/* build FCP_CMND IU */
+		memset(ctx->fcp_cmnd, 0, sizeof(struct fcp_cmnd));
+		int_to_scsilun(sp->cmd->device->lun, &ctx->fcp_cmnd->lun);
+		ctx->fcp_cmnd->additional_cdb_len = additional_cdb_len;
+
+		if (cmd->sc_data_direction == DMA_TO_DEVICE)
+			ctx->fcp_cmnd->additional_cdb_len |= 1;
+		else if (cmd->sc_data_direction == DMA_FROM_DEVICE)
+			ctx->fcp_cmnd->additional_cdb_len |= 2;
+
+		/*
+		 * Update tagged queuing modifier -- default is TSK_SIMPLE (0).
+		 */
+		if (scsi_populate_tag_msg(cmd, tag)) {
+			switch (tag[0]) {
+			case HEAD_OF_QUEUE_TAG:
+				ctx->fcp_cmnd->task_attribute =
+				    TSK_HEAD_OF_QUEUE;
+				break;
+			case ORDERED_QUEUE_TAG:
+				ctx->fcp_cmnd->task_attribute =
+				    TSK_ORDERED;
+				break;
+			}
+		}
+
+		/* Populate the FCP_PRIO. */
+		if (ha->flags.fcp_prio_enabled)
+			ctx->fcp_cmnd->task_attribute |=
+			    sp->fcport->fcp_prio << 3;
+
+		memcpy(ctx->fcp_cmnd->cdb, cmd->cmnd, cmd->cmd_len);
+
+		fcp_dl = (uint32_t *)(ctx->fcp_cmnd->cdb + 16 +
+		    additional_cdb_len);
+		*fcp_dl = htonl((uint32_t)scsi_bufflen(cmd));
+
+		cmd_pkt->fcp_cmnd_dseg_len = cpu_to_le16(ctx->fcp_cmnd_len);
+		cmd_pkt->fcp_cmnd_dseg_address[0] =
+		    cpu_to_le32(LSD(ctx->fcp_cmnd_dma));
+		cmd_pkt->fcp_cmnd_dseg_address[1] =
+		    cpu_to_le32(MSD(ctx->fcp_cmnd_dma));
+
+		sp->flags |= SRB_FCP_CMND_DMA_VALID;
+		cmd_pkt->byte_count = cpu_to_le32((uint32_t)scsi_bufflen(cmd));
+		/* Set total data segment count. */
+		cmd_pkt->entry_count = (uint8_t)req_cnt;
+		/* Specify response queue number where
+		 * completion should happen
+		 */
+		cmd_pkt->entry_status = (uint8_t) rsp->id;
+	} else {
+		struct cmd_type_7 *cmd_pkt;
+		req_cnt = qla24xx_calc_iocbs(vha, tot_dsds);
+		if (req->cnt < (req_cnt + 2)) {
+			cnt = (uint16_t)RD_REG_DWORD_RELAXED(
+			    &reg->req_q_out[0]);
+			if (req->ring_index < cnt)
+				req->cnt = cnt - req->ring_index;
+			else
+				req->cnt = req->length -
+					(req->ring_index - cnt);
+		}
+		if (req->cnt < (req_cnt + 2))
+			goto queuing_error;
+
+		cmd_pkt = (struct cmd_type_7 *)req->ring_ptr;
+		cmd_pkt->handle = MAKE_HANDLE(req->id, handle);
+
+		/* Zero out remaining portion of packet. */
+		/* tagged queuing modifier -- default is TSK_SIMPLE (0).*/
+		clr_ptr = (uint32_t *)cmd_pkt + 2;
+		memset(clr_ptr, 0, REQUEST_ENTRY_SIZE - 8);
+		cmd_pkt->dseg_count = cpu_to_le16(tot_dsds);
+
+		/* Set NPORT-ID and LUN number*/
+		cmd_pkt->nport_handle = cpu_to_le16(sp->fcport->loop_id);
+		cmd_pkt->port_id[0] = sp->fcport->d_id.b.al_pa;
+		cmd_pkt->port_id[1] = sp->fcport->d_id.b.area;
+		cmd_pkt->port_id[2] = sp->fcport->d_id.b.domain;
+		cmd_pkt->vp_index = sp->fcport->vp_idx;
+
+		int_to_scsilun(sp->cmd->device->lun, &cmd_pkt->lun);
+		host_to_fcp_swap((uint8_t *)&cmd_pkt->lun,
+			sizeof(cmd_pkt->lun));
+
+		/*
+		 * Update tagged queuing modifier -- default is TSK_SIMPLE (0).
+		 */
+		if (scsi_populate_tag_msg(cmd, tag)) {
+			switch (tag[0]) {
+			case HEAD_OF_QUEUE_TAG:
+				cmd_pkt->task = TSK_HEAD_OF_QUEUE;
+				break;
+			case ORDERED_QUEUE_TAG:
+				cmd_pkt->task = TSK_ORDERED;
+				break;
+			}
+		}
+
+		/* Populate the FCP_PRIO. */
+		if (ha->flags.fcp_prio_enabled)
+			cmd_pkt->task |= sp->fcport->fcp_prio << 3;
+
+		/* Load SCSI command packet. */
+		memcpy(cmd_pkt->fcp_cdb, cmd->cmnd, cmd->cmd_len);
+		host_to_fcp_swap(cmd_pkt->fcp_cdb, sizeof(cmd_pkt->fcp_cdb));
+
+		cmd_pkt->byte_count = cpu_to_le32((uint32_t)scsi_bufflen(cmd));
+
+		/* Build IOCB segments */
+		qla24xx_build_scsi_iocbs(sp, cmd_pkt, tot_dsds);
+
+		/* Set total data segment count. */
+		cmd_pkt->entry_count = (uint8_t)req_cnt;
+		/* Specify response queue number where
+		 * completion should happen.
+		 */
+		cmd_pkt->entry_status = (uint8_t) rsp->id;
+
+	}
+	/* Build command packet. */
+	req->current_outstanding_cmd = handle;
+	req->outstanding_cmds[handle] = sp;
+	sp->handle = handle;
+	sp->cmd->host_scribble = (unsigned char *)(unsigned long)handle;
+	req->cnt -= req_cnt;
+	wmb();
+
+	/* Adjust ring index. */
+	req->ring_index++;
+	if (req->ring_index == req->length) {
+		req->ring_index = 0;
+		req->ring_ptr = req->ring;
+	} else
+		req->ring_ptr++;
+
+	sp->flags |= SRB_DMA_VALID;
+
+	/* Set chip new ring index. */
+	/* write, read and verify logic */
+	dbval = dbval | (req->id << 8) | (req->ring_index << 16);
+	if (ql2xdbwr)
+		qla82xx_wr_32(ha, ha->nxdb_wr_ptr, dbval);
+	else {
+		WRT_REG_DWORD(
+			(unsigned long __iomem *)ha->nxdb_wr_ptr,
+			dbval);
+		wmb();
+		while (RD_REG_DWORD(ha->nxdb_rd_ptr) != dbval) {
+			WRT_REG_DWORD(
+				(unsigned long __iomem *)ha->nxdb_wr_ptr,
+				dbval);
+			wmb();
+		}
+	}
+
+	/* Manage unprocessed RIO/ZIO commands in response queue. */
+	if (vha->flags.process_response_queue &&
+	    rsp->ring_ptr->signature != RESPONSE_PROCESSED)
+		qla24xx_process_response_queue(vha, rsp);
+
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	return QLA_SUCCESS;
+
+queuing_error_fcp_cmnd:
+	dma_pool_free(ha->fcp_cmnd_dma_pool, ctx->fcp_cmnd, ctx->fcp_cmnd_dma);
+queuing_error:
+	if (tot_dsds)
+		scsi_dma_unmap(cmd);
+
+	if (sp->ctx) {
+		mempool_free(sp->ctx, ha->ctx_mempool);
+		sp->ctx = NULL;
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	return QLA_FUNCTION_FAILED;
+}
+
 int
 qla2x00_start_sp(srb_t *sp)
 {
@@ -2196,8 +2629,8 @@ qla2x00_start_sp(srb_t *sp)
 		break;
 	case SRB_CT_CMD:
 		IS_FWI2_CAPABLE(ha) ?
-		qla24xx_ct_iocb(sp, pkt) :
-		qla2x00_ct_iocb(sp, pkt);
+		    qla24xx_ct_iocb(sp, pkt) :
+		    qla2x00_ct_iocb(sp, pkt);
 		break;
 	case SRB_ADISC_CMD:
 		IS_FWI2_CAPABLE(ha) ?
@@ -2212,7 +2645,7 @@ qla2x00_start_sp(srb_t *sp)
 	}
 
 	wmb();
-	qla2x00_start_iocbs(sp);
+	qla2x00_start_iocbs(sp->fcport->vha, ha->req_q_map[0]);
 done:
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 	return rval;
diff --git a/drivers/scsi/qla2xxx/qla_isr.c b/drivers/scsi/qla2xxx/qla_isr.c
index 7b91b290ffd6..e804585cc59c 100644
--- a/drivers/scsi/qla2xxx/qla_isr.c
+++ b/drivers/scsi/qla2xxx/qla_isr.c
@@ -242,32 +242,34 @@ static void
 qla2x00_mbx_completion(scsi_qla_host_t *vha, uint16_t mb0)
 {
 	uint16_t	cnt;
+	uint32_t	mboxes;
 	uint16_t __iomem *wptr;
 	struct qla_hw_data *ha = vha->hw;
 	struct device_reg_2xxx __iomem *reg = &ha->iobase->isp;
 
+	/* Read all mbox registers? */
+	mboxes = (1 << ha->mbx_count) - 1;
+	if (!ha->mcp)
+		ql_dbg(ql_dbg_async, vha, 0x5001, "MBX pointer ERRROR.\n");
+	else
+		mboxes = ha->mcp->in_mb;
+
 	/* Load return mailbox registers. */
 	ha->flags.mbox_int = 1;
 	ha->mailbox_out[0] = mb0;
+	mboxes >>= 1;
 	wptr = (uint16_t __iomem *)MAILBOX_REG(ha, reg, 1);
 
 	for (cnt = 1; cnt < ha->mbx_count; cnt++) {
 		if (IS_QLA2200(ha) && cnt == 8)
 			wptr = (uint16_t __iomem *)MAILBOX_REG(ha, reg, 8);
-		if (cnt == 4 || cnt == 5)
+		if ((cnt == 4 || cnt == 5) && (mboxes & BIT_0))
 			ha->mailbox_out[cnt] = qla2x00_debounce_register(wptr);
-		else
+		else if (mboxes & BIT_0)
 			ha->mailbox_out[cnt] = RD_REG_WORD(wptr);
 
 		wptr++;
-	}
-
-	if (ha->mcp) {
-		ql_dbg(ql_dbg_async, vha, 0x5000,
-		    "Got mbx completion. cmd=%x.\n", ha->mcp->mb[0]);
-	} else {
-		ql_dbg(ql_dbg_async, vha, 0x5001,
-		    "MBX pointer ERROR.\n");
+		mboxes >>= 1;
 	}
 }
 
@@ -298,7 +300,7 @@ qla81xx_idc_event(scsi_qla_host_t *vha, uint16_t aen, uint16_t descr)
 		return;
 
 	ql_dbg(ql_dbg_async, vha, 0x5022,
-	    "Inter-Driver Commucation %s -- ACK timeout=%d.\n",
+	    "%lu Inter-Driver Communication %s -- ACK timeout=%d.\n",
 	    vha->host_no, event[aen & 0xff], timeout);
 
 	rval = qla2x00_post_idc_ack_work(vha, mb);
@@ -453,7 +455,7 @@ skip_rio:
 		break;
 
 	case MBA_LIP_OCCURRED:		/* Loop Initialization Procedure */
-		ql_log(ql_log_info, vha, 0x5009,
+		ql_dbg(ql_dbg_async, vha, 0x5009,
 		    "LIP occurred (%x).\n", mb[1]);
 
 		if (atomic_read(&vha->loop_state) != LOOP_DOWN) {
@@ -487,7 +489,7 @@ skip_rio:
 			ha->link_data_rate = mb[1];
 		}
 
-		ql_log(ql_log_info, vha, 0x500a,
+		ql_dbg(ql_dbg_async, vha, 0x500a,
 		    "LOOP UP detected (%s Gbps).\n", link_speed);
 
 		vha->flags.management_server_logged_in = 0;
@@ -497,7 +499,7 @@ skip_rio:
 	case MBA_LOOP_DOWN:		/* Loop Down Event */
 		mbx = IS_QLA81XX(ha) ? RD_REG_WORD(&reg24->mailbox4) : 0;
 		mbx = IS_QLA82XX(ha) ? RD_REG_WORD(&reg82->mailbox_out[4]) : mbx;
-		ql_log(ql_log_info, vha, 0x500b,
+		ql_dbg(ql_dbg_async, vha, 0x500b,
 		    "LOOP DOWN detected (%x %x %x %x).\n",
 		    mb[1], mb[2], mb[3], mbx);
 
@@ -519,7 +521,7 @@ skip_rio:
 		break;
 
 	case MBA_LIP_RESET:		/* LIP reset occurred */
-		ql_log(ql_log_info, vha, 0x500c,
+		ql_dbg(ql_dbg_async, vha, 0x500c,
 		    "LIP reset occurred (%x).\n", mb[1]);
 
 		if (atomic_read(&vha->loop_state) != LOOP_DOWN) {
@@ -587,7 +589,7 @@ skip_rio:
 		if (IS_QLA2100(ha))
 			break;
 
-		ql_log(ql_log_info, vha, 0x500f,
+		ql_dbg(ql_dbg_async, vha, 0x500f,
 		    "Configuration change detected: value=%x.\n", mb[1]);
 
 		if (atomic_read(&vha->loop_state) != LOOP_DOWN) {
@@ -920,15 +922,15 @@ qla2x00_mbx_iocb_entry(scsi_qla_host_t *vha, struct req_que *req,
 	    QLA_LOGIO_LOGIN_RETRIED : 0;
 	if (mbx->entry_status) {
 		ql_dbg(ql_dbg_async, vha, 0x5043,
-		    "Async-%s error entry - portid=%02x%02x%02x "
+		    "Async-%s error entry - hdl=%x portid=%02x%02x%02x "
 		    "entry-status=%x status=%x state-flag=%x "
-		    "status-flags=%x.\n",
-		    type, fcport->d_id.b.domain, fcport->d_id.b.area,
+		    "status-flags=%x.\n", type, sp->handle,
+		    fcport->d_id.b.domain, fcport->d_id.b.area,
 		    fcport->d_id.b.al_pa, mbx->entry_status,
 		    le16_to_cpu(mbx->status), le16_to_cpu(mbx->state_flags),
 		    le16_to_cpu(mbx->status_flags));
 
-		ql_dump_buffer(ql_dbg_async + ql_dbg_buffer, vha, 0x5057,
+		ql_dump_buffer(ql_dbg_async + ql_dbg_buffer, vha, 0x5029,
 		    (uint8_t *)mbx, sizeof(*mbx));
 
 		goto logio_done;
@@ -940,9 +942,10 @@ qla2x00_mbx_iocb_entry(scsi_qla_host_t *vha, struct req_que *req,
 		status = 0;
 	if (!status && le16_to_cpu(mbx->mb0) == MBS_COMMAND_COMPLETE) {
 		ql_dbg(ql_dbg_async, vha, 0x5045,
-		    "Async-%s complete - portid=%02x%02x%02x mbx1=%x.\n",
-		    type, fcport->d_id.b.domain, fcport->d_id.b.area,
-		    fcport->d_id.b.al_pa, le16_to_cpu(mbx->mb1));
+		    "Async-%s complete - hdl=%x portid=%02x%02x%02x mbx1=%x.\n",
+		    type, sp->handle, fcport->d_id.b.domain,
+		    fcport->d_id.b.area, fcport->d_id.b.al_pa,
+		    le16_to_cpu(mbx->mb1));
 
 		data[0] = MBS_COMMAND_COMPLETE;
 		if (ctx->type == SRB_LOGIN_CMD) {
@@ -968,11 +971,10 @@ qla2x00_mbx_iocb_entry(scsi_qla_host_t *vha, struct req_que *req,
 	}
 
 	ql_log(ql_log_warn, vha, 0x5046,
-	    "Async-%s failed - portid=%02x%02x%02x status=%x "
-	    "mb0=%x mb1=%x mb2=%x mb6=%x mb7=%x.\n",
-	    type, fcport->d_id.b.domain,
-	    fcport->d_id.b.area, fcport->d_id.b.al_pa, status,
-	    le16_to_cpu(mbx->mb0), le16_to_cpu(mbx->mb1),
+	    "Async-%s failed - hdl=%x portid=%02x%02x%02x status=%x "
+	    "mb0=%x mb1=%x mb2=%x mb6=%x mb7=%x.\n", type, sp->handle,
+	    fcport->d_id.b.domain, fcport->d_id.b.area, fcport->d_id.b.al_pa,
+	    status, le16_to_cpu(mbx->mb0), le16_to_cpu(mbx->mb1),
 	    le16_to_cpu(mbx->mb2), le16_to_cpu(mbx->mb6),
 	    le16_to_cpu(mbx->mb7));
 
@@ -1036,7 +1038,7 @@ qla2x00_ct_entry(scsi_qla_host_t *vha, struct req_que *req,
 			bsg_job->reply->result = DID_ERROR << 16;
 			bsg_job->reply->reply_payload_rcv_len = 0;
 		}
-		ql_dump_buffer(ql_dbg_async + ql_dbg_buffer, vha, 0x5058,
+		ql_dump_buffer(ql_dbg_async + ql_dbg_buffer, vha, 0x5035,
 		    (uint8_t *)pkt, sizeof(*pkt));
 	} else {
 		bsg_job->reply->result =  DID_OK << 16;
@@ -1111,9 +1113,9 @@ qla24xx_els_ct_entry(scsi_qla_host_t *vha, struct req_que *req,
 				le16_to_cpu(((struct els_sts_entry_24xx*)pkt)->total_byte_count);
 
 			ql_log(ql_log_info, vha, 0x503f,
-			    "ELS-CT pass-through-%s error comp_status-status=0x%x "
+			    "ELS-CT pass-through-%s error hdl=%x comp_status-status=0x%x "
 			    "error subcode 1=0x%x error subcode 2=0x%x total_byte = 0x%x.\n",
-			    type, comp_status, fw_status[1], fw_status[2],
+			    type, sp->handle, comp_status, fw_status[1], fw_status[2],
 			    le16_to_cpu(((struct els_sts_entry_24xx *)
 				pkt)->total_byte_count));
 			fw_sts_ptr = ((uint8_t*)bsg_job->req->sense) + sizeof(struct fc_bsg_reply);
@@ -1121,9 +1123,9 @@ qla24xx_els_ct_entry(scsi_qla_host_t *vha, struct req_que *req,
 		}
 		else {
 			ql_log(ql_log_info, vha, 0x5040,
-			    "ELS-CT pass-through-%s error comp_status-status=0x%x "
+			    "ELS-CT pass-through-%s error hdl=%x comp_status-status=0x%x "
 			    "error subcode 1=0x%x error subcode 2=0x%x.\n",
-			    type, comp_status,
+			    type, sp->handle, comp_status,
 			    le16_to_cpu(((struct els_sts_entry_24xx *)
 				pkt)->error_subcode_1),
 			    le16_to_cpu(((struct els_sts_entry_24xx *)
@@ -1184,11 +1186,12 @@ qla24xx_logio_entry(scsi_qla_host_t *vha, struct req_que *req,
 		QLA_LOGIO_LOGIN_RETRIED : 0;
 	if (logio->entry_status) {
 		ql_log(ql_log_warn, vha, 0x5034,
-		    "Async-%s error entry - "
+		    "Async-%s error entry - hdl=%x"
 		    "portid=%02x%02x%02x entry-status=%x.\n",
-		    type, fcport->d_id.b.domain, fcport->d_id.b.area,
-		    fcport->d_id.b.al_pa, logio->entry_status);
-		ql_dump_buffer(ql_dbg_async + ql_dbg_buffer, vha, 0x5059,
+		    type, sp->handle, fcport->d_id.b.domain,
+		    fcport->d_id.b.area, fcport->d_id.b.al_pa,
+		    logio->entry_status);
+		ql_dump_buffer(ql_dbg_async + ql_dbg_buffer, vha, 0x504d,
 		    (uint8_t *)logio, sizeof(*logio));
 
 		goto logio_done;
@@ -1196,10 +1199,9 @@ qla24xx_logio_entry(scsi_qla_host_t *vha, struct req_que *req,
 
 	if (le16_to_cpu(logio->comp_status) == CS_COMPLETE) {
 		ql_dbg(ql_dbg_async, vha, 0x5036,
-		    "Async-%s complete - portid=%02x%02x%02x "
-		    "iop0=%x.\n",
-		    type, fcport->d_id.b.domain, fcport->d_id.b.area,
-		    fcport->d_id.b.al_pa,
+		    "Async-%s complete - hdl=%x portid=%02x%02x%02x "
+		    "iop0=%x.\n", type, sp->handle, fcport->d_id.b.domain,
+		    fcport->d_id.b.area, fcport->d_id.b.al_pa,
 		    le32_to_cpu(logio->io_parameter[0]));
 
 		data[0] = MBS_COMMAND_COMPLETE;
@@ -1238,9 +1240,8 @@ qla24xx_logio_entry(scsi_qla_host_t *vha, struct req_que *req,
 	}
 
 	ql_dbg(ql_dbg_async, vha, 0x5037,
-	    "Async-%s failed - portid=%02x%02x%02x comp=%x "
-	    "iop0=%x iop1=%x.\n",
-	    type, fcport->d_id.b.domain,
+	    "Async-%s failed - hdl=%x portid=%02x%02x%02x comp=%x "
+	    "iop0=%x iop1=%x.\n", type, sp->handle, fcport->d_id.b.domain,
 	    fcport->d_id.b.area, fcport->d_id.b.al_pa,
 	    le16_to_cpu(logio->comp_status),
 	    le32_to_cpu(logio->io_parameter[0]),
@@ -1274,25 +1275,25 @@ qla24xx_tm_iocb_entry(scsi_qla_host_t *vha, struct req_que *req,
 
 	if (sts->entry_status) {
 		ql_log(ql_log_warn, vha, 0x5038,
-		    "Async-%s error - entry-status(%x).\n",
-		    type, sts->entry_status);
+		    "Async-%s error - hdl=%x entry-status(%x).\n",
+		    type, sp->handle, sts->entry_status);
 	} else if (sts->comp_status != __constant_cpu_to_le16(CS_COMPLETE)) {
 		ql_log(ql_log_warn, vha, 0x5039,
-		    "Async-%s error - completion status(%x).\n",
-		    type, sts->comp_status);
+		    "Async-%s error - hdl=%x completion status(%x).\n",
+		    type, sp->handle, sts->comp_status);
 	} else if (!(le16_to_cpu(sts->scsi_status) &
 	    SS_RESPONSE_INFO_LEN_VALID)) {
 		ql_log(ql_log_warn, vha, 0x503a,
-		    "Async-%s error - no response info(%x).\n",
-		    type, sts->scsi_status);
+		    "Async-%s error - hdl=%x no response info(%x).\n",
+		    type, sp->handle, sts->scsi_status);
 	} else if (le32_to_cpu(sts->rsp_data_len) < 4) {
 		ql_log(ql_log_warn, vha, 0x503b,
-		    "Async-%s error - not enough response(%d).\n",
-		    type, sts->rsp_data_len);
+		    "Async-%s error - hdl=%x not enough response(%d).\n",
+		    type, sp->handle, sts->rsp_data_len);
 	} else if (sts->data[3]) {
 		ql_log(ql_log_warn, vha, 0x503c,
-		    "Async-%s error - response(%x).\n",
-		    type, sts->data[3]);
+		    "Async-%s error - hdl=%x response(%x).\n",
+		    type, sp->handle, sts->data[3]);
 	} else {
 		error = 0;
 	}
@@ -1337,9 +1338,6 @@ qla2x00_process_response_queue(struct rsp_que *rsp)
 		}
 
 		if (pkt->entry_status != 0) {
-			ql_log(ql_log_warn, vha, 0x5035,
-			    "Process error entry.\n");
-
 			qla2x00_error_entry(vha, rsp, pkt);
 			((response_t *)pkt)->signature = RESPONSE_PROCESSED;
 			wmb();
@@ -1391,7 +1389,6 @@ qla2x00_process_response_queue(struct rsp_que *rsp)
 }
 
 static inline void
-
 qla2x00_handle_sense(srb_t *sp, uint8_t *sense_data, uint32_t par_sense_len,
     uint32_t sense_len, struct rsp_que *rsp)
 {
@@ -1413,13 +1410,14 @@ qla2x00_handle_sense(srb_t *sp, uint8_t *sense_data, uint32_t par_sense_len,
 	if (sp->request_sense_length != 0)
 		rsp->status_srb = sp;
 
-	ql_dbg(ql_dbg_io, vha, 0x301c,
-	    "Check condition Sense data, scsi(%ld:%d:%d:%d) cmd=%p.\n",
-	    sp->fcport->vha->host_no, cp->device->channel, cp->device->id,
-	    cp->device->lun, cp);
-	if (sense_len)
+	if (sense_len) {
+		ql_dbg(ql_dbg_io + ql_dbg_buffer, vha, 0x301c,
+		    "Check condition Sense data, nexus%ld:%d:%d cmd=%p.\n",
+		    sp->fcport->vha->host_no, cp->device->id, cp->device->lun,
+		    cp);
 		ql_dump_buffer(ql_dbg_io + ql_dbg_buffer, vha, 0x302b,
 		    cp->sense_buffer, sense_len);
+	}
 }
 
 struct scsi_dif_tuple {
@@ -1506,7 +1504,7 @@ qla2x00_handle_dif_error(srb_t *sp, struct sts_entry_24xx *sts24)
 			}
 
 			if (k != blocks_done) {
-				qla_printk(KERN_WARNING, sp->fcport->vha->hw,
+				ql_log(ql_log_warn, vha, 0x302f,
 				    "unexpected tag values tag:lba=%x:%llx)\n",
 				    e_ref_tag, (unsigned long long)lba_s);
 				return 1;
@@ -1611,7 +1609,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 		sp = NULL;
 
 	if (sp == NULL) {
-		ql_log(ql_log_warn, vha, 0x3017,
+		ql_dbg(ql_dbg_io, vha, 0x3017,
 		    "Invalid status handle (0x%x).\n", sts->handle);
 
 		if (IS_QLA82XX(ha))
@@ -1623,7 +1621,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 	}
 	cp = sp->cmd;
 	if (cp == NULL) {
-		ql_log(ql_log_warn, vha, 0x3018,
+		ql_dbg(ql_dbg_io, vha, 0x3018,
 		    "Command already returned (0x%x/%p).\n",
 		    sts->handle, sp);
 
@@ -1670,7 +1668,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 			par_sense_len -= rsp_info_len;
 		}
 		if (rsp_info_len > 3 && rsp_info[3]) {
-			ql_log(ql_log_warn, vha, 0x3019,
+			ql_dbg(ql_dbg_io, vha, 0x3019,
 			    "FCP I/O protocol failure (0x%x/0x%x).\n",
 			    rsp_info_len, rsp_info[3]);
 
@@ -1701,7 +1699,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 			if (!lscsi_status &&
 			    ((unsigned)(scsi_bufflen(cp) - resid) <
 			     cp->underflow)) {
-				ql_log(ql_log_warn, vha, 0x301a,
+				ql_dbg(ql_dbg_io, vha, 0x301a,
 				    "Mid-layer underflow "
 				    "detected (0x%x of 0x%x bytes).\n",
 				    resid, scsi_bufflen(cp));
@@ -1713,7 +1711,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 		cp->result = DID_OK << 16 | lscsi_status;
 
 		if (lscsi_status == SAM_STAT_TASK_SET_FULL) {
-			ql_log(ql_log_warn, vha, 0x301b,
+			ql_dbg(ql_dbg_io, vha, 0x301b,
 			    "QUEUE FULL detected.\n");
 			break;
 		}
@@ -1735,7 +1733,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 		scsi_set_resid(cp, resid);
 		if (scsi_status & SS_RESIDUAL_UNDER) {
 			if (IS_FWI2_CAPABLE(ha) && fw_resid_len != resid_len) {
-				ql_log(ql_log_warn, vha, 0x301d,
+				ql_dbg(ql_dbg_io, vha, 0x301d,
 				    "Dropped frame(s) detected "
 				    "(0x%x of 0x%x bytes).\n",
 				    resid, scsi_bufflen(cp));
@@ -1747,7 +1745,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 			if (!lscsi_status &&
 			    ((unsigned)(scsi_bufflen(cp) - resid) <
 			    cp->underflow)) {
-				ql_log(ql_log_warn, vha, 0x301e,
+				ql_dbg(ql_dbg_io, vha, 0x301e,
 				    "Mid-layer underflow "
 				    "detected (0x%x of 0x%x bytes).\n",
 				    resid, scsi_bufflen(cp));
@@ -1756,7 +1754,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 				break;
 			}
 		} else {
-			ql_log(ql_log_warn, vha, 0x301f,
+			ql_dbg(ql_dbg_io, vha, 0x301f,
 			    "Dropped frame(s) detected (0x%x "
 			    "of 0x%x bytes).\n", resid, scsi_bufflen(cp));
 
@@ -1774,7 +1772,7 @@ check_scsi_status:
 		 */
 		if (lscsi_status != 0) {
 			if (lscsi_status == SAM_STAT_TASK_SET_FULL) {
-				ql_log(ql_log_warn, vha, 0x3020,
+				ql_dbg(ql_dbg_io, vha, 0x3020,
 				    "QUEUE FULL detected.\n");
 				logit = 1;
 				break;
@@ -1838,10 +1836,15 @@ out:
 	if (logit)
 		ql_dbg(ql_dbg_io, vha, 0x3022,
 		    "FCP command status: 0x%x-0x%x (0x%x) "
-		    "oxid=0x%x cdb=%02x%02x%02x len=0x%x "
+		    "nexus=%ld:%d:%d portid=%02x%02x%02x oxid=0x%x "
+		    "cdb=%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x len=0x%x "
 		    "rsp_info=0x%x resid=0x%x fw_resid=0x%x.\n",
-		    comp_status, scsi_status, cp->result, ox_id, cp->cmnd[0],
-		    cp->cmnd[1], cp->cmnd[2], scsi_bufflen(cp), rsp_info_len,
+		    comp_status, scsi_status, cp->result, vha->host_no,
+		    cp->device->id, cp->device->lun, fcport->d_id.b.domain,
+		    fcport->d_id.b.area, fcport->d_id.b.al_pa, ox_id,
+		    cp->cmnd[0], cp->cmnd[1], cp->cmnd[2], cp->cmnd[3],
+		    cp->cmnd[4], cp->cmnd[5], cp->cmnd[6], cp->cmnd[7],
+		    cp->cmnd[8], cp->cmnd[9], scsi_bufflen(cp), rsp_info_len,
 		    resid_len, fw_resid_len);
 
 	if (rsp->status_srb == NULL)
@@ -1899,6 +1902,45 @@ qla2x00_status_cont_entry(struct rsp_que *rsp, sts_cont_entry_t *pkt)
 	}
 }
 
+static int
+qla2x00_free_sp_ctx(scsi_qla_host_t *vha, srb_t *sp)
+{
+	struct qla_hw_data *ha = vha->hw;
+	struct srb_ctx *ctx;
+
+	if (!sp->ctx)
+		return 1;
+
+	ctx = sp->ctx;
+
+	if (ctx->type == SRB_LOGIN_CMD ||
+	    ctx->type == SRB_LOGOUT_CMD ||
+	    ctx->type == SRB_TM_CMD) {
+		ctx->u.iocb_cmd->done(sp);
+		return 0;
+	} else if (ctx->type == SRB_ADISC_CMD) {
+		ctx->u.iocb_cmd->free(sp);
+		return 0;
+	} else {
+		struct fc_bsg_job *bsg_job;
+
+		bsg_job = ctx->u.bsg_job;
+		if (ctx->type == SRB_ELS_CMD_HST ||
+		    ctx->type == SRB_CT_CMD)
+			kfree(sp->fcport);
+
+		bsg_job->reply->reply_data.ctels_reply.status =
+		    FC_CTELS_STATUS_OK;
+		bsg_job->reply->result = DID_ERROR << 16;
+		bsg_job->reply->reply_payload_rcv_len = 0;
+		kfree(sp->ctx);
+		mempool_free(sp, ha->srb_mempool);
+		bsg_job->job_done(bsg_job);
+		return 0;
+	}
+	return 1;
+}
+
 /**
  * qla2x00_error_entry() - Process an error entry.
  * @ha: SCSI driver HA context
@@ -1909,7 +1951,7 @@ qla2x00_error_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, sts_entry_t *pkt)
 {
 	srb_t *sp;
 	struct qla_hw_data *ha = vha->hw;
-	uint32_t handle = LSW(pkt->handle);
+	const char func[] = "ERROR-IOCB";
 	uint16_t que = MSW(pkt->handle);
 	struct req_que *req = ha->req_q_map[que];
 
@@ -1932,28 +1974,20 @@ qla2x00_error_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, sts_entry_t *pkt)
 		ql_dbg(ql_dbg_async, vha, 0x502f,
 		    "UNKNOWN flag error.\n");
 
-	/* Validate handle. */
-	if (handle < MAX_OUTSTANDING_COMMANDS)
-		sp = req->outstanding_cmds[handle];
-	else
-		sp = NULL;
-
+	sp = qla2x00_get_sp_from_handle(vha, func, req, pkt);
 	if (sp) {
-		/* Free outstanding command slot. */
-		req->outstanding_cmds[handle] = NULL;
-
-		/* Bad payload or header */
-		if (pkt->entry_status &
-		    (RF_INV_E_ORDER | RF_INV_E_COUNT |
-		     RF_INV_E_PARAM | RF_INV_E_TYPE)) {
-			sp->cmd->result = DID_ERROR << 16;
-		} else if (pkt->entry_status & RF_BUSY) {
-			sp->cmd->result = DID_BUS_BUSY << 16;
-		} else {
-			sp->cmd->result = DID_ERROR << 16;
+		if (qla2x00_free_sp_ctx(vha, sp)) {
+			if (pkt->entry_status &
+			    (RF_INV_E_ORDER | RF_INV_E_COUNT |
+			     RF_INV_E_PARAM | RF_INV_E_TYPE)) {
+				sp->cmd->result = DID_ERROR << 16;
+			} else if (pkt->entry_status & RF_BUSY) {
+				sp->cmd->result = DID_BUS_BUSY << 16;
+			} else {
+				sp->cmd->result = DID_ERROR << 16;
+			}
+			qla2x00_sp_compl(ha, sp);
 		}
-		qla2x00_sp_compl(ha, sp);
-
 	} else if (pkt->entry_type == COMMAND_A64_TYPE || pkt->entry_type ==
 		COMMAND_TYPE || pkt->entry_type == COMMAND_TYPE_7
 		|| pkt->entry_type == COMMAND_TYPE_6) {
@@ -1977,26 +2011,30 @@ static void
 qla24xx_mbx_completion(scsi_qla_host_t *vha, uint16_t mb0)
 {
 	uint16_t	cnt;
+	uint32_t	mboxes;
 	uint16_t __iomem *wptr;
 	struct qla_hw_data *ha = vha->hw;
 	struct device_reg_24xx __iomem *reg = &ha->iobase->isp24;
 
+	/* Read all mbox registers? */
+	mboxes = (1 << ha->mbx_count) - 1;
+	if (!ha->mcp)
+		ql_dbg(ql_dbg_async, vha, 0x504e, "MBX pointer ERRROR.\n");
+	else
+		mboxes = ha->mcp->in_mb;
+
 	/* Load return mailbox registers. */
 	ha->flags.mbox_int = 1;
 	ha->mailbox_out[0] = mb0;
+	mboxes >>= 1;
 	wptr = (uint16_t __iomem *)&reg->mailbox1;
 
 	for (cnt = 1; cnt < ha->mbx_count; cnt++) {
-		ha->mailbox_out[cnt] = RD_REG_WORD(wptr);
-		wptr++;
-	}
+		if (mboxes & BIT_0)
+			ha->mailbox_out[cnt] = RD_REG_WORD(wptr);
 
-	if (ha->mcp) {
-		ql_dbg(ql_dbg_async, vha, 0x504d,
-		    "Got mailbox completion. cmd=%x.\n", ha->mcp->mb[0]);
-	} else {
-		ql_dbg(ql_dbg_async, vha, 0x504e,
-		    "MBX pointer ERROR.\n");
+		mboxes >>= 1;
+		wptr++;
 	}
 }
 
@@ -2025,9 +2063,6 @@ void qla24xx_process_response_queue(struct scsi_qla_host *vha,
 		}
 
 		if (pkt->entry_status != 0) {
-			ql_dbg(ql_dbg_async, vha, 0x5029,
-			    "Process error entry.\n");
-
 			qla2x00_error_entry(vha, rsp, (sts_entry_t *) pkt);
 			((response_t *)pkt)->signature = RESPONSE_PROCESSED;
 			wmb();
diff --git a/drivers/scsi/qla2xxx/qla_mbx.c b/drivers/scsi/qla2xxx/qla_mbx.c
index 82a33533ed26..34344d3f8658 100644
--- a/drivers/scsi/qla2xxx/qla_mbx.c
+++ b/drivers/scsi/qla2xxx/qla_mbx.c
@@ -2887,7 +2887,7 @@ qla24xx_report_id_acquisition(scsi_qla_host_t *vha,
 		if (vp_idx == 0 && (MSB(stat) != 1))
 			goto reg_needed;
 
-		if (MSB(stat) == 1) {
+		if (MSB(stat) != 0) {
 			ql_dbg(ql_dbg_mbx, vha, 0x10ba,
 			    "Could not acquire ID for VP[%d].\n", vp_idx);
 			return;
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index 1873940a7ccb..1cd46cd7ff90 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -369,7 +369,7 @@ qla82xx_pci_set_crbwindow_2M(struct qla_hw_data *ha, ulong *off)
 		ql_dbg(ql_dbg_p3p, vha, 0xb000,
 		    "%s: Written crbwin (0x%x) "
 		    "!= Read crbwin (0x%x), off=0x%lx.\n",
-		    ha->crb_win, win_read, *off);
+		    __func__, ha->crb_win, win_read, *off);
 	}
 	*off = (*off & MASK(16)) + CRB_INDIRECT_2M + ha->nx_pcibase;
 }
@@ -409,7 +409,7 @@ qla82xx_pci_set_crbwindow(struct qla_hw_data *ha, u64 off)
 	}
 	/* strange address given */
 	ql_dbg(ql_dbg_p3p, vha, 0xb001,
-	    "%x: Warning: unm_nic_pci_set_crbwindow "
+	    "%s: Warning: unm_nic_pci_set_crbwindow "
 	    "called with an unknown address(%llx).\n",
 	    QLA2XXX_DRIVER_NAME, off);
 	return off;
@@ -1711,12 +1711,12 @@ qla82xx_iospace_config(struct qla_hw_data *ha)
 	ql_dbg_pci(ql_dbg_multiq, ha->pdev, 0xc006,
 	    "nx_pci_base=%p iobase=%p "
 	    "max_req_queues=%d msix_count=%d.\n",
-	    ha->nx_pcibase, ha->iobase,
+	    (void *)ha->nx_pcibase, ha->iobase,
 	    ha->max_req_queues, ha->msix_count);
 	ql_dbg_pci(ql_dbg_init, ha->pdev, 0x0010,
 	    "nx_pci_base=%p iobase=%p "
 	    "max_req_queues=%d msix_count=%d.\n",
-	    ha->nx_pcibase, ha->iobase,
+	    (void *)ha->nx_pcibase, ha->iobase,
 	    ha->max_req_queues, ha->msix_count);
 	return 0;
 
@@ -1744,7 +1744,7 @@ qla82xx_pci_config(scsi_qla_host_t *vha)
 	ret = pci_set_mwi(ha->pdev);
 	ha->chip_revision = ha->pdev->revision;
 	ql_dbg(ql_dbg_init, vha, 0x0043,
-	    "Chip revision:%ld.\n",
+	    "Chip revision:%d.\n",
 	    ha->chip_revision);
 	return 0;
 }
@@ -2023,13 +2023,9 @@ qla82xx_mbx_completion(scsi_qla_host_t *vha, uint16_t mb0)
 		wptr++;
 	}
 
-	if (ha->mcp) {
-		ql_dbg(ql_dbg_async, vha, 0x5052,
-		    "Got mailbox completion. cmd=%x.\n", ha->mcp->mb[0]);
-	} else {
+	if (!ha->mcp)
 		ql_dbg(ql_dbg_async, vha, 0x5053,
 		    "MBX pointer ERROR.\n");
-	}
 }
 
 /*
@@ -2543,484 +2539,6 @@ qla82xx_start_firmware(scsi_qla_host_t *vha)
 	return qla82xx_check_rcvpeg_state(ha);
 }
 
-static inline int
-qla2xx_build_scsi_type_6_iocbs(srb_t *sp, struct cmd_type_6 *cmd_pkt,
-	uint16_t tot_dsds)
-{
-	uint32_t *cur_dsd = NULL;
-	scsi_qla_host_t	*vha;
-	struct qla_hw_data *ha;
-	struct scsi_cmnd *cmd;
-	struct	scatterlist *cur_seg;
-	uint32_t *dsd_seg;
-	void *next_dsd;
-	uint8_t avail_dsds;
-	uint8_t first_iocb = 1;
-	uint32_t dsd_list_len;
-	struct dsd_dma *dsd_ptr;
-	struct ct6_dsd *ctx;
-
-	cmd = sp->cmd;
-
-	/* Update entry type to indicate Command Type 3 IOCB */
-	*((uint32_t *)(&cmd_pkt->entry_type)) =
-		__constant_cpu_to_le32(COMMAND_TYPE_6);
-
-	/* No data transfer */
-	if (!scsi_bufflen(cmd) || cmd->sc_data_direction == DMA_NONE) {
-		cmd_pkt->byte_count = __constant_cpu_to_le32(0);
-		return 0;
-	}
-
-	vha = sp->fcport->vha;
-	ha = vha->hw;
-
-	/* Set transfer direction */
-	if (cmd->sc_data_direction == DMA_TO_DEVICE) {
-		cmd_pkt->control_flags =
-		    __constant_cpu_to_le16(CF_WRITE_DATA);
-		ha->qla_stats.output_bytes += scsi_bufflen(cmd);
-	} else if (cmd->sc_data_direction == DMA_FROM_DEVICE) {
-		cmd_pkt->control_flags =
-		    __constant_cpu_to_le16(CF_READ_DATA);
-		ha->qla_stats.input_bytes += scsi_bufflen(cmd);
-	}
-
-	cur_seg = scsi_sglist(cmd);
-	ctx = sp->ctx;
-
-	while (tot_dsds) {
-		avail_dsds = (tot_dsds > QLA_DSDS_PER_IOCB) ?
-		    QLA_DSDS_PER_IOCB : tot_dsds;
-		tot_dsds -= avail_dsds;
-		dsd_list_len = (avail_dsds + 1) * QLA_DSD_SIZE;
-
-		dsd_ptr = list_first_entry(&ha->gbl_dsd_list,
-		    struct dsd_dma, list);
-		next_dsd = dsd_ptr->dsd_addr;
-		list_del(&dsd_ptr->list);
-		ha->gbl_dsd_avail--;
-		list_add_tail(&dsd_ptr->list, &ctx->dsd_list);
-		ctx->dsd_use_cnt++;
-		ha->gbl_dsd_inuse++;
-
-		if (first_iocb) {
-			first_iocb = 0;
-			dsd_seg = (uint32_t *)&cmd_pkt->fcp_data_dseg_address;
-			*dsd_seg++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
-			*dsd_seg++ = cpu_to_le32(MSD(dsd_ptr->dsd_list_dma));
-			cmd_pkt->fcp_data_dseg_len = cpu_to_le32(dsd_list_len);
-		} else {
-			*cur_dsd++ = cpu_to_le32(LSD(dsd_ptr->dsd_list_dma));
-			*cur_dsd++ = cpu_to_le32(MSD(dsd_ptr->dsd_list_dma));
-			*cur_dsd++ = cpu_to_le32(dsd_list_len);
-		}
-		cur_dsd = (uint32_t *)next_dsd;
-		while (avail_dsds) {
-			dma_addr_t	sle_dma;
-
-			sle_dma = sg_dma_address(cur_seg);
-			*cur_dsd++ = cpu_to_le32(LSD(sle_dma));
-			*cur_dsd++ = cpu_to_le32(MSD(sle_dma));
-			*cur_dsd++ = cpu_to_le32(sg_dma_len(cur_seg));
-			cur_seg = sg_next(cur_seg);
-			avail_dsds--;
-		}
-	}
-
-	/* Null termination */
-	*cur_dsd++ =  0;
-	*cur_dsd++ = 0;
-	*cur_dsd++ = 0;
-	cmd_pkt->control_flags |= CF_DATA_SEG_DESCR_ENABLE;
-	return 0;
-}
-
-/*
- * qla82xx_calc_dsd_lists() - Determine number of DSD list required
- * for Command Type 6.
- *
- * @dsds: number of data segment decriptors needed
- *
- * Returns the number of dsd list needed to store @dsds.
- */
-inline uint16_t
-qla82xx_calc_dsd_lists(uint16_t dsds)
-{
-	uint16_t dsd_lists = 0;
-
-	dsd_lists = (dsds/QLA_DSDS_PER_IOCB);
-	if (dsds % QLA_DSDS_PER_IOCB)
-		dsd_lists++;
-	return dsd_lists;
-}
-
-/*
- * qla82xx_start_scsi() - Send a SCSI command to the ISP
- * @sp: command to send to the ISP
- *
- * Returns non-zero if a failure occurred, else zero.
- */
-int
-qla82xx_start_scsi(srb_t *sp)
-{
-	int		ret, nseg;
-	unsigned long   flags;
-	struct scsi_cmnd *cmd;
-	uint32_t	*clr_ptr;
-	uint32_t        index;
-	uint32_t	handle;
-	uint16_t	cnt;
-	uint16_t	req_cnt;
-	uint16_t	tot_dsds;
-	struct device_reg_82xx __iomem *reg;
-	uint32_t dbval;
-	uint32_t *fcp_dl;
-	uint8_t additional_cdb_len;
-	struct ct6_dsd *ctx;
-	struct scsi_qla_host *vha = sp->fcport->vha;
-	struct qla_hw_data *ha = vha->hw;
-	struct req_que *req = NULL;
-	struct rsp_que *rsp = NULL;
-	char		tag[2];
-
-	/* Setup device pointers. */
-	ret = 0;
-	reg = &ha->iobase->isp82;
-	cmd = sp->cmd;
-	req = vha->req;
-	rsp = ha->rsp_q_map[0];
-
-	/* So we know we haven't pci_map'ed anything yet */
-	tot_dsds = 0;
-
-	dbval = 0x04 | (ha->portnum << 5);
-
-	/* Send marker if required */
-	if (vha->marker_needed != 0) {
-		if (qla2x00_marker(vha, req,
-			rsp, 0, 0, MK_SYNC_ALL) != QLA_SUCCESS) {
-			ql_log(ql_log_warn, vha, 0x300c,
-			    "qla2x00_marker failed for cmd=%p.\n", cmd);
-			return QLA_FUNCTION_FAILED;
-		}
-		vha->marker_needed = 0;
-	}
-
-	/* Acquire ring specific lock */
-	spin_lock_irqsave(&ha->hardware_lock, flags);
-
-	/* Check for room in outstanding command list. */
-	handle = req->current_outstanding_cmd;
-	for (index = 1; index < MAX_OUTSTANDING_COMMANDS; index++) {
-		handle++;
-		if (handle == MAX_OUTSTANDING_COMMANDS)
-			handle = 1;
-		if (!req->outstanding_cmds[handle])
-			break;
-	}
-	if (index == MAX_OUTSTANDING_COMMANDS)
-		goto queuing_error;
-
-	/* Map the sg table so we have an accurate count of sg entries needed */
-	if (scsi_sg_count(cmd)) {
-		nseg = dma_map_sg(&ha->pdev->dev, scsi_sglist(cmd),
-		    scsi_sg_count(cmd), cmd->sc_data_direction);
-		if (unlikely(!nseg))
-			goto queuing_error;
-	} else
-		nseg = 0;
-
-	tot_dsds = nseg;
-
-	if (tot_dsds > ql2xshiftctondsd) {
-		struct cmd_type_6 *cmd_pkt;
-		uint16_t more_dsd_lists = 0;
-		struct dsd_dma *dsd_ptr;
-		uint16_t i;
-
-		more_dsd_lists = qla82xx_calc_dsd_lists(tot_dsds);
-		if ((more_dsd_lists + ha->gbl_dsd_inuse) >= NUM_DSD_CHAIN) {
-			ql_dbg(ql_dbg_io, vha, 0x300d,
-			    "Num of DSD list %d is than %d for cmd=%p.\n",
-			    more_dsd_lists + ha->gbl_dsd_inuse, NUM_DSD_CHAIN,
-			    cmd);
-			goto queuing_error;
-		}
-
-		if (more_dsd_lists <= ha->gbl_dsd_avail)
-			goto sufficient_dsds;
-		else
-			more_dsd_lists -= ha->gbl_dsd_avail;
-
-		for (i = 0; i < more_dsd_lists; i++) {
-			dsd_ptr = kzalloc(sizeof(struct dsd_dma), GFP_ATOMIC);
-			if (!dsd_ptr) {
-				ql_log(ql_log_fatal, vha, 0x300e,
-				    "Failed to allocate memory for dsd_dma "
-				    "for cmd=%p.\n", cmd);
-				goto queuing_error;
-			}
-
-			dsd_ptr->dsd_addr = dma_pool_alloc(ha->dl_dma_pool,
-				GFP_ATOMIC, &dsd_ptr->dsd_list_dma);
-			if (!dsd_ptr->dsd_addr) {
-				kfree(dsd_ptr);
-				ql_log(ql_log_fatal, vha, 0x300f,
-				    "Failed to allocate memory for dsd_addr "
-				    "for cmd=%p.\n", cmd);
-				goto queuing_error;
-			}
-			list_add_tail(&dsd_ptr->list, &ha->gbl_dsd_list);
-			ha->gbl_dsd_avail++;
-		}
-
-sufficient_dsds:
-		req_cnt = 1;
-
-		if (req->cnt < (req_cnt + 2)) {
-			cnt = (uint16_t)RD_REG_DWORD_RELAXED(
-				&reg->req_q_out[0]);
-			if (req->ring_index < cnt)
-				req->cnt = cnt - req->ring_index;
-			else
-				req->cnt = req->length -
-					(req->ring_index - cnt);
-		}
-
-		if (req->cnt < (req_cnt + 2))
-			goto queuing_error;
-
-		ctx = sp->ctx = mempool_alloc(ha->ctx_mempool, GFP_ATOMIC);
-		if (!sp->ctx) {
-			ql_log(ql_log_fatal, vha, 0x3010,
-			    "Failed to allocate ctx for cmd=%p.\n", cmd);
-			goto queuing_error;
-		}
-		memset(ctx, 0, sizeof(struct ct6_dsd));
-		ctx->fcp_cmnd = dma_pool_alloc(ha->fcp_cmnd_dma_pool,
-			GFP_ATOMIC, &ctx->fcp_cmnd_dma);
-		if (!ctx->fcp_cmnd) {
-			ql_log(ql_log_fatal, vha, 0x3011,
-			    "Failed to allocate fcp_cmnd for cmd=%p.\n", cmd);
-			goto queuing_error_fcp_cmnd;
-		}
-
-		/* Initialize the DSD list and dma handle */
-		INIT_LIST_HEAD(&ctx->dsd_list);
-		ctx->dsd_use_cnt = 0;
-
-		if (cmd->cmd_len > 16) {
-			additional_cdb_len = cmd->cmd_len - 16;
-			if ((cmd->cmd_len % 4) != 0) {
-				/* SCSI command bigger than 16 bytes must be
-				 * multiple of 4
-				 */
-				ql_log(ql_log_warn, vha, 0x3012,
-				    "scsi cmd len %d not multiple of 4 "
-				    "for cmd=%p.\n", cmd->cmd_len, cmd);
-				goto queuing_error_fcp_cmnd;
-			}
-			ctx->fcp_cmnd_len = 12 + cmd->cmd_len + 4;
-		} else {
-			additional_cdb_len = 0;
-			ctx->fcp_cmnd_len = 12 + 16 + 4;
-		}
-
-		cmd_pkt = (struct cmd_type_6 *)req->ring_ptr;
-		cmd_pkt->handle = MAKE_HANDLE(req->id, handle);
-
-		/* Zero out remaining portion of packet. */
-		/*    tagged queuing modifier -- default is TSK_SIMPLE (0). */
-		clr_ptr = (uint32_t *)cmd_pkt + 2;
-		memset(clr_ptr, 0, REQUEST_ENTRY_SIZE - 8);
-		cmd_pkt->dseg_count = cpu_to_le16(tot_dsds);
-
-		/* Set NPORT-ID and LUN number*/
-		cmd_pkt->nport_handle = cpu_to_le16(sp->fcport->loop_id);
-		cmd_pkt->port_id[0] = sp->fcport->d_id.b.al_pa;
-		cmd_pkt->port_id[1] = sp->fcport->d_id.b.area;
-		cmd_pkt->port_id[2] = sp->fcport->d_id.b.domain;
-		cmd_pkt->vp_index = sp->fcport->vp_idx;
-
-		/* Build IOCB segments */
-		if (qla2xx_build_scsi_type_6_iocbs(sp, cmd_pkt, tot_dsds))
-			goto queuing_error_fcp_cmnd;
-
-		int_to_scsilun(sp->cmd->device->lun, &cmd_pkt->lun);
-		host_to_fcp_swap((uint8_t *)&cmd_pkt->lun, sizeof(cmd_pkt->lun));
-
-		/* build FCP_CMND IU */
-		memset(ctx->fcp_cmnd, 0, sizeof(struct fcp_cmnd));
-		int_to_scsilun(sp->cmd->device->lun, &ctx->fcp_cmnd->lun);
-		ctx->fcp_cmnd->additional_cdb_len = additional_cdb_len;
-
-		if (cmd->sc_data_direction == DMA_TO_DEVICE)
-			ctx->fcp_cmnd->additional_cdb_len |= 1;
-		else if (cmd->sc_data_direction == DMA_FROM_DEVICE)
-			ctx->fcp_cmnd->additional_cdb_len |= 2;
-
-		/*
-		 * Update tagged queuing modifier -- default is TSK_SIMPLE (0).
-		 */
-		if (scsi_populate_tag_msg(cmd, tag)) {
-			switch (tag[0]) {
-			case HEAD_OF_QUEUE_TAG:
-				ctx->fcp_cmnd->task_attribute =
-				    TSK_HEAD_OF_QUEUE;
-				break;
-			case ORDERED_QUEUE_TAG:
-				ctx->fcp_cmnd->task_attribute =
-				    TSK_ORDERED;
-				break;
-			}
-		}
-
-		memcpy(ctx->fcp_cmnd->cdb, cmd->cmnd, cmd->cmd_len);
-
-		fcp_dl = (uint32_t *)(ctx->fcp_cmnd->cdb + 16 +
-		    additional_cdb_len);
-		*fcp_dl = htonl((uint32_t)scsi_bufflen(cmd));
-
-		cmd_pkt->fcp_cmnd_dseg_len = cpu_to_le16(ctx->fcp_cmnd_len);
-		cmd_pkt->fcp_cmnd_dseg_address[0] =
-		    cpu_to_le32(LSD(ctx->fcp_cmnd_dma));
-		cmd_pkt->fcp_cmnd_dseg_address[1] =
-		    cpu_to_le32(MSD(ctx->fcp_cmnd_dma));
-
-		sp->flags |= SRB_FCP_CMND_DMA_VALID;
-		cmd_pkt->byte_count = cpu_to_le32((uint32_t)scsi_bufflen(cmd));
-		/* Set total data segment count. */
-		cmd_pkt->entry_count = (uint8_t)req_cnt;
-		/* Specify response queue number where
-		 * completion should happen
-		 */
-		cmd_pkt->entry_status = (uint8_t) rsp->id;
-	} else {
-		struct cmd_type_7 *cmd_pkt;
-		req_cnt = qla24xx_calc_iocbs(vha, tot_dsds);
-		if (req->cnt < (req_cnt + 2)) {
-			cnt = (uint16_t)RD_REG_DWORD_RELAXED(
-			    &reg->req_q_out[0]);
-			if (req->ring_index < cnt)
-				req->cnt = cnt - req->ring_index;
-			else
-				req->cnt = req->length -
-					(req->ring_index - cnt);
-		}
-		if (req->cnt < (req_cnt + 2))
-			goto queuing_error;
-
-		cmd_pkt = (struct cmd_type_7 *)req->ring_ptr;
-		cmd_pkt->handle = MAKE_HANDLE(req->id, handle);
-
-		/* Zero out remaining portion of packet. */
-		/* tagged queuing modifier -- default is TSK_SIMPLE (0).*/
-		clr_ptr = (uint32_t *)cmd_pkt + 2;
-		memset(clr_ptr, 0, REQUEST_ENTRY_SIZE - 8);
-		cmd_pkt->dseg_count = cpu_to_le16(tot_dsds);
-
-		/* Set NPORT-ID and LUN number*/
-		cmd_pkt->nport_handle = cpu_to_le16(sp->fcport->loop_id);
-		cmd_pkt->port_id[0] = sp->fcport->d_id.b.al_pa;
-		cmd_pkt->port_id[1] = sp->fcport->d_id.b.area;
-		cmd_pkt->port_id[2] = sp->fcport->d_id.b.domain;
-		cmd_pkt->vp_index = sp->fcport->vp_idx;
-
-		int_to_scsilun(sp->cmd->device->lun, &cmd_pkt->lun);
-		host_to_fcp_swap((uint8_t *)&cmd_pkt->lun,
-			sizeof(cmd_pkt->lun));
-
-		/*
-		 * Update tagged queuing modifier -- default is TSK_SIMPLE (0).
-		 */
-		if (scsi_populate_tag_msg(cmd, tag)) {
-			switch (tag[0]) {
-			case HEAD_OF_QUEUE_TAG:
-				cmd_pkt->task = TSK_HEAD_OF_QUEUE;
-				break;
-			case ORDERED_QUEUE_TAG:
-				cmd_pkt->task = TSK_ORDERED;
-				break;
-			}
-		}
-
-		/* Load SCSI command packet. */
-		memcpy(cmd_pkt->fcp_cdb, cmd->cmnd, cmd->cmd_len);
-		host_to_fcp_swap(cmd_pkt->fcp_cdb, sizeof(cmd_pkt->fcp_cdb));
-
-		cmd_pkt->byte_count = cpu_to_le32((uint32_t)scsi_bufflen(cmd));
-
-		/* Build IOCB segments */
-		qla24xx_build_scsi_iocbs(sp, cmd_pkt, tot_dsds);
-
-		/* Set total data segment count. */
-		cmd_pkt->entry_count = (uint8_t)req_cnt;
-		/* Specify response queue number where
-		 * completion should happen.
-		 */
-		cmd_pkt->entry_status = (uint8_t) rsp->id;
-
-	}
-	/* Build command packet. */
-	req->current_outstanding_cmd = handle;
-	req->outstanding_cmds[handle] = sp;
-	sp->handle = handle;
-	sp->cmd->host_scribble = (unsigned char *)(unsigned long)handle;
-	req->cnt -= req_cnt;
-	wmb();
-
-	/* Adjust ring index. */
-	req->ring_index++;
-	if (req->ring_index == req->length) {
-		req->ring_index = 0;
-		req->ring_ptr = req->ring;
-	} else
-		req->ring_ptr++;
-
-	sp->flags |= SRB_DMA_VALID;
-
-	/* Set chip new ring index. */
-	/* write, read and verify logic */
-	dbval = dbval | (req->id << 8) | (req->ring_index << 16);
-	if (ql2xdbwr)
-		qla82xx_wr_32(ha, ha->nxdb_wr_ptr, dbval);
-	else {
-		WRT_REG_DWORD(
-			(unsigned long __iomem *)ha->nxdb_wr_ptr,
-			dbval);
-		wmb();
-		while (RD_REG_DWORD(ha->nxdb_rd_ptr) != dbval) {
-			WRT_REG_DWORD(
-				(unsigned long __iomem *)ha->nxdb_wr_ptr,
-				dbval);
-			wmb();
-		}
-	}
-
-	/* Manage unprocessed RIO/ZIO commands in response queue. */
-	if (vha->flags.process_response_queue &&
-	    rsp->ring_ptr->signature != RESPONSE_PROCESSED)
-		qla24xx_process_response_queue(vha, rsp);
-
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
-	return QLA_SUCCESS;
-
-queuing_error_fcp_cmnd:
-	dma_pool_free(ha->fcp_cmnd_dma_pool, ctx->fcp_cmnd, ctx->fcp_cmnd_dma);
-queuing_error:
-	if (tot_dsds)
-		scsi_dma_unmap(cmd);
-
-	if (sp->ctx) {
-		mempool_free(sp->ctx, ha->ctx_mempool);
-		sp->ctx = NULL;
-	}
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
-
-	return QLA_FUNCTION_FAILED;
-}
-
 static uint32_t *
 qla82xx_read_flash_data(scsi_qla_host_t *vha, uint32_t *dwptr, uint32_t faddr,
 	uint32_t length)
@@ -3272,9 +2790,9 @@ qla82xx_write_optrom_data(struct scsi_qla_host *vha, uint8_t *buf,
 }
 
 void
-qla82xx_start_iocbs(srb_t *sp)
+qla82xx_start_iocbs(scsi_qla_host_t *vha)
 {
-	struct qla_hw_data *ha = sp->fcport->vha->hw;
+	struct qla_hw_data *ha = vha->hw;
 	struct req_que *req = ha->req_q_map[0];
 	struct device_reg_82xx __iomem *reg;
 	uint32_t dbval;
@@ -3659,11 +3177,10 @@ qla82xx_check_md_needed(scsi_qla_host_t *vha)
 				qla82xx_md_free(vha);
 				/* ALlocate MiniDump resources */
 				qla82xx_md_prep(vha);
-			} else
-				ql_log(ql_log_info, vha, 0xb02e,
-				    "Firmware dump available to retrieve\n",
-				    vha->host_no);
-		}
+			}
+		} else
+			ql_log(ql_log_info, vha, 0xb02e,
+			    "Firmware dump available to retrieve\n");
 	}
 	return rval;
 }
@@ -3758,7 +3275,6 @@ qla82xx_device_state_handler(scsi_qla_host_t *vha)
 
 		switch (dev_state) {
 		case QLA82XX_DEV_READY:
-			qla82xx_check_md_needed(vha);
 			ha->flags.isp82xx_reset_owner = 0;
 			goto exit;
 		case QLA82XX_DEV_COLD:
@@ -4067,7 +3583,7 @@ int qla2x00_wait_for_fcoe_ctx_reset(scsi_qla_host_t *vha)
 		}
 	}
 	ql_dbg(ql_dbg_p3p, vha, 0xb027,
-	    "%s status=%d.\n", status);
+	       "%s: status=%d.\n", __func__, status);
 
 	return status;
 }
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index f9e5b85e84d8..4ed1e4a96b95 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -83,6 +83,9 @@ MODULE_PARM_DESC(ql2xextended_error_logging,
 		"\t\t0x00080000 - P3P Specific.  0x00040000 - Virtual Port.\n"
 		"\t\t0x00020000 - Buffer Dump.   0x00010000 - Misc.\n"
 		"\t\t0x7fffffff - For enabling all logs, can be too many logs.\n"
+		"\t\t0x1e400000 - Preferred value for capturing essential "
+		"debug information (equivalent to old "
+		"ql2xextended_error_logging=1).\n"
 		"\t\tDo LOGICAL OR of the value to enable more than one level");
 
 int ql2xshiftctondsd = 6;
@@ -199,7 +202,7 @@ int ql2xmdcapmask = 0x1F;
 module_param(ql2xmdcapmask, int, S_IRUGO);
 MODULE_PARM_DESC(ql2xmdcapmask,
 		"Set the Minidump driver capture mask level. "
-		"Default is 0x7F - Can be set to 0x3, 0x7, 0xF, 0x1F, 0x7F.");
+		"Default is 0x1F - Can be set to 0x3, 0x7, 0xF, 0x1F, 0x7F.");
 
 int ql2xmdenable = 1;
 module_param(ql2xmdenable, int, S_IRUGO);
@@ -847,14 +850,10 @@ qla2xxx_eh_abort(struct scsi_cmnd *cmd)
 	int wait = 0;
 	struct qla_hw_data *ha = vha->hw;
 
-	ql_dbg(ql_dbg_taskm, vha, 0x8000,
-	    "Entered %s for cmd=%p.\n", __func__, cmd);
 	if (!CMD_SP(cmd))
 		return SUCCESS;
 
 	ret = fc_block_scsi_eh(cmd);
-	ql_dbg(ql_dbg_taskm, vha, 0x8001,
-	    "Return value of fc_block_scsi_eh=%d.\n", ret);
 	if (ret != 0)
 		return ret;
 	ret = SUCCESS;
@@ -870,7 +869,8 @@ qla2xxx_eh_abort(struct scsi_cmnd *cmd)
 	}
 
 	ql_dbg(ql_dbg_taskm, vha, 0x8002,
-	    "Aborting sp=%p cmd=%p from RISC ", sp, cmd);
+	    "Aborting from RISC nexus=%ld:%d:%d sp=%p cmd=%p\n",
+	    vha->host_no, id, lun, sp, cmd);
 
 	/* Get a reference to the sp and drop the lock.*/
 	sp_get(sp);
@@ -878,10 +878,10 @@ qla2xxx_eh_abort(struct scsi_cmnd *cmd)
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 	if (ha->isp_ops->abort_command(sp)) {
 		ql_dbg(ql_dbg_taskm, vha, 0x8003,
-		    "Abort command mbx failed for cmd=%p.\n", cmd);
+		    "Abort command mbx failed cmd=%p.\n", cmd);
 	} else {
 		ql_dbg(ql_dbg_taskm, vha, 0x8004,
-		    "Abort command mbx success.\n");
+		    "Abort command mbx success cmd=%p.\n", cmd);
 		wait = 1;
 	}
 
@@ -897,13 +897,14 @@ qla2xxx_eh_abort(struct scsi_cmnd *cmd)
 	if (wait) {
 		if (qla2x00_eh_wait_on_command(cmd) != QLA_SUCCESS) {
 			ql_log(ql_log_warn, vha, 0x8006,
-			    "Abort handler timed out for cmd=%p.\n", cmd);
+			    "Abort handler timed out cmd=%p.\n", cmd);
 			ret = FAILED;
 		}
 	}
 
 	ql_log(ql_log_info, vha, 0x801c,
-	    "Abort command issued --  %d %x.\n", wait, ret);
+	    "Abort command issued nexus=%ld:%d:%d --  %d %x.\n",
+	    vha->host_no, id, lun, wait, ret);
 
 	return ret;
 }
@@ -972,19 +973,15 @@ __qla2xxx_eh_generic_reset(char *name, enum nexus_wait_type type,
 	int err;
 
 	if (!fcport) {
-		ql_log(ql_log_warn, vha, 0x8007,
-		    "fcport is NULL.\n");
 		return FAILED;
 	}
 
 	err = fc_block_scsi_eh(cmd);
-	ql_dbg(ql_dbg_taskm, vha, 0x8008,
-	    "fc_block_scsi_eh ret=%d.\n", err);
 	if (err != 0)
 		return err;
 
 	ql_log(ql_log_info, vha, 0x8009,
-	    "%s RESET ISSUED for id %d lun %d cmd=%p.\n", name,
+	    "%s RESET ISSUED nexus=%ld:%d:%d cmd=%p.\n", name, vha->host_no,
 	    cmd->device->id, cmd->device->lun, cmd);
 
 	err = 0;
@@ -1009,15 +1006,16 @@ __qla2xxx_eh_generic_reset(char *name, enum nexus_wait_type type,
 	}
 
 	ql_log(ql_log_info, vha, 0x800e,
-	    "%s RESET SUCCEEDED for id %d lun %d cmd=%p.\n", name,
-	    cmd->device->id, cmd->device->lun, cmd);
+	    "%s RESET SUCCEEDED nexus:%ld:%d:%d cmd=%p.\n", name,
+	    vha->host_no, cmd->device->id, cmd->device->lun, cmd);
 
 	return SUCCESS;
 
 eh_reset_failed:
 	ql_log(ql_log_info, vha, 0x800f,
-	    "%s RESET FAILED: %s for id %d lun %d cmd=%p.\n", name,
-	    reset_errors[err], cmd->device->id, cmd->device->lun);
+	    "%s RESET FAILED: %s nexus=%ld:%d:%d cmd=%p.\n", name,
+	    reset_errors[err], vha->host_no, cmd->device->id, cmd->device->lun,
+	    cmd);
 	return FAILED;
 }
 
@@ -1068,20 +1066,16 @@ qla2xxx_eh_bus_reset(struct scsi_cmnd *cmd)
 	lun = cmd->device->lun;
 
 	if (!fcport) {
-		ql_log(ql_log_warn, vha, 0x8010,
-		    "fcport is NULL.\n");
 		return ret;
 	}
 
 	ret = fc_block_scsi_eh(cmd);
-	ql_dbg(ql_dbg_taskm, vha, 0x8011,
-	    "fc_block_scsi_eh ret=%d.\n", ret);
 	if (ret != 0)
 		return ret;
 	ret = FAILED;
 
 	ql_log(ql_log_info, vha, 0x8012,
-	    "BUS RESET ISSUED for id %d lun %d.\n", id, lun);
+	    "BUS RESET ISSUED nexus=%ld:%d%d.\n", vha->host_no, id, lun);
 
 	if (qla2x00_wait_for_hba_online(vha) != QLA_SUCCESS) {
 		ql_log(ql_log_fatal, vha, 0x8013,
@@ -1105,7 +1099,8 @@ qla2xxx_eh_bus_reset(struct scsi_cmnd *cmd)
 
 eh_bus_reset_done:
 	ql_log(ql_log_warn, vha, 0x802b,
-	    "BUS RESET %s.\n", (ret == FAILED) ? "FAILED" : "SUCCEDED");
+	    "BUS RESET %s nexus=%ld:%d:%d.\n",
+	    (ret == FAILED) ? "FAILED" : "SUCCEDED", vha->host_no, id, lun);
 
 	return ret;
 }
@@ -1139,20 +1134,16 @@ qla2xxx_eh_host_reset(struct scsi_cmnd *cmd)
 	lun = cmd->device->lun;
 
 	if (!fcport) {
-		ql_log(ql_log_warn, vha, 0x8016,
-		    "fcport is NULL.\n");
 		return ret;
 	}
 
 	ret = fc_block_scsi_eh(cmd);
-	ql_dbg(ql_dbg_taskm, vha, 0x8017,
-	    "fc_block_scsi_eh ret=%d.\n", ret);
 	if (ret != 0)
 		return ret;
 	ret = FAILED;
 
 	ql_log(ql_log_info, vha, 0x8018,
-	    "ADAPTER RESET ISSUED for id %d lun %d.\n", id, lun);
+	    "ADAPTER RESET ISSUED nexus=%ld:%d:%d.\n", vha->host_no, id, lun);
 
 	if (qla2x00_wait_for_reset_ready(vha) != QLA_SUCCESS)
 		goto eh_host_reset_lock;
@@ -1193,8 +1184,9 @@ qla2xxx_eh_host_reset(struct scsi_cmnd *cmd)
 		ret = SUCCESS;
 
 eh_host_reset_lock:
-	qla_printk(KERN_INFO, ha, "%s: reset %s.\n", __func__,
-	    (ret == FAILED) ? "failed" : "succeeded");
+	ql_log(ql_log_info, vha, 0x8017,
+	    "ADAPTER RESET %s nexus=%ld:%d:%d.\n",
+	    (ret == FAILED) ? "FAILED" : "SUCCEEDED", vha->host_no, id, lun);
 
 	return ret;
 }
@@ -1344,10 +1336,8 @@ static void qla2x00_handle_queue_full(struct scsi_device *sdev, int qdepth)
 		return;
 
 	ql_dbg(ql_dbg_io, fcport->vha, 0x3029,
-	    "Queue depth adjusted-down "
-	    "to %d for scsi(%ld:%d:%d:%d).\n",
-	    sdev->queue_depth, fcport->vha->host_no,
-	    sdev->channel, sdev->id, sdev->lun);
+	    "Queue depth adjusted-down to %d for nexus=%ld:%d:%d.\n",
+	    sdev->queue_depth, fcport->vha->host_no, sdev->id, sdev->lun);
 }
 
 static void qla2x00_adjust_sdev_qdepth_up(struct scsi_device *sdev, int qdepth)
@@ -1369,10 +1359,8 @@ static void qla2x00_adjust_sdev_qdepth_up(struct scsi_device *sdev, int qdepth)
 		scsi_adjust_queue_depth(sdev, MSG_SIMPLE_TAG, qdepth);
 
 	ql_dbg(ql_dbg_io, vha, 0x302a,
-	    "Queue depth adjusted-up to %d for "
-	    "scsi(%ld:%d:%d:%d).\n",
-	    sdev->queue_depth, fcport->vha->host_no,
-	    sdev->channel, sdev->id, sdev->lun);
+	    "Queue depth adjusted-up to %d for nexus=%ld:%d:%d.\n",
+	    sdev->queue_depth, fcport->vha->host_no, sdev->id, sdev->lun);
 }
 
 static int
@@ -1496,6 +1484,118 @@ qla24xx_disable_intrs(struct qla_hw_data *ha)
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 }
 
+static int
+qla2x00_iospace_config(struct qla_hw_data *ha)
+{
+	resource_size_t pio;
+	uint16_t msix;
+	int cpus;
+
+	if (IS_QLA82XX(ha))
+		return qla82xx_iospace_config(ha);
+
+	if (pci_request_selected_regions(ha->pdev, ha->bars,
+	    QLA2XXX_DRIVER_NAME)) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x0011,
+		    "Failed to reserve PIO/MMIO regions (%s), aborting.\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+	if (!(ha->bars & 1))
+		goto skip_pio;
+
+	/* We only need PIO for Flash operations on ISP2312 v2 chips. */
+	pio = pci_resource_start(ha->pdev, 0);
+	if (pci_resource_flags(ha->pdev, 0) & IORESOURCE_IO) {
+		if (pci_resource_len(ha->pdev, 0) < MIN_IOBASE_LEN) {
+			ql_log_pci(ql_log_warn, ha->pdev, 0x0012,
+			    "Invalid pci I/O region size (%s).\n",
+			    pci_name(ha->pdev));
+			pio = 0;
+		}
+	} else {
+		ql_log_pci(ql_log_warn, ha->pdev, 0x0013,
+		    "Region #0 no a PIO resource (%s).\n",
+		    pci_name(ha->pdev));
+		pio = 0;
+	}
+	ha->pio_address = pio;
+	ql_dbg_pci(ql_dbg_init, ha->pdev, 0x0014,
+	    "PIO address=%llu.\n",
+	    (unsigned long long)ha->pio_address);
+
+skip_pio:
+	/* Use MMIO operations for all accesses. */
+	if (!(pci_resource_flags(ha->pdev, 1) & IORESOURCE_MEM)) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x0015,
+		    "Region #1 not an MMIO resource (%s), aborting.\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+	if (pci_resource_len(ha->pdev, 1) < MIN_IOBASE_LEN) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x0016,
+		    "Invalid PCI mem region size (%s), aborting.\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	ha->iobase = ioremap(pci_resource_start(ha->pdev, 1), MIN_IOBASE_LEN);
+	if (!ha->iobase) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0x0017,
+		    "Cannot remap MMIO (%s), aborting.\n",
+		    pci_name(ha->pdev));
+		goto iospace_error_exit;
+	}
+
+	/* Determine queue resources */
+	ha->max_req_queues = ha->max_rsp_queues = 1;
+	if ((ql2xmaxqueues <= 1 && !ql2xmultique_tag) ||
+		(ql2xmaxqueues > 1 && ql2xmultique_tag) ||
+		(!IS_QLA25XX(ha) && !IS_QLA81XX(ha)))
+		goto mqiobase_exit;
+
+	ha->mqiobase = ioremap(pci_resource_start(ha->pdev, 3),
+			pci_resource_len(ha->pdev, 3));
+	if (ha->mqiobase) {
+		ql_dbg_pci(ql_dbg_init, ha->pdev, 0x0018,
+		    "MQIO Base=%p.\n", ha->mqiobase);
+		/* Read MSIX vector size of the board */
+		pci_read_config_word(ha->pdev, QLA_PCI_MSIX_CONTROL, &msix);
+		ha->msix_count = msix;
+		/* Max queues are bounded by available msix vectors */
+		/* queue 0 uses two msix vectors */
+		if (ql2xmultique_tag) {
+			cpus = num_online_cpus();
+			ha->max_rsp_queues = (ha->msix_count - 1 > cpus) ?
+				(cpus + 1) : (ha->msix_count - 1);
+			ha->max_req_queues = 2;
+		} else if (ql2xmaxqueues > 1) {
+			ha->max_req_queues = ql2xmaxqueues > QLA_MQ_SIZE ?
+			    QLA_MQ_SIZE : ql2xmaxqueues;
+			ql_dbg_pci(ql_dbg_multiq, ha->pdev, 0xc008,
+			    "QoS mode set, max no of request queues:%d.\n",
+			    ha->max_req_queues);
+			ql_dbg_pci(ql_dbg_init, ha->pdev, 0x0019,
+			    "QoS mode set, max no of request queues:%d.\n",
+			    ha->max_req_queues);
+		}
+		ql_log_pci(ql_log_info, ha->pdev, 0x001a,
+		    "MSI-X vector count: %d.\n", msix);
+	} else
+		ql_log_pci(ql_log_info, ha->pdev, 0x001b,
+		    "BAR 3 not enabled.\n");
+
+mqiobase_exit:
+	ha->msix_count = ha->max_rsp_queues + 1;
+	ql_dbg_pci(ql_dbg_init, ha->pdev, 0x001c,
+	    "MSIX Count:%d.\n", ha->msix_count);
+	return (0);
+
+iospace_error_exit:
+	return (-ENOMEM);
+}
+
+
 static struct isp_operations qla2100_isp_ops = {
 	.pci_config		= qla2100_pci_config,
 	.reset_chip		= qla2x00_reset_chip,
@@ -1530,6 +1630,7 @@ static struct isp_operations qla2100_isp_ops = {
 	.get_flash_version	= qla2x00_get_flash_version,
 	.start_scsi		= qla2x00_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
+	.iospace_config     	= qla2x00_iospace_config,
 };
 
 static struct isp_operations qla2300_isp_ops = {
@@ -1566,6 +1667,7 @@ static struct isp_operations qla2300_isp_ops = {
 	.get_flash_version	= qla2x00_get_flash_version,
 	.start_scsi		= qla2x00_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
+	.iospace_config     	= qla2x00_iospace_config,
 };
 
 static struct isp_operations qla24xx_isp_ops = {
@@ -1602,6 +1704,7 @@ static struct isp_operations qla24xx_isp_ops = {
 	.get_flash_version	= qla24xx_get_flash_version,
 	.start_scsi		= qla24xx_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
+	.iospace_config     	= qla2x00_iospace_config,
 };
 
 static struct isp_operations qla25xx_isp_ops = {
@@ -1638,6 +1741,7 @@ static struct isp_operations qla25xx_isp_ops = {
 	.get_flash_version	= qla24xx_get_flash_version,
 	.start_scsi		= qla24xx_dif_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
+	.iospace_config     	= qla2x00_iospace_config,
 };
 
 static struct isp_operations qla81xx_isp_ops = {
@@ -1674,6 +1778,7 @@ static struct isp_operations qla81xx_isp_ops = {
 	.get_flash_version	= qla24xx_get_flash_version,
 	.start_scsi		= qla24xx_dif_start_scsi,
 	.abort_isp		= qla2x00_abort_isp,
+	.iospace_config     	= qla2x00_iospace_config,
 };
 
 static struct isp_operations qla82xx_isp_ops = {
@@ -1710,6 +1815,7 @@ static struct isp_operations qla82xx_isp_ops = {
 	.get_flash_version	= qla24xx_get_flash_version,
 	.start_scsi             = qla82xx_start_scsi,
 	.abort_isp		= qla82xx_abort_isp,
+	.iospace_config     	= qla82xx_iospace_config,
 };
 
 static inline void
@@ -1819,121 +1925,10 @@ qla2x00_set_isp_flags(struct qla_hw_data *ha)
 	else
 		ha->flags.port0 = 0;
 	ql_dbg_pci(ql_dbg_init, ha->pdev, 0x000b,
-	    "device_type=0x%x port=%d fw_srisc_address=%p.\n",
+	    "device_type=0x%x port=%d fw_srisc_address=0x%x.\n",
 	    ha->device_type, ha->flags.port0, ha->fw_srisc_address);
 }
 
-static int
-qla2x00_iospace_config(struct qla_hw_data *ha)
-{
-	resource_size_t pio;
-	uint16_t msix;
-	int cpus;
-
-	if (IS_QLA82XX(ha))
-		return qla82xx_iospace_config(ha);
-
-	if (pci_request_selected_regions(ha->pdev, ha->bars,
-	    QLA2XXX_DRIVER_NAME)) {
-		ql_log_pci(ql_log_fatal, ha->pdev, 0x0011,
-		    "Failed to reserve PIO/MMIO regions (%s), aborting.\n",
-		    pci_name(ha->pdev));
-		goto iospace_error_exit;
-	}
-	if (!(ha->bars & 1))
-		goto skip_pio;
-
-	/* We only need PIO for Flash operations on ISP2312 v2 chips. */
-	pio = pci_resource_start(ha->pdev, 0);
-	if (pci_resource_flags(ha->pdev, 0) & IORESOURCE_IO) {
-		if (pci_resource_len(ha->pdev, 0) < MIN_IOBASE_LEN) {
-			ql_log_pci(ql_log_warn, ha->pdev, 0x0012,
-			    "Invalid pci I/O region size (%s).\n",
-			    pci_name(ha->pdev));
-			pio = 0;
-		}
-	} else {
-		ql_log_pci(ql_log_warn, ha->pdev, 0x0013,
-		    "Region #0 no a PIO resource (%s).\n",
-		    pci_name(ha->pdev));
-		pio = 0;
-	}
-	ha->pio_address = pio;
-	ql_dbg_pci(ql_dbg_init, ha->pdev, 0x0014,
-	    "PIO address=%p.\n",
-	    ha->pio_address);
-
-skip_pio:
-	/* Use MMIO operations for all accesses. */
-	if (!(pci_resource_flags(ha->pdev, 1) & IORESOURCE_MEM)) {
-		ql_log_pci(ql_log_fatal, ha->pdev, 0x0015,
-		    "Region #1 not an MMIO resource (%s), aborting.\n",
-		    pci_name(ha->pdev));
-		goto iospace_error_exit;
-	}
-	if (pci_resource_len(ha->pdev, 1) < MIN_IOBASE_LEN) {
-		ql_log_pci(ql_log_fatal, ha->pdev, 0x0016,
-		    "Invalid PCI mem region size (%s), aborting.\n",
-		    pci_name(ha->pdev));
-		goto iospace_error_exit;
-	}
-
-	ha->iobase = ioremap(pci_resource_start(ha->pdev, 1), MIN_IOBASE_LEN);
-	if (!ha->iobase) {
-		ql_log_pci(ql_log_fatal, ha->pdev, 0x0017,
-		    "Cannot remap MMIO (%s), aborting.\n",
-		    pci_name(ha->pdev));
-		goto iospace_error_exit;
-	}
-
-	/* Determine queue resources */
-	ha->max_req_queues = ha->max_rsp_queues = 1;
-	if ((ql2xmaxqueues <= 1 && !ql2xmultique_tag) ||
-		(ql2xmaxqueues > 1 && ql2xmultique_tag) ||
-		(!IS_QLA25XX(ha) && !IS_QLA81XX(ha)))
-		goto mqiobase_exit;
-
-	ha->mqiobase = ioremap(pci_resource_start(ha->pdev, 3),
-			pci_resource_len(ha->pdev, 3));
-	if (ha->mqiobase) {
-		ql_dbg_pci(ql_dbg_init, ha->pdev, 0x0018,
-		    "MQIO Base=%p.\n", ha->mqiobase);
-		/* Read MSIX vector size of the board */
-		pci_read_config_word(ha->pdev, QLA_PCI_MSIX_CONTROL, &msix);
-		ha->msix_count = msix;
-		/* Max queues are bounded by available msix vectors */
-		/* queue 0 uses two msix vectors */
-		if (ql2xmultique_tag) {
-			cpus = num_online_cpus();
-			ha->max_rsp_queues = (ha->msix_count - 1 > cpus) ?
-				(cpus + 1) : (ha->msix_count - 1);
-			ha->max_req_queues = 2;
-		} else if (ql2xmaxqueues > 1) {
-			ha->max_req_queues = ql2xmaxqueues > QLA_MQ_SIZE ?
-			    QLA_MQ_SIZE : ql2xmaxqueues;
-			ql_dbg_pci(ql_dbg_multiq, ha->pdev, 0xc008,
-			    "QoS mode set, max no of request queues:%d.\n",
-			    ha->max_req_queues);
-			ql_dbg_pci(ql_dbg_init, ha->pdev, 0x0019,
-			    "QoS mode set, max no of request queues:%d.\n",
-			    ha->max_req_queues);
-		}
-		ql_log_pci(ql_log_info, ha->pdev, 0x001a,
-		    "MSI-X vector count: %d.\n", msix);
-	} else
-		ql_log_pci(ql_log_info, ha->pdev, 0x001b,
-		    "BAR 3 not enabled.\n");
-
-mqiobase_exit:
-	ha->msix_count = ha->max_rsp_queues + 1;
-	ql_dbg_pci(ql_dbg_init, ha->pdev, 0x001c,
-	    "MSIX Count:%d.\n", ha->msix_count);
-	return (0);
-
-iospace_error_exit:
-	return (-ENOMEM);
-}
-
 static void
 qla2xxx_scan_start(struct Scsi_Host *shost)
 {
@@ -2032,14 +2027,6 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		pdev->needs_freset = 1;
 	}
 
-	/* Configure PCI I/O space */
-	ret = qla2x00_iospace_config(ha);
-	if (ret)
-		goto probe_hw_failed;
-
-	ql_log_pci(ql_log_info, pdev, 0x001d,
-	    "Found an ISP%04X irq %d iobase 0x%p.\n",
-	    pdev->device, pdev->irq, ha->iobase);
 	ha->prev_topology = 0;
 	ha->init_cb_size = sizeof(init_cb_t);
 	ha->link_data_rate = PORT_SPEED_UNKNOWN;
@@ -2152,6 +2139,15 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	    "flash_data_off=%d, nvram_conf_off=%d, nvram_data_off=%d.\n",
 	    ha->isp_ops, ha->flash_conf_off, ha->flash_data_off,
 	    ha->nvram_conf_off, ha->nvram_data_off);
+
+	/* Configure PCI I/O space */
+	ret = ha->isp_ops->iospace_config(ha);
+	if (ret)
+		goto probe_hw_failed;
+
+	ql_log_pci(ql_log_info, pdev, 0x001d,
+	    "Found an ISP%04X irq %d iobase 0x%p.\n",
+	    pdev->device, pdev->irq, ha->iobase);
 	mutex_init(&ha->vport_lock);
 	init_completion(&ha->mbx_cmd_comp);
 	complete(&ha->mbx_cmd_comp);
@@ -2227,7 +2223,7 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	ql_dbg(ql_dbg_init, base_vha, 0x0033,
 	    "max_id=%d this_id=%d "
 	    "cmd_per_len=%d unique_id=%d max_cmd_len=%d max_channel=%d "
-	    "max_lun=%d transportt=%p, vendor_id=%d.\n", host->max_id,
+	    "max_lun=%d transportt=%p, vendor_id=%llu.\n", host->max_id,
 	    host->this_id, host->cmd_per_lun, host->unique_id,
 	    host->max_cmd_len, host->max_channel, host->max_lun,
 	    host->transportt, sht->vendor_id);
@@ -2382,9 +2378,6 @@ skip_dpc:
 
 	qla2x00_dfs_setup(base_vha);
 
-	ql_log(ql_log_info, base_vha, 0x00fa,
-	    "QLogic Fibre Channed HBA Driver: %s.\n",
-	    qla2x00_version_str);
 	ql_log(ql_log_info, base_vha, 0x00fb,
 	    "QLogic %s - %s.\n",
 	    ha->model_number, ha->model_desc ? ha->model_desc : "");
@@ -2833,7 +2826,7 @@ qla2x00_mem_alloc(struct qla_hw_data *ha, uint16_t req_len, uint16_t rsp_len,
 		if (!ha->sns_cmd)
 			goto fail_dma_pool;
 		ql_dbg_pci(ql_dbg_init, ha->pdev, 0x0026,
-		    "sns_cmd.\n", ha->sns_cmd);
+		    "sns_cmd: %p.\n", ha->sns_cmd);
 	} else {
 	/* Get consistent memory allocated for MS IOCB */
 		ha->ms_iocb = dma_pool_alloc(ha->s_dma_pool, GFP_KERNEL,
@@ -3460,27 +3453,21 @@ qla2x00_do_dpc(void *data)
 		schedule();
 		__set_current_state(TASK_RUNNING);
 
-		ql_dbg(ql_dbg_dpc, base_vha, 0x4001,
-		    "DPC handler waking up.\n");
-		ql_dbg(ql_dbg_dpc, base_vha, 0x4002,
-		    "dpc_flags=0x%lx.\n", base_vha->dpc_flags);
-
-		/* Initialization not yet finished. Don't do anything yet. */
-		if (!base_vha->flags.init_done)
-			continue;
+		if (!base_vha->flags.init_done || ha->flags.mbox_busy)
+			goto end_loop;
 
 		if (ha->flags.eeh_busy) {
 			ql_dbg(ql_dbg_dpc, base_vha, 0x4003,
 			    "eeh_busy=%d.\n", ha->flags.eeh_busy);
-			continue;
+			goto end_loop;
 		}
 
 		ha->dpc_active = 1;
 
-		if (ha->flags.mbox_busy) {
-			ha->dpc_active = 0;
-			continue;
-		}
+		ql_dbg(ql_dbg_dpc, base_vha, 0x4001,
+		    "DPC handler waking up.\n");
+		ql_dbg(ql_dbg_dpc, base_vha, 0x4002,
+		    "dpc_flags=0x%lx.\n", base_vha->dpc_flags);
 
 		qla2x00_do_work(base_vha);
 
@@ -3622,6 +3609,7 @@ qla2x00_do_dpc(void *data)
 		qla2x00_do_dpc_all_vps(base_vha);
 
 		ha->dpc_active = 0;
+end_loop:
 		set_current_state(TASK_INTERRUPTIBLE);
 	} /* End of while(1) */
 	__set_current_state(TASK_RUNNING);
@@ -3705,16 +3693,6 @@ qla2x00_sp_free_dma(srb_t *sp)
 		sp->flags &= ~SRB_CRC_CTX_DMA_VALID;
 	}
 
-	CMD_SP(cmd) = NULL;
-}
-
-static void
-qla2x00_sp_final_compl(struct qla_hw_data *ha, srb_t *sp)
-{
-	struct scsi_cmnd *cmd = sp->cmd;
-
-	qla2x00_sp_free_dma(sp);
-
 	if (sp->flags & SRB_FCP_CMND_DMA_VALID) {
 		struct ct6_dsd *ctx = sp->ctx;
 		dma_pool_free(ha->fcp_cmnd_dma_pool, ctx->fcp_cmnd,
@@ -3726,6 +3704,15 @@ qla2x00_sp_final_compl(struct qla_hw_data *ha, srb_t *sp)
 		sp->ctx = NULL;
 	}
 
+	CMD_SP(cmd) = NULL;
+}
+
+static void
+qla2x00_sp_final_compl(struct qla_hw_data *ha, srb_t *sp)
+{
+	struct scsi_cmnd *cmd = sp->cmd;
+
+	qla2x00_sp_free_dma(sp);
 	mempool_free(sp, ha->srb_mempool);
 	cmd->scsi_done(cmd);
 }
diff --git a/drivers/scsi/qla2xxx/qla_sup.c b/drivers/scsi/qla2xxx/qla_sup.c
index eff13563c82d..16bc72844a97 100644
--- a/drivers/scsi/qla2xxx/qla_sup.c
+++ b/drivers/scsi/qla2xxx/qla_sup.c
@@ -904,8 +904,9 @@ no_flash_data:
 	}
 done:
 	ql_dbg(ql_dbg_init, vha, 0x004d,
-	    "FDT[%x]: (0x%x/0x%x) erase=0x%x "
-	    "pr=%x upro=%x wrtd=0x%x blk=0x%x.\n", loc, mid, fid,
+	    "FDT[%s]: (0x%x/0x%x) erase=0x%x "
+	    "pr=%x wrtd=0x%x blk=0x%x.\n",
+	    loc, mid, fid,
 	    ha->fdt_erase_cmd, ha->fdt_protect_sec_cmd,
 	    ha->fdt_wrt_disable, ha->fdt_block_size);
 
diff --git a/drivers/scsi/qla4xxx/ql4_dbg.c b/drivers/scsi/qla4xxx/ql4_dbg.c
index af62c3cf8752..8d58ae274829 100644
--- a/drivers/scsi/qla4xxx/ql4_dbg.c
+++ b/drivers/scsi/qla4xxx/ql4_dbg.c
@@ -20,12 +20,12 @@ void qla4xxx_dump_buffer(void *b, uint32_t size)
 	printk("------------------------------------------------------------"
 	       "--\n");
 	for (cnt = 0; cnt < size; c++) {
-		printk(KERN_INFO "%02x", *c);
+		printk("%02x", *c);
 		if (!(++cnt % 16))
-			printk(KERN_INFO "\n");
+			printk("\n");
 
 		else
-			printk(KERN_INFO "  ");
+			printk("  ");
 	}
 	printk(KERN_INFO "\n");
 }
diff --git a/drivers/scsi/qla4xxx/ql4_def.h b/drivers/scsi/qla4xxx/ql4_def.h
index fd5edc6e166d..22a3ff02e48a 100644
--- a/drivers/scsi/qla4xxx/ql4_def.h
+++ b/drivers/scsi/qla4xxx/ql4_def.h
@@ -177,6 +177,7 @@
 #define LOGIN_TOV			12
 
 #define MAX_RESET_HA_RETRIES		2
+#define FW_ALIVE_WAIT_TOV		3
 
 #define CMD_SP(Cmnd)			((Cmnd)->SCp.ptr)
 
diff --git a/drivers/scsi/qla4xxx/ql4_isr.c b/drivers/scsi/qla4xxx/ql4_isr.c
index 827e93078b94..95828862eea0 100644
--- a/drivers/scsi/qla4xxx/ql4_isr.c
+++ b/drivers/scsi/qla4xxx/ql4_isr.c
@@ -123,13 +123,13 @@ static void qla4xxx_status_entry(struct scsi_qla_host *ha,
 
 	srb = qla4xxx_del_from_active_array(ha, le32_to_cpu(sts_entry->handle));
 	if (!srb) {
-		DEBUG2(printk(KERN_WARNING "scsi%ld: %s: Status Entry invalid "
-			      "handle 0x%x, sp=%p. This cmd may have already "
-			      "been completed.\n", ha->host_no, __func__,
-			      le32_to_cpu(sts_entry->handle), srb));
-		ql4_printk(KERN_WARNING, ha, "%s invalid status entry:"
-		    " handle=0x%0x\n", __func__, sts_entry->handle);
-		set_bit(DPC_RESET_HA, &ha->dpc_flags);
+		ql4_printk(KERN_WARNING, ha, "%s invalid status entry: "
+			   "handle=0x%0x, srb=%p\n", __func__,
+			   sts_entry->handle, srb);
+		if (is_qla8022(ha))
+			set_bit(DPC_RESET_HA_FW_CONTEXT, &ha->dpc_flags);
+		else
+			set_bit(DPC_RESET_HA, &ha->dpc_flags);
 		return;
 	}
 
@@ -563,7 +563,11 @@ static void qla4xxx_isr_decode_mailbox(struct scsi_qla_host * ha,
 		case MBOX_ASTS_DHCP_LEASE_EXPIRED:
 			DEBUG2(printk("scsi%ld: AEN %04x, ERROR Status, "
 				      "Reset HA\n", ha->host_no, mbox_status));
-			set_bit(DPC_RESET_HA, &ha->dpc_flags);
+			if (is_qla8022(ha))
+				set_bit(DPC_RESET_HA_FW_CONTEXT,
+					&ha->dpc_flags);
+			else
+				set_bit(DPC_RESET_HA, &ha->dpc_flags);
 			break;
 
 		case MBOX_ASTS_LINK_UP:
@@ -617,9 +621,13 @@ static void qla4xxx_isr_decode_mailbox(struct scsi_qla_host * ha,
 			    (mbox_sts[2] == ACB_STATE_ACQUIRING)))
 				set_bit(DPC_GET_DHCP_IP_ADDR, &ha->dpc_flags);
 			else if ((mbox_sts[3] == ACB_STATE_ACQUIRING) &&
-			    (mbox_sts[2] == ACB_STATE_VALID))
-				set_bit(DPC_RESET_HA, &ha->dpc_flags);
-			else if ((mbox_sts[3] == ACB_STATE_UNCONFIGURED))
+				 (mbox_sts[2] == ACB_STATE_VALID)) {
+				if (is_qla8022(ha))
+					set_bit(DPC_RESET_HA_FW_CONTEXT,
+						&ha->dpc_flags);
+				else
+					set_bit(DPC_RESET_HA, &ha->dpc_flags);
+			} else if ((mbox_sts[3] == ACB_STATE_UNCONFIGURED))
 				complete(&ha->disable_acb_comp);
 			break;
 
diff --git a/drivers/scsi/qla4xxx/ql4_nx.c b/drivers/scsi/qla4xxx/ql4_nx.c
index f484ff438199..8d6bc1b2ff17 100644
--- a/drivers/scsi/qla4xxx/ql4_nx.c
+++ b/drivers/scsi/qla4xxx/ql4_nx.c
@@ -1792,8 +1792,11 @@ int qla4_8xxx_device_state_handler(struct scsi_qla_host *ha)
 	int rval = QLA_SUCCESS;
 	unsigned long dev_init_timeout;
 
-	if (!test_bit(AF_INIT_DONE, &ha->flags))
+	if (!test_bit(AF_INIT_DONE, &ha->flags)) {
+		qla4_8xxx_idc_lock(ha);
 		qla4_8xxx_set_drv_active(ha);
+		qla4_8xxx_idc_unlock(ha);
+	}
 
 	dev_state = qla4_8xxx_rd_32(ha, QLA82XX_CRB_DEV_STATE);
 	ql4_printk(KERN_INFO, ha, "1:Device state is 0x%x = %s\n", dev_state,
@@ -1802,8 +1805,8 @@ int qla4_8xxx_device_state_handler(struct scsi_qla_host *ha)
 	/* wait for 30 seconds for device to go ready */
 	dev_init_timeout = jiffies + (ha->nx_dev_init_timeout * HZ);
 
+	qla4_8xxx_idc_lock(ha);
 	while (1) {
-		qla4_8xxx_idc_lock(ha);
 
 		if (time_after_eq(jiffies, dev_init_timeout)) {
 			ql4_printk(KERN_WARNING, ha, "Device init failed!\n");
@@ -1819,15 +1822,14 @@ int qla4_8xxx_device_state_handler(struct scsi_qla_host *ha)
 		/* NOTE: Make sure idc unlocked upon exit of switch statement */
 		switch (dev_state) {
 		case QLA82XX_DEV_READY:
-			qla4_8xxx_idc_unlock(ha);
 			goto exit;
 		case QLA82XX_DEV_COLD:
 			rval = qla4_8xxx_device_bootstrap(ha);
-			qla4_8xxx_idc_unlock(ha);
 			goto exit;
 		case QLA82XX_DEV_INITIALIZING:
 			qla4_8xxx_idc_unlock(ha);
 			msleep(1000);
+			qla4_8xxx_idc_lock(ha);
 			break;
 		case QLA82XX_DEV_NEED_RESET:
 			if (!ql4xdontresethba) {
@@ -1836,32 +1838,37 @@ int qla4_8xxx_device_state_handler(struct scsi_qla_host *ha)
 				 * reset handler */
 				dev_init_timeout = jiffies +
 					(ha->nx_dev_init_timeout * HZ);
+			} else {
+				qla4_8xxx_idc_unlock(ha);
+				msleep(1000);
+				qla4_8xxx_idc_lock(ha);
 			}
-			qla4_8xxx_idc_unlock(ha);
 			break;
 		case QLA82XX_DEV_NEED_QUIESCENT:
-			qla4_8xxx_idc_unlock(ha);
 			/* idc locked/unlocked in handler */
 			qla4_8xxx_need_qsnt_handler(ha);
-			qla4_8xxx_idc_lock(ha);
-			/* fall thru needs idc_locked */
+			break;
 		case QLA82XX_DEV_QUIESCENT:
 			qla4_8xxx_idc_unlock(ha);
 			msleep(1000);
+			qla4_8xxx_idc_lock(ha);
 			break;
 		case QLA82XX_DEV_FAILED:
 			qla4_8xxx_idc_unlock(ha);
 			qla4xxx_dead_adapter_cleanup(ha);
 			rval = QLA_ERROR;
+			qla4_8xxx_idc_lock(ha);
 			goto exit;
 		default:
 			qla4_8xxx_idc_unlock(ha);
 			qla4xxx_dead_adapter_cleanup(ha);
 			rval = QLA_ERROR;
+			qla4_8xxx_idc_lock(ha);
 			goto exit;
 		}
 	}
 exit:
+	qla4_8xxx_idc_unlock(ha);
 	return rval;
 }
 
diff --git a/drivers/scsi/qla4xxx/ql4_os.c b/drivers/scsi/qla4xxx/ql4_os.c
index 78bf700b365f..ec393a00c038 100644
--- a/drivers/scsi/qla4xxx/ql4_os.c
+++ b/drivers/scsi/qla4xxx/ql4_os.c
@@ -935,7 +935,16 @@ qla4xxx_iface_set_param(struct Scsi_Host *shost, void *data, uint32_t len)
 		goto exit_init_fw_cb;
 	}
 
-	qla4xxx_disable_acb(ha);
+	rval = qla4xxx_disable_acb(ha);
+	if (rval != QLA_SUCCESS) {
+		ql4_printk(KERN_ERR, ha, "%s: disable acb mbx failed\n",
+			   __func__);
+		rval = -EIO;
+		goto exit_init_fw_cb;
+	}
+
+	wait_for_completion_timeout(&ha->disable_acb_comp,
+				    DISABLE_ACB_TOV * HZ);
 
 	qla4xxx_initcb_to_acb(init_fw_cb);
 
@@ -1966,9 +1975,10 @@ mem_alloc_error_exit:
  *
  * Context: Interrupt
  **/
-static void qla4_8xxx_check_fw_alive(struct scsi_qla_host *ha)
+static int qla4_8xxx_check_fw_alive(struct scsi_qla_host *ha)
 {
-	uint32_t fw_heartbeat_counter, halt_status;
+	uint32_t fw_heartbeat_counter;
+	int status = QLA_SUCCESS;
 
 	fw_heartbeat_counter = qla4_8xxx_rd_32(ha, QLA82XX_PEG_ALIVE_COUNTER);
 	/* If PEG_ALIVE_COUNTER is 0xffffffff, AER/EEH is in progress, ignore */
@@ -1976,7 +1986,7 @@ static void qla4_8xxx_check_fw_alive(struct scsi_qla_host *ha)
 		DEBUG2(printk(KERN_WARNING "scsi%ld: %s: Device in frozen "
 		    "state, QLA82XX_PEG_ALIVE_COUNTER is 0xffffffff\n",
 		    ha->host_no, __func__));
-		return;
+		return status;
 	}
 
 	if (ha->fw_heartbeat_counter == fw_heartbeat_counter) {
@@ -1984,8 +1994,6 @@ static void qla4_8xxx_check_fw_alive(struct scsi_qla_host *ha)
 		/* FW not alive after 2 seconds */
 		if (ha->seconds_since_last_heartbeat == 2) {
 			ha->seconds_since_last_heartbeat = 0;
-			halt_status = qla4_8xxx_rd_32(ha,
-						      QLA82XX_PEG_HALT_STATUS1);
 
 			ql4_printk(KERN_INFO, ha,
 				   "scsi(%ld): %s, Dumping hw/fw registers:\n "
@@ -1993,7 +2001,9 @@ static void qla4_8xxx_check_fw_alive(struct scsi_qla_host *ha)
 				   " 0x%x,\n PEG_NET_0_PC: 0x%x, PEG_NET_1_PC:"
 				   " 0x%x,\n PEG_NET_2_PC: 0x%x, PEG_NET_3_PC:"
 				   " 0x%x,\n PEG_NET_4_PC: 0x%x\n",
-				   ha->host_no, __func__, halt_status,
+				   ha->host_no, __func__,
+				   qla4_8xxx_rd_32(ha,
+						   QLA82XX_PEG_HALT_STATUS1),
 				   qla4_8xxx_rd_32(ha,
 						   QLA82XX_PEG_HALT_STATUS2),
 				   qla4_8xxx_rd_32(ha, QLA82XX_CRB_PEG_NET_0 +
@@ -2006,24 +2016,13 @@ static void qla4_8xxx_check_fw_alive(struct scsi_qla_host *ha)
 						   0x3c),
 				   qla4_8xxx_rd_32(ha, QLA82XX_CRB_PEG_NET_4 +
 						   0x3c));
-
-			/* Since we cannot change dev_state in interrupt
-			 * context, set appropriate DPC flag then wakeup
-			 * DPC */
-			if (halt_status & HALT_STATUS_UNRECOVERABLE)
-				set_bit(DPC_HA_UNRECOVERABLE, &ha->dpc_flags);
-			else {
-				printk("scsi%ld: %s: detect abort needed!\n",
-				    ha->host_no, __func__);
-				set_bit(DPC_RESET_HA, &ha->dpc_flags);
-			}
-			qla4xxx_wake_dpc(ha);
-			qla4xxx_mailbox_premature_completion(ha);
+			status = QLA_ERROR;
 		}
 	} else
 		ha->seconds_since_last_heartbeat = 0;
 
 	ha->fw_heartbeat_counter = fw_heartbeat_counter;
+	return status;
 }
 
 /**
@@ -2034,14 +2033,13 @@ static void qla4_8xxx_check_fw_alive(struct scsi_qla_host *ha)
  **/
 void qla4_8xxx_watchdog(struct scsi_qla_host *ha)
 {
-	uint32_t dev_state;
-
-	dev_state = qla4_8xxx_rd_32(ha, QLA82XX_CRB_DEV_STATE);
+	uint32_t dev_state, halt_status;
 
 	/* don't poll if reset is going on */
 	if (!(test_bit(DPC_RESET_ACTIVE, &ha->dpc_flags) ||
 	    test_bit(DPC_RESET_HA, &ha->dpc_flags) ||
 	    test_bit(DPC_RETRY_RESET_HA, &ha->dpc_flags))) {
+		dev_state = qla4_8xxx_rd_32(ha, QLA82XX_CRB_DEV_STATE);
 		if (dev_state == QLA82XX_DEV_NEED_RESET &&
 		    !test_bit(DPC_RESET_HA, &ha->dpc_flags)) {
 			if (!ql4xdontresethba) {
@@ -2049,7 +2047,6 @@ void qla4_8xxx_watchdog(struct scsi_qla_host *ha)
 				    "NEED RESET!\n", __func__);
 				set_bit(DPC_RESET_HA, &ha->dpc_flags);
 				qla4xxx_wake_dpc(ha);
-				qla4xxx_mailbox_premature_completion(ha);
 			}
 		} else if (dev_state == QLA82XX_DEV_NEED_QUIESCENT &&
 		    !test_bit(DPC_HA_NEED_QUIESCENT, &ha->dpc_flags)) {
@@ -2059,7 +2056,24 @@ void qla4_8xxx_watchdog(struct scsi_qla_host *ha)
 			qla4xxx_wake_dpc(ha);
 		} else  {
 			/* Check firmware health */
-			qla4_8xxx_check_fw_alive(ha);
+			if (qla4_8xxx_check_fw_alive(ha)) {
+				halt_status = qla4_8xxx_rd_32(ha,
+						QLA82XX_PEG_HALT_STATUS1);
+
+				/* Since we cannot change dev_state in interrupt
+				 * context, set appropriate DPC flag then wakeup
+				 * DPC */
+				if (halt_status & HALT_STATUS_UNRECOVERABLE)
+					set_bit(DPC_HA_UNRECOVERABLE,
+						&ha->dpc_flags);
+				else {
+					ql4_printk(KERN_INFO, ha, "%s: detect "
+						   "abort needed!\n", __func__);
+					set_bit(DPC_RESET_HA, &ha->dpc_flags);
+				}
+				qla4xxx_mailbox_premature_completion(ha);
+				qla4xxx_wake_dpc(ha);
+			}
 		}
 	}
 }
@@ -2414,6 +2428,8 @@ static int qla4xxx_recover_adapter(struct scsi_qla_host *ha)
 {
 	int status = QLA_ERROR;
 	uint8_t reset_chip = 0;
+	uint32_t dev_state;
+	unsigned long wait;
 
 	/* Stall incoming I/O until we are done */
 	scsi_block_requests(ha->host);
@@ -2464,8 +2480,29 @@ static int qla4xxx_recover_adapter(struct scsi_qla_host *ha)
 	 * or if stop_firmware fails for ISP-82xx.
 	 * This is the default case for ISP-4xxx */
 	if (!is_qla8022(ha) || reset_chip) {
+		if (!is_qla8022(ha))
+			goto chip_reset;
+
+		/* Check if 82XX firmware is alive or not
+		 * We may have arrived here from NEED_RESET
+		 * detection only */
+		if (test_bit(AF_FW_RECOVERY, &ha->flags))
+			goto chip_reset;
+
+		wait = jiffies + (FW_ALIVE_WAIT_TOV * HZ);
+		while (time_before(jiffies, wait)) {
+			if (qla4_8xxx_check_fw_alive(ha)) {
+				qla4xxx_mailbox_premature_completion(ha);
+				break;
+			}
+
+			set_current_state(TASK_UNINTERRUPTIBLE);
+			schedule_timeout(HZ);
+		}
+
 		if (!test_bit(AF_FW_RECOVERY, &ha->flags))
 			qla4xxx_cmd_wait(ha);
+chip_reset:
 		qla4xxx_process_aen(ha, FLUSH_DDB_CHANGED_AENS);
 		qla4xxx_abort_active_cmds(ha, DID_RESET << 16);
 		DEBUG2(ql4_printk(KERN_INFO, ha,
@@ -2501,6 +2538,25 @@ recover_ha_init_adapter:
 		 * Since we don't want to block the DPC for too long
 		 * with multiple resets in the same thread,
 		 * utilize DPC to retry */
+		if (is_qla8022(ha)) {
+			qla4_8xxx_idc_lock(ha);
+			dev_state = qla4_8xxx_rd_32(ha, QLA82XX_CRB_DEV_STATE);
+			qla4_8xxx_idc_unlock(ha);
+			if (dev_state == QLA82XX_DEV_FAILED) {
+				ql4_printk(KERN_INFO, ha, "%s: don't retry "
+					   "recover adapter. H/W is in Failed "
+					   "state\n", __func__);
+				qla4xxx_dead_adapter_cleanup(ha);
+				clear_bit(DPC_RETRY_RESET_HA, &ha->dpc_flags);
+				clear_bit(DPC_RESET_HA, &ha->dpc_flags);
+				clear_bit(DPC_RESET_HA_FW_CONTEXT,
+						&ha->dpc_flags);
+				status = QLA_ERROR;
+
+				goto exit_recover;
+			}
+		}
+
 		if (!test_bit(DPC_RETRY_RESET_HA, &ha->dpc_flags)) {
 			ha->retry_reset_ha_cnt = MAX_RESET_HA_RETRIES;
 			DEBUG2(printk("scsi%ld: recover adapter - retrying "
@@ -2539,6 +2595,7 @@ recover_ha_init_adapter:
 		clear_bit(DPC_RETRY_RESET_HA, &ha->dpc_flags);
 	}
 
+exit_recover:
 	ha->adapter_error_count++;
 
 	if (test_bit(AF_ONLINE, &ha->flags))
@@ -2806,6 +2863,7 @@ dpc_post_reset_ha:
  **/
 static void qla4xxx_free_adapter(struct scsi_qla_host *ha)
 {
+	qla4xxx_abort_active_cmds(ha, DID_NO_CONNECT << 16);
 
 	if (test_bit(AF_INTERRUPTS_ON, &ha->flags)) {
 		/* Turn-off interrupts on the card. */
@@ -4816,6 +4874,20 @@ static int qla4xxx_eh_target_reset(struct scsi_cmnd *cmd)
 }
 
 /**
+ * qla4xxx_is_eh_active - check if error handler is running
+ * @shost: Pointer to SCSI Host struct
+ *
+ * This routine finds that if reset host is called in EH
+ * scenario or from some application like sg_reset
+ **/
+static int qla4xxx_is_eh_active(struct Scsi_Host *shost)
+{
+	if (shost->shost_state == SHOST_RECOVERY)
+		return 1;
+	return 0;
+}
+
+/**
  * qla4xxx_eh_host_reset - kernel callback
  * @cmd: Pointer to Linux's SCSI command structure
  *
@@ -4832,6 +4904,11 @@ static int qla4xxx_eh_host_reset(struct scsi_cmnd *cmd)
 	if (ql4xdontresethba) {
 		DEBUG2(printk("scsi%ld: %s: Don't Reset HBA\n",
 		     ha->host_no, __func__));
+
+		/* Clear outstanding srb in queues */
+		if (qla4xxx_is_eh_active(cmd->device->host))
+			qla4xxx_abort_active_cmds(ha, DID_ABORT << 16);
+
 		return FAILED;
 	}
 
diff --git a/drivers/scsi/qla4xxx/ql4_version.h b/drivers/scsi/qla4xxx/ql4_version.h
index 5254e57968f5..26a3fa34a33c 100644
--- a/drivers/scsi/qla4xxx/ql4_version.h
+++ b/drivers/scsi/qla4xxx/ql4_version.h
@@ -5,4 +5,4 @@
  * See LICENSE.qla4xxx for copyright and licensing details.
  */
 
-#define QLA4XXX_DRIVER_VERSION	"5.02.00-k9"
+#define QLA4XXX_DRIVER_VERSION	"5.02.00-k10"
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index dc6131e6a1ba..5f84a148eb14 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -1812,7 +1812,7 @@ int scsi_error_handler(void *data)
 		 * what we need to do to get it up and online again (if we can).
 		 * If we fail, we end up taking the thing offline.
 		 */
-		if (scsi_autopm_get_host(shost) != 0) {
+		if (!shost->eh_noresume && scsi_autopm_get_host(shost) != 0) {
 			SCSI_LOG_ERROR_RECOVERY(1,
 				printk(KERN_ERR "Error handler scsi_eh_%d "
 						"unable to autoresume\n",
@@ -1833,7 +1833,8 @@ int scsi_error_handler(void *data)
 		 * which are still online.
 		 */
 		scsi_restart_operations(shost);
-		scsi_autopm_put_host(shost);
+		if (!shost->eh_noresume)
+			scsi_autopm_put_host(shost);
 		set_current_state(TASK_INTERRUPTIBLE);
 	}
 	__set_current_state(TASK_RUNNING);
diff --git a/drivers/scsi/scsi_pm.c b/drivers/scsi/scsi_pm.c
index d329f8b12e2b..bf8bf79e6a1f 100644
--- a/drivers/scsi/scsi_pm.c
+++ b/drivers/scsi/scsi_pm.c
@@ -49,8 +49,22 @@ static int scsi_bus_suspend_common(struct device *dev, pm_message_t msg)
 {
 	int err = 0;
 
-	if (scsi_is_sdev_device(dev))
+	if (scsi_is_sdev_device(dev)) {
+		/*
+		 * sd is the only high-level SCSI driver to implement runtime
+		 * PM, and sd treats runtime suspend, system suspend, and
+		 * system hibernate identically (but not system freeze).
+		 */
+		if (pm_runtime_suspended(dev)) {
+			if (msg.event == PM_EVENT_SUSPEND ||
+			    msg.event == PM_EVENT_HIBERNATE)
+				return 0;	/* already suspended */
+
+			/* wake up device so that FREEZE will succeed */
+			pm_runtime_resume(dev);
+		}
 		err = scsi_dev_type_suspend(dev, msg);
+	}
 	return err;
 }
 
@@ -58,8 +72,17 @@ static int scsi_bus_resume_common(struct device *dev)
 {
 	int err = 0;
 
-	if (scsi_is_sdev_device(dev))
+	if (scsi_is_sdev_device(dev)) {
+		/*
+		 * Parent device may have runtime suspended as soon as
+		 * it is woken up during the system resume.
+		 *
+		 * Resume it on behalf of child.
+		 */
+		pm_runtime_get_sync(dev->parent);
 		err = scsi_dev_type_resume(dev);
+		pm_runtime_put_sync(dev->parent);
+	}
 
 	if (err == 0) {
 		pm_runtime_disable(dev);
diff --git a/drivers/scsi/scsi_priv.h b/drivers/scsi/scsi_priv.h
index 2a588955423a..68eadd1c67fd 100644
--- a/drivers/scsi/scsi_priv.h
+++ b/drivers/scsi/scsi_priv.h
@@ -45,7 +45,6 @@ static inline void scsi_log_completion(struct scsi_cmnd *cmd, int disposition)
 enum {
 	SCSI_DEVINFO_GLOBAL = 0,
 	SCSI_DEVINFO_SPI,
-	SCSI_DEVINFO_DH,
 };
 
 extern int scsi_get_device_flags(struct scsi_device *sdev,
diff --git a/drivers/scsi/scsi_transport_iscsi.c b/drivers/scsi/scsi_transport_iscsi.c
index e8447fbc31f3..cfd491437239 100644
--- a/drivers/scsi/scsi_transport_iscsi.c
+++ b/drivers/scsi/scsi_transport_iscsi.c
@@ -1030,6 +1030,7 @@ iscsi_alloc_session(struct Scsi_Host *shost, struct iscsi_transport *transport,
 		return NULL;
 
 	session->transport = transport;
+	session->creator = -1;
 	session->recovery_tmo = 120;
 	session->state = ISCSI_SESSION_FREE;
 	INIT_DELAYED_WORK(&session->recovery_work, session_recovery_timedout);
@@ -1634,8 +1635,9 @@ EXPORT_SYMBOL_GPL(iscsi_session_event);
 
 static int
 iscsi_if_create_session(struct iscsi_internal *priv, struct iscsi_endpoint *ep,
-			struct iscsi_uevent *ev, uint32_t initial_cmdsn,
-			uint16_t cmds_max, uint16_t queue_depth)
+			struct iscsi_uevent *ev, pid_t pid,
+			uint32_t initial_cmdsn,	uint16_t cmds_max,
+			uint16_t queue_depth)
 {
 	struct iscsi_transport *transport = priv->iscsi_transport;
 	struct iscsi_cls_session *session;
@@ -1646,6 +1648,7 @@ iscsi_if_create_session(struct iscsi_internal *priv, struct iscsi_endpoint *ep,
 	if (!session)
 		return -ENOMEM;
 
+	session->creator = pid;
 	shost = iscsi_session_to_shost(session);
 	ev->r.c_session_ret.host_no = shost->host_no;
 	ev->r.c_session_ret.sid = session->sid;
@@ -1938,6 +1941,7 @@ iscsi_if_recv_msg(struct sk_buff *skb, struct nlmsghdr *nlh, uint32_t *group)
 	switch (nlh->nlmsg_type) {
 	case ISCSI_UEVENT_CREATE_SESSION:
 		err = iscsi_if_create_session(priv, ep, ev,
+					      NETLINK_CREDS(skb)->pid,
 					      ev->u.c_session.initial_cmdsn,
 					      ev->u.c_session.cmds_max,
 					      ev->u.c_session.queue_depth);
@@ -1950,6 +1954,7 @@ iscsi_if_recv_msg(struct sk_buff *skb, struct nlmsghdr *nlh, uint32_t *group)
 		}
 
 		err = iscsi_if_create_session(priv, ep, ev,
+					NETLINK_CREDS(skb)->pid,
 					ev->u.c_bound_session.initial_cmdsn,
 					ev->u.c_bound_session.cmds_max,
 					ev->u.c_bound_session.queue_depth);
@@ -2298,6 +2303,15 @@ show_priv_session_state(struct device *dev, struct device_attribute *attr,
 }
 static ISCSI_CLASS_ATTR(priv_sess, state, S_IRUGO, show_priv_session_state,
 			NULL);
+static ssize_t
+show_priv_session_creator(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	struct iscsi_cls_session *session = iscsi_dev_to_session(dev->parent);
+	return sprintf(buf, "%d\n", session->creator);
+}
+static ISCSI_CLASS_ATTR(priv_sess, creator, S_IRUGO, show_priv_session_creator,
+			NULL);
 
 #define iscsi_priv_session_attr_show(field, format)			\
 static ssize_t								\
@@ -2367,6 +2381,7 @@ static struct attribute *iscsi_session_attrs[] = {
 	&dev_attr_sess_targetalias.attr,
 	&dev_attr_priv_sess_recovery_tmo.attr,
 	&dev_attr_priv_sess_state.attr,
+	&dev_attr_priv_sess_creator.attr,
 	NULL,
 };
 
@@ -2424,6 +2439,8 @@ static umode_t iscsi_session_attr_is_visible(struct kobject *kobj,
 		return S_IRUGO | S_IWUSR;
 	else if (attr == &dev_attr_priv_sess_state.attr)
 		return S_IRUGO;
+	else if (attr == &dev_attr_priv_sess_creator.attr)
+		return S_IRUGO;
 	else {
 		WARN_ONCE(1, "Invalid session attr");
 		return 0;
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index fa3a5918009c..7b3f8075e2a5 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -50,6 +50,7 @@
 #include <linux/string_helpers.h>
 #include <linux/async.h>
 #include <linux/slab.h>
+#include <linux/pm_runtime.h>
 #include <asm/uaccess.h>
 #include <asm/unaligned.h>
 
@@ -2741,6 +2742,9 @@ static void sd_shutdown(struct device *dev)
 	if (!sdkp)
 		return;         /* this can happen */
 
+	if (pm_runtime_suspended(dev))
+		goto exit;
+
 	if (sdkp->WCE) {
 		sd_printk(KERN_NOTICE, sdkp, "Synchronizing SCSI cache\n");
 		sd_sync_cache(sdkp);
@@ -2751,6 +2755,7 @@ static void sd_shutdown(struct device *dev)
 		sd_start_stop_device(sdkp, 0);
 	}
 
+exit:
 	scsi_disk_put(sdkp);
 }
 
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index 019a7163572f..dcf7e1006426 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -971,6 +971,7 @@ static int __init s3c64xx_spi_probe(struct platform_device *pdev)
 	struct s3c64xx_spi_info *sci;
 	struct spi_master *master;
 	int ret;
+	char clk_name[16];
 
 	if (pdev->id < 0) {
 		dev_err(&pdev->dev,
@@ -984,11 +985,6 @@ static int __init s3c64xx_spi_probe(struct platform_device *pdev)
 	}
 
 	sci = pdev->dev.platform_data;
-	if (!sci->src_clk_name) {
-		dev_err(&pdev->dev,
-			"Board init must call s3c64xx_spi_set_info()\n");
-		return -EINVAL;
-	}
 
 	/* Check for availability of necessary resource */
 
@@ -1073,17 +1069,17 @@ static int __init s3c64xx_spi_probe(struct platform_device *pdev)
 		goto err4;
 	}
 
-	sdd->src_clk = clk_get(&pdev->dev, sci->src_clk_name);
+	sprintf(clk_name, "spi_busclk%d", sci->src_clk_nr);
+	sdd->src_clk = clk_get(&pdev->dev, clk_name);
 	if (IS_ERR(sdd->src_clk)) {
 		dev_err(&pdev->dev,
-			"Unable to acquire clock '%s'\n", sci->src_clk_name);
+			"Unable to acquire clock '%s'\n", clk_name);
 		ret = PTR_ERR(sdd->src_clk);
 		goto err5;
 	}
 
 	if (clk_enable(sdd->src_clk)) {
-		dev_err(&pdev->dev, "Couldn't enable clock '%s'\n",
-							sci->src_clk_name);
+		dev_err(&pdev->dev, "Couldn't enable clock '%s'\n", clk_name);
 		ret = -EBUSY;
 		goto err6;
 	}
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 25cdff36a78a..21e2f4b87f14 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -72,8 +72,6 @@ source "drivers/staging/octeon/Kconfig"
 
 source "drivers/staging/serqt_usb2/Kconfig"
 
-source "drivers/staging/spectra/Kconfig"
-
 source "drivers/staging/quatech_usb2/Kconfig"
 
 source "drivers/staging/vt6655/Kconfig"
@@ -116,8 +114,6 @@ source "drivers/staging/bcm/Kconfig"
 
 source "drivers/staging/ft1000/Kconfig"
 
-source "drivers/staging/intel_sst/Kconfig"
-
 source "drivers/staging/speakup/Kconfig"
 
 source "drivers/staging/cptm1217/Kconfig"
@@ -132,4 +128,8 @@ source "drivers/staging/nvec/Kconfig"
 
 source "drivers/staging/media/Kconfig"
 
+source "drivers/staging/omapdrm/Kconfig"
+
+source "drivers/staging/android/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index a25f3f26c7ff..7c5808d7212d 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -21,7 +21,6 @@ obj-$(CONFIG_RTL8192E)		+= rtl8192e/
 obj-$(CONFIG_R8712U)		+= rtl8712/
 obj-$(CONFIG_RTS_PSTOR)		+= rts_pstor/
 obj-$(CONFIG_RTS5139)		+= rts5139/
-obj-$(CONFIG_SPECTRA)		+= spectra/
 obj-$(CONFIG_TRANZPORT)		+= frontier/
 obj-$(CONFIG_POHMELFS)		+= pohmelfs/
 obj-$(CONFIG_IDE_PHISON)	+= phison/
@@ -50,10 +49,11 @@ obj-$(CONFIG_SBE_2T3E3)		+= sbe-2t3e3/
 obj-$(CONFIG_USB_ENESTORAGE)	+= keucr/
 obj-$(CONFIG_BCM_WIMAX)		+= bcm/
 obj-$(CONFIG_FT1000)		+= ft1000/
-obj-$(CONFIG_SND_INTEL_SST)	+= intel_sst/
 obj-$(CONFIG_SPEAKUP)		+= speakup/
 obj-$(CONFIG_TOUCHSCREEN_CLEARPAD_TM1217)	+= cptm1217/
 obj-$(CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4)	+= ste_rmi4/
 obj-$(CONFIG_DRM_PSB)		+= gma500/
 obj-$(CONFIG_INTEL_MEI)		+= mei/
 obj-$(CONFIG_MFD_NVEC)		+= nvec/
+obj-$(CONFIG_DRM_OMAP)		+= omapdrm/
+obj-$(CONFIG_ANDROID)		+= android/
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
new file mode 100644
index 000000000000..becf711117ef
--- /dev/null
+++ b/drivers/staging/android/Kconfig
@@ -0,0 +1,110 @@
+menu "Android"
+
+config ANDROID
+	bool "Android Drivers"
+	default N
+	---help---
+	  Enable support for various drivers needed on the Android platform
+
+if ANDROID
+
+config ANDROID_BINDER_IPC
+	bool "Android Binder IPC Driver"
+	default n
+
+config ASHMEM
+	bool "Enable the Anonymous Shared Memory Subsystem"
+	default n
+	depends on SHMEM || TINY_SHMEM
+	help
+	  The ashmem subsystem is a new shared memory allocator, similar to
+	  POSIX SHM but with different behavior and sporting a simpler
+	  file-based API.
+
+config ANDROID_LOGGER
+	tristate "Android log driver"
+	default n
+
+config ANDROID_RAM_CONSOLE
+	bool "Android RAM buffer console"
+	default n
+
+config ANDROID_RAM_CONSOLE_ENABLE_VERBOSE
+	bool "Enable verbose console messages on Android RAM console"
+	default y
+	depends on ANDROID_RAM_CONSOLE
+
+menuconfig ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	bool "Android RAM Console Enable error correction"
+	default n
+	depends on ANDROID_RAM_CONSOLE
+	depends on !ANDROID_RAM_CONSOLE_EARLY_INIT
+	select REED_SOLOMON
+	select REED_SOLOMON_ENC8
+	select REED_SOLOMON_DEC8
+
+if ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_DATA_SIZE
+	int "Android RAM Console Data data size"
+	default 128
+	help
+	  Must be a power of 2.
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_ECC_SIZE
+	int "Android RAM Console ECC size"
+	default 16
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE
+	int "Android RAM Console Symbol size"
+	default 8
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_POLYNOMIAL
+	hex "Android RAM Console Polynomial"
+	default 0x19 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 4)
+	default 0x29 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 5)
+	default 0x61 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 6)
+	default 0x89 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 7)
+	default 0x11d if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 8)
+
+endif # ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+
+config ANDROID_RAM_CONSOLE_EARLY_INIT
+	bool "Start Android RAM console early"
+	default n
+	depends on ANDROID_RAM_CONSOLE
+
+config ANDROID_RAM_CONSOLE_EARLY_ADDR
+	hex "Android RAM console virtual address"
+	default 0
+	depends on ANDROID_RAM_CONSOLE_EARLY_INIT
+
+config ANDROID_RAM_CONSOLE_EARLY_SIZE
+	hex "Android RAM console buffer size"
+	default 0
+	depends on ANDROID_RAM_CONSOLE_EARLY_INIT
+
+config ANDROID_TIMED_OUTPUT
+	bool "Timed output class driver"
+	default y
+
+config ANDROID_TIMED_GPIO
+	tristate "Android timed gpio driver"
+	depends on GENERIC_GPIO && ANDROID_TIMED_OUTPUT
+	default n
+
+config ANDROID_LOW_MEMORY_KILLER
+	bool "Android Low Memory Killer"
+	default N
+	---help---
+	  Register processes to be killed when memory is low
+
+config ANDROID_PMEM
+	bool "Android pmem allocator"
+	depends on ARM
+
+source "drivers/staging/android/switch/Kconfig"
+
+endif # if ANDROID
+
+endmenu
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
new file mode 100644
index 000000000000..eaed1ff64f0f
--- /dev/null
+++ b/drivers/staging/android/Makefile
@@ -0,0 +1,9 @@
+obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
+obj-$(CONFIG_ASHMEM)			+= ashmem.o
+obj-$(CONFIG_ANDROID_LOGGER)		+= logger.o
+obj-$(CONFIG_ANDROID_RAM_CONSOLE)	+= ram_console.o
+obj-$(CONFIG_ANDROID_TIMED_OUTPUT)	+= timed_output.o
+obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
+obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
+obj-$(CONFIG_ANDROID_PMEM)		+= pmem.o
+obj-$(CONFIG_ANDROID_SWITCH)		+= switch/
diff --git a/drivers/staging/android/TODO b/drivers/staging/android/TODO
new file mode 100644
index 000000000000..e59c5be4be2b
--- /dev/null
+++ b/drivers/staging/android/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse fixes
+	- rename files to be not so "generic"
+	- make sure things build as modules properly
+	- add proper arch dependancies as needed
+	- audit userspace interfaces to make sure they are sane
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and Cc:
+Brian Swetland <swetland@google.com>
diff --git a/drivers/staging/android/android_pmem.h b/drivers/staging/android/android_pmem.h
new file mode 100644
index 000000000000..f633621f5be3
--- /dev/null
+++ b/drivers/staging/android/android_pmem.h
@@ -0,0 +1,93 @@
+/* include/linux/android_pmem.h
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of 
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _ANDROID_PMEM_H_
+#define _ANDROID_PMEM_H_
+
+#define PMEM_IOCTL_MAGIC 'p'
+#define PMEM_GET_PHYS		_IOW(PMEM_IOCTL_MAGIC, 1, unsigned int)
+#define PMEM_MAP		_IOW(PMEM_IOCTL_MAGIC, 2, unsigned int)
+#define PMEM_GET_SIZE		_IOW(PMEM_IOCTL_MAGIC, 3, unsigned int)
+#define PMEM_UNMAP		_IOW(PMEM_IOCTL_MAGIC, 4, unsigned int)
+/* This ioctl will allocate pmem space, backing the file, it will fail
+ * if the file already has an allocation, pass it the len as the argument
+ * to the ioctl */
+#define PMEM_ALLOCATE		_IOW(PMEM_IOCTL_MAGIC, 5, unsigned int)
+/* This will connect a one pmem file to another, pass the file that is already
+ * backed in memory as the argument to the ioctl
+ */
+#define PMEM_CONNECT		_IOW(PMEM_IOCTL_MAGIC, 6, unsigned int)
+/* Returns the total size of the pmem region it is sent to as a pmem_region
+ * struct (with offset set to 0). 
+ */
+#define PMEM_GET_TOTAL_SIZE	_IOW(PMEM_IOCTL_MAGIC, 7, unsigned int)
+#define PMEM_CACHE_FLUSH	_IOW(PMEM_IOCTL_MAGIC, 8, unsigned int)
+
+struct android_pmem_platform_data
+{
+	const char* name;
+	/* starting physical address of memory region */
+	unsigned long start;
+	/* size of memory region */
+	unsigned long size;
+	/* set to indicate the region should not be managed with an allocator */
+	unsigned no_allocator;
+	/* set to indicate maps of this region should be cached, if a mix of
+	 * cached and uncached is desired, set this and open the device with
+	 * O_SYNC to get an uncached region */
+	unsigned cached;
+	/* The MSM7k has bits to enable a write buffer in the bus controller*/
+	unsigned buffered;
+};
+
+struct pmem_region {
+	unsigned long offset;
+	unsigned long len;
+};
+
+#ifdef CONFIG_ANDROID_PMEM
+int is_pmem_file(struct file *file);
+int get_pmem_file(int fd, unsigned long *start, unsigned long *vstart,
+		  unsigned long *end, struct file **filp);
+int get_pmem_user_addr(struct file *file, unsigned long *start,
+		       unsigned long *end);
+void put_pmem_file(struct file* file);
+void flush_pmem_file(struct file *file, unsigned long start, unsigned long len);
+int pmem_setup(struct android_pmem_platform_data *pdata,
+	       long (*ioctl)(struct file *, unsigned int, unsigned long),
+	       int (*release)(struct inode *, struct file *));
+int pmem_remap(struct pmem_region *region, struct file *file,
+	       unsigned operation);
+
+#else
+static inline int is_pmem_file(struct file *file) { return 0; }
+static inline int get_pmem_file(int fd, unsigned long *start,
+				unsigned long *vstart, unsigned long *end,
+				struct file **filp) { return -ENOSYS; }
+static inline int get_pmem_user_addr(struct file *file, unsigned long *start,
+				     unsigned long *end) { return -ENOSYS; }
+static inline void put_pmem_file(struct file* file) { return; }
+static inline void flush_pmem_file(struct file *file, unsigned long start,
+				   unsigned long len) { return; }
+static inline int pmem_setup(struct android_pmem_platform_data *pdata,
+	      long (*ioctl)(struct file *, unsigned int, unsigned long),
+	      int (*release)(struct inode *, struct file *)) { return -ENOSYS; }
+
+static inline int pmem_remap(struct pmem_region *region, struct file *file,
+			     unsigned operation) { return -ENOSYS; }
+#endif
+
+#endif //_ANDROID_PPP_H_
+
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
new file mode 100644
index 000000000000..99052bfd3a2d
--- /dev/null
+++ b/drivers/staging/android/ashmem.c
@@ -0,0 +1,752 @@
+/* mm/ashmem.c
+**
+** Anonymous Shared Memory Subsystem, ashmem
+**
+** Copyright (C) 2008 Google, Inc.
+**
+** Robert Love <rlove@google.com>
+**
+** This software is licensed under the terms of the GNU General Public
+** License version 2, as published by the Free Software Foundation, and
+** may be copied, distributed, and modified under those terms.
+**
+** This program is distributed in the hope that it will be useful,
+** but WITHOUT ANY WARRANTY; without even the implied warranty of
+** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+** GNU General Public License for more details.
+*/
+
+#include <linux/module.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/security.h>
+#include <linux/mm.h>
+#include <linux/mman.h>
+#include <linux/uaccess.h>
+#include <linux/personality.h>
+#include <linux/bitops.h>
+#include <linux/mutex.h>
+#include <linux/shmem_fs.h>
+#include "ashmem.h"
+
+#define ASHMEM_NAME_PREFIX "dev/ashmem/"
+#define ASHMEM_NAME_PREFIX_LEN (sizeof(ASHMEM_NAME_PREFIX) - 1)
+#define ASHMEM_FULL_NAME_LEN (ASHMEM_NAME_LEN + ASHMEM_NAME_PREFIX_LEN)
+
+/*
+ * ashmem_area - anonymous shared memory area
+ * Lifecycle: From our parent file's open() until its release()
+ * Locking: Protected by `ashmem_mutex'
+ * Big Note: Mappings do NOT pin this structure; it dies on close()
+ */
+struct ashmem_area {
+	char name[ASHMEM_FULL_NAME_LEN]; /* optional name in /proc/pid/maps */
+	struct list_head unpinned_list;	 /* list of all ashmem areas */
+	struct file *file;		 /* the shmem-based backing file */
+	size_t size;			 /* size of the mapping, in bytes */
+	unsigned long prot_mask;	 /* allowed prot bits, as vm_flags */
+};
+
+/*
+ * ashmem_range - represents an interval of unpinned (evictable) pages
+ * Lifecycle: From unpin to pin
+ * Locking: Protected by `ashmem_mutex'
+ */
+struct ashmem_range {
+	struct list_head lru;		/* entry in LRU list */
+	struct list_head unpinned;	/* entry in its area's unpinned list */
+	struct ashmem_area *asma;	/* associated area */
+	size_t pgstart;			/* starting page, inclusive */
+	size_t pgend;			/* ending page, inclusive */
+	unsigned int purged;		/* ASHMEM_NOT or ASHMEM_WAS_PURGED */
+};
+
+/* LRU list of unpinned pages, protected by ashmem_mutex */
+static LIST_HEAD(ashmem_lru_list);
+
+/* Count of pages on our LRU list, protected by ashmem_mutex */
+static unsigned long lru_count;
+
+/*
+ * ashmem_mutex - protects the list of and each individual ashmem_area
+ *
+ * Lock Ordering: ashmex_mutex -> i_mutex -> i_alloc_sem
+ */
+static DEFINE_MUTEX(ashmem_mutex);
+
+static struct kmem_cache *ashmem_area_cachep __read_mostly;
+static struct kmem_cache *ashmem_range_cachep __read_mostly;
+
+#define range_size(range) \
+	((range)->pgend - (range)->pgstart + 1)
+
+#define range_on_lru(range) \
+	((range)->purged == ASHMEM_NOT_PURGED)
+
+#define page_range_subsumes_range(range, start, end) \
+	(((range)->pgstart >= (start)) && ((range)->pgend <= (end)))
+
+#define page_range_subsumed_by_range(range, start, end) \
+	(((range)->pgstart <= (start)) && ((range)->pgend >= (end)))
+
+#define page_in_range(range, page) \
+	(((range)->pgstart <= (page)) && ((range)->pgend >= (page)))
+
+#define page_range_in_range(range, start, end) \
+	(page_in_range(range, start) || page_in_range(range, end) || \
+		page_range_subsumes_range(range, start, end))
+
+#define range_before_page(range, page) \
+	((range)->pgend < (page))
+
+#define PROT_MASK		(PROT_EXEC | PROT_READ | PROT_WRITE)
+
+static inline void lru_add(struct ashmem_range *range)
+{
+	list_add_tail(&range->lru, &ashmem_lru_list);
+	lru_count += range_size(range);
+}
+
+static inline void lru_del(struct ashmem_range *range)
+{
+	list_del(&range->lru);
+	lru_count -= range_size(range);
+}
+
+/*
+ * range_alloc - allocate and initialize a new ashmem_range structure
+ *
+ * 'asma' - associated ashmem_area
+ * 'prev_range' - the previous ashmem_range in the sorted asma->unpinned list
+ * 'purged' - initial purge value (ASMEM_NOT_PURGED or ASHMEM_WAS_PURGED)
+ * 'start' - starting page, inclusive
+ * 'end' - ending page, inclusive
+ *
+ * Caller must hold ashmem_mutex.
+ */
+static int range_alloc(struct ashmem_area *asma,
+		       struct ashmem_range *prev_range, unsigned int purged,
+		       size_t start, size_t end)
+{
+	struct ashmem_range *range;
+
+	range = kmem_cache_zalloc(ashmem_range_cachep, GFP_KERNEL);
+	if (unlikely(!range))
+		return -ENOMEM;
+
+	range->asma = asma;
+	range->pgstart = start;
+	range->pgend = end;
+	range->purged = purged;
+
+	list_add_tail(&range->unpinned, &prev_range->unpinned);
+
+	if (range_on_lru(range))
+		lru_add(range);
+
+	return 0;
+}
+
+static void range_del(struct ashmem_range *range)
+{
+	list_del(&range->unpinned);
+	if (range_on_lru(range))
+		lru_del(range);
+	kmem_cache_free(ashmem_range_cachep, range);
+}
+
+/*
+ * range_shrink - shrinks a range
+ *
+ * Caller must hold ashmem_mutex.
+ */
+static inline void range_shrink(struct ashmem_range *range,
+				size_t start, size_t end)
+{
+	size_t pre = range_size(range);
+
+	range->pgstart = start;
+	range->pgend = end;
+
+	if (range_on_lru(range))
+		lru_count -= pre - range_size(range);
+}
+
+static int ashmem_open(struct inode *inode, struct file *file)
+{
+	struct ashmem_area *asma;
+	int ret;
+
+	ret = generic_file_open(inode, file);
+	if (unlikely(ret))
+		return ret;
+
+	asma = kmem_cache_zalloc(ashmem_area_cachep, GFP_KERNEL);
+	if (unlikely(!asma))
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&asma->unpinned_list);
+	memcpy(asma->name, ASHMEM_NAME_PREFIX, ASHMEM_NAME_PREFIX_LEN);
+	asma->prot_mask = PROT_MASK;
+	file->private_data = asma;
+
+	return 0;
+}
+
+static int ashmem_release(struct inode *ignored, struct file *file)
+{
+	struct ashmem_area *asma = file->private_data;
+	struct ashmem_range *range, *next;
+
+	mutex_lock(&ashmem_mutex);
+	list_for_each_entry_safe(range, next, &asma->unpinned_list, unpinned)
+		range_del(range);
+	mutex_unlock(&ashmem_mutex);
+
+	if (asma->file)
+		fput(asma->file);
+	kmem_cache_free(ashmem_area_cachep, asma);
+
+	return 0;
+}
+
+static ssize_t ashmem_read(struct file *file, char __user *buf,
+			   size_t len, loff_t *pos)
+{
+	struct ashmem_area *asma = file->private_data;
+	int ret = 0;
+
+	mutex_lock(&ashmem_mutex);
+
+	/* If size is not set, or set to 0, always return EOF. */
+	if (asma->size == 0)
+		goto out;
+
+	if (!asma->file) {
+		ret = -EBADF;
+		goto out;
+	}
+
+	ret = asma->file->f_op->read(asma->file, buf, len, pos);
+	if (ret < 0)
+		goto out;
+
+	/** Update backing file pos, since f_ops->read() doesn't */
+	asma->file->f_pos = *pos;
+
+out:
+	mutex_unlock(&ashmem_mutex);
+	return ret;
+}
+
+static loff_t ashmem_llseek(struct file *file, loff_t offset, int origin)
+{
+	struct ashmem_area *asma = file->private_data;
+	int ret;
+
+	mutex_lock(&ashmem_mutex);
+
+	if (asma->size == 0) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (!asma->file) {
+		ret = -EBADF;
+		goto out;
+	}
+
+	ret = asma->file->f_op->llseek(asma->file, offset, origin);
+	if (ret < 0)
+		goto out;
+
+	/** Copy f_pos from backing file, since f_ops->llseek() sets it */
+	file->f_pos = asma->file->f_pos;
+
+out:
+	mutex_unlock(&ashmem_mutex);
+	return ret;
+}
+
+static inline unsigned long calc_vm_may_flags(unsigned long prot)
+{
+	return _calc_vm_trans(prot, PROT_READ,  VM_MAYREAD) |
+	       _calc_vm_trans(prot, PROT_WRITE, VM_MAYWRITE) |
+	       _calc_vm_trans(prot, PROT_EXEC,  VM_MAYEXEC);
+}
+
+static int ashmem_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct ashmem_area *asma = file->private_data;
+	int ret = 0;
+
+	mutex_lock(&ashmem_mutex);
+
+	/* user needs to SET_SIZE before mapping */
+	if (unlikely(!asma->size)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* requested protection bits must match our allowed protection mask */
+	if (unlikely((vma->vm_flags & ~calc_vm_prot_bits(asma->prot_mask)) &
+		     calc_vm_prot_bits(PROT_MASK))) {
+		ret = -EPERM;
+		goto out;
+	}
+	vma->vm_flags &= ~calc_vm_may_flags(~asma->prot_mask);
+
+	if (!asma->file) {
+		char *name = ASHMEM_NAME_DEF;
+		struct file *vmfile;
+
+		if (asma->name[ASHMEM_NAME_PREFIX_LEN] != '\0')
+			name = asma->name;
+
+		/* ... and allocate the backing shmem file */
+		vmfile = shmem_file_setup(name, asma->size, vma->vm_flags);
+		if (unlikely(IS_ERR(vmfile))) {
+			ret = PTR_ERR(vmfile);
+			goto out;
+		}
+		asma->file = vmfile;
+	}
+	get_file(asma->file);
+
+	/*
+	 * XXX - Reworked to use shmem_zero_setup() instead of 
+	 * shmem_set_file while we're in staging. -jstultz
+	 */
+	if (vma->vm_flags & VM_SHARED) {
+		ret = shmem_zero_setup(vma);
+		if (ret) {
+			fput(asma->file);
+			goto out;
+		}
+	}
+
+	if (vma->vm_file)
+		fput(vma->vm_file);
+	vma->vm_file = asma->file;
+	vma->vm_flags |= VM_CAN_NONLINEAR;
+
+out:
+	mutex_unlock(&ashmem_mutex);
+	return ret;
+}
+
+/*
+ * ashmem_shrink - our cache shrinker, called from mm/vmscan.c :: shrink_slab
+ *
+ * 'nr_to_scan' is the number of objects (pages) to prune, or 0 to query how
+ * many objects (pages) we have in total.
+ *
+ * 'gfp_mask' is the mask of the allocation that got us into this mess.
+ *
+ * Return value is the number of objects (pages) remaining, or -1 if we cannot
+ * proceed without risk of deadlock (due to gfp_mask).
+ *
+ * We approximate LRU via least-recently-unpinned, jettisoning unpinned partial
+ * chunks of ashmem regions LRU-wise one-at-a-time until we hit 'nr_to_scan'
+ * pages freed.
+ */
+static int ashmem_shrink(struct shrinker *s, struct shrink_control *sc)
+{
+	struct ashmem_range *range, *next;
+
+	/* We might recurse into filesystem code, so bail out if necessary */
+	if (sc->nr_to_scan && !(sc->gfp_mask & __GFP_FS))
+		return -1;
+	if (!sc->nr_to_scan)
+		return lru_count;
+
+	mutex_lock(&ashmem_mutex);
+	list_for_each_entry_safe(range, next, &ashmem_lru_list, lru) {
+		struct inode *inode = range->asma->file->f_dentry->d_inode;
+		loff_t start = range->pgstart * PAGE_SIZE;
+		loff_t end = (range->pgend + 1) * PAGE_SIZE - 1;
+
+		vmtruncate_range(inode, start, end);
+		range->purged = ASHMEM_WAS_PURGED;
+		lru_del(range);
+
+		sc->nr_to_scan -= range_size(range);
+		if (sc->nr_to_scan <= 0)
+			break;
+	}
+	mutex_unlock(&ashmem_mutex);
+
+	return lru_count;
+}
+
+static struct shrinker ashmem_shrinker = {
+	.shrink = ashmem_shrink,
+	.seeks = DEFAULT_SEEKS * 4,
+};
+
+static int set_prot_mask(struct ashmem_area *asma, unsigned long prot)
+{
+	int ret = 0;
+
+	mutex_lock(&ashmem_mutex);
+
+	/* the user can only remove, not add, protection bits */
+	if (unlikely((asma->prot_mask & prot) != prot)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* does the application expect PROT_READ to imply PROT_EXEC? */
+	if ((prot & PROT_READ) && (current->personality & READ_IMPLIES_EXEC))
+		prot |= PROT_EXEC;
+
+	asma->prot_mask = prot;
+
+out:
+	mutex_unlock(&ashmem_mutex);
+	return ret;
+}
+
+static int set_name(struct ashmem_area *asma, void __user *name)
+{
+	int ret = 0;
+
+	mutex_lock(&ashmem_mutex);
+
+	/* cannot change an existing mapping's name */
+	if (unlikely(asma->file)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (unlikely(copy_from_user(asma->name + ASHMEM_NAME_PREFIX_LEN,
+				    name, ASHMEM_NAME_LEN)))
+		ret = -EFAULT;
+	asma->name[ASHMEM_FULL_NAME_LEN-1] = '\0';
+
+out:
+	mutex_unlock(&ashmem_mutex);
+
+	return ret;
+}
+
+static int get_name(struct ashmem_area *asma, void __user *name)
+{
+	int ret = 0;
+
+	mutex_lock(&ashmem_mutex);
+	if (asma->name[ASHMEM_NAME_PREFIX_LEN] != '\0') {
+		size_t len;
+
+		/*
+		 * Copying only `len', instead of ASHMEM_NAME_LEN, bytes
+		 * prevents us from revealing one user's stack to another.
+		 */
+		len = strlen(asma->name + ASHMEM_NAME_PREFIX_LEN) + 1;
+		if (unlikely(copy_to_user(name,
+				asma->name + ASHMEM_NAME_PREFIX_LEN, len)))
+			ret = -EFAULT;
+	} else {
+		if (unlikely(copy_to_user(name, ASHMEM_NAME_DEF,
+					  sizeof(ASHMEM_NAME_DEF))))
+			ret = -EFAULT;
+	}
+	mutex_unlock(&ashmem_mutex);
+
+	return ret;
+}
+
+/*
+ * ashmem_pin - pin the given ashmem region, returning whether it was
+ * previously purged (ASHMEM_WAS_PURGED) or not (ASHMEM_NOT_PURGED).
+ *
+ * Caller must hold ashmem_mutex.
+ */
+static int ashmem_pin(struct ashmem_area *asma, size_t pgstart, size_t pgend)
+{
+	struct ashmem_range *range, *next;
+	int ret = ASHMEM_NOT_PURGED;
+
+	list_for_each_entry_safe(range, next, &asma->unpinned_list, unpinned) {
+		/* moved past last applicable page; we can short circuit */
+		if (range_before_page(range, pgstart))
+			break;
+
+		/*
+		 * The user can ask us to pin pages that span multiple ranges,
+		 * or to pin pages that aren't even unpinned, so this is messy.
+		 *
+		 * Four cases:
+		 * 1. The requested range subsumes an existing range, so we
+		 *    just remove the entire matching range.
+		 * 2. The requested range overlaps the start of an existing
+		 *    range, so we just update that range.
+		 * 3. The requested range overlaps the end of an existing
+		 *    range, so we just update that range.
+		 * 4. The requested range punches a hole in an existing range,
+		 *    so we have to update one side of the range and then
+		 *    create a new range for the other side.
+		 */
+		if (page_range_in_range(range, pgstart, pgend)) {
+			ret |= range->purged;
+
+			/* Case #1: Easy. Just nuke the whole thing. */
+			if (page_range_subsumes_range(range, pgstart, pgend)) {
+				range_del(range);
+				continue;
+			}
+
+			/* Case #2: We overlap from the start, so adjust it */
+			if (range->pgstart >= pgstart) {
+				range_shrink(range, pgend + 1, range->pgend);
+				continue;
+			}
+
+			/* Case #3: We overlap from the rear, so adjust it */
+			if (range->pgend <= pgend) {
+				range_shrink(range, range->pgstart, pgstart-1);
+				continue;
+			}
+
+			/*
+			 * Case #4: We eat a chunk out of the middle. A bit
+			 * more complicated, we allocate a new range for the
+			 * second half and adjust the first chunk's endpoint.
+			 */
+			range_alloc(asma, range, range->purged,
+				    pgend + 1, range->pgend);
+			range_shrink(range, range->pgstart, pgstart - 1);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ * ashmem_unpin - unpin the given range of pages. Returns zero on success.
+ *
+ * Caller must hold ashmem_mutex.
+ */
+static int ashmem_unpin(struct ashmem_area *asma, size_t pgstart, size_t pgend)
+{
+	struct ashmem_range *range, *next;
+	unsigned int purged = ASHMEM_NOT_PURGED;
+
+restart:
+	list_for_each_entry_safe(range, next, &asma->unpinned_list, unpinned) {
+		/* short circuit: this is our insertion point */
+		if (range_before_page(range, pgstart))
+			break;
+
+		/*
+		 * The user can ask us to unpin pages that are already entirely
+		 * or partially pinned. We handle those two cases here.
+		 */
+		if (page_range_subsumed_by_range(range, pgstart, pgend))
+			return 0;
+		if (page_range_in_range(range, pgstart, pgend)) {
+			pgstart = min_t(size_t, range->pgstart, pgstart),
+			pgend = max_t(size_t, range->pgend, pgend);
+			purged |= range->purged;
+			range_del(range);
+			goto restart;
+		}
+	}
+
+	return range_alloc(asma, range, purged, pgstart, pgend);
+}
+
+/*
+ * ashmem_get_pin_status - Returns ASHMEM_IS_UNPINNED if _any_ pages in the
+ * given interval are unpinned and ASHMEM_IS_PINNED otherwise.
+ *
+ * Caller must hold ashmem_mutex.
+ */
+static int ashmem_get_pin_status(struct ashmem_area *asma, size_t pgstart,
+				 size_t pgend)
+{
+	struct ashmem_range *range;
+	int ret = ASHMEM_IS_PINNED;
+
+	list_for_each_entry(range, &asma->unpinned_list, unpinned) {
+		if (range_before_page(range, pgstart))
+			break;
+		if (page_range_in_range(range, pgstart, pgend)) {
+			ret = ASHMEM_IS_UNPINNED;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static int ashmem_pin_unpin(struct ashmem_area *asma, unsigned long cmd,
+			    void __user *p)
+{
+	struct ashmem_pin pin;
+	size_t pgstart, pgend;
+	int ret = -EINVAL;
+
+	if (unlikely(!asma->file))
+		return -EINVAL;
+
+	if (unlikely(copy_from_user(&pin, p, sizeof(pin))))
+		return -EFAULT;
+
+	/* per custom, you can pass zero for len to mean "everything onward" */
+	if (!pin.len)
+		pin.len = PAGE_ALIGN(asma->size) - pin.offset;
+
+	if (unlikely((pin.offset | pin.len) & ~PAGE_MASK))
+		return -EINVAL;
+
+	if (unlikely(((__u32) -1) - pin.offset < pin.len))
+		return -EINVAL;
+
+	if (unlikely(PAGE_ALIGN(asma->size) < pin.offset + pin.len))
+		return -EINVAL;
+
+	pgstart = pin.offset / PAGE_SIZE;
+	pgend = pgstart + (pin.len / PAGE_SIZE) - 1;
+
+	mutex_lock(&ashmem_mutex);
+
+	switch (cmd) {
+	case ASHMEM_PIN:
+		ret = ashmem_pin(asma, pgstart, pgend);
+		break;
+	case ASHMEM_UNPIN:
+		ret = ashmem_unpin(asma, pgstart, pgend);
+		break;
+	case ASHMEM_GET_PIN_STATUS:
+		ret = ashmem_get_pin_status(asma, pgstart, pgend);
+		break;
+	}
+
+	mutex_unlock(&ashmem_mutex);
+
+	return ret;
+}
+
+static long ashmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct ashmem_area *asma = file->private_data;
+	long ret = -ENOTTY;
+
+	switch (cmd) {
+	case ASHMEM_SET_NAME:
+		ret = set_name(asma, (void __user *) arg);
+		break;
+	case ASHMEM_GET_NAME:
+		ret = get_name(asma, (void __user *) arg);
+		break;
+	case ASHMEM_SET_SIZE:
+		ret = -EINVAL;
+		if (!asma->file) {
+			ret = 0;
+			asma->size = (size_t) arg;
+		}
+		break;
+	case ASHMEM_GET_SIZE:
+		ret = asma->size;
+		break;
+	case ASHMEM_SET_PROT_MASK:
+		ret = set_prot_mask(asma, arg);
+		break;
+	case ASHMEM_GET_PROT_MASK:
+		ret = asma->prot_mask;
+		break;
+	case ASHMEM_PIN:
+	case ASHMEM_UNPIN:
+	case ASHMEM_GET_PIN_STATUS:
+		ret = ashmem_pin_unpin(asma, cmd, (void __user *) arg);
+		break;
+	case ASHMEM_PURGE_ALL_CACHES:
+		ret = -EPERM;
+		if (capable(CAP_SYS_ADMIN)) {
+			struct shrink_control sc = {
+				.gfp_mask = GFP_KERNEL,
+				.nr_to_scan = 0,
+			};
+			ret = ashmem_shrink(&ashmem_shrinker, &sc);
+			sc.nr_to_scan = ret;
+			ashmem_shrink(&ashmem_shrinker, &sc);
+		}
+		break;
+	}
+
+	return ret;
+}
+
+static struct file_operations ashmem_fops = {
+	.owner = THIS_MODULE,
+	.open = ashmem_open,
+	.release = ashmem_release,
+	.read = ashmem_read,
+	.llseek = ashmem_llseek,
+	.mmap = ashmem_mmap,
+	.unlocked_ioctl = ashmem_ioctl,
+	.compat_ioctl = ashmem_ioctl,
+};
+
+static struct miscdevice ashmem_misc = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ashmem",
+	.fops = &ashmem_fops,
+};
+
+static int __init ashmem_init(void)
+{
+	int ret;
+
+	ashmem_area_cachep = kmem_cache_create("ashmem_area_cache",
+					  sizeof(struct ashmem_area),
+					  0, 0, NULL);
+	if (unlikely(!ashmem_area_cachep)) {
+		printk(KERN_ERR "ashmem: failed to create slab cache\n");
+		return -ENOMEM;
+	}
+
+	ashmem_range_cachep = kmem_cache_create("ashmem_range_cache",
+					  sizeof(struct ashmem_range),
+					  0, 0, NULL);
+	if (unlikely(!ashmem_range_cachep)) {
+		printk(KERN_ERR "ashmem: failed to create slab cache\n");
+		return -ENOMEM;
+	}
+
+	ret = misc_register(&ashmem_misc);
+	if (unlikely(ret)) {
+		printk(KERN_ERR "ashmem: failed to register misc device!\n");
+		return ret;
+	}
+
+	register_shrinker(&ashmem_shrinker);
+
+	printk(KERN_INFO "ashmem: initialized\n");
+
+	return 0;
+}
+
+static void __exit ashmem_exit(void)
+{
+	int ret;
+
+	unregister_shrinker(&ashmem_shrinker);
+
+	ret = misc_deregister(&ashmem_misc);
+	if (unlikely(ret))
+		printk(KERN_ERR "ashmem: failed to unregister misc device!\n");
+
+	kmem_cache_destroy(ashmem_range_cachep);
+	kmem_cache_destroy(ashmem_area_cachep);
+
+	printk(KERN_INFO "ashmem: unloaded\n");
+}
+
+module_init(ashmem_init);
+module_exit(ashmem_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/ashmem.h b/drivers/staging/android/ashmem.h
new file mode 100644
index 000000000000..1976b10ef93e
--- /dev/null
+++ b/drivers/staging/android/ashmem.h
@@ -0,0 +1,48 @@
+/*
+ * include/linux/ashmem.h
+ *
+ * Copyright 2008 Google Inc.
+ * Author: Robert Love
+ *
+ * This file is dual licensed.  It may be redistributed and/or modified
+ * under the terms of the Apache 2.0 License OR version 2 of the GNU
+ * General Public License.
+ */
+
+#ifndef _LINUX_ASHMEM_H
+#define _LINUX_ASHMEM_H
+
+#include <linux/limits.h>
+#include <linux/ioctl.h>
+
+#define ASHMEM_NAME_LEN		256
+
+#define ASHMEM_NAME_DEF		"dev/ashmem"
+
+/* Return values from ASHMEM_PIN: Was the mapping purged while unpinned? */
+#define ASHMEM_NOT_PURGED	0
+#define ASHMEM_WAS_PURGED	1
+
+/* Return values from ASHMEM_GET_PIN_STATUS: Is the mapping pinned? */
+#define ASHMEM_IS_UNPINNED	0
+#define ASHMEM_IS_PINNED	1
+
+struct ashmem_pin {
+	__u32 offset;	/* offset into region, in bytes, page-aligned */
+	__u32 len;	/* length forward from offset, in bytes, page-aligned */
+};
+
+#define __ASHMEMIOC		0x77
+
+#define ASHMEM_SET_NAME		_IOW(__ASHMEMIOC, 1, char[ASHMEM_NAME_LEN])
+#define ASHMEM_GET_NAME		_IOR(__ASHMEMIOC, 2, char[ASHMEM_NAME_LEN])
+#define ASHMEM_SET_SIZE		_IOW(__ASHMEMIOC, 3, size_t)
+#define ASHMEM_GET_SIZE		_IO(__ASHMEMIOC, 4)
+#define ASHMEM_SET_PROT_MASK	_IOW(__ASHMEMIOC, 5, unsigned long)
+#define ASHMEM_GET_PROT_MASK	_IO(__ASHMEMIOC, 6)
+#define ASHMEM_PIN		_IOW(__ASHMEMIOC, 7, struct ashmem_pin)
+#define ASHMEM_UNPIN		_IOW(__ASHMEMIOC, 8, struct ashmem_pin)
+#define ASHMEM_GET_PIN_STATUS	_IO(__ASHMEMIOC, 9)
+#define ASHMEM_PURGE_ALL_CACHES	_IO(__ASHMEMIOC, 10)
+
+#endif	/* _LINUX_ASHMEM_H */
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
new file mode 100644
index 000000000000..7491801a661c
--- /dev/null
+++ b/drivers/staging/android/binder.c
@@ -0,0 +1,3600 @@
+/* binder.c
+ *
+ * Android IPC Subsystem
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <asm/cacheflush.h>
+#include <linux/fdtable.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/nsproxy.h>
+#include <linux/poll.h>
+#include <linux/debugfs.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include <linux/slab.h>
+
+#include "binder.h"
+
+static DEFINE_MUTEX(binder_lock);
+static DEFINE_MUTEX(binder_deferred_lock);
+
+static HLIST_HEAD(binder_procs);
+static HLIST_HEAD(binder_deferred_list);
+static HLIST_HEAD(binder_dead_nodes);
+
+static struct dentry *binder_debugfs_dir_entry_root;
+static struct dentry *binder_debugfs_dir_entry_proc;
+static struct binder_node *binder_context_mgr_node;
+static uid_t binder_context_mgr_uid = -1;
+static int binder_last_id;
+static struct workqueue_struct *binder_deferred_workqueue;
+
+#define BINDER_DEBUG_ENTRY(name) \
+static int binder_##name##_open(struct inode *inode, struct file *file) \
+{ \
+	return single_open(file, binder_##name##_show, inode->i_private); \
+} \
+\
+static const struct file_operations binder_##name##_fops = { \
+	.owner = THIS_MODULE, \
+	.open = binder_##name##_open, \
+	.read = seq_read, \
+	.llseek = seq_lseek, \
+	.release = single_release, \
+}
+
+static int binder_proc_show(struct seq_file *m, void *unused);
+BINDER_DEBUG_ENTRY(proc);
+
+/* This is only defined in include/asm-arm/sizes.h */
+#ifndef SZ_1K
+#define SZ_1K                               0x400
+#endif
+
+#ifndef SZ_4M
+#define SZ_4M                               0x400000
+#endif
+
+#define FORBIDDEN_MMAP_FLAGS                (VM_WRITE)
+
+#define BINDER_SMALL_BUF_SIZE (PAGE_SIZE * 64)
+
+enum {
+	BINDER_DEBUG_USER_ERROR             = 1U << 0,
+	BINDER_DEBUG_FAILED_TRANSACTION     = 1U << 1,
+	BINDER_DEBUG_DEAD_TRANSACTION       = 1U << 2,
+	BINDER_DEBUG_OPEN_CLOSE             = 1U << 3,
+	BINDER_DEBUG_DEAD_BINDER            = 1U << 4,
+	BINDER_DEBUG_DEATH_NOTIFICATION     = 1U << 5,
+	BINDER_DEBUG_READ_WRITE             = 1U << 6,
+	BINDER_DEBUG_USER_REFS              = 1U << 7,
+	BINDER_DEBUG_THREADS                = 1U << 8,
+	BINDER_DEBUG_TRANSACTION            = 1U << 9,
+	BINDER_DEBUG_TRANSACTION_COMPLETE   = 1U << 10,
+	BINDER_DEBUG_FREE_BUFFER            = 1U << 11,
+	BINDER_DEBUG_INTERNAL_REFS          = 1U << 12,
+	BINDER_DEBUG_BUFFER_ALLOC           = 1U << 13,
+	BINDER_DEBUG_PRIORITY_CAP           = 1U << 14,
+	BINDER_DEBUG_BUFFER_ALLOC_ASYNC     = 1U << 15,
+};
+static uint32_t binder_debug_mask = BINDER_DEBUG_USER_ERROR |
+	BINDER_DEBUG_FAILED_TRANSACTION | BINDER_DEBUG_DEAD_TRANSACTION;
+module_param_named(debug_mask, binder_debug_mask, uint, S_IWUSR | S_IRUGO);
+
+static int binder_debug_no_lock;
+module_param_named(proc_no_lock, binder_debug_no_lock, bool, S_IWUSR | S_IRUGO);
+
+static DECLARE_WAIT_QUEUE_HEAD(binder_user_error_wait);
+static int binder_stop_on_user_error;
+
+static int binder_set_stop_on_user_error(const char *val,
+					 struct kernel_param *kp)
+{
+	int ret;
+	ret = param_set_int(val, kp);
+	if (binder_stop_on_user_error < 2)
+		wake_up(&binder_user_error_wait);
+	return ret;
+}
+module_param_call(stop_on_user_error, binder_set_stop_on_user_error,
+	param_get_int, &binder_stop_on_user_error, S_IWUSR | S_IRUGO);
+
+#define binder_debug(mask, x...) \
+	do { \
+		if (binder_debug_mask & mask) \
+			printk(KERN_INFO x); \
+	} while (0)
+
+#define binder_user_error(x...) \
+	do { \
+		if (binder_debug_mask & BINDER_DEBUG_USER_ERROR) \
+			printk(KERN_INFO x); \
+		if (binder_stop_on_user_error) \
+			binder_stop_on_user_error = 2; \
+	} while (0)
+
+enum binder_stat_types {
+	BINDER_STAT_PROC,
+	BINDER_STAT_THREAD,
+	BINDER_STAT_NODE,
+	BINDER_STAT_REF,
+	BINDER_STAT_DEATH,
+	BINDER_STAT_TRANSACTION,
+	BINDER_STAT_TRANSACTION_COMPLETE,
+	BINDER_STAT_COUNT
+};
+
+struct binder_stats {
+	int br[_IOC_NR(BR_FAILED_REPLY) + 1];
+	int bc[_IOC_NR(BC_DEAD_BINDER_DONE) + 1];
+	int obj_created[BINDER_STAT_COUNT];
+	int obj_deleted[BINDER_STAT_COUNT];
+};
+
+static struct binder_stats binder_stats;
+
+static inline void binder_stats_deleted(enum binder_stat_types type)
+{
+	binder_stats.obj_deleted[type]++;
+}
+
+static inline void binder_stats_created(enum binder_stat_types type)
+{
+	binder_stats.obj_created[type]++;
+}
+
+struct binder_transaction_log_entry {
+	int debug_id;
+	int call_type;
+	int from_proc;
+	int from_thread;
+	int target_handle;
+	int to_proc;
+	int to_thread;
+	int to_node;
+	int data_size;
+	int offsets_size;
+};
+struct binder_transaction_log {
+	int next;
+	int full;
+	struct binder_transaction_log_entry entry[32];
+};
+static struct binder_transaction_log binder_transaction_log;
+static struct binder_transaction_log binder_transaction_log_failed;
+
+static struct binder_transaction_log_entry *binder_transaction_log_add(
+	struct binder_transaction_log *log)
+{
+	struct binder_transaction_log_entry *e;
+	e = &log->entry[log->next];
+	memset(e, 0, sizeof(*e));
+	log->next++;
+	if (log->next == ARRAY_SIZE(log->entry)) {
+		log->next = 0;
+		log->full = 1;
+	}
+	return e;
+}
+
+struct binder_work {
+	struct list_head entry;
+	enum {
+		BINDER_WORK_TRANSACTION = 1,
+		BINDER_WORK_TRANSACTION_COMPLETE,
+		BINDER_WORK_NODE,
+		BINDER_WORK_DEAD_BINDER,
+		BINDER_WORK_DEAD_BINDER_AND_CLEAR,
+		BINDER_WORK_CLEAR_DEATH_NOTIFICATION,
+	} type;
+};
+
+struct binder_node {
+	int debug_id;
+	struct binder_work work;
+	union {
+		struct rb_node rb_node;
+		struct hlist_node dead_node;
+	};
+	struct binder_proc *proc;
+	struct hlist_head refs;
+	int internal_strong_refs;
+	int local_weak_refs;
+	int local_strong_refs;
+	void __user *ptr;
+	void __user *cookie;
+	unsigned has_strong_ref:1;
+	unsigned pending_strong_ref:1;
+	unsigned has_weak_ref:1;
+	unsigned pending_weak_ref:1;
+	unsigned has_async_transaction:1;
+	unsigned accept_fds:1;
+	unsigned min_priority:8;
+	struct list_head async_todo;
+};
+
+struct binder_ref_death {
+	struct binder_work work;
+	void __user *cookie;
+};
+
+struct binder_ref {
+	/* Lookups needed: */
+	/*   node + proc => ref (transaction) */
+	/*   desc + proc => ref (transaction, inc/dec ref) */
+	/*   node => refs + procs (proc exit) */
+	int debug_id;
+	struct rb_node rb_node_desc;
+	struct rb_node rb_node_node;
+	struct hlist_node node_entry;
+	struct binder_proc *proc;
+	struct binder_node *node;
+	uint32_t desc;
+	int strong;
+	int weak;
+	struct binder_ref_death *death;
+};
+
+struct binder_buffer {
+	struct list_head entry; /* free and allocated entries by addesss */
+	struct rb_node rb_node; /* free entry by size or allocated entry */
+				/* by address */
+	unsigned free:1;
+	unsigned allow_user_free:1;
+	unsigned async_transaction:1;
+	unsigned debug_id:29;
+
+	struct binder_transaction *transaction;
+
+	struct binder_node *target_node;
+	size_t data_size;
+	size_t offsets_size;
+	uint8_t data[0];
+};
+
+enum binder_deferred_state {
+	BINDER_DEFERRED_PUT_FILES    = 0x01,
+	BINDER_DEFERRED_FLUSH        = 0x02,
+	BINDER_DEFERRED_RELEASE      = 0x04,
+};
+
+struct binder_proc {
+	struct hlist_node proc_node;
+	struct rb_root threads;
+	struct rb_root nodes;
+	struct rb_root refs_by_desc;
+	struct rb_root refs_by_node;
+	int pid;
+	struct vm_area_struct *vma;
+	struct task_struct *tsk;
+	struct files_struct *files;
+	struct hlist_node deferred_work_node;
+	int deferred_work;
+	void *buffer;
+	ptrdiff_t user_buffer_offset;
+
+	struct list_head buffers;
+	struct rb_root free_buffers;
+	struct rb_root allocated_buffers;
+	size_t free_async_space;
+
+	struct page **pages;
+	size_t buffer_size;
+	uint32_t buffer_free;
+	struct list_head todo;
+	wait_queue_head_t wait;
+	struct binder_stats stats;
+	struct list_head delivered_death;
+	int max_threads;
+	int requested_threads;
+	int requested_threads_started;
+	int ready_threads;
+	long default_priority;
+	struct dentry *debugfs_entry;
+};
+
+enum {
+	BINDER_LOOPER_STATE_REGISTERED  = 0x01,
+	BINDER_LOOPER_STATE_ENTERED     = 0x02,
+	BINDER_LOOPER_STATE_EXITED      = 0x04,
+	BINDER_LOOPER_STATE_INVALID     = 0x08,
+	BINDER_LOOPER_STATE_WAITING     = 0x10,
+	BINDER_LOOPER_STATE_NEED_RETURN = 0x20
+};
+
+struct binder_thread {
+	struct binder_proc *proc;
+	struct rb_node rb_node;
+	int pid;
+	int looper;
+	struct binder_transaction *transaction_stack;
+	struct list_head todo;
+	uint32_t return_error; /* Write failed, return error code in read buf */
+	uint32_t return_error2; /* Write failed, return error code in read */
+		/* buffer. Used when sending a reply to a dead process that */
+		/* we are also waiting on */
+	wait_queue_head_t wait;
+	struct binder_stats stats;
+};
+
+struct binder_transaction {
+	int debug_id;
+	struct binder_work work;
+	struct binder_thread *from;
+	struct binder_transaction *from_parent;
+	struct binder_proc *to_proc;
+	struct binder_thread *to_thread;
+	struct binder_transaction *to_parent;
+	unsigned need_reply:1;
+	/* unsigned is_dead:1; */	/* not used at the moment */
+
+	struct binder_buffer *buffer;
+	unsigned int	code;
+	unsigned int	flags;
+	long	priority;
+	long	saved_priority;
+	uid_t	sender_euid;
+};
+
+static void
+binder_defer_work(struct binder_proc *proc, enum binder_deferred_state defer);
+
+/*
+ * copied from get_unused_fd_flags
+ */
+int task_get_unused_fd_flags(struct binder_proc *proc, int flags)
+{
+	struct files_struct *files = proc->files;
+	int fd, error;
+	struct fdtable *fdt;
+	unsigned long rlim_cur;
+	unsigned long irqs;
+
+	if (files == NULL)
+		return -ESRCH;
+
+	error = -EMFILE;
+	spin_lock(&files->file_lock);
+
+repeat:
+	fdt = files_fdtable(files);
+	fd = find_next_zero_bit(fdt->open_fds->fds_bits, fdt->max_fds,
+				files->next_fd);
+
+	/*
+	 * N.B. For clone tasks sharing a files structure, this test
+	 * will limit the total number of files that can be opened.
+	 */
+	rlim_cur = 0;
+	if (lock_task_sighand(proc->tsk, &irqs)) {
+		rlim_cur = proc->tsk->signal->rlim[RLIMIT_NOFILE].rlim_cur;
+		unlock_task_sighand(proc->tsk, &irqs);
+	}
+	if (fd >= rlim_cur)
+		goto out;
+
+	/* Do we need to expand the fd array or fd set?  */
+	error = expand_files(files, fd);
+	if (error < 0)
+		goto out;
+
+	if (error) {
+		/*
+		 * If we needed to expand the fs array we
+		 * might have blocked - try again.
+		 */
+		error = -EMFILE;
+		goto repeat;
+	}
+
+	FD_SET(fd, fdt->open_fds);
+	if (flags & O_CLOEXEC)
+		FD_SET(fd, fdt->close_on_exec);
+	else
+		FD_CLR(fd, fdt->close_on_exec);
+	files->next_fd = fd + 1;
+#if 1
+	/* Sanity check */
+	if (fdt->fd[fd] != NULL) {
+		printk(KERN_WARNING "get_unused_fd: slot %d not NULL!\n", fd);
+		fdt->fd[fd] = NULL;
+	}
+#endif
+	error = fd;
+
+out:
+	spin_unlock(&files->file_lock);
+	return error;
+}
+
+/*
+ * copied from fd_install
+ */
+static void task_fd_install(
+	struct binder_proc *proc, unsigned int fd, struct file *file)
+{
+	struct files_struct *files = proc->files;
+	struct fdtable *fdt;
+
+	if (files == NULL)
+		return;
+
+	spin_lock(&files->file_lock);
+	fdt = files_fdtable(files);
+	BUG_ON(fdt->fd[fd] != NULL);
+	rcu_assign_pointer(fdt->fd[fd], file);
+	spin_unlock(&files->file_lock);
+}
+
+/*
+ * copied from __put_unused_fd in open.c
+ */
+static void __put_unused_fd(struct files_struct *files, unsigned int fd)
+{
+	struct fdtable *fdt = files_fdtable(files);
+	__FD_CLR(fd, fdt->open_fds);
+	if (fd < files->next_fd)
+		files->next_fd = fd;
+}
+
+/*
+ * copied from sys_close
+ */
+static long task_close_fd(struct binder_proc *proc, unsigned int fd)
+{
+	struct file *filp;
+	struct files_struct *files = proc->files;
+	struct fdtable *fdt;
+	int retval;
+
+	if (files == NULL)
+		return -ESRCH;
+
+	spin_lock(&files->file_lock);
+	fdt = files_fdtable(files);
+	if (fd >= fdt->max_fds)
+		goto out_unlock;
+	filp = fdt->fd[fd];
+	if (!filp)
+		goto out_unlock;
+	rcu_assign_pointer(fdt->fd[fd], NULL);
+	FD_CLR(fd, fdt->close_on_exec);
+	__put_unused_fd(files, fd);
+	spin_unlock(&files->file_lock);
+	retval = filp_close(filp, files);
+
+	/* can't restart close syscall because file table entry was cleared */
+	if (unlikely(retval == -ERESTARTSYS ||
+		     retval == -ERESTARTNOINTR ||
+		     retval == -ERESTARTNOHAND ||
+		     retval == -ERESTART_RESTARTBLOCK))
+		retval = -EINTR;
+
+	return retval;
+
+out_unlock:
+	spin_unlock(&files->file_lock);
+	return -EBADF;
+}
+
+static void binder_set_nice(long nice)
+{
+	long min_nice;
+	if (can_nice(current, nice)) {
+		set_user_nice(current, nice);
+		return;
+	}
+	min_nice = 20 - current->signal->rlim[RLIMIT_NICE].rlim_cur;
+	binder_debug(BINDER_DEBUG_PRIORITY_CAP,
+		     "binder: %d: nice value %ld not allowed use "
+		     "%ld instead\n", current->pid, nice, min_nice);
+	set_user_nice(current, min_nice);
+	if (min_nice < 20)
+		return;
+	binder_user_error("binder: %d RLIMIT_NICE not set\n", current->pid);
+}
+
+static size_t binder_buffer_size(struct binder_proc *proc,
+				 struct binder_buffer *buffer)
+{
+	if (list_is_last(&buffer->entry, &proc->buffers))
+		return proc->buffer + proc->buffer_size - (void *)buffer->data;
+	else
+		return (size_t)list_entry(buffer->entry.next,
+			struct binder_buffer, entry) - (size_t)buffer->data;
+}
+
+static void binder_insert_free_buffer(struct binder_proc *proc,
+				      struct binder_buffer *new_buffer)
+{
+	struct rb_node **p = &proc->free_buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_buffer *buffer;
+	size_t buffer_size;
+	size_t new_buffer_size;
+
+	BUG_ON(!new_buffer->free);
+
+	new_buffer_size = binder_buffer_size(proc, new_buffer);
+
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: add free buffer, size %zd, "
+		     "at %p\n", proc->pid, new_buffer_size, new_buffer);
+
+	while (*p) {
+		parent = *p;
+		buffer = rb_entry(parent, struct binder_buffer, rb_node);
+		BUG_ON(!buffer->free);
+
+		buffer_size = binder_buffer_size(proc, buffer);
+
+		if (new_buffer_size < buffer_size)
+			p = &parent->rb_left;
+		else
+			p = &parent->rb_right;
+	}
+	rb_link_node(&new_buffer->rb_node, parent, p);
+	rb_insert_color(&new_buffer->rb_node, &proc->free_buffers);
+}
+
+static void binder_insert_allocated_buffer(struct binder_proc *proc,
+					   struct binder_buffer *new_buffer)
+{
+	struct rb_node **p = &proc->allocated_buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_buffer *buffer;
+
+	BUG_ON(new_buffer->free);
+
+	while (*p) {
+		parent = *p;
+		buffer = rb_entry(parent, struct binder_buffer, rb_node);
+		BUG_ON(buffer->free);
+
+		if (new_buffer < buffer)
+			p = &parent->rb_left;
+		else if (new_buffer > buffer)
+			p = &parent->rb_right;
+		else
+			BUG();
+	}
+	rb_link_node(&new_buffer->rb_node, parent, p);
+	rb_insert_color(&new_buffer->rb_node, &proc->allocated_buffers);
+}
+
+static struct binder_buffer *binder_buffer_lookup(struct binder_proc *proc,
+						  void __user *user_ptr)
+{
+	struct rb_node *n = proc->allocated_buffers.rb_node;
+	struct binder_buffer *buffer;
+	struct binder_buffer *kern_ptr;
+
+	kern_ptr = user_ptr - proc->user_buffer_offset
+		- offsetof(struct binder_buffer, data);
+
+	while (n) {
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+		BUG_ON(buffer->free);
+
+		if (kern_ptr < buffer)
+			n = n->rb_left;
+		else if (kern_ptr > buffer)
+			n = n->rb_right;
+		else
+			return buffer;
+	}
+	return NULL;
+}
+
+static int binder_update_page_range(struct binder_proc *proc, int allocate,
+				    void *start, void *end,
+				    struct vm_area_struct *vma)
+{
+	void *page_addr;
+	unsigned long user_page_addr;
+	struct vm_struct tmp_area;
+	struct page **page;
+	struct mm_struct *mm;
+
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: %s pages %p-%p\n", proc->pid,
+		     allocate ? "allocate" : "free", start, end);
+
+	if (end <= start)
+		return 0;
+
+	if (vma)
+		mm = NULL;
+	else
+		mm = get_task_mm(proc->tsk);
+
+	if (mm) {
+		down_write(&mm->mmap_sem);
+		vma = proc->vma;
+	}
+
+	if (allocate == 0)
+		goto free_range;
+
+	if (vma == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf failed to "
+		       "map pages in userspace, no vma\n", proc->pid);
+		goto err_no_vma;
+	}
+
+	for (page_addr = start; page_addr < end; page_addr += PAGE_SIZE) {
+		int ret;
+		struct page **page_array_ptr;
+		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
+
+		BUG_ON(*page);
+		*page = alloc_page(GFP_KERNEL | __GFP_ZERO);
+		if (*page == NULL) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "for page at %p\n", proc->pid, page_addr);
+			goto err_alloc_page_failed;
+		}
+		tmp_area.addr = page_addr;
+		tmp_area.size = PAGE_SIZE + PAGE_SIZE /* guard page? */;
+		page_array_ptr = page;
+		ret = map_vm_area(&tmp_area, PAGE_KERNEL, &page_array_ptr);
+		if (ret) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "to map page at %p in kernel\n",
+			       proc->pid, page_addr);
+			goto err_map_kernel_failed;
+		}
+		user_page_addr =
+			(uintptr_t)page_addr + proc->user_buffer_offset;
+		ret = vm_insert_page(vma, user_page_addr, page[0]);
+		if (ret) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "to map page at %lx in userspace\n",
+			       proc->pid, user_page_addr);
+			goto err_vm_insert_page_failed;
+		}
+		/* vm_insert_page does not seem to increment the refcount */
+	}
+	if (mm) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+	return 0;
+
+free_range:
+	for (page_addr = end - PAGE_SIZE; page_addr >= start;
+	     page_addr -= PAGE_SIZE) {
+		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
+		if (vma)
+			zap_page_range(vma, (uintptr_t)page_addr +
+				proc->user_buffer_offset, PAGE_SIZE, NULL);
+err_vm_insert_page_failed:
+		unmap_kernel_range((unsigned long)page_addr, PAGE_SIZE);
+err_map_kernel_failed:
+		__free_page(*page);
+		*page = NULL;
+err_alloc_page_failed:
+		;
+	}
+err_no_vma:
+	if (mm) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+	return -ENOMEM;
+}
+
+static struct binder_buffer *binder_alloc_buf(struct binder_proc *proc,
+					      size_t data_size,
+					      size_t offsets_size, int is_async)
+{
+	struct rb_node *n = proc->free_buffers.rb_node;
+	struct binder_buffer *buffer;
+	size_t buffer_size;
+	struct rb_node *best_fit = NULL;
+	void *has_page_addr;
+	void *end_page_addr;
+	size_t size;
+
+	if (proc->vma == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf, no vma\n",
+		       proc->pid);
+		return NULL;
+	}
+
+	size = ALIGN(data_size, sizeof(void *)) +
+		ALIGN(offsets_size, sizeof(void *));
+
+	if (size < data_size || size < offsets_size) {
+		binder_user_error("binder: %d: got transaction with invalid "
+			"size %zd-%zd\n", proc->pid, data_size, offsets_size);
+		return NULL;
+	}
+
+	if (is_async &&
+	    proc->free_async_space < size + sizeof(struct binder_buffer)) {
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+			     "binder: %d: binder_alloc_buf size %zd"
+			     "failed, no async space left\n", proc->pid, size);
+		return NULL;
+	}
+
+	while (n) {
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+		BUG_ON(!buffer->free);
+		buffer_size = binder_buffer_size(proc, buffer);
+
+		if (size < buffer_size) {
+			best_fit = n;
+			n = n->rb_left;
+		} else if (size > buffer_size)
+			n = n->rb_right;
+		else {
+			best_fit = n;
+			break;
+		}
+	}
+	if (best_fit == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf size %zd failed, "
+		       "no address space\n", proc->pid, size);
+		return NULL;
+	}
+	if (n == NULL) {
+		buffer = rb_entry(best_fit, struct binder_buffer, rb_node);
+		buffer_size = binder_buffer_size(proc, buffer);
+	}
+
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: binder_alloc_buf size %zd got buff"
+		     "er %p size %zd\n", proc->pid, size, buffer, buffer_size);
+
+	has_page_addr =
+		(void *)(((uintptr_t)buffer->data + buffer_size) & PAGE_MASK);
+	if (n == NULL) {
+		if (size + sizeof(struct binder_buffer) + 4 >= buffer_size)
+			buffer_size = size; /* no room for other buffers */
+		else
+			buffer_size = size + sizeof(struct binder_buffer);
+	}
+	end_page_addr =
+		(void *)PAGE_ALIGN((uintptr_t)buffer->data + buffer_size);
+	if (end_page_addr > has_page_addr)
+		end_page_addr = has_page_addr;
+	if (binder_update_page_range(proc, 1,
+	    (void *)PAGE_ALIGN((uintptr_t)buffer->data), end_page_addr, NULL))
+		return NULL;
+
+	rb_erase(best_fit, &proc->free_buffers);
+	buffer->free = 0;
+	binder_insert_allocated_buffer(proc, buffer);
+	if (buffer_size != size) {
+		struct binder_buffer *new_buffer = (void *)buffer->data + size;
+		list_add(&new_buffer->entry, &buffer->entry);
+		new_buffer->free = 1;
+		binder_insert_free_buffer(proc, new_buffer);
+	}
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: binder_alloc_buf size %zd got "
+		     "%p\n", proc->pid, size, buffer);
+	buffer->data_size = data_size;
+	buffer->offsets_size = offsets_size;
+	buffer->async_transaction = is_async;
+	if (is_async) {
+		proc->free_async_space -= size + sizeof(struct binder_buffer);
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC_ASYNC,
+			     "binder: %d: binder_alloc_buf size %zd "
+			     "async free %zd\n", proc->pid, size,
+			     proc->free_async_space);
+	}
+
+	return buffer;
+}
+
+static void *buffer_start_page(struct binder_buffer *buffer)
+{
+	return (void *)((uintptr_t)buffer & PAGE_MASK);
+}
+
+static void *buffer_end_page(struct binder_buffer *buffer)
+{
+	return (void *)(((uintptr_t)(buffer + 1) - 1) & PAGE_MASK);
+}
+
+static void binder_delete_free_buffer(struct binder_proc *proc,
+				      struct binder_buffer *buffer)
+{
+	struct binder_buffer *prev, *next = NULL;
+	int free_page_end = 1;
+	int free_page_start = 1;
+
+	BUG_ON(proc->buffers.next == &buffer->entry);
+	prev = list_entry(buffer->entry.prev, struct binder_buffer, entry);
+	BUG_ON(!prev->free);
+	if (buffer_end_page(prev) == buffer_start_page(buffer)) {
+		free_page_start = 0;
+		if (buffer_end_page(prev) == buffer_end_page(buffer))
+			free_page_end = 0;
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+			     "binder: %d: merge free, buffer %p "
+			     "share page with %p\n", proc->pid, buffer, prev);
+	}
+
+	if (!list_is_last(&buffer->entry, &proc->buffers)) {
+		next = list_entry(buffer->entry.next,
+				  struct binder_buffer, entry);
+		if (buffer_start_page(next) == buffer_end_page(buffer)) {
+			free_page_end = 0;
+			if (buffer_start_page(next) ==
+			    buffer_start_page(buffer))
+				free_page_start = 0;
+			binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+				     "binder: %d: merge free, buffer"
+				     " %p share page with %p\n", proc->pid,
+				     buffer, prev);
+		}
+	}
+	list_del(&buffer->entry);
+	if (free_page_start || free_page_end) {
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+			     "binder: %d: merge free, buffer %p do "
+			     "not share page%s%s with with %p or %p\n",
+			     proc->pid, buffer, free_page_start ? "" : " end",
+			     free_page_end ? "" : " start", prev, next);
+		binder_update_page_range(proc, 0, free_page_start ?
+			buffer_start_page(buffer) : buffer_end_page(buffer),
+			(free_page_end ? buffer_end_page(buffer) :
+			buffer_start_page(buffer)) + PAGE_SIZE, NULL);
+	}
+}
+
+static void binder_free_buf(struct binder_proc *proc,
+			    struct binder_buffer *buffer)
+{
+	size_t size, buffer_size;
+
+	buffer_size = binder_buffer_size(proc, buffer);
+
+	size = ALIGN(buffer->data_size, sizeof(void *)) +
+		ALIGN(buffer->offsets_size, sizeof(void *));
+
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: binder_free_buf %p size %zd buffer"
+		     "_size %zd\n", proc->pid, buffer, size, buffer_size);
+
+	BUG_ON(buffer->free);
+	BUG_ON(size > buffer_size);
+	BUG_ON(buffer->transaction != NULL);
+	BUG_ON((void *)buffer < proc->buffer);
+	BUG_ON((void *)buffer > proc->buffer + proc->buffer_size);
+
+	if (buffer->async_transaction) {
+		proc->free_async_space += size + sizeof(struct binder_buffer);
+
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC_ASYNC,
+			     "binder: %d: binder_free_buf size %zd "
+			     "async free %zd\n", proc->pid, size,
+			     proc->free_async_space);
+	}
+
+	binder_update_page_range(proc, 0,
+		(void *)PAGE_ALIGN((uintptr_t)buffer->data),
+		(void *)(((uintptr_t)buffer->data + buffer_size) & PAGE_MASK),
+		NULL);
+	rb_erase(&buffer->rb_node, &proc->allocated_buffers);
+	buffer->free = 1;
+	if (!list_is_last(&buffer->entry, &proc->buffers)) {
+		struct binder_buffer *next = list_entry(buffer->entry.next,
+						struct binder_buffer, entry);
+		if (next->free) {
+			rb_erase(&next->rb_node, &proc->free_buffers);
+			binder_delete_free_buffer(proc, next);
+		}
+	}
+	if (proc->buffers.next != &buffer->entry) {
+		struct binder_buffer *prev = list_entry(buffer->entry.prev,
+						struct binder_buffer, entry);
+		if (prev->free) {
+			binder_delete_free_buffer(proc, buffer);
+			rb_erase(&prev->rb_node, &proc->free_buffers);
+			buffer = prev;
+		}
+	}
+	binder_insert_free_buffer(proc, buffer);
+}
+
+static struct binder_node *binder_get_node(struct binder_proc *proc,
+					   void __user *ptr)
+{
+	struct rb_node *n = proc->nodes.rb_node;
+	struct binder_node *node;
+
+	while (n) {
+		node = rb_entry(n, struct binder_node, rb_node);
+
+		if (ptr < node->ptr)
+			n = n->rb_left;
+		else if (ptr > node->ptr)
+			n = n->rb_right;
+		else
+			return node;
+	}
+	return NULL;
+}
+
+static struct binder_node *binder_new_node(struct binder_proc *proc,
+					   void __user *ptr,
+					   void __user *cookie)
+{
+	struct rb_node **p = &proc->nodes.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_node *node;
+
+	while (*p) {
+		parent = *p;
+		node = rb_entry(parent, struct binder_node, rb_node);
+
+		if (ptr < node->ptr)
+			p = &(*p)->rb_left;
+		else if (ptr > node->ptr)
+			p = &(*p)->rb_right;
+		else
+			return NULL;
+	}
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (node == NULL)
+		return NULL;
+	binder_stats_created(BINDER_STAT_NODE);
+	rb_link_node(&node->rb_node, parent, p);
+	rb_insert_color(&node->rb_node, &proc->nodes);
+	node->debug_id = ++binder_last_id;
+	node->proc = proc;
+	node->ptr = ptr;
+	node->cookie = cookie;
+	node->work.type = BINDER_WORK_NODE;
+	INIT_LIST_HEAD(&node->work.entry);
+	INIT_LIST_HEAD(&node->async_todo);
+	binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+		     "binder: %d:%d node %d u%p c%p created\n",
+		     proc->pid, current->pid, node->debug_id,
+		     node->ptr, node->cookie);
+	return node;
+}
+
+static int binder_inc_node(struct binder_node *node, int strong, int internal,
+			   struct list_head *target_list)
+{
+	if (strong) {
+		if (internal) {
+			if (target_list == NULL &&
+			    node->internal_strong_refs == 0 &&
+			    !(node == binder_context_mgr_node &&
+			    node->has_strong_ref)) {
+				printk(KERN_ERR "binder: invalid inc strong "
+					"node for %d\n", node->debug_id);
+				return -EINVAL;
+			}
+			node->internal_strong_refs++;
+		} else
+			node->local_strong_refs++;
+		if (!node->has_strong_ref && target_list) {
+			list_del_init(&node->work.entry);
+			list_add_tail(&node->work.entry, target_list);
+		}
+	} else {
+		if (!internal)
+			node->local_weak_refs++;
+		if (!node->has_weak_ref && list_empty(&node->work.entry)) {
+			if (target_list == NULL) {
+				printk(KERN_ERR "binder: invalid inc weak node "
+					"for %d\n", node->debug_id);
+				return -EINVAL;
+			}
+			list_add_tail(&node->work.entry, target_list);
+		}
+	}
+	return 0;
+}
+
+static int binder_dec_node(struct binder_node *node, int strong, int internal)
+{
+	if (strong) {
+		if (internal)
+			node->internal_strong_refs--;
+		else
+			node->local_strong_refs--;
+		if (node->local_strong_refs || node->internal_strong_refs)
+			return 0;
+	} else {
+		if (!internal)
+			node->local_weak_refs--;
+		if (node->local_weak_refs || !hlist_empty(&node->refs))
+			return 0;
+	}
+	if (node->proc && (node->has_strong_ref || node->has_weak_ref)) {
+		if (list_empty(&node->work.entry)) {
+			list_add_tail(&node->work.entry, &node->proc->todo);
+			wake_up_interruptible(&node->proc->wait);
+		}
+	} else {
+		if (hlist_empty(&node->refs) && !node->local_strong_refs &&
+		    !node->local_weak_refs) {
+			list_del_init(&node->work.entry);
+			if (node->proc) {
+				rb_erase(&node->rb_node, &node->proc->nodes);
+				binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+					     "binder: refless node %d deleted\n",
+					     node->debug_id);
+			} else {
+				hlist_del(&node->dead_node);
+				binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+					     "binder: dead node %d deleted\n",
+					     node->debug_id);
+			}
+			kfree(node);
+			binder_stats_deleted(BINDER_STAT_NODE);
+		}
+	}
+
+	return 0;
+}
+
+
+static struct binder_ref *binder_get_ref(struct binder_proc *proc,
+					 uint32_t desc)
+{
+	struct rb_node *n = proc->refs_by_desc.rb_node;
+	struct binder_ref *ref;
+
+	while (n) {
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
+
+		if (desc < ref->desc)
+			n = n->rb_left;
+		else if (desc > ref->desc)
+			n = n->rb_right;
+		else
+			return ref;
+	}
+	return NULL;
+}
+
+static struct binder_ref *binder_get_ref_for_node(struct binder_proc *proc,
+						  struct binder_node *node)
+{
+	struct rb_node *n;
+	struct rb_node **p = &proc->refs_by_node.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_ref *ref, *new_ref;
+
+	while (*p) {
+		parent = *p;
+		ref = rb_entry(parent, struct binder_ref, rb_node_node);
+
+		if (node < ref->node)
+			p = &(*p)->rb_left;
+		else if (node > ref->node)
+			p = &(*p)->rb_right;
+		else
+			return ref;
+	}
+	new_ref = kzalloc(sizeof(*ref), GFP_KERNEL);
+	if (new_ref == NULL)
+		return NULL;
+	binder_stats_created(BINDER_STAT_REF);
+	new_ref->debug_id = ++binder_last_id;
+	new_ref->proc = proc;
+	new_ref->node = node;
+	rb_link_node(&new_ref->rb_node_node, parent, p);
+	rb_insert_color(&new_ref->rb_node_node, &proc->refs_by_node);
+
+	new_ref->desc = (node == binder_context_mgr_node) ? 0 : 1;
+	for (n = rb_first(&proc->refs_by_desc); n != NULL; n = rb_next(n)) {
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
+		if (ref->desc > new_ref->desc)
+			break;
+		new_ref->desc = ref->desc + 1;
+	}
+
+	p = &proc->refs_by_desc.rb_node;
+	while (*p) {
+		parent = *p;
+		ref = rb_entry(parent, struct binder_ref, rb_node_desc);
+
+		if (new_ref->desc < ref->desc)
+			p = &(*p)->rb_left;
+		else if (new_ref->desc > ref->desc)
+			p = &(*p)->rb_right;
+		else
+			BUG();
+	}
+	rb_link_node(&new_ref->rb_node_desc, parent, p);
+	rb_insert_color(&new_ref->rb_node_desc, &proc->refs_by_desc);
+	if (node) {
+		hlist_add_head(&new_ref->node_entry, &node->refs);
+
+		binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+			     "binder: %d new ref %d desc %d for "
+			     "node %d\n", proc->pid, new_ref->debug_id,
+			     new_ref->desc, node->debug_id);
+	} else {
+		binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+			     "binder: %d new ref %d desc %d for "
+			     "dead node\n", proc->pid, new_ref->debug_id,
+			      new_ref->desc);
+	}
+	return new_ref;
+}
+
+static void binder_delete_ref(struct binder_ref *ref)
+{
+	binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+		     "binder: %d delete ref %d desc %d for "
+		     "node %d\n", ref->proc->pid, ref->debug_id,
+		     ref->desc, ref->node->debug_id);
+
+	rb_erase(&ref->rb_node_desc, &ref->proc->refs_by_desc);
+	rb_erase(&ref->rb_node_node, &ref->proc->refs_by_node);
+	if (ref->strong)
+		binder_dec_node(ref->node, 1, 1);
+	hlist_del(&ref->node_entry);
+	binder_dec_node(ref->node, 0, 1);
+	if (ref->death) {
+		binder_debug(BINDER_DEBUG_DEAD_BINDER,
+			     "binder: %d delete ref %d desc %d "
+			     "has death notification\n", ref->proc->pid,
+			     ref->debug_id, ref->desc);
+		list_del(&ref->death->work.entry);
+		kfree(ref->death);
+		binder_stats_deleted(BINDER_STAT_DEATH);
+	}
+	kfree(ref);
+	binder_stats_deleted(BINDER_STAT_REF);
+}
+
+static int binder_inc_ref(struct binder_ref *ref, int strong,
+			  struct list_head *target_list)
+{
+	int ret;
+	if (strong) {
+		if (ref->strong == 0) {
+			ret = binder_inc_node(ref->node, 1, 1, target_list);
+			if (ret)
+				return ret;
+		}
+		ref->strong++;
+	} else {
+		if (ref->weak == 0) {
+			ret = binder_inc_node(ref->node, 0, 1, target_list);
+			if (ret)
+				return ret;
+		}
+		ref->weak++;
+	}
+	return 0;
+}
+
+
+static int binder_dec_ref(struct binder_ref *ref, int strong)
+{
+	if (strong) {
+		if (ref->strong == 0) {
+			binder_user_error("binder: %d invalid dec strong, "
+					  "ref %d desc %d s %d w %d\n",
+					  ref->proc->pid, ref->debug_id,
+					  ref->desc, ref->strong, ref->weak);
+			return -EINVAL;
+		}
+		ref->strong--;
+		if (ref->strong == 0) {
+			int ret;
+			ret = binder_dec_node(ref->node, strong, 1);
+			if (ret)
+				return ret;
+		}
+	} else {
+		if (ref->weak == 0) {
+			binder_user_error("binder: %d invalid dec weak, "
+					  "ref %d desc %d s %d w %d\n",
+					  ref->proc->pid, ref->debug_id,
+					  ref->desc, ref->strong, ref->weak);
+			return -EINVAL;
+		}
+		ref->weak--;
+	}
+	if (ref->strong == 0 && ref->weak == 0)
+		binder_delete_ref(ref);
+	return 0;
+}
+
+static void binder_pop_transaction(struct binder_thread *target_thread,
+				   struct binder_transaction *t)
+{
+	if (target_thread) {
+		BUG_ON(target_thread->transaction_stack != t);
+		BUG_ON(target_thread->transaction_stack->from != target_thread);
+		target_thread->transaction_stack =
+			target_thread->transaction_stack->from_parent;
+		t->from = NULL;
+	}
+	t->need_reply = 0;
+	if (t->buffer)
+		t->buffer->transaction = NULL;
+	kfree(t);
+	binder_stats_deleted(BINDER_STAT_TRANSACTION);
+}
+
+static void binder_send_failed_reply(struct binder_transaction *t,
+				     uint32_t error_code)
+{
+	struct binder_thread *target_thread;
+	BUG_ON(t->flags & TF_ONE_WAY);
+	while (1) {
+		target_thread = t->from;
+		if (target_thread) {
+			if (target_thread->return_error != BR_OK &&
+			   target_thread->return_error2 == BR_OK) {
+				target_thread->return_error2 =
+					target_thread->return_error;
+				target_thread->return_error = BR_OK;
+			}
+			if (target_thread->return_error == BR_OK) {
+				binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
+					     "binder: send failed reply for "
+					     "transaction %d to %d:%d\n",
+					      t->debug_id, target_thread->proc->pid,
+					      target_thread->pid);
+
+				binder_pop_transaction(target_thread, t);
+				target_thread->return_error = error_code;
+				wake_up_interruptible(&target_thread->wait);
+			} else {
+				printk(KERN_ERR "binder: reply failed, target "
+					"thread, %d:%d, has error code %d "
+					"already\n", target_thread->proc->pid,
+					target_thread->pid,
+					target_thread->return_error);
+			}
+			return;
+		} else {
+			struct binder_transaction *next = t->from_parent;
+
+			binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
+				     "binder: send failed reply "
+				     "for transaction %d, target dead\n",
+				     t->debug_id);
+
+			binder_pop_transaction(target_thread, t);
+			if (next == NULL) {
+				binder_debug(BINDER_DEBUG_DEAD_BINDER,
+					     "binder: reply failed,"
+					     " no target thread at root\n");
+				return;
+			}
+			t = next;
+			binder_debug(BINDER_DEBUG_DEAD_BINDER,
+				     "binder: reply failed, no target "
+				     "thread -- retry %d\n", t->debug_id);
+		}
+	}
+}
+
+static void binder_transaction_buffer_release(struct binder_proc *proc,
+					      struct binder_buffer *buffer,
+					      size_t *failed_at)
+{
+	size_t *offp, *off_end;
+	int debug_id = buffer->debug_id;
+
+	binder_debug(BINDER_DEBUG_TRANSACTION,
+		     "binder: %d buffer release %d, size %zd-%zd, failed at %p\n",
+		     proc->pid, buffer->debug_id,
+		     buffer->data_size, buffer->offsets_size, failed_at);
+
+	if (buffer->target_node)
+		binder_dec_node(buffer->target_node, 1, 0);
+
+	offp = (size_t *)(buffer->data + ALIGN(buffer->data_size, sizeof(void *)));
+	if (failed_at)
+		off_end = failed_at;
+	else
+		off_end = (void *)offp + buffer->offsets_size;
+	for (; offp < off_end; offp++) {
+		struct flat_binder_object *fp;
+		if (*offp > buffer->data_size - sizeof(*fp) ||
+		    buffer->data_size < sizeof(*fp) ||
+		    !IS_ALIGNED(*offp, sizeof(void *))) {
+			printk(KERN_ERR "binder: transaction release %d bad"
+					"offset %zd, size %zd\n", debug_id,
+					*offp, buffer->data_size);
+			continue;
+		}
+		fp = (struct flat_binder_object *)(buffer->data + *offp);
+		switch (fp->type) {
+		case BINDER_TYPE_BINDER:
+		case BINDER_TYPE_WEAK_BINDER: {
+			struct binder_node *node = binder_get_node(proc, fp->binder);
+			if (node == NULL) {
+				printk(KERN_ERR "binder: transaction release %d"
+				       " bad node %p\n", debug_id, fp->binder);
+				break;
+			}
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        node %d u%p\n",
+				     node->debug_id, node->ptr);
+			binder_dec_node(node, fp->type == BINDER_TYPE_BINDER, 0);
+		} break;
+		case BINDER_TYPE_HANDLE:
+		case BINDER_TYPE_WEAK_HANDLE: {
+			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+			if (ref == NULL) {
+				printk(KERN_ERR "binder: transaction release %d"
+				       " bad handle %ld\n", debug_id,
+				       fp->handle);
+				break;
+			}
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        ref %d desc %d (node %d)\n",
+				     ref->debug_id, ref->desc, ref->node->debug_id);
+			binder_dec_ref(ref, fp->type == BINDER_TYPE_HANDLE);
+		} break;
+
+		case BINDER_TYPE_FD:
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        fd %ld\n", fp->handle);
+			if (failed_at)
+				task_close_fd(proc, fp->handle);
+			break;
+
+		default:
+			printk(KERN_ERR "binder: transaction release %d bad "
+			       "object type %lx\n", debug_id, fp->type);
+			break;
+		}
+	}
+}
+
+static void binder_transaction(struct binder_proc *proc,
+			       struct binder_thread *thread,
+			       struct binder_transaction_data *tr, int reply)
+{
+	struct binder_transaction *t;
+	struct binder_work *tcomplete;
+	size_t *offp, *off_end;
+	struct binder_proc *target_proc;
+	struct binder_thread *target_thread = NULL;
+	struct binder_node *target_node = NULL;
+	struct list_head *target_list;
+	wait_queue_head_t *target_wait;
+	struct binder_transaction *in_reply_to = NULL;
+	struct binder_transaction_log_entry *e;
+	uint32_t return_error;
+
+	e = binder_transaction_log_add(&binder_transaction_log);
+	e->call_type = reply ? 2 : !!(tr->flags & TF_ONE_WAY);
+	e->from_proc = proc->pid;
+	e->from_thread = thread->pid;
+	e->target_handle = tr->target.handle;
+	e->data_size = tr->data_size;
+	e->offsets_size = tr->offsets_size;
+
+	if (reply) {
+		in_reply_to = thread->transaction_stack;
+		if (in_reply_to == NULL) {
+			binder_user_error("binder: %d:%d got reply transaction "
+					  "with no transaction stack\n",
+					  proc->pid, thread->pid);
+			return_error = BR_FAILED_REPLY;
+			goto err_empty_call_stack;
+		}
+		binder_set_nice(in_reply_to->saved_priority);
+		if (in_reply_to->to_thread != thread) {
+			binder_user_error("binder: %d:%d got reply transaction "
+				"with bad transaction stack,"
+				" transaction %d has target %d:%d\n",
+				proc->pid, thread->pid, in_reply_to->debug_id,
+				in_reply_to->to_proc ?
+				in_reply_to->to_proc->pid : 0,
+				in_reply_to->to_thread ?
+				in_reply_to->to_thread->pid : 0);
+			return_error = BR_FAILED_REPLY;
+			in_reply_to = NULL;
+			goto err_bad_call_stack;
+		}
+		thread->transaction_stack = in_reply_to->to_parent;
+		target_thread = in_reply_to->from;
+		if (target_thread == NULL) {
+			return_error = BR_DEAD_REPLY;
+			goto err_dead_binder;
+		}
+		if (target_thread->transaction_stack != in_reply_to) {
+			binder_user_error("binder: %d:%d got reply transaction "
+				"with bad target transaction stack %d, "
+				"expected %d\n",
+				proc->pid, thread->pid,
+				target_thread->transaction_stack ?
+				target_thread->transaction_stack->debug_id : 0,
+				in_reply_to->debug_id);
+			return_error = BR_FAILED_REPLY;
+			in_reply_to = NULL;
+			target_thread = NULL;
+			goto err_dead_binder;
+		}
+		target_proc = target_thread->proc;
+	} else {
+		if (tr->target.handle) {
+			struct binder_ref *ref;
+			ref = binder_get_ref(proc, tr->target.handle);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d got "
+					"transaction to invalid handle\n",
+					proc->pid, thread->pid);
+				return_error = BR_FAILED_REPLY;
+				goto err_invalid_target_handle;
+			}
+			target_node = ref->node;
+		} else {
+			target_node = binder_context_mgr_node;
+			if (target_node == NULL) {
+				return_error = BR_DEAD_REPLY;
+				goto err_no_context_mgr_node;
+			}
+		}
+		e->to_node = target_node->debug_id;
+		target_proc = target_node->proc;
+		if (target_proc == NULL) {
+			return_error = BR_DEAD_REPLY;
+			goto err_dead_binder;
+		}
+		if (!(tr->flags & TF_ONE_WAY) && thread->transaction_stack) {
+			struct binder_transaction *tmp;
+			tmp = thread->transaction_stack;
+			if (tmp->to_thread != thread) {
+				binder_user_error("binder: %d:%d got new "
+					"transaction with bad transaction stack"
+					", transaction %d has target %d:%d\n",
+					proc->pid, thread->pid, tmp->debug_id,
+					tmp->to_proc ? tmp->to_proc->pid : 0,
+					tmp->to_thread ?
+					tmp->to_thread->pid : 0);
+				return_error = BR_FAILED_REPLY;
+				goto err_bad_call_stack;
+			}
+			while (tmp) {
+				if (tmp->from && tmp->from->proc == target_proc)
+					target_thread = tmp->from;
+				tmp = tmp->from_parent;
+			}
+		}
+	}
+	if (target_thread) {
+		e->to_thread = target_thread->pid;
+		target_list = &target_thread->todo;
+		target_wait = &target_thread->wait;
+	} else {
+		target_list = &target_proc->todo;
+		target_wait = &target_proc->wait;
+	}
+	e->to_proc = target_proc->pid;
+
+	/* TODO: reuse incoming transaction for reply */
+	t = kzalloc(sizeof(*t), GFP_KERNEL);
+	if (t == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_alloc_t_failed;
+	}
+	binder_stats_created(BINDER_STAT_TRANSACTION);
+
+	tcomplete = kzalloc(sizeof(*tcomplete), GFP_KERNEL);
+	if (tcomplete == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_alloc_tcomplete_failed;
+	}
+	binder_stats_created(BINDER_STAT_TRANSACTION_COMPLETE);
+
+	t->debug_id = ++binder_last_id;
+	e->debug_id = t->debug_id;
+
+	if (reply)
+		binder_debug(BINDER_DEBUG_TRANSACTION,
+			     "binder: %d:%d BC_REPLY %d -> %d:%d, "
+			     "data %p-%p size %zd-%zd\n",
+			     proc->pid, thread->pid, t->debug_id,
+			     target_proc->pid, target_thread->pid,
+			     tr->data.ptr.buffer, tr->data.ptr.offsets,
+			     tr->data_size, tr->offsets_size);
+	else
+		binder_debug(BINDER_DEBUG_TRANSACTION,
+			     "binder: %d:%d BC_TRANSACTION %d -> "
+			     "%d - node %d, data %p-%p size %zd-%zd\n",
+			     proc->pid, thread->pid, t->debug_id,
+			     target_proc->pid, target_node->debug_id,
+			     tr->data.ptr.buffer, tr->data.ptr.offsets,
+			     tr->data_size, tr->offsets_size);
+
+	if (!reply && !(tr->flags & TF_ONE_WAY))
+		t->from = thread;
+	else
+		t->from = NULL;
+	t->sender_euid = proc->tsk->cred->euid;
+	t->to_proc = target_proc;
+	t->to_thread = target_thread;
+	t->code = tr->code;
+	t->flags = tr->flags;
+	t->priority = task_nice(current);
+	t->buffer = binder_alloc_buf(target_proc, tr->data_size,
+		tr->offsets_size, !reply && (t->flags & TF_ONE_WAY));
+	if (t->buffer == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_binder_alloc_buf_failed;
+	}
+	t->buffer->allow_user_free = 0;
+	t->buffer->debug_id = t->debug_id;
+	t->buffer->transaction = t;
+	t->buffer->target_node = target_node;
+	if (target_node)
+		binder_inc_node(target_node, 1, 0, NULL);
+
+	offp = (size_t *)(t->buffer->data + ALIGN(tr->data_size, sizeof(void *)));
+
+	if (copy_from_user(t->buffer->data, tr->data.ptr.buffer, tr->data_size)) {
+		binder_user_error("binder: %d:%d got transaction with invalid "
+			"data ptr\n", proc->pid, thread->pid);
+		return_error = BR_FAILED_REPLY;
+		goto err_copy_data_failed;
+	}
+	if (copy_from_user(offp, tr->data.ptr.offsets, tr->offsets_size)) {
+		binder_user_error("binder: %d:%d got transaction with invalid "
+			"offsets ptr\n", proc->pid, thread->pid);
+		return_error = BR_FAILED_REPLY;
+		goto err_copy_data_failed;
+	}
+	if (!IS_ALIGNED(tr->offsets_size, sizeof(size_t))) {
+		binder_user_error("binder: %d:%d got transaction with "
+			"invalid offsets size, %zd\n",
+			proc->pid, thread->pid, tr->offsets_size);
+		return_error = BR_FAILED_REPLY;
+		goto err_bad_offset;
+	}
+	off_end = (void *)offp + tr->offsets_size;
+	for (; offp < off_end; offp++) {
+		struct flat_binder_object *fp;
+		if (*offp > t->buffer->data_size - sizeof(*fp) ||
+		    t->buffer->data_size < sizeof(*fp) ||
+		    !IS_ALIGNED(*offp, sizeof(void *))) {
+			binder_user_error("binder: %d:%d got transaction with "
+				"invalid offset, %zd\n",
+				proc->pid, thread->pid, *offp);
+			return_error = BR_FAILED_REPLY;
+			goto err_bad_offset;
+		}
+		fp = (struct flat_binder_object *)(t->buffer->data + *offp);
+		switch (fp->type) {
+		case BINDER_TYPE_BINDER:
+		case BINDER_TYPE_WEAK_BINDER: {
+			struct binder_ref *ref;
+			struct binder_node *node = binder_get_node(proc, fp->binder);
+			if (node == NULL) {
+				node = binder_new_node(proc, fp->binder, fp->cookie);
+				if (node == NULL) {
+					return_error = BR_FAILED_REPLY;
+					goto err_binder_new_node_failed;
+				}
+				node->min_priority = fp->flags & FLAT_BINDER_FLAG_PRIORITY_MASK;
+				node->accept_fds = !!(fp->flags & FLAT_BINDER_FLAG_ACCEPTS_FDS);
+			}
+			if (fp->cookie != node->cookie) {
+				binder_user_error("binder: %d:%d sending u%p "
+					"node %d, cookie mismatch %p != %p\n",
+					proc->pid, thread->pid,
+					fp->binder, node->debug_id,
+					fp->cookie, node->cookie);
+				goto err_binder_get_ref_for_node_failed;
+			}
+			ref = binder_get_ref_for_node(target_proc, node);
+			if (ref == NULL) {
+				return_error = BR_FAILED_REPLY;
+				goto err_binder_get_ref_for_node_failed;
+			}
+			if (fp->type == BINDER_TYPE_BINDER)
+				fp->type = BINDER_TYPE_HANDLE;
+			else
+				fp->type = BINDER_TYPE_WEAK_HANDLE;
+			fp->handle = ref->desc;
+			binder_inc_ref(ref, fp->type == BINDER_TYPE_HANDLE,
+				       &thread->todo);
+
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        node %d u%p -> ref %d desc %d\n",
+				     node->debug_id, node->ptr, ref->debug_id,
+				     ref->desc);
+		} break;
+		case BINDER_TYPE_HANDLE:
+		case BINDER_TYPE_WEAK_HANDLE: {
+			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d got "
+					"transaction with invalid "
+					"handle, %ld\n", proc->pid,
+					thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_binder_get_ref_failed;
+			}
+			if (ref->node->proc == target_proc) {
+				if (fp->type == BINDER_TYPE_HANDLE)
+					fp->type = BINDER_TYPE_BINDER;
+				else
+					fp->type = BINDER_TYPE_WEAK_BINDER;
+				fp->binder = ref->node->ptr;
+				fp->cookie = ref->node->cookie;
+				binder_inc_node(ref->node, fp->type == BINDER_TYPE_BINDER, 0, NULL);
+				binder_debug(BINDER_DEBUG_TRANSACTION,
+					     "        ref %d desc %d -> node %d u%p\n",
+					     ref->debug_id, ref->desc, ref->node->debug_id,
+					     ref->node->ptr);
+			} else {
+				struct binder_ref *new_ref;
+				new_ref = binder_get_ref_for_node(target_proc, ref->node);
+				if (new_ref == NULL) {
+					return_error = BR_FAILED_REPLY;
+					goto err_binder_get_ref_for_node_failed;
+				}
+				fp->handle = new_ref->desc;
+				binder_inc_ref(new_ref, fp->type == BINDER_TYPE_HANDLE, NULL);
+				binder_debug(BINDER_DEBUG_TRANSACTION,
+					     "        ref %d desc %d -> ref %d desc %d (node %d)\n",
+					     ref->debug_id, ref->desc, new_ref->debug_id,
+					     new_ref->desc, ref->node->debug_id);
+			}
+		} break;
+
+		case BINDER_TYPE_FD: {
+			int target_fd;
+			struct file *file;
+
+			if (reply) {
+				if (!(in_reply_to->flags & TF_ACCEPT_FDS)) {
+					binder_user_error("binder: %d:%d got reply with fd, %ld, but target does not allow fds\n",
+						proc->pid, thread->pid, fp->handle);
+					return_error = BR_FAILED_REPLY;
+					goto err_fd_not_allowed;
+				}
+			} else if (!target_node->accept_fds) {
+				binder_user_error("binder: %d:%d got transaction with fd, %ld, but target does not allow fds\n",
+					proc->pid, thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_fd_not_allowed;
+			}
+
+			file = fget(fp->handle);
+			if (file == NULL) {
+				binder_user_error("binder: %d:%d got transaction with invalid fd, %ld\n",
+					proc->pid, thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_fget_failed;
+			}
+			target_fd = task_get_unused_fd_flags(target_proc, O_CLOEXEC);
+			if (target_fd < 0) {
+				fput(file);
+				return_error = BR_FAILED_REPLY;
+				goto err_get_unused_fd_failed;
+			}
+			task_fd_install(target_proc, target_fd, file);
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        fd %ld -> %d\n", fp->handle, target_fd);
+			/* TODO: fput? */
+			fp->handle = target_fd;
+		} break;
+
+		default:
+			binder_user_error("binder: %d:%d got transactio"
+				"n with invalid object type, %lx\n",
+				proc->pid, thread->pid, fp->type);
+			return_error = BR_FAILED_REPLY;
+			goto err_bad_object_type;
+		}
+	}
+	if (reply) {
+		BUG_ON(t->buffer->async_transaction != 0);
+		binder_pop_transaction(target_thread, in_reply_to);
+	} else if (!(t->flags & TF_ONE_WAY)) {
+		BUG_ON(t->buffer->async_transaction != 0);
+		t->need_reply = 1;
+		t->from_parent = thread->transaction_stack;
+		thread->transaction_stack = t;
+	} else {
+		BUG_ON(target_node == NULL);
+		BUG_ON(t->buffer->async_transaction != 1);
+		if (target_node->has_async_transaction) {
+			target_list = &target_node->async_todo;
+			target_wait = NULL;
+		} else
+			target_node->has_async_transaction = 1;
+	}
+	t->work.type = BINDER_WORK_TRANSACTION;
+	list_add_tail(&t->work.entry, target_list);
+	tcomplete->type = BINDER_WORK_TRANSACTION_COMPLETE;
+	list_add_tail(&tcomplete->entry, &thread->todo);
+	if (target_wait)
+		wake_up_interruptible(target_wait);
+	return;
+
+err_get_unused_fd_failed:
+err_fget_failed:
+err_fd_not_allowed:
+err_binder_get_ref_for_node_failed:
+err_binder_get_ref_failed:
+err_binder_new_node_failed:
+err_bad_object_type:
+err_bad_offset:
+err_copy_data_failed:
+	binder_transaction_buffer_release(target_proc, t->buffer, offp);
+	t->buffer->transaction = NULL;
+	binder_free_buf(target_proc, t->buffer);
+err_binder_alloc_buf_failed:
+	kfree(tcomplete);
+	binder_stats_deleted(BINDER_STAT_TRANSACTION_COMPLETE);
+err_alloc_tcomplete_failed:
+	kfree(t);
+	binder_stats_deleted(BINDER_STAT_TRANSACTION);
+err_alloc_t_failed:
+err_bad_call_stack:
+err_empty_call_stack:
+err_dead_binder:
+err_invalid_target_handle:
+err_no_context_mgr_node:
+	binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
+		     "binder: %d:%d transaction failed %d, size %zd-%zd\n",
+		     proc->pid, thread->pid, return_error,
+		     tr->data_size, tr->offsets_size);
+
+	{
+		struct binder_transaction_log_entry *fe;
+		fe = binder_transaction_log_add(&binder_transaction_log_failed);
+		*fe = *e;
+	}
+
+	BUG_ON(thread->return_error != BR_OK);
+	if (in_reply_to) {
+		thread->return_error = BR_TRANSACTION_COMPLETE;
+		binder_send_failed_reply(in_reply_to, return_error);
+	} else
+		thread->return_error = return_error;
+}
+
+int binder_thread_write(struct binder_proc *proc, struct binder_thread *thread,
+			void __user *buffer, int size, signed long *consumed)
+{
+	uint32_t cmd;
+	void __user *ptr = buffer + *consumed;
+	void __user *end = buffer + size;
+
+	while (ptr < end && thread->return_error == BR_OK) {
+		if (get_user(cmd, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		if (_IOC_NR(cmd) < ARRAY_SIZE(binder_stats.bc)) {
+			binder_stats.bc[_IOC_NR(cmd)]++;
+			proc->stats.bc[_IOC_NR(cmd)]++;
+			thread->stats.bc[_IOC_NR(cmd)]++;
+		}
+		switch (cmd) {
+		case BC_INCREFS:
+		case BC_ACQUIRE:
+		case BC_RELEASE:
+		case BC_DECREFS: {
+			uint32_t target;
+			struct binder_ref *ref;
+			const char *debug_string;
+
+			if (get_user(target, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (target == 0 && binder_context_mgr_node &&
+			    (cmd == BC_INCREFS || cmd == BC_ACQUIRE)) {
+				ref = binder_get_ref_for_node(proc,
+					       binder_context_mgr_node);
+				if (ref->desc != target) {
+					binder_user_error("binder: %d:"
+						"%d tried to acquire "
+						"reference to desc 0, "
+						"got %d instead\n",
+						proc->pid, thread->pid,
+						ref->desc);
+				}
+			} else
+				ref = binder_get_ref(proc, target);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d refcou"
+					"nt change on invalid ref %d\n",
+					proc->pid, thread->pid, target);
+				break;
+			}
+			switch (cmd) {
+			case BC_INCREFS:
+				debug_string = "IncRefs";
+				binder_inc_ref(ref, 0, NULL);
+				break;
+			case BC_ACQUIRE:
+				debug_string = "Acquire";
+				binder_inc_ref(ref, 1, NULL);
+				break;
+			case BC_RELEASE:
+				debug_string = "Release";
+				binder_dec_ref(ref, 1);
+				break;
+			case BC_DECREFS:
+			default:
+				debug_string = "DecRefs";
+				binder_dec_ref(ref, 0);
+				break;
+			}
+			binder_debug(BINDER_DEBUG_USER_REFS,
+				     "binder: %d:%d %s ref %d desc %d s %d w %d for node %d\n",
+				     proc->pid, thread->pid, debug_string, ref->debug_id,
+				     ref->desc, ref->strong, ref->weak, ref->node->debug_id);
+			break;
+		}
+		case BC_INCREFS_DONE:
+		case BC_ACQUIRE_DONE: {
+			void __user *node_ptr;
+			void *cookie;
+			struct binder_node *node;
+
+			if (get_user(node_ptr, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			if (get_user(cookie, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			node = binder_get_node(proc, node_ptr);
+			if (node == NULL) {
+				binder_user_error("binder: %d:%d "
+					"%s u%p no match\n",
+					proc->pid, thread->pid,
+					cmd == BC_INCREFS_DONE ?
+					"BC_INCREFS_DONE" :
+					"BC_ACQUIRE_DONE",
+					node_ptr);
+				break;
+			}
+			if (cookie != node->cookie) {
+				binder_user_error("binder: %d:%d %s u%p node %d"
+					" cookie mismatch %p != %p\n",
+					proc->pid, thread->pid,
+					cmd == BC_INCREFS_DONE ?
+					"BC_INCREFS_DONE" : "BC_ACQUIRE_DONE",
+					node_ptr, node->debug_id,
+					cookie, node->cookie);
+				break;
+			}
+			if (cmd == BC_ACQUIRE_DONE) {
+				if (node->pending_strong_ref == 0) {
+					binder_user_error("binder: %d:%d "
+						"BC_ACQUIRE_DONE node %d has "
+						"no pending acquire request\n",
+						proc->pid, thread->pid,
+						node->debug_id);
+					break;
+				}
+				node->pending_strong_ref = 0;
+			} else {
+				if (node->pending_weak_ref == 0) {
+					binder_user_error("binder: %d:%d "
+						"BC_INCREFS_DONE node %d has "
+						"no pending increfs request\n",
+						proc->pid, thread->pid,
+						node->debug_id);
+					break;
+				}
+				node->pending_weak_ref = 0;
+			}
+			binder_dec_node(node, cmd == BC_ACQUIRE_DONE, 0);
+			binder_debug(BINDER_DEBUG_USER_REFS,
+				     "binder: %d:%d %s node %d ls %d lw %d\n",
+				     proc->pid, thread->pid,
+				     cmd == BC_INCREFS_DONE ? "BC_INCREFS_DONE" : "BC_ACQUIRE_DONE",
+				     node->debug_id, node->local_strong_refs, node->local_weak_refs);
+			break;
+		}
+		case BC_ATTEMPT_ACQUIRE:
+			printk(KERN_ERR "binder: BC_ATTEMPT_ACQUIRE not supported\n");
+			return -EINVAL;
+		case BC_ACQUIRE_RESULT:
+			printk(KERN_ERR "binder: BC_ACQUIRE_RESULT not supported\n");
+			return -EINVAL;
+
+		case BC_FREE_BUFFER: {
+			void __user *data_ptr;
+			struct binder_buffer *buffer;
+
+			if (get_user(data_ptr, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+
+			buffer = binder_buffer_lookup(proc, data_ptr);
+			if (buffer == NULL) {
+				binder_user_error("binder: %d:%d "
+					"BC_FREE_BUFFER u%p no match\n",
+					proc->pid, thread->pid, data_ptr);
+				break;
+			}
+			if (!buffer->allow_user_free) {
+				binder_user_error("binder: %d:%d "
+					"BC_FREE_BUFFER u%p matched "
+					"unreturned buffer\n",
+					proc->pid, thread->pid, data_ptr);
+				break;
+			}
+			binder_debug(BINDER_DEBUG_FREE_BUFFER,
+				     "binder: %d:%d BC_FREE_BUFFER u%p found buffer %d for %s transaction\n",
+				     proc->pid, thread->pid, data_ptr, buffer->debug_id,
+				     buffer->transaction ? "active" : "finished");
+
+			if (buffer->transaction) {
+				buffer->transaction->buffer = NULL;
+				buffer->transaction = NULL;
+			}
+			if (buffer->async_transaction && buffer->target_node) {
+				BUG_ON(!buffer->target_node->has_async_transaction);
+				if (list_empty(&buffer->target_node->async_todo))
+					buffer->target_node->has_async_transaction = 0;
+				else
+					list_move_tail(buffer->target_node->async_todo.next, &thread->todo);
+			}
+			binder_transaction_buffer_release(proc, buffer, NULL);
+			binder_free_buf(proc, buffer);
+			break;
+		}
+
+		case BC_TRANSACTION:
+		case BC_REPLY: {
+			struct binder_transaction_data tr;
+
+			if (copy_from_user(&tr, ptr, sizeof(tr)))
+				return -EFAULT;
+			ptr += sizeof(tr);
+			binder_transaction(proc, thread, &tr, cmd == BC_REPLY);
+			break;
+		}
+
+		case BC_REGISTER_LOOPER:
+			binder_debug(BINDER_DEBUG_THREADS,
+				     "binder: %d:%d BC_REGISTER_LOOPER\n",
+				     proc->pid, thread->pid);
+			if (thread->looper & BINDER_LOOPER_STATE_ENTERED) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_REGISTER_LOOPER called "
+					"after BC_ENTER_LOOPER\n",
+					proc->pid, thread->pid);
+			} else if (proc->requested_threads == 0) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_REGISTER_LOOPER called "
+					"without request\n",
+					proc->pid, thread->pid);
+			} else {
+				proc->requested_threads--;
+				proc->requested_threads_started++;
+			}
+			thread->looper |= BINDER_LOOPER_STATE_REGISTERED;
+			break;
+		case BC_ENTER_LOOPER:
+			binder_debug(BINDER_DEBUG_THREADS,
+				     "binder: %d:%d BC_ENTER_LOOPER\n",
+				     proc->pid, thread->pid);
+			if (thread->looper & BINDER_LOOPER_STATE_REGISTERED) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_ENTER_LOOPER called after "
+					"BC_REGISTER_LOOPER\n",
+					proc->pid, thread->pid);
+			}
+			thread->looper |= BINDER_LOOPER_STATE_ENTERED;
+			break;
+		case BC_EXIT_LOOPER:
+			binder_debug(BINDER_DEBUG_THREADS,
+				     "binder: %d:%d BC_EXIT_LOOPER\n",
+				     proc->pid, thread->pid);
+			thread->looper |= BINDER_LOOPER_STATE_EXITED;
+			break;
+
+		case BC_REQUEST_DEATH_NOTIFICATION:
+		case BC_CLEAR_DEATH_NOTIFICATION: {
+			uint32_t target;
+			void __user *cookie;
+			struct binder_ref *ref;
+			struct binder_ref_death *death;
+
+			if (get_user(target, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (get_user(cookie, (void __user * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			ref = binder_get_ref(proc, target);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d %s "
+					"invalid ref %d\n",
+					proc->pid, thread->pid,
+					cmd == BC_REQUEST_DEATH_NOTIFICATION ?
+					"BC_REQUEST_DEATH_NOTIFICATION" :
+					"BC_CLEAR_DEATH_NOTIFICATION",
+					target);
+				break;
+			}
+
+			binder_debug(BINDER_DEBUG_DEATH_NOTIFICATION,
+				     "binder: %d:%d %s %p ref %d desc %d s %d w %d for node %d\n",
+				     proc->pid, thread->pid,
+				     cmd == BC_REQUEST_DEATH_NOTIFICATION ?
+				     "BC_REQUEST_DEATH_NOTIFICATION" :
+				     "BC_CLEAR_DEATH_NOTIFICATION",
+				     cookie, ref->debug_id, ref->desc,
+				     ref->strong, ref->weak, ref->node->debug_id);
+
+			if (cmd == BC_REQUEST_DEATH_NOTIFICATION) {
+				if (ref->death) {
+					binder_user_error("binder: %d:%"
+						"d BC_REQUEST_DEATH_NOTI"
+						"FICATION death notific"
+						"ation already set\n",
+						proc->pid, thread->pid);
+					break;
+				}
+				death = kzalloc(sizeof(*death), GFP_KERNEL);
+				if (death == NULL) {
+					thread->return_error = BR_ERROR;
+					binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
+						     "binder: %d:%d "
+						     "BC_REQUEST_DEATH_NOTIFICATION failed\n",
+						     proc->pid, thread->pid);
+					break;
+				}
+				binder_stats_created(BINDER_STAT_DEATH);
+				INIT_LIST_HEAD(&death->work.entry);
+				death->cookie = cookie;
+				ref->death = death;
+				if (ref->node->proc == NULL) {
+					ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+					if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+						list_add_tail(&ref->death->work.entry, &thread->todo);
+					} else {
+						list_add_tail(&ref->death->work.entry, &proc->todo);
+						wake_up_interruptible(&proc->wait);
+					}
+				}
+			} else {
+				if (ref->death == NULL) {
+					binder_user_error("binder: %d:%"
+						"d BC_CLEAR_DEATH_NOTIFI"
+						"CATION death notificat"
+						"ion not active\n",
+						proc->pid, thread->pid);
+					break;
+				}
+				death = ref->death;
+				if (death->cookie != cookie) {
+					binder_user_error("binder: %d:%"
+						"d BC_CLEAR_DEATH_NOTIFI"
+						"CATION death notificat"
+						"ion cookie mismatch "
+						"%p != %p\n",
+						proc->pid, thread->pid,
+						death->cookie, cookie);
+					break;
+				}
+				ref->death = NULL;
+				if (list_empty(&death->work.entry)) {
+					death->work.type = BINDER_WORK_CLEAR_DEATH_NOTIFICATION;
+					if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+						list_add_tail(&death->work.entry, &thread->todo);
+					} else {
+						list_add_tail(&death->work.entry, &proc->todo);
+						wake_up_interruptible(&proc->wait);
+					}
+				} else {
+					BUG_ON(death->work.type != BINDER_WORK_DEAD_BINDER);
+					death->work.type = BINDER_WORK_DEAD_BINDER_AND_CLEAR;
+				}
+			}
+		} break;
+		case BC_DEAD_BINDER_DONE: {
+			struct binder_work *w;
+			void __user *cookie;
+			struct binder_ref_death *death = NULL;
+			if (get_user(cookie, (void __user * __user *)ptr))
+				return -EFAULT;
+
+			ptr += sizeof(void *);
+			list_for_each_entry(w, &proc->delivered_death, entry) {
+				struct binder_ref_death *tmp_death = container_of(w, struct binder_ref_death, work);
+				if (tmp_death->cookie == cookie) {
+					death = tmp_death;
+					break;
+				}
+			}
+			binder_debug(BINDER_DEBUG_DEAD_BINDER,
+				     "binder: %d:%d BC_DEAD_BINDER_DONE %p found %p\n",
+				     proc->pid, thread->pid, cookie, death);
+			if (death == NULL) {
+				binder_user_error("binder: %d:%d BC_DEAD"
+					"_BINDER_DONE %p not found\n",
+					proc->pid, thread->pid, cookie);
+				break;
+			}
+
+			list_del_init(&death->work.entry);
+			if (death->work.type == BINDER_WORK_DEAD_BINDER_AND_CLEAR) {
+				death->work.type = BINDER_WORK_CLEAR_DEATH_NOTIFICATION;
+				if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+					list_add_tail(&death->work.entry, &thread->todo);
+				} else {
+					list_add_tail(&death->work.entry, &proc->todo);
+					wake_up_interruptible(&proc->wait);
+				}
+			}
+		} break;
+
+		default:
+			printk(KERN_ERR "binder: %d:%d unknown command %d\n",
+			       proc->pid, thread->pid, cmd);
+			return -EINVAL;
+		}
+		*consumed = ptr - buffer;
+	}
+	return 0;
+}
+
+void binder_stat_br(struct binder_proc *proc, struct binder_thread *thread,
+		    uint32_t cmd)
+{
+	if (_IOC_NR(cmd) < ARRAY_SIZE(binder_stats.br)) {
+		binder_stats.br[_IOC_NR(cmd)]++;
+		proc->stats.br[_IOC_NR(cmd)]++;
+		thread->stats.br[_IOC_NR(cmd)]++;
+	}
+}
+
+static int binder_has_proc_work(struct binder_proc *proc,
+				struct binder_thread *thread)
+{
+	return !list_empty(&proc->todo) ||
+		(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN);
+}
+
+static int binder_has_thread_work(struct binder_thread *thread)
+{
+	return !list_empty(&thread->todo) || thread->return_error != BR_OK ||
+		(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN);
+}
+
+static int binder_thread_read(struct binder_proc *proc,
+			      struct binder_thread *thread,
+			      void  __user *buffer, int size,
+			      signed long *consumed, int non_block)
+{
+	void __user *ptr = buffer + *consumed;
+	void __user *end = buffer + size;
+
+	int ret = 0;
+	int wait_for_proc_work;
+
+	if (*consumed == 0) {
+		if (put_user(BR_NOOP, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+	}
+
+retry:
+	wait_for_proc_work = thread->transaction_stack == NULL &&
+				list_empty(&thread->todo);
+
+	if (thread->return_error != BR_OK && ptr < end) {
+		if (thread->return_error2 != BR_OK) {
+			if (put_user(thread->return_error2, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (ptr == end)
+				goto done;
+			thread->return_error2 = BR_OK;
+		}
+		if (put_user(thread->return_error, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		thread->return_error = BR_OK;
+		goto done;
+	}
+
+
+	thread->looper |= BINDER_LOOPER_STATE_WAITING;
+	if (wait_for_proc_work)
+		proc->ready_threads++;
+	mutex_unlock(&binder_lock);
+	if (wait_for_proc_work) {
+		if (!(thread->looper & (BINDER_LOOPER_STATE_REGISTERED |
+					BINDER_LOOPER_STATE_ENTERED))) {
+			binder_user_error("binder: %d:%d ERROR: Thread waiting "
+				"for process work before calling BC_REGISTER_"
+				"LOOPER or BC_ENTER_LOOPER (state %x)\n",
+				proc->pid, thread->pid, thread->looper);
+			wait_event_interruptible(binder_user_error_wait,
+						 binder_stop_on_user_error < 2);
+		}
+		binder_set_nice(proc->default_priority);
+		if (non_block) {
+			if (!binder_has_proc_work(proc, thread))
+				ret = -EAGAIN;
+		} else
+			ret = wait_event_interruptible_exclusive(proc->wait, binder_has_proc_work(proc, thread));
+	} else {
+		if (non_block) {
+			if (!binder_has_thread_work(thread))
+				ret = -EAGAIN;
+		} else
+			ret = wait_event_interruptible(thread->wait, binder_has_thread_work(thread));
+	}
+	mutex_lock(&binder_lock);
+	if (wait_for_proc_work)
+		proc->ready_threads--;
+	thread->looper &= ~BINDER_LOOPER_STATE_WAITING;
+
+	if (ret)
+		return ret;
+
+	while (1) {
+		uint32_t cmd;
+		struct binder_transaction_data tr;
+		struct binder_work *w;
+		struct binder_transaction *t = NULL;
+
+		if (!list_empty(&thread->todo))
+			w = list_first_entry(&thread->todo, struct binder_work, entry);
+		else if (!list_empty(&proc->todo) && wait_for_proc_work)
+			w = list_first_entry(&proc->todo, struct binder_work, entry);
+		else {
+			if (ptr - buffer == 4 && !(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN)) /* no data added */
+				goto retry;
+			break;
+		}
+
+		if (end - ptr < sizeof(tr) + 4)
+			break;
+
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION: {
+			t = container_of(w, struct binder_transaction, work);
+		} break;
+		case BINDER_WORK_TRANSACTION_COMPLETE: {
+			cmd = BR_TRANSACTION_COMPLETE;
+			if (put_user(cmd, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+
+			binder_stat_br(proc, thread, cmd);
+			binder_debug(BINDER_DEBUG_TRANSACTION_COMPLETE,
+				     "binder: %d:%d BR_TRANSACTION_COMPLETE\n",
+				     proc->pid, thread->pid);
+
+			list_del(&w->entry);
+			kfree(w);
+			binder_stats_deleted(BINDER_STAT_TRANSACTION_COMPLETE);
+		} break;
+		case BINDER_WORK_NODE: {
+			struct binder_node *node = container_of(w, struct binder_node, work);
+			uint32_t cmd = BR_NOOP;
+			const char *cmd_name;
+			int strong = node->internal_strong_refs || node->local_strong_refs;
+			int weak = !hlist_empty(&node->refs) || node->local_weak_refs || strong;
+			if (weak && !node->has_weak_ref) {
+				cmd = BR_INCREFS;
+				cmd_name = "BR_INCREFS";
+				node->has_weak_ref = 1;
+				node->pending_weak_ref = 1;
+				node->local_weak_refs++;
+			} else if (strong && !node->has_strong_ref) {
+				cmd = BR_ACQUIRE;
+				cmd_name = "BR_ACQUIRE";
+				node->has_strong_ref = 1;
+				node->pending_strong_ref = 1;
+				node->local_strong_refs++;
+			} else if (!strong && node->has_strong_ref) {
+				cmd = BR_RELEASE;
+				cmd_name = "BR_RELEASE";
+				node->has_strong_ref = 0;
+			} else if (!weak && node->has_weak_ref) {
+				cmd = BR_DECREFS;
+				cmd_name = "BR_DECREFS";
+				node->has_weak_ref = 0;
+			}
+			if (cmd != BR_NOOP) {
+				if (put_user(cmd, (uint32_t __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(uint32_t);
+				if (put_user(node->ptr, (void * __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(void *);
+				if (put_user(node->cookie, (void * __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(void *);
+
+				binder_stat_br(proc, thread, cmd);
+				binder_debug(BINDER_DEBUG_USER_REFS,
+					     "binder: %d:%d %s %d u%p c%p\n",
+					     proc->pid, thread->pid, cmd_name, node->debug_id, node->ptr, node->cookie);
+			} else {
+				list_del_init(&w->entry);
+				if (!weak && !strong) {
+					binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+						     "binder: %d:%d node %d u%p c%p deleted\n",
+						     proc->pid, thread->pid, node->debug_id,
+						     node->ptr, node->cookie);
+					rb_erase(&node->rb_node, &proc->nodes);
+					kfree(node);
+					binder_stats_deleted(BINDER_STAT_NODE);
+				} else {
+					binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+						     "binder: %d:%d node %d u%p c%p state unchanged\n",
+						     proc->pid, thread->pid, node->debug_id, node->ptr,
+						     node->cookie);
+				}
+			}
+		} break;
+		case BINDER_WORK_DEAD_BINDER:
+		case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+		case BINDER_WORK_CLEAR_DEATH_NOTIFICATION: {
+			struct binder_ref_death *death;
+			uint32_t cmd;
+
+			death = container_of(w, struct binder_ref_death, work);
+			if (w->type == BINDER_WORK_CLEAR_DEATH_NOTIFICATION)
+				cmd = BR_CLEAR_DEATH_NOTIFICATION_DONE;
+			else
+				cmd = BR_DEAD_BINDER;
+			if (put_user(cmd, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (put_user(death->cookie, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			binder_debug(BINDER_DEBUG_DEATH_NOTIFICATION,
+				     "binder: %d:%d %s %p\n",
+				      proc->pid, thread->pid,
+				      cmd == BR_DEAD_BINDER ?
+				      "BR_DEAD_BINDER" :
+				      "BR_CLEAR_DEATH_NOTIFICATION_DONE",
+				      death->cookie);
+
+			if (w->type == BINDER_WORK_CLEAR_DEATH_NOTIFICATION) {
+				list_del(&w->entry);
+				kfree(death);
+				binder_stats_deleted(BINDER_STAT_DEATH);
+			} else
+				list_move(&w->entry, &proc->delivered_death);
+			if (cmd == BR_DEAD_BINDER)
+				goto done; /* DEAD_BINDER notifications can cause transactions */
+		} break;
+		}
+
+		if (!t)
+			continue;
+
+		BUG_ON(t->buffer == NULL);
+		if (t->buffer->target_node) {
+			struct binder_node *target_node = t->buffer->target_node;
+			tr.target.ptr = target_node->ptr;
+			tr.cookie =  target_node->cookie;
+			t->saved_priority = task_nice(current);
+			if (t->priority < target_node->min_priority &&
+			    !(t->flags & TF_ONE_WAY))
+				binder_set_nice(t->priority);
+			else if (!(t->flags & TF_ONE_WAY) ||
+				 t->saved_priority > target_node->min_priority)
+				binder_set_nice(target_node->min_priority);
+			cmd = BR_TRANSACTION;
+		} else {
+			tr.target.ptr = NULL;
+			tr.cookie = NULL;
+			cmd = BR_REPLY;
+		}
+		tr.code = t->code;
+		tr.flags = t->flags;
+		tr.sender_euid = t->sender_euid;
+
+		if (t->from) {
+			struct task_struct *sender = t->from->proc->tsk;
+			tr.sender_pid = task_tgid_nr_ns(sender,
+							current->nsproxy->pid_ns);
+		} else {
+			tr.sender_pid = 0;
+		}
+
+		tr.data_size = t->buffer->data_size;
+		tr.offsets_size = t->buffer->offsets_size;
+		tr.data.ptr.buffer = (void *)t->buffer->data +
+					proc->user_buffer_offset;
+		tr.data.ptr.offsets = tr.data.ptr.buffer +
+					ALIGN(t->buffer->data_size,
+					    sizeof(void *));
+
+		if (put_user(cmd, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		if (copy_to_user(ptr, &tr, sizeof(tr)))
+			return -EFAULT;
+		ptr += sizeof(tr);
+
+		binder_stat_br(proc, thread, cmd);
+		binder_debug(BINDER_DEBUG_TRANSACTION,
+			     "binder: %d:%d %s %d %d:%d, cmd %d"
+			     "size %zd-%zd ptr %p-%p\n",
+			     proc->pid, thread->pid,
+			     (cmd == BR_TRANSACTION) ? "BR_TRANSACTION" :
+			     "BR_REPLY",
+			     t->debug_id, t->from ? t->from->proc->pid : 0,
+			     t->from ? t->from->pid : 0, cmd,
+			     t->buffer->data_size, t->buffer->offsets_size,
+			     tr.data.ptr.buffer, tr.data.ptr.offsets);
+
+		list_del(&t->work.entry);
+		t->buffer->allow_user_free = 1;
+		if (cmd == BR_TRANSACTION && !(t->flags & TF_ONE_WAY)) {
+			t->to_parent = thread->transaction_stack;
+			t->to_thread = thread;
+			thread->transaction_stack = t;
+		} else {
+			t->buffer->transaction = NULL;
+			kfree(t);
+			binder_stats_deleted(BINDER_STAT_TRANSACTION);
+		}
+		break;
+	}
+
+done:
+
+	*consumed = ptr - buffer;
+	if (proc->requested_threads + proc->ready_threads == 0 &&
+	    proc->requested_threads_started < proc->max_threads &&
+	    (thread->looper & (BINDER_LOOPER_STATE_REGISTERED |
+	     BINDER_LOOPER_STATE_ENTERED)) /* the user-space code fails to */
+	     /*spawn a new thread if we leave this out */) {
+		proc->requested_threads++;
+		binder_debug(BINDER_DEBUG_THREADS,
+			     "binder: %d:%d BR_SPAWN_LOOPER\n",
+			     proc->pid, thread->pid);
+		if (put_user(BR_SPAWN_LOOPER, (uint32_t __user *)buffer))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+static void binder_release_work(struct list_head *list)
+{
+	struct binder_work *w;
+	while (!list_empty(list)) {
+		w = list_first_entry(list, struct binder_work, entry);
+		list_del_init(&w->entry);
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION: {
+			struct binder_transaction *t;
+
+			t = container_of(w, struct binder_transaction, work);
+			if (t->buffer->target_node && !(t->flags & TF_ONE_WAY))
+				binder_send_failed_reply(t, BR_DEAD_REPLY);
+		} break;
+		case BINDER_WORK_TRANSACTION_COMPLETE: {
+			kfree(w);
+			binder_stats_deleted(BINDER_STAT_TRANSACTION_COMPLETE);
+		} break;
+		default:
+			break;
+		}
+	}
+
+}
+
+static struct binder_thread *binder_get_thread(struct binder_proc *proc)
+{
+	struct binder_thread *thread = NULL;
+	struct rb_node *parent = NULL;
+	struct rb_node **p = &proc->threads.rb_node;
+
+	while (*p) {
+		parent = *p;
+		thread = rb_entry(parent, struct binder_thread, rb_node);
+
+		if (current->pid < thread->pid)
+			p = &(*p)->rb_left;
+		else if (current->pid > thread->pid)
+			p = &(*p)->rb_right;
+		else
+			break;
+	}
+	if (*p == NULL) {
+		thread = kzalloc(sizeof(*thread), GFP_KERNEL);
+		if (thread == NULL)
+			return NULL;
+		binder_stats_created(BINDER_STAT_THREAD);
+		thread->proc = proc;
+		thread->pid = current->pid;
+		init_waitqueue_head(&thread->wait);
+		INIT_LIST_HEAD(&thread->todo);
+		rb_link_node(&thread->rb_node, parent, p);
+		rb_insert_color(&thread->rb_node, &proc->threads);
+		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
+		thread->return_error = BR_OK;
+		thread->return_error2 = BR_OK;
+	}
+	return thread;
+}
+
+static int binder_free_thread(struct binder_proc *proc,
+			      struct binder_thread *thread)
+{
+	struct binder_transaction *t;
+	struct binder_transaction *send_reply = NULL;
+	int active_transactions = 0;
+
+	rb_erase(&thread->rb_node, &proc->threads);
+	t = thread->transaction_stack;
+	if (t && t->to_thread == thread)
+		send_reply = t;
+	while (t) {
+		active_transactions++;
+		binder_debug(BINDER_DEBUG_DEAD_TRANSACTION,
+			     "binder: release %d:%d transaction %d "
+			     "%s, still active\n", proc->pid, thread->pid,
+			     t->debug_id,
+			     (t->to_thread == thread) ? "in" : "out");
+
+		if (t->to_thread == thread) {
+			t->to_proc = NULL;
+			t->to_thread = NULL;
+			if (t->buffer) {
+				t->buffer->transaction = NULL;
+				t->buffer = NULL;
+			}
+			t = t->to_parent;
+		} else if (t->from == thread) {
+			t->from = NULL;
+			t = t->from_parent;
+		} else
+			BUG();
+	}
+	if (send_reply)
+		binder_send_failed_reply(send_reply, BR_DEAD_REPLY);
+	binder_release_work(&thread->todo);
+	kfree(thread);
+	binder_stats_deleted(BINDER_STAT_THREAD);
+	return active_transactions;
+}
+
+static unsigned int binder_poll(struct file *filp,
+				struct poll_table_struct *wait)
+{
+	struct binder_proc *proc = filp->private_data;
+	struct binder_thread *thread = NULL;
+	int wait_for_proc_work;
+
+	mutex_lock(&binder_lock);
+	thread = binder_get_thread(proc);
+
+	wait_for_proc_work = thread->transaction_stack == NULL &&
+		list_empty(&thread->todo) && thread->return_error == BR_OK;
+	mutex_unlock(&binder_lock);
+
+	if (wait_for_proc_work) {
+		if (binder_has_proc_work(proc, thread))
+			return POLLIN;
+		poll_wait(filp, &proc->wait, wait);
+		if (binder_has_proc_work(proc, thread))
+			return POLLIN;
+	} else {
+		if (binder_has_thread_work(thread))
+			return POLLIN;
+		poll_wait(filp, &thread->wait, wait);
+		if (binder_has_thread_work(thread))
+			return POLLIN;
+	}
+	return 0;
+}
+
+static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int ret;
+	struct binder_proc *proc = filp->private_data;
+	struct binder_thread *thread;
+	unsigned int size = _IOC_SIZE(cmd);
+	void __user *ubuf = (void __user *)arg;
+
+	/*printk(KERN_INFO "binder_ioctl: %d:%d %x %lx\n", proc->pid, current->pid, cmd, arg);*/
+
+	ret = wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+	if (ret)
+		return ret;
+
+	mutex_lock(&binder_lock);
+	thread = binder_get_thread(proc);
+	if (thread == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	switch (cmd) {
+	case BINDER_WRITE_READ: {
+		struct binder_write_read bwr;
+		if (size != sizeof(struct binder_write_read)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (copy_from_user(&bwr, ubuf, sizeof(bwr))) {
+			ret = -EFAULT;
+			goto err;
+		}
+		binder_debug(BINDER_DEBUG_READ_WRITE,
+			     "binder: %d:%d write %ld at %08lx, read %ld at %08lx\n",
+			     proc->pid, thread->pid, bwr.write_size, bwr.write_buffer,
+			     bwr.read_size, bwr.read_buffer);
+
+		if (bwr.write_size > 0) {
+			ret = binder_thread_write(proc, thread, (void __user *)bwr.write_buffer, bwr.write_size, &bwr.write_consumed);
+			if (ret < 0) {
+				bwr.read_consumed = 0;
+				if (copy_to_user(ubuf, &bwr, sizeof(bwr)))
+					ret = -EFAULT;
+				goto err;
+			}
+		}
+		if (bwr.read_size > 0) {
+			ret = binder_thread_read(proc, thread, (void __user *)bwr.read_buffer, bwr.read_size, &bwr.read_consumed, filp->f_flags & O_NONBLOCK);
+			if (!list_empty(&proc->todo))
+				wake_up_interruptible(&proc->wait);
+			if (ret < 0) {
+				if (copy_to_user(ubuf, &bwr, sizeof(bwr)))
+					ret = -EFAULT;
+				goto err;
+			}
+		}
+		binder_debug(BINDER_DEBUG_READ_WRITE,
+			     "binder: %d:%d wrote %ld of %ld, read return %ld of %ld\n",
+			     proc->pid, thread->pid, bwr.write_consumed, bwr.write_size,
+			     bwr.read_consumed, bwr.read_size);
+		if (copy_to_user(ubuf, &bwr, sizeof(bwr))) {
+			ret = -EFAULT;
+			goto err;
+		}
+		break;
+	}
+	case BINDER_SET_MAX_THREADS:
+		if (copy_from_user(&proc->max_threads, ubuf, sizeof(proc->max_threads))) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	case BINDER_SET_CONTEXT_MGR:
+		if (binder_context_mgr_node != NULL) {
+			printk(KERN_ERR "binder: BINDER_SET_CONTEXT_MGR already set\n");
+			ret = -EBUSY;
+			goto err;
+		}
+		if (binder_context_mgr_uid != -1) {
+			if (binder_context_mgr_uid != current->cred->euid) {
+				printk(KERN_ERR "binder: BINDER_SET_"
+				       "CONTEXT_MGR bad uid %d != %d\n",
+				       current->cred->euid,
+				       binder_context_mgr_uid);
+				ret = -EPERM;
+				goto err;
+			}
+		} else
+			binder_context_mgr_uid = current->cred->euid;
+		binder_context_mgr_node = binder_new_node(proc, NULL, NULL);
+		if (binder_context_mgr_node == NULL) {
+			ret = -ENOMEM;
+			goto err;
+		}
+		binder_context_mgr_node->local_weak_refs++;
+		binder_context_mgr_node->local_strong_refs++;
+		binder_context_mgr_node->has_strong_ref = 1;
+		binder_context_mgr_node->has_weak_ref = 1;
+		break;
+	case BINDER_THREAD_EXIT:
+		binder_debug(BINDER_DEBUG_THREADS, "binder: %d:%d exit\n",
+			     proc->pid, thread->pid);
+		binder_free_thread(proc, thread);
+		thread = NULL;
+		break;
+	case BINDER_VERSION:
+		if (size != sizeof(struct binder_version)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (put_user(BINDER_CURRENT_PROTOCOL_VERSION, &((struct binder_version *)ubuf)->protocol_version)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+	ret = 0;
+err:
+	if (thread)
+		thread->looper &= ~BINDER_LOOPER_STATE_NEED_RETURN;
+	mutex_unlock(&binder_lock);
+	wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+	if (ret && ret != -ERESTARTSYS)
+		printk(KERN_INFO "binder: %d:%d ioctl %x %lx returned %d\n", proc->pid, current->pid, cmd, arg, ret);
+	return ret;
+}
+
+static void binder_vma_open(struct vm_area_struct *vma)
+{
+	struct binder_proc *proc = vma->vm_private_data;
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder: %d open vm area %lx-%lx (%ld K) vma %lx pagep %lx\n",
+		     proc->pid, vma->vm_start, vma->vm_end,
+		     (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags,
+		     (unsigned long)pgprot_val(vma->vm_page_prot));
+	dump_stack();
+}
+
+static void binder_vma_close(struct vm_area_struct *vma)
+{
+	struct binder_proc *proc = vma->vm_private_data;
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder: %d close vm area %lx-%lx (%ld K) vma %lx pagep %lx\n",
+		     proc->pid, vma->vm_start, vma->vm_end,
+		     (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags,
+		     (unsigned long)pgprot_val(vma->vm_page_prot));
+	proc->vma = NULL;
+	binder_defer_work(proc, BINDER_DEFERRED_PUT_FILES);
+}
+
+static struct vm_operations_struct binder_vm_ops = {
+	.open = binder_vma_open,
+	.close = binder_vma_close,
+};
+
+static int binder_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	int ret;
+	struct vm_struct *area;
+	struct binder_proc *proc = filp->private_data;
+	const char *failure_string;
+	struct binder_buffer *buffer;
+
+	if ((vma->vm_end - vma->vm_start) > SZ_4M)
+		vma->vm_end = vma->vm_start + SZ_4M;
+
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder_mmap: %d %lx-%lx (%ld K) vma %lx pagep %lx\n",
+		     proc->pid, vma->vm_start, vma->vm_end,
+		     (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags,
+		     (unsigned long)pgprot_val(vma->vm_page_prot));
+
+	if (vma->vm_flags & FORBIDDEN_MMAP_FLAGS) {
+		ret = -EPERM;
+		failure_string = "bad vm_flags";
+		goto err_bad_arg;
+	}
+	vma->vm_flags = (vma->vm_flags | VM_DONTCOPY) & ~VM_MAYWRITE;
+
+	if (proc->buffer) {
+		ret = -EBUSY;
+		failure_string = "already mapped";
+		goto err_already_mapped;
+	}
+
+	area = get_vm_area(vma->vm_end - vma->vm_start, VM_IOREMAP);
+	if (area == NULL) {
+		ret = -ENOMEM;
+		failure_string = "get_vm_area";
+		goto err_get_vm_area_failed;
+	}
+	proc->buffer = area->addr;
+	proc->user_buffer_offset = vma->vm_start - (uintptr_t)proc->buffer;
+
+#ifdef CONFIG_CPU_CACHE_VIPT
+	if (cache_is_vipt_aliasing()) {
+		while (CACHE_COLOUR((vma->vm_start ^ (uint32_t)proc->buffer))) {
+			printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p bad alignment\n", proc->pid, vma->vm_start, vma->vm_end, proc->buffer);
+			vma->vm_start += PAGE_SIZE;
+		}
+	}
+#endif
+	proc->pages = kzalloc(sizeof(proc->pages[0]) * ((vma->vm_end - vma->vm_start) / PAGE_SIZE), GFP_KERNEL);
+	if (proc->pages == NULL) {
+		ret = -ENOMEM;
+		failure_string = "alloc page array";
+		goto err_alloc_pages_failed;
+	}
+	proc->buffer_size = vma->vm_end - vma->vm_start;
+
+	vma->vm_ops = &binder_vm_ops;
+	vma->vm_private_data = proc;
+
+	if (binder_update_page_range(proc, 1, proc->buffer, proc->buffer + PAGE_SIZE, vma)) {
+		ret = -ENOMEM;
+		failure_string = "alloc small buf";
+		goto err_alloc_small_buf_failed;
+	}
+	buffer = proc->buffer;
+	INIT_LIST_HEAD(&proc->buffers);
+	list_add(&buffer->entry, &proc->buffers);
+	buffer->free = 1;
+	binder_insert_free_buffer(proc, buffer);
+	proc->free_async_space = proc->buffer_size / 2;
+	barrier();
+	proc->files = get_files_struct(current);
+	proc->vma = vma;
+
+	/*printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p\n",
+		 proc->pid, vma->vm_start, vma->vm_end, proc->buffer);*/
+	return 0;
+
+err_alloc_small_buf_failed:
+	kfree(proc->pages);
+	proc->pages = NULL;
+err_alloc_pages_failed:
+	vfree(proc->buffer);
+	proc->buffer = NULL;
+err_get_vm_area_failed:
+err_already_mapped:
+err_bad_arg:
+	printk(KERN_ERR "binder_mmap: %d %lx-%lx %s failed %d\n",
+	       proc->pid, vma->vm_start, vma->vm_end, failure_string, ret);
+	return ret;
+}
+
+static int binder_open(struct inode *nodp, struct file *filp)
+{
+	struct binder_proc *proc;
+
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE, "binder_open: %d:%d\n",
+		     current->group_leader->pid, current->pid);
+
+	proc = kzalloc(sizeof(*proc), GFP_KERNEL);
+	if (proc == NULL)
+		return -ENOMEM;
+	get_task_struct(current);
+	proc->tsk = current;
+	INIT_LIST_HEAD(&proc->todo);
+	init_waitqueue_head(&proc->wait);
+	proc->default_priority = task_nice(current);
+	mutex_lock(&binder_lock);
+	binder_stats_created(BINDER_STAT_PROC);
+	hlist_add_head(&proc->proc_node, &binder_procs);
+	proc->pid = current->group_leader->pid;
+	INIT_LIST_HEAD(&proc->delivered_death);
+	filp->private_data = proc;
+	mutex_unlock(&binder_lock);
+
+	if (binder_debugfs_dir_entry_proc) {
+		char strbuf[11];
+		snprintf(strbuf, sizeof(strbuf), "%u", proc->pid);
+		proc->debugfs_entry = debugfs_create_file(strbuf, S_IRUGO,
+			binder_debugfs_dir_entry_proc, proc, &binder_proc_fops);
+	}
+
+	return 0;
+}
+
+static int binder_flush(struct file *filp, fl_owner_t id)
+{
+	struct binder_proc *proc = filp->private_data;
+
+	binder_defer_work(proc, BINDER_DEFERRED_FLUSH);
+
+	return 0;
+}
+
+static void binder_deferred_flush(struct binder_proc *proc)
+{
+	struct rb_node *n;
+	int wake_count = 0;
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n)) {
+		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
+		if (thread->looper & BINDER_LOOPER_STATE_WAITING) {
+			wake_up_interruptible(&thread->wait);
+			wake_count++;
+		}
+	}
+	wake_up_interruptible_all(&proc->wait);
+
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder_flush: %d woke %d threads\n", proc->pid,
+		     wake_count);
+}
+
+static int binder_release(struct inode *nodp, struct file *filp)
+{
+	struct binder_proc *proc = filp->private_data;
+	debugfs_remove(proc->debugfs_entry);
+	binder_defer_work(proc, BINDER_DEFERRED_RELEASE);
+
+	return 0;
+}
+
+static void binder_deferred_release(struct binder_proc *proc)
+{
+	struct hlist_node *pos;
+	struct binder_transaction *t;
+	struct rb_node *n;
+	int threads, nodes, incoming_refs, outgoing_refs, buffers, active_transactions, page_count;
+
+	BUG_ON(proc->vma);
+	BUG_ON(proc->files);
+
+	hlist_del(&proc->proc_node);
+	if (binder_context_mgr_node && binder_context_mgr_node->proc == proc) {
+		binder_debug(BINDER_DEBUG_DEAD_BINDER,
+			     "binder_release: %d context_mgr_node gone\n",
+			     proc->pid);
+		binder_context_mgr_node = NULL;
+	}
+
+	threads = 0;
+	active_transactions = 0;
+	while ((n = rb_first(&proc->threads))) {
+		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+		threads++;
+		active_transactions += binder_free_thread(proc, thread);
+	}
+	nodes = 0;
+	incoming_refs = 0;
+	while ((n = rb_first(&proc->nodes))) {
+		struct binder_node *node = rb_entry(n, struct binder_node, rb_node);
+
+		nodes++;
+		rb_erase(&node->rb_node, &proc->nodes);
+		list_del_init(&node->work.entry);
+		if (hlist_empty(&node->refs)) {
+			kfree(node);
+			binder_stats_deleted(BINDER_STAT_NODE);
+		} else {
+			struct binder_ref *ref;
+			int death = 0;
+
+			node->proc = NULL;
+			node->local_strong_refs = 0;
+			node->local_weak_refs = 0;
+			hlist_add_head(&node->dead_node, &binder_dead_nodes);
+
+			hlist_for_each_entry(ref, pos, &node->refs, node_entry) {
+				incoming_refs++;
+				if (ref->death) {
+					death++;
+					if (list_empty(&ref->death->work.entry)) {
+						ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+						list_add_tail(&ref->death->work.entry, &ref->proc->todo);
+						wake_up_interruptible(&ref->proc->wait);
+					} else
+						BUG();
+				}
+			}
+			binder_debug(BINDER_DEBUG_DEAD_BINDER,
+				     "binder: node %d now dead, "
+				     "refs %d, death %d\n", node->debug_id,
+				     incoming_refs, death);
+		}
+	}
+	outgoing_refs = 0;
+	while ((n = rb_first(&proc->refs_by_desc))) {
+		struct binder_ref *ref = rb_entry(n, struct binder_ref,
+						  rb_node_desc);
+		outgoing_refs++;
+		binder_delete_ref(ref);
+	}
+	binder_release_work(&proc->todo);
+	buffers = 0;
+
+	while ((n = rb_first(&proc->allocated_buffers))) {
+		struct binder_buffer *buffer = rb_entry(n, struct binder_buffer,
+							rb_node);
+		t = buffer->transaction;
+		if (t) {
+			t->buffer = NULL;
+			buffer->transaction = NULL;
+			printk(KERN_ERR "binder: release proc %d, "
+			       "transaction %d, not freed\n",
+			       proc->pid, t->debug_id);
+			/*BUG();*/
+		}
+		binder_free_buf(proc, buffer);
+		buffers++;
+	}
+
+	binder_stats_deleted(BINDER_STAT_PROC);
+
+	page_count = 0;
+	if (proc->pages) {
+		int i;
+		for (i = 0; i < proc->buffer_size / PAGE_SIZE; i++) {
+			if (proc->pages[i]) {
+				void *page_addr = proc->buffer + i * PAGE_SIZE;
+				binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+					     "binder_release: %d: "
+					     "page %d at %p not freed\n",
+					     proc->pid, i,
+					     page_addr);
+				unmap_kernel_range((unsigned long)page_addr,
+					PAGE_SIZE);
+				__free_page(proc->pages[i]);
+				page_count++;
+			}
+		}
+		kfree(proc->pages);
+		vfree(proc->buffer);
+	}
+
+	put_task_struct(proc->tsk);
+
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder_release: %d threads %d, nodes %d (ref %d), "
+		     "refs %d, active transactions %d, buffers %d, "
+		     "pages %d\n",
+		     proc->pid, threads, nodes, incoming_refs, outgoing_refs,
+		     active_transactions, buffers, page_count);
+
+	kfree(proc);
+}
+
+static void binder_deferred_func(struct work_struct *work)
+{
+	struct binder_proc *proc;
+	struct files_struct *files;
+
+	int defer;
+	do {
+		mutex_lock(&binder_lock);
+		mutex_lock(&binder_deferred_lock);
+		if (!hlist_empty(&binder_deferred_list)) {
+			proc = hlist_entry(binder_deferred_list.first,
+					struct binder_proc, deferred_work_node);
+			hlist_del_init(&proc->deferred_work_node);
+			defer = proc->deferred_work;
+			proc->deferred_work = 0;
+		} else {
+			proc = NULL;
+			defer = 0;
+		}
+		mutex_unlock(&binder_deferred_lock);
+
+		files = NULL;
+		if (defer & BINDER_DEFERRED_PUT_FILES) {
+			files = proc->files;
+			if (files)
+				proc->files = NULL;
+		}
+
+		if (defer & BINDER_DEFERRED_FLUSH)
+			binder_deferred_flush(proc);
+
+		if (defer & BINDER_DEFERRED_RELEASE)
+			binder_deferred_release(proc); /* frees proc */
+
+		mutex_unlock(&binder_lock);
+		if (files)
+			put_files_struct(files);
+	} while (proc);
+}
+static DECLARE_WORK(binder_deferred_work, binder_deferred_func);
+
+static void
+binder_defer_work(struct binder_proc *proc, enum binder_deferred_state defer)
+{
+	mutex_lock(&binder_deferred_lock);
+	proc->deferred_work |= defer;
+	if (hlist_unhashed(&proc->deferred_work_node)) {
+		hlist_add_head(&proc->deferred_work_node,
+				&binder_deferred_list);
+		queue_work(binder_deferred_workqueue, &binder_deferred_work);
+	}
+	mutex_unlock(&binder_deferred_lock);
+}
+
+static void print_binder_transaction(struct seq_file *m, const char *prefix,
+				     struct binder_transaction *t)
+{
+	seq_printf(m,
+		   "%s %d: %p from %d:%d to %d:%d code %x flags %x pri %ld r%d",
+		   prefix, t->debug_id, t,
+		   t->from ? t->from->proc->pid : 0,
+		   t->from ? t->from->pid : 0,
+		   t->to_proc ? t->to_proc->pid : 0,
+		   t->to_thread ? t->to_thread->pid : 0,
+		   t->code, t->flags, t->priority, t->need_reply);
+	if (t->buffer == NULL) {
+		seq_puts(m, " buffer free\n");
+		return;
+	}
+	if (t->buffer->target_node)
+		seq_printf(m, " node %d",
+			   t->buffer->target_node->debug_id);
+	seq_printf(m, " size %zd:%zd data %p\n",
+		   t->buffer->data_size, t->buffer->offsets_size,
+		   t->buffer->data);
+}
+
+static void print_binder_buffer(struct seq_file *m, const char *prefix,
+				struct binder_buffer *buffer)
+{
+	seq_printf(m, "%s %d: %p size %zd:%zd %s\n",
+		   prefix, buffer->debug_id, buffer->data,
+		   buffer->data_size, buffer->offsets_size,
+		   buffer->transaction ? "active" : "delivered");
+}
+
+static void print_binder_work(struct seq_file *m, const char *prefix,
+			      const char *transaction_prefix,
+			      struct binder_work *w)
+{
+	struct binder_node *node;
+	struct binder_transaction *t;
+
+	switch (w->type) {
+	case BINDER_WORK_TRANSACTION:
+		t = container_of(w, struct binder_transaction, work);
+		print_binder_transaction(m, transaction_prefix, t);
+		break;
+	case BINDER_WORK_TRANSACTION_COMPLETE:
+		seq_printf(m, "%stransaction complete\n", prefix);
+		break;
+	case BINDER_WORK_NODE:
+		node = container_of(w, struct binder_node, work);
+		seq_printf(m, "%snode work %d: u%p c%p\n",
+			   prefix, node->debug_id, node->ptr, node->cookie);
+		break;
+	case BINDER_WORK_DEAD_BINDER:
+		seq_printf(m, "%shas dead binder\n", prefix);
+		break;
+	case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+		seq_printf(m, "%shas cleared dead binder\n", prefix);
+		break;
+	case BINDER_WORK_CLEAR_DEATH_NOTIFICATION:
+		seq_printf(m, "%shas cleared death notification\n", prefix);
+		break;
+	default:
+		seq_printf(m, "%sunknown work: type %d\n", prefix, w->type);
+		break;
+	}
+}
+
+static void print_binder_thread(struct seq_file *m,
+				struct binder_thread *thread,
+				int print_always)
+{
+	struct binder_transaction *t;
+	struct binder_work *w;
+	size_t start_pos = m->count;
+	size_t header_pos;
+
+	seq_printf(m, "  thread %d: l %02x\n", thread->pid, thread->looper);
+	header_pos = m->count;
+	t = thread->transaction_stack;
+	while (t) {
+		if (t->from == thread) {
+			print_binder_transaction(m,
+						 "    outgoing transaction", t);
+			t = t->from_parent;
+		} else if (t->to_thread == thread) {
+			print_binder_transaction(m,
+						 "    incoming transaction", t);
+			t = t->to_parent;
+		} else {
+			print_binder_transaction(m, "    bad transaction", t);
+			t = NULL;
+		}
+	}
+	list_for_each_entry(w, &thread->todo, entry) {
+		print_binder_work(m, "    ", "    pending transaction", w);
+	}
+	if (!print_always && m->count == header_pos)
+		m->count = start_pos;
+}
+
+static void print_binder_node(struct seq_file *m, struct binder_node *node)
+{
+	struct binder_ref *ref;
+	struct hlist_node *pos;
+	struct binder_work *w;
+	int count;
+
+	count = 0;
+	hlist_for_each_entry(ref, pos, &node->refs, node_entry)
+		count++;
+
+	seq_printf(m, "  node %d: u%p c%p hs %d hw %d ls %d lw %d is %d iw %d",
+		   node->debug_id, node->ptr, node->cookie,
+		   node->has_strong_ref, node->has_weak_ref,
+		   node->local_strong_refs, node->local_weak_refs,
+		   node->internal_strong_refs, count);
+	if (count) {
+		seq_puts(m, " proc");
+		hlist_for_each_entry(ref, pos, &node->refs, node_entry)
+			seq_printf(m, " %d", ref->proc->pid);
+	}
+	seq_puts(m, "\n");
+	list_for_each_entry(w, &node->async_todo, entry)
+		print_binder_work(m, "    ",
+				  "    pending async transaction", w);
+}
+
+static void print_binder_ref(struct seq_file *m, struct binder_ref *ref)
+{
+	seq_printf(m, "  ref %d: desc %d %snode %d s %d w %d d %p\n",
+		   ref->debug_id, ref->desc, ref->node->proc ? "" : "dead ",
+		   ref->node->debug_id, ref->strong, ref->weak, ref->death);
+}
+
+static void print_binder_proc(struct seq_file *m,
+			      struct binder_proc *proc, int print_all)
+{
+	struct binder_work *w;
+	struct rb_node *n;
+	size_t start_pos = m->count;
+	size_t header_pos;
+
+	seq_printf(m, "proc %d\n", proc->pid);
+	header_pos = m->count;
+
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n))
+		print_binder_thread(m, rb_entry(n, struct binder_thread,
+						rb_node), print_all);
+	for (n = rb_first(&proc->nodes); n != NULL; n = rb_next(n)) {
+		struct binder_node *node = rb_entry(n, struct binder_node,
+						    rb_node);
+		if (print_all || node->has_async_transaction)
+			print_binder_node(m, node);
+	}
+	if (print_all) {
+		for (n = rb_first(&proc->refs_by_desc);
+		     n != NULL;
+		     n = rb_next(n))
+			print_binder_ref(m, rb_entry(n, struct binder_ref,
+						     rb_node_desc));
+	}
+	for (n = rb_first(&proc->allocated_buffers); n != NULL; n = rb_next(n))
+		print_binder_buffer(m, "  buffer",
+				    rb_entry(n, struct binder_buffer, rb_node));
+	list_for_each_entry(w, &proc->todo, entry)
+		print_binder_work(m, "  ", "  pending transaction", w);
+	list_for_each_entry(w, &proc->delivered_death, entry) {
+		seq_puts(m, "  has delivered dead binder\n");
+		break;
+	}
+	if (!print_all && m->count == header_pos)
+		m->count = start_pos;
+}
+
+static const char *binder_return_strings[] = {
+	"BR_ERROR",
+	"BR_OK",
+	"BR_TRANSACTION",
+	"BR_REPLY",
+	"BR_ACQUIRE_RESULT",
+	"BR_DEAD_REPLY",
+	"BR_TRANSACTION_COMPLETE",
+	"BR_INCREFS",
+	"BR_ACQUIRE",
+	"BR_RELEASE",
+	"BR_DECREFS",
+	"BR_ATTEMPT_ACQUIRE",
+	"BR_NOOP",
+	"BR_SPAWN_LOOPER",
+	"BR_FINISHED",
+	"BR_DEAD_BINDER",
+	"BR_CLEAR_DEATH_NOTIFICATION_DONE",
+	"BR_FAILED_REPLY"
+};
+
+static const char *binder_command_strings[] = {
+	"BC_TRANSACTION",
+	"BC_REPLY",
+	"BC_ACQUIRE_RESULT",
+	"BC_FREE_BUFFER",
+	"BC_INCREFS",
+	"BC_ACQUIRE",
+	"BC_RELEASE",
+	"BC_DECREFS",
+	"BC_INCREFS_DONE",
+	"BC_ACQUIRE_DONE",
+	"BC_ATTEMPT_ACQUIRE",
+	"BC_REGISTER_LOOPER",
+	"BC_ENTER_LOOPER",
+	"BC_EXIT_LOOPER",
+	"BC_REQUEST_DEATH_NOTIFICATION",
+	"BC_CLEAR_DEATH_NOTIFICATION",
+	"BC_DEAD_BINDER_DONE"
+};
+
+static const char *binder_objstat_strings[] = {
+	"proc",
+	"thread",
+	"node",
+	"ref",
+	"death",
+	"transaction",
+	"transaction_complete"
+};
+
+static void print_binder_stats(struct seq_file *m, const char *prefix,
+			       struct binder_stats *stats)
+{
+	int i;
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->bc) !=
+		     ARRAY_SIZE(binder_command_strings));
+	for (i = 0; i < ARRAY_SIZE(stats->bc); i++) {
+		if (stats->bc[i])
+			seq_printf(m, "%s%s: %d\n", prefix,
+				   binder_command_strings[i], stats->bc[i]);
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->br) !=
+		     ARRAY_SIZE(binder_return_strings));
+	for (i = 0; i < ARRAY_SIZE(stats->br); i++) {
+		if (stats->br[i])
+			seq_printf(m, "%s%s: %d\n", prefix,
+				   binder_return_strings[i], stats->br[i]);
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->obj_created) !=
+		     ARRAY_SIZE(binder_objstat_strings));
+	BUILD_BUG_ON(ARRAY_SIZE(stats->obj_created) !=
+		     ARRAY_SIZE(stats->obj_deleted));
+	for (i = 0; i < ARRAY_SIZE(stats->obj_created); i++) {
+		if (stats->obj_created[i] || stats->obj_deleted[i])
+			seq_printf(m, "%s%s: active %d total %d\n", prefix,
+				binder_objstat_strings[i],
+				stats->obj_created[i] - stats->obj_deleted[i],
+				stats->obj_created[i]);
+	}
+}
+
+static void print_binder_proc_stats(struct seq_file *m,
+				    struct binder_proc *proc)
+{
+	struct binder_work *w;
+	struct rb_node *n;
+	int count, strong, weak;
+
+	seq_printf(m, "proc %d\n", proc->pid);
+	count = 0;
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n))
+		count++;
+	seq_printf(m, "  threads: %d\n", count);
+	seq_printf(m, "  requested threads: %d+%d/%d\n"
+			"  ready threads %d\n"
+			"  free async space %zd\n", proc->requested_threads,
+			proc->requested_threads_started, proc->max_threads,
+			proc->ready_threads, proc->free_async_space);
+	count = 0;
+	for (n = rb_first(&proc->nodes); n != NULL; n = rb_next(n))
+		count++;
+	seq_printf(m, "  nodes: %d\n", count);
+	count = 0;
+	strong = 0;
+	weak = 0;
+	for (n = rb_first(&proc->refs_by_desc); n != NULL; n = rb_next(n)) {
+		struct binder_ref *ref = rb_entry(n, struct binder_ref,
+						  rb_node_desc);
+		count++;
+		strong += ref->strong;
+		weak += ref->weak;
+	}
+	seq_printf(m, "  refs: %d s %d w %d\n", count, strong, weak);
+
+	count = 0;
+	for (n = rb_first(&proc->allocated_buffers); n != NULL; n = rb_next(n))
+		count++;
+	seq_printf(m, "  buffers: %d\n", count);
+
+	count = 0;
+	list_for_each_entry(w, &proc->todo, entry) {
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION:
+			count++;
+			break;
+		default:
+			break;
+		}
+	}
+	seq_printf(m, "  pending transactions: %d\n", count);
+
+	print_binder_stats(m, "  ", &proc->stats);
+}
+
+
+static int binder_state_show(struct seq_file *m, void *unused)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	struct binder_node *node;
+	int do_lock = !binder_debug_no_lock;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	seq_puts(m, "binder state:\n");
+
+	if (!hlist_empty(&binder_dead_nodes))
+		seq_puts(m, "dead nodes:\n");
+	hlist_for_each_entry(node, pos, &binder_dead_nodes, dead_node)
+		print_binder_node(m, node);
+
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node)
+		print_binder_proc(m, proc, 1);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	return 0;
+}
+
+static int binder_stats_show(struct seq_file *m, void *unused)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	int do_lock = !binder_debug_no_lock;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	seq_puts(m, "binder stats:\n");
+
+	print_binder_stats(m, "", &binder_stats);
+
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node)
+		print_binder_proc_stats(m, proc);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	return 0;
+}
+
+static int binder_transactions_show(struct seq_file *m, void *unused)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	int do_lock = !binder_debug_no_lock;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	seq_puts(m, "binder transactions:\n");
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node)
+		print_binder_proc(m, proc, 0);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	return 0;
+}
+
+static int binder_proc_show(struct seq_file *m, void *unused)
+{
+	struct binder_proc *proc = m->private;
+	int do_lock = !binder_debug_no_lock;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+	seq_puts(m, "binder proc state:\n");
+	print_binder_proc(m, proc, 1);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	return 0;
+}
+
+static void print_binder_transaction_log_entry(struct seq_file *m,
+					struct binder_transaction_log_entry *e)
+{
+	seq_printf(m,
+		   "%d: %s from %d:%d to %d:%d node %d handle %d size %d:%d\n",
+		   e->debug_id, (e->call_type == 2) ? "reply" :
+		   ((e->call_type == 1) ? "async" : "call "), e->from_proc,
+		   e->from_thread, e->to_proc, e->to_thread, e->to_node,
+		   e->target_handle, e->data_size, e->offsets_size);
+}
+
+static int binder_transaction_log_show(struct seq_file *m, void *unused)
+{
+	struct binder_transaction_log *log = m->private;
+	int i;
+
+	if (log->full) {
+		for (i = log->next; i < ARRAY_SIZE(log->entry); i++)
+			print_binder_transaction_log_entry(m, &log->entry[i]);
+	}
+	for (i = 0; i < log->next; i++)
+		print_binder_transaction_log_entry(m, &log->entry[i]);
+	return 0;
+}
+
+static const struct file_operations binder_fops = {
+	.owner = THIS_MODULE,
+	.poll = binder_poll,
+	.unlocked_ioctl = binder_ioctl,
+	.mmap = binder_mmap,
+	.open = binder_open,
+	.flush = binder_flush,
+	.release = binder_release,
+};
+
+static struct miscdevice binder_miscdev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "binder",
+	.fops = &binder_fops
+};
+
+BINDER_DEBUG_ENTRY(state);
+BINDER_DEBUG_ENTRY(stats);
+BINDER_DEBUG_ENTRY(transactions);
+BINDER_DEBUG_ENTRY(transaction_log);
+
+static int __init binder_init(void)
+{
+	int ret;
+
+	binder_deferred_workqueue = create_singlethread_workqueue("binder");
+	if (!binder_deferred_workqueue)
+		return -ENOMEM;
+
+	binder_debugfs_dir_entry_root = debugfs_create_dir("binder", NULL);
+	if (binder_debugfs_dir_entry_root)
+		binder_debugfs_dir_entry_proc = debugfs_create_dir("proc",
+						 binder_debugfs_dir_entry_root);
+	ret = misc_register(&binder_miscdev);
+	if (binder_debugfs_dir_entry_root) {
+		debugfs_create_file("state",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    NULL,
+				    &binder_state_fops);
+		debugfs_create_file("stats",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    NULL,
+				    &binder_stats_fops);
+		debugfs_create_file("transactions",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    NULL,
+				    &binder_transactions_fops);
+		debugfs_create_file("transaction_log",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    &binder_transaction_log,
+				    &binder_transaction_log_fops);
+		debugfs_create_file("failed_transaction_log",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    &binder_transaction_log_failed,
+				    &binder_transaction_log_fops);
+	}
+	return ret;
+}
+
+device_initcall(binder_init);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/android/binder.h b/drivers/staging/android/binder.h
new file mode 100644
index 000000000000..25ab6f2759e9
--- /dev/null
+++ b/drivers/staging/android/binder.h
@@ -0,0 +1,330 @@
+/*
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * Based on, but no longer compatible with, the original
+ * OpenBinder.org binder driver interface, which is:
+ *
+ * Copyright (c) 2005 Palmsource, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _LINUX_BINDER_H
+#define _LINUX_BINDER_H
+
+#include <linux/ioctl.h>
+
+#define B_PACK_CHARS(c1, c2, c3, c4) \
+	((((c1)<<24)) | (((c2)<<16)) | (((c3)<<8)) | (c4))
+#define B_TYPE_LARGE 0x85
+
+enum {
+	BINDER_TYPE_BINDER	= B_PACK_CHARS('s', 'b', '*', B_TYPE_LARGE),
+	BINDER_TYPE_WEAK_BINDER	= B_PACK_CHARS('w', 'b', '*', B_TYPE_LARGE),
+	BINDER_TYPE_HANDLE	= B_PACK_CHARS('s', 'h', '*', B_TYPE_LARGE),
+	BINDER_TYPE_WEAK_HANDLE	= B_PACK_CHARS('w', 'h', '*', B_TYPE_LARGE),
+	BINDER_TYPE_FD		= B_PACK_CHARS('f', 'd', '*', B_TYPE_LARGE),
+};
+
+enum {
+	FLAT_BINDER_FLAG_PRIORITY_MASK = 0xff,
+	FLAT_BINDER_FLAG_ACCEPTS_FDS = 0x100,
+};
+
+/*
+ * This is the flattened representation of a Binder object for transfer
+ * between processes.  The 'offsets' supplied as part of a binder transaction
+ * contains offsets into the data where these structures occur.  The Binder
+ * driver takes care of re-writing the structure type and data as it moves
+ * between processes.
+ */
+struct flat_binder_object {
+	/* 8 bytes for large_flat_header. */
+	unsigned long		type;
+	unsigned long		flags;
+
+	/* 8 bytes of data. */
+	union {
+		void		*binder;	/* local object */
+		signed long	handle;		/* remote object */
+	};
+
+	/* extra data associated with local object */
+	void			*cookie;
+};
+
+/*
+ * On 64-bit platforms where user code may run in 32-bits the driver must
+ * translate the buffer (and local binder) addresses apropriately.
+ */
+
+struct binder_write_read {
+	signed long	write_size;	/* bytes to write */
+	signed long	write_consumed;	/* bytes consumed by driver */
+	unsigned long	write_buffer;
+	signed long	read_size;	/* bytes to read */
+	signed long	read_consumed;	/* bytes consumed by driver */
+	unsigned long	read_buffer;
+};
+
+/* Use with BINDER_VERSION, driver fills in fields. */
+struct binder_version {
+	/* driver protocol version -- increment with incompatible change */
+	signed long	protocol_version;
+};
+
+/* This is the current protocol version. */
+#define BINDER_CURRENT_PROTOCOL_VERSION 7
+
+#define BINDER_WRITE_READ		_IOWR('b', 1, struct binder_write_read)
+#define	BINDER_SET_IDLE_TIMEOUT		_IOW('b', 3, int64_t)
+#define	BINDER_SET_MAX_THREADS		_IOW('b', 5, size_t)
+#define	BINDER_SET_IDLE_PRIORITY	_IOW('b', 6, int)
+#define	BINDER_SET_CONTEXT_MGR		_IOW('b', 7, int)
+#define	BINDER_THREAD_EXIT		_IOW('b', 8, int)
+#define BINDER_VERSION			_IOWR('b', 9, struct binder_version)
+
+/*
+ * NOTE: Two special error codes you should check for when calling
+ * in to the driver are:
+ *
+ * EINTR -- The operation has been interupted.  This should be
+ * handled by retrying the ioctl() until a different error code
+ * is returned.
+ *
+ * ECONNREFUSED -- The driver is no longer accepting operations
+ * from your process.  That is, the process is being destroyed.
+ * You should handle this by exiting from your process.  Note
+ * that once this error code is returned, all further calls to
+ * the driver from any thread will return this same code.
+ */
+
+enum transaction_flags {
+	TF_ONE_WAY	= 0x01,	/* this is a one-way call: async, no return */
+	TF_ROOT_OBJECT	= 0x04,	/* contents are the component's root object */
+	TF_STATUS_CODE	= 0x08,	/* contents are a 32-bit status code */
+	TF_ACCEPT_FDS	= 0x10,	/* allow replies with file descriptors */
+};
+
+struct binder_transaction_data {
+	/* The first two are only used for bcTRANSACTION and brTRANSACTION,
+	 * identifying the target and contents of the transaction.
+	 */
+	union {
+		size_t	handle;	/* target descriptor of command transaction */
+		void	*ptr;	/* target descriptor of return transaction */
+	} target;
+	void		*cookie;	/* target object cookie */
+	unsigned int	code;		/* transaction command */
+
+	/* General information about the transaction. */
+	unsigned int	flags;
+	pid_t		sender_pid;
+	uid_t		sender_euid;
+	size_t		data_size;	/* number of bytes of data */
+	size_t		offsets_size;	/* number of bytes of offsets */
+
+	/* If this transaction is inline, the data immediately
+	 * follows here; otherwise, it ends with a pointer to
+	 * the data buffer.
+	 */
+	union {
+		struct {
+			/* transaction data */
+			const void	*buffer;
+			/* offsets from buffer to flat_binder_object structs */
+			const void	*offsets;
+		} ptr;
+		uint8_t	buf[8];
+	} data;
+};
+
+struct binder_ptr_cookie {
+	void *ptr;
+	void *cookie;
+};
+
+struct binder_pri_desc {
+	int priority;
+	int desc;
+};
+
+struct binder_pri_ptr_cookie {
+	int priority;
+	void *ptr;
+	void *cookie;
+};
+
+enum BinderDriverReturnProtocol {
+	BR_ERROR = _IOR('r', 0, int),
+	/*
+	 * int: error code
+	 */
+
+	BR_OK = _IO('r', 1),
+	/* No parameters! */
+
+	BR_TRANSACTION = _IOR('r', 2, struct binder_transaction_data),
+	BR_REPLY = _IOR('r', 3, struct binder_transaction_data),
+	/*
+	 * binder_transaction_data: the received command.
+	 */
+
+	BR_ACQUIRE_RESULT = _IOR('r', 4, int),
+	/*
+	 * not currently supported
+	 * int: 0 if the last bcATTEMPT_ACQUIRE was not successful.
+	 * Else the remote object has acquired a primary reference.
+	 */
+
+	BR_DEAD_REPLY = _IO('r', 5),
+	/*
+	 * The target of the last transaction (either a bcTRANSACTION or
+	 * a bcATTEMPT_ACQUIRE) is no longer with us.  No parameters.
+	 */
+
+	BR_TRANSACTION_COMPLETE = _IO('r', 6),
+	/*
+	 * No parameters... always refers to the last transaction requested
+	 * (including replies).  Note that this will be sent even for
+	 * asynchronous transactions.
+	 */
+
+	BR_INCREFS = _IOR('r', 7, struct binder_ptr_cookie),
+	BR_ACQUIRE = _IOR('r', 8, struct binder_ptr_cookie),
+	BR_RELEASE = _IOR('r', 9, struct binder_ptr_cookie),
+	BR_DECREFS = _IOR('r', 10, struct binder_ptr_cookie),
+	/*
+	 * void *:	ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BR_ATTEMPT_ACQUIRE = _IOR('r', 11, struct binder_pri_ptr_cookie),
+	/*
+	 * not currently supported
+	 * int:	priority
+	 * void *: ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BR_NOOP = _IO('r', 12),
+	/*
+	 * No parameters.  Do nothing and examine the next command.  It exists
+	 * primarily so that we can replace it with a BR_SPAWN_LOOPER command.
+	 */
+
+	BR_SPAWN_LOOPER = _IO('r', 13),
+	/*
+	 * No parameters.  The driver has determined that a process has no
+	 * threads waiting to service incomming transactions.  When a process
+	 * receives this command, it must spawn a new service thread and
+	 * register it via bcENTER_LOOPER.
+	 */
+
+	BR_FINISHED = _IO('r', 14),
+	/*
+	 * not currently supported
+	 * stop threadpool thread
+	 */
+
+	BR_DEAD_BINDER = _IOR('r', 15, void *),
+	/*
+	 * void *: cookie
+	 */
+	BR_CLEAR_DEATH_NOTIFICATION_DONE = _IOR('r', 16, void *),
+	/*
+	 * void *: cookie
+	 */
+
+	BR_FAILED_REPLY = _IO('r', 17),
+	/*
+	 * The the last transaction (either a bcTRANSACTION or
+	 * a bcATTEMPT_ACQUIRE) failed (e.g. out of memory).  No parameters.
+	 */
+};
+
+enum BinderDriverCommandProtocol {
+	BC_TRANSACTION = _IOW('c', 0, struct binder_transaction_data),
+	BC_REPLY = _IOW('c', 1, struct binder_transaction_data),
+	/*
+	 * binder_transaction_data: the sent command.
+	 */
+
+	BC_ACQUIRE_RESULT = _IOW('c', 2, int),
+	/*
+	 * not currently supported
+	 * int:  0 if the last BR_ATTEMPT_ACQUIRE was not successful.
+	 * Else you have acquired a primary reference on the object.
+	 */
+
+	BC_FREE_BUFFER = _IOW('c', 3, int),
+	/*
+	 * void *: ptr to transaction data received on a read
+	 */
+
+	BC_INCREFS = _IOW('c', 4, int),
+	BC_ACQUIRE = _IOW('c', 5, int),
+	BC_RELEASE = _IOW('c', 6, int),
+	BC_DECREFS = _IOW('c', 7, int),
+	/*
+	 * int:	descriptor
+	 */
+
+	BC_INCREFS_DONE = _IOW('c', 8, struct binder_ptr_cookie),
+	BC_ACQUIRE_DONE = _IOW('c', 9, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BC_ATTEMPT_ACQUIRE = _IOW('c', 10, struct binder_pri_desc),
+	/*
+	 * not currently supported
+	 * int: priority
+	 * int: descriptor
+	 */
+
+	BC_REGISTER_LOOPER = _IO('c', 11),
+	/*
+	 * No parameters.
+	 * Register a spawned looper thread with the device.
+	 */
+
+	BC_ENTER_LOOPER = _IO('c', 12),
+	BC_EXIT_LOOPER = _IO('c', 13),
+	/*
+	 * No parameters.
+	 * These two commands are sent as an application-level thread
+	 * enters and exits the binder loop, respectively.  They are
+	 * used so the binder can have an accurate count of the number
+	 * of looping threads it has available.
+	 */
+
+	BC_REQUEST_DEATH_NOTIFICATION = _IOW('c', 14, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie
+	 */
+
+	BC_CLEAR_DEATH_NOTIFICATION = _IOW('c', 15, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie
+	 */
+
+	BC_DEAD_BINDER_DONE = _IOW('c', 16, void *),
+	/*
+	 * void *: cookie
+	 */
+};
+
+#endif /* _LINUX_BINDER_H */
+
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
new file mode 100644
index 000000000000..ffc2d043dd8e
--- /dev/null
+++ b/drivers/staging/android/logger.c
@@ -0,0 +1,616 @@
+/*
+ * drivers/misc/logger.c
+ *
+ * A Logging Subsystem
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * Robert Love <rlove@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/sched.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include "logger.h"
+
+#include <asm/ioctls.h>
+
+/*
+ * struct logger_log - represents a specific log, such as 'main' or 'radio'
+ *
+ * This structure lives from module insertion until module removal, so it does
+ * not need additional reference counting. The structure is protected by the
+ * mutex 'mutex'.
+ */
+struct logger_log {
+	unsigned char		*buffer;/* the ring buffer itself */
+	struct miscdevice	misc;	/* misc device representing the log */
+	wait_queue_head_t	wq;	/* wait queue for readers */
+	struct list_head	readers; /* this log's readers */
+	struct mutex		mutex;	/* mutex protecting buffer */
+	size_t			w_off;	/* current write head offset */
+	size_t			head;	/* new readers start here */
+	size_t			size;	/* size of the log */
+};
+
+/*
+ * struct logger_reader - a logging device open for reading
+ *
+ * This object lives from open to release, so we don't need additional
+ * reference counting. The structure is protected by log->mutex.
+ */
+struct logger_reader {
+	struct logger_log	*log;	/* associated log */
+	struct list_head	list;	/* entry in logger_log's list */
+	size_t			r_off;	/* current read head offset */
+};
+
+/* logger_offset - returns index 'n' into the log via (optimized) modulus */
+#define logger_offset(n)	((n) & (log->size - 1))
+
+/*
+ * file_get_log - Given a file structure, return the associated log
+ *
+ * This isn't aesthetic. We have several goals:
+ *
+ *	1) Need to quickly obtain the associated log during an I/O operation
+ *	2) Readers need to maintain state (logger_reader)
+ *	3) Writers need to be very fast (open() should be a near no-op)
+ *
+ * In the reader case, we can trivially go file->logger_reader->logger_log.
+ * For a writer, we don't want to maintain a logger_reader, so we just go
+ * file->logger_log. Thus what file->private_data points at depends on whether
+ * or not the file was opened for reading. This function hides that dirtiness.
+ */
+static inline struct logger_log *file_get_log(struct file *file)
+{
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader = file->private_data;
+		return reader->log;
+	} else
+		return file->private_data;
+}
+
+/*
+ * get_entry_len - Grabs the length of the payload of the next entry starting
+ * from 'off'.
+ *
+ * Caller needs to hold log->mutex.
+ */
+static __u32 get_entry_len(struct logger_log *log, size_t off)
+{
+	__u16 val;
+
+	switch (log->size - off) {
+	case 1:
+		memcpy(&val, log->buffer + off, 1);
+		memcpy(((char *) &val) + 1, log->buffer, 1);
+		break;
+	default:
+		memcpy(&val, log->buffer + off, 2);
+	}
+
+	return sizeof(struct logger_entry) + val;
+}
+
+/*
+ * do_read_log_to_user - reads exactly 'count' bytes from 'log' into the
+ * user-space buffer 'buf'. Returns 'count' on success.
+ *
+ * Caller must hold log->mutex.
+ */
+static ssize_t do_read_log_to_user(struct logger_log *log,
+				   struct logger_reader *reader,
+				   char __user *buf,
+				   size_t count)
+{
+	size_t len;
+
+	/*
+	 * We read from the log in two disjoint operations. First, we read from
+	 * the current read head offset up to 'count' bytes or to the end of
+	 * the log, whichever comes first.
+	 */
+	len = min(count, log->size - reader->r_off);
+	if (copy_to_user(buf, log->buffer + reader->r_off, len))
+		return -EFAULT;
+
+	/*
+	 * Second, we read any remaining bytes, starting back at the head of
+	 * the log.
+	 */
+	if (count != len)
+		if (copy_to_user(buf + len, log->buffer, count - len))
+			return -EFAULT;
+
+	reader->r_off = logger_offset(reader->r_off + count);
+
+	return count;
+}
+
+/*
+ * logger_read - our log's read() method
+ *
+ * Behavior:
+ *
+ *	- O_NONBLOCK works
+ *	- If there are no log entries to read, blocks until log is written to
+ *	- Atomically reads exactly one log entry
+ *
+ * Optimal read size is LOGGER_ENTRY_MAX_LEN. Will set errno to EINVAL if read
+ * buffer is insufficient to hold next entry.
+ */
+static ssize_t logger_read(struct file *file, char __user *buf,
+			   size_t count, loff_t *pos)
+{
+	struct logger_reader *reader = file->private_data;
+	struct logger_log *log = reader->log;
+	ssize_t ret;
+	DEFINE_WAIT(wait);
+
+start:
+	while (1) {
+		prepare_to_wait(&log->wq, &wait, TASK_INTERRUPTIBLE);
+
+		mutex_lock(&log->mutex);
+		ret = (log->w_off == reader->r_off);
+		mutex_unlock(&log->mutex);
+		if (!ret)
+			break;
+
+		if (file->f_flags & O_NONBLOCK) {
+			ret = -EAGAIN;
+			break;
+		}
+
+		if (signal_pending(current)) {
+			ret = -EINTR;
+			break;
+		}
+
+		schedule();
+	}
+
+	finish_wait(&log->wq, &wait);
+	if (ret)
+		return ret;
+
+	mutex_lock(&log->mutex);
+
+	/* is there still something to read or did we race? */
+	if (unlikely(log->w_off == reader->r_off)) {
+		mutex_unlock(&log->mutex);
+		goto start;
+	}
+
+	/* get the size of the next entry */
+	ret = get_entry_len(log, reader->r_off);
+	if (count < ret) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* get exactly one entry from the log */
+	ret = do_read_log_to_user(log, reader, buf, ret);
+
+out:
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+/*
+ * get_next_entry - return the offset of the first valid entry at least 'len'
+ * bytes after 'off'.
+ *
+ * Caller must hold log->mutex.
+ */
+static size_t get_next_entry(struct logger_log *log, size_t off, size_t len)
+{
+	size_t count = 0;
+
+	do {
+		size_t nr = get_entry_len(log, off);
+		off = logger_offset(off + nr);
+		count += nr;
+	} while (count < len);
+
+	return off;
+}
+
+/*
+ * clock_interval - is a < c < b in mod-space? Put another way, does the line
+ * from a to b cross c?
+ */
+static inline int clock_interval(size_t a, size_t b, size_t c)
+{
+	if (b < a) {
+		if (a < c || b >= c)
+			return 1;
+	} else {
+		if (a < c && b >= c)
+			return 1;
+	}
+
+	return 0;
+}
+
+/*
+ * fix_up_readers - walk the list of all readers and "fix up" any who were
+ * lapped by the writer; also do the same for the default "start head".
+ * We do this by "pulling forward" the readers and start head to the first
+ * entry after the new write head.
+ *
+ * The caller needs to hold log->mutex.
+ */
+static void fix_up_readers(struct logger_log *log, size_t len)
+{
+	size_t old = log->w_off;
+	size_t new = logger_offset(old + len);
+	struct logger_reader *reader;
+
+	if (clock_interval(old, new, log->head))
+		log->head = get_next_entry(log, log->head, len);
+
+	list_for_each_entry(reader, &log->readers, list)
+		if (clock_interval(old, new, reader->r_off))
+			reader->r_off = get_next_entry(log, reader->r_off, len);
+}
+
+/*
+ * do_write_log - writes 'len' bytes from 'buf' to 'log'
+ *
+ * The caller needs to hold log->mutex.
+ */
+static void do_write_log(struct logger_log *log, const void *buf, size_t count)
+{
+	size_t len;
+
+	len = min(count, log->size - log->w_off);
+	memcpy(log->buffer + log->w_off, buf, len);
+
+	if (count != len)
+		memcpy(log->buffer, buf + len, count - len);
+
+	log->w_off = logger_offset(log->w_off + count);
+
+}
+
+/*
+ * do_write_log_user - writes 'len' bytes from the user-space buffer 'buf' to
+ * the log 'log'
+ *
+ * The caller needs to hold log->mutex.
+ *
+ * Returns 'count' on success, negative error code on failure.
+ */
+static ssize_t do_write_log_from_user(struct logger_log *log,
+				      const void __user *buf, size_t count)
+{
+	size_t len;
+
+	len = min(count, log->size - log->w_off);
+	if (len && copy_from_user(log->buffer + log->w_off, buf, len))
+		return -EFAULT;
+
+	if (count != len)
+		if (copy_from_user(log->buffer, buf + len, count - len))
+			return -EFAULT;
+
+	log->w_off = logger_offset(log->w_off + count);
+
+	return count;
+}
+
+/*
+ * logger_aio_write - our write method, implementing support for write(),
+ * writev(), and aio_write(). Writes are our fast path, and we try to optimize
+ * them above all else.
+ */
+ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
+			 unsigned long nr_segs, loff_t ppos)
+{
+	struct logger_log *log = file_get_log(iocb->ki_filp);
+	size_t orig = log->w_off;
+	struct logger_entry header;
+	struct timespec now;
+	ssize_t ret = 0;
+
+	now = current_kernel_time();
+
+	header.pid = current->tgid;
+	header.tid = current->pid;
+	header.sec = now.tv_sec;
+	header.nsec = now.tv_nsec;
+	header.len = min_t(size_t, iocb->ki_left, LOGGER_ENTRY_MAX_PAYLOAD);
+
+	/* null writes succeed, return zero */
+	if (unlikely(!header.len))
+		return 0;
+
+	mutex_lock(&log->mutex);
+
+	/*
+	 * Fix up any readers, pulling them forward to the first readable
+	 * entry after (what will be) the new write offset. We do this now
+	 * because if we partially fail, we can end up with clobbered log
+	 * entries that encroach on readable buffer.
+	 */
+	fix_up_readers(log, sizeof(struct logger_entry) + header.len);
+
+	do_write_log(log, &header, sizeof(struct logger_entry));
+
+	while (nr_segs-- > 0) {
+		size_t len;
+		ssize_t nr;
+
+		/* figure out how much of this vector we can keep */
+		len = min_t(size_t, iov->iov_len, header.len - ret);
+
+		/* write out this segment's payload */
+		nr = do_write_log_from_user(log, iov->iov_base, len);
+		if (unlikely(nr < 0)) {
+			log->w_off = orig;
+			mutex_unlock(&log->mutex);
+			return nr;
+		}
+
+		iov++;
+		ret += nr;
+	}
+
+	mutex_unlock(&log->mutex);
+
+	/* wake up any blocked readers */
+	wake_up_interruptible(&log->wq);
+
+	return ret;
+}
+
+static struct logger_log *get_log_from_minor(int);
+
+/*
+ * logger_open - the log's open() file operation
+ *
+ * Note how near a no-op this is in the write-only case. Keep it that way!
+ */
+static int logger_open(struct inode *inode, struct file *file)
+{
+	struct logger_log *log;
+	int ret;
+
+	ret = nonseekable_open(inode, file);
+	if (ret)
+		return ret;
+
+	log = get_log_from_minor(MINOR(inode->i_rdev));
+	if (!log)
+		return -ENODEV;
+
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader;
+
+		reader = kmalloc(sizeof(struct logger_reader), GFP_KERNEL);
+		if (!reader)
+			return -ENOMEM;
+
+		reader->log = log;
+		INIT_LIST_HEAD(&reader->list);
+
+		mutex_lock(&log->mutex);
+		reader->r_off = log->head;
+		list_add_tail(&reader->list, &log->readers);
+		mutex_unlock(&log->mutex);
+
+		file->private_data = reader;
+	} else
+		file->private_data = log;
+
+	return 0;
+}
+
+/*
+ * logger_release - the log's release file operation
+ *
+ * Note this is a total no-op in the write-only case. Keep it that way!
+ */
+static int logger_release(struct inode *ignored, struct file *file)
+{
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader = file->private_data;
+		list_del(&reader->list);
+		kfree(reader);
+	}
+
+	return 0;
+}
+
+/*
+ * logger_poll - the log's poll file operation, for poll/select/epoll
+ *
+ * Note we always return POLLOUT, because you can always write() to the log.
+ * Note also that, strictly speaking, a return value of POLLIN does not
+ * guarantee that the log is readable without blocking, as there is a small
+ * chance that the writer can lap the reader in the interim between poll()
+ * returning and the read() request.
+ */
+static unsigned int logger_poll(struct file *file, poll_table *wait)
+{
+	struct logger_reader *reader;
+	struct logger_log *log;
+	unsigned int ret = POLLOUT | POLLWRNORM;
+
+	if (!(file->f_mode & FMODE_READ))
+		return ret;
+
+	reader = file->private_data;
+	log = reader->log;
+
+	poll_wait(file, &log->wq, wait);
+
+	mutex_lock(&log->mutex);
+	if (log->w_off != reader->r_off)
+		ret |= POLLIN | POLLRDNORM;
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+static long logger_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct logger_log *log = file_get_log(file);
+	struct logger_reader *reader;
+	long ret = -ENOTTY;
+
+	mutex_lock(&log->mutex);
+
+	switch (cmd) {
+	case LOGGER_GET_LOG_BUF_SIZE:
+		ret = log->size;
+		break;
+	case LOGGER_GET_LOG_LEN:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		if (log->w_off >= reader->r_off)
+			ret = log->w_off - reader->r_off;
+		else
+			ret = (log->size - reader->r_off) + log->w_off;
+		break;
+	case LOGGER_GET_NEXT_ENTRY_LEN:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		if (log->w_off != reader->r_off)
+			ret = get_entry_len(log, reader->r_off);
+		else
+			ret = 0;
+		break;
+	case LOGGER_FLUSH_LOG:
+		if (!(file->f_mode & FMODE_WRITE)) {
+			ret = -EBADF;
+			break;
+		}
+		list_for_each_entry(reader, &log->readers, list)
+			reader->r_off = log->w_off;
+		log->head = log->w_off;
+		ret = 0;
+		break;
+	}
+
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+static const struct file_operations logger_fops = {
+	.owner = THIS_MODULE,
+	.read = logger_read,
+	.aio_write = logger_aio_write,
+	.poll = logger_poll,
+	.unlocked_ioctl = logger_ioctl,
+	.compat_ioctl = logger_ioctl,
+	.open = logger_open,
+	.release = logger_release,
+};
+
+/*
+ * Defines a log structure with name 'NAME' and a size of 'SIZE' bytes, which
+ * must be a power of two, greater than LOGGER_ENTRY_MAX_LEN, and less than
+ * LONG_MAX minus LOGGER_ENTRY_MAX_LEN.
+ */
+#define DEFINE_LOGGER_DEVICE(VAR, NAME, SIZE) \
+static unsigned char _buf_ ## VAR[SIZE]; \
+static struct logger_log VAR = { \
+	.buffer = _buf_ ## VAR, \
+	.misc = { \
+		.minor = MISC_DYNAMIC_MINOR, \
+		.name = NAME, \
+		.fops = &logger_fops, \
+		.parent = NULL, \
+	}, \
+	.wq = __WAIT_QUEUE_HEAD_INITIALIZER(VAR .wq), \
+	.readers = LIST_HEAD_INIT(VAR .readers), \
+	.mutex = __MUTEX_INITIALIZER(VAR .mutex), \
+	.w_off = 0, \
+	.head = 0, \
+	.size = SIZE, \
+};
+
+DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 256*1024)
+DEFINE_LOGGER_DEVICE(log_events, LOGGER_LOG_EVENTS, 256*1024)
+DEFINE_LOGGER_DEVICE(log_radio, LOGGER_LOG_RADIO, 256*1024)
+DEFINE_LOGGER_DEVICE(log_system, LOGGER_LOG_SYSTEM, 256*1024)
+
+static struct logger_log *get_log_from_minor(int minor)
+{
+	if (log_main.misc.minor == minor)
+		return &log_main;
+	if (log_events.misc.minor == minor)
+		return &log_events;
+	if (log_radio.misc.minor == minor)
+		return &log_radio;
+	if (log_system.misc.minor == minor)
+		return &log_system;
+	return NULL;
+}
+
+static int __init init_log(struct logger_log *log)
+{
+	int ret;
+
+	ret = misc_register(&log->misc);
+	if (unlikely(ret)) {
+		printk(KERN_ERR "logger: failed to register misc "
+		       "device for log '%s'!\n", log->misc.name);
+		return ret;
+	}
+
+	printk(KERN_INFO "logger: created %luK log '%s'\n",
+	       (unsigned long) log->size >> 10, log->misc.name);
+
+	return 0;
+}
+
+static int __init logger_init(void)
+{
+	int ret;
+
+	ret = init_log(&log_main);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_events);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_radio);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_system);
+	if (unlikely(ret))
+		goto out;
+
+out:
+	return ret;
+}
+device_initcall(logger_init);
diff --git a/drivers/staging/android/logger.h b/drivers/staging/android/logger.h
new file mode 100644
index 000000000000..2cb06e9d8f98
--- /dev/null
+++ b/drivers/staging/android/logger.h
@@ -0,0 +1,49 @@
+/* include/linux/logger.h
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ * Author: Robert Love <rlove@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _LINUX_LOGGER_H
+#define _LINUX_LOGGER_H
+
+#include <linux/types.h>
+#include <linux/ioctl.h>
+
+struct logger_entry {
+	__u16		len;	/* length of the payload */
+	__u16		__pad;	/* no matter what, we get 2 bytes of padding */
+	__s32		pid;	/* generating process's pid */
+	__s32		tid;	/* generating process's tid */
+	__s32		sec;	/* seconds since Epoch */
+	__s32		nsec;	/* nanoseconds */
+	char		msg[0];	/* the entry's payload */
+};
+
+#define LOGGER_LOG_RADIO	"log_radio"	/* radio-related messages */
+#define LOGGER_LOG_EVENTS	"log_events"	/* system/hardware events */
+#define LOGGER_LOG_SYSTEM	"log_system"	/* system/framework messages */
+#define LOGGER_LOG_MAIN		"log_main"	/* everything else */
+
+#define LOGGER_ENTRY_MAX_LEN		(4*1024)
+#define LOGGER_ENTRY_MAX_PAYLOAD	\
+	(LOGGER_ENTRY_MAX_LEN - sizeof(struct logger_entry))
+
+#define __LOGGERIO	0xAE
+
+#define LOGGER_GET_LOG_BUF_SIZE		_IO(__LOGGERIO, 1) /* size of log */
+#define LOGGER_GET_LOG_LEN		_IO(__LOGGERIO, 2) /* used log len */
+#define LOGGER_GET_NEXT_ENTRY_LEN	_IO(__LOGGERIO, 3) /* next entry len */
+#define LOGGER_FLUSH_LOG		_IO(__LOGGERIO, 4) /* flush log */
+
+#endif /* _LINUX_LOGGER_H */
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
new file mode 100644
index 000000000000..2d8d2b796101
--- /dev/null
+++ b/drivers/staging/android/lowmemorykiller.c
@@ -0,0 +1,219 @@
+/* drivers/misc/lowmemorykiller.c
+ *
+ * The lowmemorykiller driver lets user-space specify a set of memory thresholds
+ * where processes with a range of oom_adj values will get killed. Specify the
+ * minimum oom_adj values in /sys/module/lowmemorykiller/parameters/adj and the
+ * number of free pages in /sys/module/lowmemorykiller/parameters/minfree. Both
+ * files take a comma separated list of numbers in ascending order.
+ *
+ * For example, write "0,8" to /sys/module/lowmemorykiller/parameters/adj and
+ * "1024,4096" to /sys/module/lowmemorykiller/parameters/minfree to kill
+ * processes with a oom_adj value of 8 or higher when the free memory drops
+ * below 4096 pages and kill processes with a oom_adj value of 0 or higher
+ * when the free memory drops below 1024 pages.
+ *
+ * The driver considers memory used for caches to be free, but if a large
+ * percentage of the cached memory is locked this can be very inaccurate
+ * and processes may not get killed until the normal oom killer is triggered.
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/oom.h>
+#include <linux/sched.h>
+#include <linux/profile.h>
+#include <linux/notifier.h>
+
+static uint32_t lowmem_debug_level = 2;
+static int lowmem_adj[6] = {
+	0,
+	1,
+	6,
+	12,
+};
+static int lowmem_adj_size = 4;
+static size_t lowmem_minfree[6] = {
+	3 * 512,	/* 6MB */
+	2 * 1024,	/* 8MB */
+	4 * 1024,	/* 16MB */
+	16 * 1024,	/* 64MB */
+};
+static int lowmem_minfree_size = 4;
+
+static struct task_struct *lowmem_deathpending;
+
+#define lowmem_print(level, x...)			\
+	do {						\
+		if (lowmem_debug_level >= (level))	\
+			printk(x);			\
+	} while (0)
+
+static int
+task_notify_func(struct notifier_block *self, unsigned long val, void *data);
+
+static struct notifier_block task_nb = {
+	.notifier_call	= task_notify_func,
+};
+
+static int
+task_notify_func(struct notifier_block *self, unsigned long val, void *data)
+{
+	struct task_struct *task = data;
+	if (task == lowmem_deathpending) {
+		lowmem_deathpending = NULL;
+		task_handoff_unregister(&task_nb);
+	}
+	return NOTIFY_OK;
+}
+
+static int lowmem_shrink(struct shrinker *s, struct shrink_control *sc)
+{
+	struct task_struct *p;
+	struct task_struct *selected = NULL;
+	int rem = 0;
+	int tasksize;
+	int i;
+	int min_adj = OOM_ADJUST_MAX + 1;
+	int selected_tasksize = 0;
+	int selected_oom_adj;
+	int array_size = ARRAY_SIZE(lowmem_adj);
+	int other_free = global_page_state(NR_FREE_PAGES);
+	int other_file = global_page_state(NR_FILE_PAGES) -
+						global_page_state(NR_SHMEM);
+
+	/*
+	 * If we already have a death outstanding, then
+	 * bail out right away; indicating to vmscan
+	 * that we have nothing further to offer on
+	 * this pass.
+	 *
+	 * Note: Currently you need CONFIG_PROFILING
+	 * for this to work correctly.
+	 */
+	if (lowmem_deathpending)
+		return 0;
+
+	if (lowmem_adj_size < array_size)
+		array_size = lowmem_adj_size;
+	if (lowmem_minfree_size < array_size)
+		array_size = lowmem_minfree_size;
+	for (i = 0; i < array_size; i++) {
+		if (other_free < lowmem_minfree[i] &&
+		    other_file < lowmem_minfree[i]) {
+			min_adj = lowmem_adj[i];
+			break;
+		}
+	}
+	if (sc->nr_to_scan > 0)
+		lowmem_print(3, "lowmem_shrink %lu, %x, ofree %d %d, ma %d\n",
+				sc->nr_to_scan, sc->gfp_mask, other_free,
+				other_file, min_adj);
+	rem = global_page_state(NR_ACTIVE_ANON) +
+		global_page_state(NR_ACTIVE_FILE) +
+		global_page_state(NR_INACTIVE_ANON) +
+		global_page_state(NR_INACTIVE_FILE);
+	if (sc->nr_to_scan <= 0 || min_adj == OOM_ADJUST_MAX + 1) {
+		lowmem_print(5, "lowmem_shrink %lu, %x, return %d\n",
+			     sc->nr_to_scan, sc->gfp_mask, rem);
+		return rem;
+	}
+	selected_oom_adj = min_adj;
+
+	read_lock(&tasklist_lock);
+	for_each_process(p) {
+		struct mm_struct *mm;
+		struct signal_struct *sig;
+		int oom_adj;
+
+		task_lock(p);
+		mm = p->mm;
+		sig = p->signal;
+		if (!mm || !sig) {
+			task_unlock(p);
+			continue;
+		}
+		oom_adj = sig->oom_adj;
+		if (oom_adj < min_adj) {
+			task_unlock(p);
+			continue;
+		}
+		tasksize = get_mm_rss(mm);
+		task_unlock(p);
+		if (tasksize <= 0)
+			continue;
+		if (selected) {
+			if (oom_adj < selected_oom_adj)
+				continue;
+			if (oom_adj == selected_oom_adj &&
+			    tasksize <= selected_tasksize)
+				continue;
+		}
+		selected = p;
+		selected_tasksize = tasksize;
+		selected_oom_adj = oom_adj;
+		lowmem_print(2, "select %d (%s), adj %d, size %d, to kill\n",
+			     p->pid, p->comm, oom_adj, tasksize);
+	}
+	if (selected) {
+		lowmem_print(1, "send sigkill to %d (%s), adj %d, size %d\n",
+			     selected->pid, selected->comm,
+			     selected_oom_adj, selected_tasksize);
+		/*
+		 * If CONFIG_PROFILING is off, then task_handoff_register()
+		 * is a nop. In that case we don't want to stall the killer
+		 * by setting lowmem_deathpending.
+		 */
+#ifdef CONFIG_PROFILING
+		lowmem_deathpending = selected;
+		task_handoff_register(&task_nb);
+#endif
+		force_sig(SIGKILL, selected);
+		rem -= selected_tasksize;
+	}
+	lowmem_print(4, "lowmem_shrink %lu, %x, return %d\n",
+		     sc->nr_to_scan, sc->gfp_mask, rem);
+	read_unlock(&tasklist_lock);
+	return rem;
+}
+
+static struct shrinker lowmem_shrinker = {
+	.shrink = lowmem_shrink,
+	.seeks = DEFAULT_SEEKS * 16
+};
+
+static int __init lowmem_init(void)
+{
+	register_shrinker(&lowmem_shrinker);
+	return 0;
+}
+
+static void __exit lowmem_exit(void)
+{
+	unregister_shrinker(&lowmem_shrinker);
+}
+
+module_param_named(cost, lowmem_shrinker.seeks, int, S_IRUGO | S_IWUSR);
+module_param_array_named(adj, lowmem_adj, int, &lowmem_adj_size,
+			 S_IRUGO | S_IWUSR);
+module_param_array_named(minfree, lowmem_minfree, uint, &lowmem_minfree_size,
+			 S_IRUGO | S_IWUSR);
+module_param_named(debug_level, lowmem_debug_level, uint, S_IRUGO | S_IWUSR);
+
+module_init(lowmem_init);
+module_exit(lowmem_exit);
+
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/staging/android/pmem.c b/drivers/staging/android/pmem.c
new file mode 100644
index 000000000000..7d97032c6508
--- /dev/null
+++ b/drivers/staging/android/pmem.c
@@ -0,0 +1,1345 @@
+/* pmem.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/mm.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/debugfs.h>
+#include <linux/mempolicy.h>
+#include <linux/sched.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/cacheflush.h>
+#include "android_pmem.h"
+
+#define PMEM_MAX_DEVICES 10
+#define PMEM_MAX_ORDER 128
+#define PMEM_MIN_ALLOC PAGE_SIZE
+
+#define PMEM_DEBUG 1
+
+/* indicates that a refernce to this file has been taken via get_pmem_file,
+ * the file should not be released until put_pmem_file is called */
+#define PMEM_FLAGS_BUSY 0x1
+/* indicates that this is a suballocation of a larger master range */
+#define PMEM_FLAGS_CONNECTED 0x1 << 1
+/* indicates this is a master and not a sub allocation and that it is mmaped */
+#define PMEM_FLAGS_MASTERMAP 0x1 << 2
+/* submap and unsubmap flags indicate:
+ * 00: subregion has never been mmaped
+ * 10: subregion has been mmaped, reference to the mm was taken
+ * 11: subretion has ben released, refernece to the mm still held
+ * 01: subretion has been released, reference to the mm has been released
+ */
+#define PMEM_FLAGS_SUBMAP 0x1 << 3
+#define PMEM_FLAGS_UNSUBMAP 0x1 << 4
+
+
+struct pmem_data {
+	/* in alloc mode: an index into the bitmap
+	 * in no_alloc mode: the size of the allocation */
+	int index;
+	/* see flags above for descriptions */
+	unsigned int flags;
+	/* protects this data field, if the mm_mmap sem will be held at the
+	 * same time as this sem, the mm sem must be taken first (as this is
+	 * the order for vma_open and vma_close ops */
+	struct rw_semaphore sem;
+	/* info about the mmaping process */
+	struct vm_area_struct *vma;
+	/* task struct of the mapping process */
+	struct task_struct *task;
+	/* process id of teh mapping process */
+	pid_t pid;
+	/* file descriptor of the master */
+	int master_fd;
+	/* file struct of the master */
+	struct file *master_file;
+	/* a list of currently available regions if this is a suballocation */
+	struct list_head region_list;
+	/* a linked list of data so we can access them for debugging */
+	struct list_head list;
+#if PMEM_DEBUG
+	int ref;
+#endif
+};
+
+struct pmem_bits {
+	unsigned allocated:1;		/* 1 if allocated, 0 if free */
+	unsigned order:7;		/* size of the region in pmem space */
+};
+
+struct pmem_region_node {
+	struct pmem_region region;
+	struct list_head list;
+};
+
+#define PMEM_DEBUG_MSGS 0
+#if PMEM_DEBUG_MSGS
+#define DLOG(fmt,args...) \
+	do { printk(KERN_INFO "[%s:%s:%d] "fmt, __FILE__, __func__, __LINE__, \
+		    ##args); } \
+	while (0)
+#else
+#define DLOG(x...) do {} while (0)
+#endif
+
+struct pmem_info {
+	struct miscdevice dev;
+	/* physical start address of the remaped pmem space */
+	unsigned long base;
+	/* vitual start address of the remaped pmem space */
+	unsigned char __iomem *vbase;
+	/* total size of the pmem space */
+	unsigned long size;
+	/* number of entries in the pmem space */
+	unsigned long num_entries;
+	/* pfn of the garbage page in memory */
+	unsigned long garbage_pfn;
+	/* index of the garbage page in the pmem space */
+	int garbage_index;
+	/* the bitmap for the region indicating which entries are allocated
+	 * and which are free */
+	struct pmem_bits *bitmap;
+	/* indicates the region should not be managed with an allocator */
+	unsigned no_allocator;
+	/* indicates maps of this region should be cached, if a mix of
+	 * cached and uncached is desired, set this and open the device with
+	 * O_SYNC to get an uncached region */
+	unsigned cached;
+	unsigned buffered;
+	/* in no_allocator mode the first mapper gets the whole space and sets
+	 * this flag */
+	unsigned allocated;
+	/* for debugging, creates a list of pmem file structs, the
+	 * data_list_lock should be taken before pmem_data->sem if both are
+	 * needed */
+	struct mutex data_list_lock;
+	struct list_head data_list;
+	/* pmem_sem protects the bitmap array
+	 * a write lock should be held when modifying entries in bitmap
+	 * a read lock should be held when reading data from bits or
+	 * dereferencing a pointer into bitmap
+	 *
+	 * pmem_data->sem protects the pmem data of a particular file
+	 * Many of the function that require the pmem_data->sem have a non-
+	 * locking version for when the caller is already holding that sem.
+	 *
+	 * IF YOU TAKE BOTH LOCKS TAKE THEM IN THIS ORDER:
+	 * down(pmem_data->sem) => down(bitmap_sem)
+	 */
+	struct rw_semaphore bitmap_sem;
+
+	long (*ioctl)(struct file *, unsigned int, unsigned long);
+	int (*release)(struct inode *, struct file *);
+};
+
+static struct pmem_info pmem[PMEM_MAX_DEVICES];
+static int id_count;
+
+#define PMEM_IS_FREE(id, index) !(pmem[id].bitmap[index].allocated)
+#define PMEM_ORDER(id, index) pmem[id].bitmap[index].order
+#define PMEM_BUDDY_INDEX(id, index) (index ^ (1 << PMEM_ORDER(id, index)))
+#define PMEM_NEXT_INDEX(id, index) (index + (1 << PMEM_ORDER(id, index)))
+#define PMEM_OFFSET(index) (index * PMEM_MIN_ALLOC)
+#define PMEM_START_ADDR(id, index) (PMEM_OFFSET(index) + pmem[id].base)
+#define PMEM_LEN(id, index) ((1 << PMEM_ORDER(id, index)) * PMEM_MIN_ALLOC)
+#define PMEM_END_ADDR(id, index) (PMEM_START_ADDR(id, index) + \
+	PMEM_LEN(id, index))
+#define PMEM_START_VADDR(id, index) (PMEM_OFFSET(id, index) + pmem[id].vbase)
+#define PMEM_END_VADDR(id, index) (PMEM_START_VADDR(id, index) + \
+	PMEM_LEN(id, index))
+#define PMEM_REVOKED(data) (data->flags & PMEM_FLAGS_REVOKED)
+#define PMEM_IS_PAGE_ALIGNED(addr) (!((addr) & (~PAGE_MASK)))
+#define PMEM_IS_SUBMAP(data) ((data->flags & PMEM_FLAGS_SUBMAP) && \
+	(!(data->flags & PMEM_FLAGS_UNSUBMAP)))
+
+static int pmem_release(struct inode *, struct file *);
+static int pmem_mmap(struct file *, struct vm_area_struct *);
+static int pmem_open(struct inode *, struct file *);
+static long pmem_ioctl(struct file *, unsigned int, unsigned long);
+
+struct file_operations pmem_fops = {
+	.release = pmem_release,
+	.mmap = pmem_mmap,
+	.open = pmem_open,
+	.unlocked_ioctl = pmem_ioctl,
+};
+
+static int get_id(struct file *file)
+{
+	return MINOR(file->f_dentry->d_inode->i_rdev);
+}
+
+int is_pmem_file(struct file *file)
+{
+	int id;
+
+	if (unlikely(!file || !file->f_dentry || !file->f_dentry->d_inode))
+		return 0;
+	id = get_id(file);
+	if (unlikely(id >= PMEM_MAX_DEVICES))
+		return 0;
+	if (unlikely(file->f_dentry->d_inode->i_rdev !=
+	     MKDEV(MISC_MAJOR, pmem[id].dev.minor)))
+		return 0;
+	return 1;
+}
+
+static int has_allocation(struct file *file)
+{
+	struct pmem_data *data;
+	/* check is_pmem_file first if not accessed via pmem_file_ops */
+
+	if (unlikely(!file->private_data))
+		return 0;
+	data = (struct pmem_data *)file->private_data;
+	if (unlikely(data->index < 0))
+		return 0;
+	return 1;
+}
+
+static int is_master_owner(struct file *file)
+{
+	struct file *master_file;
+	struct pmem_data *data;
+	int put_needed, ret = 0;
+
+	if (!is_pmem_file(file) || !has_allocation(file))
+		return 0;
+	data = (struct pmem_data *)file->private_data;
+	if (PMEM_FLAGS_MASTERMAP & data->flags)
+		return 1;
+	master_file = fget_light(data->master_fd, &put_needed);
+	if (master_file && data->master_file == master_file)
+		ret = 1;
+	fput_light(master_file, put_needed);
+	return ret;
+}
+
+static int pmem_free(int id, int index)
+{
+	/* caller should hold the write lock on pmem_sem! */
+	int buddy, curr = index;
+	DLOG("index %d\n", index);
+
+	if (pmem[id].no_allocator) {
+		pmem[id].allocated = 0;
+		return 0;
+	}
+	/* clean up the bitmap, merging any buddies */
+	pmem[id].bitmap[curr].allocated = 0;
+	/* find a slots buddy Buddy# = Slot# ^ (1 << order)
+	 * if the buddy is also free merge them
+	 * repeat until the buddy is not free or end of the bitmap is reached
+	 */
+	do {
+		buddy = PMEM_BUDDY_INDEX(id, curr);
+		if (PMEM_IS_FREE(id, buddy) &&
+				PMEM_ORDER(id, buddy) == PMEM_ORDER(id, curr)) {
+			PMEM_ORDER(id, buddy)++;
+			PMEM_ORDER(id, curr)++;
+			curr = min(buddy, curr);
+		} else {
+			break;
+		}
+	} while (curr < pmem[id].num_entries);
+
+	return 0;
+}
+
+static void pmem_revoke(struct file *file, struct pmem_data *data);
+
+static int pmem_release(struct inode *inode, struct file *file)
+{
+	struct pmem_data *data = (struct pmem_data *)file->private_data;
+	struct pmem_region_node *region_node;
+	struct list_head *elt, *elt2;
+	int id = get_id(file), ret = 0;
+
+
+	mutex_lock(&pmem[id].data_list_lock);
+	/* if this file is a master, revoke all the memory in the connected
+	 *  files */
+	if (PMEM_FLAGS_MASTERMAP & data->flags) {
+		struct pmem_data *sub_data;
+		list_for_each(elt, &pmem[id].data_list) {
+			sub_data = list_entry(elt, struct pmem_data, list);
+			down_read(&sub_data->sem);
+			if (PMEM_IS_SUBMAP(sub_data) &&
+			    file == sub_data->master_file) {
+				up_read(&sub_data->sem);
+				pmem_revoke(file, sub_data);
+			}  else
+				up_read(&sub_data->sem);
+		}
+	}
+	list_del(&data->list);
+	mutex_unlock(&pmem[id].data_list_lock);
+
+
+	down_write(&data->sem);
+
+	/* if its not a conencted file and it has an allocation, free it */
+	if (!(PMEM_FLAGS_CONNECTED & data->flags) && has_allocation(file)) {
+		down_write(&pmem[id].bitmap_sem);
+		ret = pmem_free(id, data->index);
+		up_write(&pmem[id].bitmap_sem);
+	}
+
+	/* if this file is a submap (mapped, connected file), downref the
+	 * task struct */
+	if (PMEM_FLAGS_SUBMAP & data->flags)
+		if (data->task) {
+			put_task_struct(data->task);
+			data->task = NULL;
+		}
+
+	file->private_data = NULL;
+
+	list_for_each_safe(elt, elt2, &data->region_list) {
+		region_node = list_entry(elt, struct pmem_region_node, list);
+		list_del(elt);
+		kfree(region_node);
+	}
+	BUG_ON(!list_empty(&data->region_list));
+
+	up_write(&data->sem);
+	kfree(data);
+	if (pmem[id].release)
+		ret = pmem[id].release(inode, file);
+
+	return ret;
+}
+
+static int pmem_open(struct inode *inode, struct file *file)
+{
+	struct pmem_data *data;
+	int id = get_id(file);
+	int ret = 0;
+
+	DLOG("current %u file %p(%d)\n", current->pid, file, file_count(file));
+	/* setup file->private_data to indicate its unmapped */
+	/*  you can only open a pmem device one time */
+	if (file->private_data != NULL)
+		return -1;
+	data = kmalloc(sizeof(struct pmem_data), GFP_KERNEL);
+	if (!data) {
+		printk("pmem: unable to allocate memory for pmem metadata.");
+		return -1;
+	}
+	data->flags = 0;
+	data->index = -1;
+	data->task = NULL;
+	data->vma = NULL;
+	data->pid = 0;
+	data->master_file = NULL;
+#if PMEM_DEBUG
+	data->ref = 0;
+#endif
+	INIT_LIST_HEAD(&data->region_list);
+	init_rwsem(&data->sem);
+
+	file->private_data = data;
+	INIT_LIST_HEAD(&data->list);
+
+	mutex_lock(&pmem[id].data_list_lock);
+	list_add(&data->list, &pmem[id].data_list);
+	mutex_unlock(&pmem[id].data_list_lock);
+	return ret;
+}
+
+static unsigned long pmem_order(unsigned long len)
+{
+	int i;
+
+	len = (len + PMEM_MIN_ALLOC - 1)/PMEM_MIN_ALLOC;
+	len--;
+	for (i = 0; i < sizeof(len)*8; i++)
+		if (len >> i == 0)
+			break;
+	return i;
+}
+
+static int pmem_allocate(int id, unsigned long len)
+{
+	/* caller should hold the write lock on pmem_sem! */
+	/* return the corresponding pdata[] entry */
+	int curr = 0;
+	int end = pmem[id].num_entries;
+	int best_fit = -1;
+	unsigned long order = pmem_order(len);
+
+	if (pmem[id].no_allocator) {
+		DLOG("no allocator");
+		if ((len > pmem[id].size) || pmem[id].allocated)
+			return -1;
+		pmem[id].allocated = 1;
+		return len;
+	}
+
+	if (order > PMEM_MAX_ORDER)
+		return -1;
+	DLOG("order %lx\n", order);
+
+	/* look through the bitmap:
+	 * 	if you find a free slot of the correct order use it
+	 * 	otherwise, use the best fit (smallest with size > order) slot
+	 */
+	while (curr < end) {
+		if (PMEM_IS_FREE(id, curr)) {
+			if (PMEM_ORDER(id, curr) == (unsigned char)order) {
+				/* set the not free bit and clear others */
+				best_fit = curr;
+				break;
+			}
+			if (PMEM_ORDER(id, curr) > (unsigned char)order &&
+			    (best_fit < 0 ||
+			     PMEM_ORDER(id, curr) < PMEM_ORDER(id, best_fit)))
+				best_fit = curr;
+		}
+		curr = PMEM_NEXT_INDEX(id, curr);
+	}
+
+	/* if best_fit < 0, there are no suitable slots,
+	 * return an error
+	 */
+	if (best_fit < 0) {
+		printk("pmem: no space left to allocate!\n");
+		return -1;
+	}
+
+	/* now partition the best fit:
+	 * 	split the slot into 2 buddies of order - 1
+	 * 	repeat until the slot is of the correct order
+	 */
+	while (PMEM_ORDER(id, best_fit) > (unsigned char)order) {
+		int buddy;
+		PMEM_ORDER(id, best_fit) -= 1;
+		buddy = PMEM_BUDDY_INDEX(id, best_fit);
+		PMEM_ORDER(id, buddy) = PMEM_ORDER(id, best_fit);
+	}
+	pmem[id].bitmap[best_fit].allocated = 1;
+	return best_fit;
+}
+
+static pgprot_t pmem_access_prot(struct file *file, pgprot_t vma_prot)
+{
+	int id = get_id(file);
+#ifdef pgprot_noncached
+	if (pmem[id].cached == 0 || file->f_flags & O_SYNC)
+		return pgprot_noncached(vma_prot);
+#endif
+#ifdef pgprot_ext_buffered
+	else if (pmem[id].buffered)
+		return pgprot_ext_buffered(vma_prot);
+#endif
+	return vma_prot;
+}
+
+static unsigned long pmem_start_addr(int id, struct pmem_data *data)
+{
+	if (pmem[id].no_allocator)
+		return PMEM_START_ADDR(id, 0);
+	else
+		return PMEM_START_ADDR(id, data->index);
+
+}
+
+static void *pmem_start_vaddr(int id, struct pmem_data *data)
+{
+	return pmem_start_addr(id, data) - pmem[id].base + pmem[id].vbase;
+}
+
+static unsigned long pmem_len(int id, struct pmem_data *data)
+{
+	if (pmem[id].no_allocator)
+		return data->index;
+	else
+		return PMEM_LEN(id, data->index);
+}
+
+static int pmem_map_garbage(int id, struct vm_area_struct *vma,
+			    struct pmem_data *data, unsigned long offset,
+			    unsigned long len)
+{
+	int i, garbage_pages = len >> PAGE_SHIFT;
+
+	vma->vm_flags |= VM_IO | VM_RESERVED | VM_PFNMAP | VM_SHARED | VM_WRITE;
+	for (i = 0; i < garbage_pages; i++) {
+		if (vm_insert_pfn(vma, vma->vm_start + offset + (i * PAGE_SIZE),
+		    pmem[id].garbage_pfn))
+			return -EAGAIN;
+	}
+	return 0;
+}
+
+static int pmem_unmap_pfn_range(int id, struct vm_area_struct *vma,
+				struct pmem_data *data, unsigned long offset,
+				unsigned long len)
+{
+	int garbage_pages;
+	DLOG("unmap offset %lx len %lx\n", offset, len);
+
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(len));
+
+	garbage_pages = len >> PAGE_SHIFT;
+	zap_page_range(vma, vma->vm_start + offset, len, NULL);
+	pmem_map_garbage(id, vma, data, offset, len);
+	return 0;
+}
+
+static int pmem_map_pfn_range(int id, struct vm_area_struct *vma,
+			      struct pmem_data *data, unsigned long offset,
+			      unsigned long len)
+{
+	DLOG("map offset %lx len %lx\n", offset, len);
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(vma->vm_start));
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(vma->vm_end));
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(len));
+	BUG_ON(!PMEM_IS_PAGE_ALIGNED(offset));
+
+	if (io_remap_pfn_range(vma, vma->vm_start + offset,
+		(pmem_start_addr(id, data) + offset) >> PAGE_SHIFT,
+		len, vma->vm_page_prot)) {
+		return -EAGAIN;
+	}
+	return 0;
+}
+
+static int pmem_remap_pfn_range(int id, struct vm_area_struct *vma,
+			      struct pmem_data *data, unsigned long offset,
+			      unsigned long len)
+{
+	/* hold the mm semp for the vma you are modifying when you call this */
+	BUG_ON(!vma);
+	zap_page_range(vma, vma->vm_start + offset, len, NULL);
+	return pmem_map_pfn_range(id, vma, data, offset, len);
+}
+
+static void pmem_vma_open(struct vm_area_struct *vma)
+{
+	struct file *file = vma->vm_file;
+	struct pmem_data *data = file->private_data;
+	int id = get_id(file);
+	/* this should never be called as we don't support copying pmem
+	 * ranges via fork */
+	BUG_ON(!has_allocation(file));
+	down_write(&data->sem);
+	/* remap the garbage pages, forkers don't get access to the data */
+	pmem_unmap_pfn_range(id, vma, data, 0, vma->vm_start - vma->vm_end);
+	up_write(&data->sem);
+}
+
+static void pmem_vma_close(struct vm_area_struct *vma)
+{
+	struct file *file = vma->vm_file;
+	struct pmem_data *data = file->private_data;
+
+	DLOG("current %u ppid %u file %p count %d\n", current->pid,
+	     current->parent->pid, file, file_count(file));
+	if (unlikely(!is_pmem_file(file) || !has_allocation(file))) {
+		printk(KERN_WARNING "pmem: something is very wrong, you are "
+		       "closing a vm backing an allocation that doesn't "
+		       "exist!\n");
+		return;
+	}
+	down_write(&data->sem);
+	if (data->vma == vma) {
+		data->vma = NULL;
+		if ((data->flags & PMEM_FLAGS_CONNECTED) &&
+		    (data->flags & PMEM_FLAGS_SUBMAP))
+			data->flags |= PMEM_FLAGS_UNSUBMAP;
+	}
+	/* the kernel is going to free this vma now anyway */
+	up_write(&data->sem);
+}
+
+static struct vm_operations_struct vm_ops = {
+	.open = pmem_vma_open,
+	.close = pmem_vma_close,
+};
+
+static int pmem_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct pmem_data *data;
+	int index;
+	unsigned long vma_size =  vma->vm_end - vma->vm_start;
+	int ret = 0, id = get_id(file);
+
+	if (vma->vm_pgoff || !PMEM_IS_PAGE_ALIGNED(vma_size)) {
+#if PMEM_DEBUG
+		printk(KERN_ERR "pmem: mmaps must be at offset zero, aligned"
+				" and a multiple of pages_size.\n");
+#endif
+		return -EINVAL;
+	}
+
+	data = (struct pmem_data *)file->private_data;
+	down_write(&data->sem);
+	/* check this file isn't already mmaped, for submaps check this file
+	 * has never been mmaped */
+	if ((data->flags & PMEM_FLAGS_SUBMAP) ||
+	    (data->flags & PMEM_FLAGS_UNSUBMAP)) {
+#if PMEM_DEBUG
+		printk(KERN_ERR "pmem: you can only mmap a pmem file once, "
+		       "this file is already mmaped. %x\n", data->flags);
+#endif
+		ret = -EINVAL;
+		goto error;
+	}
+	/* if file->private_data == unalloced, alloc*/
+	if (data && data->index == -1) {
+		down_write(&pmem[id].bitmap_sem);
+		index = pmem_allocate(id, vma->vm_end - vma->vm_start);
+		up_write(&pmem[id].bitmap_sem);
+		data->index = index;
+	}
+	/* either no space was available or an error occured */
+	if (!has_allocation(file)) {
+		ret = -EINVAL;
+		printk("pmem: could not find allocation for map.\n");
+		goto error;
+	}
+
+	if (pmem_len(id, data) < vma_size) {
+#if PMEM_DEBUG
+		printk(KERN_WARNING "pmem: mmap size [%lu] does not match"
+		       "size of backing region [%lu].\n", vma_size,
+		       pmem_len(id, data));
+#endif
+		ret = -EINVAL;
+		goto error;
+	}
+
+	vma->vm_pgoff = pmem_start_addr(id, data) >> PAGE_SHIFT;
+	vma->vm_page_prot = pmem_access_prot(file, vma->vm_page_prot);
+
+	if (data->flags & PMEM_FLAGS_CONNECTED) {
+		struct pmem_region_node *region_node;
+		struct list_head *elt;
+		if (pmem_map_garbage(id, vma, data, 0, vma_size)) {
+			printk("pmem: mmap failed in kernel!\n");
+			ret = -EAGAIN;
+			goto error;
+		}
+		list_for_each(elt, &data->region_list) {
+			region_node = list_entry(elt, struct pmem_region_node,
+						 list);
+			DLOG("remapping file: %p %lx %lx\n", file,
+				region_node->region.offset,
+				region_node->region.len);
+			if (pmem_remap_pfn_range(id, vma, data,
+						 region_node->region.offset,
+						 region_node->region.len)) {
+				ret = -EAGAIN;
+				goto error;
+			}
+		}
+		data->flags |= PMEM_FLAGS_SUBMAP;
+		get_task_struct(current->group_leader);
+		data->task = current->group_leader;
+		data->vma = vma;
+#if PMEM_DEBUG
+		data->pid = current->pid;
+#endif
+		DLOG("submmapped file %p vma %p pid %u\n", file, vma,
+		     current->pid);
+	} else {
+		if (pmem_map_pfn_range(id, vma, data, 0, vma_size)) {
+			printk(KERN_INFO "pmem: mmap failed in kernel!\n");
+			ret = -EAGAIN;
+			goto error;
+		}
+		data->flags |= PMEM_FLAGS_MASTERMAP;
+		data->pid = current->pid;
+	}
+	vma->vm_ops = &vm_ops;
+error:
+	up_write(&data->sem);
+	return ret;
+}
+
+/* the following are the api for accessing pmem regions by other drivers
+ * from inside the kernel */
+int get_pmem_user_addr(struct file *file, unsigned long *start,
+		   unsigned long *len)
+{
+	struct pmem_data *data;
+	if (!is_pmem_file(file) || !has_allocation(file)) {
+#if PMEM_DEBUG
+		printk(KERN_INFO "pmem: requested pmem data from invalid"
+				  "file.\n");
+#endif
+		return -1;
+	}
+	data = (struct pmem_data *)file->private_data;
+	down_read(&data->sem);
+	if (data->vma) {
+		*start = data->vma->vm_start;
+		*len = data->vma->vm_end - data->vma->vm_start;
+	} else {
+		*start = 0;
+		*len = 0;
+	}
+	up_read(&data->sem);
+	return 0;
+}
+
+int get_pmem_addr(struct file *file, unsigned long *start,
+		  unsigned long *vstart, unsigned long *len)
+{
+	struct pmem_data *data;
+	int id;
+
+	if (!is_pmem_file(file) || !has_allocation(file)) {
+		return -1;
+	}
+
+	data = (struct pmem_data *)file->private_data;
+	if (data->index == -1) {
+#if PMEM_DEBUG
+		printk(KERN_INFO "pmem: requested pmem data from file with no "
+		       "allocation.\n");
+		return -1;
+#endif
+	}
+	id = get_id(file);
+
+	down_read(&data->sem);
+	*start = pmem_start_addr(id, data);
+	*len = pmem_len(id, data);
+	*vstart = (unsigned long)pmem_start_vaddr(id, data);
+	up_read(&data->sem);
+#if PMEM_DEBUG
+	down_write(&data->sem);
+	data->ref++;
+	up_write(&data->sem);
+#endif
+	return 0;
+}
+
+int get_pmem_file(int fd, unsigned long *start, unsigned long *vstart,
+		  unsigned long *len, struct file **filp)
+{
+	struct file *file;
+
+	file = fget(fd);
+	if (unlikely(file == NULL)) {
+		printk(KERN_INFO "pmem: requested data from file descriptor "
+		       "that doesn't exist.");
+		return -1;
+	}
+
+	if (get_pmem_addr(file, start, vstart, len))
+		goto end;
+
+	if (filp)
+		*filp = file;
+	return 0;
+end:
+	fput(file);
+	return -1;
+}
+
+void put_pmem_file(struct file *file)
+{
+	struct pmem_data *data;
+	int id;
+
+	if (!is_pmem_file(file))
+		return;
+	id = get_id(file);
+	data = (struct pmem_data *)file->private_data;
+#if PMEM_DEBUG
+	down_write(&data->sem);
+	if (data->ref == 0) {
+		printk("pmem: pmem_put > pmem_get %s (pid %d)\n",
+		       pmem[id].dev.name, data->pid);
+		BUG();
+	}
+	data->ref--;
+	up_write(&data->sem);
+#endif
+	fput(file);
+}
+
+void flush_pmem_file(struct file *file, unsigned long offset, unsigned long len)
+{
+	struct pmem_data *data;
+	int id;
+	void *vaddr;
+	struct pmem_region_node *region_node;
+	struct list_head *elt;
+	void *flush_start, *flush_end;
+
+	if (!is_pmem_file(file) || !has_allocation(file)) {
+		return;
+	}
+
+	id = get_id(file);
+	data = (struct pmem_data *)file->private_data;
+	if (!pmem[id].cached || file->f_flags & O_SYNC)
+		return;
+
+	down_read(&data->sem);
+	vaddr = pmem_start_vaddr(id, data);
+	/* if this isn't a submmapped file, flush the whole thing */
+	if (unlikely(!(data->flags & PMEM_FLAGS_CONNECTED))) {
+		dmac_flush_range(vaddr, vaddr + pmem_len(id, data));
+		goto end;
+	}
+	/* otherwise, flush the region of the file we are drawing */
+	list_for_each(elt, &data->region_list) {
+		region_node = list_entry(elt, struct pmem_region_node, list);
+		if ((offset >= region_node->region.offset) &&
+		    ((offset + len) <= (region_node->region.offset +
+			region_node->region.len))) {
+			flush_start = vaddr + region_node->region.offset;
+			flush_end = flush_start + region_node->region.len;
+			dmac_flush_range(flush_start, flush_end);
+			break;
+		}
+	}
+end:
+	up_read(&data->sem);
+}
+
+static int pmem_connect(unsigned long connect, struct file *file)
+{
+	struct pmem_data *data = (struct pmem_data *)file->private_data;
+	struct pmem_data *src_data;
+	struct file *src_file;
+	int ret = 0, put_needed;
+
+	down_write(&data->sem);
+	/* retrieve the src file and check it is a pmem file with an alloc */
+	src_file = fget_light(connect, &put_needed);
+	DLOG("connect %p to %p\n", file, src_file);
+	if (!src_file) {
+		printk("pmem: src file not found!\n");
+		ret = -EINVAL;
+		goto err_no_file;
+	}
+	if (unlikely(!is_pmem_file(src_file) || !has_allocation(src_file))) {
+		printk(KERN_INFO "pmem: src file is not a pmem file or has no "
+		       "alloc!\n");
+		ret = -EINVAL;
+		goto err_bad_file;
+	}
+	src_data = (struct pmem_data *)src_file->private_data;
+
+	if (has_allocation(file) && (data->index != src_data->index)) {
+		printk("pmem: file is already mapped but doesn't match this"
+		       " src_file!\n");
+		ret = -EINVAL;
+		goto err_bad_file;
+	}
+	data->index = src_data->index;
+	data->flags |= PMEM_FLAGS_CONNECTED;
+	data->master_fd = connect;
+	data->master_file = src_file;
+
+err_bad_file:
+	fput_light(src_file, put_needed);
+err_no_file:
+	up_write(&data->sem);
+	return ret;
+}
+
+static void pmem_unlock_data_and_mm(struct pmem_data *data,
+				    struct mm_struct *mm)
+{
+	up_write(&data->sem);
+	if (mm != NULL) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+}
+
+static int pmem_lock_data_and_mm(struct file *file, struct pmem_data *data,
+				 struct mm_struct **locked_mm)
+{
+	int ret = 0;
+	struct mm_struct *mm = NULL;
+	*locked_mm = NULL;
+lock_mm:
+	down_read(&data->sem);
+	if (PMEM_IS_SUBMAP(data)) {
+		mm = get_task_mm(data->task);
+		if (!mm) {
+#if PMEM_DEBUG
+			printk("pmem: can't remap task is gone!\n");
+#endif
+			up_read(&data->sem);
+			return -1;
+		}
+	}
+	up_read(&data->sem);
+
+	if (mm)
+		down_write(&mm->mmap_sem);
+
+	down_write(&data->sem);
+	/* check that the file didn't get mmaped before we could take the
+	 * data sem, this should be safe b/c you can only submap each file
+	 * once */
+	if (PMEM_IS_SUBMAP(data) && !mm) {
+		pmem_unlock_data_and_mm(data, mm);
+		up_write(&data->sem);
+		goto lock_mm;
+	}
+	/* now check that vma.mm is still there, it could have been
+	 * deleted by vma_close before we could get the data->sem */
+	if ((data->flags & PMEM_FLAGS_UNSUBMAP) && (mm != NULL)) {
+		/* might as well release this */
+		if (data->flags & PMEM_FLAGS_SUBMAP) {
+			put_task_struct(data->task);
+			data->task = NULL;
+			/* lower the submap flag to show the mm is gone */
+			data->flags &= ~(PMEM_FLAGS_SUBMAP);
+		}
+		pmem_unlock_data_and_mm(data, mm);
+		return -1;
+	}
+	*locked_mm = mm;
+	return ret;
+}
+
+int pmem_remap(struct pmem_region *region, struct file *file,
+		      unsigned operation)
+{
+	int ret;
+	struct pmem_region_node *region_node;
+	struct mm_struct *mm = NULL;
+	struct list_head *elt, *elt2;
+	int id = get_id(file);
+	struct pmem_data *data = (struct pmem_data *)file->private_data;
+
+	/* pmem region must be aligned on a page boundry */
+	if (unlikely(!PMEM_IS_PAGE_ALIGNED(region->offset) ||
+		 !PMEM_IS_PAGE_ALIGNED(region->len))) {
+#if PMEM_DEBUG
+		printk("pmem: request for unaligned pmem suballocation "
+		       "%lx %lx\n", region->offset, region->len);
+#endif
+		return -EINVAL;
+	}
+
+	/* if userspace requests a region of len 0, there's nothing to do */
+	if (region->len == 0)
+		return 0;
+
+	/* lock the mm and data */
+	ret = pmem_lock_data_and_mm(file, data, &mm);
+	if (ret)
+		return 0;
+
+	/* only the owner of the master file can remap the client fds
+	 * that back in it */
+	if (!is_master_owner(file)) {
+#if PMEM_DEBUG
+		printk("pmem: remap requested from non-master process\n");
+#endif
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* check that the requested range is within the src allocation */
+	if (unlikely((region->offset > pmem_len(id, data)) ||
+		     (region->len > pmem_len(id, data)) ||
+		     (region->offset + region->len > pmem_len(id, data)))) {
+#if PMEM_DEBUG
+		printk(KERN_INFO "pmem: suballoc doesn't fit in src_file!\n");
+#endif
+		ret = -EINVAL;
+		goto err;
+	}
+
+	if (operation == PMEM_MAP) {
+		region_node = kmalloc(sizeof(struct pmem_region_node),
+			      GFP_KERNEL);
+		if (!region_node) {
+			ret = -ENOMEM;
+#if PMEM_DEBUG
+			printk(KERN_INFO "No space to allocate metadata!");
+#endif
+			goto err;
+		}
+		region_node->region = *region;
+		list_add(&region_node->list, &data->region_list);
+	} else if (operation == PMEM_UNMAP) {
+		int found = 0;
+		list_for_each_safe(elt, elt2, &data->region_list) {
+			region_node = list_entry(elt, struct pmem_region_node,
+				      list);
+			if (region->len == 0 ||
+			    (region_node->region.offset == region->offset &&
+			    region_node->region.len == region->len)) {
+				list_del(elt);
+				kfree(region_node);
+				found = 1;
+			}
+		}
+		if (!found) {
+#if PMEM_DEBUG
+			printk("pmem: Unmap region does not map any mapped "
+				"region!");
+#endif
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+
+	if (data->vma && PMEM_IS_SUBMAP(data)) {
+		if (operation == PMEM_MAP)
+			ret = pmem_remap_pfn_range(id, data->vma, data,
+						   region->offset, region->len);
+		else if (operation == PMEM_UNMAP)
+			ret = pmem_unmap_pfn_range(id, data->vma, data,
+						   region->offset, region->len);
+	}
+
+err:
+	pmem_unlock_data_and_mm(data, mm);
+	return ret;
+}
+
+static void pmem_revoke(struct file *file, struct pmem_data *data)
+{
+	struct pmem_region_node *region_node;
+	struct list_head *elt, *elt2;
+	struct mm_struct *mm = NULL;
+	int id = get_id(file);
+	int ret = 0;
+
+	data->master_file = NULL;
+	ret = pmem_lock_data_and_mm(file, data, &mm);
+	/* if lock_data_and_mm fails either the task that mapped the fd, or
+	 * the vma that mapped it have already gone away, nothing more
+	 * needs to be done */
+	if (ret)
+		return;
+	/* unmap everything */
+	/* delete the regions and region list nothing is mapped any more */
+	if (data->vma)
+		list_for_each_safe(elt, elt2, &data->region_list) {
+			region_node = list_entry(elt, struct pmem_region_node,
+						 list);
+			pmem_unmap_pfn_range(id, data->vma, data,
+					     region_node->region.offset,
+					     region_node->region.len);
+			list_del(elt);
+			kfree(region_node);
+	}
+	/* delete the master file */
+	pmem_unlock_data_and_mm(data, mm);
+}
+
+static void pmem_get_size(struct pmem_region *region, struct file *file)
+{
+	struct pmem_data *data = (struct pmem_data *)file->private_data;
+	int id = get_id(file);
+
+	if (!has_allocation(file)) {
+		region->offset = 0;
+		region->len = 0;
+		return;
+	} else {
+		region->offset = pmem_start_addr(id, data);
+		region->len = pmem_len(id, data);
+	}
+	DLOG("offset %lx len %lx\n", region->offset, region->len);
+}
+
+
+static long pmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct pmem_data *data;
+	int id = get_id(file);
+
+	switch (cmd) {
+	case PMEM_GET_PHYS:
+		{
+			struct pmem_region region;
+			DLOG("get_phys\n");
+			if (!has_allocation(file)) {
+				region.offset = 0;
+				region.len = 0;
+			} else {
+				data = (struct pmem_data *)file->private_data;
+				region.offset = pmem_start_addr(id, data);
+				region.len = pmem_len(id, data);
+			}
+			printk(KERN_INFO "pmem: request for physical address of pmem region "
+					"from process %d.\n", current->pid);
+			if (copy_to_user((void __user *)arg, &region,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			break;
+		}
+	case PMEM_MAP:
+		{
+			struct pmem_region region;
+			if (copy_from_user(&region, (void __user *)arg,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			data = (struct pmem_data *)file->private_data;
+			return pmem_remap(&region, file, PMEM_MAP);
+		}
+		break;
+	case PMEM_UNMAP:
+		{
+			struct pmem_region region;
+			if (copy_from_user(&region, (void __user *)arg,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			data = (struct pmem_data *)file->private_data;
+			return pmem_remap(&region, file, PMEM_UNMAP);
+			break;
+		}
+	case PMEM_GET_SIZE:
+		{
+			struct pmem_region region;
+			DLOG("get_size\n");
+			pmem_get_size(&region, file);
+			if (copy_to_user((void __user *)arg, &region,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			break;
+		}
+	case PMEM_GET_TOTAL_SIZE:
+		{
+			struct pmem_region region;
+			DLOG("get total size\n");
+			region.offset = 0;
+			get_id(file);
+			region.len = pmem[id].size;
+			if (copy_to_user((void __user *)arg, &region,
+						sizeof(struct pmem_region)))
+				return -EFAULT;
+			break;
+		}
+	case PMEM_ALLOCATE:
+		{
+			if (has_allocation(file))
+				return -EINVAL;
+			data = (struct pmem_data *)file->private_data;
+			data->index = pmem_allocate(id, arg);
+			break;
+		}
+	case PMEM_CONNECT:
+		DLOG("connect\n");
+		return pmem_connect(arg, file);
+		break;
+	case PMEM_CACHE_FLUSH:
+		{
+			struct pmem_region region;
+			DLOG("flush\n");
+			if (copy_from_user(&region, (void __user *)arg,
+					   sizeof(struct pmem_region)))
+				return -EFAULT;
+			flush_pmem_file(file, region.offset, region.len);
+			break;
+		}
+	default:
+		if (pmem[id].ioctl)
+			return pmem[id].ioctl(file, cmd, arg);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+#if PMEM_DEBUG
+static ssize_t debug_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t debug_read(struct file *file, char __user *buf, size_t count,
+			  loff_t *ppos)
+{
+	struct list_head *elt, *elt2;
+	struct pmem_data *data;
+	struct pmem_region_node *region_node;
+	int id = (int)file->private_data;
+	const int debug_bufmax = 4096;
+	static char buffer[4096];
+	int n = 0;
+
+	DLOG("debug open\n");
+	n = scnprintf(buffer, debug_bufmax,
+		      "pid #: mapped regions (offset, len) (offset,len)...\n");
+
+	mutex_lock(&pmem[id].data_list_lock);
+	list_for_each(elt, &pmem[id].data_list) {
+		data = list_entry(elt, struct pmem_data, list);
+		down_read(&data->sem);
+		n += scnprintf(buffer + n, debug_bufmax - n, "pid %u:",
+				data->pid);
+		list_for_each(elt2, &data->region_list) {
+			region_node = list_entry(elt2, struct pmem_region_node,
+				      list);
+			n += scnprintf(buffer + n, debug_bufmax - n,
+					"(%lx,%lx) ",
+					region_node->region.offset,
+					region_node->region.len);
+		}
+		n += scnprintf(buffer + n, debug_bufmax - n, "\n");
+		up_read(&data->sem);
+	}
+	mutex_unlock(&pmem[id].data_list_lock);
+
+	n++;
+	buffer[n] = 0;
+	return simple_read_from_buffer(buf, count, ppos, buffer, n);
+}
+
+static struct file_operations debug_fops = {
+	.read = debug_read,
+	.open = debug_open,
+};
+#endif
+
+#if 0
+static struct miscdevice pmem_dev = {
+	.name = "pmem",
+	.fops = &pmem_fops,
+};
+#endif
+
+int pmem_setup(struct android_pmem_platform_data *pdata,
+	       long (*ioctl)(struct file *, unsigned int, unsigned long),
+	       int (*release)(struct inode *, struct file *))
+{
+	int err = 0;
+	int i, index = 0;
+	int id = id_count;
+	id_count++;
+
+	pmem[id].no_allocator = pdata->no_allocator;
+	pmem[id].cached = pdata->cached;
+	pmem[id].buffered = pdata->buffered;
+	pmem[id].base = pdata->start;
+	pmem[id].size = pdata->size;
+	pmem[id].ioctl = ioctl;
+	pmem[id].release = release;
+	init_rwsem(&pmem[id].bitmap_sem);
+	mutex_init(&pmem[id].data_list_lock);
+	INIT_LIST_HEAD(&pmem[id].data_list);
+	pmem[id].dev.name = pdata->name;
+	pmem[id].dev.minor = id;
+	pmem[id].dev.fops = &pmem_fops;
+	printk(KERN_INFO "%s: %d init\n", pdata->name, pdata->cached);
+
+	err = misc_register(&pmem[id].dev);
+	if (err) {
+		printk(KERN_ALERT "Unable to register pmem driver!\n");
+		goto err_cant_register_device;
+	}
+	pmem[id].num_entries = pmem[id].size / PMEM_MIN_ALLOC;
+
+	pmem[id].bitmap = kmalloc(pmem[id].num_entries *
+				  sizeof(struct pmem_bits), GFP_KERNEL);
+	if (!pmem[id].bitmap)
+		goto err_no_mem_for_metadata;
+
+	memset(pmem[id].bitmap, 0, sizeof(struct pmem_bits) *
+					  pmem[id].num_entries);
+
+	for (i = sizeof(pmem[id].num_entries) * 8 - 1; i >= 0; i--) {
+		if ((pmem[id].num_entries) &  1<<i) {
+			PMEM_ORDER(id, index) = i;
+			index = PMEM_NEXT_INDEX(id, index);
+		}
+	}
+
+	if (pmem[id].cached)
+		pmem[id].vbase = ioremap_cached(pmem[id].base,
+						pmem[id].size);
+#ifdef ioremap_ext_buffered
+	else if (pmem[id].buffered)
+		pmem[id].vbase = ioremap_ext_buffered(pmem[id].base,
+						      pmem[id].size);
+#endif
+	else
+		pmem[id].vbase = ioremap(pmem[id].base, pmem[id].size);
+
+	if (pmem[id].vbase == 0)
+		goto error_cant_remap;
+
+	pmem[id].garbage_pfn = page_to_pfn(alloc_page(GFP_KERNEL));
+	if (pmem[id].no_allocator)
+		pmem[id].allocated = 0;
+
+#if PMEM_DEBUG
+	debugfs_create_file(pdata->name, S_IFREG | S_IRUGO, NULL, (void *)id,
+			    &debug_fops);
+#endif
+	return 0;
+error_cant_remap:
+	kfree(pmem[id].bitmap);
+err_no_mem_for_metadata:
+	misc_deregister(&pmem[id].dev);
+err_cant_register_device:
+	return -1;
+}
+
+static int pmem_probe(struct platform_device *pdev)
+{
+	struct android_pmem_platform_data *pdata;
+
+	if (!pdev || !pdev->dev.platform_data) {
+		printk(KERN_ALERT "Unable to probe pmem!\n");
+		return -1;
+	}
+	pdata = pdev->dev.platform_data;
+	return pmem_setup(pdata, NULL, NULL);
+}
+
+
+static int pmem_remove(struct platform_device *pdev)
+{
+	int id = pdev->id;
+	__free_page(pfn_to_page(pmem[id].garbage_pfn));
+	misc_deregister(&pmem[id].dev);
+	return 0;
+}
+
+static struct platform_driver pmem_driver = {
+	.probe = pmem_probe,
+	.remove = pmem_remove,
+	.driver = { .name = "android_pmem" }
+};
+
+
+static int __init pmem_init(void)
+{
+	return platform_driver_register(&pmem_driver);
+}
+
+static void __exit pmem_exit(void)
+{
+	platform_driver_unregister(&pmem_driver);
+}
+
+module_init(pmem_init);
+module_exit(pmem_exit);
+
diff --git a/drivers/staging/android/ram_console.c b/drivers/staging/android/ram_console.c
new file mode 100644
index 000000000000..6d4d67924f22
--- /dev/null
+++ b/drivers/staging/android/ram_console.c
@@ -0,0 +1,443 @@
+/* drivers/android/ram_console.c
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/console.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include "ram_console.h"
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+#include <linux/rslib.h>
+#endif
+
+struct ram_console_buffer {
+	uint32_t    sig;
+	uint32_t    start;
+	uint32_t    size;
+	uint8_t     data[0];
+};
+
+#define RAM_CONSOLE_SIG (0x43474244) /* DBGC */
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+static char __initdata
+	ram_console_old_log_init_buffer[CONFIG_ANDROID_RAM_CONSOLE_EARLY_SIZE];
+#endif
+static char *ram_console_old_log;
+static size_t ram_console_old_log_size;
+
+static struct ram_console_buffer *ram_console_buffer;
+static size_t ram_console_buffer_size;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+static char *ram_console_par_buffer;
+static struct rs_control *ram_console_rs_decoder;
+static int ram_console_corrected_bytes;
+static int ram_console_bad_blocks;
+#define ECC_BLOCK_SIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_DATA_SIZE
+#define ECC_SIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_ECC_SIZE
+#define ECC_SYMSIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE
+#define ECC_POLY CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_POLYNOMIAL
+#endif
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+static void ram_console_encode_rs8(uint8_t *data, size_t len, uint8_t *ecc)
+{
+	int i;
+	uint16_t par[ECC_SIZE];
+	/* Initialize the parity buffer */
+	memset(par, 0, sizeof(par));
+	encode_rs8(ram_console_rs_decoder, data, len, par, 0);
+	for (i = 0; i < ECC_SIZE; i++)
+		ecc[i] = par[i];
+}
+
+static int ram_console_decode_rs8(void *data, size_t len, uint8_t *ecc)
+{
+	int i;
+	uint16_t par[ECC_SIZE];
+	for (i = 0; i < ECC_SIZE; i++)
+		par[i] = ecc[i];
+	return decode_rs8(ram_console_rs_decoder, data, par, len,
+				NULL, 0, NULL, 0, NULL);
+}
+#endif
+
+static void ram_console_update(const char *s, unsigned int count)
+{
+	struct ram_console_buffer *buffer = ram_console_buffer;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	uint8_t *buffer_end = buffer->data + ram_console_buffer_size;
+	uint8_t *block;
+	uint8_t *par;
+	int size = ECC_BLOCK_SIZE;
+#endif
+	memcpy(buffer->data + buffer->start, s, count);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	block = buffer->data + (buffer->start & ~(ECC_BLOCK_SIZE - 1));
+	par = ram_console_par_buffer +
+	      (buffer->start / ECC_BLOCK_SIZE) * ECC_SIZE;
+	do {
+		if (block + ECC_BLOCK_SIZE > buffer_end)
+			size = buffer_end - block;
+		ram_console_encode_rs8(block, size, par);
+		block += ECC_BLOCK_SIZE;
+		par += ECC_SIZE;
+	} while (block < buffer->data + buffer->start + count);
+#endif
+}
+
+static void ram_console_update_header(void)
+{
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	struct ram_console_buffer *buffer = ram_console_buffer;
+	uint8_t *par;
+	par = ram_console_par_buffer +
+	      DIV_ROUND_UP(ram_console_buffer_size, ECC_BLOCK_SIZE) * ECC_SIZE;
+	ram_console_encode_rs8((uint8_t *)buffer, sizeof(*buffer), par);
+#endif
+}
+
+static void
+ram_console_write(struct console *console, const char *s, unsigned int count)
+{
+	int rem;
+	struct ram_console_buffer *buffer = ram_console_buffer;
+
+	if (count > ram_console_buffer_size) {
+		s += count - ram_console_buffer_size;
+		count = ram_console_buffer_size;
+	}
+	rem = ram_console_buffer_size - buffer->start;
+	if (rem < count) {
+		ram_console_update(s, rem);
+		s += rem;
+		count -= rem;
+		buffer->start = 0;
+		buffer->size = ram_console_buffer_size;
+	}
+	ram_console_update(s, count);
+
+	buffer->start += count;
+	if (buffer->size < ram_console_buffer_size)
+		buffer->size += count;
+	ram_console_update_header();
+}
+
+static struct console ram_console = {
+	.name	= "ram",
+	.write	= ram_console_write,
+	.flags	= CON_PRINTBUFFER | CON_ENABLED,
+	.index	= -1,
+};
+
+void ram_console_enable_console(int enabled)
+{
+	if (enabled)
+		ram_console.flags |= CON_ENABLED;
+	else
+		ram_console.flags &= ~CON_ENABLED;
+}
+
+static void __init
+ram_console_save_old(struct ram_console_buffer *buffer, const char *bootinfo,
+	char *dest)
+{
+	size_t old_log_size = buffer->size;
+	size_t bootinfo_size = 0;
+	size_t total_size = old_log_size;
+	char *ptr;
+	const char *bootinfo_label = "Boot info:\n";
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	uint8_t *block;
+	uint8_t *par;
+	char strbuf[80];
+	int strbuf_len = 0;
+
+	block = buffer->data;
+	par = ram_console_par_buffer;
+	while (block < buffer->data + buffer->size) {
+		int numerr;
+		int size = ECC_BLOCK_SIZE;
+		if (block + size > buffer->data + ram_console_buffer_size)
+			size = buffer->data + ram_console_buffer_size - block;
+		numerr = ram_console_decode_rs8(block, size, par);
+		if (numerr > 0) {
+#if 0
+			printk(KERN_INFO "ram_console: error in block %p, %d\n",
+			       block, numerr);
+#endif
+			ram_console_corrected_bytes += numerr;
+		} else if (numerr < 0) {
+#if 0
+			printk(KERN_INFO "ram_console: uncorrectable error in "
+			       "block %p\n", block);
+#endif
+			ram_console_bad_blocks++;
+		}
+		block += ECC_BLOCK_SIZE;
+		par += ECC_SIZE;
+	}
+	if (ram_console_corrected_bytes || ram_console_bad_blocks)
+		strbuf_len = snprintf(strbuf, sizeof(strbuf),
+			"\n%d Corrected bytes, %d unrecoverable blocks\n",
+			ram_console_corrected_bytes, ram_console_bad_blocks);
+	else
+		strbuf_len = snprintf(strbuf, sizeof(strbuf),
+				      "\nNo errors detected\n");
+	if (strbuf_len >= sizeof(strbuf))
+		strbuf_len = sizeof(strbuf) - 1;
+	total_size += strbuf_len;
+#endif
+
+	if (bootinfo)
+		bootinfo_size = strlen(bootinfo) + strlen(bootinfo_label);
+	total_size += bootinfo_size;
+
+	if (dest == NULL) {
+		dest = kmalloc(total_size, GFP_KERNEL);
+		if (dest == NULL) {
+			printk(KERN_ERR
+			       "ram_console: failed to allocate buffer\n");
+			return;
+		}
+	}
+
+	ram_console_old_log = dest;
+	ram_console_old_log_size = total_size;
+	memcpy(ram_console_old_log,
+	       &buffer->data[buffer->start], buffer->size - buffer->start);
+	memcpy(ram_console_old_log + buffer->size - buffer->start,
+	       &buffer->data[0], buffer->start);
+	ptr = ram_console_old_log + old_log_size;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	memcpy(ptr, strbuf, strbuf_len);
+	ptr += strbuf_len;
+#endif
+	if (bootinfo) {
+		memcpy(ptr, bootinfo_label, strlen(bootinfo_label));
+		ptr += strlen(bootinfo_label);
+		memcpy(ptr, bootinfo, bootinfo_size);
+		ptr += bootinfo_size;
+	}
+}
+
+static int __init ram_console_init(struct ram_console_buffer *buffer,
+				   size_t buffer_size, const char *bootinfo,
+				   char *old_buf)
+{
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	int numerr;
+	uint8_t *par;
+#endif
+	ram_console_buffer = buffer;
+	ram_console_buffer_size =
+		buffer_size - sizeof(struct ram_console_buffer);
+
+	if (ram_console_buffer_size > buffer_size) {
+		pr_err("ram_console: buffer %p, invalid size %zu, "
+		       "datasize %zu\n", buffer, buffer_size,
+		       ram_console_buffer_size);
+		return 0;
+	}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	ram_console_buffer_size -= (DIV_ROUND_UP(ram_console_buffer_size,
+						ECC_BLOCK_SIZE) + 1) * ECC_SIZE;
+
+	if (ram_console_buffer_size > buffer_size) {
+		pr_err("ram_console: buffer %p, invalid size %zu, "
+		       "non-ecc datasize %zu\n",
+		       buffer, buffer_size, ram_console_buffer_size);
+		return 0;
+	}
+
+	ram_console_par_buffer = buffer->data + ram_console_buffer_size;
+
+
+	/* first consecutive root is 0
+	 * primitive element to generate roots = 1
+	 */
+	ram_console_rs_decoder = init_rs(ECC_SYMSIZE, ECC_POLY, 0, 1, ECC_SIZE);
+	if (ram_console_rs_decoder == NULL) {
+		printk(KERN_INFO "ram_console: init_rs failed\n");
+		return 0;
+	}
+
+	ram_console_corrected_bytes = 0;
+	ram_console_bad_blocks = 0;
+
+	par = ram_console_par_buffer +
+	      DIV_ROUND_UP(ram_console_buffer_size, ECC_BLOCK_SIZE) * ECC_SIZE;
+
+	numerr = ram_console_decode_rs8(buffer, sizeof(*buffer), par);
+	if (numerr > 0) {
+		printk(KERN_INFO "ram_console: error in header, %d\n", numerr);
+		ram_console_corrected_bytes += numerr;
+	} else if (numerr < 0) {
+		printk(KERN_INFO
+		       "ram_console: uncorrectable error in header\n");
+		ram_console_bad_blocks++;
+	}
+#endif
+
+	if (buffer->sig == RAM_CONSOLE_SIG) {
+		if (buffer->size > ram_console_buffer_size
+		    || buffer->start > buffer->size)
+			printk(KERN_INFO "ram_console: found existing invalid "
+			       "buffer, size %d, start %d\n",
+			       buffer->size, buffer->start);
+		else {
+			printk(KERN_INFO "ram_console: found existing buffer, "
+			       "size %d, start %d\n",
+			       buffer->size, buffer->start);
+			ram_console_save_old(buffer, bootinfo, old_buf);
+		}
+	} else {
+		printk(KERN_INFO "ram_console: no valid data in buffer "
+		       "(sig = 0x%08x)\n", buffer->sig);
+	}
+
+	buffer->sig = RAM_CONSOLE_SIG;
+	buffer->start = 0;
+	buffer->size = 0;
+
+	register_console(&ram_console);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ENABLE_VERBOSE
+	console_verbose();
+#endif
+	return 0;
+}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+static int __init ram_console_early_init(void)
+{
+	return ram_console_init((struct ram_console_buffer *)
+		CONFIG_ANDROID_RAM_CONSOLE_EARLY_ADDR,
+		CONFIG_ANDROID_RAM_CONSOLE_EARLY_SIZE,
+		NULL,
+		ram_console_old_log_init_buffer);
+}
+#else
+static int ram_console_driver_probe(struct platform_device *pdev)
+{
+	struct resource *res = pdev->resource;
+	size_t start;
+	size_t buffer_size;
+	void *buffer;
+	const char *bootinfo = NULL;
+	struct ram_console_platform_data *pdata = pdev->dev.platform_data;
+
+	if (res == NULL || pdev->num_resources != 1 ||
+	    !(res->flags & IORESOURCE_MEM)) {
+		printk(KERN_ERR "ram_console: invalid resource, %p %d flags "
+		       "%lx\n", res, pdev->num_resources, res ? res->flags : 0);
+		return -ENXIO;
+	}
+	buffer_size = res->end - res->start + 1;
+	start = res->start;
+	printk(KERN_INFO "ram_console: got buffer at %zx, size %zx\n",
+	       start, buffer_size);
+	buffer = ioremap(res->start, buffer_size);
+	if (buffer == NULL) {
+		printk(KERN_ERR "ram_console: failed to map memory\n");
+		return -ENOMEM;
+	}
+
+	if (pdata)
+		bootinfo = pdata->bootinfo;
+
+	return ram_console_init(buffer, buffer_size, bootinfo, NULL/* allocate */);
+}
+
+static struct platform_driver ram_console_driver = {
+	.probe = ram_console_driver_probe,
+	.driver		= {
+		.name	= "ram_console",
+	},
+};
+
+static int __init ram_console_module_init(void)
+{
+	int err;
+	err = platform_driver_register(&ram_console_driver);
+	return err;
+}
+#endif
+
+static ssize_t ram_console_read_old(struct file *file, char __user *buf,
+				    size_t len, loff_t *offset)
+{
+	loff_t pos = *offset;
+	ssize_t count;
+
+	if (pos >= ram_console_old_log_size)
+		return 0;
+
+	count = min(len, (size_t)(ram_console_old_log_size - pos));
+	if (copy_to_user(buf, ram_console_old_log + pos, count))
+		return -EFAULT;
+
+	*offset += count;
+	return count;
+}
+
+static const struct file_operations ram_console_file_ops = {
+	.owner = THIS_MODULE,
+	.read = ram_console_read_old,
+};
+
+static int __init ram_console_late_init(void)
+{
+	struct proc_dir_entry *entry;
+
+	if (ram_console_old_log == NULL)
+		return 0;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+	ram_console_old_log = kmalloc(ram_console_old_log_size, GFP_KERNEL);
+	if (ram_console_old_log == NULL) {
+		printk(KERN_ERR
+		       "ram_console: failed to allocate buffer for old log\n");
+		ram_console_old_log_size = 0;
+		return 0;
+	}
+	memcpy(ram_console_old_log,
+	       ram_console_old_log_init_buffer, ram_console_old_log_size);
+#endif
+	entry = create_proc_entry("last_kmsg", S_IFREG | S_IRUGO, NULL);
+	if (!entry) {
+		printk(KERN_ERR "ram_console: failed to create proc entry\n");
+		kfree(ram_console_old_log);
+		ram_console_old_log = NULL;
+		return 0;
+	}
+
+	entry->proc_fops = &ram_console_file_ops;
+	entry->size = ram_console_old_log_size;
+	return 0;
+}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+console_initcall(ram_console_early_init);
+#else
+postcore_initcall(ram_console_module_init);
+#endif
+late_initcall(ram_console_late_init);
+
diff --git a/drivers/staging/android/ram_console.h b/drivers/staging/android/ram_console.h
new file mode 100644
index 000000000000..9f1125c11066
--- /dev/null
+++ b/drivers/staging/android/ram_console.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_
+#define _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_
+
+struct ram_console_platform_data {
+	const char *bootinfo;
+};
+
+#endif /* _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_ */
diff --git a/drivers/staging/android/switch/Kconfig b/drivers/staging/android/switch/Kconfig
new file mode 100644
index 000000000000..36846f62f4bc
--- /dev/null
+++ b/drivers/staging/android/switch/Kconfig
@@ -0,0 +1,11 @@
+menuconfig ANDROID_SWITCH
+	tristate "Android Switch class support"
+	help
+	  Say Y here to enable Android switch class support. This allows
+	  monitoring switches by userspace via sysfs and uevent.
+
+config ANDROID_SWITCH_GPIO
+	tristate "Android GPIO Switch support"
+	depends on GENERIC_GPIO && ANDROID_SWITCH
+	help
+	  Say Y here to enable GPIO based switch support.
diff --git a/drivers/staging/android/switch/Makefile b/drivers/staging/android/switch/Makefile
new file mode 100644
index 000000000000..d76bfdcedfaf
--- /dev/null
+++ b/drivers/staging/android/switch/Makefile
@@ -0,0 +1,4 @@
+# Android Switch Class Driver
+obj-$(CONFIG_ANDROID_SWITCH)		+= switch_class.o
+obj-$(CONFIG_ANDROID_SWITCH_GPIO)	+= switch_gpio.o
+
diff --git a/drivers/staging/android/switch/switch.h b/drivers/staging/android/switch/switch.h
new file mode 100644
index 000000000000..4fcb3109875a
--- /dev/null
+++ b/drivers/staging/android/switch/switch.h
@@ -0,0 +1,53 @@
+/*
+ *  Switch class driver
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+*/
+
+#ifndef __LINUX_SWITCH_H__
+#define __LINUX_SWITCH_H__
+
+struct switch_dev {
+	const char	*name;
+	struct device	*dev;
+	int		index;
+	int		state;
+
+	ssize_t	(*print_name)(struct switch_dev *sdev, char *buf);
+	ssize_t	(*print_state)(struct switch_dev *sdev, char *buf);
+};
+
+struct gpio_switch_platform_data {
+	const char *name;
+	unsigned	gpio;
+
+	/* if NULL, switch_dev.name will be printed */
+	const char *name_on;
+	const char *name_off;
+	/* if NULL, "0" or "1" will be printed */
+	const char *state_on;
+	const char *state_off;
+};
+
+extern int switch_dev_register(struct switch_dev *sdev);
+extern void switch_dev_unregister(struct switch_dev *sdev);
+
+static inline int switch_get_state(struct switch_dev *sdev)
+{
+	return sdev->state;
+}
+
+extern void switch_set_state(struct switch_dev *sdev, int state);
+
+#endif /* __LINUX_SWITCH_H__ */
diff --git a/drivers/staging/android/switch/switch_class.c b/drivers/staging/android/switch/switch_class.c
new file mode 100644
index 000000000000..74680446fc66
--- /dev/null
+++ b/drivers/staging/android/switch/switch_class.c
@@ -0,0 +1,174 @@
+/*
+ * switch_class.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+*/
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+#include "switch.h"
+
+struct class *switch_class;
+static atomic_t device_count;
+
+static ssize_t state_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct switch_dev *sdev = (struct switch_dev *)
+		dev_get_drvdata(dev);
+
+	if (sdev->print_state) {
+		int ret = sdev->print_state(sdev, buf);
+		if (ret >= 0)
+			return ret;
+	}
+	return sprintf(buf, "%d\n", sdev->state);
+}
+
+static ssize_t name_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct switch_dev *sdev = (struct switch_dev *)
+		dev_get_drvdata(dev);
+
+	if (sdev->print_name) {
+		int ret = sdev->print_name(sdev, buf);
+		if (ret >= 0)
+			return ret;
+	}
+	return sprintf(buf, "%s\n", sdev->name);
+}
+
+static DEVICE_ATTR(state, S_IRUGO | S_IWUSR, state_show, NULL);
+static DEVICE_ATTR(name, S_IRUGO | S_IWUSR, name_show, NULL);
+
+void switch_set_state(struct switch_dev *sdev, int state)
+{
+	char name_buf[120];
+	char state_buf[120];
+	char *prop_buf;
+	char *envp[3];
+	int env_offset = 0;
+	int length;
+
+	if (sdev->state != state) {
+		sdev->state = state;
+
+		prop_buf = (char *)get_zeroed_page(GFP_KERNEL);
+		if (prop_buf) {
+			length = name_show(sdev->dev, NULL, prop_buf);
+			if (length > 0) {
+				if (prop_buf[length - 1] == '\n')
+					prop_buf[length - 1] = 0;
+				snprintf(name_buf, sizeof(name_buf),
+					"SWITCH_NAME=%s", prop_buf);
+				envp[env_offset++] = name_buf;
+			}
+			length = state_show(sdev->dev, NULL, prop_buf);
+			if (length > 0) {
+				if (prop_buf[length - 1] == '\n')
+					prop_buf[length - 1] = 0;
+				snprintf(state_buf, sizeof(state_buf),
+					"SWITCH_STATE=%s", prop_buf);
+				envp[env_offset++] = state_buf;
+			}
+			envp[env_offset] = NULL;
+			kobject_uevent_env(&sdev->dev->kobj, KOBJ_CHANGE, envp);
+			free_page((unsigned long)prop_buf);
+		} else {
+			printk(KERN_ERR "out of memory in switch_set_state\n");
+			kobject_uevent(&sdev->dev->kobj, KOBJ_CHANGE);
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(switch_set_state);
+
+static int create_switch_class(void)
+{
+	if (!switch_class) {
+		switch_class = class_create(THIS_MODULE, "switch");
+		if (IS_ERR(switch_class))
+			return PTR_ERR(switch_class);
+		atomic_set(&device_count, 0);
+	}
+
+	return 0;
+}
+
+int switch_dev_register(struct switch_dev *sdev)
+{
+	int ret;
+
+	if (!switch_class) {
+		ret = create_switch_class();
+		if (ret < 0)
+			return ret;
+	}
+
+	sdev->index = atomic_inc_return(&device_count);
+	sdev->dev = device_create(switch_class, NULL,
+		MKDEV(0, sdev->index), NULL, sdev->name);
+	if (IS_ERR(sdev->dev))
+		return PTR_ERR(sdev->dev);
+
+	ret = device_create_file(sdev->dev, &dev_attr_state);
+	if (ret < 0)
+		goto err_create_file_1;
+	ret = device_create_file(sdev->dev, &dev_attr_name);
+	if (ret < 0)
+		goto err_create_file_2;
+
+	dev_set_drvdata(sdev->dev, sdev);
+	sdev->state = 0;
+	return 0;
+
+err_create_file_2:
+	device_remove_file(sdev->dev, &dev_attr_state);
+err_create_file_1:
+	device_destroy(switch_class, MKDEV(0, sdev->index));
+	printk(KERN_ERR "switch: Failed to register driver %s\n", sdev->name);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(switch_dev_register);
+
+void switch_dev_unregister(struct switch_dev *sdev)
+{
+	device_remove_file(sdev->dev, &dev_attr_name);
+	device_remove_file(sdev->dev, &dev_attr_state);
+	device_destroy(switch_class, MKDEV(0, sdev->index));
+	dev_set_drvdata(sdev->dev, NULL);
+}
+EXPORT_SYMBOL_GPL(switch_dev_unregister);
+
+static int __init switch_class_init(void)
+{
+	return create_switch_class();
+}
+
+static void __exit switch_class_exit(void)
+{
+	class_destroy(switch_class);
+}
+
+module_init(switch_class_init);
+module_exit(switch_class_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("Switch class driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/switch/switch_gpio.c b/drivers/staging/android/switch/switch_gpio.c
new file mode 100644
index 000000000000..38b2c2f6004e
--- /dev/null
+++ b/drivers/staging/android/switch/switch_gpio.c
@@ -0,0 +1,172 @@
+/*
+ * switch_gpio.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+*/
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/gpio.h>
+#include "switch.h"
+
+struct gpio_switch_data {
+	struct switch_dev sdev;
+	unsigned gpio;
+	const char *name_on;
+	const char *name_off;
+	const char *state_on;
+	const char *state_off;
+	int irq;
+	struct work_struct work;
+};
+
+static void gpio_switch_work(struct work_struct *work)
+{
+	int state;
+	struct gpio_switch_data	*data =
+		container_of(work, struct gpio_switch_data, work);
+
+	state = gpio_get_value(data->gpio);
+	switch_set_state(&data->sdev, state);
+}
+
+static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
+{
+	struct gpio_switch_data *switch_data =
+	    (struct gpio_switch_data *)dev_id;
+
+	schedule_work(&switch_data->work);
+	return IRQ_HANDLED;
+}
+
+static ssize_t switch_gpio_print_state(struct switch_dev *sdev, char *buf)
+{
+	struct gpio_switch_data	*switch_data =
+		container_of(sdev, struct gpio_switch_data, sdev);
+	const char *state;
+	if (switch_get_state(sdev))
+		state = switch_data->state_on;
+	else
+		state = switch_data->state_off;
+
+	if (state)
+		return sprintf(buf, "%s\n", state);
+	return -1;
+}
+
+static int gpio_switch_probe(struct platform_device *pdev)
+{
+	struct gpio_switch_platform_data *pdata = pdev->dev.platform_data;
+	struct gpio_switch_data *switch_data;
+	int ret = 0;
+
+	if (!pdata)
+		return -EBUSY;
+
+	switch_data = kzalloc(sizeof(struct gpio_switch_data), GFP_KERNEL);
+	if (!switch_data)
+		return -ENOMEM;
+
+	switch_data->sdev.name = pdata->name;
+	switch_data->gpio = pdata->gpio;
+	switch_data->name_on = pdata->name_on;
+	switch_data->name_off = pdata->name_off;
+	switch_data->state_on = pdata->state_on;
+	switch_data->state_off = pdata->state_off;
+	switch_data->sdev.print_state = switch_gpio_print_state;
+
+	ret = switch_dev_register(&switch_data->sdev);
+	if (ret < 0)
+		goto err_switch_dev_register;
+
+	ret = gpio_request(switch_data->gpio, pdev->name);
+	if (ret < 0)
+		goto err_request_gpio;
+
+	ret = gpio_direction_input(switch_data->gpio);
+	if (ret < 0)
+		goto err_set_gpio_input;
+
+	INIT_WORK(&switch_data->work, gpio_switch_work);
+
+	switch_data->irq = gpio_to_irq(switch_data->gpio);
+	if (switch_data->irq < 0) {
+		ret = switch_data->irq;
+		goto err_detect_irq_num_failed;
+	}
+
+	ret = request_irq(switch_data->irq, gpio_irq_handler,
+			  IRQF_TRIGGER_LOW, pdev->name, switch_data);
+	if (ret < 0)
+		goto err_request_irq;
+
+	/* Perform initial detection */
+	gpio_switch_work(&switch_data->work);
+
+	return 0;
+
+err_request_irq:
+err_detect_irq_num_failed:
+err_set_gpio_input:
+	gpio_free(switch_data->gpio);
+err_request_gpio:
+	switch_dev_unregister(&switch_data->sdev);
+err_switch_dev_register:
+	kfree(switch_data);
+
+	return ret;
+}
+
+static int __devexit gpio_switch_remove(struct platform_device *pdev)
+{
+	struct gpio_switch_data *switch_data = platform_get_drvdata(pdev);
+
+	cancel_work_sync(&switch_data->work);
+	gpio_free(switch_data->gpio);
+	switch_dev_unregister(&switch_data->sdev);
+	kfree(switch_data);
+
+	return 0;
+}
+
+static struct platform_driver gpio_switch_driver = {
+	.probe		= gpio_switch_probe,
+	.remove		= __devexit_p(gpio_switch_remove),
+	.driver		= {
+		.name	= "switch-gpio",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init gpio_switch_init(void)
+{
+	return platform_driver_register(&gpio_switch_driver);
+}
+
+static void __exit gpio_switch_exit(void)
+{
+	platform_driver_unregister(&gpio_switch_driver);
+}
+
+module_init(gpio_switch_init);
+module_exit(gpio_switch_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("GPIO Switch driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
new file mode 100644
index 000000000000..a64481c3e86d
--- /dev/null
+++ b/drivers/staging/android/timed_gpio.c
@@ -0,0 +1,176 @@
+/* drivers/misc/timed_gpio.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/hrtimer.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+
+#include "timed_output.h"
+#include "timed_gpio.h"
+
+
+struct timed_gpio_data {
+	struct timed_output_dev dev;
+	struct hrtimer timer;
+	spinlock_t lock;
+	unsigned 	gpio;
+	int 		max_timeout;
+	u8 		active_low;
+};
+
+static enum hrtimer_restart gpio_timer_func(struct hrtimer *timer)
+{
+	struct timed_gpio_data *data =
+		container_of(timer, struct timed_gpio_data, timer);
+
+	gpio_direction_output(data->gpio, data->active_low ? 1 : 0);
+	return HRTIMER_NORESTART;
+}
+
+static int gpio_get_time(struct timed_output_dev *dev)
+{
+	struct timed_gpio_data	*data =
+		container_of(dev, struct timed_gpio_data, dev);
+
+	if (hrtimer_active(&data->timer)) {
+		ktime_t r = hrtimer_get_remaining(&data->timer);
+		struct timeval t = ktime_to_timeval(r);
+		return t.tv_sec * 1000 + t.tv_usec / 1000;
+	} else
+		return 0;
+}
+
+static void gpio_enable(struct timed_output_dev *dev, int value)
+{
+	struct timed_gpio_data	*data =
+		container_of(dev, struct timed_gpio_data, dev);
+	unsigned long	flags;
+
+	spin_lock_irqsave(&data->lock, flags);
+
+	/* cancel previous timer and set GPIO according to value */
+	hrtimer_cancel(&data->timer);
+	gpio_direction_output(data->gpio, data->active_low ? !value : !!value);
+
+	if (value > 0) {
+		if (value > data->max_timeout)
+			value = data->max_timeout;
+
+		hrtimer_start(&data->timer,
+			ktime_set(value / 1000, (value % 1000) * 1000000),
+			HRTIMER_MODE_REL);
+	}
+
+	spin_unlock_irqrestore(&data->lock, flags);
+}
+
+static int timed_gpio_probe(struct platform_device *pdev)
+{
+	struct timed_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timed_gpio *cur_gpio;
+	struct timed_gpio_data *gpio_data, *gpio_dat;
+	int i, j, ret = 0;
+
+	if (!pdata)
+		return -EBUSY;
+
+	gpio_data = kzalloc(sizeof(struct timed_gpio_data) * pdata->num_gpios,
+			GFP_KERNEL);
+	if (!gpio_data)
+		return -ENOMEM;
+
+	for (i = 0; i < pdata->num_gpios; i++) {
+		cur_gpio = &pdata->gpios[i];
+		gpio_dat = &gpio_data[i];
+
+		hrtimer_init(&gpio_dat->timer, CLOCK_MONOTONIC,
+				HRTIMER_MODE_REL);
+		gpio_dat->timer.function = gpio_timer_func;
+		spin_lock_init(&gpio_dat->lock);
+
+		gpio_dat->dev.name = cur_gpio->name;
+		gpio_dat->dev.get_time = gpio_get_time;
+		gpio_dat->dev.enable = gpio_enable;
+		ret = gpio_request(cur_gpio->gpio, cur_gpio->name);
+		if (ret >= 0) {
+			ret = timed_output_dev_register(&gpio_dat->dev);
+			if (ret < 0)
+				gpio_free(cur_gpio->gpio);
+		}
+		if (ret < 0) {
+			for (j = 0; j < i; j++) {
+				timed_output_dev_unregister(&gpio_data[i].dev);
+				gpio_free(gpio_data[i].gpio);
+			}
+			kfree(gpio_data);
+			return ret;
+		}
+
+		gpio_dat->gpio = cur_gpio->gpio;
+		gpio_dat->max_timeout = cur_gpio->max_timeout;
+		gpio_dat->active_low = cur_gpio->active_low;
+		gpio_direction_output(gpio_dat->gpio, gpio_dat->active_low);
+	}
+
+	platform_set_drvdata(pdev, gpio_data);
+
+	return 0;
+}
+
+static int timed_gpio_remove(struct platform_device *pdev)
+{
+	struct timed_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timed_gpio_data *gpio_data = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < pdata->num_gpios; i++) {
+		timed_output_dev_unregister(&gpio_data[i].dev);
+		gpio_free(gpio_data[i].gpio);
+	}
+
+	kfree(gpio_data);
+
+	return 0;
+}
+
+static struct platform_driver timed_gpio_driver = {
+	.probe		= timed_gpio_probe,
+	.remove		= timed_gpio_remove,
+	.driver		= {
+		.name		= TIMED_GPIO_NAME,
+		.owner		= THIS_MODULE,
+	},
+};
+
+static int __init timed_gpio_init(void)
+{
+	return platform_driver_register(&timed_gpio_driver);
+}
+
+static void __exit timed_gpio_exit(void)
+{
+	platform_driver_unregister(&timed_gpio_driver);
+}
+
+module_init(timed_gpio_init);
+module_exit(timed_gpio_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("timed gpio driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/timed_gpio.h b/drivers/staging/android/timed_gpio.h
new file mode 100644
index 000000000000..a0e15f8be3f7
--- /dev/null
+++ b/drivers/staging/android/timed_gpio.h
@@ -0,0 +1,33 @@
+/* include/linux/timed_gpio.h
+ *
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+*/
+
+#ifndef _LINUX_TIMED_GPIO_H
+#define _LINUX_TIMED_GPIO_H
+
+#define TIMED_GPIO_NAME "timed-gpio"
+
+struct timed_gpio {
+	const char *name;
+	unsigned 	gpio;
+	int		max_timeout;
+	u8 		active_low;
+};
+
+struct timed_gpio_platform_data {
+	int 		num_gpios;
+	struct timed_gpio *gpios;
+};
+
+#endif
diff --git a/drivers/staging/android/timed_output.c b/drivers/staging/android/timed_output.c
new file mode 100644
index 000000000000..f373422308e0
--- /dev/null
+++ b/drivers/staging/android/timed_output.c
@@ -0,0 +1,123 @@
+/* drivers/misc/timed_output.c
+ *
+ * Copyright (C) 2009 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+
+#include "timed_output.h"
+
+static struct class *timed_output_class;
+static atomic_t device_count;
+
+static ssize_t enable_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct timed_output_dev *tdev = dev_get_drvdata(dev);
+	int remaining = tdev->get_time(tdev);
+
+	return sprintf(buf, "%d\n", remaining);
+}
+
+static ssize_t enable_store(
+		struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t size)
+{
+	struct timed_output_dev *tdev = dev_get_drvdata(dev);
+	int value;
+
+	if (sscanf(buf, "%d", &value) != 1)
+		return -EINVAL;
+
+	tdev->enable(tdev, value);
+
+	return size;
+}
+
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR, enable_show, enable_store);
+
+static int create_timed_output_class(void)
+{
+	if (!timed_output_class) {
+		timed_output_class = class_create(THIS_MODULE, "timed_output");
+		if (IS_ERR(timed_output_class))
+			return PTR_ERR(timed_output_class);
+		atomic_set(&device_count, 0);
+	}
+
+	return 0;
+}
+
+int timed_output_dev_register(struct timed_output_dev *tdev)
+{
+	int ret;
+
+	if (!tdev || !tdev->name || !tdev->enable || !tdev->get_time)
+		return -EINVAL;
+
+	ret = create_timed_output_class();
+	if (ret < 0)
+		return ret;
+
+	tdev->index = atomic_inc_return(&device_count);
+	tdev->dev = device_create(timed_output_class, NULL,
+		MKDEV(0, tdev->index), NULL, tdev->name);
+	if (IS_ERR(tdev->dev))
+		return PTR_ERR(tdev->dev);
+
+	ret = device_create_file(tdev->dev, &dev_attr_enable);
+	if (ret < 0)
+		goto err_create_file;
+
+	dev_set_drvdata(tdev->dev, tdev);
+	tdev->state = 0;
+	return 0;
+
+err_create_file:
+	device_destroy(timed_output_class, MKDEV(0, tdev->index));
+	printk(KERN_ERR "timed_output: Failed to register driver %s\n",
+			tdev->name);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(timed_output_dev_register);
+
+void timed_output_dev_unregister(struct timed_output_dev *tdev)
+{
+	device_remove_file(tdev->dev, &dev_attr_enable);
+	device_destroy(timed_output_class, MKDEV(0, tdev->index));
+	dev_set_drvdata(tdev->dev, NULL);
+}
+EXPORT_SYMBOL_GPL(timed_output_dev_unregister);
+
+static int __init timed_output_init(void)
+{
+	return create_timed_output_class();
+}
+
+static void __exit timed_output_exit(void)
+{
+	class_destroy(timed_output_class);
+}
+
+module_init(timed_output_init);
+module_exit(timed_output_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("timed output class driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/timed_output.h b/drivers/staging/android/timed_output.h
new file mode 100644
index 000000000000..ec907ab2ff54
--- /dev/null
+++ b/drivers/staging/android/timed_output.h
@@ -0,0 +1,37 @@
+/* include/linux/timed_output.h
+ *
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+*/
+
+#ifndef _LINUX_TIMED_OUTPUT_H
+#define _LINUX_TIMED_OUTPUT_H
+
+struct timed_output_dev {
+	const char	*name;
+
+	/* enable the output and set the timer */
+	void	(*enable)(struct timed_output_dev *sdev, int timeout);
+
+	/* returns the current number of milliseconds remaining on the timer */
+	int		(*get_time)(struct timed_output_dev *sdev);
+
+	/* private data */
+	struct device	*dev;
+	int		index;
+	int		state;
+};
+
+extern int timed_output_dev_register(struct timed_output_dev *dev);
+extern void timed_output_dev_unregister(struct timed_output_dev *dev);
+
+#endif
diff --git a/drivers/staging/asus_oled/asus_oled.c b/drivers/staging/asus_oled/asus_oled.c
index 7bb7da7959a2..e77e4e0396cf 100644
--- a/drivers/staging/asus_oled/asus_oled.c
+++ b/drivers/staging/asus_oled/asus_oled.c
@@ -201,7 +201,7 @@ static ssize_t set_enabled(struct device *dev, struct device_attribute *attr,
 	struct usb_interface *intf = to_usb_interface(dev);
 	struct asus_oled_dev *odev = usb_get_intfdata(intf);
 	unsigned long value;
-	if (strict_strtoul(buf, 10, &value))
+	if (kstrtoul(buf, 10, &value))
 		return -EINVAL;
 
 	enable_oled(odev, value);
@@ -217,7 +217,7 @@ static ssize_t class_set_enabled(struct device *device,
 		(struct asus_oled_dev *) dev_get_drvdata(device);
 	unsigned long value;
 
-	if (strict_strtoul(buf, 10, &value))
+	if (kstrtoul(buf, 10, &value))
 		return -EINVAL;
 
 	enable_oled(odev, value);
diff --git a/drivers/staging/bcm/Bcmchar.c b/drivers/staging/bcm/Bcmchar.c
index 2fa658eb74dc..179707b5e7c7 100644
--- a/drivers/staging/bcm/Bcmchar.c
+++ b/drivers/staging/bcm/Bcmchar.c
@@ -161,6 +161,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 	INT Status = STATUS_FAILURE;
 	int timeout = 0;
 	IOCTL_BUFFER IoBuffer;
+	int bytes;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "Parameters Passed to control IOCTL cmd=0x%X arg=0x%lX", cmd, arg);
 
@@ -230,11 +231,16 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		if (!temp_buff)
 			return -ENOMEM;
 
-		Status = rdmalt(Adapter, (UINT)sRdmBuffer.Register,
+		bytes = rdmalt(Adapter, (UINT)sRdmBuffer.Register,
 				(PUINT)temp_buff, Bufflen);
-		if (Status == STATUS_SUCCESS) {
-			if (copy_to_user(IoBuffer.OutputBuffer, temp_buff, IoBuffer.OutputLength))
-				Status = -EFAULT;
+		if (bytes > 0) {
+			Status = STATUS_SUCCESS;
+			if (copy_to_user(IoBuffer.OutputBuffer, temp_buff, bytes)) {
+				kfree(temp_buff);
+				return -EFAULT;
+			}
+		} else {
+			Status = bytes;
 		}
 
 		kfree(temp_buff);
@@ -302,7 +308,11 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		if (copy_from_user(&sRdmBuffer, IoBuffer.InputBuffer, IoBuffer.InputLength))
 			return -EFAULT;
 
-		/* FIXME: don't trust user supplied length */
+		if (IoBuffer.OutputLength > USHRT_MAX ||
+			IoBuffer.OutputLength == 0) {
+			return -EINVAL;
+		}
+
 		temp_buff = kmalloc(IoBuffer.OutputLength, GFP_KERNEL);
 		if (!temp_buff)
 			return STATUS_FAILURE;
@@ -318,11 +328,17 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		}
 
 		uiTempVar = sRdmBuffer.Register & EEPROM_REJECT_MASK;
-		Status = rdmaltWithLock(Adapter, (UINT)sRdmBuffer.Register, (PUINT)temp_buff, IoBuffer.OutputLength);
+		bytes = rdmaltWithLock(Adapter, (UINT)sRdmBuffer.Register, (PUINT)temp_buff, IoBuffer.OutputLength);
 
-		if (Status == STATUS_SUCCESS)
-			if (copy_to_user(IoBuffer.OutputBuffer, temp_buff, IoBuffer.OutputLength))
-				Status = -EFAULT;
+		if (bytes > 0) {
+			Status = STATUS_SUCCESS;
+			if (copy_to_user(IoBuffer.OutputBuffer, temp_buff, bytes)) {
+				kfree(temp_buff);
+				return -EFAULT;
+			}
+		} else {
+			Status = bytes;
+		}
 
 		kfree(temp_buff);
 		break;
@@ -437,12 +453,14 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 			}
 		}
 
-		Status = rdmaltWithLock(Adapter, (UINT)GPIO_MODE_REGISTER, (PUINT)ucResetValue, sizeof(UINT));
-
-		if (STATUS_SUCCESS != Status) {
+		bytes = rdmaltWithLock(Adapter, (UINT)GPIO_MODE_REGISTER, (PUINT)ucResetValue, sizeof(UINT));
+		if (bytes < 0) {
+			Status = bytes;
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 					"GPIO_MODE_REGISTER read failed");
 			break;
+		} else {
+			Status = STATUS_SUCCESS;
 		}
 
 		/* Set the gpio mode register to output */
@@ -519,12 +537,15 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		uiBit = gpio_info.uiGpioNumber;
 
 		/* Set the gpio output register */
-		Status = rdmaltWithLock(Adapter, (UINT)GPIO_PIN_STATE_REGISTER,
+		bytes = rdmaltWithLock(Adapter, (UINT)GPIO_PIN_STATE_REGISTER,
 					(PUINT)ucRead, sizeof(UINT));
 
-		if (Status != STATUS_SUCCESS) {
+		if (bytes < 0) {
+			Status = bytes;
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "RDM Failed\n");
 			return Status;
+		} else {
+			Status = STATUS_SUCCESS;
 		}
 	}
 	break;
@@ -590,11 +611,14 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		}
 
 		if (pgpio_multi_info[WIMAX_IDX].uiGPIOMask) {
-			Status = rdmaltWithLock(Adapter, (UINT)GPIO_PIN_STATE_REGISTER, (PUINT)ucResetValue, sizeof(UINT));
+			bytes = rdmaltWithLock(Adapter, (UINT)GPIO_PIN_STATE_REGISTER, (PUINT)ucResetValue, sizeof(UINT));
 
-			if (Status != STATUS_SUCCESS) {
+			if (bytes < 0) {
+				Status = bytes;
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "RDM to GPIO_PIN_STATE_REGISTER Failed.");
 				return Status;
+			} else {
+				Status = STATUS_SUCCESS;
 			}
 
 			pgpio_multi_info[WIMAX_IDX].uiGPIOValue = (*(UINT *)ucResetValue &
@@ -605,7 +629,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 					"Failed while copying Content to IOBufer for user space err:%d", Status);
-			break;
+			return -EFAULT;
 		}
 	}
 	break;
@@ -629,11 +653,14 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		if (copy_from_user(&gpio_multi_mode, IoBuffer.InputBuffer, IoBuffer.InputLength))
 			return -EFAULT;
 
-		Status = rdmaltWithLock(Adapter, (UINT)GPIO_MODE_REGISTER, (PUINT)ucResetValue, sizeof(UINT));
+		bytes = rdmaltWithLock(Adapter, (UINT)GPIO_MODE_REGISTER, (PUINT)ucResetValue, sizeof(UINT));
 
-		if (STATUS_SUCCESS != Status) {
+		if (bytes < 0) {
+			Status = bytes;
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Read of GPIO_MODE_REGISTER failed");
 			return Status;
+		} else {
+			Status = STATUS_SUCCESS;
 		}
 
 		/* Validating the request */
@@ -678,7 +705,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 					"Failed while copying Content to IOBufer for user space err:%d", Status);
-			break;
+			return -EFAULT;
 		}
 	}
 	break;
@@ -706,9 +733,8 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 			return -ENOMEM;
 
 		if (copy_from_user(pvBuffer, IoBuffer.InputBuffer, IoBuffer.InputLength)) {
-			Status = -EFAULT;
 			kfree(pvBuffer);
-			break;
+			return -EFAULT;
 		}
 
 		down(&Adapter->LowPowerModeSync);
@@ -733,8 +759,7 @@ cntrlEnd:
 	}
 
 	case IOCTL_BCM_BUFFER_DOWNLOAD_START: {
-		INT NVMAccess = down_trylock(&Adapter->NVMRdmWrmLock);
-		if (NVMAccess) {
+		if (down_trylock(&Adapter->NVMRdmWrmLock)) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 					"IOCTL_BCM_CHIP_RESET not allowed as EEPROM Read/Write is in progress\n");
 			return -EACCES;
@@ -743,157 +768,162 @@ cntrlEnd:
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 				"Starting the firmware download PID =0x%x!!!!\n", current->pid);
 
-		if (!down_trylock(&Adapter->fw_download_sema)) {
-			Adapter->bBinDownloaded = FALSE;
-			Adapter->fw_download_process_pid = current->pid;
-			Adapter->bCfgDownloaded = FALSE;
-			Adapter->fw_download_done = FALSE;
-			netif_carrier_off(Adapter->dev);
-			netif_stop_queue(Adapter->dev);
-			Status = reset_card_proc(Adapter);
-			if (Status) {
-				pr_err(PFX "%s: reset_card_proc Failed!\n", Adapter->dev->name);
-				up(&Adapter->fw_download_sema);
-				up(&Adapter->NVMRdmWrmLock);
-				break;
-			}
-			mdelay(10);
-		} else {
-			Status = -EBUSY;
+		if (down_trylock(&Adapter->fw_download_sema))
+			return -EBUSY;
+
+		Adapter->bBinDownloaded = FALSE;
+		Adapter->fw_download_process_pid = current->pid;
+		Adapter->bCfgDownloaded = FALSE;
+		Adapter->fw_download_done = FALSE;
+		netif_carrier_off(Adapter->dev);
+		netif_stop_queue(Adapter->dev);
+		Status = reset_card_proc(Adapter);
+		if (Status) {
+			pr_err(PFX "%s: reset_card_proc Failed!\n", Adapter->dev->name);
+			up(&Adapter->fw_download_sema);
+			up(&Adapter->NVMRdmWrmLock);
+			return Status;
 		}
+		mdelay(10);
 
 		up(&Adapter->NVMRdmWrmLock);
-		break;
+		return Status;
 	}
 
 	case IOCTL_BCM_BUFFER_DOWNLOAD: {
 		FIRMWARE_INFO *psFwInfo = NULL;
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Starting the firmware download PID =0x%x!!!!\n", current->pid);
-		do {
-			if (!down_trylock(&Adapter->fw_download_sema)) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-						"Invalid way to download buffer. Use Start and then call this!!!\n");
-				Status = -EINVAL;
-				break;
-			}
-
-			/* Copy Ioctl Buffer structure */
-			if (copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER)))
-				return -EFAULT;
 
+		if (!down_trylock(&Adapter->fw_download_sema)) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-					"Length for FW DLD is : %lx\n", IoBuffer.InputLength);
+					"Invalid way to download buffer. Use Start and then call this!!!\n");
+			up(&Adapter->fw_download_sema);
+			Status = -EINVAL;
+			return Status;
+		}
 
-			if (IoBuffer.InputLength > sizeof(FIRMWARE_INFO))
-				return -EINVAL;
+		/* Copy Ioctl Buffer structure */
+		if (copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER))) {
+			up(&Adapter->fw_download_sema);
+			return -EFAULT;
+		}
 
-			psFwInfo = kmalloc(sizeof(*psFwInfo), GFP_KERNEL);
-			if (!psFwInfo)
-				return -ENOMEM;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+				"Length for FW DLD is : %lx\n", IoBuffer.InputLength);
 
-			if (copy_from_user(psFwInfo, IoBuffer.InputBuffer, IoBuffer.InputLength))
-				return -EFAULT;
+		if (IoBuffer.InputLength > sizeof(FIRMWARE_INFO)) {
+			up(&Adapter->fw_download_sema);
+			return -EINVAL;
+		}
 
-			if (!psFwInfo->pvMappedFirmwareAddress ||
-				(psFwInfo->u32FirmwareLength == 0)) {
+		psFwInfo = kmalloc(sizeof(*psFwInfo), GFP_KERNEL);
+		if (!psFwInfo) {
+			up(&Adapter->fw_download_sema);
+			return -ENOMEM;
+		}
 
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Something else is wrong %lu\n",
-						psFwInfo->u32FirmwareLength);
-				Status = -EINVAL;
-				break;
-			}
+		if (copy_from_user(psFwInfo, IoBuffer.InputBuffer, IoBuffer.InputLength)) {
+			up(&Adapter->fw_download_sema);
+			return -EFAULT;
+		}
 
-			Status = bcm_ioctl_fw_download(Adapter, psFwInfo);
+		if (!psFwInfo->pvMappedFirmwareAddress ||
+			(psFwInfo->u32FirmwareLength == 0)) {
 
-			if (Status != STATUS_SUCCESS) {
-				if (psFwInfo->u32StartingAddress == CONFIG_BEGIN_ADDR)
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "IOCTL: Configuration File Upload Failed\n");
-				else
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,	"IOCTL: Firmware File Upload Failed\n");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Something else is wrong %lu\n",
+					psFwInfo->u32FirmwareLength);
+			up(&Adapter->fw_download_sema);
+			Status = -EINVAL;
+			return Status;
+		}
 
-				/* up(&Adapter->fw_download_sema); */
+		Status = bcm_ioctl_fw_download(Adapter, psFwInfo);
 
-				if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-					Adapter->DriverState = DRIVER_INIT;
-					Adapter->LEDInfo.bLedInitDone = FALSE;
-					wake_up(&Adapter->LEDInfo.notify_led_event);
-				}
-			}
-			break;
+		if (Status != STATUS_SUCCESS) {
+			if (psFwInfo->u32StartingAddress == CONFIG_BEGIN_ADDR)
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "IOCTL: Configuration File Upload Failed\n");
+			else
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,	"IOCTL: Firmware File Upload Failed\n");
+
+			/* up(&Adapter->fw_download_sema); */
 
-		} while (0);
+			if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
+				Adapter->DriverState = DRIVER_INIT;
+				Adapter->LEDInfo.bLedInitDone = FALSE;
+				wake_up(&Adapter->LEDInfo.notify_led_event);
+			}
+		}
 
 		if (Status != STATUS_SUCCESS)
 			up(&Adapter->fw_download_sema);
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, OSAL_DBG, DBG_LVL_ALL, "IOCTL: Firmware File Uploaded\n");
 		kfree(psFwInfo);
-		break;
+		return Status;
 	}
 
 	case IOCTL_BCM_BUFFER_DOWNLOAD_STOP: {
-		INT NVMAccess = down_trylock(&Adapter->NVMRdmWrmLock);
+		if (!down_trylock(&Adapter->fw_download_sema)) {
+			up(&Adapter->fw_download_sema);
+			return -EINVAL;
+		}
 
-		if (NVMAccess) {
+		if (down_trylock(&Adapter->NVMRdmWrmLock)) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 					"FW download blocked as EEPROM Read/Write is in progress\n");
 			up(&Adapter->fw_download_sema);
 			return -EACCES;
 		}
 
-		if (down_trylock(&Adapter->fw_download_sema)) {
-			Adapter->bBinDownloaded = TRUE;
-			Adapter->bCfgDownloaded = TRUE;
-			atomic_set(&Adapter->CurrNumFreeTxDesc, 0);
-			Adapter->CurrNumRecvDescs = 0;
-			Adapter->downloadDDR = 0;
-
-			/* setting the Mips to Run */
-			Status = run_card_proc(Adapter);
+		Adapter->bBinDownloaded = TRUE;
+		Adapter->bCfgDownloaded = TRUE;
+		atomic_set(&Adapter->CurrNumFreeTxDesc, 0);
+		Adapter->CurrNumRecvDescs = 0;
+		Adapter->downloadDDR = 0;
 
-			if (Status) {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Firm Download Failed\n");
-				up(&Adapter->fw_download_sema);
-				up(&Adapter->NVMRdmWrmLock);
-				break;
-			} else {
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-						DBG_LVL_ALL, "Firm Download Over...\n");
-			}
-
-			mdelay(10);
-
-			/* Wait for MailBox Interrupt */
-			if (StartInterruptUrb((PS_INTERFACE_ADAPTER)Adapter->pvInterfaceAdapter))
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Unable to send interrupt...\n");
-
-			timeout = 5*HZ;
-			Adapter->waiting_to_fw_download_done = FALSE;
-			wait_event_timeout(Adapter->ioctl_fw_dnld_wait_queue,
-					Adapter->waiting_to_fw_download_done, timeout);
-			Adapter->fw_download_process_pid = INVALID_PID;
-			Adapter->fw_download_done = TRUE;
-			atomic_set(&Adapter->CurrNumFreeTxDesc, 0);
-			Adapter->CurrNumRecvDescs = 0;
-			Adapter->PrevNumRecvDescs = 0;
-			atomic_set(&Adapter->cntrlpktCnt, 0);
-			Adapter->LinkUpStatus = 0;
-			Adapter->LinkStatus = 0;
+		/* setting the Mips to Run */
+		Status = run_card_proc(Adapter);
 
-			if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
-				Adapter->DriverState = FW_DOWNLOAD_DONE;
-				wake_up(&Adapter->LEDInfo.notify_led_event);
-			}
-
-			if (!timeout)
-				Status = -ENODEV;
+		if (Status) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Firm Download Failed\n");
+			up(&Adapter->fw_download_sema);
+			up(&Adapter->NVMRdmWrmLock);
+			return Status;
 		} else {
-			Status = -EINVAL;
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+					DBG_LVL_ALL, "Firm Download Over...\n");
+		}
+
+		mdelay(10);
+
+		/* Wait for MailBox Interrupt */
+		if (StartInterruptUrb((PS_INTERFACE_ADAPTER)Adapter->pvInterfaceAdapter))
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Unable to send interrupt...\n");
+
+		timeout = 5*HZ;
+		Adapter->waiting_to_fw_download_done = FALSE;
+		wait_event_timeout(Adapter->ioctl_fw_dnld_wait_queue,
+				Adapter->waiting_to_fw_download_done, timeout);
+		Adapter->fw_download_process_pid = INVALID_PID;
+		Adapter->fw_download_done = TRUE;
+		atomic_set(&Adapter->CurrNumFreeTxDesc, 0);
+		Adapter->CurrNumRecvDescs = 0;
+		Adapter->PrevNumRecvDescs = 0;
+		atomic_set(&Adapter->cntrlpktCnt, 0);
+		Adapter->LinkUpStatus = 0;
+		Adapter->LinkStatus = 0;
+
+		if (Adapter->LEDInfo.led_thread_running & BCM_LED_THREAD_RUNNING_ACTIVELY) {
+			Adapter->DriverState = FW_DOWNLOAD_DONE;
+			wake_up(&Adapter->LEDInfo.notify_led_event);
 		}
 
+		if (!timeout)
+			Status = -ENODEV;
+
 		up(&Adapter->fw_download_sema);
 		up(&Adapter->NVMRdmWrmLock);
-		break;
+		return Status;
 	}
 
 	case IOCTL_BE_BUCKET_SIZE:
@@ -969,11 +999,15 @@ cntrlEnd:
 	}
 
 	case IOCTL_BCM_GET_DRIVER_VERSION: {
+		ulong len;
+
 		/* Copy Ioctl Buffer structure */
 		if (copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER)))
 			return -EFAULT;
 
-		if (copy_to_user(IoBuffer.OutputBuffer, VER_FILEVERSION_STR, IoBuffer.OutputLength))
+		len = min_t(ulong, IoBuffer.OutputLength, strlen(VER_FILEVERSION_STR) + 1);
+
+		if (copy_to_user(IoBuffer.OutputBuffer, VER_FILEVERSION_STR, len))
 			return -EFAULT;
 		Status = STATUS_SUCCESS;
 		break;
@@ -985,8 +1019,7 @@ cntrlEnd:
 		/* Copy Ioctl Buffer structure */
 		if (copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER))) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "copy_from_user failed..\n");
-			Status = -EFAULT;
-			break;
+			return -EFAULT;
 		}
 
 		if (IoBuffer.OutputLength != sizeof(link_state)) {
@@ -1001,8 +1034,7 @@ cntrlEnd:
 
 		if (copy_to_user(IoBuffer.OutputBuffer, &link_state, min_t(size_t, sizeof(link_state), IoBuffer.OutputLength))) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy_to_user Failed..\n");
-			Status = -EFAULT;
-			break;
+			return -EFAULT;
 		}
 		Status = STATUS_SUCCESS;
 		break;
@@ -1068,8 +1100,10 @@ cntrlEnd:
 		GetDroppedAppCntrlPktMibs(temp_buff, pTarang);
 
 		if (Status != STATUS_FAILURE)
-			if (copy_to_user(IoBuffer.OutputBuffer, temp_buff, sizeof(S_MIBS_HOST_STATS_MIBS)))
-				Status = -EFAULT;
+			if (copy_to_user(IoBuffer.OutputBuffer, temp_buff, sizeof(S_MIBS_HOST_STATS_MIBS))) {
+				kfree(temp_buff);
+				return -EFAULT;
+			}
 
 		kfree(temp_buff);
 		break;
@@ -1103,7 +1137,9 @@ cntrlEnd:
 		if (copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER)))
 			return -EFAULT;
 
-		/* FIXME: restrict length */
+		if (IoBuffer.InputLength < sizeof(ULONG) * 2)
+			return -EINVAL;
+
 		pvBuffer = kmalloc(IoBuffer.InputLength, GFP_KERNEL);
 		if (!pvBuffer)
 			return -ENOMEM;
@@ -1111,8 +1147,7 @@ cntrlEnd:
 		/* Get WrmBuffer structure */
 		if (copy_from_user(pvBuffer, IoBuffer.InputBuffer, IoBuffer.InputLength)) {
 			kfree(pvBuffer);
-			Status = -EFAULT;
-			break;
+			return -EFAULT;
 		}
 
 		pBulkBuffer = (PBULKWRM_BUFFER)pvBuffer;
@@ -1242,8 +1277,7 @@ cntrlEnd:
 		memset(&tv1, 0, sizeof(struct timeval));
 		if ((Adapter->eNVMType == NVM_FLASH) && (Adapter->uiFlashLayoutMajorVersion == 0)) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "The Flash Control Section is Corrupted. Hence Rejection on NVM Read/Write\n");
-			Status = -EFAULT;
-			break;
+			return -EFAULT;
 		}
 
 		if (IsFlash2x(Adapter)) {
@@ -1252,7 +1286,7 @@ cntrlEnd:
 				(Adapter->eActiveDSD != DSD2)) {
 
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "No DSD is active..hence NVM Command is blocked");
-				return STATUS_FAILURE ;
+				return STATUS_FAILURE;
 			}
 		}
 
@@ -1271,8 +1305,7 @@ cntrlEnd:
 
 		if ((stNVMReadWrite.uiOffset + stNVMReadWrite.uiNumBytes) > Adapter->uiNVMDSDSize) {
 			/* BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Can't allow access beyond NVM Size: 0x%x 0x%x\n", stNVMReadWrite.uiOffset, stNVMReadWrite.uiNumBytes); */
-			Status = STATUS_FAILURE;
-			break;
+			return STATUS_FAILURE;
 		}
 
 		pReadData = kzalloc(stNVMReadWrite.uiNumBytes, GFP_KERNEL);
@@ -1280,9 +1313,8 @@ cntrlEnd:
 			return -ENOMEM;
 
 		if (copy_from_user(pReadData, stNVMReadWrite.pBuffer, stNVMReadWrite.uiNumBytes)) {
-			Status = -EFAULT;
 			kfree(pReadData);
-			break;
+			return -EFAULT;
 		}
 
 		do_gettimeofday(&tv0);
@@ -1309,7 +1341,7 @@ cntrlEnd:
 
 			if (copy_to_user(stNVMReadWrite.pBuffer, pReadData, stNVMReadWrite.uiNumBytes)) {
 				kfree(pReadData);
-				Status = -EFAULT;
+				return -EFAULT;
 			}
 		} else {
 			down(&Adapter->NVMRdmWrmLock);
@@ -1377,9 +1409,8 @@ cntrlEnd:
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, " timetaken by Write/read :%ld msec\n", (tv1.tv_sec - tv0.tv_sec)*1000 + (tv1.tv_usec - tv0.tv_usec)/1000);
 
 		kfree(pReadData);
-		Status = STATUS_SUCCESS;
+		return STATUS_SUCCESS;
 	}
-	break;
 
 	case IOCTL_BCM_FLASH2X_SECTION_READ: {
 		FLASH2X_READWRITE sFlash2xRead = {0};
@@ -1456,7 +1487,9 @@ cntrlEnd:
 			Status = copy_to_user(OutPutBuff, pReadBuff, ReadBytes);
 			if (Status) {
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "Copy to use failed with status :%d", Status);
-				break;
+				up(&Adapter->NVMRdmWrmLock);
+				kfree(pReadBuff);
+				return -EFAULT;
 			}
 			NOB = NOB - ReadBytes;
 			if (NOB) {
@@ -1548,7 +1581,9 @@ cntrlEnd:
 			Status = copy_from_user(pWriteBuff, InputAddr, WriteBytes);
 			if (Status) {
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy to user failed with status :%d", Status);
-				break;
+				up(&Adapter->NVMRdmWrmLock);
+				kfree(pWriteBuff);
+				return -EFAULT;
 			}
 			BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, pWriteBuff, WriteBytes);
 
@@ -1608,8 +1643,10 @@ cntrlEnd:
 
 		BcmGetFlash2xSectionalBitMap(Adapter, psFlash2xBitMap);
 		up(&Adapter->NVMRdmWrmLock);
-		if (copy_to_user(IoBuffer.OutputBuffer, psFlash2xBitMap, sizeof(FLASH2X_BITMAP)))
-			Status = -EFAULT;
+		if (copy_to_user(IoBuffer.OutputBuffer, psFlash2xBitMap, sizeof(FLASH2X_BITMAP))) {
+			kfree(psFlash2xBitMap);
+			return -EFAULT;
+		}
 
 		kfree(psFlash2xBitMap);
 	}
@@ -1627,13 +1664,13 @@ cntrlEnd:
 		Status = copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER));
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy of IOCTL BUFFER failed");
-			return Status;
+			return -EFAULT;
 		}
 
 		Status = copy_from_user(&eFlash2xSectionVal, IoBuffer.InputBuffer, sizeof(INT));
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy of flash section val failed");
-			return Status;
+			return -EFAULT;
 		}
 
 		down(&Adapter->NVMRdmWrmLock);
@@ -1677,13 +1714,13 @@ cntrlEnd:
 		Status = copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER));
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy of IOCTL BUFFER failed Status :%d", Status);
-			return Status;
+			return -EFAULT;
 		}
 
 		Status = copy_from_user(&sCopySectStrut, IoBuffer.InputBuffer, sizeof(FLASH2X_COPY_SECTION));
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy of Copy_Section_Struct failed with Status :%d", Status);
-			return Status;
+			return -EFAULT;
 		}
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "Source SEction :%x", sCopySectStrut.SrcSection);
@@ -1744,7 +1781,7 @@ cntrlEnd:
 		Status = copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER));
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy of IOCTL BUFFER failed");
-			break;
+			return -EFAULT;
 		}
 
 		if (Adapter->eNVMType != NVM_FLASH) {
@@ -1783,12 +1820,12 @@ cntrlEnd:
 		Status = copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER));
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy of IOCTL BUFFER failed");
-			return Status;
+			return -EFAULT;
 		}
 		Status = copy_from_user(&eFlash2xSectionVal, IoBuffer.InputBuffer, sizeof(INT));
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy of flash section val failed");
-			return Status;
+			return -EFAULT;
 		}
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "Read Section :%d", eFlash2xSectionVal);
@@ -1830,8 +1867,7 @@ cntrlEnd:
 		/* Copy Ioctl Buffer structure */
 		if (copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER))) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "copy_from_user 1 failed\n");
-			Status = -EFAULT;
-			break;
+			return -EFAULT;
 		}
 
 		if (copy_from_user(&stNVMRead, IoBuffer.OutputBuffer, sizeof(NVM_READWRITE)))
@@ -1886,7 +1922,9 @@ cntrlEnd:
 			Status = copy_to_user(OutPutBuff, pReadBuff, ReadBytes);
 			if (Status) {
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Copy to use failed with status :%d", Status);
-				break;
+				up(&Adapter->NVMRdmWrmLock);
+				kfree(pReadBuff);
+				return -EFAULT;
 			}
 			NOB = NOB - ReadBytes;
 			if (NOB) {
@@ -1907,8 +1945,7 @@ cntrlEnd:
 		Status = copy_from_user(&IoBuffer, argp, sizeof(IOCTL_BUFFER));
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "copy of Ioctl buffer is failed from user space");
-			Status = -EFAULT;
-			break;
+			return -EFAULT;
 		}
 
 		if (IoBuffer.InputLength != sizeof(unsigned long)) {
@@ -1919,8 +1956,7 @@ cntrlEnd:
 		Status = copy_from_user(&RxCntrlMsgBitMask, IoBuffer.InputBuffer, IoBuffer.InputLength);
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "copy of control bit mask failed from user space");
-			Status = -EFAULT;
-			break;
+			return -EFAULT;
 		}
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "\n Got user defined cntrl msg bit mask :%lx", RxCntrlMsgBitMask);
 		pTarang->RxCntrlMsgBitMask = RxCntrlMsgBitMask;
diff --git a/drivers/staging/bcm/HandleControlPacket.c b/drivers/staging/bcm/HandleControlPacket.c
index 2b1e9e17e11c..b058e30b2ca6 100644
--- a/drivers/staging/bcm/HandleControlPacket.c
+++ b/drivers/staging/bcm/HandleControlPacket.c
@@ -1,195 +1,208 @@
 /**
-@file HandleControlPacket.c
-This file contains the routines to deal with
-sending and receiving of control packets.
-*/
+ * @file HandleControlPacket.c
+ * This file contains the routines to deal with
+ * sending and receiving of control packets.
+ */
 #include "headers.h"
 
 /**
-When a control packet is received, analyze the
-"status" and call appropriate response function.
-Enqueue the control packet for Application.
-@return None
-*/
+ * When a control packet is received, analyze the
+ * "status" and call appropriate response function.
+ * Enqueue the control packet for Application.
+ * @return None
+ */
 static VOID handle_rx_control_packet(PMINI_ADAPTER Adapter, struct sk_buff *skb)
 {
-	PPER_TARANG_DATA	pTarang = NULL;
+	PPER_TARANG_DATA pTarang = NULL;
 	BOOLEAN HighPriorityMessage = FALSE;
-	struct sk_buff * newPacket = NULL;
+	struct sk_buff *newPacket = NULL;
 	CHAR cntrl_msg_mask_bit = 0;
-	BOOLEAN drop_pkt_flag = TRUE ;
+	BOOLEAN drop_pkt_flag = TRUE;
 	USHORT usStatus = *(PUSHORT)(skb->data);
 
 	if (netif_msg_pktdata(Adapter))
 		print_hex_dump(KERN_DEBUG, PFX "rx control: ", DUMP_PREFIX_NONE,
-			       16, 1, skb->data, skb->len, 0);
-
-	switch(usStatus)
-	{
-		case CM_RESPONSES:               // 0xA0
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL, "MAC Version Seems to be Non Multi-Classifier, rejected by Driver");
-			HighPriorityMessage = TRUE ;
-			break;
-		case CM_CONTROL_NEWDSX_MULTICLASSIFIER_RESP:
-			HighPriorityMessage = TRUE ;
-			if(Adapter->LinkStatus==LINKUP_DONE)
-			{
-				CmControlResponseMessage(Adapter,(skb->data +sizeof(USHORT)));
-			}
-			break;
-		case LINK_CONTROL_RESP:          //0xA2
-		case STATUS_RSP:          //0xA1
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL,"LINK_CONTROL_RESP");
-			HighPriorityMessage = TRUE ;
-			LinkControlResponseMessage(Adapter,(skb->data + sizeof(USHORT)));
-			break;
-		case STATS_POINTER_RESP:       //0xA6
-			HighPriorityMessage = TRUE ;
-			StatisticsResponse(Adapter, (skb->data + sizeof(USHORT)));
-			break;
-		case IDLE_MODE_STATUS:			//0xA3
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL,"IDLE_MODE_STATUS Type Message Got from F/W");
-			InterfaceIdleModeRespond(Adapter, (PUINT)(skb->data +
-						sizeof(USHORT)));
-			HighPriorityMessage = TRUE ;
-			break;
-
-		case AUTH_SS_HOST_MSG:
-			HighPriorityMessage = TRUE ;
-			break;
-
-	 	default:
-			BCM_DEBUG_PRINT( Adapter,DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL,"Got Default Response");
-			/* Let the Application Deal with This Packet */
-			break;
+				16, 1, skb->data, skb->len, 0);
+
+	switch (usStatus) {
+	case CM_RESPONSES:               /* 0xA0 */
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
+			DBG_LVL_ALL,
+			"MAC Version Seems to be Non Multi-Classifier, rejected by Driver");
+		HighPriorityMessage = TRUE;
+		break;
+	case CM_CONTROL_NEWDSX_MULTICLASSIFIER_RESP:
+		HighPriorityMessage = TRUE;
+		if (Adapter->LinkStatus == LINKUP_DONE)
+			CmControlResponseMessage(Adapter,
+				(skb->data + sizeof(USHORT)));
+		break;
+	case LINK_CONTROL_RESP:          /* 0xA2 */
+	case STATUS_RSP:                 /* 0xA1 */
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
+			DBG_LVL_ALL, "LINK_CONTROL_RESP");
+		HighPriorityMessage = TRUE;
+		LinkControlResponseMessage(Adapter,
+			(skb->data + sizeof(USHORT)));
+		break;
+	case STATS_POINTER_RESP:         /* 0xA6 */
+		HighPriorityMessage = TRUE;
+		StatisticsResponse(Adapter, (skb->data + sizeof(USHORT)));
+		break;
+	case IDLE_MODE_STATUS:           /* 0xA3 */
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
+			DBG_LVL_ALL,
+			"IDLE_MODE_STATUS Type Message Got from F/W");
+		InterfaceIdleModeRespond(Adapter, (PUINT)(skb->data +
+					sizeof(USHORT)));
+		HighPriorityMessage = TRUE;
+		break;
+
+	case AUTH_SS_HOST_MSG:
+		HighPriorityMessage = TRUE;
+		break;
+
+	default:
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
+			DBG_LVL_ALL, "Got Default Response");
+		/* Let the Application Deal with This Packet */
+		break;
 	}
 
-	//Queue The Control Packet to The Application Queues
+	/* Queue The Control Packet to The Application Queues */
 	down(&Adapter->RxAppControlQueuelock);
 
-	for (pTarang = Adapter->pTarangs; pTarang; pTarang = pTarang->next)
-    {
-       	if(Adapter->device_removed)
-		{
+	for (pTarang = Adapter->pTarangs; pTarang; pTarang = pTarang->next) {
+		if (Adapter->device_removed)
 			break;
-		}
 
-		drop_pkt_flag = TRUE ;
+		drop_pkt_flag = TRUE;
 		/*
-			There are cntrl msg from A0 to AC. It has been mapped to 0 to C bit in the cntrl mask.
-			Also, by default AD to BF has been masked to the rest of the bits... which wil be ON by default.
-			if mask bit is enable to particular pkt status, send it out to app else stop it.
-		*/
+		 * There are cntrl msg from A0 to AC. It has been mapped to 0 to
+		 * C bit in the cntrl mask.
+		 * Also, by default AD to BF has been masked to the rest of the
+		 * bits... which wil be ON by default.
+		 * if mask bit is enable to particular pkt status, send it out
+		 * to app else stop it.
+		 */
 		cntrl_msg_mask_bit = (usStatus & 0x1F);
-		//printk("\ninew  msg  mask bit which is disable in mask:%X", cntrl_msg_mask_bit);
-		if(pTarang->RxCntrlMsgBitMask & (1<<cntrl_msg_mask_bit))
-				drop_pkt_flag = FALSE;
-
-		if ((drop_pkt_flag == TRUE)  || (pTarang->AppCtrlQueueLen > MAX_APP_QUEUE_LEN) ||
-					((pTarang->AppCtrlQueueLen > MAX_APP_QUEUE_LEN/2) && (HighPriorityMessage == FALSE)))
-		{
+		/*
+		 * printk("\ninew  msg  mask bit which is disable in mask:%X",
+		 *	cntrl_msg_mask_bit);
+		 */
+		if (pTarang->RxCntrlMsgBitMask & (1 << cntrl_msg_mask_bit))
+			drop_pkt_flag = FALSE;
+
+		if ((drop_pkt_flag == TRUE) ||
+				(pTarang->AppCtrlQueueLen > MAX_APP_QUEUE_LEN)
+				|| ((pTarang->AppCtrlQueueLen >
+					MAX_APP_QUEUE_LEN / 2) &&
+				    (HighPriorityMessage == FALSE))) {
 			/*
-				Assumption:-
-				1. every tarang manages it own dropped pkt statitistics
-				2. Total packet dropped per tarang will be equal to the sum of all types of dropped
-					pkt by that tarang only.
-
-			*/
-			switch(*(PUSHORT)skb->data)
-			{
-				case CM_RESPONSES:
-					pTarang->stDroppedAppCntrlMsgs.cm_responses++;
-					break;
-				case CM_CONTROL_NEWDSX_MULTICLASSIFIER_RESP:
-				 pTarang->stDroppedAppCntrlMsgs.cm_control_newdsx_multiclassifier_resp++;
-					 break;
-				case LINK_CONTROL_RESP:
-					pTarang->stDroppedAppCntrlMsgs.link_control_resp++;
-					break;
-				case STATUS_RSP:
-					pTarang->stDroppedAppCntrlMsgs.status_rsp++;
-					break;
-				case STATS_POINTER_RESP:
-					pTarang->stDroppedAppCntrlMsgs.stats_pointer_resp++;
-					break;
-				case IDLE_MODE_STATUS:
-					pTarang->stDroppedAppCntrlMsgs.idle_mode_status++ ;
-					break;
-				case AUTH_SS_HOST_MSG:
-					pTarang->stDroppedAppCntrlMsgs.auth_ss_host_msg++ ;
-					break;
+			 * Assumption:-
+			 * 1. every tarang manages it own dropped pkt
+			 *    statitistics
+			 * 2. Total packet dropped per tarang will be equal to
+			 *    the sum of all types of dropped pkt by that
+			 *    tarang only.
+			 */
+			switch (*(PUSHORT)skb->data) {
+			case CM_RESPONSES:
+				pTarang->stDroppedAppCntrlMsgs.cm_responses++;
+				break;
+			case CM_CONTROL_NEWDSX_MULTICLASSIFIER_RESP:
+				pTarang->stDroppedAppCntrlMsgs.cm_control_newdsx_multiclassifier_resp++;
+				break;
+			case LINK_CONTROL_RESP:
+				pTarang->stDroppedAppCntrlMsgs.link_control_resp++;
+				break;
+			case STATUS_RSP:
+				pTarang->stDroppedAppCntrlMsgs.status_rsp++;
+				break;
+			case STATS_POINTER_RESP:
+				pTarang->stDroppedAppCntrlMsgs.stats_pointer_resp++;
+				break;
+			case IDLE_MODE_STATUS:
+				pTarang->stDroppedAppCntrlMsgs.idle_mode_status++;
+				break;
+			case AUTH_SS_HOST_MSG:
+				pTarang->stDroppedAppCntrlMsgs.auth_ss_host_msg++;
+				break;
 			default:
-					pTarang->stDroppedAppCntrlMsgs.low_priority_message++ ;
-					break;
+				pTarang->stDroppedAppCntrlMsgs.low_priority_message++;
+				break;
 			}
 
 			continue;
 		}
 
-        newPacket = skb_clone(skb, GFP_KERNEL);
-        if (!newPacket)
-           break;
-        ENQUEUEPACKET(pTarang->RxAppControlHead,pTarang->RxAppControlTail,
-				newPacket);
-        pTarang->AppCtrlQueueLen++;
-    }
+		newPacket = skb_clone(skb, GFP_KERNEL);
+		if (!newPacket)
+			break;
+		ENQUEUEPACKET(pTarang->RxAppControlHead,
+				pTarang->RxAppControlTail, newPacket);
+		pTarang->AppCtrlQueueLen++;
+	}
 	up(&Adapter->RxAppControlQueuelock);
-    wake_up(&Adapter->process_read_wait_queue);
-    dev_kfree_skb(skb);
-	BCM_DEBUG_PRINT( Adapter,DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL, "After wake_up_interruptible");
+	wake_up(&Adapter->process_read_wait_queue);
+	dev_kfree_skb(skb);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL,
+			"After wake_up_interruptible");
 }
 
 /**
-@ingroup ctrl_pkt_functions
-Thread to handle control pkt reception
-*/
-int control_packet_handler  (PMINI_ADAPTER Adapter  /**< pointer to adapter object*/
-						)
+ * @ingroup ctrl_pkt_functions
+ * Thread to handle control pkt reception
+ */
+int control_packet_handler(PMINI_ADAPTER Adapter /* pointer to adapter object*/)
 {
-	struct sk_buff *ctrl_packet= NULL;
+	struct sk_buff *ctrl_packet = NULL;
 	unsigned long flags = 0;
-	//struct timeval tv ;
-	//int *puiBuffer = NULL ;
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL, "Entering to make thread wait on control packet event!");
-	while(1)
-	{
+	/* struct timeval tv; */
+	/* int *puiBuffer = NULL; */
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL,
+		"Entering to make thread wait on control packet event!");
+	while (1) {
 		wait_event_interruptible(Adapter->process_rx_cntrlpkt,
-								 atomic_read(&Adapter->cntrlpktCnt) ||
-								 Adapter->bWakeUpDevice ||
-								 kthread_should_stop()
-								);
+			atomic_read(&Adapter->cntrlpktCnt) ||
+			Adapter->bWakeUpDevice ||
+			kthread_should_stop());
 
 
-		if(kthread_should_stop())
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL, "Exiting \n");
+		if (kthread_should_stop()) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CP_CTRL_PKT,
+				DBG_LVL_ALL, "Exiting\n");
 			return 0;
 		}
-		if(TRUE == Adapter->bWakeUpDevice)
-		{
+		if (TRUE == Adapter->bWakeUpDevice) {
 			Adapter->bWakeUpDevice = FALSE;
-			if((FALSE == Adapter->bTriedToWakeUpFromlowPowerMode) &&
-				((TRUE == Adapter->IdleMode)|| (TRUE == Adapter->bShutStatus)))
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, CP_CTRL_PKT, DBG_LVL_ALL, "Calling InterfaceAbortIdlemode\n");
-	//			Adapter->bTriedToWakeUpFromlowPowerMode = TRUE;
-				InterfaceIdleModeWakeup (Adapter);
+			if ((FALSE == Adapter->bTriedToWakeUpFromlowPowerMode)
+					&& ((TRUE == Adapter->IdleMode) ||
+					    (TRUE == Adapter->bShutStatus))) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+					CP_CTRL_PKT, DBG_LVL_ALL,
+					"Calling InterfaceAbortIdlemode\n");
+				/*
+				 * Adapter->bTriedToWakeUpFromlowPowerMode
+				 *					= TRUE;
+				 */
+				InterfaceIdleModeWakeup(Adapter);
 			}
 			continue;
 		}
 
-		while(atomic_read(&Adapter->cntrlpktCnt))
-		{
+		while (atomic_read(&Adapter->cntrlpktCnt)) {
 			spin_lock_irqsave(&Adapter->control_queue_lock, flags);
 			ctrl_packet = Adapter->RxControlHead;
-			if(ctrl_packet)
-			{
-				DEQUEUEPACKET(Adapter->RxControlHead,Adapter->RxControlTail);
-//				Adapter->RxControlHead=ctrl_packet->next;
+			if (ctrl_packet) {
+				DEQUEUEPACKET(Adapter->RxControlHead,
+					Adapter->RxControlTail);
+				/* Adapter->RxControlHead=ctrl_packet->next; */
 			}
 
-			spin_unlock_irqrestore (&Adapter->control_queue_lock, flags);
-		 	handle_rx_control_packet(Adapter, ctrl_packet);
+			spin_unlock_irqrestore(&Adapter->control_queue_lock,
+						flags);
+			handle_rx_control_packet(Adapter, ctrl_packet);
 			atomic_dec(&Adapter->cntrlpktCnt);
 		}
 
@@ -201,22 +214,22 @@ int control_packet_handler  (PMINI_ADAPTER Adapter  /**< pointer to adapter obje
 INT flushAllAppQ(void)
 {
 	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	PPER_TARANG_DATA	pTarang = NULL;
+	PPER_TARANG_DATA pTarang = NULL;
 	struct sk_buff *PacketToDrop = NULL;
-	for(pTarang = Adapter->pTarangs; pTarang; pTarang = pTarang->next)
-	{
-		while(pTarang->RxAppControlHead != NULL)
-		{
-			PacketToDrop=pTarang->RxAppControlHead;
-			DEQUEUEPACKET(pTarang->RxAppControlHead,pTarang->RxAppControlTail);
+	for (pTarang = Adapter->pTarangs; pTarang; pTarang = pTarang->next) {
+		while (pTarang->RxAppControlHead != NULL) {
+			PacketToDrop = pTarang->RxAppControlHead;
+			DEQUEUEPACKET(pTarang->RxAppControlHead,
+					pTarang->RxAppControlTail);
 			dev_kfree_skb(PacketToDrop);
 		}
 		pTarang->AppCtrlQueueLen = 0;
-		//dropped contrl packet statistics also should be reset.
-		memset((PVOID)&pTarang->stDroppedAppCntrlMsgs, 0, sizeof(S_MIBS_DROPPED_APP_CNTRL_MESSAGES));
+		/* dropped contrl packet statistics also should be reset. */
+		memset((PVOID)&pTarang->stDroppedAppCntrlMsgs, 0,
+			sizeof(S_MIBS_DROPPED_APP_CNTRL_MESSAGES));
 
 	}
-	return STATUS_SUCCESS ;
+	return STATUS_SUCCESS;
 }
 
 
diff --git a/drivers/staging/bcm/InterfaceDld.c b/drivers/staging/bcm/InterfaceDld.c
index bcd86bbef2fd..65c352f35681 100644
--- a/drivers/staging/bcm/InterfaceDld.c
+++ b/drivers/staging/bcm/InterfaceDld.c
@@ -62,6 +62,7 @@ int InterfaceFileReadbackFromChip(PVOID arg, struct file *flp, unsigned int on_c
 	static int fw_down;
 	INT Status = STATUS_SUCCESS;
 	PS_INTERFACE_ADAPTER psIntfAdapter = (PS_INTERFACE_ADAPTER)arg;
+	int bytes;
 
 	buff = kmalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_DMA);
 	buff_readback = kmalloc(MAX_TRANSFER_CTRL_BYTE_USB , GFP_DMA);
@@ -94,8 +95,9 @@ int InterfaceFileReadbackFromChip(PVOID arg, struct file *flp, unsigned int on_c
 			break;
 		}
 
-		Status = InterfaceRDM(psIntfAdapter, on_chip_loc, buff_readback, len);
-		if (Status) {
+		bytes = InterfaceRDM(psIntfAdapter, on_chip_loc, buff_readback, len);
+		if (bytes < 0) {
+			Status = bytes;
 			BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "RDM of len %d Failed! %d", len, reg);
 			goto exit;
 		}
@@ -302,6 +304,7 @@ static INT buffRdbkVerify(PMINI_ADAPTER Adapter, PUCHAR mappedbuffer, UINT u32Fi
 	UINT len = u32FirmwareLength;
 	INT retval = STATUS_SUCCESS;
 	PUCHAR readbackbuff = kzalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_KERNEL);
+	int bytes;
 
 	if (NULL == readbackbuff) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "MEMORY ALLOCATION FAILED");
@@ -310,9 +313,10 @@ static INT buffRdbkVerify(PMINI_ADAPTER Adapter, PUCHAR mappedbuffer, UINT u32Fi
 
 	while (u32FirmwareLength && !retval) {
 		len = MIN_VAL(u32FirmwareLength, MAX_TRANSFER_CTRL_BYTE_USB);
-		retval = rdm(Adapter, u32StartingAddress, readbackbuff, len);
+		bytes = rdm(Adapter, u32StartingAddress, readbackbuff, len);
 
-		if (retval) {
+		if (bytes < 0) {
+			retval = bytes;
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "rdm failed with status %d", retval);
 			break;
 		}
diff --git a/drivers/staging/bcm/InterfaceIdleMode.c b/drivers/staging/bcm/InterfaceIdleMode.c
index 96fa4ead7930..faeb03e62c06 100644
--- a/drivers/staging/bcm/InterfaceIdleMode.c
+++ b/drivers/staging/bcm/InterfaceIdleMode.c
@@ -46,6 +46,7 @@ int InterfaceIdleModeRespond(PMINI_ADAPTER Adapter, unsigned int* puiBuffer)
 {
 	int	status = STATUS_SUCCESS;
 	unsigned int	uiRegRead = 0;
+	int bytes;
 
 	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, IDLE_MODE, DBG_LVL_ALL,"SubType of Message :0x%X", ntohl(*puiBuffer));
 
@@ -77,16 +78,16 @@ int InterfaceIdleModeRespond(PMINI_ADAPTER Adapter, unsigned int* puiBuffer)
 			else if(Adapter->ulPowerSaveMode != DEVICE_POWERSAVE_MODE_AS_PROTOCOL_IDLE_MODE)
 			{
 				//clear on read Register
-				status = rdmalt(Adapter, DEVICE_INT_OUT_EP_REG0, &uiRegRead, sizeof(uiRegRead));
-				if(status)
-				{
+				bytes = rdmalt(Adapter, DEVICE_INT_OUT_EP_REG0, &uiRegRead, sizeof(uiRegRead));
+				if (bytes < 0) {
+					status = bytes;
 					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "rdm failed while clearing H/W Abort Reg0");
 					return status;
 				}
 				//clear on read Register
-				status = rdmalt (Adapter, DEVICE_INT_OUT_EP_REG1, &uiRegRead, sizeof(uiRegRead));
-				if(status)
-				{
+				bytes = rdmalt(Adapter, DEVICE_INT_OUT_EP_REG1, &uiRegRead, sizeof(uiRegRead));
+				if (bytes < 0) {
+					status = bytes;
 					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "rdm failed while clearing H/W Abort	Reg1");
 					return status;
 				}
@@ -117,9 +118,9 @@ int InterfaceIdleModeRespond(PMINI_ADAPTER Adapter, unsigned int* puiBuffer)
 					Adapter->chip_id== BCS220_3)
 			{
 
-				status = rdmalt(Adapter, HPM_CONFIG_MSW, &uiRegRead, sizeof(uiRegRead));
-				if(status)
-				{
+				bytes = rdmalt(Adapter, HPM_CONFIG_MSW, &uiRegRead, sizeof(uiRegRead));
+				if (bytes < 0) {
+					status = bytes;
 					BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, IDLE_MODE, DBG_LVL_ALL, "rdm failed while Reading HPM_CONFIG_LDO145 Reg 0\n");
 					return status;
 				}
@@ -266,6 +267,8 @@ void InterfaceHandleShutdownModeWakeup(PMINI_ADAPTER Adapter)
 {
 	unsigned int uiRegVal = 0;
 	INT Status = 0;
+	int bytes;
+
 	if(Adapter->ulPowerSaveMode == DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING)
 	{
 		// clear idlemode interrupt.
@@ -282,16 +285,16 @@ void InterfaceHandleShutdownModeWakeup(PMINI_ADAPTER Adapter)
 	{
 
         //clear Interrupt EP registers.
-		Status = rdmalt(Adapter,DEVICE_INT_OUT_EP_REG0, &uiRegVal, sizeof(uiRegVal));
-		if(Status)
-		{
+		bytes = rdmalt(Adapter,DEVICE_INT_OUT_EP_REG0, &uiRegVal, sizeof(uiRegVal));
+		if (bytes < 0) {
+			Status = bytes;
 			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"RDM of DEVICE_INT_OUT_EP_REG0 failed with Err :%d", Status);
 			return;
 		}
 
-        Status = rdmalt(Adapter,DEVICE_INT_OUT_EP_REG1, &uiRegVal, sizeof(uiRegVal));
-		if(Status)
-		{
+		bytes = rdmalt(Adapter,DEVICE_INT_OUT_EP_REG1, &uiRegVal, sizeof(uiRegVal));
+		if (bytes < 0) {
+			Status = bytes;
 			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"RDM of DEVICE_INT_OUT_EP_REG1 failed with Err :%d", Status);
 			return;
 		}
diff --git a/drivers/staging/bcm/InterfaceInit.c b/drivers/staging/bcm/InterfaceInit.c
index a09d35108f04..8e3c586a699c 100644
--- a/drivers/staging/bcm/InterfaceInit.c
+++ b/drivers/staging/bcm/InterfaceInit.c
@@ -68,7 +68,7 @@ static void InterfaceAdapterFree(PS_INTERFACE_ADAPTER psIntfAdapter)
 static void ConfigureEndPointTypesThroughEEPROM(PMINI_ADAPTER Adapter)
 {
 	unsigned long ulReg = 0;
-	int ret;
+	int bytes;
 
 	/* Program EP2 MAX_PKT_SIZE */
 	ulReg = ntohl(EP2_MPS_REG);
@@ -94,8 +94,8 @@ static void ConfigureEndPointTypesThroughEEPROM(PMINI_ADAPTER Adapter)
 	BeceemEEPROMBulkWrite(Adapter, (PUCHAR)&ulReg, 0x140, 4, TRUE);
 
 	/* Program TX EP as interrupt(Alternate Setting) */
-	ret = rdmalt(Adapter, 0x0F0110F8, (u32 *)&ulReg, sizeof(u32));
-	if (ret) {
+	bytes = rdmalt(Adapter, 0x0F0110F8, (u32 *)&ulReg, sizeof(u32));
+	if (bytes < 0) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_INITEXIT, DRV_ENTRY, DBG_LVL_ALL,
 			"reading of Tx EP failed\n");
 		return;
@@ -430,6 +430,7 @@ static int InterfaceAdapterInit(PS_INTERFACE_ADAPTER psIntfAdapter)
 	int usedIntOutForBulkTransfer = 0 ;
 	BOOLEAN bBcm16 = FALSE;
 	UINT uiData = 0;
+	int bytes;
 
 	/* Store the usb dev into interface adapter */
 	psIntfAdapter->udev = usb_get_dev(interface_to_usbdev(psIntfAdapter->interface));
@@ -438,9 +439,10 @@ static int InterfaceAdapterInit(PS_INTERFACE_ADAPTER psIntfAdapter)
 	psIntfAdapter->psAdapter->interface_rdm = BcmRDM;
 	psIntfAdapter->psAdapter->interface_wrm = BcmWRM;
 
-	retval = rdmalt(psIntfAdapter->psAdapter, CHIP_ID_REG,
+	bytes = rdmalt(psIntfAdapter->psAdapter, CHIP_ID_REG,
 			(u32 *)&(psIntfAdapter->psAdapter->chip_id), sizeof(u32));
-	if (retval) {
+	if (bytes < 0) {
+		retval = bytes;
 		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_PRINTK, 0, 0, "CHIP ID Read Failed\n");
 		return retval;
 	}
diff --git a/drivers/staging/bcm/InterfaceMisc.c b/drivers/staging/bcm/InterfaceMisc.c
index 61f878b4f56c..2218faeaf8ac 100644
--- a/drivers/staging/bcm/InterfaceMisc.c
+++ b/drivers/staging/bcm/InterfaceMisc.c
@@ -5,7 +5,7 @@ INT InterfaceRDM(PS_INTERFACE_ADAPTER psIntfAdapter,
 		PVOID buff,
 		INT len)
 {
-	int retval = 0;
+	int bytes;
 	USHORT usRetries = 0;
 
 	if (psIntfAdapter == NULL) {
@@ -30,7 +30,7 @@ INT InterfaceRDM(PS_INTERFACE_ADAPTER psIntfAdapter,
 	psIntfAdapter->psAdapter->DeviceAccess = TRUE;
 
 	do {
-		retval = usb_control_msg(psIntfAdapter->udev,
+		bytes = usb_control_msg(psIntfAdapter->udev,
 					usb_rcvctrlpipe(psIntfAdapter->udev, 0),
 					0x02,
 					0xC2,
@@ -41,22 +41,20 @@ INT InterfaceRDM(PS_INTERFACE_ADAPTER psIntfAdapter,
 					5000);
 
 		usRetries++;
-		if (-ENODEV == retval) {
+		if (-ENODEV == bytes) {
 			psIntfAdapter->psAdapter->device_removed = TRUE;
 			break;
 		}
 
-	} while ((retval < 0) && (usRetries < MAX_RDM_WRM_RETIRES));
+	} while ((bytes < 0) && (usRetries < MAX_RDM_WRM_RETIRES));
 
-	if (retval < 0)	{
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, RDM, DBG_LVL_ALL, "RDM failed status :%d, retires :%d", retval, usRetries);
-		psIntfAdapter->psAdapter->DeviceAccess = FALSE;
-		return retval;
-	} else {
-		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, RDM, DBG_LVL_ALL, "RDM sent %d", retval);
-		psIntfAdapter->psAdapter->DeviceAccess = FALSE;
-		return STATUS_SUCCESS;
-	}
+	if (bytes < 0)
+		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, RDM, DBG_LVL_ALL, "RDM failed status :%d, retires :%d", bytes, usRetries);
+	else
+		BCM_DEBUG_PRINT(psIntfAdapter->psAdapter, DBG_TYPE_OTHERS, RDM, DBG_LVL_ALL, "RDM sent %d", bytes);
+
+	psIntfAdapter->psAdapter->DeviceAccess = FALSE;
+	return bytes;
 }
 
 INT InterfaceWRM(PS_INTERFACE_ADAPTER psIntfAdapter,
diff --git a/drivers/staging/bcm/Misc.c b/drivers/staging/bcm/Misc.c
index e9f29d597518..c7725e141fd5 100644
--- a/drivers/staging/bcm/Misc.c
+++ b/drivers/staging/bcm/Misc.c
@@ -814,6 +814,7 @@ int reset_card_proc(PMINI_ADAPTER ps_adapter)
 	PMINI_ADAPTER Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	PS_INTERFACE_ADAPTER psIntfAdapter = NULL;
 	unsigned int value = 0, uiResetValue = 0;
+	int bytes;
 
 	psIntfAdapter = ((PS_INTERFACE_ADAPTER)(ps_adapter->pvInterfaceAdapter));
 	ps_adapter->bDDRInitDone = FALSE;
@@ -848,8 +849,9 @@ int reset_card_proc(PMINI_ADAPTER ps_adapter)
 			ps_adapter->chip_id == BCS250_BC ||
 			ps_adapter->chip_id == BCS220_3) {
 
-			retval = rdmalt(ps_adapter, HPM_CONFIG_LDO145, &value, sizeof(value));
-			if (retval < 0) {
+			bytes = rdmalt(ps_adapter, HPM_CONFIG_LDO145, &value, sizeof(value));
+			if (bytes < 0) {
+				retval = bytes;
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "read failed with status :%d", retval);
 				goto err_exit;
 			}
@@ -862,8 +864,9 @@ int reset_card_proc(PMINI_ADAPTER ps_adapter)
 			}
 		}
 	} else {
-		retval = rdmalt(ps_adapter, 0x0f007018, &value, sizeof(value));
-		if (retval < 0) {
+		bytes = rdmalt(ps_adapter, 0x0f007018, &value, sizeof(value));
+		if (bytes < 0) {
+			retval = bytes;
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "read failed with status :%d", retval);
 			goto err_exit;
 		}
@@ -925,11 +928,16 @@ err_exit:
 
 int run_card_proc(PMINI_ADAPTER ps_adapter)
 {
+	int status = STATUS_SUCCESS;
+	int bytes;
+
 	unsigned int value = 0;
 	{
-		if (rdmalt(ps_adapter, CLOCK_RESET_CNTRL_REG_1, &value, sizeof(value)) < 0) {
+		bytes = rdmalt(ps_adapter, CLOCK_RESET_CNTRL_REG_1, &value, sizeof(value));
+		if (bytes < 0) {
+			status = bytes;
 			BCM_DEBUG_PRINT(ps_adapter, DBG_TYPE_INITEXIT, MP_INIT, DBG_LVL_ALL, "%s:%d\n", __func__, __LINE__);
-			return STATUS_FAILURE;
+			return status;
 		}
 
 		if (ps_adapter->bFlashBoot)
@@ -942,7 +950,7 @@ int run_card_proc(PMINI_ADAPTER ps_adapter)
 			return STATUS_FAILURE;
 		}
 	}
-	return STATUS_SUCCESS;
+	return status;
 }
 
 int InitCardAndDownloadFirmware(PMINI_ADAPTER ps_adapter)
@@ -1215,6 +1223,7 @@ static unsigned char *ReadMacAddrEEPROM(PMINI_ADAPTER Adapter, ulong dwAddress)
 	int status = 0, i = 0;
 	unsigned int temp = 0;
 	unsigned char *pucmacaddr = kmalloc(MAC_ADDRESS_SIZE, GFP_KERNEL);
+	int bytes;
 
 	if (!pucmacaddr) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "No Buffers to Read the EEPROM Address\n");
@@ -1231,8 +1240,9 @@ static unsigned char *ReadMacAddrEEPROM(PMINI_ADAPTER Adapter, ulong dwAddress)
 	}
 
 	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
-		status = rdmalt(Adapter, EEPROM_READ_DATA_Q_REG, &temp, sizeof(temp));
-		if (status != STATUS_SUCCESS) {
+		bytes = rdmalt(Adapter, EEPROM_READ_DATA_Q_REG, &temp, sizeof(temp));
+		if (bytes < 0) {
+			status = bytes;
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "rdm Failed..\n");
 			kfree(pucmacaddr);
 			pucmacaddr = NULL;
@@ -1574,11 +1584,13 @@ void update_per_sf_desc_cnts(PMINI_ADAPTER Adapter)
 {
 	INT iIndex = 0;
 	u32 uibuff[MAX_TARGET_DSX_BUFFERS];
+	int bytes;
 
 	if (!atomic_read(&Adapter->uiMBupdate))
 		return;
 
-	if (rdmaltWithLock(Adapter, TARGET_SFID_TXDESC_MAP_LOC, (PUINT)uibuff, sizeof(UINT) * MAX_TARGET_DSX_BUFFERS) < 0) {
+	bytes = rdmaltWithLock(Adapter, TARGET_SFID_TXDESC_MAP_LOC, (PUINT)uibuff, sizeof(UINT) * MAX_TARGET_DSX_BUFFERS);
+	if (bytes < 0) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "rdm failed\n");
 		return;
 	}
diff --git a/drivers/staging/bcm/hostmibs.c b/drivers/staging/bcm/hostmibs.c
index c13ea5c9a2aa..101c4e31249e 100644
--- a/drivers/staging/bcm/hostmibs.c
+++ b/drivers/staging/bcm/hostmibs.c
@@ -1,4 +1,3 @@
-
 /*
  * File Name: hostmibs.c
  *
@@ -6,73 +5,72 @@
  *
  * Abstract: This file contains the routines to copy the statistics used by
  * the driver to the Host MIBS structure and giving the same to Application.
- *
  */
+
 #include "headers.h"
 
-INT  ProcessGetHostMibs(PMINI_ADAPTER Adapter, S_MIBS_HOST_STATS_MIBS *pstHostMibs)
+INT ProcessGetHostMibs(PMINI_ADAPTER Adapter, S_MIBS_HOST_STATS_MIBS *pstHostMibs)
 {
-	S_SERVICEFLOW_ENTRY    *pstServiceFlowEntry = NULL;
-	S_PHS_RULE             *pstPhsRule          = NULL;
-	S_CLASSIFIER_TABLE     *pstClassifierTable  = NULL;
-	S_CLASSIFIER_ENTRY     *pstClassifierRule   = NULL;
-	PPHS_DEVICE_EXTENSION  pDeviceExtension     = (PPHS_DEVICE_EXTENSION)&Adapter->stBCMPhsContext;
+	S_SERVICEFLOW_ENTRY *pstServiceFlowEntry = NULL;
+	S_PHS_RULE *pstPhsRule = NULL;
+	S_CLASSIFIER_TABLE *pstClassifierTable = NULL;
+	S_CLASSIFIER_ENTRY *pstClassifierRule = NULL;
+	PPHS_DEVICE_EXTENSION pDeviceExtension = (PPHS_DEVICE_EXTENSION) &Adapter->stBCMPhsContext;
 
-	UINT nClassifierIndex = 0, nPhsTableIndex = 0,nSfIndex = 0, uiIndex = 0;
+	UINT nClassifierIndex = 0, nPhsTableIndex = 0, nSfIndex = 0, uiIndex = 0;
 
-	if(pDeviceExtension == NULL)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, HOST_MIBS, DBG_LVL_ALL, "Invalid Device Extension\n");
+	if (pDeviceExtension == NULL) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, HOST_MIBS, DBG_LVL_ALL, "Invalid Device Extension\n");
 		return STATUS_FAILURE;
 	}
 
-	//Copy the classifier Table
-	for(nClassifierIndex=0; nClassifierIndex < MAX_CLASSIFIERS;
-			nClassifierIndex++)
-	{
-		if(Adapter->astClassifierTable[nClassifierIndex].bUsed == TRUE)
-			memcpy((PVOID)&pstHostMibs->astClassifierTable[nClassifierIndex],
-				(PVOID)&Adapter->astClassifierTable[nClassifierIndex],
-				sizeof(S_MIBS_CLASSIFIER_RULE));
+	/* Copy the classifier Table */
+	for (nClassifierIndex = 0; nClassifierIndex < MAX_CLASSIFIERS; nClassifierIndex++) {
+		if (Adapter->astClassifierTable[nClassifierIndex].bUsed == TRUE)
+			memcpy((PVOID) & pstHostMibs->
+			       astClassifierTable[nClassifierIndex],
+			       (PVOID) & Adapter->
+			       astClassifierTable[nClassifierIndex],
+			       sizeof(S_MIBS_CLASSIFIER_RULE));
 	}
 
-  //Copy the SF Table
-	for(nSfIndex=0; nSfIndex < NO_OF_QUEUES ; nSfIndex++)
-	{
-	if(Adapter->PackInfo[nSfIndex].bValid)
-	{
-			memcpy((PVOID)&pstHostMibs->astSFtable[nSfIndex],(PVOID)&Adapter->PackInfo[nSfIndex],sizeof(S_MIBS_SERVICEFLOW_TABLE));
-	}
-	else
-	{
-		//if index in not valid, don't process this for the PHS table. Go For the next entry.
-		continue ;
-	}
+	/* Copy the SF Table */
+	for (nSfIndex = 0; nSfIndex < NO_OF_QUEUES; nSfIndex++) {
+		if (Adapter->PackInfo[nSfIndex].bValid) {
+			memcpy((PVOID) & pstHostMibs->astSFtable[nSfIndex],
+			       (PVOID) & Adapter->PackInfo[nSfIndex],
+			       sizeof(S_MIBS_SERVICEFLOW_TABLE));
+		} else {
+			/* If index in not valid,
+			 * don't process this for the PHS table.
+			 * Go For the next entry.
+			 */
+			continue;
+		}
 
-		//Retrieve the SFID Entry Index for requested Service Flow
-		if(PHS_INVALID_TABLE_INDEX == GetServiceFlowEntry(pDeviceExtension->pstServiceFlowPhsRulesTable,
-						  Adapter->PackInfo[nSfIndex].usVCID_Value ,&pstServiceFlowEntry))
-		{
+		/* Retrieve the SFID Entry Index for requested Service Flow */
+		if (PHS_INVALID_TABLE_INDEX ==
+		    GetServiceFlowEntry(pDeviceExtension->
+					pstServiceFlowPhsRulesTable,
+					Adapter->PackInfo[nSfIndex].
+					usVCID_Value, &pstServiceFlowEntry))
 
-		continue;
-		}
+			continue;
 
 		pstClassifierTable = pstServiceFlowEntry->pstClassifierTable;
 
-
-		for(uiIndex = 0; uiIndex < MAX_PHSRULE_PER_SF; uiIndex++)
-		{
+		for (uiIndex = 0; uiIndex < MAX_PHSRULE_PER_SF; uiIndex++) {
 			pstClassifierRule = &pstClassifierTable->stActivePhsRulesList[uiIndex];
 
-		if(pstClassifierRule->bUsed)
-		{
-			pstPhsRule = pstClassifierRule->pstPhsRule;
+			if (pstClassifierRule->bUsed) {
+				pstPhsRule = pstClassifierRule->pstPhsRule;
 
-			pstHostMibs->astPhsRulesTable[nPhsTableIndex].ulSFID = Adapter->PackInfo[nSfIndex].ulSFID;
+				pstHostMibs->astPhsRulesTable[nPhsTableIndex].
+				    ulSFID = Adapter->PackInfo[nSfIndex].ulSFID;
 
-			memcpy(&pstHostMibs->astPhsRulesTable[nPhsTableIndex].u8PHSI,
-						&pstPhsRule->u8PHSI,
-						sizeof(S_PHS_RULE));
+				memcpy(&pstHostMibs->
+				       astPhsRulesTable[nPhsTableIndex].u8PHSI,
+				       &pstPhsRule->u8PHSI, sizeof(S_PHS_RULE));
 				nPhsTableIndex++;
 
 			}
@@ -81,65 +79,63 @@ INT  ProcessGetHostMibs(PMINI_ADAPTER Adapter, S_MIBS_HOST_STATS_MIBS *pstHostMi
 
 	}
 
-
-	//copy other Host Statistics parameters
+	/* Copy other Host Statistics parameters */
 	pstHostMibs->stHostInfo.GoodTransmits = Adapter->dev->stats.tx_packets;
 	pstHostMibs->stHostInfo.GoodReceives = Adapter->dev->stats.rx_packets;
-	pstHostMibs->stHostInfo.CurrNumFreeDesc =
-			atomic_read(&Adapter->CurrNumFreeTxDesc);
+	pstHostMibs->stHostInfo.CurrNumFreeDesc = atomic_read(&Adapter->CurrNumFreeTxDesc);
 	pstHostMibs->stHostInfo.BEBucketSize = Adapter->BEBucketSize;
 	pstHostMibs->stHostInfo.rtPSBucketSize = Adapter->rtPSBucketSize;
 	pstHostMibs->stHostInfo.TimerActive = Adapter->TimerActive;
 	pstHostMibs->stHostInfo.u32TotalDSD = Adapter->u32TotalDSD;
 
-	memcpy(pstHostMibs->stHostInfo.aTxPktSizeHist,Adapter->aTxPktSizeHist,sizeof(UINT32)*MIBS_MAX_HIST_ENTRIES);
-	memcpy(pstHostMibs->stHostInfo.aRxPktSizeHist,Adapter->aRxPktSizeHist,sizeof(UINT32)*MIBS_MAX_HIST_ENTRIES);
+	memcpy(pstHostMibs->stHostInfo.aTxPktSizeHist, Adapter->aTxPktSizeHist, sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
+	memcpy(pstHostMibs->stHostInfo.aRxPktSizeHist, Adapter->aRxPktSizeHist, sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
 
 	return STATUS_SUCCESS;
 }
 
-
 VOID GetDroppedAppCntrlPktMibs(S_MIBS_HOST_STATS_MIBS *pstHostMibs, const PPER_TARANG_DATA pTarang)
 {
 	memcpy(&(pstHostMibs->stDroppedAppCntrlMsgs),
-	       &(pTarang->stDroppedAppCntrlMsgs),sizeof(S_MIBS_DROPPED_APP_CNTRL_MESSAGES));
+	       &(pTarang->stDroppedAppCntrlMsgs),
+	       sizeof(S_MIBS_DROPPED_APP_CNTRL_MESSAGES));
 }
 
-
-VOID CopyMIBSExtendedSFParameters(PMINI_ADAPTER Adapter,
-		CServiceFlowParamSI *psfLocalSet, UINT uiSearchRuleIndex)
+VOID CopyMIBSExtendedSFParameters(PMINI_ADAPTER Adapter, CServiceFlowParamSI *psfLocalSet, UINT uiSearchRuleIndex)
 {
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfSfid = psfLocalSet->u32SFID;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsMaxSustainedRate = psfLocalSet->u32MaxSustainedTrafficRate;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsMaxTrafficBurst = psfLocalSet->u32MaxTrafficBurst;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsMinReservedRate = psfLocalSet->u32MinReservedTrafficRate;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsToleratedJitter = psfLocalSet->u32ToleratedJitter;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsMaxLatency = psfLocalSet->u32MaximumLatency;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsFixedVsVariableSduInd = psfLocalSet->u8FixedLengthVSVariableLengthSDUIndicator;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsFixedVsVariableSduInd = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsFixedVsVariableSduInd);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsSduSize = psfLocalSet->u8SDUSize;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsSduSize = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsSduSize);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsSfSchedulingType = psfLocalSet->u8ServiceFlowSchedulingType;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsSfSchedulingType = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsSfSchedulingType);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqEnable = psfLocalSet->u8ARQEnable;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqEnable = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqEnable);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqWindowSize = ntohs(psfLocalSet->u16ARQWindowSize);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqWindowSize = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqWindowSize);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqBlockLifetime = ntohs(psfLocalSet->u16ARQBlockLifeTime);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqBlockLifetime = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqBlockLifetime);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqSyncLossTimeout = ntohs(psfLocalSet->u16ARQSyncLossTimeOut);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqSyncLossTimeout = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqSyncLossTimeout);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqDeliverInOrder = psfLocalSet->u8ARQDeliverInOrder;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqDeliverInOrder = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqDeliverInOrder);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqRxPurgeTimeout = ntohs(psfLocalSet->u16ARQRxPurgeTimeOut);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqRxPurgeTimeout = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqRxPurgeTimeout);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqBlockSize = ntohs(psfLocalSet->u16ARQBlockSize);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqBlockSize = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsArqBlockSize);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsReqTxPolicy = psfLocalSet->u8RequesttransmissionPolicy;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsReqTxPolicy = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsReqTxPolicy);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnSfCsSpecification = psfLocalSet->u8CSSpecification;
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnSfCsSpecification = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnSfCsSpecification);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsTargetSaid = ntohs(psfLocalSet->u16TargetSAID);
-	Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsTargetSaid = ntohl(Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable.wmanIfCmnCpsTargetSaid);
+	S_MIBS_EXTSERVICEFLOW_PARAMETERS *t = &Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable;
+
+	t->wmanIfSfid = psfLocalSet->u32SFID;
+	t->wmanIfCmnCpsMaxSustainedRate = psfLocalSet->u32MaxSustainedTrafficRate;
+	t->wmanIfCmnCpsMaxTrafficBurst = psfLocalSet->u32MaxTrafficBurst;
+	t->wmanIfCmnCpsMinReservedRate = psfLocalSet->u32MinReservedTrafficRate;
+	t->wmanIfCmnCpsToleratedJitter = psfLocalSet->u32ToleratedJitter;
+	t->wmanIfCmnCpsMaxLatency = psfLocalSet->u32MaximumLatency;
+	t->wmanIfCmnCpsFixedVsVariableSduInd = psfLocalSet->u8FixedLengthVSVariableLengthSDUIndicator;
+	t->wmanIfCmnCpsFixedVsVariableSduInd = ntohl(t->wmanIfCmnCpsFixedVsVariableSduInd);
+	t->wmanIfCmnCpsSduSize = psfLocalSet->u8SDUSize;
+	t->wmanIfCmnCpsSduSize = ntohl(t->wmanIfCmnCpsSduSize);
+	t->wmanIfCmnCpsSfSchedulingType = psfLocalSet->u8ServiceFlowSchedulingType;
+	t->wmanIfCmnCpsSfSchedulingType = ntohl(t->wmanIfCmnCpsSfSchedulingType);
+	t->wmanIfCmnCpsArqEnable = psfLocalSet->u8ARQEnable;
+	t->wmanIfCmnCpsArqEnable = ntohl(t->wmanIfCmnCpsArqEnable);
+	t->wmanIfCmnCpsArqWindowSize = ntohs(psfLocalSet->u16ARQWindowSize);
+	t->wmanIfCmnCpsArqWindowSize = ntohl(t->wmanIfCmnCpsArqWindowSize);
+	t->wmanIfCmnCpsArqBlockLifetime = ntohs(psfLocalSet->u16ARQBlockLifeTime);
+	t->wmanIfCmnCpsArqBlockLifetime = ntohl(t->wmanIfCmnCpsArqBlockLifetime);
+	t->wmanIfCmnCpsArqSyncLossTimeout = ntohs(psfLocalSet->u16ARQSyncLossTimeOut);
+	t->wmanIfCmnCpsArqSyncLossTimeout = ntohl(t->wmanIfCmnCpsArqSyncLossTimeout);
+	t->wmanIfCmnCpsArqDeliverInOrder = psfLocalSet->u8ARQDeliverInOrder;
+	t->wmanIfCmnCpsArqDeliverInOrder = ntohl(t->wmanIfCmnCpsArqDeliverInOrder);
+	t->wmanIfCmnCpsArqRxPurgeTimeout = ntohs(psfLocalSet->u16ARQRxPurgeTimeOut);
+	t->wmanIfCmnCpsArqRxPurgeTimeout = ntohl(t->wmanIfCmnCpsArqRxPurgeTimeout);
+	t->wmanIfCmnCpsArqBlockSize = ntohs(psfLocalSet->u16ARQBlockSize);
+	t->wmanIfCmnCpsArqBlockSize = ntohl(t->wmanIfCmnCpsArqBlockSize);
+	t->wmanIfCmnCpsReqTxPolicy = psfLocalSet->u8RequesttransmissionPolicy;
+	t->wmanIfCmnCpsReqTxPolicy = ntohl(t->wmanIfCmnCpsReqTxPolicy);
+	t->wmanIfCmnSfCsSpecification = psfLocalSet->u8CSSpecification;
+	t->wmanIfCmnSfCsSpecification = ntohl(t->wmanIfCmnSfCsSpecification);
+	t->wmanIfCmnCpsTargetSaid = ntohs(psfLocalSet->u16TargetSAID);
+	t->wmanIfCmnCpsTargetSaid = ntohl(t->wmanIfCmnCpsTargetSaid);
 
 }
diff --git a/drivers/staging/bcm/led_control.c b/drivers/staging/bcm/led_control.c
index 16e939fa15d6..c7f488629722 100644
--- a/drivers/staging/bcm/led_control.c
+++ b/drivers/staging/bcm/led_control.c
@@ -5,65 +5,69 @@
 
 static B_UINT16 CFG_CalculateChecksum(B_UINT8 *pu8Buffer, B_UINT32 u32Size)
 {
-	B_UINT16 	u16CheckSum=0;
-	while(u32Size--) {
+	B_UINT16 u16CheckSum = 0;
+	while (u32Size--) {
 		u16CheckSum += (B_UINT8)~(*pu8Buffer);
-	    pu8Buffer++;
+		pu8Buffer++;
 	}
 	return u16CheckSum;
 }
+
 BOOLEAN IsReqGpioIsLedInNVM(PMINI_ADAPTER Adapter, UINT gpios)
 {
-	INT Status ;
-	Status = (Adapter->gpioBitMap & gpios) ^ gpios ;
-	if(Status)
+	INT Status;
+	Status = (Adapter->gpioBitMap & gpios) ^ gpios;
+	if (Status)
 		return FALSE;
 	else
 		return TRUE;
 }
 
-static INT LED_Blink(PMINI_ADAPTER Adapter, UINT GPIO_Num, UCHAR uiLedIndex, ULONG timeout, INT num_of_time, LedEventInfo_t currdriverstate)
+static INT LED_Blink(PMINI_ADAPTER Adapter, UINT GPIO_Num, UCHAR uiLedIndex,
+		ULONG timeout, INT num_of_time, LedEventInfo_t currdriverstate)
 {
 	int Status = STATUS_SUCCESS;
 	BOOLEAN bInfinite = FALSE;
 
-	/*Check if num_of_time is -ve. If yes, blink led in infinite loop*/
-	if(num_of_time < 0)
-	{
+	/* Check if num_of_time is -ve. If yes, blink led in infinite loop */
+	if (num_of_time < 0) {
 		bInfinite = TRUE;
 		num_of_time = 1;
 	}
-	while(num_of_time)
-	{
-
-		if(currdriverstate == Adapter->DriverState)
+	while (num_of_time) {
+		if (currdriverstate == Adapter->DriverState)
 			TURN_ON_LED(GPIO_Num, uiLedIndex);
 
-		/*Wait for timeout after setting on the LED*/
-		Status = wait_event_interruptible_timeout(Adapter->LEDInfo.notify_led_event,
-					currdriverstate != Adapter->DriverState || kthread_should_stop(),
-					msecs_to_jiffies(timeout));
-
-		if(kthread_should_stop())
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL, "Led thread got signal to exit..hence exiting");
-			Adapter->LEDInfo.led_thread_running= BCM_LED_THREAD_DISABLED;
+		/* Wait for timeout after setting on the LED */
+		Status = wait_event_interruptible_timeout(
+				Adapter->LEDInfo.notify_led_event,
+				currdriverstate != Adapter->DriverState ||
+					kthread_should_stop(),
+				msecs_to_jiffies(timeout));
+
+		if (kthread_should_stop()) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+				DBG_LVL_ALL,
+				"Led thread got signal to exit..hence exiting");
+			Adapter->LEDInfo.led_thread_running =
+					BCM_LED_THREAD_DISABLED;
 			TURN_OFF_LED(GPIO_Num, uiLedIndex);
-			Status=EVENT_SIGNALED;
+			Status = EVENT_SIGNALED;
 			break;
 		}
-		if(Status)
-		{
+		if (Status) {
 			TURN_OFF_LED(GPIO_Num, uiLedIndex);
-			Status=EVENT_SIGNALED;
+			Status = EVENT_SIGNALED;
 			break;
 		}
 
 		TURN_OFF_LED(GPIO_Num, uiLedIndex);
-		Status = wait_event_interruptible_timeout(Adapter->LEDInfo.notify_led_event,
-					currdriverstate!= Adapter->DriverState || kthread_should_stop(),
-					msecs_to_jiffies(timeout));
-		if(bInfinite == FALSE)
+		Status = wait_event_interruptible_timeout(
+				Adapter->LEDInfo.notify_led_event,
+				currdriverstate != Adapter->DriverState ||
+					kthread_should_stop(),
+				msecs_to_jiffies(timeout));
+		if (bInfinite == FALSE)
 			num_of_time--;
 	}
 	return Status;
@@ -71,19 +75,19 @@ static INT LED_Blink(PMINI_ADAPTER Adapter, UINT GPIO_Num, UCHAR uiLedIndex, ULO
 
 static INT ScaleRateofTransfer(ULONG rate)
 {
-	if(rate <= 3)
+	if (rate <= 3)
 		return rate;
-	else if((rate > 3) && (rate <= 100))
+	else if ((rate > 3) && (rate <= 100))
 		return 5;
-	else if((rate > 100) && (rate <= 200))
+	else if ((rate > 100) && (rate <= 200))
 		return 6;
-	else if((rate > 200) && (rate <= 300))
+	else if ((rate > 200) && (rate <= 300))
 		return 7;
-	else if((rate > 300) && (rate <= 400))
+	else if ((rate > 300) && (rate <= 400))
 		return 8;
-	else if((rate > 400) && (rate <= 500))
+	else if ((rate > 400) && (rate <= 500))
 		return 9;
-	else if((rate > 500) && (rate <= 600))
+	else if ((rate > 500) && (rate <= 600))
 		return 10;
 	else
 		return MAX_NUM_OF_BLINKS;
@@ -92,215 +96,232 @@ static INT ScaleRateofTransfer(ULONG rate)
 
 
 static INT LED_Proportional_Blink(PMINI_ADAPTER Adapter, UCHAR GPIO_Num_tx,
-		UCHAR uiTxLedIndex, UCHAR GPIO_Num_rx, UCHAR uiRxLedIndex, LedEventInfo_t currdriverstate)
+		UCHAR uiTxLedIndex, UCHAR GPIO_Num_rx, UCHAR uiRxLedIndex,
+		LedEventInfo_t currdriverstate)
 {
-	/* Initial values of TX and RX packets*/
+	/* Initial values of TX and RX packets */
 	ULONG64 Initial_num_of_packts_tx = 0, Initial_num_of_packts_rx = 0;
-	/*values of TX and RX packets after 1 sec*/
+	/* values of TX and RX packets after 1 sec */
 	ULONG64 Final_num_of_packts_tx = 0, Final_num_of_packts_rx = 0;
-	/*Rate of transfer of Tx and Rx in 1 sec*/
+	/* Rate of transfer of Tx and Rx in 1 sec */
 	ULONG64 rate_of_transfer_tx = 0, rate_of_transfer_rx = 0;
 	int Status = STATUS_SUCCESS;
 	INT num_of_time = 0, num_of_time_tx = 0, num_of_time_rx = 0;
 	UINT remDelay = 0;
 	BOOLEAN bBlinkBothLED = TRUE;
-	//UINT GPIO_num = DISABLE_GPIO_NUM;
+	/* UINT GPIO_num = DISABLE_GPIO_NUM; */
 	ulong timeout = 0;
 
-	/*Read initial value of packets sent/received */
+	/* Read initial value of packets sent/received */
 	Initial_num_of_packts_tx = Adapter->dev->stats.tx_packets;
 	Initial_num_of_packts_rx = Adapter->dev->stats.rx_packets;
 
-	/*Scale the rate of transfer to no of blinks.*/
-	num_of_time_tx= ScaleRateofTransfer((ULONG)rate_of_transfer_tx);
-	num_of_time_rx= ScaleRateofTransfer((ULONG)rate_of_transfer_rx);
+	/* Scale the rate of transfer to no of blinks. */
+	num_of_time_tx = ScaleRateofTransfer((ULONG)rate_of_transfer_tx);
+	num_of_time_rx = ScaleRateofTransfer((ULONG)rate_of_transfer_rx);
 
-	while((Adapter->device_removed == FALSE))
-	{
+	while ((Adapter->device_removed == FALSE)) {
 		timeout = 50;
-		/*Blink Tx and Rx LED when both Tx and Rx is in normal bandwidth*/
-		if(bBlinkBothLED)
-		{
-			/*Assign minimum number of blinks of either Tx or Rx.*/
-			if(num_of_time_tx > num_of_time_rx)
+		/*
+		 * Blink Tx and Rx LED when both Tx and Rx is
+		 * in normal bandwidth
+		 */
+		if (bBlinkBothLED) {
+			/*
+			 * Assign minimum number of blinks of
+			 * either Tx or Rx.
+			 */
+			if (num_of_time_tx > num_of_time_rx)
 				num_of_time = num_of_time_rx;
 			else
 				num_of_time = num_of_time_tx;
-			if(num_of_time > 0)
-			{
-				/*Blink both Tx and Rx LEDs*/
-				if(LED_Blink(Adapter, 1<<GPIO_Num_tx, uiTxLedIndex, timeout, num_of_time,currdriverstate)
+			if (num_of_time > 0) {
+				/* Blink both Tx and Rx LEDs */
+				if (LED_Blink(Adapter, 1 << GPIO_Num_tx,
+						uiTxLedIndex, timeout,
+						num_of_time, currdriverstate)
 							== EVENT_SIGNALED)
-				{
 					return EVENT_SIGNALED;
-				}
-				if(LED_Blink(Adapter, 1<<GPIO_Num_rx, uiRxLedIndex, timeout, num_of_time,currdriverstate)
+
+				if (LED_Blink(Adapter, 1 << GPIO_Num_rx,
+						uiRxLedIndex, timeout,
+						num_of_time, currdriverstate)
 							== EVENT_SIGNALED)
-				{
 					return EVENT_SIGNALED;
-				}
 
 			}
 
-			if(num_of_time == num_of_time_tx)
-			{
-				/*Blink pending rate of Rx*/
-				if(LED_Blink(Adapter, (1 << GPIO_Num_rx), uiRxLedIndex, timeout,
-						num_of_time_rx-num_of_time,currdriverstate) == EVENT_SIGNALED)
-				{
+			if (num_of_time == num_of_time_tx) {
+				/* Blink pending rate of Rx */
+				if (LED_Blink(Adapter, (1 << GPIO_Num_rx),
+						uiRxLedIndex, timeout,
+						num_of_time_rx-num_of_time,
+						currdriverstate)
+							== EVENT_SIGNALED)
 					return EVENT_SIGNALED;
-				}
+
 				num_of_time = num_of_time_rx;
-			}
-			else
-			{
-				/*Blink pending rate of Tx*/
-				if(LED_Blink(Adapter, 1<<GPIO_Num_tx, uiTxLedIndex, timeout,
-					num_of_time_tx-num_of_time,currdriverstate) == EVENT_SIGNALED)
-				{
+			} else {
+				/* Blink pending rate of Tx */
+				if (LED_Blink(Adapter, 1 << GPIO_Num_tx,
+						uiTxLedIndex, timeout,
+						num_of_time_tx-num_of_time,
+						currdriverstate)
+							== EVENT_SIGNALED)
 					return EVENT_SIGNALED;
-				}
+
 				num_of_time = num_of_time_tx;
 			}
-		}
-		else
-		{
-			if(num_of_time == num_of_time_tx)
-			{
-				/*Blink pending rate of Rx*/
-				if(LED_Blink(Adapter, 1<<GPIO_Num_tx, uiTxLedIndex, timeout, num_of_time,currdriverstate)
+		} else {
+			if (num_of_time == num_of_time_tx) {
+				/* Blink pending rate of Rx */
+				if (LED_Blink(Adapter, 1 << GPIO_Num_tx,
+						uiTxLedIndex, timeout,
+						num_of_time, currdriverstate)
 							== EVENT_SIGNALED)
-				{
 					return EVENT_SIGNALED;
-				}
-			}
-			else
-			{
-				/*Blink pending rate of Tx*/
-				if(LED_Blink(Adapter, 1<<GPIO_Num_rx, uiRxLedIndex, timeout,
-						num_of_time,currdriverstate) == EVENT_SIGNALED)
-				{
+			} else {
+				/* Blink pending rate of Tx */
+				if (LED_Blink(Adapter, 1 << GPIO_Num_rx,
+						uiRxLedIndex, timeout,
+						num_of_time, currdriverstate)
+							== EVENT_SIGNALED)
 					return EVENT_SIGNALED;
-				}
 			}
 		}
-		/* If Tx/Rx rate is less than maximum blinks per second,
-			 * wait till delay completes to 1 second
-			 */
+
+		/*
+		 * If Tx/Rx rate is less than maximum blinks per second,
+		 * wait till delay completes to 1 second
+		 */
 		remDelay = MAX_NUM_OF_BLINKS - num_of_time;
-		if(remDelay > 0)
-		{
-			timeout= 100 * remDelay;
-			Status = wait_event_interruptible_timeout(Adapter->LEDInfo.notify_led_event,
-						currdriverstate!= Adapter->DriverState ||kthread_should_stop() ,
-						msecs_to_jiffies (timeout));
-
-			if(kthread_should_stop())
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL, "Led thread got signal to exit..hence exiting");
-				Adapter->LEDInfo.led_thread_running= BCM_LED_THREAD_DISABLED;
+		if (remDelay > 0) {
+			timeout = 100 * remDelay;
+			Status = wait_event_interruptible_timeout(
+					Adapter->LEDInfo.notify_led_event,
+					currdriverstate != Adapter->DriverState
+						|| kthread_should_stop(),
+					msecs_to_jiffies(timeout));
+
+			if (kthread_should_stop()) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+					LED_DUMP_INFO, DBG_LVL_ALL,
+					"Led thread got signal to exit..hence exiting");
+				Adapter->LEDInfo.led_thread_running =
+						BCM_LED_THREAD_DISABLED;
 				return EVENT_SIGNALED;
 			}
-			if(Status)
+			if (Status)
 				return EVENT_SIGNALED;
 		}
 
-		/*Turn off both Tx and Rx LEDs before next second*/
-		TURN_OFF_LED(1<<GPIO_Num_tx, uiTxLedIndex);
-		TURN_OFF_LED(1<<GPIO_Num_rx, uiTxLedIndex);
+		/* Turn off both Tx and Rx LEDs before next second */
+		TURN_OFF_LED(1 << GPIO_Num_tx, uiTxLedIndex);
+		TURN_OFF_LED(1 << GPIO_Num_rx, uiTxLedIndex);
 
 		/*
- 		 * Read the Tx & Rx packets transmission after 1 second and
- 		 * calculate rate of transfer
- 		 */
+		 * Read the Tx & Rx packets transmission after 1 second and
+		 * calculate rate of transfer
+		 */
 		Final_num_of_packts_tx = Adapter->dev->stats.tx_packets;
 		Final_num_of_packts_rx = Adapter->dev->stats.rx_packets;
 
-		rate_of_transfer_tx = Final_num_of_packts_tx - Initial_num_of_packts_tx;
-		rate_of_transfer_rx = Final_num_of_packts_rx - Initial_num_of_packts_rx;
+		rate_of_transfer_tx = Final_num_of_packts_tx -
+						Initial_num_of_packts_tx;
+		rate_of_transfer_rx = Final_num_of_packts_rx -
+						Initial_num_of_packts_rx;
 
-		/*Read initial value of packets sent/received */
+		/* Read initial value of packets sent/received */
 		Initial_num_of_packts_tx = Final_num_of_packts_tx;
-		Initial_num_of_packts_rx = Final_num_of_packts_rx ;
+		Initial_num_of_packts_rx = Final_num_of_packts_rx;
 
-		/*Scale the rate of transfer to no of blinks.*/
-		num_of_time_tx= ScaleRateofTransfer((ULONG)rate_of_transfer_tx);
-		num_of_time_rx= ScaleRateofTransfer((ULONG)rate_of_transfer_rx);
+		/* Scale the rate of transfer to no of blinks. */
+		num_of_time_tx =
+			ScaleRateofTransfer((ULONG)rate_of_transfer_tx);
+		num_of_time_rx =
+			ScaleRateofTransfer((ULONG)rate_of_transfer_rx);
 
 	}
 	return Status;
 }
 
-
-//-----------------------------------------------------------------------------
-// Procedure:   ValidateDSDParamsChecksum
-//
-// Description: Reads DSD Params and validates checkusm.
-//
-// Arguments:
-//      Adapter - Pointer to Adapter structure.
-//      ulParamOffset - Start offset of the DSD parameter to be read and validated.
-//      usParamLen - Length of the DSD Parameter.
-//
-// Returns:
-//  <OSAL_STATUS_CODE>
-//-----------------------------------------------------------------------------
-
-static INT ValidateDSDParamsChecksum(
-													PMINI_ADAPTER Adapter,
-													ULONG  ulParamOffset,
-													USHORT usParamLen )
+/*
+ * -----------------------------------------------------------------------------
+ * Procedure:   ValidateDSDParamsChecksum
+ *
+ * Description: Reads DSD Params and validates checkusm.
+ *
+ * Arguments:
+ *      Adapter - Pointer to Adapter structure.
+ *      ulParamOffset - Start offset of the DSD parameter to be read and
+ *			validated.
+ *      usParamLen - Length of the DSD Parameter.
+ *
+ * Returns:
+ *  <OSAL_STATUS_CODE>
+ * -----------------------------------------------------------------------------
+ */
+static INT ValidateDSDParamsChecksum(PMINI_ADAPTER Adapter, ULONG ulParamOffset,
+					USHORT usParamLen)
 {
 	INT Status = STATUS_SUCCESS;
-	PUCHAR puBuffer 		    = NULL;
-	USHORT usChksmOrg		    = 0;
+	PUCHAR puBuffer = NULL;
+	USHORT usChksmOrg = 0;
 	USHORT usChecksumCalculated = 0;
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread:ValidateDSDParamsChecksum: 0x%lx 0x%X",ulParamOffset, usParamLen);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
+		"LED Thread:ValidateDSDParamsChecksum: 0x%lx 0x%X",
+		ulParamOffset, usParamLen);
 
 	puBuffer = kmalloc(usParamLen, GFP_KERNEL);
-	if(!puBuffer)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: ValidateDSDParamsChecksum Allocation failed");
+	if (!puBuffer) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+			DBG_LVL_ALL,
+			"LED Thread: ValidateDSDParamsChecksum Allocation failed");
 		return -ENOMEM;
 
 	}
 
-    //
-    //	Read the DSD data from the parameter offset.
-    //
-	if(STATUS_SUCCESS != BeceemNVMRead(Adapter,(PUINT)puBuffer,ulParamOffset,usParamLen))
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: ValidateDSDParamsChecksum BeceemNVMRead failed");
-		Status=STATUS_IMAGE_CHECKSUM_MISMATCH;
+	/* Read the DSD data from the parameter offset. */
+	if (STATUS_SUCCESS != BeceemNVMRead(Adapter, (PUINT)puBuffer,
+			ulParamOffset, usParamLen)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+			DBG_LVL_ALL,
+			"LED Thread: ValidateDSDParamsChecksum BeceemNVMRead failed");
+		Status = STATUS_IMAGE_CHECKSUM_MISMATCH;
 		goto exit;
 	}
 
-	//
-	//	Calculate the checksum of the data read from the DSD parameter.
-	//
-	usChecksumCalculated = CFG_CalculateChecksum(puBuffer,usParamLen);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: usCheckSumCalculated = 0x%x\n", usChecksumCalculated);
-
-	//
-	//	End of the DSD parameter will have a TWO bytes checksum stored in it. Read it and compare with the calculated
-	//	Checksum.
-	//
-	if(STATUS_SUCCESS != BeceemNVMRead(Adapter,(PUINT)&usChksmOrg,ulParamOffset+usParamLen,2))
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: ValidateDSDParamsChecksum BeceemNVMRead failed");
-		Status=STATUS_IMAGE_CHECKSUM_MISMATCH;
+	/* Calculate the checksum of the data read from the DSD parameter. */
+	usChecksumCalculated = CFG_CalculateChecksum(puBuffer, usParamLen);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
+		"LED Thread: usCheckSumCalculated = 0x%x\n",
+		usChecksumCalculated);
+
+	/*
+	 * End of the DSD parameter will have a TWO bytes checksum stored in it.
+	 * Read it and compare with the calculated Checksum.
+	 */
+	if (STATUS_SUCCESS != BeceemNVMRead(Adapter, (PUINT)&usChksmOrg,
+			ulParamOffset+usParamLen, 2)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+			DBG_LVL_ALL,
+			"LED Thread: ValidateDSDParamsChecksum BeceemNVMRead failed");
+		Status = STATUS_IMAGE_CHECKSUM_MISMATCH;
 		goto exit;
 	}
 	usChksmOrg = ntohs(usChksmOrg);
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: usChksmOrg = 0x%x", usChksmOrg);
-
-	//
-	//  	Compare the checksum calculated with the checksum read from DSD section
-	//
-	if(usChecksumCalculated ^ usChksmOrg)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: ValidateDSDParamsChecksum: Checksums don't match");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
+		"LED Thread: usChksmOrg = 0x%x", usChksmOrg);
+
+	/*
+	 * Compare the checksum calculated with the checksum read
+	 * from DSD section
+	 */
+	if (usChecksumCalculated ^ usChksmOrg) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+			DBG_LVL_ALL,
+			"LED Thread: ValidateDSDParamsChecksum: Checksums don't match");
 		Status = STATUS_IMAGE_CHECKSUM_MISMATCH;
 		goto exit;
 	}
@@ -311,523 +332,526 @@ exit:
 }
 
 
-//-----------------------------------------------------------------------------
-// Procedure:   ValidateHWParmStructure
-//
-// Description: Validates HW Parameters.
-//
-// Arguments:
-//      Adapter - Pointer to Adapter structure.
-//      ulHwParamOffset - Start offset of the HW parameter Section to be read and validated.
-//
-// Returns:
-//  <OSAL_STATUS_CODE>
-//-----------------------------------------------------------------------------
-
+/*
+ * -----------------------------------------------------------------------------
+ * Procedure:   ValidateHWParmStructure
+ *
+ * Description: Validates HW Parameters.
+ *
+ * Arguments:
+ *      Adapter - Pointer to Adapter structure.
+ *      ulHwParamOffset - Start offset of the HW parameter Section to be read
+ *				and validated.
+ *
+ * Returns:
+ *  <OSAL_STATUS_CODE>
+ * -----------------------------------------------------------------------------
+ */
 static INT ValidateHWParmStructure(PMINI_ADAPTER Adapter, ULONG ulHwParamOffset)
 {
 
-	INT Status = STATUS_SUCCESS ;
+	INT Status = STATUS_SUCCESS;
 	USHORT HwParamLen = 0;
-	// Add DSD start offset to the hwParamOffset to get the actual address.
+	/*
+	 * Add DSD start offset to the hwParamOffset to get
+	 * the actual address.
+	 */
 	ulHwParamOffset += DSD_START_OFFSET;
 
-	/*Read the Length of HW_PARAM structure*/
-	BeceemNVMRead(Adapter,(PUINT)&HwParamLen,ulHwParamOffset,2);
+	/* Read the Length of HW_PARAM structure */
+	BeceemNVMRead(Adapter, (PUINT)&HwParamLen, ulHwParamOffset, 2);
 	HwParamLen = ntohs(HwParamLen);
-	if(0==HwParamLen || HwParamLen > Adapter->uiNVMDSDSize)
-	{
+	if (0 == HwParamLen || HwParamLen > Adapter->uiNVMDSDSize)
 		return STATUS_IMAGE_CHECKSUM_MISMATCH;
-	}
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL, "LED Thread:HwParamLen = 0x%x", HwParamLen);
-	Status =ValidateDSDParamsChecksum(Adapter,ulHwParamOffset,HwParamLen);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
+		"LED Thread:HwParamLen = 0x%x", HwParamLen);
+	Status = ValidateDSDParamsChecksum(Adapter, ulHwParamOffset,
+						HwParamLen);
 	return Status;
 } /* ValidateHWParmStructure() */
 
-static int ReadLEDInformationFromEEPROM(PMINI_ADAPTER Adapter, UCHAR GPIO_Array[])
+static int ReadLEDInformationFromEEPROM(PMINI_ADAPTER Adapter,
+					UCHAR GPIO_Array[])
 {
 	int Status = STATUS_SUCCESS;
 
-	ULONG  dwReadValue 		= 0;
-	USHORT usHwParamData 	= 0;
-	USHORT usEEPROMVersion  = 0;
-	UCHAR  ucIndex 			= 0;
-	UCHAR  ucGPIOInfo[32] 	= {0};
+	ULONG  dwReadValue	= 0;
+	USHORT usHwParamData	= 0;
+	USHORT usEEPROMVersion	= 0;
+	UCHAR  ucIndex		= 0;
+	UCHAR  ucGPIOInfo[32]	= {0};
 
-	BeceemNVMRead(Adapter,(PUINT)&usEEPROMVersion,EEPROM_VERSION_OFFSET,2);
+	BeceemNVMRead(Adapter, (PUINT)&usEEPROMVersion,
+			EEPROM_VERSION_OFFSET, 2);
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"usEEPROMVersion: Minor:0x%X Major:0x%x",usEEPROMVersion&0xFF, ((usEEPROMVersion>>8)&0xFF));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
+		"usEEPROMVersion: Minor:0x%X Major:0x%x",
+		usEEPROMVersion&0xFF, ((usEEPROMVersion>>8)&0xFF));
 
 
-	if(((usEEPROMVersion>>8)&0xFF) < EEPROM_MAP5_MAJORVERSION)
-	{
-		BeceemNVMRead(Adapter,(PUINT)&usHwParamData,EEPROM_HW_PARAM_POINTER_ADDRESS,2);
+	if (((usEEPROMVersion>>8)&0xFF) < EEPROM_MAP5_MAJORVERSION) {
+		BeceemNVMRead(Adapter, (PUINT)&usHwParamData,
+			EEPROM_HW_PARAM_POINTER_ADDRESS, 2);
 		usHwParamData = ntohs(usHwParamData);
 		dwReadValue   = usHwParamData;
-	}
-	else
-	{
-		//
-		// Validate Compatibility section and then read HW param if compatibility section is valid.
-		//
+	} else {
+		/*
+		 * Validate Compatibility section and then read HW param
+		 * if compatibility section is valid.
+		 */
 		Status = ValidateDSDParamsChecksum(Adapter,
-			                   DSD_START_OFFSET,
-			                   COMPATIBILITY_SECTION_LENGTH_MAP5);
+				DSD_START_OFFSET,
+				COMPATIBILITY_SECTION_LENGTH_MAP5);
 
-		if(Status != STATUS_SUCCESS)
-		{
+		if (Status != STATUS_SUCCESS)
 			return Status;
-		}
-		BeceemNVMRead(Adapter,(PUINT)&dwReadValue,EEPROM_HW_PARAM_POINTER_ADDRRES_MAP5,4);
+
+		BeceemNVMRead(Adapter, (PUINT)&dwReadValue,
+			EEPROM_HW_PARAM_POINTER_ADDRRES_MAP5, 4);
 		dwReadValue = ntohl(dwReadValue);
 	}
 
 
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: Start address of HW_PARAM structure = 0x%lx",dwReadValue);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
+		"LED Thread: Start address of HW_PARAM structure = 0x%lx",
+		dwReadValue);
 
-	//
-	// Validate if the address read out is within the DSD.
-	// Adapter->uiNVMDSDSize gives whole DSD size inclusive of Autoinit.
-	// lower limit should be above DSD_START_OFFSET and
-	// upper limit should be below (Adapter->uiNVMDSDSize-DSD_START_OFFSET)
-	//
-	if(dwReadValue < DSD_START_OFFSET ||
-	   dwReadValue > (Adapter->uiNVMDSDSize-DSD_START_OFFSET))
-	{
+	/*
+	 * Validate if the address read out is within the DSD.
+	 * Adapter->uiNVMDSDSize gives whole DSD size inclusive of Autoinit.
+	 * lower limit should be above DSD_START_OFFSET and
+	 * upper limit should be below (Adapter->uiNVMDSDSize-DSD_START_OFFSET)
+	 */
+	if (dwReadValue < DSD_START_OFFSET ||
+			dwReadValue > (Adapter->uiNVMDSDSize-DSD_START_OFFSET))
 		return STATUS_IMAGE_CHECKSUM_MISMATCH;
-	}
 
 	Status = ValidateHWParmStructure(Adapter, dwReadValue);
-	if(Status){
+	if (Status)
 		return Status;
-	}
 
 	/*
-	  Add DSD_START_OFFSET to the offset read from the EEPROM.
-	  This will give the actual start HW Parameters start address.
-	  To read GPIO section, add GPIO offset further.
-	*/
-
-	dwReadValue += DSD_START_OFFSET; // = start address of hw param section.
-	dwReadValue += GPIO_SECTION_START_OFFSET; // = GPIO start offset within HW Param section.
-
-	/* Read the GPIO values for 32 GPIOs from EEPROM and map the function
- 	 * number to GPIO pin number to GPIO_Array
- 	 */
-	BeceemNVMRead(Adapter, (UINT *)ucGPIOInfo,dwReadValue,32);
-	for(ucIndex = 0; ucIndex < 32; ucIndex++)
-	 {
-
-		 switch(ucGPIOInfo[ucIndex])
-			{
-				case RED_LED:
-				{
-				 	GPIO_Array[RED_LED] = ucIndex;
-				 	Adapter->gpioBitMap |= (1<<ucIndex);
-					break;
-				}
-				case BLUE_LED:
-				{
-					GPIO_Array[BLUE_LED] = ucIndex;
-					Adapter->gpioBitMap |= (1<<ucIndex);
-					break;
-				}
-				case YELLOW_LED:
-				{
-					 GPIO_Array[YELLOW_LED] = ucIndex;
-					 Adapter->gpioBitMap |= (1<<ucIndex);
-					 break;
-				}
-				case GREEN_LED:
-				{
-					GPIO_Array[GREEN_LED] = ucIndex;
-				 	Adapter->gpioBitMap |= (1<<ucIndex);
-					break;
-				}
-				default:
-					break;
-			}
+	 * Add DSD_START_OFFSET to the offset read from the EEPROM.
+	 * This will give the actual start HW Parameters start address.
+	 * To read GPIO section, add GPIO offset further.
+	 */
+
+	dwReadValue +=
+		DSD_START_OFFSET; /* = start address of hw param section. */
+	dwReadValue += GPIO_SECTION_START_OFFSET;
+			/* = GPIO start offset within HW Param section. */
 
+	/*
+	 * Read the GPIO values for 32 GPIOs from EEPROM and map the function
+	 * number to GPIO pin number to GPIO_Array
+	 */
+	BeceemNVMRead(Adapter, (UINT *)ucGPIOInfo, dwReadValue, 32);
+	for (ucIndex = 0; ucIndex < 32; ucIndex++) {
+
+		switch (ucGPIOInfo[ucIndex]) {
+		case RED_LED:
+			GPIO_Array[RED_LED] = ucIndex;
+			Adapter->gpioBitMap |= (1 << ucIndex);
+			break;
+		case BLUE_LED:
+			GPIO_Array[BLUE_LED] = ucIndex;
+			Adapter->gpioBitMap |= (1 << ucIndex);
+			break;
+		case YELLOW_LED:
+			GPIO_Array[YELLOW_LED] = ucIndex;
+			Adapter->gpioBitMap |= (1 << ucIndex);
+			break;
+		case GREEN_LED:
+			GPIO_Array[GREEN_LED] = ucIndex;
+			Adapter->gpioBitMap |= (1 << ucIndex);
+			break;
+		default:
+			break;
 		}
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"GPIO's bit map correspond to LED :0x%X",Adapter->gpioBitMap);
-	 return Status;
+
+	}
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
+		"GPIO's bit map correspond to LED :0x%X", Adapter->gpioBitMap);
+	return Status;
 }
 
 
-static int ReadConfigFileStructure(PMINI_ADAPTER Adapter, BOOLEAN *bEnableThread)
+static int ReadConfigFileStructure(PMINI_ADAPTER Adapter,
+					BOOLEAN *bEnableThread)
 {
 	int Status = STATUS_SUCCESS;
-	UCHAR GPIO_Array[NUM_OF_LEDS+1]; /*Array to store GPIO numbers from EEPROM*/
+	/* Array to store GPIO numbers from EEPROM */
+	UCHAR GPIO_Array[NUM_OF_LEDS+1];
 	UINT uiIndex = 0;
 	UINT uiNum_of_LED_Type = 0;
 	PUCHAR puCFGData	= NULL;
 	UCHAR bData = 0;
 	memset(GPIO_Array, DISABLE_GPIO_NUM, NUM_OF_LEDS+1);
 
-	if(!Adapter->pstargetparams || IS_ERR(Adapter->pstargetparams))
-	{
-		BCM_DEBUG_PRINT (Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL, "Target Params not Avail.\n");
+	if (!Adapter->pstargetparams || IS_ERR(Adapter->pstargetparams)) {
+		BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+			DBG_LVL_ALL, "Target Params not Avail.\n");
 		return -ENOENT;
 	}
 
-	/*Populate GPIO_Array with GPIO numbers for LED functions*/
-	/*Read the GPIO numbers from EEPROM*/
+	/* Populate GPIO_Array with GPIO numbers for LED functions */
+	/* Read the GPIO numbers from EEPROM */
 	Status = ReadLEDInformationFromEEPROM(Adapter, GPIO_Array);
-	if(Status == STATUS_IMAGE_CHECKSUM_MISMATCH)
-	{
+	if (Status == STATUS_IMAGE_CHECKSUM_MISMATCH) {
 		*bEnableThread = FALSE;
 		return STATUS_SUCCESS;
-	}
-	else if(Status)
-	{
+	} else if (Status) {
 		*bEnableThread = FALSE;
 		return Status;
 	}
-  /*
-     * CONFIG file read successfully. Deallocate the memory of
-     * uiFileNameBufferSize
-     */
-	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: Config file read successfully\n");
+
+	/*
+	 * CONFIG file read successfully. Deallocate the memory of
+	 * uiFileNameBufferSize
+	 */
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,
+		"LED Thread: Config file read successfully\n");
 	puCFGData = (PUCHAR) &Adapter->pstargetparams->HostDrvrConfig1;
 
 	/*
- 	 * Offset for HostDrvConfig1, HostDrvConfig2, HostDrvConfig3 which
- 	 * will have the information of LED type, LED on state for different
- 	 * driver state and LED blink state.
- 	 */
+	 * Offset for HostDrvConfig1, HostDrvConfig2, HostDrvConfig3 which
+	 * will have the information of LED type, LED on state for different
+	 * driver state and LED blink state.
+	 */
 
-	for(uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++)
-	{
+	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
 		bData = *puCFGData;
 
-		/*Check Bit 8 for polarity. If it is set, polarity is reverse polarity*/
-		if(bData & 0x80)
-		{
+		/*
+		 * Check Bit 8 for polarity. If it is set,
+		 * polarity is reverse polarity
+		 */
+		if (bData & 0x80) {
 			Adapter->LEDInfo.LEDState[uiIndex].BitPolarity = 0;
-			/*unset the bit 8*/
+			/* unset the bit 8 */
 			bData = bData & 0x7f;
 		}
 
 		Adapter->LEDInfo.LEDState[uiIndex].LED_Type = bData;
-		if(bData <= NUM_OF_LEDS)
-			Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num = GPIO_Array[bData];
+		if (bData <= NUM_OF_LEDS)
+			Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num =
+							GPIO_Array[bData];
 		else
-			Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num = DISABLE_GPIO_NUM;
+			Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num =
+							DISABLE_GPIO_NUM;
 
 		puCFGData++;
 		bData = *puCFGData;
 		Adapter->LEDInfo.LEDState[uiIndex].LED_On_State = bData;
 		puCFGData++;
 		bData = *puCFGData;
-		Adapter->LEDInfo.LEDState[uiIndex].LED_Blink_State= bData;
+		Adapter->LEDInfo.LEDState[uiIndex].LED_Blink_State = bData;
 		puCFGData++;
 	}
 
-	/*Check if all the LED settings are disabled. If it is disabled, dont launch the LED control thread.*/
-	for(uiIndex = 0; uiIndex<NUM_OF_LEDS; uiIndex++)
-	{
-		if((Adapter->LEDInfo.LEDState[uiIndex].LED_Type == DISABLE_GPIO_NUM) ||
-	 		(Adapter->LEDInfo.LEDState[uiIndex].LED_Type == 0x7f) ||
+	/*
+	 * Check if all the LED settings are disabled. If it is disabled,
+	 * dont launch the LED control thread.
+	 */
+	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
+		if ((Adapter->LEDInfo.LEDState[uiIndex].LED_Type == DISABLE_GPIO_NUM) ||
+			(Adapter->LEDInfo.LEDState[uiIndex].LED_Type == 0x7f) ||
 			(Adapter->LEDInfo.LEDState[uiIndex].LED_Type == 0))
 			uiNum_of_LED_Type++;
 	}
-	if(uiNum_of_LED_Type >= NUM_OF_LEDS)
+	if (uiNum_of_LED_Type >= NUM_OF_LEDS)
 		*bEnableThread = FALSE;
 
 	return Status;
 }
-//--------------------------------------------------------------------------
-// Procedure:   LedGpioInit
-//
-// Description: Initializes LED GPIOs. Makes the LED GPIOs to OUTPUT mode and make the
-//			  initial state to be OFF.
-//
-// Arguments:
-//      Adapter - Pointer to MINI_ADAPTER structure.
-//
-// Returns: VOID
-//
-//-----------------------------------------------------------------------------
 
+/*
+ * -----------------------------------------------------------------------------
+ * Procedure:   LedGpioInit
+ *
+ * Description: Initializes LED GPIOs. Makes the LED GPIOs to OUTPUT mode
+ *			  and make the initial state to be OFF.
+ *
+ * Arguments:
+ *      Adapter - Pointer to MINI_ADAPTER structure.
+ *
+ * Returns: VOID
+ *
+ * -----------------------------------------------------------------------------
+ */
 static VOID LedGpioInit(PMINI_ADAPTER Adapter)
 {
 	UINT uiResetValue = 0;
 	UINT uiIndex      = 0;
 
 	/* Set all LED GPIO Mode to output mode */
-	if(rdmalt(Adapter, GPIO_MODE_REGISTER, &uiResetValue, sizeof(uiResetValue)) <0)
-		BCM_DEBUG_PRINT (Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: RDM Failed\n");
-	for(uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++)
-	{
-		if(Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num != DISABLE_GPIO_NUM)
+	if (rdmalt(Adapter, GPIO_MODE_REGISTER, &uiResetValue,
+			sizeof(uiResetValue)) < 0)
+		BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+			DBG_LVL_ALL, "LED Thread: RDM Failed\n");
+	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
+		if (Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num !=
+				DISABLE_GPIO_NUM)
 			uiResetValue |= (1 << Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num);
-		TURN_OFF_LED(1<<Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num,uiIndex);
+		TURN_OFF_LED(1 << Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num,
+				uiIndex);
 	}
-	if(wrmalt(Adapter, GPIO_MODE_REGISTER, &uiResetValue, sizeof(uiResetValue)) < 0)
-		BCM_DEBUG_PRINT (Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: WRM Failed\n");
+	if (wrmalt(Adapter, GPIO_MODE_REGISTER, &uiResetValue,
+			sizeof(uiResetValue)) < 0)
+		BCM_DEBUG_PRINT (Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+			DBG_LVL_ALL, "LED Thread: WRM Failed\n");
 
-	Adapter->LEDInfo.bIdle_led_off =  FALSE;
+	Adapter->LEDInfo.bIdle_led_off = FALSE;
 }
-//-----------------------------------------------------------------------------
 
-static INT BcmGetGPIOPinInfo(PMINI_ADAPTER Adapter, UCHAR *GPIO_num_tx, UCHAR *GPIO_num_rx ,UCHAR *uiLedTxIndex, UCHAR *uiLedRxIndex,LedEventInfo_t currdriverstate)
+static INT BcmGetGPIOPinInfo(PMINI_ADAPTER Adapter, UCHAR *GPIO_num_tx,
+		UCHAR *GPIO_num_rx, UCHAR *uiLedTxIndex, UCHAR *uiLedRxIndex,
+		LedEventInfo_t currdriverstate)
 {
 	UINT uiIndex = 0;
 
 	*GPIO_num_tx = DISABLE_GPIO_NUM;
 	*GPIO_num_rx = DISABLE_GPIO_NUM;
 
-	for(uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++)
-	{
+	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
 
-		if((currdriverstate == NORMAL_OPERATION)||
-			(currdriverstate == IDLEMODE_EXIT)||
-			(currdriverstate == FW_DOWNLOAD))
-		{
-			if(Adapter->LEDInfo.LEDState[uiIndex].LED_Blink_State & currdriverstate)
-			{
-				if(Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num != DISABLE_GPIO_NUM)
-				{
-					if(*GPIO_num_tx == DISABLE_GPIO_NUM)
-					{
+		if ((currdriverstate == NORMAL_OPERATION) ||
+				(currdriverstate == IDLEMODE_EXIT) ||
+				(currdriverstate == FW_DOWNLOAD)) {
+			if (Adapter->LEDInfo.LEDState[uiIndex].LED_Blink_State &
+					currdriverstate) {
+				if (Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num
+						!= DISABLE_GPIO_NUM) {
+					if (*GPIO_num_tx == DISABLE_GPIO_NUM) {
 						*GPIO_num_tx = Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num;
 						*uiLedTxIndex = uiIndex;
-					}
-					else
-					{
+					} else {
 						*GPIO_num_rx = Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num;
 						*uiLedRxIndex = uiIndex;
 					}
 				}
 			}
-		}
-		else
-		{
-			if(Adapter->LEDInfo.LEDState[uiIndex].LED_On_State & currdriverstate)
-			{
-				if(Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num != DISABLE_GPIO_NUM)
-				{
+		} else {
+			if (Adapter->LEDInfo.LEDState[uiIndex].LED_On_State
+					& currdriverstate) {
+				if (Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num
+						!= DISABLE_GPIO_NUM) {
 					*GPIO_num_tx = Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num;
 					*uiLedTxIndex = uiIndex;
 				}
 			}
 		}
 	}
-	return STATUS_SUCCESS ;
+	return STATUS_SUCCESS;
 }
 static VOID LEDControlThread(PMINI_ADAPTER Adapter)
 {
 	UINT uiIndex = 0;
 	UCHAR GPIO_num = 0;
-	UCHAR uiLedIndex = 0 ;
+	UCHAR uiLedIndex = 0;
 	UINT uiResetValue = 0;
 	LedEventInfo_t currdriverstate = 0;
 	ulong timeout = 0;
 
 	INT Status = 0;
 
-	UCHAR  dummyGPIONum = 0;
-	UCHAR  dummyIndex = 0;
+	UCHAR dummyGPIONum = 0;
+	UCHAR dummyIndex = 0;
 
-	//currdriverstate = Adapter->DriverState;
+	/* currdriverstate = Adapter->DriverState; */
 	Adapter->LEDInfo.bIdleMode_tx_from_host = FALSE;
 
-	/*Wait till event is triggered*/
-	//wait_event(Adapter->LEDInfo.notify_led_event,
-			//	currdriverstate!= Adapter->DriverState);
+	/*
+	 * Wait till event is triggered
+	 *
+	 * wait_event(Adapter->LEDInfo.notify_led_event,
+	 *	currdriverstate!= Adapter->DriverState);
+	 */
 
-	GPIO_num = DISABLE_GPIO_NUM ;
+	GPIO_num = DISABLE_GPIO_NUM;
 
-	while(TRUE)
-	{
-		/*Wait till event is triggered*/
-		if( (GPIO_num == DISABLE_GPIO_NUM)
+	while (TRUE) {
+		/* Wait till event is triggered */
+		if ((GPIO_num == DISABLE_GPIO_NUM)
 						||
-			((currdriverstate != FW_DOWNLOAD) &&
-			 (currdriverstate != NORMAL_OPERATION) &&
-			 (currdriverstate != LOWPOWER_MODE_ENTER))
-			 			||
-			 (currdriverstate == LED_THREAD_INACTIVE)	)
-		{
-			Status = wait_event_interruptible(Adapter->LEDInfo.notify_led_event,
-				currdriverstate != Adapter->DriverState || kthread_should_stop());
-		}
-
-		if(kthread_should_stop() || Adapter->device_removed )
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL, "Led thread got signal to exit..hence exiting");
-			Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_DISABLED;
-			TURN_OFF_LED(1<<GPIO_num, uiLedIndex);
-			return ;//STATUS_FAILURE;
+				((currdriverstate != FW_DOWNLOAD) &&
+				 (currdriverstate != NORMAL_OPERATION) &&
+				 (currdriverstate != LOWPOWER_MODE_ENTER))
+						||
+				(currdriverstate == LED_THREAD_INACTIVE))
+			Status = wait_event_interruptible(
+					Adapter->LEDInfo.notify_led_event,
+					currdriverstate != Adapter->DriverState
+						|| kthread_should_stop());
+
+		if (kthread_should_stop() || Adapter->device_removed) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+				DBG_LVL_ALL,
+				"Led thread got signal to exit..hence exiting");
+			Adapter->LEDInfo.led_thread_running =
+						BCM_LED_THREAD_DISABLED;
+			TURN_OFF_LED(1 << GPIO_num, uiLedIndex);
+			return; /* STATUS_FAILURE; */
 		}
 
-		if(GPIO_num != DISABLE_GPIO_NUM)
-		{
-			TURN_OFF_LED(1<<GPIO_num, uiLedIndex);
-		}
+		if (GPIO_num != DISABLE_GPIO_NUM)
+			TURN_OFF_LED(1 << GPIO_num, uiLedIndex);
 
-		if(Adapter->LEDInfo.bLedInitDone == FALSE)
-		{
+		if (Adapter->LEDInfo.bLedInitDone == FALSE) {
 			LedGpioInit(Adapter);
 			Adapter->LEDInfo.bLedInitDone = TRUE;
 		}
 
-		switch(Adapter->DriverState)
-		{
-			case DRIVER_INIT:
-			{
-				currdriverstate = DRIVER_INIT;//Adapter->DriverState;
-				BcmGetGPIOPinInfo(Adapter, &GPIO_num, &dummyGPIONum, &uiLedIndex, &dummyIndex, currdriverstate);
+		switch (Adapter->DriverState) {
+		case DRIVER_INIT:
+			currdriverstate = DRIVER_INIT;
+					/* Adapter->DriverState; */
+			BcmGetGPIOPinInfo(Adapter, &GPIO_num, &dummyGPIONum,
+				&uiLedIndex, &dummyIndex, currdriverstate);
+
+			if (GPIO_num != DISABLE_GPIO_NUM)
+				TURN_ON_LED(1 << GPIO_num, uiLedIndex);
 
-				if(GPIO_num  != DISABLE_GPIO_NUM)
-				{
-					TURN_ON_LED(1<<GPIO_num, uiLedIndex);
-				}
-			}
 			break;
-			case FW_DOWNLOAD:
-			{
-				//BCM_DEBUG_PRINT (Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: FW_DN_DONE called\n");
-				currdriverstate = FW_DOWNLOAD;
-				BcmGetGPIOPinInfo(Adapter, &GPIO_num, &dummyGPIONum,  &uiLedIndex, &dummyIndex, currdriverstate);
-
-				if(GPIO_num != DISABLE_GPIO_NUM)
-				{
-					timeout = 50;
-					LED_Blink(Adapter, 1<<GPIO_num, uiLedIndex, timeout, -1,currdriverstate);
-				}
+		case FW_DOWNLOAD:
+			/*
+			 * BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+			 *	LED_DUMP_INFO, DBG_LVL_ALL,
+			 *	"LED Thread: FW_DN_DONE called\n");
+			 */
+			currdriverstate = FW_DOWNLOAD;
+			BcmGetGPIOPinInfo(Adapter, &GPIO_num, &dummyGPIONum,
+				&uiLedIndex, &dummyIndex, currdriverstate);
+
+			if (GPIO_num != DISABLE_GPIO_NUM) {
+				timeout = 50;
+				LED_Blink(Adapter, 1 << GPIO_num, uiLedIndex,
+					timeout, -1, currdriverstate);
 			}
 			break;
-			case FW_DOWNLOAD_DONE:
-			{
-				currdriverstate = FW_DOWNLOAD_DONE;
-				BcmGetGPIOPinInfo(Adapter, &GPIO_num, &dummyGPIONum, &uiLedIndex, &dummyIndex,currdriverstate);
-				if(GPIO_num != DISABLE_GPIO_NUM)
-				{
-					TURN_ON_LED(1<<GPIO_num, uiLedIndex);
-				}
-			}
+		case FW_DOWNLOAD_DONE:
+			currdriverstate = FW_DOWNLOAD_DONE;
+			BcmGetGPIOPinInfo(Adapter, &GPIO_num, &dummyGPIONum,
+				&uiLedIndex, &dummyIndex, currdriverstate);
+			if (GPIO_num != DISABLE_GPIO_NUM)
+				TURN_ON_LED(1 << GPIO_num, uiLedIndex);
 			break;
 
-			case SHUTDOWN_EXIT:
-			//no break, continue to NO_NETWORK_ENTRY state as well.
-
-			case NO_NETWORK_ENTRY:
-			{
-				currdriverstate = NO_NETWORK_ENTRY;
-				BcmGetGPIOPinInfo(Adapter, &GPIO_num, &dummyGPIONum, &uiLedIndex,&dummyGPIONum,currdriverstate);
-				if(GPIO_num != DISABLE_GPIO_NUM)
-				{
-					TURN_ON_LED(1<<GPIO_num, uiLedIndex);
-				}
-			}
+		case SHUTDOWN_EXIT:
+			/*
+			 * no break, continue to NO_NETWORK_ENTRY
+			 * state as well.
+			 */
+		case NO_NETWORK_ENTRY:
+			currdriverstate = NO_NETWORK_ENTRY;
+			BcmGetGPIOPinInfo(Adapter, &GPIO_num, &dummyGPIONum,
+				&uiLedIndex, &dummyGPIONum, currdriverstate);
+			if (GPIO_num != DISABLE_GPIO_NUM)
+				TURN_ON_LED(1 << GPIO_num, uiLedIndex);
 			break;
-			case NORMAL_OPERATION:
+		case NORMAL_OPERATION:
 			{
 				UCHAR GPIO_num_tx = DISABLE_GPIO_NUM;
 				UCHAR GPIO_num_rx = DISABLE_GPIO_NUM;
 				UCHAR uiLEDTx = 0;
 				UCHAR uiLEDRx = 0;
 				currdriverstate = NORMAL_OPERATION;
-				Adapter->LEDInfo.bIdle_led_off =  FALSE;
-
-				BcmGetGPIOPinInfo(Adapter, &GPIO_num_tx, &GPIO_num_rx, &uiLEDTx,&uiLEDRx,currdriverstate);
-				if((GPIO_num_tx == DISABLE_GPIO_NUM) && (GPIO_num_rx == DISABLE_GPIO_NUM))
-				{
-					GPIO_num = DISABLE_GPIO_NUM ;
-				}
-				else
-				{
-					/*If single LED is selected, use same for both Tx and Rx*/
-					if(GPIO_num_tx == DISABLE_GPIO_NUM)
-					{
+				Adapter->LEDInfo.bIdle_led_off = FALSE;
+
+				BcmGetGPIOPinInfo(Adapter, &GPIO_num_tx,
+					&GPIO_num_rx, &uiLEDTx, &uiLEDRx,
+					currdriverstate);
+				if ((GPIO_num_tx == DISABLE_GPIO_NUM) &&
+						(GPIO_num_rx ==
+						 DISABLE_GPIO_NUM)) {
+					GPIO_num = DISABLE_GPIO_NUM;
+				} else {
+					/*
+					 * If single LED is selected, use same
+					 * for both Tx and Rx
+					 */
+					if (GPIO_num_tx == DISABLE_GPIO_NUM) {
 						GPIO_num_tx = GPIO_num_rx;
 						uiLEDTx = uiLEDRx;
-					}
-					else if(GPIO_num_rx == DISABLE_GPIO_NUM)
-					{
+					} else if (GPIO_num_rx ==
+							DISABLE_GPIO_NUM) {
 						GPIO_num_rx = GPIO_num_tx;
 						uiLEDRx = uiLEDTx;
 					}
-				/*Blink the LED in proportionate to Tx and Rx transmissions.*/
-					LED_Proportional_Blink(Adapter, GPIO_num_tx, uiLEDTx, GPIO_num_rx, uiLEDRx,currdriverstate);
+					/*
+					 * Blink the LED in proportionate
+					 * to Tx and Rx transmissions.
+					 */
+					LED_Proportional_Blink(Adapter,
+						GPIO_num_tx, uiLEDTx,
+						GPIO_num_rx, uiLEDRx,
+						currdriverstate);
 				}
 			}
 			break;
-			case LOWPOWER_MODE_ENTER:
-			{
-				currdriverstate  = LOWPOWER_MODE_ENTER;
-				if( DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING == Adapter->ulPowerSaveMode)
-				{
-					/* Turn OFF all the LED */
-					uiResetValue = 0;
-					for(uiIndex =0; uiIndex < NUM_OF_LEDS; uiIndex++)
-					{
-						if(Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num != DISABLE_GPIO_NUM)
-						TURN_OFF_LED((1<<Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num),uiIndex);
-					}
-
+		case LOWPOWER_MODE_ENTER:
+			currdriverstate = LOWPOWER_MODE_ENTER;
+			if (DEVICE_POWERSAVE_MODE_AS_MANUAL_CLOCK_GATING ==
+					Adapter->ulPowerSaveMode) {
+				/* Turn OFF all the LED */
+				uiResetValue = 0;
+				for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
+					if (Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num != DISABLE_GPIO_NUM)
+						TURN_OFF_LED((1 << Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num), uiIndex);
 				}
-				/* Turn off LED And WAKE-UP for Sendinf IDLE mode ACK */
-				Adapter->LEDInfo.bLedInitDone = FALSE;
-				Adapter->LEDInfo.bIdle_led_off =  TRUE;
-				wake_up(&Adapter->LEDInfo.idleModeSyncEvent);
-				GPIO_num = DISABLE_GPIO_NUM;
-				break;
-			}
-			case IDLEMODE_CONTINUE:
-			{
-				currdriverstate = IDLEMODE_CONTINUE;
-				GPIO_num = DISABLE_GPIO_NUM;
+
 			}
+			/* Turn off LED And WAKE-UP for Sendinf IDLE mode ACK */
+			Adapter->LEDInfo.bLedInitDone = FALSE;
+			Adapter->LEDInfo.bIdle_led_off = TRUE;
+			wake_up(&Adapter->LEDInfo.idleModeSyncEvent);
+			GPIO_num = DISABLE_GPIO_NUM;
 			break;
-			case IDLEMODE_EXIT:
-			{
-			}
+		case IDLEMODE_CONTINUE:
+			currdriverstate = IDLEMODE_CONTINUE;
+			GPIO_num = DISABLE_GPIO_NUM;
 			break;
-			case DRIVER_HALT:
-			{
-				currdriverstate = DRIVER_HALT;
-				GPIO_num = DISABLE_GPIO_NUM;
-				for(uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++)
-				{
-					if(Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num !=
-						DISABLE_GPIO_NUM)
-						TURN_OFF_LED((1<<Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num),uiIndex);
-				}
-				//Adapter->DriverState = DRIVER_INIT;
+		case IDLEMODE_EXIT:
+			break;
+		case DRIVER_HALT:
+			currdriverstate = DRIVER_HALT;
+			GPIO_num = DISABLE_GPIO_NUM;
+			for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
+				if (Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num
+						!= DISABLE_GPIO_NUM)
+					TURN_OFF_LED((1 << Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num), uiIndex);
 			}
+			/* Adapter->DriverState = DRIVER_INIT; */
 			break;
-			case LED_THREAD_INACTIVE :
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"InActivating LED thread...");
-				currdriverstate = LED_THREAD_INACTIVE;
-				Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_RUNNING_INACTIVELY ;
-				Adapter->LEDInfo.bLedInitDone = FALSE ;
-				//disable ALL LED
-				for(uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++)
-				{
-					if(Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num !=
-						DISABLE_GPIO_NUM)
-						TURN_OFF_LED((1<<Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num),uiIndex);
-				}
+		case LED_THREAD_INACTIVE:
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+				DBG_LVL_ALL, "InActivating LED thread...");
+			currdriverstate = LED_THREAD_INACTIVE;
+			Adapter->LEDInfo.led_thread_running =
+					BCM_LED_THREAD_RUNNING_INACTIVELY;
+			Adapter->LEDInfo.bLedInitDone = FALSE;
+			/* disable ALL LED */
+			for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
+				if (Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num
+						!= DISABLE_GPIO_NUM)
+					TURN_OFF_LED((1 << Adapter->LEDInfo.LEDState[uiIndex].GPIO_Num), uiIndex);
 			}
 			break;
-			case LED_THREAD_ACTIVE :
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"Activating LED thread again...");
-				if(Adapter->LinkUpStatus == FALSE)
-					Adapter->DriverState = NO_NETWORK_ENTRY;
-				else
-					Adapter->DriverState = NORMAL_OPERATION;
+		case LED_THREAD_ACTIVE:
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+				DBG_LVL_ALL, "Activating LED thread again...");
+			if (Adapter->LinkUpStatus == FALSE)
+				Adapter->DriverState = NO_NETWORK_ENTRY;
+			else
+				Adapter->DriverState = NORMAL_OPERATION;
 
-				Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_RUNNING_ACTIVELY ;
-			}
+			Adapter->LEDInfo.led_thread_running =
+					BCM_LED_THREAD_RUNNING_ACTIVELY;
+			break;
+			/* return; */
+		default:
 			break;
-			//return;
-			default:
-				break;
 		}
 	}
 	Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_DISABLED;
@@ -839,49 +863,54 @@ int InitLedSettings(PMINI_ADAPTER Adapter)
 	BOOLEAN bEnableThread = TRUE;
 	UCHAR uiIndex = 0;
 
-	/*Initially set BitPolarity to normal polarity. The bit 8 of LED type
- * 	  is used to change the polarity of the LED.*/
+	/*
+	 * Initially set BitPolarity to normal polarity. The bit 8 of LED type
+	 * is used to change the polarity of the LED.
+	 */
 
-	for(uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++) {
+	for (uiIndex = 0; uiIndex < NUM_OF_LEDS; uiIndex++)
 		Adapter->LEDInfo.LEDState[uiIndex].BitPolarity = 1;
-	}
 
-	/*Read the LED settings of CONFIG file and map it to GPIO numbers in EEPROM*/
+	/*
+	 * Read the LED settings of CONFIG file and map it
+	 * to GPIO numbers in EEPROM
+	 */
 	Status = ReadConfigFileStructure(Adapter, &bEnableThread);
-	if(STATUS_SUCCESS != Status)
-	{
-		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL,"LED Thread: FAILED in ReadConfigFileStructure\n");
+	if (STATUS_SUCCESS != Status) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+			DBG_LVL_ALL,
+			"LED Thread: FAILED in ReadConfigFileStructure\n");
 		return Status;
 	}
 
-	if(Adapter->LEDInfo.led_thread_running)
-	{
-		if(bEnableThread)
+	if (Adapter->LEDInfo.led_thread_running) {
+		if (bEnableThread) {
 			;
-		else
-		{
+		} else {
 			Adapter->DriverState = DRIVER_HALT;
 			wake_up(&Adapter->LEDInfo.notify_led_event);
-			Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_DISABLED;
+			Adapter->LEDInfo.led_thread_running =
+						BCM_LED_THREAD_DISABLED;
 		}
 
-	}
-
-	else if(bEnableThread)
-	{
-		/*Create secondary thread to handle the LEDs*/
+	} else if (bEnableThread) {
+		/* Create secondary thread to handle the LEDs */
 		init_waitqueue_head(&Adapter->LEDInfo.notify_led_event);
 		init_waitqueue_head(&Adapter->LEDInfo.idleModeSyncEvent);
-		Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_RUNNING_ACTIVELY;
-		Adapter->LEDInfo.bIdle_led_off =  FALSE;
-		Adapter->LEDInfo.led_cntrl_threadid = kthread_run((int (*)(void *))
-            LEDControlThread, Adapter, "led_control_thread");
-		if(IS_ERR(Adapter->LEDInfo.led_cntrl_threadid))
-    	{
-        	BCM_DEBUG_PRINT(Adapter,DBG_TYPE_OTHERS, LED_DUMP_INFO, DBG_LVL_ALL, "Not able to spawn Kernel Thread\n");
-			Adapter->LEDInfo.led_thread_running = BCM_LED_THREAD_DISABLED;
-        	return PTR_ERR(Adapter->LEDInfo.led_cntrl_threadid);
-    	}
+		Adapter->LEDInfo.led_thread_running =
+					BCM_LED_THREAD_RUNNING_ACTIVELY;
+		Adapter->LEDInfo.bIdle_led_off = FALSE;
+		Adapter->LEDInfo.led_cntrl_threadid =
+			kthread_run((int (*)(void *)) LEDControlThread,
+			Adapter, "led_control_thread");
+		if (IS_ERR(Adapter->LEDInfo.led_cntrl_threadid)) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, LED_DUMP_INFO,
+				DBG_LVL_ALL,
+				"Not able to spawn Kernel Thread\n");
+			Adapter->LEDInfo.led_thread_running =
+				BCM_LED_THREAD_DISABLED;
+			return PTR_ERR(Adapter->LEDInfo.led_cntrl_threadid);
+		}
 	}
 	return Status;
 }
diff --git a/drivers/staging/bcm/nvm.c b/drivers/staging/bcm/nvm.c
index 3de0daf5edb2..7d703cb3c5e0 100644
--- a/drivers/staging/bcm/nvm.c
+++ b/drivers/staging/bcm/nvm.c
@@ -78,7 +78,7 @@ static UCHAR ReadEEPROMStatusRegister( PMINI_ADAPTER Adapter )
 	{
 		value=0;
 		uiStatus = 0 ;
-		rdmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG,&uiStatus, sizeof(uiStatus));
+		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
 		if(Adapter->device_removed == TRUE)
 		{
 			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Modem has got removed hence exiting....");
@@ -93,7 +93,7 @@ static UCHAR ReadEEPROMStatusRegister( PMINI_ADAPTER Adapter )
 			wrmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
 
 			value =0;
-			rdmalt(Adapter, EEPROM_READ_DATAQ_REG,&value, sizeof(value));
+			rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
 			uiData = (UCHAR)value;
 
 			break;
@@ -102,8 +102,8 @@ static UCHAR ReadEEPROMStatusRegister( PMINI_ADAPTER Adapter )
 		dwRetries-- ;
 		if ( dwRetries == 0 )
 		{
-			 rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG,&value, sizeof(value));
-			 rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG,&value1, sizeof(value1));
+			 rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
+			 rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG, &value1, sizeof(value1));
 			 BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"0x3004 = %x 0x3008 = %x, retries = %d failed.\n",value,value1,  MAX_EEPROM_RETRIES*RETRIES_PER_DELAY);
 			return uiData;
 		}
@@ -158,7 +158,7 @@ INT ReadBeceemEEPROMBulk( PMINI_ADAPTER Adapter,
 		{
 
 		uiStatus = 0;
-		rdmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
+		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
 		if(Adapter->device_removed == TRUE)
 		{
 			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Modem has got Removed.hence exiting from loop...");
@@ -202,8 +202,8 @@ INT ReadBeceemEEPROMBulk( PMINI_ADAPTER Adapter,
 		{
 			value=0;
 			value1=0;
-			rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG,&value, sizeof(value));
-			rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG,&value1, sizeof(value1));
+			rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &value, sizeof(value));
+			rdmalt(Adapter, EEPROM_SPI_Q_STATUS_REG, &value1, sizeof(value1));
 			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0, "dwNumWords %d 0x3004 = %x 0x3008 = %x  retries = %d failed.\n", dwNumWords, value,  value1,  MAX_EEPROM_RETRIES*RETRIES_PER_DELAY);
 			return STATUS_FAILURE;
 		}
@@ -217,22 +217,22 @@ INT ReadBeceemEEPROMBulk( PMINI_ADAPTER Adapter,
 		pvalue = (PUCHAR)(pdwData + dwIndex);
 
 		value =0;
-		rdmalt(Adapter, EEPROM_READ_DATAQ_REG,&value, sizeof(value));
+		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
 
 		pvalue[0] = value;
 
 		value = 0;
-		rdmalt(Adapter, EEPROM_READ_DATAQ_REG,&value, sizeof(value));
+		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
 
 		pvalue[1] = value;
 
 		value =0;
-		rdmalt(Adapter, EEPROM_READ_DATAQ_REG,&value, sizeof(value));
+		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
 
 		pvalue[2] = value;
 
 		value = 0;
-		rdmalt(Adapter, EEPROM_READ_DATAQ_REG,&value, sizeof(value));
+		rdmalt(Adapter, EEPROM_READ_DATAQ_REG, &value, sizeof(value));
 
 		pvalue[3] = value;
 	}
@@ -445,6 +445,7 @@ static INT BeceemFlashBulkRead(
 	UINT uiBytesToRead = uiNumBytes;
 	INT Status = 0;
 	UINT uiPartOffset = 0;
+	int bytes;
 
 	if(Adapter->device_removed )
 	{
@@ -469,9 +470,9 @@ static INT BeceemFlashBulkRead(
 		uiBytesToRead = MAX_RW_SIZE - (uiOffset%MAX_RW_SIZE);
 		uiBytesToRead = MIN(uiNumBytes,uiBytesToRead);
 
-		if(rdm(Adapter,uiPartOffset, (PCHAR)pBuffer+uiIndex,uiBytesToRead))
-		{
-			Status = -1;
+		bytes = rdm(Adapter, uiPartOffset, (PCHAR)pBuffer+uiIndex, uiBytesToRead);
+		if (bytes < 0) {
+			Status = bytes;
 			Adapter->SelectedChip = RESET_CHIP_SELECT;
 			return Status;
 		}
@@ -488,9 +489,9 @@ static INT BeceemFlashBulkRead(
 
 		uiBytesToRead = MIN(uiNumBytes,MAX_RW_SIZE);
 
-		if(rdm(Adapter,uiPartOffset, (PCHAR)pBuffer+uiIndex,uiBytesToRead))
-		{
-			Status = -1;
+		bytes = rdm(Adapter, uiPartOffset, (PCHAR)pBuffer+uiIndex, uiBytesToRead);
+		if (bytes < 0) {
+			Status = bytes;
 			break;
 		}
 
@@ -613,6 +614,7 @@ static INT FlashSectorErase(PMINI_ADAPTER Adapter,
 	UINT iIndex = 0, iRetries = 0;
 	UINT uiStatus = 0;
 	UINT value;
+	int bytes;
 
 	for(iIndex=0;iIndex<numOfSectors;iIndex++)
 	{
@@ -632,10 +634,11 @@ static INT FlashSectorErase(PMINI_ADAPTER Adapter,
 				return STATUS_FAILURE;
 			}
 
-			if(rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus)) < 0 )
-			{
-				BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Reading status of FLASH_SPI_READQ_REG fails");
-				return STATUS_FAILURE;
+			bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
+			if (bytes < 0) {
+				uiStatus = bytes;
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
+				return uiStatus;
 			}
 			iRetries++;
 			//After every try lets make the CPU free for 10 ms. generally time taken by the
@@ -679,6 +682,7 @@ static INT flashByteWrite(
 
 	UINT value;
 	ULONG ulData = *(PUCHAR)pData;
+	int bytes;
 
 //
 // need not write 0xFF because write requires an erase and erase will
@@ -720,10 +724,11 @@ static INT flashByteWrite(
 			return STATUS_FAILURE;
 	  	}
 	  	//__udelay(1);
-	  	if(rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus)) < 0)
-	  	{
-	  		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Reading status of FLASH_SPI_READQ_REG fails");
-			return STATUS_FAILURE;
+		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
+		if (bytes < 0) {
+			uiStatus = bytes;
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
+			return uiStatus;
 		}
 	  	iRetries--;
 		if( iRetries && ((iRetries % FLASH_PER_RETRIES_DELAY) == 0))
@@ -771,6 +776,7 @@ static INT flashWrite(
 
 	UINT value;
 	UINT uiErasePattern[4] = {0xFFFFFFFF,0xFFFFFFFF,0xFFFFFFFF,0xFFFFFFFF};
+	int bytes;
 //
 // need not write 0xFFFFFFFF because write requires an erase and erase will
 // make whole sector 0xFFFFFFFF.
@@ -803,10 +809,11 @@ static INT flashWrite(
 			return STATUS_FAILURE;
 	  	}
 	  	//__udelay(1);
-	  	if(rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus)) < 0 )
-	  	{
-	  		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Reading status of FLASH_SPI_READQ_REG fails");
-			return STATUS_FAILURE;
+		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
+		if (bytes < 0) {
+			uiStatus = bytes;
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
+			return uiStatus;
 		}
 
 		iRetries--;
@@ -849,6 +856,7 @@ static INT flashByteWriteStatus(
 	INT  iRetries = MAX_FLASH_RETRIES * FLASH_PER_RETRIES_DELAY; //3
 	ULONG ulData  = *(PUCHAR)pData;
 	UINT value;
+	int bytes;
 
 //
 // need not write 0xFFFFFFFF because write requires an erase and erase will
@@ -891,10 +899,11 @@ static INT flashByteWriteStatus(
 			return STATUS_FAILURE;
 		}
 		//__udelay(1);
-		if(rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus)) < 0)
-		{
-			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Reading status of FLASH_SPI_READQ_REG fails");
-			return STATUS_FAILURE;
+		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
+		if (bytes < 0) {
+			uiStatus = bytes;
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
+			return uiStatus;
 		}
 
 		iRetries--;
@@ -935,6 +944,7 @@ static INT flashWriteStatus(
 	//UINT uiReadBack = 0;
 	UINT value;
 	UINT uiErasePattern[4] = {0xFFFFFFFF,0xFFFFFFFF,0xFFFFFFFF,0xFFFFFFFF};
+	int bytes;
 
 //
 // need not write 0xFFFFFFFF because write requires an erase and erase will
@@ -967,10 +977,11 @@ static INT flashWriteStatus(
 			return STATUS_FAILURE;
 	  	}
 	  	//__udelay(1);
-	  	if(rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus)) < 0)
-	  	{
-	  		BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Reading status of FLASH_SPI_READQ_REG fails");
-			return STATUS_FAILURE;
+		bytes = rdmalt(Adapter, FLASH_SPI_READQ_REG, &uiStatus, sizeof(uiStatus));
+		if (bytes < 0) {
+			uiStatus = bytes;
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "Reading status of FLASH_SPI_READQ_REG fails");
+			return uiStatus;
 		}
 	  	iRetries--;
 		//this will ensure that in there will be no changes in the current path.
@@ -1841,7 +1852,7 @@ static INT BeceemEEPROMWritePage( PMINI_ADAPTER Adapter, UINT uiData[], UINT uiO
 	 * What we are checking if the previous write has completed, and this
 	 * may take time. We should wait till the Empty bit is set. */
 	uiStatus = 0;
-	rdmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG,&uiStatus, sizeof(uiStatus)) ;
+	rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
 	while ( ( uiStatus & EEPROM_WRITE_QUEUE_EMPTY ) == 0 )
 	{
 		uiRetries--;
@@ -1855,7 +1866,7 @@ static INT BeceemEEPROMWritePage( PMINI_ADAPTER Adapter, UINT uiData[], UINT uiO
 					msleep(1);
 
 		uiStatus = 0;
-		rdmalt( Adapter, EEPROM_SPI_Q_STATUS1_REG,&uiStatus, sizeof(uiStatus)) ;
+		rdmalt(Adapter, EEPROM_SPI_Q_STATUS1_REG, &uiStatus, sizeof(uiStatus));
 		if(Adapter->device_removed == TRUE)
 		{
 			BCM_DEBUG_PRINT(Adapter,DBG_TYPE_PRINTK, 0, 0,"Modem got removed hence exiting from loop....");
@@ -2500,7 +2511,7 @@ static ULONG BcmReadFlashRDID(PMINI_ADAPTER Adapter)
 // Read SPI READQ REG. The output will be WWXXYYZZ.
 // The ID is 3Bytes long and is WWXXYY. ZZ needs to be Ignored.
 //
-	rdmalt(Adapter, FLASH_SPI_READQ_REG,(PUINT)&ulRDID, sizeof(ulRDID));
+	rdmalt(Adapter, FLASH_SPI_READQ_REG, (PUINT)&ulRDID, sizeof(ulRDID));
 
 	return (ulRDID >>8);
 
@@ -4735,8 +4746,8 @@ static INT BcmDoChipSelect(PMINI_ADAPTER Adapter, UINT offset)
 	Adapter->SelectedChip = ChipNum ;
 
 	//bit[13..12]  will select the appropriate chip
-	rdmalt(Adapter,FLASH_CONFIG_REG, &FlashConfig, 4);
-	rdmalt(Adapter,FLASH_GPIO_CONFIG_REG, &GPIOConfig, 4);
+	rdmalt(Adapter, FLASH_CONFIG_REG, &FlashConfig, 4);
+	rdmalt(Adapter, FLASH_GPIO_CONFIG_REG, &GPIOConfig, 4);
 
 	{
 		switch(ChipNum)
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 5e78c77d5a08..0d18d80bcd25 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -1479,10 +1479,10 @@ static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 
 	dev_file_info = comedi_get_device_file_info(minor);
 	if (dev_file_info == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 	dev = dev_file_info->device;
 	if (dev == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 
 	mutex_lock(&dev->mutex);
 	if (!dev->attached) {
@@ -1556,10 +1556,10 @@ static unsigned int comedi_poll(struct file *file, poll_table * wait)
 	dev_file_info = comedi_get_device_file_info(minor);
 
 	if (dev_file_info == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 	dev = dev_file_info->device;
 	if (dev == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 
 	mutex_lock(&dev->mutex);
 	if (!dev->attached) {
@@ -1610,10 +1610,10 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 	dev_file_info = comedi_get_device_file_info(minor);
 
 	if (dev_file_info == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 	dev = dev_file_info->device;
 	if (dev == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 
 	if (!dev->attached) {
 		DPRINTK("no driver configured on comedi%i\n", dev->minor);
@@ -1721,10 +1721,10 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 	dev_file_info = comedi_get_device_file_info(minor);
 
 	if (dev_file_info == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 	dev = dev_file_info->device;
 	if (dev == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 
 	if (!dev->attached) {
 		DPRINTK("no driver configured on comedi%i\n", dev->minor);
@@ -1931,10 +1931,10 @@ static int comedi_close(struct inode *inode, struct file *file)
 	dev_file_info = comedi_get_device_file_info(minor);
 
 	if (dev_file_info == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 	dev = dev_file_info->device;
 	if (dev == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 
 	mutex_lock(&dev->mutex);
 
@@ -1973,10 +1973,10 @@ static int comedi_fasync(int fd, struct file *file, int on)
 	dev_file_info = comedi_get_device_file_info(minor);
 
 	if (dev_file_info == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 	dev = dev_file_info->device;
 	if (dev == NULL)
-	        return -ENODEV;
+		return -ENODEV;
 
 	return fasync_helper(fd, file, on, &dev->async_queue);
 }
@@ -2479,18 +2479,18 @@ static ssize_t store_max_read_buffer_kb(struct device *dev,
 					const char *buf, size_t count)
 {
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned long new_max_size_kb;
-	uint64_t new_max_size;
+	unsigned int new_max_size_kb;
+	unsigned int new_max_size;
+	int ret;
 	struct comedi_subdevice *const read_subdevice =
 	    comedi_get_read_subdevice(info);
 
-	if (strict_strtoul(buf, 10, &new_max_size_kb))
-		return -EINVAL;
-	if (new_max_size_kb != (uint32_t) new_max_size_kb)
-		return -EINVAL;
-	new_max_size = ((uint64_t) new_max_size_kb) * bytes_per_kibi;
-	if (new_max_size != (uint32_t) new_max_size)
+	ret = kstrtouint(buf, 10, &new_max_size_kb);
+	if (ret)
+		return ret;
+	if (new_max_size_kb > (UINT_MAX / bytes_per_kibi))
 		return -EINVAL;
+	new_max_size = new_max_size_kb * bytes_per_kibi;
 
 	mutex_lock(&info->device->mutex);
 	if (read_subdevice == NULL ||
@@ -2540,19 +2540,19 @@ static ssize_t store_read_buffer_kb(struct device *dev,
 				    const char *buf, size_t count)
 {
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned long new_size_kb;
-	uint64_t new_size;
+	unsigned int new_size_kb;
+	unsigned int new_size;
 	int retval;
+	int ret;
 	struct comedi_subdevice *const read_subdevice =
 	    comedi_get_read_subdevice(info);
 
-	if (strict_strtoul(buf, 10, &new_size_kb))
-		return -EINVAL;
-	if (new_size_kb != (uint32_t) new_size_kb)
-		return -EINVAL;
-	new_size = ((uint64_t) new_size_kb) * bytes_per_kibi;
-	if (new_size != (uint32_t) new_size)
+	ret = kstrtouint(buf, 10, &new_size_kb);
+	if (ret)
+		return ret;
+	if (new_size_kb > (UINT_MAX / bytes_per_kibi))
 		return -EINVAL;
+	new_size = new_size_kb * bytes_per_kibi;
 
 	mutex_lock(&info->device->mutex);
 	if (read_subdevice == NULL ||
@@ -2606,18 +2606,18 @@ static ssize_t store_max_write_buffer_kb(struct device *dev,
 					 const char *buf, size_t count)
 {
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned long new_max_size_kb;
-	uint64_t new_max_size;
+	unsigned int new_max_size_kb;
+	unsigned int new_max_size;
+	int ret;
 	struct comedi_subdevice *const write_subdevice =
 	    comedi_get_write_subdevice(info);
 
-	if (strict_strtoul(buf, 10, &new_max_size_kb))
-		return -EINVAL;
-	if (new_max_size_kb != (uint32_t) new_max_size_kb)
-		return -EINVAL;
-	new_max_size = ((uint64_t) new_max_size_kb) * bytes_per_kibi;
-	if (new_max_size != (uint32_t) new_max_size)
+	ret = kstrtouint(buf, 10, &new_max_size_kb);
+	if (ret)
+		return ret;
+	if (new_max_size_kb > (UINT_MAX / bytes_per_kibi))
 		return -EINVAL;
+	new_max_size = new_max_size_kb * bytes_per_kibi;
 
 	mutex_lock(&info->device->mutex);
 	if (write_subdevice == NULL ||
@@ -2667,19 +2667,19 @@ static ssize_t store_write_buffer_kb(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct comedi_device_file_info *info = dev_get_drvdata(dev);
-	unsigned long new_size_kb;
-	uint64_t new_size;
+	unsigned int new_size_kb;
+	unsigned int new_size;
 	int retval;
+	int ret;
 	struct comedi_subdevice *const write_subdevice =
 	    comedi_get_write_subdevice(info);
 
-	if (strict_strtoul(buf, 10, &new_size_kb))
-		return -EINVAL;
-	if (new_size_kb != (uint32_t) new_size_kb)
+	ret = kstrtouint(buf, 10, &new_size_kb);
+	if (ret)
+		return ret;
+	if (new_size_kb > (UINT_MAX / bytes_per_kibi))
 		return -EINVAL;
 	new_size = ((uint64_t) new_size_kb) * bytes_per_kibi;
-	if (new_size != (uint32_t) new_size)
-		return -EINVAL;
 
 	mutex_lock(&info->device->mutex);
 	if (write_subdevice == NULL ||
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.c b/drivers/staging/comedi/drivers/addi-data/addi_common.c
index 6fb7594319c6..ca5bd9b8704a 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.c
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.c
@@ -145,78 +145,77 @@ void fpu_end(void)
 
 static DEFINE_PCI_DEVICE_TABLE(addi_apci_tbl) = {
 #ifdef CONFIG_APCI_3120
-	{APCI3120_BOARD_VENDOR_ID, 0x818D, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI3120_BOARD_VENDOR_ID, 0x818D)},
 #endif
 #ifdef CONFIG_APCI_1032
-	{APCI1032_BOARD_VENDOR_ID, 0x1003, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI1032_BOARD_VENDOR_ID, 0x1003)},
 #endif
 #ifdef CONFIG_APCI_1516
-	{APCI1516_BOARD_VENDOR_ID, 0x1001, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI1516_BOARD_VENDOR_ID, 0x1001)},
 #endif
 #ifdef CONFIG_APCI_2016
-	{APCI2016_BOARD_VENDOR_ID, 0x1002, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI2016_BOARD_VENDOR_ID, 0x1002)},
 #endif
 #ifdef CONFIG_APCI_2032
-	{APCI2032_BOARD_VENDOR_ID, 0x1004, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI2032_BOARD_VENDOR_ID, 0x1004)},
 #endif
 #ifdef CONFIG_APCI_2200
-	{APCI2200_BOARD_VENDOR_ID, 0x1005, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI2200_BOARD_VENDOR_ID, 0x1005)},
 #endif
 #ifdef CONFIG_APCI_1564
-	{APCI1564_BOARD_VENDOR_ID, 0x1006, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI1564_BOARD_VENDOR_ID, 0x1006)},
 #endif
 #ifdef CONFIG_APCI_1500
-	{APCI1500_BOARD_VENDOR_ID, 0x80fc, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI1500_BOARD_VENDOR_ID, 0x80fc)},
 #endif
 #ifdef CONFIG_APCI_3001
-	{APCI3120_BOARD_VENDOR_ID, 0x828D, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI3120_BOARD_VENDOR_ID, 0x828D)},
 #endif
 #ifdef CONFIG_APCI_3501
-	{APCI3501_BOARD_VENDOR_ID, 0x3001, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI3501_BOARD_VENDOR_ID, 0x3001)},
 #endif
 #ifdef CONFIG_APCI_035
-	{APCI035_BOARD_VENDOR_ID, 0x0300, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI035_BOARD_VENDOR_ID,  0x0300)},
 #endif
 #ifdef CONFIG_APCI_3200
-	{APCI3200_BOARD_VENDOR_ID, 0x3000, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI3200_BOARD_VENDOR_ID, 0x3000)},
 #endif
 #ifdef CONFIG_APCI_3300
-	{APCI3200_BOARD_VENDOR_ID, 0x3007, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI3200_BOARD_VENDOR_ID, 0x3007)},
 #endif
 #ifdef CONFIG_APCI_1710
-	{APCI1710_BOARD_VENDOR_ID, APCI1710_BOARD_DEVICE_ID,
-		PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(APCI1710_BOARD_VENDOR_ID, APCI1710_BOARD_DEVICE_ID)},
 #endif
 #ifdef CONFIG_APCI_16XX
-	{0x15B8, 0x1009, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x100A, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x1009)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x100A)},
 #endif
 #ifdef CONFIG_APCI_3XXX
-	{0x15B8, 0x3010, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x300F, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x300E, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3013, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3014, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3015, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3016, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3017, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3018, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3019, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x301A, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x301B, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x301C, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x301D, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x301E, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x301F, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3020, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3021, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3022, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3023, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x300B, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3002, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3003, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3004, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x15B8, 0x3024, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3010)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x300F)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x300E)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3013)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3014)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3015)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3016)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3017)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3018)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3019)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301A)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301B)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301C)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301D)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301E)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x301F)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3020)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3021)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3022)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3023)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x300B)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3002)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3003)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3004)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ADDIDATA, 0x3024)},
 #endif
 	{0}
 };
@@ -1019,7 +1018,7 @@ static const struct addi_board boardtypes[] = {
 #endif
 #ifdef CONFIG_APCI_16XX
 	{"apci1648",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x1009,
 			128,
 			0,
@@ -1075,7 +1074,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI16XX_InsnBitsWriteTTLIO},
 
 	{"apci1696",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x100A,
 			128,
 			0,
@@ -1132,7 +1131,7 @@ static const struct addi_board boardtypes[] = {
 #endif
 #ifdef CONFIG_APCI_3XXX
 	{"apci3000-16",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3010,
 			256,
 			256,
@@ -1188,7 +1187,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3000-8",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x300F,
 			256,
 			256,
@@ -1244,7 +1243,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3000-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x300E,
 			256,
 			256,
@@ -1300,7 +1299,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3006-16",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3013,
 			256,
 			256,
@@ -1356,7 +1355,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3006-8",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3014,
 			256,
 			256,
@@ -1412,7 +1411,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3006-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3015,
 			256,
 			256,
@@ -1468,7 +1467,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3010-16",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3016,
 			256,
 			256,
@@ -1524,7 +1523,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3010-8",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3017,
 			256,
 			256,
@@ -1580,7 +1579,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3010-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3018,
 			256,
 			256,
@@ -1636,7 +1635,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3016-16",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3019,
 			256,
 			256,
@@ -1692,7 +1691,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3016-8",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x301A,
 			256,
 			256,
@@ -1748,7 +1747,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3016-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x301B,
 			256,
 			256,
@@ -1804,7 +1803,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3100-16-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x301C,
 			256,
 			256,
@@ -1860,7 +1859,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3100-8-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x301D,
 			256,
 			256,
@@ -1916,7 +1915,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3106-16-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x301E,
 			256,
 			256,
@@ -1972,7 +1971,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3106-8-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x301F,
 			256,
 			256,
@@ -2028,7 +2027,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3110-16-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3020,
 			256,
 			256,
@@ -2084,7 +2083,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3110-8-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3021,
 			256,
 			256,
@@ -2140,7 +2139,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3116-16-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3022,
 			256,
 			256,
@@ -2196,7 +2195,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3116-8-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3023,
 			256,
 			256,
@@ -2252,7 +2251,7 @@ static const struct addi_board boardtypes[] = {
 		i_APCI3XXX_InsnWriteTTLIO},
 
 	{"apci3003",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x300B,
 			256,
 			256,
@@ -2307,7 +2306,7 @@ static const struct addi_board boardtypes[] = {
 		NULL},
 
 	{"apci3002-16",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3002,
 			256,
 			256,
@@ -2362,7 +2361,7 @@ static const struct addi_board boardtypes[] = {
 		NULL},
 
 	{"apci3002-8",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3003,
 			256,
 			256,
@@ -2417,7 +2416,7 @@ static const struct addi_board boardtypes[] = {
 		NULL},
 
 	{"apci3002-4",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3004,
 			256,
 			256,
@@ -2472,7 +2471,7 @@ static const struct addi_board boardtypes[] = {
 		NULL},
 
 	{"apci3500",
-			0x15B8,
+			PCI_VENDOR_ID_ADDIDATA,
 			0x3024,
 			256,
 			256,
diff --git a/drivers/staging/comedi/drivers/adl_pci7230.c b/drivers/staging/comedi/drivers/adl_pci7230.c
index 72a7258b5b92..20d570554fa4 100644
--- a/drivers/staging/comedi/drivers/adl_pci7230.c
+++ b/drivers/staging/comedi/drivers/adl_pci7230.c
@@ -44,15 +44,7 @@ Configuration Options:
 #define PCI_DEVICE_ID_PCI7230 0x7230
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci7230_pci_table) = {
-	{
-		PCI_VENDOR_ID_ADLINK,
-		PCI_DEVICE_ID_PCI7230,
-		PCI_ANY_ID,
-		PCI_ANY_ID,
-		0,
-		0,
-		0
-	},
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7230) },
 	{0}
 };
 
diff --git a/drivers/staging/comedi/drivers/adl_pci7296.c b/drivers/staging/comedi/drivers/adl_pci7296.c
index f28fe6bec050..9a2320537bdb 100644
--- a/drivers/staging/comedi/drivers/adl_pci7296.c
+++ b/drivers/staging/comedi/drivers/adl_pci7296.c
@@ -49,10 +49,8 @@ Configuration Options:
 #define PCI_DEVICE_ID_PCI7296 0x7296
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci7296_pci_table) = {
-	{
-	PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7296, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7296) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, adl_pci7296_pci_table);
diff --git a/drivers/staging/comedi/drivers/adl_pci7432.c b/drivers/staging/comedi/drivers/adl_pci7432.c
index 262da7b29b28..86ee21976041 100644
--- a/drivers/staging/comedi/drivers/adl_pci7432.c
+++ b/drivers/staging/comedi/drivers/adl_pci7432.c
@@ -44,10 +44,8 @@ Configuration Options:
 #define PCI_DEVICE_ID_PCI7432 0x7432
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci7432_pci_table) = {
-	{
-	PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7432, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI7432) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, adl_pci7432_pci_table);
diff --git a/drivers/staging/comedi/drivers/adl_pci8164.c b/drivers/staging/comedi/drivers/adl_pci8164.c
index 767a594935c7..3b83d65bc1bc 100644
--- a/drivers/staging/comedi/drivers/adl_pci8164.c
+++ b/drivers/staging/comedi/drivers/adl_pci8164.c
@@ -57,10 +57,8 @@ Configuration Options:
 #define PCI_DEVICE_ID_PCI8164 0x8164
 
 static DEFINE_PCI_DEVICE_TABLE(adl_pci8164_pci_table) = {
-	{
-	PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI8164, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI_DEVICE_ID_PCI8164) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, adl_pci8164_pci_table);
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index fc48bae42ab7..2a9bd88a4abb 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -311,10 +311,8 @@ static const struct comedi_lrange pci9111_hr_ai_range = {
 };
 
 static DEFINE_PCI_DEVICE_TABLE(pci9111_pci_table) = {
-	{ PCI_VENDOR_ID_ADLINK, PCI9111_HR_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
-	  0, 0, 0 },
-	/* { PCI_VENDOR_ID_ADLINK, PCI9111_HG_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,
-	 *   0, 0, 0 }, */
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI9111_HR_DEVICE_ID) },
+	/* { PCI_DEVICE(PCI_VENDOR_ID_ADLINK, PCI9111_HG_DEVICE_ID) }, */
 	{ 0 }
 };
 
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index da2b75b15d4e..8318c82a555a 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -1382,16 +1382,14 @@ static int pci1710_attach(struct comedi_device *dev,
 	int i;
 	int board_index;
 
-	printk("comedi%d: adv_pci1710: ", dev->minor);
+	dev_info(dev->hw_dev, "comedi%d: adv_pci1710:\n", dev->minor);
 
 	opt_bus = it->options[0];
 	opt_slot = it->options[1];
 
 	ret = alloc_private(dev, sizeof(struct pci1710_private));
-	if (ret < 0) {
-		printk(" - Allocation failed!\n");
+	if (ret < 0)
 		return -ENOMEM;
-	}
 
 	/* Look for matching PCI device */
 	errstr = "not found!";
@@ -1436,10 +1434,10 @@ static int pci1710_attach(struct comedi_device *dev,
 
 	if (!pcidev) {
 		if (opt_bus || opt_slot) {
-			printk(" - Card at b:s %d:%d %s\n",
-			       opt_bus, opt_slot, errstr);
+			dev_err(dev->hw_dev, "- Card at b:s %d:%d %s\n",
+				opt_bus, opt_slot, errstr);
 		} else {
-			printk(" - Card %s\n", errstr);
+			dev_err(dev->hw_dev, "- Card %s\n", errstr);
 		}
 		return -EIO;
 	}
@@ -1450,8 +1448,8 @@ static int pci1710_attach(struct comedi_device *dev,
 	irq = pcidev->irq;
 	iobase = pci_resource_start(pcidev, 2);
 
-	printk(", b:s:f=%d:%d:%d, io=0x%4lx", pci_bus, pci_slot, pci_func,
-	       iobase);
+	dev_dbg(dev->hw_dev, "b:s:f=%d:%d:%d, io=0x%4lx\n", pci_bus, pci_slot,
+		pci_func, iobase);
 
 	dev->iobase = iobase;
 
@@ -1471,10 +1469,8 @@ static int pci1710_attach(struct comedi_device *dev,
 		n_subdevices++;
 
 	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0) {
-		printk(" - Allocation failed!\n");
+	if (ret < 0)
 		return ret;
-	}
 
 	pci1710_reset(dev);
 
@@ -1483,24 +1479,20 @@ static int pci1710_attach(struct comedi_device *dev,
 			if (request_irq(irq, interrupt_service_pci1710,
 					IRQF_SHARED, "Advantech PCI-1710",
 					dev)) {
-				printk
-				    (", unable to allocate IRQ %d, DISABLING IT",
-				     irq);
+				dev_dbg(dev->hw_dev, "unable to allocate IRQ %d, DISABLING IT",
+					irq);
 				irq = 0;	/* Can't use IRQ */
 			} else {
-				printk(", irq=%u", irq);
+				dev_dbg(dev->hw_dev, "irq=%u", irq);
 			}
 		} else {
-			printk(", IRQ disabled");
+			dev_dbg(dev->hw_dev, "IRQ disabled");
 		}
 	} else {
 		irq = 0;
 	}
 
 	dev->irq = irq;
-
-	printk(".\n");
-
 	subdev = 0;
 
 	if (this_board->n_aichan) {
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index 69334f6f64e8..537e58534275 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -1106,13 +1106,10 @@ static int pci_dio_attach(struct comedi_device *dev,
 	unsigned long iobase;
 	struct pci_dev *pcidev = NULL;
 
-	printk("comedi%d: adv_pci_dio: ", dev->minor);
 
 	ret = alloc_private(dev, sizeof(struct pci_dio_private));
-	if (ret < 0) {
-		printk(", Error: Cann't allocate private memory!\n");
+	if (ret < 0)
 		return -ENOMEM;
-	}
 
 	for_each_pci_dev(pcidev) {
 		/*  loop through cards supported by this driver */
@@ -1140,19 +1137,18 @@ static int pci_dio_attach(struct comedi_device *dev,
 	}
 
 	if (!dev->board_ptr) {
-		printk(", Error: Requested type of the card was not found!\n");
+		dev_err(dev->hw_dev, "Error: Requested type of the card was not found!\n");
 		return -EIO;
 	}
 
 	if (comedi_pci_enable(pcidev, driver_pci_dio.driver_name)) {
-		printk
-		    (", Error: Can't enable PCI device and request regions!\n");
+		dev_err(dev->hw_dev, "Error: Can't enable PCI device and request regions!\n");
 		return -EIO;
 	}
 	iobase = pci_resource_start(pcidev, this_board->main_pci_region);
-	printk(", b:s:f=%d:%d:%d, io=0x%4lx",
-	       pcidev->bus->number, PCI_SLOT(pcidev->devfn),
-	       PCI_FUNC(pcidev->devfn), iobase);
+	dev_dbg(dev->hw_dev, "b:s:f=%d:%d:%d, io=0x%4lx\n",
+		pcidev->bus->number, PCI_SLOT(pcidev->devfn),
+		PCI_FUNC(pcidev->devfn), iobase);
 
 	dev->iobase = iobase;
 	dev->board_name = this_board->name;
@@ -1177,15 +1173,10 @@ static int pci_dio_attach(struct comedi_device *dev,
 	}
 
 	ret = alloc_subdevices(dev, n_subdevices);
-	if (ret < 0) {
-		printk(", Error: Cann't allocate subdevice memory!\n");
+	if (ret < 0)
 		return ret;
-	}
-
-	printk(".\n");
 
 	subdev = 0;
-
 	for (i = 0; i < MAX_DI_SUBDEVS; i++)
 		if (this_board->sdi[i].chans) {
 			s = dev->subdevices + subdev;
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index 93bbe4ec318d..566cc4411452 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -421,12 +421,9 @@ static const struct dio200_layout_struct dio200_layouts[] = {
 
 #ifdef CONFIG_COMEDI_PCI
 static DEFINE_PCI_DEVICE_TABLE(dio200_pci_table) = {
-	{
-	PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI215,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI272,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI215) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI272) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, dio200_pci_table);
diff --git a/drivers/staging/comedi/drivers/amplc_pc236.c b/drivers/staging/comedi/drivers/amplc_pc236.c
index 48246cd50d47..7972cadd403e 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236.c
@@ -134,10 +134,8 @@ static const struct pc236_board pc236_boards[] = {
 
 #ifdef CONFIG_COMEDI_PCI
 static DEFINE_PCI_DEVICE_TABLE(pc236_pci_table) = {
-	{
-	PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI236,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI236) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, pc236_pci_table);
diff --git a/drivers/staging/comedi/drivers/amplc_pc263.c b/drivers/staging/comedi/drivers/amplc_pc263.c
index 8a3388079094..191ac0d23ce7 100644
--- a/drivers/staging/comedi/drivers/amplc_pc263.c
+++ b/drivers/staging/comedi/drivers/amplc_pc263.c
@@ -101,10 +101,8 @@ static const struct pc263_board pc263_boards[] = {
 
 #ifdef CONFIG_COMEDI_PCI
 static DEFINE_PCI_DEVICE_TABLE(pc263_pci_table) = {
-	{
-	PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI263,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI263) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, pc263_pci_table);
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 1b5ba1c27259..b278917cec25 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -384,12 +384,9 @@ static const struct pci224_board pci224_boards[] = {
  */
 
 static DEFINE_PCI_DEVICE_TABLE(pci224_pci_table) = {
-	{
-	PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI224,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI234,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI224) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI234) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, pci224_pci_table);
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 7edeb1103dc8..538979551c8e 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -501,12 +501,9 @@ static const struct pci230_board pci230_boards[] = {
 };
 
 static DEFINE_PCI_DEVICE_TABLE(pci230_pci_table) = {
-	{
-	PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_PCI230, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_PCI260, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_PCI230) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_PCI260) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, pci230_pci_table);
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index e171c56112d8..49404f49f7b7 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -99,7 +99,7 @@ static struct comedi_driver driver_das16cs = {
 	.detach = das16cs_detach,
 };
 
-static struct pcmcia_device *cur_dev = NULL;
+static struct pcmcia_device *cur_dev;
 
 static const struct comedi_lrange das16cs_ai_range = { 4, {
 							   RANGE(-10, 10),
@@ -150,7 +150,7 @@ static const struct das16cs_board *das16cs_probe(struct comedi_device *dev,
 			return das16cs_boards + i;
 	}
 
-	printk("unknown board!\n");
+	dev_dbg(dev->hw_dev, "unknown board!\n");
 
 	return NULL;
 }
@@ -163,20 +163,19 @@ static int das16cs_attach(struct comedi_device *dev,
 	int ret;
 	int i;
 
-	printk("comedi%d: cb_das16_cs: ", dev->minor);
+	dev_dbg(dev->hw_dev, "comedi%d: cb_das16_cs: attached\n", dev->minor);
 
 	link = cur_dev;		/* XXX hack */
 	if (!link)
 		return -EIO;
 
 	dev->iobase = link->resource[0]->start;
-	printk("I/O base=0x%04lx ", dev->iobase);
+	dev_dbg(dev->hw_dev, "I/O base=0x%04lx\n", dev->iobase);
 
-	printk("fingerprint:\n");
+	dev_dbg(dev->hw_dev, "fingerprint:\n");
 	for (i = 0; i < 48; i += 2)
-		printk("%04x ", inw(dev->iobase + i));
+		dev_dbg(dev->hw_dev, "%04x\n", inw(dev->iobase + i));
 
-	printk("\n");
 
 	ret = request_irq(link->irq, das16cs_interrupt,
 			  IRQF_SHARED, "cb_das16_cs", dev);
@@ -185,7 +184,7 @@ static int das16cs_attach(struct comedi_device *dev,
 
 	dev->irq = link->irq;
 
-	printk("irq=%u ", dev->irq);
+	dev_dbg(dev->hw_dev, "irq=%u\n", dev->irq);
 
 	dev->board_ptr = das16cs_probe(dev, link);
 	if (!dev->board_ptr)
@@ -252,14 +251,13 @@ static int das16cs_attach(struct comedi_device *dev,
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
-	printk("attached\n");
 
 	return 1;
 }
 
 static int das16cs_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: das16cs: remove\n", dev->minor);
+	dev_dbg(dev->hw_dev, "comedi%d: das16cs: remove\n", dev->minor);
 
 	if (dev->irq)
 		free_irq(dev->irq, dev);
@@ -312,7 +310,7 @@ static int das16cs_ai_rinsn(struct comedi_device *dev,
 				break;
 		}
 		if (to == TIMEOUT) {
-			printk("cb_das16_cs: ai timeout\n");
+			dev_dbg(dev->hw_dev, "cb_das16_cs: ai timeout\n");
 			return -ETIME;
 		}
 		data[i] = (unsigned short)inw(dev->iobase + 0);
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index 61968a505f24..7e4ffcfdac62 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -565,8 +565,6 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 	int index;
 	int i;
 
-	printk("comedi%d: cb_pcidas: ", dev->minor);
-
 /*
  * Allocate the private structure area.
  */
@@ -576,7 +574,6 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 /*
  * Probe the device to determine what device in the series it is.
  */
-	printk("\n");
 
 	for_each_pci_dev(pcidev) {
 		/*  is it not a computer boards card? */
@@ -600,20 +597,20 @@ static int cb_pcidas_attach(struct comedi_device *dev,
 		}
 	}
 
-	printk("No supported ComputerBoards/MeasurementComputing card found on "
-	       "requested position\n");
+	dev_err(dev->hw_dev, "No supported ComputerBoards/MeasurementComputing card found on requested position\n");
 	return -EIO;
 
 found:
 
-	printk("Found %s on bus %i, slot %i\n", cb_pcidas_boards[index].name,
-	       pcidev->bus->number, PCI_SLOT(pcidev->devfn));
+	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n",
+		cb_pcidas_boards[index].name, pcidev->bus->number,
+		PCI_SLOT(pcidev->devfn));
 
 	/*
 	 * Enable PCI device and reserve I/O ports.
 	 */
 	if (comedi_pci_enable(pcidev, "cb_pcidas")) {
-		printk(" Failed to enable PCI device and request regions\n");
+		dev_err(dev->hw_dev, "Failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
 	/*
@@ -639,7 +636,8 @@ found:
 	/*  get irq */
 	if (request_irq(devpriv->pci_dev->irq, cb_pcidas_interrupt,
 			IRQF_SHARED, "cb_pcidas", dev)) {
-		printk(" unable to allocate irq %d\n", devpriv->pci_dev->irq);
+		dev_dbg(dev->hw_dev, "unable to allocate irq %d\n",
+			devpriv->pci_dev->irq);
 		return -EINVAL;
 	}
 	dev->irq = devpriv->pci_dev->irq;
@@ -768,7 +766,6 @@ found:
  */
 static int cb_pcidas_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: cb_pcidas: remove\n", dev->minor);
 
 	if (devpriv) {
 		if (devpriv->s5933_config) {
@@ -776,8 +773,8 @@ static int cb_pcidas_detach(struct comedi_device *dev)
 			outl(INTCSR_INBOX_INTR_STATUS,
 			     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 #ifdef CB_PCIDAS_DEBUG
-			printk("detaching, incsr is 0x%x\n",
-			       inl(devpriv->s5933_config + AMCC_OP_REG_INTCSR));
+			dev_dbg(dev->hw_dev, "detaching, incsr is 0x%x\n",
+				inl(devpriv->s5933_config + AMCC_OP_REG_INTCSR));
 #endif
 		}
 	}
@@ -858,7 +855,8 @@ static int ai_config_calibration_source(struct comedi_device *dev,
 	unsigned int source = data[1];
 
 	if (source >= num_calibration_sources) {
-		printk("invalid calibration source: %i\n", source);
+		dev_err(dev->hw_dev, "invalid calibration source: %i\n",
+			source);
 		return -EINVAL;
 	}
 
@@ -1279,7 +1277,7 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	outw(bits, devpriv->control_status + ADCMUX_CONT);
 
 #ifdef CB_PCIDAS_DEBUG
-	printk("comedi: sent 0x%x to adcmux control\n", bits);
+	dev_dbg(dev->hw_dev, "comedi: sent 0x%x to adcmux control\n", bits);
 #endif
 
 	/*  load counters */
@@ -1306,7 +1304,8 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 		devpriv->adc_fifo_bits |= INT_FHF;	/* interrupt fifo half full */
 	}
 #ifdef CB_PCIDAS_DEBUG
-	printk("comedi: adc_fifo_bits are 0x%x\n", devpriv->adc_fifo_bits);
+	dev_dbg(dev->hw_dev, "comedi: adc_fifo_bits are 0x%x\n",
+		devpriv->adc_fifo_bits);
 #endif
 	/*  enable (and clear) interrupts */
 	outw(devpriv->adc_fifo_bits | EOAI | INT | LADFUL,
@@ -1332,7 +1331,7 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 		bits |= BURSTE;
 	outw(bits, devpriv->control_status + TRIG_CONTSTAT);
 #ifdef CB_PCIDAS_DEBUG
-	printk("comedi: sent 0x%x to trig control\n", bits);
+	dev_dbg(dev->hw_dev, "comedi: sent 0x%x to trig control\n", bits);
 #endif
 
 	return 0;
@@ -1549,7 +1548,8 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->adc_fifo_bits |= DAEMIE | DAHFIE;
 #ifdef CB_PCIDAS_DEBUG
-	printk("comedi: adc_fifo_bits are 0x%x\n", devpriv->adc_fifo_bits);
+	dev_dbg(dev->hw_dev, "comedi: adc_fifo_bits are 0x%x\n",
+		devpriv->adc_fifo_bits);
 #endif
 	/*  enable and clear interrupts */
 	outw(devpriv->adc_fifo_bits | DAEMI | DAHFI,
@@ -1559,7 +1559,8 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 	devpriv->ao_control_bits |= DAC_START | DACEN | DAC_EMPTY;
 	outw(devpriv->ao_control_bits, devpriv->control_status + DAC_CSR);
 #ifdef CB_PCIDAS_DEBUG
-	printk("comedi: sent 0x%x to dac control\n", devpriv->ao_control_bits);
+	dev_dbg(dev->hw_dev, "comedi: sent 0x%x to dac control\n",
+		devpriv->ao_control_bits);
 #endif
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
@@ -1587,8 +1588,9 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 
 	s5933_status = inl(devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 #ifdef CB_PCIDAS_DEBUG
-	printk("intcsr 0x%x\n", s5933_status);
-	printk("mbef 0x%x\n", inl(devpriv->s5933_config + AMCC_OP_REG_MBEF));
+	dev_dbg(dev->hw_dev, "intcsr 0x%x\n", s5933_status);
+	dev_dbg(dev->hw_dev, "mbef 0x%x\n",
+		inl(devpriv->s5933_config + AMCC_OP_REG_MBEF));
 #endif
 
 	if ((INTCSR_INTR_ASSERTED & s5933_status) == 0)
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index 1e324198996c..c9e8c4785768 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -1739,8 +1739,6 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	uint32_t local_range, local_decode;
 	int retval;
 
-	printk("comedi%d: cb_pcidas64\n", dev->minor);
-
 /*
  * Allocate the private structure area.
  */
@@ -1781,12 +1779,11 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -EIO;
 	}
 
-	printk("Found %s on bus %i, slot %i\n", board(dev)->name,
-	       pcidev->bus->number, PCI_SLOT(pcidev->devfn));
+	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n", board(dev)->name,
+		pcidev->bus->number, PCI_SLOT(pcidev->devfn));
 
 	if (comedi_pci_enable(pcidev, driver_cb_pcidas.driver_name)) {
-		printk(KERN_WARNING
-		       " failed to enable PCI device and request regions\n");
+		dev_warn(dev->hw_dev, "failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
 	pci_set_master(pcidev);
@@ -1814,7 +1811,7 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	if (!priv(dev)->plx9080_iobase || !priv(dev)->main_iobase
 	    || !priv(dev)->dio_counter_iobase) {
-		printk(" failed to remap io memory\n");
+		dev_warn(dev->hw_dev, "failed to remap io memory\n");
 		return -ENOMEM;
 	}
 
@@ -1850,17 +1847,19 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	priv(dev)->hw_revision =
 	    hw_revision(dev, readw(priv(dev)->main_iobase + HW_STATUS_REG));
-	printk(" stc hardware revision %i\n", priv(dev)->hw_revision);
+	dev_dbg(dev->hw_dev, "stc hardware revision %i\n",
+		priv(dev)->hw_revision);
 	init_plx9080(dev);
 	init_stc_registers(dev);
 	/*  get irq */
 	if (request_irq(pcidev->irq, handle_interrupt, IRQF_SHARED,
 			"cb_pcidas64", dev)) {
-		printk(" unable to allocate irq %u\n", pcidev->irq);
+		dev_dbg(dev->hw_dev, "unable to allocate irq %u\n",
+			pcidev->irq);
 		return -EINVAL;
 	}
 	dev->irq = pcidev->irq;
-	printk(" irq %u\n", dev->irq);
+	dev_dbg(dev->hw_dev, "irq %u\n", dev->irq);
 
 	retval = setup_subdevices(dev);
 	if (retval < 0)
@@ -1882,8 +1881,6 @@ static int detach(struct comedi_device *dev)
 {
 	unsigned int i;
 
-	printk("comedi%d: cb_pcidas: remove\n", dev->minor);
-
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (priv(dev)) {
@@ -2093,7 +2090,8 @@ static int ai_config_calibration_source(struct comedi_device *dev,
 	else
 		num_calibration_sources = 8;
 	if (source >= num_calibration_sources) {
-		printk("invalid calibration source: %i\n", source);
+		dev_dbg(dev->hw_dev, "invalid calibration source: %i\n",
+			source);
 		return -EINVAL;
 	}
 
@@ -2924,7 +2922,7 @@ static void pio_drain_ai_fifo_16(struct comedi_device *dev)
 		}
 
 		if (num_samples < 0) {
-			printk(" cb_pcidas64: bug! num_samples < 0\n");
+			dev_err(dev->hw_dev, "cb_pcidas64: bug! num_samples < 0\n");
 			break;
 		}
 
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index 49102b3a6c4a..abba220a767f 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -282,7 +282,6 @@ static int cb_pcidda_attach(struct comedi_device *dev,
 	struct pci_dev *pcidev = NULL;
 	int index;
 
-	printk("comedi%d: cb_pcidda: ", dev->minor);
 
 /*
  * Allocate the private structure area.
@@ -293,7 +292,6 @@ static int cb_pcidda_attach(struct comedi_device *dev,
 /*
  * Probe the device to determine what device in the series it is.
  */
-	printk("\n");
 
 	for_each_pci_dev(pcidev) {
 		if (pcidev->vendor == PCI_VENDOR_ID_CB) {
@@ -312,22 +310,21 @@ static int cb_pcidda_attach(struct comedi_device *dev,
 		}
 	}
 	if (!pcidev) {
-		printk
-		    ("Not a ComputerBoards/MeasurementComputing card on requested position\n");
+		dev_err(dev->hw_dev, "Not a ComputerBoards/MeasurementComputing card on requested position\n");
 		return -EIO;
 	}
 found:
 	devpriv->pci_dev = pcidev;
 	dev->board_ptr = cb_pcidda_boards + index;
 	/*  "thisboard" macro can be used from here. */
-	printk("Found %s at requested position\n", thisboard->name);
+	dev_dbg(dev->hw_dev, "Found %s at requested position\n",
+		thisboard->name);
 
 	/*
 	 * Enable PCI device and request regions.
 	 */
 	if (comedi_pci_enable(pcidev, thisboard->name)) {
-		printk
-		    ("cb_pcidda: failed to enable PCI device and request regions\n");
+		dev_err(dev->hw_dev, "cb_pcidda: failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
 
@@ -377,12 +374,11 @@ found:
 	s = dev->subdevices + 2;
 	subdev_8255_init(dev, s, NULL, devpriv->digitalio + PORT2A);
 
-	printk(" eeprom:");
+	dev_dbg(dev->hw_dev, "eeprom:\n");
 	for (index = 0; index < EEPROM_SIZE; index++) {
 		devpriv->eeprom_data[index] = cb_pcidda_read_eeprom(dev, index);
-		printk(" %i:0x%x ", index, devpriv->eeprom_data[index]);
+		dev_dbg(dev->hw_dev, "%i:0x%x\n", index, devpriv->eeprom_data[index]);
 	}
-	printk("\n");
 
 	/*  set calibrations dacs */
 	for (index = 0; index < thisboard->ao_chans; index++)
@@ -417,8 +413,6 @@ static int cb_pcidda_detach(struct comedi_device *dev)
 		subdev_8255_cleanup(dev, dev->subdevices + 2);
 	}
 
-	printk("comedi%d: cb_pcidda: remove\n", dev->minor);
-
 	return 0;
 }
 
diff --git a/drivers/staging/comedi/drivers/cb_pcidio.c b/drivers/staging/comedi/drivers/cb_pcidio.c
index 79477a595ef9..8f3215239a15 100644
--- a/drivers/staging/comedi/drivers/cb_pcidio.c
+++ b/drivers/staging/comedi/drivers/cb_pcidio.c
@@ -184,8 +184,6 @@ static int pcidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	int index;
 	int i;
 
-	printk("comedi%d: cb_pcidio: \n", dev->minor);
-
 /*
  * Allocate the private structure area.  alloc_private() is a
  * convenient macro defined in comedidev.h.
@@ -223,8 +221,7 @@ static int pcidio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		}
 	}
 
-	printk("No supported ComputerBoards/MeasurementComputing card found on "
-	       "requested position\n");
+	dev_err(dev->hw_dev, "No supported ComputerBoards/MeasurementComputing card found on requested position\n");
 	return -EIO;
 
 found:
@@ -236,14 +233,12 @@ found:
 	dev->board_name = thisboard->name;
 
 	devpriv->pci_dev = pcidev;
-	printk("Found %s on bus %i, slot %i\n", thisboard->name,
-	       devpriv->pci_dev->bus->number,
-	       PCI_SLOT(devpriv->pci_dev->devfn));
-	if (comedi_pci_enable(pcidev, thisboard->name)) {
-		printk
-		    ("cb_pcidio: failed to enable PCI device and request regions\n");
+	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n", thisboard->name,
+		devpriv->pci_dev->bus->number,
+		PCI_SLOT(devpriv->pci_dev->devfn));
+	if (comedi_pci_enable(pcidev, thisboard->name))
 		return -EIO;
-	}
+
 	devpriv->dio_reg_base
 	    =
 	    pci_resource_start(devpriv->pci_dev,
@@ -259,11 +254,10 @@ found:
 	for (i = 0; i < thisboard->n_8255; i++) {
 		subdev_8255_init(dev, dev->subdevices + i,
 				 NULL, devpriv->dio_reg_base + i * 4);
-		printk(" subdev %d: base = 0x%lx\n", i,
-		       devpriv->dio_reg_base + i * 4);
+		dev_dbg(dev->hw_dev, "subdev %d: base = 0x%lx\n", i,
+			devpriv->dio_reg_base + i * 4);
 	}
 
-	printk("attached\n");
 	return 1;
 }
 
@@ -277,7 +271,6 @@ found:
  */
 static int pcidio_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: cb_pcidio: remove\n", dev->minor);
 	if (devpriv) {
 		if (devpriv->pci_dev) {
 			if (devpriv->dio_reg_base)
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index b1b832b65bc1..8ba694263bd3 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -212,8 +212,6 @@ static int cb_pcimdas_attach(struct comedi_device *dev,
 	int index;
 	/* int i; */
 
-	printk("comedi%d: cb_pcimdas: ", dev->minor);
-
 /*
  * Allocate the private structure area.
  */
@@ -223,7 +221,6 @@ static int cb_pcimdas_attach(struct comedi_device *dev,
 /*
  * Probe the device to determine what device in the series it is.
  */
-	printk("\n");
 
 	for_each_pci_dev(pcidev) {
 		/*  is it not a computer boards card? */
@@ -248,26 +245,26 @@ static int cb_pcimdas_attach(struct comedi_device *dev,
 		}
 	}
 
-	printk("No supported ComputerBoards/MeasurementComputing card found on "
-	       "requested position\n");
+	dev_err(dev->hw_dev, "No supported ComputerBoards/MeasurementComputing card found on requested position\n");
 	return -EIO;
 
 found:
 
-	printk("Found %s on bus %i, slot %i\n", cb_pcimdas_boards[index].name,
-	       pcidev->bus->number, PCI_SLOT(pcidev->devfn));
+	dev_dbg(dev->hw_dev, "Found %s on bus %i, slot %i\n",
+		cb_pcimdas_boards[index].name, pcidev->bus->number,
+		PCI_SLOT(pcidev->devfn));
 
 	/*  Warn about non-tested features */
 	switch (thisboard->device_id) {
 	case 0x56:
 		break;
 	default:
-		printk("THIS CARD IS UNSUPPORTED.\n"
-		       "PLEASE REPORT USAGE TO <mocelet@sucs.org>\n");
+		dev_dbg(dev->hw_dev, "THIS CARD IS UNSUPPORTED.\n"
+			"PLEASE REPORT USAGE TO <mocelet@sucs.org>\n");
 	}
 
 	if (comedi_pci_enable(pcidev, "cb_pcimdas")) {
-		printk(" Failed to enable PCI device and request regions\n");
+		dev_err(dev->hw_dev, "Failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
 
@@ -277,13 +274,11 @@ found:
 	devpriv->BADR3 = pci_resource_start(devpriv->pci_dev, 3);
 	devpriv->BADR4 = pci_resource_start(devpriv->pci_dev, 4);
 
-#ifdef CBPCIMDAS_DEBUG
-	printk("devpriv->BADR0 = 0x%lx\n", devpriv->BADR0);
-	printk("devpriv->BADR1 = 0x%lx\n", devpriv->BADR1);
-	printk("devpriv->BADR2 = 0x%lx\n", devpriv->BADR2);
-	printk("devpriv->BADR3 = 0x%lx\n", devpriv->BADR3);
-	printk("devpriv->BADR4 = 0x%lx\n", devpriv->BADR4);
-#endif
+	dev_dbg(dev->hw_dev, "devpriv->BADR0 = 0x%lx\n", devpriv->BADR0);
+	dev_dbg(dev->hw_dev, "devpriv->BADR1 = 0x%lx\n", devpriv->BADR1);
+	dev_dbg(dev->hw_dev, "devpriv->BADR2 = 0x%lx\n", devpriv->BADR2);
+	dev_dbg(dev->hw_dev, "devpriv->BADR3 = 0x%lx\n", devpriv->BADR3);
+	dev_dbg(dev->hw_dev, "devpriv->BADR4 = 0x%lx\n", devpriv->BADR4);
 
 /* Dont support IRQ yet */
 /*  get irq */
@@ -333,8 +328,6 @@ found:
 	else
 		s->type = COMEDI_SUBD_UNUSED;
 
-	printk("attached\n");
-
 	return 1;
 }
 
@@ -348,16 +341,19 @@ found:
  */
 static int cb_pcimdas_detach(struct comedi_device *dev)
 {
-#ifdef CBPCIMDAS_DEBUG
 	if (devpriv) {
-		printk("devpriv->BADR0 = 0x%lx\n", devpriv->BADR0);
-		printk("devpriv->BADR1 = 0x%lx\n", devpriv->BADR1);
-		printk("devpriv->BADR2 = 0x%lx\n", devpriv->BADR2);
-		printk("devpriv->BADR3 = 0x%lx\n", devpriv->BADR3);
-		printk("devpriv->BADR4 = 0x%lx\n", devpriv->BADR4);
+		dev_dbg(dev->hw_dev, "devpriv->BADR0 = 0x%lx\n",
+			devpriv->BADR0);
+		dev_dbg(dev->hw_dev, "devpriv->BADR1 = 0x%lx\n",
+			devpriv->BADR1);
+		dev_dbg(dev->hw_dev, "devpriv->BADR2 = 0x%lx\n",
+			devpriv->BADR2);
+		dev_dbg(dev->hw_dev, "devpriv->BADR3 = 0x%lx\n",
+			devpriv->BADR3);
+		dev_dbg(dev->hw_dev, "devpriv->BADR4 = 0x%lx\n",
+			devpriv->BADR4);
 	}
-#endif
-	printk("comedi%d: cb_pcimdas: remove\n", dev->minor);
+
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (devpriv) {
diff --git a/drivers/staging/comedi/drivers/cb_pcimdda.c b/drivers/staging/comedi/drivers/cb_pcimdda.c
index 8c981a89ab63..40bddfa22220 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdda.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdda.c
@@ -105,7 +105,8 @@ struct board_struct {
 	int ao_bits;
 	int dio_chans;
 	int dio_method;
-	int dio_offset;		/* how many bytes into the BADR are the DIO ports */
+	/* how many bytes into the BADR are the DIO ports */
+	int dio_offset;
 	int regs_badrindex;	/* IO Region for the control, analog output,
 				   and DIO registers */
 	int reg_sz;		/* number of bytes of registers in io region */
@@ -144,17 +145,18 @@ static const struct board_struct boards[] = {
 /* Please add your PCI vendor ID to comedidev.h, and it will be forwarded
  * upstream. */
 static DEFINE_PCI_DEVICE_TABLE(pci_table) = {
-	{
-	PCI_VENDOR_ID_COMPUTERBOARDS, PCI_ID_PCIM_DDA06_16, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, PCI_ID_PCIM_DDA06_16) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, pci_table);
 
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.  */
+/*
+ * this structure is for data unique to this hardware driver.  If
+ * several hardware drivers keep similar information in this structure,
+ * feel free to suggest moving the variable to the struct comedi_device
+ * struct.
+ */
 struct board_private_struct {
 	unsigned long registers;	/* set by probe */
 	unsigned long dio_registers;
@@ -335,7 +337,10 @@ static int attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (thisboard->dio_chans) {
 		switch (thisboard->dio_method) {
 		case DIO_8255:
-			/* this is a straight 8255, so register us with the 8255 driver */
+			/*
+			 * this is a straight 8255, so register us with
+			 * the 8255 driver
+			 */
 			subdev_8255_init(dev, s, NULL, devpriv->dio_registers);
 			devpriv->attached_to_8255 = 1;
 			break;
@@ -436,8 +441,11 @@ static int ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	for (i = 0; i < insn->n; i++) {
 		inw(devpriv->registers + chan * 2);
-		/* should I set data[i] to the result of the actual read on the register
-		   or the cached unsigned int in devpriv->ao_readback[]? */
+		/*
+		 * should I set data[i] to the result of the actual read
+		 * on the register or the cached unsigned int in
+		 * devpriv->ao_readback[]?
+		 */
 		data[i] = devpriv->ao_readback[chan];
 	}
 
diff --git a/drivers/staging/comedi/drivers/contec_pci_dio.c b/drivers/staging/comedi/drivers/contec_pci_dio.c
index 871f109bcfa3..e3659bd6e85e 100644
--- a/drivers/staging/comedi/drivers/contec_pci_dio.c
+++ b/drivers/staging/comedi/drivers/contec_pci_dio.c
@@ -57,10 +57,9 @@ static const struct contec_board contec_boards[] = {
 
 #define PCI_DEVICE_ID_PIO1616L 0x8172
 static DEFINE_PCI_DEVICE_TABLE(contec_pci_table) = {
-	{
-	PCI_VENDOR_ID_CONTEC, PCI_DEVICE_ID_PIO1616L, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, PIO1616L}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_CONTEC, PCI_DEVICE_ID_PIO1616L),
+		.driver_data = PIO1616L },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, contec_pci_table);
@@ -197,8 +196,8 @@ static int contec_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_insn *insn, unsigned int *data)
 {
 
-	printk("contec_do_insn_bits called\n");
-	printk(" data: %d %d\n", data[0], data[1]);
+	dev_dbg(dev->hw_dev, "contec_do_insn_bits called\n");
+	dev_dbg(dev->hw_dev, "data: %d %d\n", data[0], data[1]);
 
 	if (insn->n != 2)
 		return -EINVAL;
@@ -206,8 +205,8 @@ static int contec_do_insn_bits(struct comedi_device *dev,
 	if (data[0]) {
 		s->state &= ~data[0];
 		s->state |= data[0] & data[1];
-		printk("  out: %d on %lx\n", s->state,
-		       dev->iobase + thisboard->out_offs);
+		dev_dbg(dev->hw_dev, "out: %d on %lx\n", s->state,
+			dev->iobase + thisboard->out_offs);
 		outw(s->state, dev->iobase + thisboard->out_offs);
 	}
 	return 2;
@@ -218,8 +217,8 @@ static int contec_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_insn *insn, unsigned int *data)
 {
 
-	printk("contec_di_insn_bits called\n");
-	printk(" data: %d %d\n", data[0], data[1]);
+	dev_dbg(dev->hw_dev, "contec_di_insn_bits called\n");
+	dev_dbg(dev->hw_dev, "data: %d %d\n", data[0], data[1]);
 
 	if (insn->n != 2)
 		return -EINVAL;
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index 82be77daa7d7..e61c6a8f2857 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -325,9 +325,8 @@ static const struct daq200_boardtype boardtypes[] = {
 #define this_board ((const struct daq200_boardtype *)dev->board_ptr)
 
 static DEFINE_PCI_DEVICE_TABLE(daqboard2000_pci_table) = {
-	{
-	0x1616, 0x0409, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(0x1616, 0x0409) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, daqboard2000_pci_table);
@@ -430,16 +429,14 @@ static int daqboard2000_ai_insn_read(struct comedi_device *dev,
 		/* Enable reading from the scanlist FIFO */
 		fpga->acqControl = DAQBOARD2000_SeqStartScanList;
 		for (timeout = 0; timeout < 20; timeout++) {
-			if (fpga->acqControl & DAQBOARD2000_AcqConfigPipeFull) {
+			if (fpga->acqControl & DAQBOARD2000_AcqConfigPipeFull)
 				break;
-			}
 			/* udelay(2); */
 		}
 		fpga->acqControl = DAQBOARD2000_AdcPacerEnable;
 		for (timeout = 0; timeout < 20; timeout++) {
-			if (fpga->acqControl & DAQBOARD2000_AcqLogicScanning) {
+			if (fpga->acqControl & DAQBOARD2000_AcqLogicScanning)
 				break;
-			}
 			/* udelay(2); */
 		}
 		for (timeout = 0; timeout < 20; timeout++) {
@@ -465,9 +462,8 @@ static int daqboard2000_ao_insn_read(struct comedi_device *dev,
 	int i;
 	int chan = CR_CHAN(insn->chanspec);
 
-	for (i = 0; i < insn->n; i++) {
+	for (i = 0; i < insn->n; i++)
 		data[i] = devpriv->ao_readback[chan];
-	}
 
 	return i;
 }
@@ -490,9 +486,8 @@ static int daqboard2000_ao_insn_write(struct comedi_device *dev,
 		/* fpga->dacControl = (chan + 2) * 0x0010 | 0x0001; udelay(1000); */
 		fpga->dacSetting[chan] = data[i];
 		for (timeout = 0; timeout < 20; timeout++) {
-			if ((fpga->dacControl & ((chan + 1) * 0x0010)) == 0) {
+			if ((fpga->dacControl & ((chan + 1) * 0x0010)) == 0)
 				break;
-			}
 			/* udelay(2); */
 		}
 		devpriv->ao_readback[chan] = data[i];
@@ -507,7 +502,7 @@ static int daqboard2000_ao_insn_write(struct comedi_device *dev,
 
 static void daqboard2000_resetLocalBus(struct comedi_device *dev)
 {
-	printk("daqboard2000_resetLocalBus\n");
+	dev_dbg(dev->hw_dev, "daqboard2000_resetLocalBus\n");
 	writel(DAQBOARD2000_SECRLocalBusHi, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRLocalBusLo, devpriv->plx + 0x6c);
@@ -516,7 +511,7 @@ static void daqboard2000_resetLocalBus(struct comedi_device *dev)
 
 static void daqboard2000_reloadPLX(struct comedi_device *dev)
 {
-	printk("daqboard2000_reloadPLX\n");
+	dev_dbg(dev->hw_dev, "daqboard2000_reloadPLX\n");
 	writel(DAQBOARD2000_SECRReloadLo, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRReloadHi, devpriv->plx + 0x6c);
@@ -527,7 +522,7 @@ static void daqboard2000_reloadPLX(struct comedi_device *dev)
 
 static void daqboard2000_pulseProgPin(struct comedi_device *dev)
 {
-	printk("daqboard2000_pulseProgPin 1\n");
+	dev_dbg(dev->hw_dev, "daqboard2000_pulseProgPin 1\n");
 	writel(DAQBOARD2000_SECRProgPinHi, devpriv->plx + 0x6c);
 	udelay(10000);
 	writel(DAQBOARD2000_SECRProgPinLo, devpriv->plx + 0x6c);
@@ -579,14 +574,14 @@ static int initialize_daqboard2000(struct comedi_device *dev,
 	secr = readl(devpriv->plx + 0x6c);
 	if (!(secr & DAQBOARD2000_EEPROM_PRESENT)) {
 #ifdef DEBUG_EEPROM
-		printk("no serial eeprom\n");
+		dev_dbg(dev->hw_dev, "no serial eeprom\n");
 #endif
 		return -EIO;
 	}
 
 	for (retry = 0; retry < 3; retry++) {
 #ifdef DEBUG_EEPROM
-		printk("Programming EEPROM try %x\n", retry);
+		dev_dbg(dev->hw_dev, "Programming EEPROM try %x\n", retry);
 #endif
 
 		daqboard2000_resetLocalBus(dev);
@@ -597,7 +592,8 @@ static int initialize_daqboard2000(struct comedi_device *dev,
 				if (cpld_array[i] == 0xff
 				    && cpld_array[i + 1] == 0x20) {
 #ifdef DEBUG_EEPROM
-					printk("Preamble found at %d\n", i);
+					dev_dbg(dev->hw_dev, "Preamble found at %d\n",
+						i);
 #endif
 					break;
 				}
@@ -605,13 +601,12 @@ static int initialize_daqboard2000(struct comedi_device *dev,
 			for (; i < len; i += 2) {
 				int data =
 				    (cpld_array[i] << 8) + cpld_array[i + 1];
-				if (!daqboard2000_writeCPLD(dev, data)) {
+				if (!daqboard2000_writeCPLD(dev, data))
 					break;
-				}
 			}
 			if (i >= len) {
 #ifdef DEBUG_EEPROM
-				printk("Programmed\n");
+				dev_dbg(dev->hw_dev, "Programmed\n");
 #endif
 				daqboard2000_resetLocalBus(dev);
 				daqboard2000_reloadPLX(dev);
@@ -658,9 +653,8 @@ static void daqboard2000_activateReferenceDacs(struct comedi_device *dev)
 	/*  Set the + reference dac value in the FPGA */
 	fpga->refDacs = 0x80 | DAQBOARD2000_PosRefDacSelect;
 	for (timeout = 0; timeout < 20; timeout++) {
-		if ((fpga->dacControl & DAQBOARD2000_RefBusy) == 0) {
+		if ((fpga->dacControl & DAQBOARD2000_RefBusy) == 0)
 			break;
-		}
 		udelay(2);
 	}
 /*  printk("DAQBOARD2000_PosRefDacSelect %d\n", timeout);*/
@@ -668,9 +662,8 @@ static void daqboard2000_activateReferenceDacs(struct comedi_device *dev)
 	/*  Set the - reference dac value in the FPGA */
 	fpga->refDacs = 0x80 | DAQBOARD2000_NegRefDacSelect;
 	for (timeout = 0; timeout < 20; timeout++) {
-		if ((fpga->dacControl & DAQBOARD2000_RefBusy) == 0) {
+		if ((fpga->dacControl & DAQBOARD2000_RefBusy) == 0)
 			break;
-		}
 		udelay(2);
 	}
 /*  printk("DAQBOARD2000_NegRefDacSelect %d\n", timeout);*/
@@ -737,15 +730,13 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	unsigned int aux_len;
 	int bus, slot;
 
-	printk("comedi%d: daqboard2000:", dev->minor);
-
 	bus = it->options[0];
 	slot = it->options[1];
 
 	result = alloc_private(dev, sizeof(struct daqboard2000_private));
-	if (result < 0) {
+	if (result < 0)
 		return -ENOMEM;
-	}
+
 	for (card = pci_get_device(0x1616, 0x0409, NULL);
 	     card != NULL; card = pci_get_device(0x1616, 0x0409, card)) {
 		if (bus || slot) {
@@ -759,10 +750,10 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	}
 	if (!card) {
 		if (bus || slot)
-			printk(" no daqboard2000 found at bus/slot: %d/%d\n",
-			       bus, slot);
+			dev_err(dev->hw_dev, "no daqboard2000 found at bus/slot: %d/%d\n",
+				bus, slot);
 		else
-			printk(" no daqboard2000 found\n");
+			dev_err(dev->hw_dev, "no daqboard2000 found\n");
 		return -EIO;
 	} else {
 		u32 id;
@@ -772,7 +763,8 @@ static int daqboard2000_attach(struct comedi_device *dev,
 		      subsystem_device << 16) | card->subsystem_vendor;
 		for (i = 0; i < n_boardtypes; i++) {
 			if (boardtypes[i].id == id) {
-				printk(" %s", boardtypes[i].name);
+				dev_dbg(dev->hw_dev, "%s\n",
+					boardtypes[i].name);
 				dev->board_ptr = boardtypes + i;
 			}
 		}
@@ -786,7 +778,7 @@ static int daqboard2000_attach(struct comedi_device *dev,
 
 	result = comedi_pci_enable(card, "daqboard2000");
 	if (result < 0) {
-		printk(" failed to enable PCI device and request regions\n");
+		dev_err(dev->hw_dev, "failed to enable PCI device and request regions\n");
 		return -EIO;
 	}
 	devpriv->got_regions = 1;
@@ -794,9 +786,8 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	    ioremap(pci_resource_start(card, 0), DAQBOARD2000_PLX_SIZE);
 	devpriv->daq =
 	    ioremap(pci_resource_start(card, 2), DAQBOARD2000_DAQ_SIZE);
-	if (!devpriv->plx || !devpriv->daq) {
+	if (!devpriv->plx || !devpriv->daq)
 		return -ENOMEM;
-	}
 
 	result = alloc_subdevices(dev, 3);
 	if (result < 0)
@@ -817,7 +808,7 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	if (aux_data && aux_len) {
 		result = initialize_daqboard2000(dev, aux_data, aux_len);
 	} else {
-		printk("no FPGA initialization code, aborting\n");
+		dev_dbg(dev->hw_dev, "no FPGA initialization code, aborting\n");
 		result = -EIO;
 	}
 	if (result < 0)
@@ -857,30 +848,26 @@ static int daqboard2000_attach(struct comedi_device *dev,
 	result = subdev_8255_init(dev, s, daqboard2000_8255_cb,
 				  (unsigned long)(dev->iobase + 0x40));
 
-	printk("\n");
 out:
 	return result;
 }
 
 static int daqboard2000_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: daqboard2000: remove\n", dev->minor);
-
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 2);
 
-	if (dev->irq) {
+	if (dev->irq)
 		free_irq(dev->irq, dev);
-	}
+
 	if (devpriv) {
 		if (devpriv->daq)
 			iounmap(devpriv->daq);
 		if (devpriv->plx)
 			iounmap(devpriv->plx);
 		if (devpriv->pci_dev) {
-			if (devpriv->got_regions) {
+			if (devpriv->got_regions)
 				comedi_pci_disable(devpriv->pci_dev);
-			}
 			pci_dev_put(devpriv->pci_dev);
 		}
 	}
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index 3141dc80fe74..c2dd0ed36a73 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -506,10 +506,8 @@ struct das08_board_struct das08_cs_boards[NUM_DAS08_CS_BOARDS] = {
 
 #ifdef CONFIG_COMEDI_PCI
 static DEFINE_PCI_DEVICE_TABLE(das08_pci_table) = {
-	{
-	PCI_VENDOR_ID_COMPUTERBOARDS, PCI_DEVICE_ID_PCIDAS08,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_COMPUTERBOARDS, PCI_DEVICE_ID_PCIDAS08) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, das08_pci_table);
diff --git a/drivers/staging/comedi/drivers/das08_cs.c b/drivers/staging/comedi/drivers/das08_cs.c
index 6d91d3028178..4ad398aad72c 100644
--- a/drivers/staging/comedi/drivers/das08_cs.c
+++ b/drivers/staging/comedi/drivers/das08_cs.c
@@ -79,22 +79,20 @@ static int das08_cs_attach(struct comedi_device *dev,
 	if (ret < 0)
 		return ret;
 
-	printk("comedi%d: das08_cs: ", dev->minor);
+	dev_info(dev->hw_dev, "comedi%d: das08_cs:\n", dev->minor);
 	/*  deal with a pci board */
 
 	if (thisboard->bustype == pcmcia) {
 		if (link == NULL) {
-			printk(" no pcmcia cards found\n");
+			dev_err(dev->hw_dev, "no pcmcia cards found\n");
 			return -EIO;
 		}
 		iobase = link->resource[0]->start;
 	} else {
-		printk(" bug! board does not have PCMCIA bustype\n");
+		dev_err(dev->hw_dev, "bug! board does not have PCMCIA bustype\n");
 		return -EINVAL;
 	}
 
-	printk("\n");
-
 	return das08_common_attach(dev, iobase);
 }
 
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index a5ce3b2abe4a..5376e718e3d7 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -384,20 +384,20 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 	byte = 0;
 	/* if we are using external start trigger (also board dislikes having
 	 * both start and conversion triggers external simultaneously) */
-	if (cmd->start_src == TRIG_EXT && cmd->convert_src != TRIG_EXT) {
+	if (cmd->start_src == TRIG_EXT && cmd->convert_src != TRIG_EXT)
 		byte |= EXT_TRIG_BIT;
-	}
+
 	outb(byte, dev->iobase + DAS16M1_CS);
 	/* clear interrupt bit */
 	outb(0, dev->iobase + DAS16M1_CLEAR_INTR);
 
 	/* enable interrupts and internal pacer */
 	devpriv->control_state &= ~PACER_MASK;
-	if (cmd->convert_src == TRIG_TIMER) {
+	if (cmd->convert_src == TRIG_TIMER)
 		devpriv->control_state |= INT_PACER;
-	} else {
+	else
 		devpriv->control_state |= EXT_PACER;
-	}
+
 	devpriv->control_state |= INTE;
 	outb(devpriv->control_state, dev->iobase + DAS16M1_INTR_CONTROL);
 
@@ -531,9 +531,8 @@ static void munge_sample_array(short *array, unsigned int num_elements)
 {
 	unsigned int i;
 
-	for (i = 0; i < num_elements; i++) {
+	for (i = 0; i < num_elements; i++)
 		array[i] = munge_sample(array[i]);
-	}
 }
 
 static void das16m1_handler(struct comedi_device *dev, unsigned int status)
@@ -668,25 +667,20 @@ static int das16m1_attach(struct comedi_device *dev,
 
 	iobase = it->options[0];
 
-	printk("comedi%d: das16m1:", dev->minor);
-
 	ret = alloc_private(dev, sizeof(struct das16m1_private_struct));
 	if (ret < 0)
 		return ret;
 
 	dev->board_name = thisboard->name;
 
-	printk(" io 0x%lx-0x%lx 0x%lx-0x%lx",
-	       iobase, iobase + DAS16M1_SIZE,
-	       iobase + DAS16M1_82C55, iobase + DAS16M1_82C55 + DAS16M1_SIZE2);
 	if (!request_region(iobase, DAS16M1_SIZE, driver_das16m1.driver_name)) {
-		printk(" I/O port conflict\n");
+		comedi_error(dev, "I/O port conflict\n");
 		return -EIO;
 	}
 	if (!request_region(iobase + DAS16M1_82C55, DAS16M1_SIZE2,
 			    driver_das16m1.driver_name)) {
 		release_region(iobase, DAS16M1_SIZE);
-		printk(" I/O port conflict\n");
+		comedi_error(dev, "I/O port conflict\n");
 		return -EIO;
 	}
 	dev->iobase = iobase;
@@ -697,17 +691,17 @@ static int das16m1_attach(struct comedi_device *dev,
 	if (das16m1_irq_bits(irq) >= 0) {
 		ret = request_irq(irq, das16m1_interrupt, 0,
 				  driver_das16m1.driver_name, dev);
-		if (ret < 0) {
-			printk(", irq unavailable\n");
+		if (ret < 0)
 			return ret;
-		}
 		dev->irq = irq;
-		printk(", irq %u\n", irq);
+		printk
+		    ("irq %u\n", irq);
 	} else if (irq == 0) {
-		printk(", no irq\n");
+		printk
+		    (", no irq\n");
 	} else {
-		printk(", invalid irq\n"
-		       " valid irqs are 2, 3, 5, 7, 10, 11, 12, or 15\n");
+		comedi_error(dev, "invalid irq\n"
+			     " valid irqs are 2, 3, 5, 7, 10, 11, 12, or 15\n");
 		return -EINVAL;
 	}
 
@@ -771,7 +765,6 @@ static int das16m1_attach(struct comedi_device *dev,
 
 static int das16m1_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: das16m1: remove\n", dev->minor);
 
 /* das16m1_reset(dev); */
 
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index a6df30b7fd7c..99ada5a53b9e 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -573,22 +573,23 @@ static int das1800_init_dma(struct comedi_device *dev, unsigned int dma0,
 			devpriv->dma_bits |= DMA_CH7_CH5;
 			break;
 		default:
-			printk(" only supports dma channels 5 through 7\n"
-			       " Dual dma only allows the following combinations:\n"
-			       " dma 5,6 / 6,7 / or 7,5\n");
+			dev_err(dev->hw_dev, " only supports dma channels 5 through 7\n"
+				" Dual dma only allows the following combinations:\n"
+				" dma 5,6 / 6,7 / or 7,5\n");
 			return -EINVAL;
 			break;
 		}
 		if (request_dma(dma0, driver_das1800.driver_name)) {
-			printk(" failed to allocate dma channel %i\n", dma0);
+			dev_err(dev->hw_dev, "failed to allocate dma channel %i\n",
+				dma0);
 			return -EINVAL;
 		}
 		devpriv->dma0 = dma0;
 		devpriv->dma_current = dma0;
 		if (dma1) {
 			if (request_dma(dma1, driver_das1800.driver_name)) {
-				printk(" failed to allocate dma channel %i\n",
-				       dma1);
+				dev_err(dev->hw_dev, "failed to allocate dma channel %i\n",
+					dma1);
 				return -EINVAL;
 			}
 			devpriv->dma1 = dma1;
@@ -631,20 +632,20 @@ static int das1800_attach(struct comedi_device *dev,
 	if (alloc_private(dev, sizeof(struct das1800_private)) < 0)
 		return -ENOMEM;
 
-	printk("comedi%d: %s: io 0x%lx", dev->minor, driver_das1800.driver_name,
-	       iobase);
+	printk(KERN_DEBUG "comedi%d: %s: io 0x%lx", dev->minor,
+	       driver_das1800.driver_name, iobase);
 	if (irq) {
-		printk(", irq %u", irq);
+		printk(KERN_CONT ", irq %u", irq);
 		if (dma0) {
-			printk(", dma %u", dma0);
+			printk(KERN_CONT ", dma %u", dma0);
 			if (dma1)
-				printk(" and %u", dma1);
+				printk(KERN_CONT " and %u", dma1);
 		}
 	}
-	printk("\n");
+	printk(KERN_CONT "\n");
 
 	if (iobase == 0) {
-		printk(" io base address required\n");
+		dev_err(dev->hw_dev, "io base address required\n");
 		return -EINVAL;
 	}
 
@@ -659,7 +660,7 @@ static int das1800_attach(struct comedi_device *dev,
 
 	board = das1800_probe(dev);
 	if (board < 0) {
-		printk(" unable to determine board type\n");
+		dev_err(dev->hw_dev, "unable to determine board type\n");
 		return -ENODEV;
 	}
 
@@ -683,7 +684,8 @@ static int das1800_attach(struct comedi_device *dev,
 	if (irq) {
 		if (request_irq(irq, das1800_interrupt, 0,
 				driver_das1800.driver_name, dev)) {
-			printk(" unable to allocate irq %u\n", irq);
+			dev_dbg(dev->hw_dev, "unable to allocate irq %u\n",
+				irq);
 			return -EINVAL;
 		}
 	}
@@ -712,7 +714,7 @@ static int das1800_attach(struct comedi_device *dev,
 		devpriv->irq_dma_bits |= 0x38;
 		break;
 	default:
-		printk(" irq out of range\n");
+		dev_err(dev->hw_dev, "irq out of range\n");
 		return -EINVAL;
 		break;
 	}
@@ -813,8 +815,8 @@ static int das1800_detach(struct comedi_device *dev)
 		kfree(devpriv->ai_buf1);
 	}
 
-	printk("comedi%d: %s: remove\n", dev->minor,
-	       driver_das1800.driver_name);
+	dev_dbg(dev->hw_dev, "comedi%d: %s: remove\n", dev->minor,
+		driver_das1800.driver_name);
 
 	return 0;
 };
@@ -833,8 +835,8 @@ static int das1800_probe(struct comedi_device *dev)
 	case 0x3:
 		if (board == das1801st_da || board == das1802st_da ||
 		    board == das1701st_da || board == das1702st_da) {
-			printk(" Board model: %s\n",
-			       das1800_boards[board].name);
+			dev_dbg(dev->hw_dev, "Board model: %s\n",
+				das1800_boards[board].name);
 			return board;
 		}
 		printk
@@ -843,8 +845,8 @@ static int das1800_probe(struct comedi_device *dev)
 		break;
 	case 0x4:
 		if (board == das1802hr_da || board == das1702hr_da) {
-			printk(" Board model: %s\n",
-			       das1800_boards[board].name);
+			dev_dbg(dev->hw_dev, "Board model: %s\n",
+				das1800_boards[board].name);
 			return board;
 		}
 		printk
@@ -854,8 +856,8 @@ static int das1800_probe(struct comedi_device *dev)
 	case 0x5:
 		if (board == das1801ao || board == das1802ao ||
 		    board == das1701ao || board == das1702ao) {
-			printk(" Board model: %s\n",
-			       das1800_boards[board].name);
+			dev_dbg(dev->hw_dev, "Board model: %s\n",
+				das1800_boards[board].name);
 			return board;
 		}
 		printk
@@ -864,18 +866,19 @@ static int das1800_probe(struct comedi_device *dev)
 		break;
 	case 0x6:
 		if (board == das1802hr || board == das1702hr) {
-			printk(" Board model: %s\n",
-			       das1800_boards[board].name);
+			dev_dbg(dev->hw_dev, "Board model: %s\n",
+				das1800_boards[board].name);
 			return board;
 		}
-		printk(" Board model (probed, not recommended): das-1802hr\n");
+		printk
+		    (" Board model (probed, not recommended): das-1802hr\n");
 		return das1802hr;
 		break;
 	case 0x7:
 		if (board == das1801st || board == das1802st ||
 		    board == das1701st || board == das1702st) {
-			printk(" Board model: %s\n",
-			       das1800_boards[board].name);
+			dev_dbg(dev->hw_dev, "Board model: %s\n",
+				das1800_boards[board].name);
 			return board;
 		}
 		printk
@@ -884,8 +887,8 @@ static int das1800_probe(struct comedi_device *dev)
 		break;
 	case 0x8:
 		if (board == das1801hc || board == das1802hc) {
-			printk(" Board model: %s\n",
-			       das1800_boards[board].name);
+			dev_dbg(dev->hw_dev, "Board model: %s\n",
+				das1800_boards[board].name);
 			return board;
 		}
 		printk
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index 6328f5280b66..f25684145e84 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -171,7 +171,7 @@ static irqreturn_t intr_handler(int irq, void *d)
 	struct comedi_subdevice *s = dev->subdevices;
 
 	if (!dev->attached || devpriv->das6402_ignoreirq) {
-		printk("das6402: BUG: spurious interrupt\n");
+		dev_warn(dev->hw_dev, "BUG: spurious interrupt\n");
 		return IRQ_HANDLED;
 	}
 #ifdef DEBUG
@@ -228,9 +228,7 @@ static int das6402_ai_cancel(struct comedi_device *dev,
 	 */
 
 	devpriv->das6402_ignoreirq = 1;
-#ifdef DEBUG
-	printk("das6402: Stopping acquisition\n");
-#endif
+	dev_dbg(dev->hw_dev, "Stopping acquisition\n");
 	devpriv->das6402_ignoreirq = 1;
 	outb_p(0x02, dev->iobase + 10);	/* disable external trigging */
 	outw_p(SCANL, dev->iobase + 2);	/* resets the card fifo */
@@ -246,10 +244,7 @@ static int das6402_ai_mode2(struct comedi_device *dev,
 			    struct comedi_subdevice *s, comedi_trig * it)
 {
 	devpriv->das6402_ignoreirq = 1;
-
-#ifdef DEBUG
-	printk("das6402: Starting acquisition\n");
-#endif
+	dev_dbg(dev->hw_dev, "Starting acquisition\n");
 	outb_p(0x03, dev->iobase + 10);	/* enable external trigging */
 	outw_p(SCANL, dev->iobase + 2);	/* resets the card fifo */
 	outb_p(IRQ | CONVSRC | BURSTEN | INTE, dev->iobase + 9);
@@ -329,10 +324,8 @@ static int das6402_attach(struct comedi_device *dev,
 	if (iobase == 0)
 		iobase = 0x300;
 
-	printk("comedi%d: das6402: 0x%04lx", dev->minor, iobase);
-
 	if (!request_region(iobase, DAS6402_SIZE, "das6402")) {
-		printk(" I/O port conflict\n");
+		dev_err(dev->hw_dev, "I/O port conflict\n");
 		return -EIO;
 	}
 	dev->iobase = iobase;
@@ -340,14 +333,12 @@ static int das6402_attach(struct comedi_device *dev,
 	/* should do a probe here */
 
 	irq = it->options[0];
-	printk(" ( irq = %u )", irq);
+	dev_dbg(dev->hw_dev, "( irq = %u )\n", irq);
 	ret = request_irq(irq, intr_handler, 0, "das6402", dev);
-	if (ret < 0) {
-		printk("irq conflict\n");
+	if (ret < 0)
 		return ret;
-	}
-	dev->irq = irq;
 
+	dev->irq = irq;
 	ret = alloc_private(dev, sizeof(struct das6402_private));
 	if (ret < 0)
 		return ret;
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index 96d41ad76956..6e347b40fe61 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -296,47 +296,47 @@ static int das800_probe(struct comedi_device *dev)
 	switch (id_bits) {
 	case 0x0:
 		if (board == das800) {
-			printk(" Board model: DAS-800\n");
+			dev_dbg(dev->hw_dev, "Board model: DAS-800\n");
 			return board;
 		}
 		if (board == ciodas800) {
-			printk(" Board model: CIO-DAS800\n");
+			dev_dbg(dev->hw_dev, "Board model: CIO-DAS800\n");
 			return board;
 		}
-		printk(" Board model (probed): DAS-800\n");
+		dev_dbg(dev->hw_dev, "Board model (probed): DAS-800\n");
 		return das800;
 		break;
 	case 0x2:
 		if (board == das801) {
-			printk(" Board model: DAS-801\n");
+			dev_dbg(dev->hw_dev, "Board model: DAS-801\n");
 			return board;
 		}
 		if (board == ciodas801) {
-			printk(" Board model: CIO-DAS801\n");
+			dev_dbg(dev->hw_dev, "Board model: CIO-DAS801\n");
 			return board;
 		}
-		printk(" Board model (probed): DAS-801\n");
+		dev_dbg(dev->hw_dev, "Board model (probed): DAS-801\n");
 		return das801;
 		break;
 	case 0x3:
 		if (board == das802) {
-			printk(" Board model: DAS-802\n");
+			dev_dbg(dev->hw_dev, "Board model: DAS-802\n");
 			return board;
 		}
 		if (board == ciodas802) {
-			printk(" Board model: CIO-DAS802\n");
+			dev_dbg(dev->hw_dev, "Board model: CIO-DAS802\n");
 			return board;
 		}
 		if (board == ciodas80216) {
-			printk(" Board model: CIO-DAS802/16\n");
+			dev_dbg(dev->hw_dev, "Board model: CIO-DAS802/16\n");
 			return board;
 		}
-		printk(" Board model (probed): DAS-802\n");
+		dev_dbg(dev->hw_dev, "Board model (probed): DAS-802\n");
 		return das802;
 		break;
 	default:
-		printk(" Board model: probe returned 0x%x (unknown)\n",
-		       id_bits);
+		dev_dbg(dev->hw_dev, "Board model: probe returned 0x%x (unknown)\n",
+			id_bits);
 		return board;
 		break;
 	}
@@ -466,42 +466,43 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	unsigned long irq_flags;
 	int board;
 
-	printk("comedi%d: das800: io 0x%lx", dev->minor, iobase);
+	dev_info(dev->hw_dev, "comedi%d: das800: io 0x%lx\n", dev->minor,
+		 iobase);
 	if (irq)
-		printk(", irq %u", irq);
-	printk("\n");
+		dev_dbg(dev->hw_dev, "irq %u\n", irq);
 
 	/* allocate and initialize dev->private */
 	if (alloc_private(dev, sizeof(struct das800_private)) < 0)
 		return -ENOMEM;
 
 	if (iobase == 0) {
-		printk("io base address required for das800\n");
+		dev_err(dev->hw_dev, "io base address required for das800\n");
 		return -EINVAL;
 	}
 
 	/* check if io addresses are available */
 	if (!request_region(iobase, DAS800_SIZE, "das800")) {
-		printk("I/O port conflict\n");
+		dev_err(dev->hw_dev, "I/O port conflict\n");
 		return -EIO;
 	}
 	dev->iobase = iobase;
 
 	board = das800_probe(dev);
 	if (board < 0) {
-		printk("unable to determine board type\n");
+		dev_dbg(dev->hw_dev, "unable to determine board type\n");
 		return -ENODEV;
 	}
 	dev->board_ptr = das800_boards + board;
 
 	/* grab our IRQ */
 	if (irq == 1 || irq > 7) {
-		printk("irq out of range\n");
+		dev_err(dev->hw_dev, "irq out of range\n");
 		return -EINVAL;
 	}
 	if (irq) {
 		if (request_irq(irq, das800_interrupt, 0, "das800", dev)) {
-			printk("unable to allocate irq %u\n", irq);
+			dev_err(dev->hw_dev, "unable to allocate irq %u\n",
+				irq);
 			return -EINVAL;
 		}
 	}
@@ -557,7 +558,7 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static int das800_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: das800: remove\n", dev->minor);
+	dev_info(dev->hw_dev, "comedi%d: das800: remove\n", dev->minor);
 
 	/* only free stuff if it has been allocated by _attach */
 	if (dev->iobase)
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 6170f7bac46e..0a7979e52999 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -352,7 +352,8 @@ static int dt3k_send_cmd(struct comedi_device *dev, unsigned int cmd)
 	if ((status & DT3000_COMPLETION_MASK) == DT3000_NOERROR)
 		return 0;
 
-	printk("dt3k_send_cmd() timeout/error status=0x%04x\n", status);
+	dev_dbg(dev->hw_dev, "dt3k_send_cmd() timeout/error status=0x%04x\n",
+		status);
 
 	return -ETIME;
 }
@@ -383,7 +384,7 @@ static void dt3k_writesingle(struct comedi_device *dev, unsigned int subsys,
 	dt3k_send_cmd(dev, CMD_WRITESINGLE);
 }
 
-static int debug_n_ints = 0;
+static int debug_n_ints;
 
 /* FIXME! Assumes shared interrupt is for this card. */
 /* What's this debug_n_ints stuff? Obviously needs some work... */
@@ -429,12 +430,12 @@ static char *intr_flags[] = {
 static void debug_intr_flags(unsigned int flags)
 {
 	int i;
-	printk("dt3k: intr_flags:");
+	printk(KERN_DEBUG "dt3k: intr_flags:");
 	for (i = 0; i < 8; i++) {
 		if (flags & (1 << i))
-			printk(" %s", intr_flags[i]);
+			printk(KERN_CONT " %s", intr_flags[i]);
 	}
-	printk("\n");
+	printk(KERN_CONT "\n");
 }
 #endif
 
@@ -452,7 +453,7 @@ static void dt3k_ai_empty_fifo(struct comedi_device *dev,
 	if (count < 0)
 		count += AI_FIFO_DEPTH;
 
-	printk("reading %d samples\n", count);
+	dev_dbg(dev->hw_dev, "reading %d samples\n", count);
 
 	rear = devpriv->ai_rear;
 
@@ -640,7 +641,7 @@ static int dt3k_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	int ret;
 	unsigned int mode;
 
-	printk("dt3k_ai_cmd:\n");
+	dev_dbg(dev->hw_dev, "dt3k_ai_cmd:\n");
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		chan = CR_CHAN(cmd->chanlist[i]);
 		range = CR_RANGE(cmd->chanlist[i]);
@@ -651,15 +652,15 @@ static int dt3k_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	aref = CR_AREF(cmd->chanlist[0]);
 
 	writew(cmd->scan_end_arg, devpriv->io_addr + DPR_Params(0));
-	printk("param[0]=0x%04x\n", cmd->scan_end_arg);
+	dev_dbg(dev->hw_dev, "param[0]=0x%04x\n", cmd->scan_end_arg);
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		divider = dt3k_ns_to_timer(50, &cmd->convert_arg,
 					   cmd->flags & TRIG_ROUND_MASK);
 		writew((divider >> 16), devpriv->io_addr + DPR_Params(1));
-		printk("param[1]=0x%04x\n", divider >> 16);
+		dev_dbg(dev->hw_dev, "param[1]=0x%04x\n", divider >> 16);
 		writew((divider & 0xffff), devpriv->io_addr + DPR_Params(2));
-		printk("param[2]=0x%04x\n", divider & 0xffff);
+		dev_dbg(dev->hw_dev, "param[2]=0x%04x\n", divider & 0xffff);
 	} else {
 		/* not supported */
 	}
@@ -668,21 +669,21 @@ static int dt3k_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		tscandiv = dt3k_ns_to_timer(100, &cmd->scan_begin_arg,
 					    cmd->flags & TRIG_ROUND_MASK);
 		writew((tscandiv >> 16), devpriv->io_addr + DPR_Params(3));
-		printk("param[3]=0x%04x\n", tscandiv >> 16);
+		dev_dbg(dev->hw_dev, "param[3]=0x%04x\n", tscandiv >> 16);
 		writew((tscandiv & 0xffff), devpriv->io_addr + DPR_Params(4));
-		printk("param[4]=0x%04x\n", tscandiv & 0xffff);
+		dev_dbg(dev->hw_dev, "param[4]=0x%04x\n", tscandiv & 0xffff);
 	} else {
 		/* not supported */
 	}
 
 	mode = DT3000_AD_RETRIG_INTERNAL | 0 | 0;
 	writew(mode, devpriv->io_addr + DPR_Params(5));
-	printk("param[5]=0x%04x\n", mode);
+	dev_dbg(dev->hw_dev, "param[5]=0x%04x\n", mode);
 	writew(aref == AREF_DIFF, devpriv->io_addr + DPR_Params(6));
-	printk("param[6]=0x%04x\n", aref == AREF_DIFF);
+	dev_dbg(dev->hw_dev, "param[6]=0x%04x\n", aref == AREF_DIFF);
 
 	writew(AI_FIFO_DEPTH / 2, devpriv->io_addr + DPR_Params(7));
-	printk("param[7]=0x%04x\n", AI_FIFO_DEPTH / 2);
+	dev_dbg(dev->hw_dev, "param[7]=0x%04x\n", AI_FIFO_DEPTH / 2);
 
 	writew(SUBS_AI, devpriv->io_addr + DPR_SubSys);
 	ret = dt3k_send_cmd(dev, CMD_CONFIG);
@@ -848,7 +849,7 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	int bus, slot;
 	int ret = 0;
 
-	printk("dt3000:");
+	dev_dbg(dev->hw_dev, "dt3000:\n");
 	bus = it->options[0];
 	slot = it->options[1];
 
@@ -860,7 +861,7 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret < 0)
 		return ret;
 	if (ret == 0) {
-		printk(" no DT board found\n");
+		dev_warn(dev->hw_dev, "no DT board found\n");
 		return -ENODEV;
 	}
 
@@ -868,7 +869,8 @@ static int dt3000_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	if (request_irq(devpriv->pci_dev->irq, dt3k_interrupt, IRQF_SHARED,
 			"dt3000", dev)) {
-		printk(" unable to allocate IRQ %u\n", devpriv->pci_dev->irq);
+		dev_err(dev->hw_dev, "unable to allocate IRQ %u\n",
+			devpriv->pci_dev->irq);
 		return -EINVAL;
 	}
 	dev->irq = devpriv->pci_dev->irq;
diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c
index 8d98cf412709..6a79ba10630d 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.c
+++ b/drivers/staging/comedi/drivers/jr3_pci.c
@@ -71,18 +71,12 @@ static struct comedi_driver driver_jr3_pci = {
 };
 
 static DEFINE_PCI_DEVICE_TABLE(jr3_pci_pci_table) = {
-	{
-	PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_1_CHANNEL,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_1_CHANNEL_NEW,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_2_CHANNEL,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_3_CHANNEL,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_4_CHANNEL,
-		    PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_1_CHANNEL) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_1_CHANNEL_NEW) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_2_CHANNEL) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_3_CHANNEL) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_JR3, PCI_DEVICE_ID_JR3_4_CHANNEL) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, jr3_pci_pci_table);
@@ -378,14 +372,14 @@ static int jr3_pci_open(struct comedi_device *dev)
 	int i;
 	struct jr3_pci_dev_private *devpriv = dev->private;
 
-	printk("jr3_pci_open\n");
+	dev_dbg(dev->hw_dev, "jr3_pci_open\n");
 	for (i = 0; i < devpriv->n_channels; i++) {
 		struct jr3_pci_subdev_private *p;
 
 		p = dev->subdevices[i].private;
 		if (p) {
-			printk("serial: %p %d (%d)\n", p, p->serial_no,
-			       p->channel_no);
+			dev_dbg(dev->hw_dev, "serial: %p %d (%d)\n", p,
+				p->serial_no, p->channel_no);
 		}
 	}
 	return 0;
@@ -463,8 +457,8 @@ static int jr3_download_firmware(struct comedi_device *dev, const u8 * data,
 					break;
 				more = more
 				    && read_idm_word(data, size, &pos, &addr);
-				printk("Loading#%d %4.4x bytes at %4.4x\n", i,
-				       count, addr);
+				dev_dbg(dev->hw_dev, "Loading#%d %4.4x bytes at %4.4x\n",
+					i, count, addr);
 				while (more && count > 0) {
 					if (addr & 0x4000) {
 						/*  16 bit data, never seen in real life!! */
@@ -599,24 +593,24 @@ static struct poll_delay_t jr3_pci_poll_subdevice(struct comedi_subdevice *s)
 					min_full_scale =
 					    get_min_full_scales(channel);
 					printk("Obtained Min. Full Scales:\n");
-					printk("%i   ", (min_full_scale).fx);
-					printk("%i   ", (min_full_scale).fy);
-					printk("%i   ", (min_full_scale).fz);
-					printk("%i   ", (min_full_scale).mx);
-					printk("%i   ", (min_full_scale).my);
-					printk("%i   ", (min_full_scale).mz);
-					printk("\n");
+					printk(KERN_DEBUG "%i ", (min_full_scale).fx);
+					printk(KERN_CONT "%i ", (min_full_scale).fy);
+					printk(KERN_CONT "%i ", (min_full_scale).fz);
+					printk(KERN_CONT "%i ", (min_full_scale).mx);
+					printk(KERN_CONT "%i ", (min_full_scale).my);
+					printk(KERN_CONT "%i ", (min_full_scale).mz);
+					printk(KERN_CONT "\n");
 
 					max_full_scale =
 					    get_max_full_scales(channel);
 					printk("Obtained Max. Full Scales:\n");
-					printk("%i   ", (max_full_scale).fx);
-					printk("%i   ", (max_full_scale).fy);
-					printk("%i   ", (max_full_scale).fz);
-					printk("%i   ", (max_full_scale).mx);
-					printk("%i   ", (max_full_scale).my);
-					printk("%i   ", (max_full_scale).mz);
-					printk("\n");
+					printk(KERN_DEBUG "%i ", (max_full_scale).fx);
+					printk(KERN_CONT "%i ", (max_full_scale).fy);
+					printk(KERN_CONT "%i ", (max_full_scale).fz);
+					printk(KERN_CONT "%i ", (max_full_scale).mx);
+					printk(KERN_CONT "%i ", (max_full_scale).my);
+					printk(KERN_CONT "%i ", (max_full_scale).mz);
+					printk(KERN_CONT "\n");
 
 					set_full_scales(channel,
 							max_full_scale);
@@ -779,14 +773,12 @@ static int jr3_pci_attach(struct comedi_device *dev,
 	int opt_bus, opt_slot, i;
 	struct jr3_pci_dev_private *devpriv;
 
-	printk("comedi%d: jr3_pci\n", dev->minor);
-
 	opt_bus = it->options[0];
 	opt_slot = it->options[1];
 
 	if (sizeof(struct jr3_channel) != 0xc00) {
-		printk("sizeof(struct jr3_channel) = %x [expected %x]\n",
-		       (unsigned)sizeof(struct jr3_channel), 0xc00);
+		dev_err(dev->hw_dev, "sizeof(struct jr3_channel) = %x [expected %x]\n",
+			(unsigned)sizeof(struct jr3_channel), 0xc00);
 		return -EINVAL;
 	}
 
@@ -840,7 +832,7 @@ static int jr3_pci_attach(struct comedi_device *dev,
 		}
 	}
 	if (!card) {
-		printk(" no jr3_pci found\n");
+		dev_err(dev->hw_dev, "no jr3_pci found\n");
 		return -EIO;
 	} else {
 		devpriv->pci_dev = card;
@@ -875,10 +867,10 @@ static int jr3_pci_attach(struct comedi_device *dev,
 
 			p = dev->subdevices[i].private;
 			p->channel = &devpriv->iobase->channel[i].data;
-			printk("p->channel %p %p (%tx)\n",
-			       p->channel, devpriv->iobase,
-			       ((char *)(p->channel) -
-				(char *)(devpriv->iobase)));
+			dev_dbg(dev->hw_dev, "p->channel %p %p (%tx)\n",
+				p->channel, devpriv->iobase,
+				((char *)(p->channel) -
+				 (char *)(devpriv->iobase)));
 			p->channel_no = i;
 			for (j = 0; j < 8; j++) {
 				int k;
@@ -916,7 +908,7 @@ static int jr3_pci_attach(struct comedi_device *dev,
 	devpriv->iobase->channel[0].reset = 0;
 
 	result = comedi_load_firmware(dev, "jr3pci.idm", jr3_download_firmware);
-	printk("Firmare load %d\n", result);
+	dev_dbg(dev->hw_dev, "Firmare load %d\n", result);
 
 	if (result < 0)
 		goto out;
@@ -934,9 +926,9 @@ static int jr3_pci_attach(struct comedi_device *dev,
  */
 	msleep_interruptible(25);
 	for (i = 0; i < 0x18; i++) {
-		printk("%c",
-		       get_u16(&devpriv->iobase->channel[0].
-			       data.copyright[i]) >> 8);
+		dev_dbg(dev->hw_dev, "%c\n",
+			get_u16(&devpriv->iobase->channel[0].
+				data.copyright[i]) >> 8);
 	}
 
 	/*  Start card timer */
@@ -963,7 +955,6 @@ static int jr3_pci_detach(struct comedi_device *dev)
 	int i;
 	struct jr3_pci_dev_private *devpriv = dev->private;
 
-	printk("comedi%d: jr3_pci: remove\n", dev->minor);
 	if (devpriv) {
 		del_timer_sync(&devpriv->timer);
 
diff --git a/drivers/staging/comedi/drivers/ke_counter.c b/drivers/staging/comedi/drivers/ke_counter.c
index 286093bca3fb..4e9e9a078652 100644
--- a/drivers/staging/comedi/drivers/ke_counter.c
+++ b/drivers/staging/comedi/drivers/ke_counter.c
@@ -52,10 +52,8 @@ static int cnt_attach(struct comedi_device *dev, struct comedi_devconfig *it);
 static int cnt_detach(struct comedi_device *dev);
 
 static DEFINE_PCI_DEVICE_TABLE(cnt_pci_table) = {
-	{
-	PCI_VENDOR_ID_KOLTER, CNT_CARD_DEVICE_ID, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_KOLTER, CNT_CARD_DEVICE_ID) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, cnt_pci_table);
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index cda4b224b30f..8b812e41c52b 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -188,12 +188,9 @@ static const struct comedi_lrange me2600_ao_range = {
 };
 
 static DEFINE_PCI_DEVICE_TABLE(me_pci_table) = {
-	{
-	PCI_VENDOR_ID_MEILHAUS, ME2600_DEVICE_ID, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	PCI_VENDOR_ID_MEILHAUS, ME2000_DEVICE_ID, PCI_ANY_ID,
-		    PCI_ANY_ID, 0, 0, 0}, {
-	0}
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, ME2600_DEVICE_ID) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MEILHAUS, ME2000_DEVICE_ID) },
+	{0}
 };
 
 MODULE_DEVICE_TABLE(pci, me_pci_table);
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index 32e675e3f0b9..c25e44c1905e 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -731,9 +731,8 @@ static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outw(trigger_bits, dev->iobase + TRIGGER_REG);
 
 	/*  start acquisition for soft trigger */
-	if (cmd->start_src == TRIG_NOW) {
+	if (cmd->start_src == TRIG_NOW)
 		outw(0, dev->iobase + FIFO_START_REG);
-	}
 #ifdef A2150_DEBUG
 	ni_dump_regs(dev);
 #endif
@@ -860,11 +859,10 @@ static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 	case TRIG_ROUND_NEAREST:
 	default:
 		/*  if least upper bound is better approximation */
-		if (lub - *period < *period - glb) {
+		if (lub - *period < *period - glb)
 			*period = lub;
-		} else {
+		else
 			*period = glb;
-		}
 		break;
 	case TRIG_ROUND_UP:
 		*period = lub;
diff --git a/drivers/staging/comedi/drivers/ni_daq_dio24.c b/drivers/staging/comedi/drivers/ni_daq_dio24.c
index 49b824c7bd2e..c0423a8c3e36 100644
--- a/drivers/staging/comedi/drivers/ni_daq_dio24.c
+++ b/drivers/staging/comedi/drivers/ni_daq_dio24.c
@@ -52,7 +52,7 @@ the PCMCIA interface.
 #include <pcmcia/cisreg.h>
 #include <pcmcia/ds.h>
 
-static struct pcmcia_device *pcmcia_cur_dev = NULL;
+static struct pcmcia_device *pcmcia_cur_dev;
 
 #define DIO24_SIZE 4		/*  size of io region used by board */
 
@@ -133,22 +133,19 @@ static int dio24_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 #endif
 		break;
 	default:
-		printk("bug! couldn't determine board type\n");
+		pr_err("bug! couldn't determine board type\n");
 		return -EINVAL;
 		break;
 	}
-	printk("comedi%d: ni_daq_dio24: %s, io 0x%lx", dev->minor,
-	       thisboard->name, iobase);
+	pr_debug("comedi%d: ni_daq_dio24: %s, io 0x%lx", dev->minor,
+		 thisboard->name, iobase);
 #ifdef incomplete
-	if (irq) {
-		printk(", irq %u", irq);
-	}
+	if (irq)
+		pr_debug("irq %u\n", irq);
 #endif
 
-	printk("\n");
-
 	if (iobase == 0) {
-		printk("io base address is zero!\n");
+		pr_err("io base address is zero!\n");
 		return -EINVAL;
 	}
 
@@ -173,7 +170,7 @@ static int dio24_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static int dio24_detach(struct comedi_device *dev)
 {
-	printk("comedi%d: ni_daq_dio24: remove\n", dev->minor);
+	dev_info(dev->hw_dev, "comedi%d: ni_daq_dio24: remove\n", dev->minor);
 
 	if (dev->subdevices)
 		subdev_8255_cleanup(dev, dev->subdevices + 0);
diff --git a/drivers/staging/comedi/drivers/ni_labpc_cs.c b/drivers/staging/comedi/drivers/ni_labpc_cs.c
index 832a5178b638..ff3840544dd4 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_cs.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_cs.c
@@ -145,7 +145,7 @@ static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		irq = link->irq;
 		break;
 	default:
-		printk("bug! couldn't determine board type\n");
+		pr_err("bug! couldn't determine board type\n");
 		return -EINVAL;
 		break;
 	}
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index 9148abdad074..0f0d995f137c 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -1470,7 +1470,7 @@ static void m_series_stc_writew(struct comedi_device *dev, uint16_t data,
 		/* FIXME: DIO_Output_Register (16 bit reg) is replaced by M_Offset_Static_Digital_Output (32 bit)
 		   and M_Offset_SCXI_Serial_Data_Out (8 bit) */
 	default:
-		printk("%s: bug! unhandled register=0x%x in switch.\n",
+		printk(KERN_WARNING "%s: bug! unhandled register=0x%x in switch.\n",
 		       __func__, reg);
 		BUG();
 		return;
@@ -1505,7 +1505,7 @@ static uint16_t m_series_stc_readw(struct comedi_device *dev, int reg)
 		offset = M_Offset_G01_Status;
 		break;
 	default:
-		printk("%s: bug! unhandled register=0x%x in switch.\n",
+		printk(KERN_WARNING "%s: bug! unhandled register=0x%x in switch.\n",
 		       __func__, reg);
 		BUG();
 		return 0;
@@ -1547,7 +1547,7 @@ static void m_series_stc_writel(struct comedi_device *dev, uint32_t data,
 		offset = M_Offset_G1_Load_B;
 		break;
 	default:
-		printk("%s: bug! unhandled register=0x%x in switch.\n",
+		printk(KERN_WARNING "%s: bug! unhandled register=0x%x in switch.\n",
 		       __func__, reg);
 		BUG();
 		return;
@@ -1573,7 +1573,7 @@ static uint32_t m_series_stc_readl(struct comedi_device *dev, int reg)
 		offset = M_Offset_G1_Save;
 		break;
 	default:
-		printk("%s: bug! unhandled register=0x%x in switch.\n",
+		printk(KERN_WARNING "%s: bug! unhandled register=0x%x in switch.\n",
 		       __func__, reg);
 		BUG();
 		return 0;
@@ -1632,9 +1632,8 @@ static void m_series_init_eeprom_buffer(struct comedi_device *dev)
 	}
 	devpriv->serial_number = be32_to_cpu(devpriv->serial_number);
 
-	for (i = 0; i < M_SERIES_EEPROM_SIZE; ++i) {
+	for (i = 0; i < M_SERIES_EEPROM_SIZE; ++i)
 		devpriv->eeprom_buffer[i] = ni_readb(Start_Cal_EEPROM + i);
-	}
 
 	writel(old_iodwbsr1_bits, devpriv->mite->mite_io_addr + MITE_IODWBSR_1);
 	writel(old_iodwbsr_bits, devpriv->mite->mite_io_addr + MITE_IODWBSR);
@@ -1665,9 +1664,9 @@ static void init_6143(struct comedi_device *dev)
 static int pcimio_detach(struct comedi_device *dev)
 {
 	mio_common_detach(dev);
-	if (dev->irq) {
+	if (dev->irq)
 		free_irq(dev->irq, dev);
-	}
+
 	if (dev->private) {
 		mite_free_ring(devpriv->ai_mite_ring);
 		mite_free_ring(devpriv->ao_mite_ring);
@@ -1685,7 +1684,7 @@ static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	int ret;
 
-	printk("comedi%d: ni_pcimio:", dev->minor);
+	dev_info(dev->hw_dev, "comedi%d: ni_pcimio:\n", dev->minor);
 
 	ret = ni_alloc_private(dev);
 	if (ret < 0)
@@ -1695,7 +1694,7 @@ static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret < 0)
 		return ret;
 
-	printk(" %s", boardtype.name);
+	dev_dbg(dev->hw_dev, "%s\n", boardtype.name);
 	dev->board_name = boardtype.name;
 
 	if (boardtype.reg_type & ni_reg_m_series_mask) {
@@ -1712,7 +1711,7 @@ static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	ret = mite_setup(devpriv->mite);
 	if (ret < 0) {
-		printk(" error setting up mite\n");
+		pr_warn("error setting up mite\n");
 		return ret;
 	}
 	comedi_set_hw_dev(dev, &devpriv->mite->pcidev->dev);
@@ -1740,13 +1739,13 @@ static int pcimio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	dev->irq = mite_irq(devpriv->mite);
 
 	if (dev->irq == 0) {
-		printk(" unknown irq (bad)\n");
+		pr_warn("unknown irq (bad)\n");
 	} else {
-		printk(" ( irq = %u )", dev->irq);
+		pr_debug("( irq = %u )\n", dev->irq);
 		ret = request_irq(dev->irq, ni_E_interrupt, NI_E_IRQ_FLAGS,
 				  DRV_NAME, dev);
 		if (ret < 0) {
-			printk(" irq not available\n");
+			pr_warn("irq not available\n");
 			dev->irq = 0;
 		}
 	}
@@ -1787,7 +1786,7 @@ static int pcimio_find_device(struct comedi_device *dev, int bus, int slot)
 			}
 		}
 	}
-	printk("no device found\n");
+	pr_warn("no device found\n");
 	mite_list_devices();
 	return -EIO;
 }
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index 0b9bee36eb5f..96cd7ec2ad53 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -155,8 +155,8 @@ static int pcl816_attach(struct comedi_device *dev,
 static int pcl816_detach(struct comedi_device *dev);
 
 #ifdef unused
-static int RTC_lock = 0;	/* RTC lock */
-static int RTC_timer_lock = 0;	/* RTC int lock */
+static int RTC_lock;	/* RTC lock */
+static int RTC_timer_lock;	/* RTC int lock */
 #endif
 
 static struct comedi_driver driver_pcl816 = {
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index b45a9bd8b489..7344a53a81c4 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -252,8 +252,8 @@ static int pcl818_attach(struct comedi_device *dev,
 static int pcl818_detach(struct comedi_device *dev);
 
 #ifdef unused
-static int RTC_lock = 0;	/* RTC lock */
-static int RTC_timer_lock = 0;	/* RTC int lock */
+static int RTC_lock;	/* RTC lock */
+static int RTC_timer_lock;	/* RTC int lock */
 #endif
 
 struct pcl818_board {
@@ -463,9 +463,8 @@ static int pcl818_ao_insn_read(struct comedi_device *dev,
 	int n;
 	int chan = CR_CHAN(insn->chanspec);
 
-	for (n = 0; n < insn->n; n++) {
+	for (n = 0; n < insn->n; n++)
 		data[n] = devpriv->ao_readback[chan];
-	}
 
 	return n;
 }
@@ -571,9 +570,9 @@ conv_finish:
 		return IRQ_HANDLED;
 	}
 	devpriv->act_chanlist_pos++;
-	if (devpriv->act_chanlist_pos >= devpriv->act_chanlist_len) {
+	if (devpriv->act_chanlist_pos >= devpriv->act_chanlist_len)
 		devpriv->act_chanlist_pos = 0;
-	}
+
 	s->async->cur_chan++;
 	if (s->async->cur_chan >= devpriv->ai_n_chan) {
 		/*  printk("E"); */
@@ -645,9 +644,9 @@ static irqreturn_t interrupt_pcl818_ai_mode13_dma(int irq, void *d)
 		comedi_buf_put(s->async, ptr[bufptr++] >> 4);	/*  get one sample */
 
 		devpriv->act_chanlist_pos++;
-		if (devpriv->act_chanlist_pos >= devpriv->act_chanlist_len) {
+		if (devpriv->act_chanlist_pos >= devpriv->act_chanlist_len)
 			devpriv->act_chanlist_pos = 0;
-		}
+
 		s->async->cur_chan++;
 		if (s->async->cur_chan >= devpriv->ai_n_chan) {
 			s->async->cur_chan = 0;
@@ -805,11 +804,10 @@ static irqreturn_t interrupt_pcl818_ai_mode13_fifo(int irq, void *d)
 		return IRQ_HANDLED;
 	}
 
-	if (lo & 2) {
+	if (lo & 2)
 		len = 512;
-	} else {
+	else
 		len = 0;
-	}
 
 	for (i = 0; i < len; i++) {
 		lo = inb(dev->iobase + PCL818_FI_DATALO);
@@ -827,9 +825,9 @@ static irqreturn_t interrupt_pcl818_ai_mode13_fifo(int irq, void *d)
 		comedi_buf_put(s->async, (lo >> 4) | (inb(dev->iobase + PCL818_FI_DATAHI) << 4));	/*  get one sample */
 
 		devpriv->act_chanlist_pos++;
-		if (devpriv->act_chanlist_pos >= devpriv->act_chanlist_len) {
+		if (devpriv->act_chanlist_pos >= devpriv->act_chanlist_len)
 			devpriv->act_chanlist_pos = 0;
-		}
+
 		s->async->cur_chan++;
 		if (s->async->cur_chan >= devpriv->ai_n_chan) {
 			s->async->cur_chan = 0;
@@ -1009,7 +1007,7 @@ static int pcl818_ai_cmd_mode(int mode, struct comedi_device *dev,
 	int divisor1 = 0, divisor2 = 0;
 	unsigned int seglen;
 
-	printk("pcl818_ai_cmd_mode()\n");
+	dev_dbg(dev->hw_dev, "pcl818_ai_cmd_mode()\n");
 	if ((!dev->irq) && (!devpriv->dma_rtc)) {
 		comedi_error(dev, "IRQ not defined!");
 		return -EINVAL;
@@ -1112,7 +1110,7 @@ static int pcl818_ai_cmd_mode(int mode, struct comedi_device *dev,
 		break;
 	}
 #endif
-	printk("pcl818_ai_cmd_mode() end\n");
+	dev_dbg(dev->hw_dev, "pcl818_ai_cmd_mode() end\n");
 	return 0;
 }
 
@@ -1309,11 +1307,9 @@ static void setup_channel_list(struct comedi_device *dev,
 */
 static int check_single_ended(unsigned int port)
 {
-	if (inb(port + PCL818_STATUS) & 0x20) {
+	if (inb(port + PCL818_STATUS) & 0x20)
 		return 1;
-	} else {
-		return 0;
-	}
+	return 0;
 }
 
 /*
@@ -1352,9 +1348,8 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (!cmd->stop_src || tmp != cmd->stop_src)
 		err++;
 
-	if (err) {
+	if (err)
 		return 1;
-	}
 
 	/* step 2: make sure trigger sources are unique and mutually compatible */
 
@@ -1377,9 +1372,8 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_COUNT)
 		err++;
 
-	if (err) {
+	if (err)
 		return 2;
-	}
 
 	/* step 3: make sure arguments are trivially compatible */
 
@@ -1421,9 +1415,8 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		}
 	}
 
-	if (err) {
+	if (err)
 		return 3;
-	}
 
 	/* step 4: fix up any arguments */
 
@@ -1438,9 +1431,8 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 			err++;
 	}
 
-	if (err) {
+	if (err)
 		return 4;
-	}
 
 	/* step 5: complain about special chanlist considerations */
 
@@ -1461,7 +1453,7 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int retval;
 
-	printk("pcl818_ai_cmd()\n");
+	dev_dbg(dev->hw_dev, "pcl818_ai_cmd()\n");
 	devpriv->ai_n_chan = cmd->chanlist_len;
 	devpriv->ai_chanlist = cmd->chanlist;
 	devpriv->ai_flags = cmd->flags;
@@ -1470,17 +1462,16 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->ai_timer1 = 0;
 	devpriv->ai_timer2 = 0;
 
-	if (cmd->stop_src == TRIG_COUNT) {
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ai_scans = cmd->stop_arg;
-	} else {
+	else
 		devpriv->ai_scans = 0;
-	}
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW) {	/*  mode 1, 3 */
 		if (cmd->convert_src == TRIG_TIMER) {	/*  mode 1 */
 			devpriv->ai_timer1 = cmd->convert_arg;
 			retval = pcl818_ai_cmd_mode(1, dev, s);
-			printk("pcl818_ai_cmd() end\n");
+			dev_dbg(dev->hw_dev, "pcl818_ai_cmd() end\n");
 			return retval;
 		}
 		if (cmd->convert_src == TRIG_EXT) {	/*  mode 3 */
@@ -1499,7 +1490,7 @@ static int pcl818_ai_cancel(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
 	if (devpriv->irq_blocked > 0) {
-		printk("pcl818_ai_cancel()\n");
+		dev_dbg(dev->hw_dev, "pcl818_ai_cancel()\n");
 		devpriv->irq_was_now_closed = 1;
 
 		switch (devpriv->ai_mode) {
@@ -1549,7 +1540,7 @@ static int pcl818_ai_cancel(struct comedi_device *dev,
 	}
 
 end:
-	printk("pcl818_ai_cancel() end\n");
+	dev_dbg(dev->hw_dev, "pcl818_ai_cancel() end\n");
 	return 0;
 }
 
@@ -1633,11 +1624,11 @@ static int set_rtc_irq_bit(unsigned char bit)
 	save_flags(flags);
 	cli();
 	val = CMOS_READ(RTC_CONTROL);
-	if (bit) {
+	if (bit)
 		val |= RTC_PIE;
-	} else {
+	else
 		val &= ~RTC_PIE;
-	}
+
 	CMOS_WRITE(val, RTC_CONTROL);
 	CMOS_READ(RTC_INTR_FLAGS);
 	restore_flags(flags);
@@ -1754,22 +1745,23 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* claim our I/O space */
 	iobase = it->options[0];
-	printk("comedi%d: pcl818:  board=%s, ioport=0x%03lx",
-	       dev->minor, this_board->name, iobase);
+	printk
+	    ("comedi%d: pcl818:  board=%s, ioport=0x%03lx",
+	     dev->minor, this_board->name, iobase);
 	devpriv->io_range = this_board->io_range;
 	if ((this_board->fifo) && (it->options[2] == -1)) {	/*  we've board with FIFO and we want to use FIFO */
 		devpriv->io_range = PCLx1xFIFO_RANGE;
 		devpriv->usefifo = 1;
 	}
 	if (!request_region(iobase, devpriv->io_range, "pcl818")) {
-		printk("I/O port conflict\n");
+		comedi_error(dev, "I/O port conflict\n");
 		return -EIO;
 	}
 
 	dev->iobase = iobase;
 
 	if (pcl818_check(iobase)) {
-		printk(", I can't detect board. FAIL!\n");
+		comedi_error(dev, "I can't detect board. FAIL!\n");
 		return -EIO;
 	}
 
@@ -1793,19 +1785,18 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 					     irq);
 					irq = 0;	/* Can't use IRQ */
 				} else {
-					printk(", irq=%u", irq);
+					printk(KERN_DEBUG "irq=%u", irq);
 				}
 			}
 		}
 	}
 
 	dev->irq = irq;
-	if (irq) {
-		devpriv->irq_free = 1;
-	} /* 1=we have allocated irq */
-	else {
+	if (irq)
+		devpriv->irq_free = 1;   /* 1=we have allocated irq */
+	else
 		devpriv->irq_free = 0;
-	}
+
 	devpriv->irq_blocked = 0;	/* number of subdevice which use IRQ */
 	devpriv->ai_mode = 0;	/* mode of irq */
 
@@ -1825,7 +1816,7 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				 "pcl818 DMA (RTC)", dev)) {
 			devpriv->dma_rtc = 1;
 			devpriv->rtc_irq = RTC_IRQ;
-			printk(", dma_irq=%u", devpriv->rtc_irq);
+			printk(KERN_DEBUG "dma_irq=%u", devpriv->rtc_irq);
 		} else {
 			RTC_lock--;
 			if (RTC_lock == 0) {
@@ -1850,34 +1841,26 @@ no_rtc:
 		if (dma < 1)
 			goto no_dma;	/* DMA disabled */
 		if (((1 << dma) & this_board->DMAbits) == 0) {
-			printk(", DMA is out of allowed range, FAIL!\n");
+			printk(KERN_ERR "DMA is out of allowed range, FAIL!\n");
 			return -EINVAL;	/* Bad DMA */
 		}
 		ret = request_dma(dma, "pcl818");
-		if (ret) {
-			printk(", unable to allocate DMA %u, FAIL!\n", dma);
+		if (ret)
 			return -EBUSY;	/* DMA isn't free */
-		}
 		devpriv->dma = dma;
-		printk(", dma=%u", dma);
 		pages = 2;	/* we need 16KB */
 		devpriv->dmabuf[0] = __get_dma_pages(GFP_KERNEL, pages);
-		if (!devpriv->dmabuf[0]) {
-			printk(", unable to allocate DMA buffer, FAIL!\n");
+		if (!devpriv->dmabuf[0])
 			/* maybe experiment with try_to_free_pages() will help .... */
 			return -EBUSY;	/* no buffer :-( */
-		}
 		devpriv->dmapages[0] = pages;
 		devpriv->hwdmaptr[0] = virt_to_bus((void *)devpriv->dmabuf[0]);
 		devpriv->hwdmasize[0] = (1 << pages) * PAGE_SIZE;
 		/* printk("%d %d %ld, ",devpriv->dmapages[0],devpriv->hwdmasize[0],PAGE_SIZE); */
 		if (devpriv->dma_rtc == 0) {	/*  we must do duble buff :-( */
 			devpriv->dmabuf[1] = __get_dma_pages(GFP_KERNEL, pages);
-			if (!devpriv->dmabuf[1]) {
-				printk
-				    (", unable to allocate DMA buffer, FAIL!\n");
+			if (!devpriv->dmabuf[1])
 				return -EBUSY;
-			}
 			devpriv->dmapages[1] = pages;
 			devpriv->hwdmaptr[1] =
 			    virt_to_bus((void *)devpriv->dmabuf[1]);
@@ -2017,11 +2000,10 @@ no_dma:
 	}
 
 	/* select 1/10MHz oscilator */
-	if ((it->options[3] == 0) || (it->options[3] == 10)) {
+	if ((it->options[3] == 0) || (it->options[3] == 10))
 		devpriv->i8253_osc_base = 100;
-	} else {
+	else
 		devpriv->i8253_osc_base = 1000;
-	}
 
 	/* max sampling speed */
 	devpriv->ns_min = this_board->ns_min;
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index 3ad04aaa1e3c..eddac00e4e29 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -371,15 +371,15 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	iobase = it->options[0];
 	irq[0] = it->options[1];
 
-	printk(KERN_INFO "comedi%d: %s: io: %lx ", dev->minor, driver.driver_name,
-	       iobase);
+	printk(KERN_INFO "comedi%d: %s: io: %lx attaching...\n", dev->minor,
+			driver.driver_name, iobase);
 
 	dev->iobase = iobase;
 
 	if (!iobase || !request_region(iobase,
 				       thisboard->total_iosize,
 				       driver.driver_name)) {
-		printk(KERN_ERR "I/O port conflict\n");
+		printk(KERN_ERR "comedi%d: I/O port conflict\n", dev->minor);
 		return -EIO;
 	}
 
@@ -394,7 +394,8 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
  * convenient macro defined in comedidev.h.
  */
 	if (alloc_private(dev, sizeof(struct pcmmio_private)) < 0) {
-		printk(KERN_ERR "cannot allocate private data structure\n");
+		printk(KERN_ERR "comedi%d: cannot allocate private data structure\n",
+				dev->minor);
 		return -ENOMEM;
 	}
 
@@ -417,7 +418,8 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	    kcalloc(n_subdevs, sizeof(struct pcmmio_subdev_private),
 		    GFP_KERNEL);
 	if (!devpriv->sprivs) {
-		printk(KERN_ERR "cannot allocate subdevice private data structures\n");
+		printk(KERN_ERR "comedi%d: cannot allocate subdevice private data structures\n",
+				dev->minor);
 		return -ENOMEM;
 	}
 	/*
@@ -427,7 +429,8 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	 * Allocate 1 AI + 1 AO + 2 DIO subdevs (24 lines per DIO)
 	 */
 	if (alloc_subdevices(dev, n_subdevs) < 0) {
-		printk(KERN_ERR "cannot allocate subdevice data structures\n");
+		printk(KERN_ERR "comedi%d: cannot allocate subdevice data structures\n",
+				dev->minor);
 		return -ENOMEM;
 	}
 
@@ -557,14 +560,15 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				 */
 
 	if (irq[0]) {
-		printk(KERN_DEBUG "irq: %u ", irq[0]);
+		printk(KERN_DEBUG "comedi%d: irq: %u\n", dev->minor, irq[0]);
 		if (thisboard->dio_num_asics == 2 && irq[1])
-			printk(KERN_DEBUG "second ASIC irq: %u ", irq[1]);
+			printk(KERN_DEBUG "comedi%d: second ASIC irq: %u\n",
+					dev->minor, irq[1]);
 	} else {
-		printk(KERN_INFO "(IRQ mode disabled) ");
+		printk(KERN_INFO "comedi%d: (IRQ mode disabled)\n", dev->minor);
 	}
 
-	printk(KERN_INFO "attached\n");
+	printk(KERN_INFO "comedi%d: attached\n", dev->minor);
 
 	return 1;
 }
@@ -663,7 +667,7 @@ static int pcmmio_dio_insn_bits(struct comedi_device *dev,
 		}
 #ifdef DAMMIT_ITS_BROKEN
 		/* DEBUG */
-		printk(KERN_DEBUG "data_out_byte %02x\n", (unsigned)byte);
+		printk("data_out_byte %02x\n", (unsigned)byte);
 #endif
 		/* save the digital input lines for this byte.. */
 		s->state |= ((unsigned int)byte) << offset;
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index b2c2c8971a32..661ba2e03892 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -295,15 +295,15 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	irq[0] = it->options[1];
 	irq[1] = it->options[2];
 
-	printk("comedi%d: %s: io: %lx ", dev->minor, driver.driver_name,
-	       iobase);
+	dev_dbg(dev->hw_dev, "comedi%d: %s: io: %lx attached\n", dev->minor,
+		driver.driver_name, iobase);
 
 	dev->iobase = iobase;
 
 	if (!iobase || !request_region(iobase,
 				       thisboard->num_asics * ASIC_IOSIZE,
 				       driver.driver_name)) {
-		printk("I/O port conflict\n");
+		dev_err(dev->hw_dev, "I/O port conflict\n");
 		return -EIO;
 	}
 
@@ -318,7 +318,7 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
  * convenient macro defined in comedidev.h.
  */
 	if (alloc_private(dev, sizeof(struct pcmuio_private)) < 0) {
-		printk("cannot allocate private data structure\n");
+		dev_warn(dev->hw_dev, "cannot allocate private data structure\n");
 		return -ENOMEM;
 	}
 
@@ -337,7 +337,7 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	    kcalloc(n_subdevs, sizeof(struct pcmuio_subdev_private),
 		    GFP_KERNEL);
 	if (!devpriv->sprivs) {
-		printk("cannot allocate subdevice private data structures\n");
+		dev_warn(dev->hw_dev, "cannot allocate subdevice private data structures\n");
 		return -ENOMEM;
 	}
 	/*
@@ -348,7 +348,7 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	 * 96-channel version of the board.
 	 */
 	if (alloc_subdevices(dev, n_subdevs) < 0) {
-		printk("cannot allocate subdevice data structures\n");
+		dev_dbg(dev->hw_dev, "cannot allocate subdevice data structures\n");
 		return -ENOMEM;
 	}
 
@@ -436,14 +436,13 @@ static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				   irqs.. */
 
 	if (irq[0]) {
-		printk("irq: %u ", irq[0]);
+		dev_dbg(dev->hw_dev, "irq: %u\n", irq[0]);
 		if (irq[1] && thisboard->num_asics == 2)
-			printk("second ASIC irq: %u ", irq[1]);
+			dev_dbg(dev->hw_dev, "second ASIC irq: %u\n", irq[1]);
 	} else {
-		printk("(IRQ mode disabled) ");
+		dev_dbg(dev->hw_dev, "(IRQ mode disabled)\n");
 	}
 
-	printk("attached\n");
 
 	return 1;
 }
@@ -460,7 +459,8 @@ static int pcmuio_detach(struct comedi_device *dev)
 {
 	int i;
 
-	printk("comedi%d: %s: remove\n", dev->minor, driver.driver_name);
+	dev_dbg(dev->hw_dev, "comedi%d: %s: remove\n", dev->minor,
+		driver.driver_name);
 	if (dev->iobase)
 		release_region(dev->iobase, ASIC_IOSIZE * thisboard->num_asics);
 
@@ -501,7 +501,8 @@ static int pcmuio_dio_insn_bits(struct comedi_device *dev,
 
 #ifdef DAMMIT_ITS_BROKEN
 	/* DEBUG */
-	printk("write mask: %08x  data: %08x\n", data[0], data[1]);
+	dev_dbg(dev->hw_dev, "write mask: %08x  data: %08x\n", data[0],
+		data[1]);
 #endif
 
 	s->state = 0;
@@ -537,7 +538,7 @@ static int pcmuio_dio_insn_bits(struct comedi_device *dev,
 		}
 #ifdef DAMMIT_ITS_BROKEN
 		/* DEBUG */
-		printk("data_out_byte %02x\n", (unsigned)byte);
+		dev_dbg(dev->hw_dev, "data_out_byte %02x\n", (unsigned)byte);
 #endif
 		/* save the digital input lines for this byte.. */
 		s->state |= ((unsigned int)byte) << offset;
@@ -548,7 +549,8 @@ static int pcmuio_dio_insn_bits(struct comedi_device *dev,
 
 #ifdef DAMMIT_ITS_BROKEN
 	/* DEBUG */
-	printk("s->state %08x data_out %08x\n", s->state, data[1]);
+	dev_dbg(dev->hw_dev, "s->state %08x data_out %08x\n", s->state,
+		data[1]);
 #endif
 
 	return 2;
@@ -951,14 +953,13 @@ pcmuio_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	spin_lock_irqsave(&subpriv->intr.spinlock, flags);
 	s->async->inttrig = 0;
-	if (subpriv->intr.active) {
+	if (subpriv->intr.active)
 		event = pcmuio_start_intr(dev, s);
-	}
+
 	spin_unlock_irqrestore(&subpriv->intr.spinlock, flags);
 
-	if (event) {
+	if (event)
 		comedi_event(dev, s);
-	}
 
 	return 1;
 }
@@ -1000,9 +1001,8 @@ static int pcmuio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 	spin_unlock_irqrestore(&subpriv->intr.spinlock, flags);
 
-	if (event) {
+	if (event)
 		comedi_event(dev, s);
-	}
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c
index ade2202b6231..d880c2f6fbc1 100644
--- a/drivers/staging/comedi/drivers/serial2002.c
+++ b/drivers/staging/comedi/drivers/serial2002.c
@@ -824,7 +824,7 @@ static int serial2002_attach(struct comedi_device *dev,
 {
 	struct comedi_subdevice *s;
 
-	printk("comedi%d: serial2002: ", dev->minor);
+	dev_dbg(dev->hw_dev, "comedi%d: attached\n", dev->minor);
 	dev->board_name = thisboard->name;
 	if (alloc_private(dev, sizeof(struct serial2002_private)) < 0)
 		return -ENOMEM;
@@ -832,7 +832,8 @@ static int serial2002_attach(struct comedi_device *dev,
 	dev->close = serial_2002_close;
 	devpriv->port = it->options[0];
 	devpriv->speed = it->options[1];
-	printk("/dev/ttyS%d @ %d\n", devpriv->port, devpriv->speed);
+	dev_dbg(dev->hw_dev, "/dev/ttyS%d @ %d\n", devpriv->port,
+		devpriv->speed);
 
 	if (alloc_subdevices(dev, 5) < 0)
 		return -ENOMEM;
@@ -891,7 +892,7 @@ static int serial2002_detach(struct comedi_device *dev)
 	struct comedi_subdevice *s;
 	int i;
 
-	printk("comedi%d: serial2002: remove\n", dev->minor);
+	dev_dbg(dev->hw_dev, "comedi%d: remove\n", dev->minor);
 	for (i = 0; i < 5; i++) {
 		s = &dev->subdevices[i];
 		kfree(s->maxdata_list);
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index 6144afb8cbaa..ca6bcf8b0231 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -1524,15 +1524,17 @@ static int usbdux_ao_inttrig(struct comedi_device *dev,
 		return -EFAULT;
 
 	down(&this_usbduxsub->sem);
+
 	if (!(this_usbduxsub->probed)) {
-		up(&this_usbduxsub->sem);
-		return -ENODEV;
+		ret = -ENODEV;
+		goto out;
 	}
 	if (trignum != 0) {
 		dev_err(&this_usbduxsub->interface->dev,
 			"comedi%d: usbdux_ao_inttrig: invalid trignum\n",
 			dev->minor);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out;
 	}
 	if (!(this_usbduxsub->ao_cmd_running)) {
 		this_usbduxsub->ao_cmd_running = 1;
@@ -1542,8 +1544,7 @@ static int usbdux_ao_inttrig(struct comedi_device *dev,
 				"comedi%d: usbdux_ao_inttrig: submitURB: "
 				"err=%d\n", dev->minor, ret);
 			this_usbduxsub->ao_cmd_running = 0;
-			up(&this_usbduxsub->sem);
-			return ret;
+			goto out;
 		}
 		s->async->inttrig = NULL;
 	} else {
@@ -1551,8 +1552,10 @@ static int usbdux_ao_inttrig(struct comedi_device *dev,
 			"comedi%d: ao_inttrig but acqu is already running.\n",
 			dev->minor);
 	}
+	ret = 1;
+out:
 	up(&this_usbduxsub->sem);
-	return 1;
+	return ret;
 }
 
 static int usbdux_ao_cmdtest(struct comedi_device *dev,
@@ -2689,6 +2692,7 @@ static int usbduxsigma_attach(struct comedi_device *dev,
 	if (ret < 0) {
 		dev_err(&udev->interface->dev,
 			"comedi%d: no space for subdev\n", dev->minor);
+		up(&udev->sem);
 		up(&start_stop_sem);
 		return ret;
 	}
diff --git a/drivers/staging/crystalhd/bc_dts_defs.h b/drivers/staging/crystalhd/bc_dts_defs.h
index fde5b0627f6b..8cd51a7aad8e 100644
--- a/drivers/staging/crystalhd/bc_dts_defs.h
+++ b/drivers/staging/crystalhd/bc_dts_defs.h
@@ -296,43 +296,79 @@ enum {
 	vdecColourPrimariesSMPTE240M,
 	vdecColourPrimariesGenericFilm,
 };
-
+/**
+ * @vdecRESOLUTION_CUSTOM: custom
+ * @vdecRESOLUTION_480i: 480i
+ * @vdecRESOLUTION_1080i: 1080i (1920x1080, 60i)
+ * @vdecRESOLUTION_NTSC: NTSC (720x483, 60i)
+ * @vdecRESOLUTION_480p: 480p (720x480, 60p)
+ * @vdecRESOLUTION_720p: 720p (1280x720, 60p)
+ * @vdecRESOLUTION_PAL1: PAL_1 (720x576, 50i)
+ * @vdecRESOLUTION_1080i25: 1080i25 (1920x1080, 50i)
+ * @vdecRESOLUTION_720p50: 720p50 (1280x720, 50p)
+ * @vdecRESOLUTION_576p: 576p (720x576, 50p)
+ * @vdecRESOLUTION_1080i29_97: 1080i (1920x1080, 59.94i)
+ * @vdecRESOLUTION_720p59_94: 720p (1280x720, 59.94p)
+ * @vdecRESOLUTION_SD_DVD: SD DVD (720x483, 60i)
+ * @vdecRESOLUTION_480p656: 480p (720x480, 60p),
+ *	output bus width 8 bit, clock 74.25MHz
+ * @vdecRESOLUTION_1080p23_976: 1080p23_976 (1920x1080, 23.976p)
+ * @vdecRESOLUTION_720p23_976: 720p23_976 (1280x720p, 23.976p)
+ * @vdecRESOLUTION_240p29_97: 240p (1440x240, 29.97p )
+ * @vdecRESOLUTION_240p30: 240p (1440x240, 30p)
+ * @vdecRESOLUTION_288p25: 288p (1440x288p, 25p)
+ * @vdecRESOLUTION_1080p29_97: 1080p29_97 (1920x1080, 29.97p)
+ * @vdecRESOLUTION_1080p30: 1080p30 (1920x1080, 30p)
+ * @vdecRESOLUTION_1080p24: 1080p24 (1920x1080, 24p)
+ * @vdecRESOLUTION_1080p25: 1080p25 (1920x1080, 25p)
+ * @vdecRESOLUTION_720p24: 720p24 (1280x720, 25p)
+ * @vdecRESOLUTION_720p29_97: 720p29.97 (1280x720, 29.97p)
+ * @vdecRESOLUTION_480p23_976: 480p23.976 (720*480, 23.976)
+ * @vdecRESOLUTION_480p29_97: 480p29.976 (720*480, 29.97p)
+ * @vdecRESOLUTION_576p25: 576p25 (720*576, 25p)
+ * @vdecRESOLUTION_480p0: 480p (720x480, 0p)
+ * @vdecRESOLUTION_480i0: 480i (720x480, 0i)
+ * @vdecRESOLUTION_576p0: 576p (720x576, 0p)
+ * @vdecRESOLUTION_720p0: 720p (1280x720, 0p)
+ * @vdecRESOLUTION_1080p0: 1080p (1920x1080, 0p)
+ * @vdecRESOLUTION_1080i0: 1080i (1920x1080, 0i)
+ */
 enum {
-	vdecRESOLUTION_CUSTOM	= 0x00000000, /* custom */
-	vdecRESOLUTION_480i	= 0x00000001, /* 480i */
-	vdecRESOLUTION_1080i	= 0x00000002, /* 1080i (1920x1080, 60i) */
-	vdecRESOLUTION_NTSC	= 0x00000003, /* NTSC (720x483, 60i) */
-	vdecRESOLUTION_480p	= 0x00000004, /* 480p (720x480, 60p) */
-	vdecRESOLUTION_720p	= 0x00000005, /* 720p (1280x720, 60p) */
-	vdecRESOLUTION_PAL1	= 0x00000006, /* PAL_1 (720x576, 50i) */
-	vdecRESOLUTION_1080i25	= 0x00000007, /* 1080i25 (1920x1080, 50i) */
-	vdecRESOLUTION_720p50	= 0x00000008, /* 720p50 (1280x720, 50p) */
-	vdecRESOLUTION_576p	= 0x00000009, /* 576p (720x576, 50p) */
-	vdecRESOLUTION_1080i29_97 = 0x0000000A, /* 1080i (1920x1080, 59.94i) */
-	vdecRESOLUTION_720p59_94  = 0x0000000B, /* 720p (1280x720, 59.94p) */
-	vdecRESOLUTION_SD_DVD	= 0x0000000C, /* SD DVD (720x483, 60i) */
-	vdecRESOLUTION_480p656	= 0x0000000D, /* 480p (720x480, 60p), output bus width 8 bit, clock 74.25MHz */
-	vdecRESOLUTION_1080p23_976 = 0x0000000E, /* 1080p23_976 (1920x1080, 23.976p) */
-	vdecRESOLUTION_720p23_976  = 0x0000000F, /* 720p23_976 (1280x720p, 23.976p) */
-	vdecRESOLUTION_240p29_97   = 0x00000010, /* 240p (1440x240, 29.97p ) */
-	vdecRESOLUTION_240p30	= 0x00000011, /* 240p (1440x240, 30p) */
-	vdecRESOLUTION_288p25	= 0x00000012, /* 288p (1440x288p, 25p) */
-	vdecRESOLUTION_1080p29_97 = 0x00000013, /* 1080p29_97 (1920x1080, 29.97p) */
-	vdecRESOLUTION_1080p30	= 0x00000014, /* 1080p30 (1920x1080, 30p) */
-	vdecRESOLUTION_1080p24	= 0x00000015, /* 1080p24 (1920x1080, 24p) */
-	vdecRESOLUTION_1080p25	= 0x00000016, /* 1080p25 (1920x1080, 25p) */
-	vdecRESOLUTION_720p24	= 0x00000017, /* 720p24 (1280x720, 25p) */
-	vdecRESOLUTION_720p29_97  = 0x00000018, /* 720p29.97 (1280x720, 29.97p) */
-	vdecRESOLUTION_480p23_976 = 0x00000019, /* 480p23.976 (720*480, 23.976) */
-	vdecRESOLUTION_480p29_97  = 0x0000001A, /* 480p29.976 (720*480, 29.97p) */
-	vdecRESOLUTION_576p25	= 0x0000001B, /* 576p25 (720*576, 25p) */
+	vdecRESOLUTION_CUSTOM	= 0x00000000,
+	vdecRESOLUTION_480i	= 0x00000001,
+	vdecRESOLUTION_1080i	= 0x00000002,
+	vdecRESOLUTION_NTSC	= 0x00000003,
+	vdecRESOLUTION_480p	= 0x00000004,
+	vdecRESOLUTION_720p	= 0x00000005,
+	vdecRESOLUTION_PAL1	= 0x00000006,
+	vdecRESOLUTION_1080i25	= 0x00000007,
+	vdecRESOLUTION_720p50	= 0x00000008,
+	vdecRESOLUTION_576p	= 0x00000009,
+	vdecRESOLUTION_1080i29_97 = 0x0000000A,
+	vdecRESOLUTION_720p59_94  = 0x0000000B,
+	vdecRESOLUTION_SD_DVD	= 0x0000000C,
+	vdecRESOLUTION_480p656	= 0x0000000D,
+	vdecRESOLUTION_1080p23_976 = 0x0000000E,
+	vdecRESOLUTION_720p23_976  = 0x0000000F,
+	vdecRESOLUTION_240p29_97   = 0x00000010,
+	vdecRESOLUTION_240p30	= 0x00000011,
+	vdecRESOLUTION_288p25	= 0x00000012,
+	vdecRESOLUTION_1080p29_97 = 0x00000013,
+	vdecRESOLUTION_1080p30	= 0x00000014,
+	vdecRESOLUTION_1080p24	= 0x00000015,
+	vdecRESOLUTION_1080p25	= 0x00000016,
+	vdecRESOLUTION_720p24	= 0x00000017,
+	vdecRESOLUTION_720p29_97  = 0x00000018,
+	vdecRESOLUTION_480p23_976 = 0x00000019,
+	vdecRESOLUTION_480p29_97  = 0x0000001A,
+	vdecRESOLUTION_576p25	= 0x0000001B,
 	/* For Zero Frame Rate */
-	vdecRESOLUTION_480p0	= 0x0000001C, /* 480p (720x480, 0p) */
-	vdecRESOLUTION_480i0	= 0x0000001D, /* 480i (720x480, 0i) */
-	vdecRESOLUTION_576p0	= 0x0000001E, /* 576p (720x576, 0p) */
-	vdecRESOLUTION_720p0	= 0x0000001F, /* 720p (1280x720, 0p) */
-	vdecRESOLUTION_1080p0	= 0x00000020, /* 1080p (1920x1080, 0p) */
-	vdecRESOLUTION_1080i0	= 0x00000021, /* 1080i (1920x1080, 0i) */
+	vdecRESOLUTION_480p0	= 0x0000001C,
+	vdecRESOLUTION_480i0	= 0x0000001D,
+	vdecRESOLUTION_576p0	= 0x0000001E,
+	vdecRESOLUTION_720p0	= 0x0000001F,
+	vdecRESOLUTION_1080p0	= 0x00000020,
+	vdecRESOLUTION_1080i0	= 0x00000021,
 };
 
 /* Bit definitions for 'flags' field */
@@ -359,14 +395,23 @@ enum _BC_OUTPUT_FORMAT {
 	MODE422_YUY2			= 0x1,
 	MODE422_UYVY			= 0x2,
 };
-
+/**
+ * struct BC_PIC_INFO_BLOCK
+ * @timeStam;: Timestamp
+ * @picture_number: Ordinal display number
+ * @width:  pixels
+ * @height:  pixels
+ * @chroma_format:  0x420, 0x422 or 0x444
+ * @n_drop;:  number of non-reference frames
+ *	remaining to be dropped
+ */
 struct BC_PIC_INFO_BLOCK {
 	/* Common fields. */
-	uint64_t	timeStamp;	/* Timestamp */
-	uint32_t	picture_number;	/* Ordinal display number  */
-	uint32_t	width;		/* pixels	    */
-	uint32_t	height;		/* pixels	    */
-	uint32_t	chroma_format;	/* 0x420, 0x422 or 0x444 */
+	uint64_t	timeStamp;
+	uint32_t	picture_number;
+	uint32_t	width;
+	uint32_t	height;
+	uint32_t	chroma_format;
 	uint32_t	pulldown;
 	uint32_t	flags;
 	uint32_t	frame_rate;
@@ -376,7 +421,8 @@ struct BC_PIC_INFO_BLOCK {
 	uint32_t	sess_num;
 	uint32_t	ycom;
 	uint32_t	custom_aspect_ratio_width_height;
-	uint32_t	n_drop;	/* number of non-reference frames remaining to be dropped */
+	uint32_t	n_drop;	/* number of non-reference frames
+					remaining to be dropped */
 
 	/* Protocol-specific extensions. */
 	union {
@@ -390,43 +436,71 @@ struct BC_PIC_INFO_BLOCK {
 /*------------------------------------------------------*
  *    ProcOut Info					*
  *------------------------------------------------------*/
-/* Optional flags for ProcOut Interface.*/
+
+/**
+ * enum POUT_OPTIONAL_IN_FLAGS - Optional flags for ProcOut Interface.
+ * @BC_POUT_FLAGS_YV12:  Copy Data in YV12 format
+ * @BC_POUT_FLAGS_STRIDE:  Stride size is valid.
+ * @BC_POUT_FLAGS_SIZE:  Take size information from Application
+ * @BC_POUT_FLAGS_INTERLACED:  copy only half the bytes
+ * @BC_POUT_FLAGS_INTERLEAVED:  interleaved frame
+ * @:  * @BC_POUT_FLAGS_FMT_CHANGE:  Data is not VALID when this flag is set
+ * @BC_POUT_FLAGS_PIB_VALID:  PIB Information valid
+ * @BC_POUT_FLAGS_ENCRYPTED:  Data is encrypted.
+ * @BC_POUT_FLAGS_FLD_BOT:  Bottom Field data
+ */
 enum POUT_OPTIONAL_IN_FLAGS_ {
 	/* Flags from App to Device */
-	BC_POUT_FLAGS_YV12	  = 0x01,	/* Copy Data in YV12 format */
-	BC_POUT_FLAGS_STRIDE	  = 0x02,	/* Stride size is valid. */
-	BC_POUT_FLAGS_SIZE	  = 0x04,	/* Take size information from Application */
-	BC_POUT_FLAGS_INTERLACED  = 0x08,	/* copy only half the bytes */
-	BC_POUT_FLAGS_INTERLEAVED = 0x10,	/* interleaved frame */
+	BC_POUT_FLAGS_YV12	  = 0x01,
+	BC_POUT_FLAGS_STRIDE	  = 0x02,
+	BC_POUT_FLAGS_SIZE	  = 0x04,
+	BC_POUT_FLAGS_INTERLACED  = 0x08,
+	BC_POUT_FLAGS_INTERLEAVED = 0x10,
 
 	/* Flags from Device to APP */
-	BC_POUT_FLAGS_FMT_CHANGE  = 0x10000,	/* Data is not VALID when this flag is set */
-	BC_POUT_FLAGS_PIB_VALID	  = 0x20000,	/* PIB Information valid */
-	BC_POUT_FLAGS_ENCRYPTED	  = 0x40000,	/* Data is encrypted. */
-	BC_POUT_FLAGS_FLD_BOT	  = 0x80000,	/* Bottom Field data */
+	BC_POUT_FLAGS_FMT_CHANGE  = 0x10000,
+	BC_POUT_FLAGS_PIB_VALID	  = 0x20000,
+	BC_POUT_FLAGS_ENCRYPTED	  = 0x40000,
+	BC_POUT_FLAGS_FLD_BOT	  = 0x80000,
 };
 
-typedef enum BC_STATUS(*dts_pout_callback)(void  *shnd, uint32_t width, uint32_t height, uint32_t stride, void *pOut);
+typedef enum BC_STATUS(*dts_pout_callback)(void  *shnd, uint32_t width,
+			uint32_t height, uint32_t stride, void *pOut);
 
 /* Line 21 Closed Caption */
 /* User Data */
 #define MAX_UD_SIZE		1792	/* 1920 - 128 */
 
+/**
+ * struct BC_DTS_PROC_OUT
+ * @Ybuff: Caller Supplied buffer for Y data
+ * @YbuffSz: Caller Supplied Y buffer size
+ * @YBuffDoneSz: Transferred Y datasize
+ * @*UVbuff: Caller Supplied buffer for UV data
+ * @UVbuffSz: Caller Supplied UV buffer size
+ * @UVBuffDoneSz: Transferred UV data size
+ * @StrideSz: Caller supplied Stride Size
+ * @PoutFlags: Call IN Flags
+ * @discCnt: Picture discontinuity count
+ * @PicInfo: Picture Information Block Data
+ * @b422Mode: Picture output Mode
+ * @bPibEnc: PIB encrypted
+ */
 struct BC_DTS_PROC_OUT {
-	uint8_t		*Ybuff;			/* Caller Supplied buffer for Y data */
-	uint32_t	YbuffSz;		/* Caller Supplied Y buffer size */
-	uint32_t	YBuffDoneSz;		/* Transferred Y datasize */
+	uint8_t		*Ybuff;
+	uint32_t	YbuffSz;
+	uint32_t	YBuffDoneSz;
 
-	uint8_t		*UVbuff;		/* Caller Supplied buffer for UV data */
-	uint32_t	UVbuffSz;		/* Caller Supplied UV buffer size */
-	uint32_t	UVBuffDoneSz;		/* Transferred UV data size */
+	uint8_t		*UVbuff;
+	uint32_t	UVbuffSz;
+	uint32_t	UVBuffDoneSz;
 
-	uint32_t	StrideSz;		/* Caller supplied Stride Size */
-	uint32_t	PoutFlags;		/* Call IN Flags */
+	uint32_t	StrideSz;
+	uint32_t	PoutFlags;
 
-	uint32_t	discCnt;		/* Picture discontinuity count */
+	uint32_t	discCnt;
 
-	struct BC_PIC_INFO_BLOCK PicInfo;		/* Picture Information Block Data */
+	struct BC_PIC_INFO_BLOCK PicInfo;
 
 	/* Line 21 Closed Caption */
 	/* User Data */
@@ -436,39 +510,47 @@ struct BC_DTS_PROC_OUT {
 	void		*hnd;
 	dts_pout_callback AppCallBack;
 	uint8_t		DropFrames;
-	uint8_t		b422Mode;		/* Picture output Mode */
-	uint8_t		bPibEnc;		/* PIB encrypted */
+	uint8_t		b422Mode;
+	uint8_t		bPibEnc;
 	uint8_t		bRevertScramble;
 
 };
-
+/**
+ * struct BC_DTS_STATUS
+ * @ReadyListCount: Number of frames in ready list (reported by driver)
+ * @PowerStateChange: Number of active state power
+ *	transitions (reported by driver)
+ * @FramesDropped:  Number of frames dropped.  (reported by DIL)
+ * @FramesCaptured: Number of frames captured. (reported by DIL)
+ * @FramesRepeated: Number of frames repeated. (reported by DIL)
+ * @InputCount:	Times compressed video has been sent to the HW.
+ *	i.e. Successful DtsProcInput() calls (reported by DIL)
+ * @InputTotalSize: Amount of compressed video that has been sent to the HW.
+ *	(reported by DIL)
+ * @InputBusyCount: Times compressed video has attempted to be sent to the HW
+ *	but the input FIFO was full. (reported by DIL)
+ * @PIBMissCount: Amount of times a PIB is invalid. (reported by DIL)
+ * @cpbEmptySize: supported only for H.264, specifically changed for
+ *	Adobe. Report size of CPB buffer available. (reported by DIL)
+ * @NextTimeStamp: TimeStamp of the next picture that will be returned
+ *	by a call to ProcOutput. Added for Adobe. Reported
+ *	back from the driver
+ */
 struct BC_DTS_STATUS {
-	uint8_t		ReadyListCount;	/* Number of frames in ready list (reported by driver) */
-	uint8_t		FreeListCount;	/* Number of frame buffers free.  (reported by driver) */
-	uint8_t		PowerStateChange; /* Number of active state power transitions (reported by driver) */
+	uint8_t		ReadyListCount;
+	uint8_t		FreeListCount;
+	uint8_t		PowerStateChange;
 	uint8_t		reserved_[1];
-
-	uint32_t	FramesDropped;	/* Number of frames dropped.  (reported by DIL) */
-	uint32_t	FramesCaptured;	/* Number of frames captured. (reported by DIL) */
-	uint32_t	FramesRepeated;	/* Number of frames repeated. (reported by DIL) */
-
-	uint32_t	InputCount;	/* Times compressed video has been sent to the HW.
-					 * i.e. Successful DtsProcInput() calls (reported by DIL) */
-	uint64_t	InputTotalSize;	/* Amount of compressed video that has been sent to the HW.
-					 * (reported by DIL) */
-	uint32_t	InputBusyCount;	/* Times compressed video has attempted to be sent to the HW
-					 * but the input FIFO was full. (reported by DIL) */
-
-	uint32_t	PIBMissCount;	/* Amount of times a PIB is invalid. (reported by DIL) */
-
-	uint32_t	cpbEmptySize;	/* supported only for H.264, specifically changed for
-					 * Adobe. Report size of CPB buffer available.
-					 * Reported by DIL */
-	uint64_t	NextTimeStamp;	/* TimeStamp of the next picture that will be returned
-					 * by a call to ProcOutput. Added for Adobe. Reported
-					 * back from the driver */
+	uint32_t	FramesDropped;
+	uint32_t	FramesCaptured;
+	uint32_t	FramesRepeated;
+	uint32_t	InputCount;
+	uint64_t	InputTotalSize;
+	uint32_t	InputBusyCount;
+	uint32_t	PIBMissCount;
+	uint32_t	cpbEmptySize;
+	uint64_t	NextTimeStamp;
 	uint8_t		reserved__[16];
-
 };
 
 #define BC_SWAP32(_v)			\
diff --git a/drivers/staging/cxt1e1/comet.h b/drivers/staging/cxt1e1/comet.h
index 5cb3afda0112..e06da4a6f6f6 100644
--- a/drivers/staging/cxt1e1/comet.h
+++ b/drivers/staging/cxt1e1/comet.h
@@ -1,7 +1,3 @@
-/*
- * $Id: comet.h,v 1.3 2005/09/28 00:10:07 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_COMET_H_
 #define _INC_COMET_H_
 
@@ -23,27 +19,9 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.3 $
- * Last changed on $Date: 2005/09/28 00:10:07 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: comet.h,v $
- * Revision 1.3  2005/09/28 00:10:07  rickd
- * Add RCS header. Switch to structure usage.
- *
- * Revision 1.2  2005/04/28 23:43:03  rickd
- * Add RCS tracking heading.
- *
- *-----------------------------------------------------------------------------
  */
 
-#if defined(__FreeBSD__) || defined (__NetBSD__)
-#include <sys/types.h>
-#else
 #include <linux/types.h>
-#endif
-
 
 #define VINT32  volatile u_int32_t
 
diff --git a/drivers/staging/cxt1e1/comet_tables.c b/drivers/staging/cxt1e1/comet_tables.c
index db1293c71a6d..84931117da35 100644
--- a/drivers/staging/cxt1e1/comet_tables.c
+++ b/drivers/staging/cxt1e1/comet_tables.c
@@ -1,7 +1,3 @@
-/*
- * $Id: comet_tables.c,v 1.2 2005/10/17 23:55:27 rickd PMCC4_3_1B $
- */
-
 /*-----------------------------------------------------------------------------
  * comet_tables.c - waveform tables for the PM4351 'COMET'
  *
@@ -20,28 +16,8 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.2 $
- * Last changed on $Date: 2005/10/17 23:55:27 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: comet_tables.c,v $
- * Revision 1.2  2005/10/17 23:55:27  rickd
- * Note that 75 Ohm transmit waveform is not supported on PMCC4.
- *
- * Revision 1.1  2005/09/28 00:10:05  rickd
- * Cosmetic alignment of tables for readability.
- *
- * Revision 1.0  2005/05/10 22:47:53  rickd
- * Initial revision
- *
- *-----------------------------------------------------------------------------
  */
 
-char SBEid_pmcc4_comet_tblc[] =
-  "@(#)comet_tables.c - $Revision: 1.2 $      (c) Copyright 2004-2005 SBE, Inc.";
-
-
 #include <linux/types.h>
 
 /*****************************************************************************
diff --git a/drivers/staging/cxt1e1/comet_tables.h b/drivers/staging/cxt1e1/comet_tables.h
index 80424a26a169..3e2e5badf787 100644
--- a/drivers/staging/cxt1e1/comet_tables.h
+++ b/drivers/staging/cxt1e1/comet_tables.h
@@ -1,7 +1,3 @@
-/*
- * $Id: comet_tables.h,v 1.5 2006/01/02 22:37:31 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_COMET_TBLS_H_
 #define _INC_COMET_TBLS_H_
 
@@ -23,26 +19,6 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.5 $
- * Last changed on $Date: 2006/01/02 22:37:31 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: comet_tables.h,v $
- * Revision 1.5  2006/01/02 22:37:31  rickd
- * Double indexed arrays need sizings to avoid CC errors under
- * gcc 4.0.0
- *
- * Revision 1.4  2005/10/17 23:55:28  rickd
- * The 75 Ohm transmit waveform is not supported on PMCC4.
- *
- * Revision 1.3  2005/09/28 00:10:08  rickd
- * Add GNU License info. Structures moved to -C- file.
- *
- * Revision 1.2  2005/04/28 23:43:04  rickd
- * Add RCS tracking heading.
- *
- *-----------------------------------------------------------------------------
  */
 
 
diff --git a/drivers/staging/cxt1e1/libsbew.h b/drivers/staging/cxt1e1/libsbew.h
index ae8f06d05bed..4254c0426db9 100644
--- a/drivers/staging/cxt1e1/libsbew.h
+++ b/drivers/staging/cxt1e1/libsbew.h
@@ -1,7 +1,3 @@
-/*
- * $Id: libsbew.h,v 2.1 2005/10/27 18:54:19 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_LIBSBEW_H_
 #define _INC_LIBSBEW_H_
 
@@ -25,32 +21,8 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 2.1 $
- * Last changed on $Date: 2005/10/27 18:54:19 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: libsbew.h,v $
- * Revision 2.1  2005/10/27 18:54:19  rickd
- * Add E1PLAIN support.
- *
- * Revision 2.0  2005/09/28 00:10:08  rickd
- * Customized for PMCC4 comet-per-port design.
- *
- * Revision 1.15  2005/03/29 00:51:31  rickd
- * File imported from C1T3 port, Revision 1.15
- *-----------------------------------------------------------------------------
  */
 
-#ifndef __KERNEL__
-#include <sys/types.h>
-#endif
-
-#ifdef __cplusplus
-extern      "C"
-{
-#endif
-
 /********************************/
 /**  set driver logging level  **/
 /********************************/
@@ -574,8 +546,4 @@ struct sbecom_port_param
     extern int  wancfg_set_tsioc (wcfg_t *, struct wanc1t3_ts_param *);
 #endif
 
-#ifdef __cplusplus
-}
-#endif
-
 #endif                          /*** _INC_LIBSBEW_H_ ***/
diff --git a/drivers/staging/cxt1e1/musycc.c b/drivers/staging/cxt1e1/musycc.c
index 5cc3423a6646..90c0f1e30f65 100644
--- a/drivers/staging/cxt1e1/musycc.c
+++ b/drivers/staging/cxt1e1/musycc.c
@@ -1,7 +1,3 @@
-/*
- * $Id: musycc.c,v 2.1 2007/08/15 23:32:17 rickd PMCC4_3_1B $
- */
-
 unsigned int max_intcnt = 0;
 unsigned int max_bh = 0;
 
@@ -24,53 +20,8 @@ unsigned int max_bh = 0;
  * For further information, contact via email: support@onestopsystems.com
  * One Stop Systems, Inc.  Escondido, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 2.1 $
- * Last changed on $Date: 2007/08/15 23:32:17 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: musycc.c,v $
- * Revision 2.1  2007/08/15 23:32:17  rickd
- * Use 'if 0' instead of GNU comment delimeter to avoid line wrap induced compiler errors.
- *
- * Revision 2.0  2007/08/15 22:13:20  rickd
- * Update to printf pointer %p usage and correct some UINT to ULONG for
- * 64bit comptibility.
- *
- * Revision 1.7  2006/04/21 00:56:40  rickd
- * workqueue files now prefixed with <sbecom> prefix.
- *
- * Revision 1.6  2005/10/27 18:54:19  rickd
- * Clean out old code.  Default to HDLC_FCS16, not TRANS.
- *
- * Revision 1.5  2005/10/17 23:55:28  rickd
- * Initial port of NCOMM support patches from original work found
- * in pmc_c4t1e1 as updated by NCOMM.  Ref: CONFIG_SBE_PMCC4_NCOMM.
- *
- * Revision 1.4  2005/10/13 20:35:25  rickd
- * Cleanup warning for unused <flags> variable.
- *
- * Revision 1.3  2005/10/13 19:19:22  rickd
- * Disable redundant driver removal cleanup code.
- *
- * Revision 1.2  2005/10/11 18:36:16  rickd
- * Clean up warning messages caused by de-implemented some <flags> associated
- * with spin_lock() removals.
- *
- * Revision 1.1  2005/10/05 00:45:28  rickd
- * Re-enable xmit on flow-controlled and full channel to fix restart hang.
- * Add some temp spin-lock debug code (rld_spin_owner).
- *
- * Revision 1.0  2005/09/28 00:10:06  rickd
- * Initial release for C4T1E1 support. Lots of transparent
- * mode updates.
- *
- *-----------------------------------------------------------------------------
  */
 
-char        SBEid_pmcc4_musyccc[] =
-"@(#)musycc.c - $Revision: 2.1 $      (c) Copyright 2004-2006 SBE, Inc.";
-
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/types.h>
diff --git a/drivers/staging/cxt1e1/musycc.h b/drivers/staging/cxt1e1/musycc.h
index 68f3660f4477..cf6b54e15689 100644
--- a/drivers/staging/cxt1e1/musycc.h
+++ b/drivers/staging/cxt1e1/musycc.h
@@ -1,7 +1,3 @@
-/*
- * $Id: musycc.h,v 1.3 2005/09/28 00:10:08 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_MUSYCC_H_
 #define _INC_MUSYCC_H_
 
@@ -24,36 +20,13 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.3 $
- * Last changed on $Date: 2005/09/28 00:10:08 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: musycc.h,v $
- * Revision 1.3  2005/09/28 00:10:08  rickd
- * Add GNU license info. Add PMCC4 PCI/DevIDs.  Implement new
- * musycc reg&bits namings. Use PORTMAP_0 GCD grouping.
- *
- * Revision 1.2  2005/04/28 23:43:04  rickd
- * Add RCS tracking heading.
- *
- *-----------------------------------------------------------------------------
  */
 
-#if defined (__FreeBSD__) || defined (__NetBSD__)
-#include <sys/types.h>
-#else
 #include <linux/types.h>
-#endif
 
 #define VINT8   volatile u_int8_t
 #define VINT32  volatile u_int32_t
 
-#ifdef __cplusplus
-extern      "C"
-{
-#endif
-
 #include "pmcc4_defs.h"
 
 
@@ -448,10 +421,6 @@ extern      "C"
 /*  This must be defined on an entire channel group (Port) basis */
 #define SUERM_THRESHOLD     0x1f
 
-#ifdef __cplusplus
-}
-#endif
-
 #undef VINT32
 #undef VINT8
 
diff --git a/drivers/staging/cxt1e1/ossiRelease.c b/drivers/staging/cxt1e1/ossiRelease.c
index a56029866c2d..f17a902e95e3 100644
--- a/drivers/staging/cxt1e1/ossiRelease.c
+++ b/drivers/staging/cxt1e1/ossiRelease.c
@@ -1,7 +1,3 @@
-/*
- * $Id: ossiRelease.c,v 1.2 2008/05/08 20:14:03 rdobbs PMCC4_3_1B $
- */
-
 /*-----------------------------------------------------------------------------
  * ossiRelease.c -
  *
@@ -26,14 +22,8 @@
  * One Stop Systems, Inc.  Escondido, California  U.S.A.
  *
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.2 $
- * Last changed on $Date: 2008/05/08 20:14:03 $
- * Changed by $Author: rdobbs $
- *-----------------------------------------------------------------------------
  */
 
-
 char pmcc4_OSSI_release[] = "$Release: PMCC4_3_1B,  Copyright (c) 2008 One Stop Systems$";
 
 /***  End-of-File  ***/
diff --git a/drivers/staging/cxt1e1/pmc93x6_eeprom.h b/drivers/staging/cxt1e1/pmc93x6_eeprom.h
index c3ada87efd26..96c48cb83260 100644
--- a/drivers/staging/cxt1e1/pmc93x6_eeprom.h
+++ b/drivers/staging/cxt1e1/pmc93x6_eeprom.h
@@ -1,7 +1,3 @@
-/*
- * $Id: pmc93x6_eeprom.h,v 1.1 2005/09/28 00:10:08 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_PMC93X6_EEPROM_H_
 #define _INC_PMC93X6_EEPROM_H_
 
@@ -23,26 +19,9 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- *-----------------------------------------------------------------------------
- * $Log: pmc93x6_eeprom.h,v $
- * Revision 1.1  2005/09/28 00:10:08  rickd
- * pmc_verify_cksum return value is char.
- *
- * Revision 1.0  2005/05/04 17:20:51  rickd
- * Initial revision
- *
- * Revision 1.0  2005/04/22 23:48:48  rickd
- * Initial revision
- *
- *-----------------------------------------------------------------------------
  */
 
-#if defined (__FreeBSD__) || defined (__NetBSD__)
-#include <sys/types.h>
-#else
 #include <linux/types.h>
-#endif
 
 #ifdef __KERNEL__
 
diff --git a/drivers/staging/cxt1e1/pmcc4.h b/drivers/staging/cxt1e1/pmcc4.h
index e046b87763a2..b0ed4ad13011 100644
--- a/drivers/staging/cxt1e1/pmcc4.h
+++ b/drivers/staging/cxt1e1/pmcc4.h
@@ -1,7 +1,3 @@
-/*
- * $Id: pmcc4.h,v 1.4 2005/11/01 19:24:48 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_PMCC4_H_
 #define _INC_PMCC4_H_
 
@@ -23,49 +19,15 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.4 $
- * Last changed on $Date: 2005/11/01 19:24:48 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: pmcc4.h,v $
- * Revision 1.4  2005/11/01 19:24:48  rickd
- * Remove de-implement function prototypes.  Several <int> to
- * <status_t> changes for consistent usage of same.
- *
- * Revision 1.3  2005/09/28 00:10:08  rickd
- * Add GNU license info. Use config params from libsbew.h
- *
- * Revision 1.2  2005/04/28 23:43:03  rickd
- * Add RCS tracking heading.
- *
- *-----------------------------------------------------------------------------
  */
 
-
-#if defined(__FreeBSD__) || defined(__NetBSD__)
-#include <sys/types.h>
-#else
-#ifndef __KERNEL__
-#include <sys/types.h>
-#else
 #include <linux/types.h>
-#endif
-#endif
-
-
 
 typedef int status_t;
 
 #define SBE_DRVR_FAIL     0
 #define SBE_DRVR_SUCCESS  1
 
-#ifdef __cplusplus
-extern      "C"
-{
-#endif
-
-
 /********************/
 /* PMCC4 memory Map */
 /********************/
@@ -105,10 +67,6 @@ extern      "C"
 #define sbeE1errSMF    0x02
 #define sbeE1CRC       0x01
 
-#ifdef __cplusplus
-}
-#endif
-
 #ifdef __KERNEL__
 
 /*
diff --git a/drivers/staging/cxt1e1/pmcc4_cpld.h b/drivers/staging/cxt1e1/pmcc4_cpld.h
index 6d8f0337aa3e..a51209bc5274 100644
--- a/drivers/staging/cxt1e1/pmcc4_cpld.h
+++ b/drivers/staging/cxt1e1/pmcc4_cpld.h
@@ -1,7 +1,3 @@
-/*
- * $Id: pmcc4_cpld.h,v 1.0 2005/09/28 00:10:08 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_PMCC4_CPLD_H_
 #define _INC_PMCC4_CPLD_H_
 
@@ -23,34 +19,9 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.0 $
- * Last changed on $Date: 2005/09/28 00:10:08 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: pmcc4_cpld.h,v $
- * Revision 1.0  2005/09/28 00:10:08  rickd
- * Initial revision
- *
- *-----------------------------------------------------------------------------
  */
 
-
-#if defined(__FreeBSD__) || defined(__NetBSD__)
-#include <sys/types.h>
-#else
-#ifndef __KERNEL__
-#include <sys/types.h>
-#else
 #include <linux/types.h>
-#endif
-#endif
-
-#ifdef __cplusplus
-extern      "C"
-{
-#endif
-
 
 /********************************/
 /* iSPLD control chip registers */
@@ -117,8 +88,4 @@ extern      "C"
 #define PMCC4_CPLD_INTR_CMT_3   0x04
 #define PMCC4_CPLD_INTR_CMT_4   0x08
 
-#ifdef __cplusplus
-}
-#endif
-
 #endif                          /* _INC_PMCC4_CPLD_H_ */
diff --git a/drivers/staging/cxt1e1/pmcc4_defs.h b/drivers/staging/cxt1e1/pmcc4_defs.h
index a39505f45c29..83ceae4324b2 100644
--- a/drivers/staging/cxt1e1/pmcc4_defs.h
+++ b/drivers/staging/cxt1e1/pmcc4_defs.h
@@ -1,7 +1,3 @@
-/*
- * $Id: pmcc4_defs.h,v 1.0 2005/09/28 00:10:09 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_PMCC4_DEFS_H_
 #define _INC_PMCC4_DEFS_H_
 
@@ -25,16 +21,6 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.0 $
- * Last changed on $Date: 2005/09/28 00:10:09 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: pmcc4_defs.h,v $
- * Revision 1.0  2005/09/28 00:10:09  rickd
- * Initial revision
- *
- *-----------------------------------------------------------------------------
  */
 
 
diff --git a/drivers/staging/cxt1e1/pmcc4_drv.c b/drivers/staging/cxt1e1/pmcc4_drv.c
index e1f07fabd22d..8a7b3a646451 100644
--- a/drivers/staging/cxt1e1/pmcc4_drv.c
+++ b/drivers/staging/cxt1e1/pmcc4_drv.c
@@ -1,8 +1,3 @@
-/*
- * $Id: pmcc4_drv.c,v 3.1 2007/08/15 23:32:17 rickd PMCC4_3_1B $
- */
-
-
 /*-----------------------------------------------------------------------------
  * pmcc4_drv.c -
  *
@@ -22,74 +17,10 @@
  * For further information, contact via email: support@onestopsystems.com
  * One Stop Systems, Inc.  Escondido, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 3.1 $
- * Last changed on $Date: 2007/08/15 23:32:17 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: pmcc4_drv.c,v $
- * Revision 3.1  2007/08/15 23:32:17  rickd
- * Use 'if 0' instead of GNU comment delimeter to avoid line wrap induced compiler errors.
- *
- * Revision 3.0  2007/08/15 22:19:55  rickd
- * Correct sizeof() castings and pi->regram to support 64bit compatibility.
- *
- * Revision 2.10  2006/04/21 00:56:40  rickd
- * workqueue files now prefixed with <sbecom> prefix.
- *
- * Revision 2.9  2005/11/01 19:22:49  rickd
- * Add sanity checks against max_port for ioctl functions.
- *
- * Revision 2.8  2005/10/27 18:59:25  rickd
- * Code cleanup.  Default channel config to HDLC_FCS16.
- *
- * Revision 2.7  2005/10/18 18:16:30  rickd
- * Further NCOMM code repairs - (1) interrupt matrix usage inconsistent
- * for indexing into nciInterrupt[][], code missing double parameters.
- * (2) check input of ncomm interrupt registration cardID for correct
- * boundary values.
- *
- * Revision 2.6  2005/10/17 23:55:28  rickd
- * Initial port of NCOMM support patches from original work found
- * in pmc_c4t1e1 as updated by NCOMM.  Ref: CONFIG_SBE_PMCC4_NCOMM.
- * Corrected NCOMMs wanpmcC4T1E1_getBaseAddress() to correctly handle
- * multiple boards.
- *
- * Revision 2.5  2005/10/13 23:01:28  rickd
- * Correct panic for illegal address reference w/in get_brdinfo on
- * first_if/last_if name acquistion under Linux 2.6
- *
- * Revision 2.4  2005/10/13 21:20:19  rickd
- * Correction of c4_cleanup() wherein next should be acquired before
- * ci_t structure is free'd.
- *
- * Revision 2.3  2005/10/13 19:20:10  rickd
- * Correct driver removal cleanup code for multiple boards.
- *
- * Revision 2.2  2005/10/11 18:34:04  rickd
- * New routine added to determine number of ports (comets) on board.
- *
- * Revision 2.1  2005/10/05 00:48:13  rickd
- * Add some RX activation trace code.
- *
- * Revision 2.0  2005/09/28 00:10:06  rickd
- * Implement 2.6 workqueue for TX/RX restart.  Correction to
- * hardware register boundary checks allows expanded access of MUSYCC.
- * Implement new musycc reg&bits namings.
- *
- *-----------------------------------------------------------------------------
  */
 
-char        OSSIid_pmcc4_drvc[] =
-"@(#)pmcc4_drv.c - $Revision: 3.1 $   (c) Copyright 2002-2007 One Stop Systems, Inc.";
-
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#if defined (__FreeBSD__) || defined (__NetBSD__)
-#include <sys/param.h>
-#include <sys/systm.h>
-#include <sys/errno.h>
-#else
 #include <linux/types.h>
 #include "pmcc4_sysdep.h"
 #include <linux/errno.h>
@@ -98,7 +29,6 @@ char        OSSIid_pmcc4_drvc[] =
 #include <linux/timer.h>        /* include for timer */
 #include <linux/hdlc.h>
 #include <asm/io.h>
-#endif
 
 #include "sbecom_inline_linux.h"
 #include "libsbew.h"
diff --git a/drivers/staging/cxt1e1/pmcc4_ioctls.h b/drivers/staging/cxt1e1/pmcc4_ioctls.h
index 6b8d65673c78..56a1ee39be18 100644
--- a/drivers/staging/cxt1e1/pmcc4_ioctls.h
+++ b/drivers/staging/cxt1e1/pmcc4_ioctls.h
@@ -1,6 +1,3 @@
-/* RCSid: $Header: /home/rickd/projects/pmcc4/include/pmcc4_ioctls.h,v 2.0 2005/09/28 00:10:09 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_PMCC4_IOCTLS_H_
 #define _INC_PMCC4_IOCTLS_H_
 
@@ -22,19 +19,6 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 2.0 $
- * Last changed on $Date: 2005/09/28 00:10:09 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: pmcc4_ioctls.h,v $
- * Revision 2.0  2005/09/28 00:10:09  rickd
- * Add GNU license info. Switch Ioctls to sbe_ioc.h usage.
- *
- * Revision 1.2  2005/04/28 23:43:03  rickd
- * Add RCS tracking heading.
- *
- *-----------------------------------------------------------------------------
  */
 
 #include "sbew_ioc.h"
diff --git a/drivers/staging/cxt1e1/sbe_bid.h b/drivers/staging/cxt1e1/sbe_bid.h
index 1f49b4061fb7..abc2e55f62fc 100644
--- a/drivers/staging/cxt1e1/sbe_bid.h
+++ b/drivers/staging/cxt1e1/sbe_bid.h
@@ -1,7 +1,3 @@
-/*
- * $Id: sbe_bid.h,v 1.0 2005/09/28 00:10:09 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_SBEBID_H_
 #define _INC_SBEBID_H_
 
@@ -24,16 +20,6 @@
  * SBE, Inc.  San Ramon, California  U.S.A.
  *
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.0 $
- * Last changed on $Date: 2005/09/28 00:10:09 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: sbe_bid.h,v $
- * Revision 1.0  2005/09/28 00:10:09  rickd
- * Initial revision
- *
- *-----------------------------------------------------------------------------
  */
 
 #define SBE_BID_REG        0x00000000   /* Board ID Register */
diff --git a/drivers/staging/cxt1e1/sbe_promformat.h b/drivers/staging/cxt1e1/sbe_promformat.h
index 746f81b15c73..aad411d185f5 100644
--- a/drivers/staging/cxt1e1/sbe_promformat.h
+++ b/drivers/staging/cxt1e1/sbe_promformat.h
@@ -1,7 +1,3 @@
-/*
- * $Id: sbe_promformat.h,v 2.2 2005/09/28 00:10:09 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_SBE_PROMFORMAT_H_
 #define _INC_SBE_PROMFORMAT_H_
 
@@ -24,19 +20,6 @@
  * SBE, Inc.  San Ramon, California  U.S.A.
  *
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 2.2 $
- * Last changed on $Date: 2005/09/28 00:10:09 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: sbe_promformat.h,v $
- * Revision 2.2  2005/09/28 00:10:09  rickd
- * Add EEPROM sample from C4T1E1 board.
- *
- * Revision 2.1  2005/05/04 17:18:24  rickd
- * Initial CI.
- *
- *-----------------------------------------------------------------------------
  */
 
 
@@ -85,12 +68,6 @@
  *
  */
 
-#ifdef __cplusplus
-extern      "C"
-{
-#endif
-
-
 #define STRUCT_OFFSET(type, symbol)  ((long)&(((type *)0)->symbol))
 
 /*------------------------------------------------------------------------
@@ -150,8 +127,4 @@ extern      "C"
         FLD_TYPE2   fldType2;
     }           PROMFORMAT;
 
-#ifdef __cplusplus
-}
-#endif
-
 #endif                          /*** _INC_SBE_PROMFORMAT_H_ ***/
diff --git a/drivers/staging/cxt1e1/sbecom_inline_linux.h b/drivers/staging/cxt1e1/sbecom_inline_linux.h
index 9ea2c0c2061f..68ed445ab0cb 100644
--- a/drivers/staging/cxt1e1/sbecom_inline_linux.h
+++ b/drivers/staging/cxt1e1/sbecom_inline_linux.h
@@ -1,7 +1,3 @@
-/*
- * $Id: sbecom_inline_linux.h,v 1.2 2007/08/15 22:51:35 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_SBECOM_INLNX_H_
 #define _INC_SBECOM_INLNX_H_
 
@@ -24,22 +20,6 @@
  * For further information, contact via email: support@onestopsystems.com
  * One Stop Systems, Inc.  Escondido, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.2 $
- * Last changed on $Date: 2007/08/15 22:51:35 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: sbecom_inline_linux.h,v $
- * Revision 1.2  2007/08/15 22:51:35  rickd
- * Remove duplicate version.h entry.
- *
- * Revision 1.1  2007/08/15 22:50:29  rickd
- * Update linux/config for 2.6.18 and later.
- *
- * Revision 1.0  2005/09/28 00:10:09  rickd
- * Initial revision
- *
- *-----------------------------------------------------------------------------
  */
 
 
diff --git a/drivers/staging/cxt1e1/sbeproc.h b/drivers/staging/cxt1e1/sbeproc.h
index 4aa53f44ec0b..e82be6afd1e8 100644
--- a/drivers/staging/cxt1e1/sbeproc.h
+++ b/drivers/staging/cxt1e1/sbeproc.h
@@ -1,7 +1,3 @@
-/*
- * $Id: sbeproc.h,v 1.2 2005/10/17 23:55:28 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_SBEPROC_H_
 #define _INC_SBEPROC_H_
 
@@ -23,22 +19,6 @@
  * For further information, contact via email: support@sbei.com
  * SBE, Inc.  San Ramon, California  U.S.A.
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.2 $
- * Last changed on $Date: 2005/10/17 23:55:28 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: sbeproc.h,v $
- * Revision 1.2  2005/10/17 23:55:28  rickd
- * sbecom_proc_brd_init() is an declared an __init function.
- *
- * Revision 1.1  2005/09/28 00:10:09  rickd
- * Remove unneeded inclusion of c4_private.h.
- *
- * Revision 1.0  2005/05/10 22:21:46  rickd
- * Initial check-in.
- *
- *-----------------------------------------------------------------------------
  */
 
 
diff --git a/drivers/staging/cxt1e1/sbew_ioc.h b/drivers/staging/cxt1e1/sbew_ioc.h
index 14d371904d1f..ce9b15c71894 100644
--- a/drivers/staging/cxt1e1/sbew_ioc.h
+++ b/drivers/staging/cxt1e1/sbew_ioc.h
@@ -1,7 +1,3 @@
-/*
- * $Id: sbew_ioc.h,v 1.0 2005/09/28 00:10:10 rickd PMCC4_3_1B $
- */
-
 #ifndef _INC_SBEWIOC_H_
 #define _INC_SBEWIOC_H_
 
@@ -24,55 +20,9 @@
  * SBE, Inc.  San Ramon, California  U.S.A.
  *
  *-----------------------------------------------------------------------------
- * RCS info:
- * RCS revision: $Revision: 1.0 $
- * Last changed on $Date: 2005/09/28 00:10:10 $
- * Changed by $Author: rickd $
- *-----------------------------------------------------------------------------
- * $Log: sbew_ioc.h,v $
- * Revision 1.0  2005/09/28 00:10:10  rickd
- * Initial revision
- *
- * Revision 1.6  2005/01/11 18:41:01  rickd
- * Add BRDADDR_GET Ioctl.
- *
- * Revision 1.5  2004/09/16 18:55:59  rickd
- * Start setting up for generic framer configuration Ioctl by switch
- * from tect3_framer_param[] to sbecom_framer_param[].
- *
- * Revision 1.4  2004/06/28 17:58:15  rickd
- * Rename IOC_TSMAP_[GS] to IOC_TSIOC_[GS] to support need for
- * multiple formats of data when setting up TimeSlots.
- *
- * Revision 1.3  2004/06/22 21:18:13  rickd
- * read_vec now() ONLY handles a single common wrt_vec array.
- *
- * Revision 1.1  2004/06/10 18:11:34  rickd
- * Add IID_GET Ioctl reference.
- *
- * Revision 1.0  2004/06/08 22:59:38  rickd
- * Initial revision
- *
- * Revision 2.0  2004/06/07 17:49:47  rickd
- * Initial library release following merge of wanc1t3/wan256 into
- * common elements for lib.
- *
- *-----------------------------------------------------------------------------
  */
 
-#ifndef __KERNEL__
-#include <sys/types.h>
-#endif
-#ifdef SunOS
-#include <sys/ioccom.h>
-#else
 #include <linux/ioctl.h>
-#endif
-
-#ifdef __cplusplus
-extern      "C"
-{
-#endif
 
 #define SBE_LOCKFILE   "/tmp/.sbewan.LCK"
 
@@ -128,9 +78,4 @@ extern      "C"
 
 #define SBE_IOC_MAXVEC    1
 
-
-#ifdef __cplusplus
-}
-#endif
-
 #endif                          /*** _INC_SBEWIOC_H_ ***/
diff --git a/drivers/staging/et131x/et131x.c b/drivers/staging/et131x/et131x.c
index 0c1c6ca8c379..2c4069fcd981 100644
--- a/drivers/staging/et131x/et131x.c
+++ b/drivers/staging/et131x/et131x.c
@@ -155,7 +155,6 @@ MODULE_DESCRIPTION("10/100/1000 Base-T Ethernet Driver "
 #define fMP_ADAPTER_FAIL_SEND_MASK	0x3ff00000
 
 /* Some offsets in PCI config space that are actually used. */
-#define ET1310_PCI_MAX_PYLD		0x4C
 #define ET1310_PCI_MAC_ADDRESS		0xA4
 #define ET1310_PCI_EEPROM_STATUS	0xB2
 #define ET1310_PCI_ACK_NACK		0xC0
@@ -178,9 +177,7 @@ MODULE_DESCRIPTION("10/100/1000 Base-T Ethernet Driver "
 
 /* RX defines */
 #define USE_FBR0 1
-
 #define FBR_CHUNKS 32
-
 #define MAX_DESC_PER_RING_RX         1024
 
 /* number of RFDs - default and min */
@@ -195,7 +192,6 @@ MODULE_DESCRIPTION("10/100/1000 Base-T Ethernet Driver "
 #endif
 
 #define NIC_MIN_NUM_RFD		64
-
 #define NUM_PACKETS_HANDLED	256
 
 #define ALCATEL_MULTICAST_PKT	0x01000000
@@ -303,8 +299,8 @@ struct fbr_lookup {
 	dma_addr_t	 ring_physaddr;
 	void		*mem_virtaddrs[MAX_DESC_PER_RING_RX / FBR_CHUNKS];
 	dma_addr_t	 mem_physaddrs[MAX_DESC_PER_RING_RX / FBR_CHUNKS];
-	uint64_t	 real_physaddr;
-	uint64_t	 offset;
+	u64		 real_physaddr;
+	u64		 offset;
 	u32		 local_full;
 	u32		 num_entries;
 	u32		 buffsize;
@@ -427,7 +423,6 @@ struct tx_ring {
 	int since_irq;
 };
 
-/* ADAPTER defines */
 /*
  * Do not change these values: if changed, then change also in respective
  * TXdma and Rxdma engines
@@ -576,8 +571,6 @@ struct et131x_adapter {
 	struct net_device_stats net_stats;
 };
 
-/* EEPROM functions */
-
 static int eeprom_wait_ready(struct pci_dev *pdev, u32 *status)
 {
 	u32 reg;
@@ -795,7 +788,7 @@ static int eeprom_read(struct et131x_adapter *adapter, u32 addr, u8 *pdata)
 	return (status & LBCIF_STATUS_ACK_ERROR) ? -EIO : 0;
 }
 
-int et131x_init_eeprom(struct et131x_adapter *adapter)
+static int et131x_init_eeprom(struct et131x_adapter *adapter)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	u8 eestatus;
@@ -868,7 +861,7 @@ int et131x_init_eeprom(struct et131x_adapter *adapter)
  * et131x_rx_dma_enable - re-start of Rx_DMA on the ET1310.
  * @adapter: pointer to our adapter structure
  */
-void et131x_rx_dma_enable(struct et131x_adapter *adapter)
+static void et131x_rx_dma_enable(struct et131x_adapter *adapter)
 {
 	/* Setup the receive dma configuration register for normal operation */
 	u32 csr =  0x2000;	/* FBR1 enable */
@@ -906,7 +899,7 @@ void et131x_rx_dma_enable(struct et131x_adapter *adapter)
  * et131x_rx_dma_disable - Stop of Rx_DMA on the ET1310
  * @adapter: pointer to our adapter structure
  */
-void et131x_rx_dma_disable(struct et131x_adapter *adapter)
+static void et131x_rx_dma_disable(struct et131x_adapter *adapter)
 {
 	u32 csr;
 	/* Setup the receive dma configuration register */
@@ -928,7 +921,7 @@ void et131x_rx_dma_disable(struct et131x_adapter *adapter)
  *
  * Mainly used after a return to the D0 (full-power) state from a lower state.
  */
-void et131x_tx_dma_enable(struct et131x_adapter *adapter)
+static void et131x_tx_dma_enable(struct et131x_adapter *adapter)
 {
 	/* Setup the transmit dma configuration register for normal
 	 * operation
@@ -948,23 +941,10 @@ static inline void add_12bit(u32 *v, int n)
 }
 
 /**
- * nic_rx_pkts - Checks the hardware for available packets
- * @adapter: pointer to our adapter
- *
- * Returns rfd, a pointer to our MPRFD.
- *
- * Checks the hardware for available packets, using completion ring
- * If packets are available, it gets an RFD from the recv_list, attaches
- * the packet to it, puts the RFD in the RecvPendList, and also returns
- * the pointer to the RFD.
- */
-/* MAC functions */
-
-/**
  * et1310_config_mac_regs1 - Initialize the first part of MAC regs
  * @adapter: pointer to our adapter structure
  */
-void et1310_config_mac_regs1(struct et131x_adapter *adapter)
+static void et1310_config_mac_regs1(struct et131x_adapter *adapter)
 {
 	struct mac_regs __iomem *macregs = &adapter->regs->mac;
 	u32 station1;
@@ -1024,7 +1004,7 @@ void et1310_config_mac_regs1(struct et131x_adapter *adapter)
  * et1310_config_mac_regs2 - Initialize the second part of MAC regs
  * @adapter: pointer to our adapter structure
  */
-void et1310_config_mac_regs2(struct et131x_adapter *adapter)
+static void et1310_config_mac_regs2(struct et131x_adapter *adapter)
 {
 	int32_t delay = 0;
 	struct mac_regs __iomem *mac = &adapter->regs->mac;
@@ -1105,7 +1085,7 @@ void et1310_config_mac_regs2(struct et131x_adapter *adapter)
  *
  * Returns 0 if the device is not in phy coma, 1 if it is in phy coma
  */
-int et1310_in_phy_coma(struct et131x_adapter *adapter)
+static int et1310_in_phy_coma(struct et131x_adapter *adapter)
 {
 	u32 pmcsr;
 
@@ -1114,15 +1094,13 @@ int et1310_in_phy_coma(struct et131x_adapter *adapter)
 	return ET_PM_PHY_SW_COMA & pmcsr ? 1 : 0;
 }
 
-void et1310_setup_device_for_multicast(struct et131x_adapter *adapter)
+static void et1310_setup_device_for_multicast(struct et131x_adapter *adapter)
 {
 	struct rxmac_regs __iomem *rxmac = &adapter->regs->rxmac;
-	uint32_t nIndex;
-	uint32_t result;
-	uint32_t hash1 = 0;
-	uint32_t hash2 = 0;
-	uint32_t hash3 = 0;
-	uint32_t hash4 = 0;
+	u32 hash1 = 0;
+	u32 hash2 = 0;
+	u32 hash3 = 0;
+	u32 hash4 = 0;
 	u32 pm_csr;
 
 	/* If ET131X_PACKET_TYPE_MULTICAST is specified, then we provision
@@ -1131,10 +1109,13 @@ void et1310_setup_device_for_multicast(struct et131x_adapter *adapter)
 	 * driver.
 	 */
 	if (adapter->packet_filter & ET131X_PACKET_TYPE_MULTICAST) {
+		int i;
+
 		/* Loop through our multicast array and set up the device */
-		for (nIndex = 0; nIndex < adapter->multicast_addr_count;
-		     nIndex++) {
-			result = ether_crc(6, adapter->multicast_list[nIndex]);
+		for (i = 0; i < adapter->multicast_addr_count; i++) {
+			u32 result;
+
+			result = ether_crc(6, adapter->multicast_list[i]);
 
 			result = (result & 0x3F800000) >> 23;
 
@@ -1163,7 +1144,7 @@ void et1310_setup_device_for_multicast(struct et131x_adapter *adapter)
 	}
 }
 
-void et1310_setup_device_for_unicast(struct et131x_adapter *adapter)
+static void et1310_setup_device_for_unicast(struct et131x_adapter *adapter)
 {
 	struct rxmac_regs __iomem *rxmac = &adapter->regs->rxmac;
 	u32 uni_pf1;
@@ -1203,7 +1184,7 @@ void et1310_setup_device_for_unicast(struct et131x_adapter *adapter)
 	}
 }
 
-void et1310_config_rxmac_regs(struct et131x_adapter *adapter)
+static void et1310_config_rxmac_regs(struct et131x_adapter *adapter)
 {
 	struct rxmac_regs __iomem *rxmac = &adapter->regs->rxmac;
 	struct phy_device *phydev = adapter->phydev;
@@ -1334,7 +1315,7 @@ void et1310_config_rxmac_regs(struct et131x_adapter *adapter)
 	writel(0x9, &rxmac->ctrl);
 }
 
-void et1310_config_txmac_regs(struct et131x_adapter *adapter)
+static void et1310_config_txmac_regs(struct et131x_adapter *adapter)
 {
 	struct txmac_regs __iomem *txmac = &adapter->regs->txmac;
 
@@ -1348,7 +1329,7 @@ void et1310_config_txmac_regs(struct et131x_adapter *adapter)
 		writel(0x40, &txmac->cf_param);
 }
 
-void et1310_config_macstat_regs(struct et131x_adapter *adapter)
+static void et1310_config_macstat_regs(struct et131x_adapter *adapter)
 {
 	struct macstat_regs __iomem *macstat =
 		&adapter->regs->macstat;
@@ -1422,7 +1403,7 @@ void et1310_config_macstat_regs(struct et131x_adapter *adapter)
  *
  * Returns 0 on success, errno on failure (as defined in errno.h)
  */
-int et131x_phy_mii_read(struct et131x_adapter *adapter, u8 addr,
+static int et131x_phy_mii_read(struct et131x_adapter *adapter, u8 addr,
 	      u8 reg, u16 *value)
 {
 	struct mac_regs __iomem *mac = &adapter->regs->mac;
@@ -1478,7 +1459,7 @@ int et131x_phy_mii_read(struct et131x_adapter *adapter, u8 addr,
 	return status;
 }
 
-int et131x_mii_read(struct et131x_adapter *adapter, u8 reg, u16 *value)
+static int et131x_mii_read(struct et131x_adapter *adapter, u8 reg, u16 *value)
 {
 	struct phy_device *phydev = adapter->phydev;
 
@@ -1498,7 +1479,7 @@ int et131x_mii_read(struct et131x_adapter *adapter, u8 reg, u16 *value)
  *
  * Return 0 on success, errno on failure (as defined in errno.h)
  */
-int et131x_mii_write(struct et131x_adapter *adapter, u8 reg, u16 value)
+static int et131x_mii_write(struct et131x_adapter *adapter, u8 reg, u16 value)
 {
 	struct mac_regs __iomem *mac = &adapter->regs->mac;
 	struct phy_device *phydev = adapter->phydev;
@@ -1564,8 +1545,9 @@ int et131x_mii_write(struct et131x_adapter *adapter, u8 reg, u16 value)
 }
 
 /* Still used from _mac for BIT_READ */
-void et1310_phy_access_mii_bit(struct et131x_adapter *adapter, u16 action,
-			       u16 regnum, u16 bitnum, u8 *value)
+static void et1310_phy_access_mii_bit(struct et131x_adapter *adapter,
+				      u16 action, u16 regnum, u16 bitnum,
+				      u8 *value)
 {
 	u16 reg;
 	u16 mask = 0x0001 << bitnum;
@@ -1591,7 +1573,7 @@ void et1310_phy_access_mii_bit(struct et131x_adapter *adapter, u16 action,
 	}
 }
 
-void et1310_config_flow_control(struct et131x_adapter *adapter)
+static void et1310_config_flow_control(struct et131x_adapter *adapter)
 {
 	struct phy_device *phydev = adapter->phydev;
 
@@ -1632,7 +1614,7 @@ void et1310_config_flow_control(struct et131x_adapter *adapter)
  * et1310_update_macstat_host_counters - Update the local copy of the statistics
  * @adapter: pointer to the adapter structure
  */
-void et1310_update_macstat_host_counters(struct et131x_adapter *adapter)
+static void et1310_update_macstat_host_counters(struct et131x_adapter *adapter)
 {
 	struct ce_stats *stats = &adapter->stats;
 	struct macstat_regs __iomem *macstat =
@@ -1664,7 +1646,7 @@ void et1310_update_macstat_host_counters(struct et131x_adapter *adapter)
  * the statistics held in the adapter structure, checking the "wrap"
  * bit for each counter.
  */
-void et1310_handle_macstat_interrupt(struct et131x_adapter *adapter)
+static void et1310_handle_macstat_interrupt(struct et131x_adapter *adapter)
 {
 	u32 carry_reg1;
 	u32 carry_reg2;
@@ -1714,9 +1696,7 @@ void et1310_handle_macstat_interrupt(struct et131x_adapter *adapter)
 		adapter->stats.tx_collisions	+= COUNTER_WRAP_12_BIT;
 }
 
-/* PHY functions */
-
-int et131x_mdio_read(struct mii_bus *bus, int phy_addr, int reg)
+static int et131x_mdio_read(struct mii_bus *bus, int phy_addr, int reg)
 {
 	struct net_device *netdev = bus->priv;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -1731,7 +1711,7 @@ int et131x_mdio_read(struct mii_bus *bus, int phy_addr, int reg)
 		return value;
 }
 
-int et131x_mdio_write(struct mii_bus *bus, int phy_addr, int reg, u16 value)
+static int et131x_mdio_write(struct mii_bus *bus, int phy_addr, int reg, u16 value)
 {
 	struct net_device *netdev = bus->priv;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -1739,7 +1719,7 @@ int et131x_mdio_write(struct mii_bus *bus, int phy_addr, int reg, u16 value)
 	return et131x_mii_write(adapter, reg, value);
 }
 
-int et131x_mdio_reset(struct mii_bus *bus)
+static int et131x_mdio_reset(struct mii_bus *bus)
 {
 	struct net_device *netdev = bus->priv;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -1759,7 +1739,7 @@ int et131x_mdio_reset(struct mii_bus *bus)
  *	Can't you see that this code processed
  *	Phy power, phy power..
  */
-void et1310_phy_power_down(struct et131x_adapter *adapter, bool down)
+static void et1310_phy_power_down(struct et131x_adapter *adapter, bool down)
 {
 	u16 data;
 
@@ -1775,7 +1755,7 @@ void et1310_phy_power_down(struct et131x_adapter *adapter, bool down)
  * @adapter: pointer to our private adapter structure
  *
  */
-void et131x_xcvr_init(struct et131x_adapter *adapter)
+static void et131x_xcvr_init(struct et131x_adapter *adapter)
 {
 	u16 imr;
 	u16 isr;
@@ -1822,7 +1802,7 @@ void et131x_xcvr_init(struct et131x_adapter *adapter)
  *
  * Used to configure the global registers on the JAGCore
  */
-void et131x_configure_global_regs(struct et131x_adapter *adapter)
+static void et131x_configure_global_regs(struct et131x_adapter *adapter)
 {
 	struct global_regs __iomem *regs = &adapter->regs->global;
 
@@ -1863,13 +1843,11 @@ void et131x_configure_global_regs(struct et131x_adapter *adapter)
 	writel(0, &regs->watchdog_timer);
 }
 
-/* PM functions */
-
 /**
  * et131x_config_rx_dma_regs - Start of Rx_DMA init sequence
  * @adapter: pointer to our adapter structure
  */
-void et131x_config_rx_dma_regs(struct et131x_adapter *adapter)
+static void et131x_config_rx_dma_regs(struct et131x_adapter *adapter)
 {
 	struct rxdma_regs __iomem *rx_dma = &adapter->regs->rxdma;
 	struct rx_ring *rx_local = &adapter->rx_ring;
@@ -1987,7 +1965,7 @@ void et131x_config_rx_dma_regs(struct et131x_adapter *adapter)
  * Configure the transmit engine with the ring buffers we have created
  * and prepare it for use.
  */
-void et131x_config_tx_dma_regs(struct et131x_adapter *adapter)
+static void et131x_config_tx_dma_regs(struct et131x_adapter *adapter)
 {
 	struct txdma_regs __iomem *txdma = &adapter->regs->txdma;
 
@@ -2017,7 +1995,7 @@ void et131x_config_tx_dma_regs(struct et131x_adapter *adapter)
  *
  * Returns 0 on success, errno on failure (as defined in errno.h)
  */
-void et131x_adapter_setup(struct et131x_adapter *adapter)
+static void et131x_adapter_setup(struct et131x_adapter *adapter)
 {
 	/* Configure the JAGCore */
 	et131x_configure_global_regs(adapter);
@@ -2044,7 +2022,7 @@ void et131x_adapter_setup(struct et131x_adapter *adapter)
  * et131x_soft_reset - Issue a soft reset to the hardware, complete for ET1310
  * @adapter: pointer to our private adapter structure
  */
-void et131x_soft_reset(struct et131x_adapter *adapter)
+static void et131x_soft_reset(struct et131x_adapter *adapter)
 {
 	/* Disable MAC Core */
 	writel(0xc00f0000, &adapter->regs->mac.cfg1);
@@ -2062,7 +2040,7 @@ void et131x_soft_reset(struct et131x_adapter *adapter)
  *	Enable the appropriate interrupts on the ET131x according to our
  *	configuration
  */
-void et131x_enable_interrupts(struct et131x_adapter *adapter)
+static void et131x_enable_interrupts(struct et131x_adapter *adapter)
 {
 	u32 mask;
 
@@ -2082,7 +2060,7 @@ void et131x_enable_interrupts(struct et131x_adapter *adapter)
  *
  *	Block all interrupts from the et131x device at the device itself
  */
-void et131x_disable_interrupts(struct et131x_adapter *adapter)
+static void et131x_disable_interrupts(struct et131x_adapter *adapter)
 {
 	/* Disable all global interrupts */
 	writel(INT_MASK_DISABLE, &adapter->regs->global.int_mask);
@@ -2092,7 +2070,7 @@ void et131x_disable_interrupts(struct et131x_adapter *adapter)
  * et131x_tx_dma_disable - Stop of Tx_DMA on the ET1310
  * @adapter: pointer to our adapter structure
  */
-void et131x_tx_dma_disable(struct et131x_adapter *adapter)
+static void et131x_tx_dma_disable(struct et131x_adapter *adapter)
 {
 	/* Setup the tramsmit dma configuration register */
 	writel(ET_TXDMA_CSR_HALT|ET_TXDMA_SNGL_EPKT,
@@ -2103,7 +2081,7 @@ void et131x_tx_dma_disable(struct et131x_adapter *adapter)
  * et131x_enable_txrx - Enable tx/rx queues
  * @netdev: device to be enabled
  */
-void et131x_enable_txrx(struct net_device *netdev)
+static void et131x_enable_txrx(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
@@ -2123,7 +2101,7 @@ void et131x_enable_txrx(struct net_device *netdev)
  * et131x_disable_txrx - Disable tx/rx queues
  * @netdev: device to be disabled
  */
-void et131x_disable_txrx(struct net_device *netdev)
+static void et131x_disable_txrx(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
@@ -2142,7 +2120,7 @@ void et131x_disable_txrx(struct net_device *netdev)
  * et131x_init_send - Initialize send data structures
  * @adapter: pointer to our private adapter structure
  */
-void et131x_init_send(struct et131x_adapter *adapter)
+static void et131x_init_send(struct et131x_adapter *adapter)
 {
 	struct tcb *tcb;
 	u32 ct;
@@ -2192,7 +2170,7 @@ void et131x_init_send(struct et131x_adapter *adapter)
  *       indicating linkup status, call the MPDisablePhyComa routine to
  *             restore JAGCore and gigE PHY
  */
-void et1310_enable_phy_coma(struct et131x_adapter *adapter)
+static void et1310_enable_phy_coma(struct et131x_adapter *adapter)
 {
 	unsigned long flags;
 	u32 pmcsr;
@@ -2231,7 +2209,7 @@ void et1310_enable_phy_coma(struct et131x_adapter *adapter)
  * et1310_disable_phy_coma - Disable the Phy Coma Mode
  * @adapter: pointer to our adapter structure
  */
-void et1310_disable_phy_coma(struct et131x_adapter *adapter)
+static void et1310_disable_phy_coma(struct et131x_adapter *adapter)
 {
 	u32 pmcsr;
 
@@ -2269,8 +2247,6 @@ void et1310_disable_phy_coma(struct et131x_adapter *adapter)
 	et131x_enable_txrx(adapter->netdev);
 }
 
-/* RX functions */
-
 static inline u32 bump_free_buff_ring(u32 *free_buff_ring, u32 limit)
 {
 	u32 tmp_free_buff_ring = *free_buff_ring;
@@ -2296,16 +2272,14 @@ static inline u32 bump_free_buff_ring(u32 *free_buff_ring, u32 limit)
  * @offset: pointer to the offset variable
  * @mask: correct mask
  */
-void et131x_align_allocated_memory(struct et131x_adapter *adapter,
-				   uint64_t *phys_addr,
-				   uint64_t *offset, uint64_t mask)
+static void et131x_align_allocated_memory(struct et131x_adapter *adapter,
+					  u64 *phys_addr, u64 *offset,
+					  u64 mask)
 {
-	uint64_t new_addr;
+	u64 new_addr = *phys_addr & ~mask;
 
 	*offset = 0;
 
-	new_addr = *phys_addr & ~mask;
-
 	if (new_addr != *phys_addr) {
 		/* Move to next aligned block */
 		new_addr += mask + 1;
@@ -2325,7 +2299,7 @@ void et131x_align_allocated_memory(struct et131x_adapter *adapter,
  * Allocates Free buffer ring 1 for sure, free buffer ring 0 if required,
  * and the Packet Status Ring.
  */
-int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
+static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 {
 	u32 i, j;
 	u32 bufsize;
@@ -2454,8 +2428,8 @@ int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 			rx_ring->fbr[1]->offset);
 #endif
 	for (i = 0; i < (rx_ring->fbr[0]->num_entries / FBR_CHUNKS); i++) {
-		u64 fbr1_offset;
 		u64 fbr1_tmp_physaddr;
+		u64 fbr1_offset;
 		u32 fbr1_align;
 
 		/* This code allocates an area of memory big enough for N
@@ -2520,8 +2494,8 @@ int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 #ifdef USE_FBR0
 	/* Same for FBR0 (if in use) */
 	for (i = 0; i < (rx_ring->fbr[1]->num_entries / FBR_CHUNKS); i++) {
-		u64 fbr0_offset;
 		u64 fbr0_tmp_physaddr;
+		u64 fbr0_offset;
 
 		fbr_chunksize =
 		    ((FBR_CHUNKS + 1) * rx_ring->fbr[1]->buffsize) - 1;
@@ -2629,7 +2603,7 @@ int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
  * et131x_rx_dma_memory_free - Free all memory allocated within this module.
  * @adapter: pointer to our private adapter structure
  */
-void et131x_rx_dma_memory_free(struct et131x_adapter *adapter)
+static void et131x_rx_dma_memory_free(struct et131x_adapter *adapter)
 {
 	u32 index;
 	u32 bufsize;
@@ -2774,7 +2748,7 @@ void et131x_rx_dma_memory_free(struct et131x_adapter *adapter)
  *
  * Returns 0 on success and errno on failure (as defined in errno.h)
  */
-int et131x_init_recv(struct et131x_adapter *adapter)
+static int et131x_init_recv(struct et131x_adapter *adapter)
 {
 	int status = -ENOMEM;
 	struct rfd *rfd = NULL;
@@ -2824,7 +2798,7 @@ int et131x_init_recv(struct et131x_adapter *adapter)
  * et131x_set_rx_dma_timer - Set the heartbeat timer according to line rate.
  * @adapter: pointer to our adapter structure
  */
-void et131x_set_rx_dma_timer(struct et131x_adapter *adapter)
+static void et131x_set_rx_dma_timer(struct et131x_adapter *adapter)
 {
 	struct phy_device *phydev = adapter->phydev;
 
@@ -2918,6 +2892,17 @@ static void nic_return_rfd(struct et131x_adapter *adapter, struct rfd *rfd)
 	WARN_ON(rx_local->num_ready_recv > rx_local->num_rfd);
 }
 
+/**
+ * nic_rx_pkts - Checks the hardware for available packets
+ * @adapter: pointer to our adapter
+ *
+ * Returns rfd, a pointer to our MPRFD.
+ *
+ * Checks the hardware for available packets, using completion ring
+ * If packets are available, it gets an RFD from the recv_list, attaches
+ * the packet to it, puts the RFD in the RecvPendList, and also returns
+ * the pointer to the RFD.
+ */
 static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
 {
 	struct rx_ring *rx_local = &adapter->rx_ring;
@@ -3139,7 +3124,7 @@ static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
  *
  * Assumption, Rcv spinlock has been acquired.
  */
-void et131x_handle_recv_interrupt(struct et131x_adapter *adapter)
+static void et131x_handle_recv_interrupt(struct et131x_adapter *adapter)
 {
 	struct rfd *rfd = NULL;
 	u32 count = 0;
@@ -3188,8 +3173,6 @@ void et131x_handle_recv_interrupt(struct et131x_adapter *adapter)
 		adapter->rx_ring.unfinished_receives = false;
 }
 
-/* TX functions */
-
 /**
  * et131x_tx_dma_memory_alloc
  * @adapter: pointer to our private adapter structure
@@ -3202,7 +3185,7 @@ void et131x_handle_recv_interrupt(struct et131x_adapter *adapter)
  * memory. The device will update the "status" in memory each time it xmits a
  * packet.
  */
-int et131x_tx_dma_memory_alloc(struct et131x_adapter *adapter)
+static int et131x_tx_dma_memory_alloc(struct et131x_adapter *adapter)
 {
 	int desc_size = 0;
 	struct tx_ring *tx_ring = &adapter->tx_ring;
@@ -3256,7 +3239,7 @@ int et131x_tx_dma_memory_alloc(struct et131x_adapter *adapter)
  *
  * Returns 0 on success and errno on failure (as defined in errno.h).
  */
-void et131x_tx_dma_memory_free(struct et131x_adapter *adapter)
+static void et131x_tx_dma_memory_free(struct et131x_adapter *adapter)
 {
 	int desc_size = 0;
 
@@ -3578,7 +3561,7 @@ static int send_packet(struct sk_buff *skb, struct et131x_adapter *adapter)
  *
  * Return 0 in almost all cases; non-zero value in extreme hard failure only
  */
-int et131x_send_packets(struct sk_buff *skb, struct net_device *netdev)
+static int et131x_send_packets(struct sk_buff *skb, struct net_device *netdev)
 {
 	int status = 0;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -3696,7 +3679,7 @@ static inline void free_send_packet(struct et131x_adapter *adapter,
  *
  * Assumption - Send spinlock has been acquired
  */
-void et131x_free_busy_send_packets(struct et131x_adapter *adapter)
+static void et131x_free_busy_send_packets(struct et131x_adapter *adapter)
 {
 	struct tcb *tcb;
 	unsigned long flags;
@@ -3743,7 +3726,7 @@ void et131x_free_busy_send_packets(struct et131x_adapter *adapter)
  *
  * Assumption - Send spinlock has been acquired
  */
-void et131x_handle_send_interrupt(struct et131x_adapter *adapter)
+static void et131x_handle_send_interrupt(struct et131x_adapter *adapter)
 {
 	unsigned long flags;
 	u32 serviced;
@@ -3798,8 +3781,6 @@ void et131x_handle_send_interrupt(struct et131x_adapter *adapter)
 	spin_unlock_irqrestore(&adapter->tcb_send_qlock, flags);
 }
 
-/* ETHTOOL functions */
-
 static int et131x_get_settings(struct net_device *netdev,
 			       struct ethtool_cmd *cmd)
 {
@@ -3965,18 +3946,16 @@ static struct ethtool_ops et131x_ethtool_ops = {
 	.get_link = ethtool_op_get_link,
 };
 
-void et131x_set_ethtool_ops(struct net_device *netdev)
+static void et131x_set_ethtool_ops(struct net_device *netdev)
 {
 	SET_ETHTOOL_OPS(netdev, &et131x_ethtool_ops);
 }
 
-/* PCI functions */
-
 /**
  * et131x_hwaddr_init - set up the MAC Address on the ET1310
  * @adapter: pointer to our private adapter structure
  */
-void et131x_hwaddr_init(struct et131x_adapter *adapter)
+static void et131x_hwaddr_init(struct et131x_adapter *adapter)
 {
 	/* If have our default mac from init and no mac address from
 	 * EEPROM then we need to generate the last octet and set it on the
@@ -4022,24 +4001,31 @@ void et131x_hwaddr_init(struct et131x_adapter *adapter)
 static int et131x_pci_init(struct et131x_adapter *adapter,
 						struct pci_dev *pdev)
 {
-	int i;
-	u8 max_payload;
-	u8 read_size_reg;
+	int cap = pci_pcie_cap(pdev);
+	u16 max_payload;
+	u16 ctl;
+	int i, rc;
 
-	if (et131x_init_eeprom(adapter) < 0)
-		return -EIO;
+	rc = et131x_init_eeprom(adapter);
+	if (rc < 0)
+		goto out;
 
+	if (!cap) {
+		dev_err(&pdev->dev, "Missing PCIe capabilities\n");
+		goto err_out;
+	}
+		
 	/* Let's set up the PORT LOGIC Register.  First we need to know what
 	 * the max_payload_size is
 	 */
-	if (pci_read_config_byte(pdev, ET1310_PCI_MAX_PYLD, &max_payload)) {
+	if (pci_read_config_word(pdev, cap + PCI_EXP_DEVCAP, &max_payload)) {
 		dev_err(&pdev->dev,
 		    "Could not read PCI config space for Max Payload Size\n");
-		return -EIO;
+		goto err_out;
 	}
 
 	/* Program the Ack/Nak latency and replay timers */
-	max_payload &= 0x07;	/* Only the lower 3 bits are valid */
+	max_payload &= 0x07;
 
 	if (max_payload < 2) {
 		static const u16 acknak[2] = { 0x76, 0xD0 };
@@ -4049,13 +4035,13 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 					       acknak[max_payload])) {
 			dev_err(&pdev->dev,
 			  "Could not write PCI config space for ACK/NAK\n");
-			return -EIO;
+			goto err_out;
 		}
 		if (pci_write_config_word(pdev, ET1310_PCI_REPLAY,
 					       replay[max_payload])) {
 			dev_err(&pdev->dev,
 			  "Could not write PCI config space for Replay Timer\n");
-			return -EIO;
+			goto err_out;
 		}
 	}
 
@@ -4065,23 +4051,22 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 	if (pci_write_config_byte(pdev, ET1310_PCI_L0L1LATENCY, 0x11)) {
 		dev_err(&pdev->dev,
 		  "Could not write PCI config space for Latency Timers\n");
-		return -EIO;
+		goto err_out;
 	}
 
 	/* Change the max read size to 2k */
-	if (pci_read_config_byte(pdev, 0x51, &read_size_reg)) {
+	if (pci_read_config_word(pdev, cap + PCI_EXP_DEVCTL, &ctl)) {
 		dev_err(&pdev->dev,
 			"Could not read PCI config space for Max read size\n");
-		return -EIO;
+		goto err_out;
 	}
 
-	read_size_reg &= 0x8f;
-	read_size_reg |= 0x40;
+	ctl = (ctl & ~PCI_EXP_DEVCTL_READRQ) | ( 0x04 << 12);
 
-	if (pci_write_config_byte(pdev, 0x51, read_size_reg)) {
+	if (pci_write_config_word(pdev, cap + PCI_EXP_DEVCTL, ctl)) {
 		dev_err(&pdev->dev,
 		      "Could not write PCI config space for Max read size\n");
-		return -EIO;
+		goto err_out;
 	}
 
 	/* Get MAC address from config space if an eeprom exists, otherwise
@@ -4096,11 +4081,15 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 		if (pci_read_config_byte(pdev, ET1310_PCI_MAC_ADDRESS + i,
 					adapter->rom_addr + i)) {
 			dev_err(&pdev->dev, "Could not read PCI config space for MAC address\n");
-			return -EIO;
+			goto err_out;
 		}
 	}
 	memcpy(adapter->addr, adapter->rom_addr, ETH_ALEN);
-	return 0;
+out:
+	return rc;
+err_out:
+	rc = -EIO;
+	goto out;
 }
 
 /**
@@ -4110,7 +4099,7 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
  * The routine called when the error timer expires, to track the number of
  * recurring errors.
  */
-void et131x_error_timer_handler(unsigned long data)
+static void et131x_error_timer_handler(unsigned long data)
 {
 	struct et131x_adapter *adapter = (struct et131x_adapter *) data;
 	struct phy_device *phydev = adapter->phydev;
@@ -4153,7 +4142,7 @@ void et131x_error_timer_handler(unsigned long data)
  *
  * Allocate all the memory blocks for send, receive and others.
  */
-int et131x_adapter_memory_alloc(struct et131x_adapter *adapter)
+static int et131x_adapter_memory_alloc(struct et131x_adapter *adapter)
 {
 	int status;
 
@@ -4188,7 +4177,7 @@ int et131x_adapter_memory_alloc(struct et131x_adapter *adapter)
  * et131x_adapter_memory_free - Free all memory allocated for use by Tx & Rx
  * @adapter: pointer to our private adapter structure
  */
-void et131x_adapter_memory_free(struct et131x_adapter *adapter)
+static void et131x_adapter_memory_free(struct et131x_adapter *adapter)
 {
 	/* Free DMA memory */
 	et131x_tx_dma_memory_free(adapter);
@@ -4364,10 +4353,6 @@ static struct et131x_adapter *et131x_adapter_init(struct net_device *netdev,
 	adapter->pdev = pci_dev_get(pdev);
 	adapter->netdev = netdev;
 
-	/* Do the same for the netdev struct */
-	netdev->irq = pdev->irq;
-	netdev->base_addr = pci_resource_start(pdev, 0);
-
 	/* Initialize spinlocks here */
 	spin_lock_init(&adapter->lock);
 	spin_lock_init(&adapter->tcb_send_qlock);
@@ -4400,6 +4385,7 @@ static void __devexit et131x_pci_remove(struct pci_dev *pdev)
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
 	unregister_netdev(netdev);
+	phy_disconnect(adapter->phydev);
 	mdiobus_unregister(adapter->mii_bus);
 	kfree(adapter->mii_bus->irq);
 	mdiobus_free(adapter->mii_bus);
@@ -4417,7 +4403,7 @@ static void __devexit et131x_pci_remove(struct pci_dev *pdev)
  * et131x_up - Bring up a device for use.
  * @netdev: device to be opened
  */
-void et131x_up(struct net_device *netdev)
+static void et131x_up(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
@@ -4429,7 +4415,7 @@ void et131x_up(struct net_device *netdev)
  * et131x_down - Bring down the device
  * @netdev: device to be broght down
  */
-void et131x_down(struct net_device *netdev)
+static void et131x_down(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
@@ -4475,8 +4461,6 @@ static SIMPLE_DEV_PM_OPS(et131x_pm_ops, et131x_suspend, et131x_resume);
 #define ET131X_PM_OPS NULL
 #endif
 
-/* ISR functions */
-
 /**
  * et131x_isr - The Interrupt Service Routine for the driver.
  * @irq: the IRQ on which the interrupt was received.
@@ -4573,7 +4557,7 @@ out:
  * scheduled to run in a deferred context by the ISR. This is where the ISR's
  * work actually gets done.
  */
-void et131x_isr_handler(struct work_struct *work)
+static void et131x_isr_handler(struct work_struct *work)
 {
 	struct et131x_adapter *adapter =
 		container_of(work, struct et131x_adapter, task);
@@ -4774,8 +4758,6 @@ void et131x_isr_handler(struct work_struct *work)
 	et131x_enable_interrupts(adapter);
 }
 
-/* NETDEV functions */
-
 /**
  * et131x_stats - Return the current device statistics.
  * @netdev: device whose stats are being queried
@@ -4829,10 +4811,12 @@ static struct net_device_stats *et131x_stats(struct net_device *netdev)
  *
  * Returns 0 on success, errno on failure (as defined in errno.h)
  */
-int et131x_open(struct net_device *netdev)
+static int et131x_open(struct net_device *netdev)
 {
-	int result = 0;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
+	struct pci_dev *pdev = adapter->pdev;
+	unsigned int irq = pdev->irq;
+	int result;
 
 	/* Start the timer to track NIC errors */
 	init_timer(&adapter->error_timer);
@@ -4841,12 +4825,9 @@ int et131x_open(struct net_device *netdev)
 	adapter->error_timer.data = (unsigned long)adapter;
 	add_timer(&adapter->error_timer);
 
-	/* Register our IRQ */
-	result = request_irq(netdev->irq, et131x_isr, IRQF_SHARED,
-					netdev->name, netdev);
+	result = request_irq(irq, et131x_isr, IRQF_SHARED, netdev->name, netdev);
 	if (result) {
-		dev_err(&adapter->pdev->dev, "could not register IRQ %d\n",
-			netdev->irq);
+		dev_err(&pdev->dev, "could not register IRQ %d\n", irq);
 		return result;
 	}
 
@@ -4863,14 +4844,14 @@ int et131x_open(struct net_device *netdev)
  *
  * Returns 0 on success, errno on failure (as defined in errno.h)
  */
-int et131x_close(struct net_device *netdev)
+static int et131x_close(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
 	et131x_down(netdev);
 
 	adapter->flags &= ~fMP_ADAPTER_INTERRUPT_IN_USE;
-	free_irq(netdev->irq, netdev);
+	free_irq(adapter->pdev->irq, netdev);
 
 	/* Stop the error timer */
 	return del_timer_sync(&adapter->error_timer);
@@ -4905,8 +4886,8 @@ static int et131x_ioctl(struct net_device *netdev, struct ifreq *reqbuf,
  */
 static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 {
+	int filter = adapter->packet_filter;
 	int status = 0;
-	uint32_t filter = adapter->packet_filter;
 	u32 ctrl;
 	u32 pf_ctrl;
 
@@ -4968,7 +4949,7 @@ static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 static void et131x_multicast(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
-	uint32_t packet_filter = 0;
+	int packet_filter;
 	unsigned long flags;
 	struct netdev_hw_addr *ha;
 	int i;
@@ -5249,40 +5230,6 @@ static const struct net_device_ops et131x_netdev_ops = {
 };
 
 /**
- * et131x_device_alloc
- *
- * Returns pointer to the allocated and initialized net_device struct for
- * this device.
- *
- * Create instances of net_device and wl_private for the new adapter and
- * register the device's entry points in the net_device structure.
- */
-struct net_device *et131x_device_alloc(void)
-{
-	struct net_device *netdev;
-
-	/* Alloc net_device and adapter structs */
-	netdev = alloc_etherdev(sizeof(struct et131x_adapter));
-
-	if (!netdev) {
-		printk(KERN_ERR "et131x: Alloc of net_device struct failed\n");
-		return NULL;
-	}
-
-	/*
-	 * Setup the function registration table (and other data) for a
-	 * net_device
-	 */
-	netdev->watchdog_timeo = ET131X_TX_TIMEOUT;
-	netdev->netdev_ops     = &et131x_netdev_ops;
-
-	/* Poll? */
-	/* netdev->poll               = &et131x_poll; */
-	/* netdev->poll_controller    = &et131x_poll_controller; */
-	return netdev;
-}
-
-/**
  * et131x_pci_setup - Perform device initialization
  * @pdev: a pointer to the device's pci_dev structure
  * @ent: this device's entry in the pci_device_id table
@@ -5297,24 +5244,26 @@ struct net_device *et131x_device_alloc(void)
 static int __devinit et131x_pci_setup(struct pci_dev *pdev,
 			       const struct pci_device_id *ent)
 {
-	int result;
 	struct net_device *netdev;
 	struct et131x_adapter *adapter;
+	int rc;
 	int ii;
 
-	result = pci_enable_device(pdev);
-	if (result) {
+	rc = pci_enable_device(pdev);
+	if (rc < 0) {
 		dev_err(&pdev->dev, "pci_enable_device() failed\n");
-		goto err_out;
+		goto out;
 	}
 
 	/* Perform some basic PCI checks */
 	if (!(pci_resource_flags(pdev, 0) & IORESOURCE_MEM)) {
 		dev_err(&pdev->dev, "Can't find PCI device's base address\n");
+		rc = -ENODEV;
 		goto err_disable;
 	}
 
-	if (pci_request_regions(pdev, DRIVER_NAME)) {
+	rc = pci_request_regions(pdev, DRIVER_NAME);
+	if (rc < 0) {
 		dev_err(&pdev->dev, "Can't get PCI resources\n");
 		goto err_disable;
 	}
@@ -5323,46 +5272,50 @@ static int __devinit et131x_pci_setup(struct pci_dev *pdev,
 
 	/* Check the DMA addressing support of this device */
 	if (!dma_set_mask(&pdev->dev, DMA_BIT_MASK(64))) {
-		result = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
-		if (result) {
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
+		if (rc < 0) {
 			dev_err(&pdev->dev,
 			  "Unable to obtain 64 bit DMA for consistent allocations\n");
 			goto err_release_res;
 		}
 	} else if (!dma_set_mask(&pdev->dev, DMA_BIT_MASK(32))) {
-		result = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
-		if (result) {
+		rc = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
+		if (rc < 0) {
 			dev_err(&pdev->dev,
 			  "Unable to obtain 32 bit DMA for consistent allocations\n");
 			goto err_release_res;
 		}
 	} else {
 		dev_err(&pdev->dev, "No usable DMA addressing method\n");
-		result = -EIO;
+		rc = -EIO;
 		goto err_release_res;
 	}
 
 	/* Allocate netdev and private adapter structs */
-	netdev = et131x_device_alloc();
+	netdev = alloc_etherdev(sizeof(struct et131x_adapter));
 	if (!netdev) {
 		dev_err(&pdev->dev, "Couldn't alloc netdev struct\n");
-		result = -ENOMEM;
+		rc = -ENOMEM;
 		goto err_release_res;
 	}
 
+	netdev->watchdog_timeo = ET131X_TX_TIMEOUT;
+	netdev->netdev_ops     = &et131x_netdev_ops;
+
 	SET_NETDEV_DEV(netdev, &pdev->dev);
 	et131x_set_ethtool_ops(netdev);
 
 	adapter = et131x_adapter_init(netdev, pdev);
 
-	/* Initialise the PCI setup for the device */
-	et131x_pci_init(adapter, pdev);
+	rc = et131x_pci_init(adapter, pdev);
+	if (rc < 0)
+		goto err_free_dev;
 
 	/* Map the bus-relative registers to system virtual memory */
 	adapter->regs = pci_ioremap_bar(pdev, 0);
 	if (!adapter->regs) {
 		dev_err(&pdev->dev, "Cannot map device registers\n");
-		result = -ENOMEM;
+		rc = -ENOMEM;
 		goto err_free_dev;
 	}
 
@@ -5376,8 +5329,8 @@ static int __devinit et131x_pci_setup(struct pci_dev *pdev,
 	et131x_disable_interrupts(adapter);
 
 	/* Allocate DMA memory */
-	result = et131x_adapter_memory_alloc(adapter);
-	if (result) {
+	rc = et131x_adapter_memory_alloc(adapter);
+	if (rc < 0) {
 		dev_err(&pdev->dev, "Could not alloc adapater memory (DMA)\n");
 		goto err_iounmap;
 	}
@@ -5395,6 +5348,8 @@ static int __devinit et131x_pci_setup(struct pci_dev *pdev,
 	adapter->boot_coma = 0;
 	et1310_disable_phy_coma(adapter);
 
+	rc = -ENOMEM;
+
 	/* Setup the mii_bus struct */
 	adapter->mii_bus = mdiobus_alloc();
 	if (!adapter->mii_bus) {
@@ -5418,13 +5373,14 @@ static int __devinit et131x_pci_setup(struct pci_dev *pdev,
 	for (ii = 0; ii < PHY_MAX_ADDR; ii++)
 		adapter->mii_bus->irq[ii] = PHY_POLL;
 
-	if (mdiobus_register(adapter->mii_bus)) {
+	rc = mdiobus_register(adapter->mii_bus);
+	if (rc < 0) {
 		dev_err(&pdev->dev, "failed to register MII bus\n");
-		mdiobus_free(adapter->mii_bus);
 		goto err_mdio_free_irq;
 	}
 
-	if (et131x_mii_probe(netdev)) {
+	rc = et131x_mii_probe(netdev);
+	if (rc < 0) {
 		dev_err(&pdev->dev, "failed to probe MII bus\n");
 		goto err_mdio_unregister;
 	}
@@ -5440,10 +5396,10 @@ static int __devinit et131x_pci_setup(struct pci_dev *pdev,
 	 */
 
 	/* Register the net_device struct with the Linux network layer */
-	result = register_netdev(netdev);
-	if (result != 0) {
+	rc = register_netdev(netdev);
+	if (rc < 0) {
 		dev_err(&pdev->dev, "register_netdev() failed\n");
-		goto err_mdio_unregister;
+		goto err_phy_disconnect;
 	}
 
 	/* Register the net_device struct with the PCI subsystem. Save a copy
@@ -5451,10 +5407,11 @@ static int __devinit et131x_pci_setup(struct pci_dev *pdev,
 	 * been initialized, just in case it needs to be quickly restored.
 	 */
 	pci_set_drvdata(pdev, netdev);
-	pci_save_state(adapter->pdev);
-
-	return result;
+out:
+	return rc;
 
+err_phy_disconnect:
+	phy_disconnect(adapter->phydev);
 err_mdio_unregister:
 	mdiobus_unregister(adapter->mii_bus);
 err_mdio_free_irq:
@@ -5472,8 +5429,7 @@ err_release_res:
 	pci_release_regions(pdev);
 err_disable:
 	pci_disable_device(pdev);
-err_out:
-	return result;
+	goto out;
 }
 
 static DEFINE_PCI_DEVICE_TABLE(et131x_pci_table) = {
diff --git a/drivers/staging/frontier/tranzport.c b/drivers/staging/frontier/tranzport.c
index c263284ddc0e..cf47a5d191fc 100644
--- a/drivers/staging/frontier/tranzport.c
+++ b/drivers/staging/frontier/tranzport.c
@@ -199,7 +199,7 @@ static void usb_tranzport_abort_transfers(struct usb_tranzport *dev)
 		struct usb_interface *intf = to_usb_interface(dev);	\
 		struct usb_tranzport *t = usb_get_intfdata(intf);	\
 		unsigned long temp;	\
-		if (strict_strtoul(buf, 10, &temp))	\
+		if (kstrtoul(buf, 10, &temp))	\
 			return -EINVAL;	\
 		t->value = temp;	\
 		return count;	\
diff --git a/drivers/staging/gma500/Kconfig b/drivers/staging/gma500/Kconfig
index bfe2166acda6..c7a2b3bc0a18 100644
--- a/drivers/staging/gma500/Kconfig
+++ b/drivers/staging/gma500/Kconfig
@@ -1,6 +1,6 @@
 config DRM_PSB
 	tristate "Intel GMA5/600 KMS Framebuffer"
-	depends on DRM && PCI && X86
+	depends on DRM && PCI && X86 && BROKEN
 	select FB_CFB_COPYAREA
         select FB_CFB_FILLRECT
         select FB_CFB_IMAGEBLIT
diff --git a/drivers/staging/gma500/accel_2d.c b/drivers/staging/gma500/accel_2d.c
index 114b99a1ce19..b8f78ebbb145 100644
--- a/drivers/staging/gma500/accel_2d.c
+++ b/drivers/staging/gma500/accel_2d.c
@@ -253,7 +253,7 @@ static void psbfb_copyarea_accel(struct fb_info *info,
 		return;
 
 	offset = psbfb->gtt->offset;
-	stride = fb->pitch;
+	stride = fb->pitches[0];
 
 	switch (fb->depth) {
 	case 8:
diff --git a/drivers/staging/gma500/cdv_intel_display.c b/drivers/staging/gma500/cdv_intel_display.c
index 7b97c600eff0..c63a32776a9e 100644
--- a/drivers/staging/gma500/cdv_intel_display.c
+++ b/drivers/staging/gma500/cdv_intel_display.c
@@ -507,9 +507,9 @@ int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
 	if (ret < 0)
 		goto psb_intel_pipe_set_base_exit;
 	start = psbfb->gtt->offset;
-	offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
+	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitch);
+	REG_WRITE(dspstride, crtc->fb->pitches[0]);
 
 	dspcntr = REG_READ(dspcntr_reg);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
diff --git a/drivers/staging/gma500/framebuffer.c b/drivers/staging/gma500/framebuffer.c
index 3f39a37456fc..b00761cba144 100644
--- a/drivers/staging/gma500/framebuffer.c
+++ b/drivers/staging/gma500/framebuffer.c
@@ -32,6 +32,7 @@
 #include <drm/drmP.h>
 #include <drm/drm.h>
 #include <drm/drm_crtc.h>
+#include <drm/drm_fb_helper.h>
 
 #include "psb_drv.h"
 #include "psb_intel_reg.h"
@@ -273,14 +274,17 @@ static struct fb_ops psbfb_unaccel_ops = {
  */
 static int psb_framebuffer_init(struct drm_device *dev,
 					struct psb_framebuffer *fb,
-					struct drm_mode_fb_cmd *mode_cmd,
+					struct drm_mode_fb_cmd2 *mode_cmd,
 					struct gtt_range *gt)
 {
+	u32 bpp, depth;
 	int ret;
 
-	if (mode_cmd->pitch & 63)
+	drm_fb_get_bpp_depth(mode_cmd->pixel_format, &depth, &bpp);
+
+	if (mode_cmd->pitches[0] & 63)
 		return -EINVAL;
-	switch (mode_cmd->bpp) {
+	switch (bpp) {
 	case 8:
 	case 16:
 	case 24:
@@ -313,7 +317,7 @@ static int psb_framebuffer_init(struct drm_device *dev,
 
 static struct drm_framebuffer *psb_framebuffer_create
 			(struct drm_device *dev,
-			 struct drm_mode_fb_cmd *mode_cmd,
+			 struct drm_mode_fb_cmd2 *mode_cmd,
 			 struct gtt_range *gt)
 {
 	struct psb_framebuffer *fb;
@@ -387,27 +391,28 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 	struct fb_info *info;
 	struct drm_framebuffer *fb;
 	struct psb_framebuffer *psbfb = &fbdev->pfb;
-	struct drm_mode_fb_cmd mode_cmd;
+	struct drm_mode_fb_cmd2 mode_cmd;
 	struct device *device = &dev->pdev->dev;
 	int size;
 	int ret;
 	struct gtt_range *backing;
 	int gtt_roll = 1;
+	u32 bpp, depth;
 
 	mode_cmd.width = sizes->surface_width;
 	mode_cmd.height = sizes->surface_height;
-	mode_cmd.bpp = sizes->surface_bpp;
+	bpp = sizes->surface_bpp;
 
 	/* No 24bit packed */
-	if (mode_cmd.bpp == 24)
-		mode_cmd.bpp = 32;
+	if (bpp == 24)
+		bpp = 32;
 
 	/* Acceleration via the GTT requires pitch to be 4096 byte aligned 
 	   (ie 1024 or 2048 pixels in normal use) */
-	mode_cmd.pitch =  ALIGN(mode_cmd.width * ((mode_cmd.bpp + 7) / 8), 4096);
-	mode_cmd.depth = sizes->surface_depth;
+	mode_cmd.pitches[0] =  ALIGN(mode_cmd.width * ((bpp + 7) / 8), 4096);
+	depth = sizes->surface_depth;
 
-	size = mode_cmd.pitch * mode_cmd.height;
+	size = mode_cmd.pitches[0] * mode_cmd.height;
 	size = ALIGN(size, PAGE_SIZE);
 
 	/* Allocate the framebuffer in the GTT with stolen page backing */
@@ -421,10 +426,10 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 
 		gtt_roll = 0;	/* Don't use GTT accelerated scrolling */
 
-		mode_cmd.pitch =  ALIGN(mode_cmd.width * ((mode_cmd.bpp + 7) / 8), 64);
-		mode_cmd.depth = sizes->surface_depth;
+		mode_cmd.pitches[0] =  ALIGN(mode_cmd.width * ((bpp + 7) / 8), 64);
+		depth = sizes->surface_depth;
 
-		size = mode_cmd.pitch * mode_cmd.height;
+		size = mode_cmd.pitches[0] * mode_cmd.height;
 		size = ALIGN(size, PAGE_SIZE);
 
 		/* Allocate the framebuffer in the GTT with stolen page
@@ -443,6 +448,8 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 	}
 	info->par = fbdev;
 
+	mode_cmd.pixel_format = drm_mode_legacy_fb_format(bpp, depth);
+
 	ret = psb_framebuffer_init(dev, psbfb, &mode_cmd, backing);
 	if (ret)
 		goto out_unref;
@@ -504,7 +511,7 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 		info->apertures->ranges[0].size = dev_priv->gtt.stolen_size;
 	}
 
-	drm_fb_helper_fill_fix(info, fb->pitch, fb->depth);
+	drm_fb_helper_fill_fix(info, fb->pitches[0], fb->depth);
 	drm_fb_helper_fill_var(info, &fbdev->psb_fb_helper,
 				sizes->fb_width, sizes->fb_height);
 
@@ -546,7 +553,7 @@ out_err1:
  */
 static struct drm_framebuffer *psb_user_framebuffer_create
 			(struct drm_device *dev, struct drm_file *filp,
-			 struct drm_mode_fb_cmd *cmd)
+			 struct drm_mode_fb_cmd2 *cmd)
 {
 	struct gtt_range *r;
 	struct drm_gem_object *obj;
@@ -555,7 +562,7 @@ static struct drm_framebuffer *psb_user_framebuffer_create
 	 *	Find the GEM object and thus the gtt range object that is
 	 *	to back this space
 	 */
-	obj = drm_gem_object_lookup(dev, filp, cmd->handle);
+	obj = drm_gem_object_lookup(dev, filp, cmd->handles[0]);
 	if (obj == NULL)
 		return ERR_PTR(-ENOENT);
 
diff --git a/drivers/staging/gma500/mdfld_intel_display.c b/drivers/staging/gma500/mdfld_intel_display.c
index 8eb827ecc3d3..0b37b7b6b02a 100644
--- a/drivers/staging/gma500/mdfld_intel_display.c
+++ b/drivers/staging/gma500/mdfld_intel_display.c
@@ -390,9 +390,9 @@ int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y, struct drm_f
 	        goto psb_intel_pipe_set_base_exit;
 
 	start = psbfb->gtt->offset;
-	offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
+	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitch);
+	REG_WRITE(dspstride, crtc->fb->pitches[0]);
 	dspcntr = REG_READ(dspcntr_reg);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 
diff --git a/drivers/staging/gma500/mrst_crtc.c b/drivers/staging/gma500/mrst_crtc.c
index c9311a573c28..980837e37d80 100644
--- a/drivers/staging/gma500/mrst_crtc.c
+++ b/drivers/staging/gma500/mrst_crtc.c
@@ -543,9 +543,9 @@ int mrst_pipe_set_base(struct drm_crtc *crtc,
 		return 0;
 
 	start = psbfb->gtt->offset;
-	offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
+	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitch);
+	REG_WRITE(dspstride, crtc->fb->pitches[0]);
 
 	dspcntr = REG_READ(dspcntr_reg);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
diff --git a/drivers/staging/gma500/power.c b/drivers/staging/gma500/power.c
index 436fe9733b16..408257038335 100644
--- a/drivers/staging/gma500/power.c
+++ b/drivers/staging/gma500/power.c
@@ -266,7 +266,7 @@ bool gma_power_begin(struct drm_device *dev, bool force_on)
 	ret = gma_resume_pci(dev->pdev);
 	if (ret == 0) {
 		/* FIXME: we want to defer this for Medfield/Oaktrail */
-		gma_resume_display(dev);
+		gma_resume_display(dev->pdev);
 		psb_irq_preinstall(dev);
 		psb_irq_postinstall(dev);
 		pm_runtime_get(&dev->pdev->dev);
diff --git a/drivers/staging/gma500/psb_drv.c b/drivers/staging/gma500/psb_drv.c
index 986a04d16ba8..95816808f867 100644
--- a/drivers/staging/gma500/psb_drv.c
+++ b/drivers/staging/gma500/psb_drv.c
@@ -1151,6 +1151,17 @@ static struct vm_operations_struct psb_gem_vm_ops = {
 	.close = drm_gem_vm_close,
 };
 
+static const struct file_operations gma500_driver_fops = {
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = psb_unlocked_ioctl,
+	.mmap = drm_gem_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+	.read = drm_read,
+};
+
 static struct drm_driver driver = {
 	.driver_features = DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | \
 			   DRIVER_IRQ_VBL | DRIVER_MODESET | DRIVER_GEM ,
@@ -1179,17 +1190,7 @@ static struct drm_driver driver = {
 	.dumb_create = psb_gem_dumb_create,
 	.dumb_map_offset = psb_gem_dumb_map_gtt,
 	.dumb_destroy = psb_gem_dumb_destroy,
-
-	.fops = {
-		 .owner = THIS_MODULE,
-		 .open = drm_open,
-		 .release = drm_release,
-		 .unlocked_ioctl = psb_unlocked_ioctl,
-		 .mmap = drm_gem_mmap,
-		 .poll = drm_poll,
-		 .fasync = drm_fasync,
-		 .read = drm_read,
-	 },
+	.fops = &gma500_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
 	.date = PSB_DRM_DRIVER_DATE,
diff --git a/drivers/staging/gma500/psb_intel_display.c b/drivers/staging/gma500/psb_intel_display.c
index caa9d86f26d8..85659613ae62 100644
--- a/drivers/staging/gma500/psb_intel_display.c
+++ b/drivers/staging/gma500/psb_intel_display.c
@@ -367,9 +367,9 @@ int psb_intel_pipe_set_base(struct drm_crtc *crtc,
 		goto psb_intel_pipe_set_base_exit;
 	start = psbfb->gtt->offset;
 
-	offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
+	offset = y * crtc->fb->pitches[0] + x * (crtc->fb->bits_per_pixel / 8);
 
-	REG_WRITE(dspstride, crtc->fb->pitch);
+	REG_WRITE(dspstride, crtc->fb->pitches[0]);
 
 	dspcntr = REG_READ(dspcntr_reg);
 	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
diff --git a/drivers/staging/hv/Kconfig b/drivers/staging/hv/Kconfig
index 072185ebe95b..60ac479a2909 100644
--- a/drivers/staging/hv/Kconfig
+++ b/drivers/staging/hv/Kconfig
@@ -3,15 +3,3 @@ config HYPERV_STORAGE
 	depends on HYPERV && SCSI
 	help
 	 Select this option to enable the Hyper-V virtual storage driver.
-
-config HYPERV_NET
-	tristate "Microsoft Hyper-V virtual network driver"
-	depends on HYPERV && NET
-	help
-	  Select this option to enable the Hyper-V virtual network driver.
-
-config HYPERV_MOUSE
-	tristate "Microsoft Hyper-V mouse driver"
-	depends on HYPERV && HID
-	help
-	  Select this option to enable the Hyper-V mouse driver.
diff --git a/drivers/staging/hv/Makefile b/drivers/staging/hv/Makefile
index 0f55ceee919b..af95a6b7e436 100644
--- a/drivers/staging/hv/Makefile
+++ b/drivers/staging/hv/Makefile
@@ -1,6 +1,3 @@
 obj-$(CONFIG_HYPERV_STORAGE)	+= hv_storvsc.o
-obj-$(CONFIG_HYPERV_NET)	+= hv_netvsc.o
-obj-$(CONFIG_HYPERV_MOUSE)	+= hv_mouse.o
 
 hv_storvsc-y := storvsc_drv.o
-hv_netvsc-y := netvsc_drv.o netvsc.o rndis_filter.o
diff --git a/drivers/staging/hv/TODO b/drivers/staging/hv/TODO
index ed4d63619df2..dea7d92dfdc1 100644
--- a/drivers/staging/hv/TODO
+++ b/drivers/staging/hv/TODO
@@ -1,7 +1,5 @@
 TODO:
-	- audit the network driver
 	- audit the scsi driver
 
 Please send patches for this code to Greg Kroah-Hartman <gregkh@suse.de>,
-Hank Janssen <hjanssen@microsoft.com>, Haiyang Zhang <haiyangz@microsoft.com>,
-K. Y. Srinivasan <kys@microsoft.com>
+Haiyang Zhang <haiyangz@microsoft.com>, and K. Y. Srinivasan <kys@microsoft.com>
diff --git a/drivers/staging/hv/storvsc_drv.c b/drivers/staging/hv/storvsc_drv.c
index ae8c33e7849c..eb853f71089a 100644
--- a/drivers/staging/hv/storvsc_drv.c
+++ b/drivers/staging/hv/storvsc_drv.c
@@ -32,6 +32,7 @@
 #include <linux/module.h>
 #include <linux/device.h>
 #include <linux/hyperv.h>
+#include <linux/mempool.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_host.h>
@@ -42,6 +43,7 @@
 #include <scsi/scsi_dbg.h>
 
 
+#define STORVSC_MIN_BUF_NR				64
 #define STORVSC_RING_BUFFER_SIZE			(20*PAGE_SIZE)
 static int storvsc_ringbuffer_size = STORVSC_RING_BUFFER_SIZE;
 
@@ -78,7 +80,7 @@ MODULE_PARM_DESC(storvsc_ringbuffer_size, "Ring buffer size (bytes)");
 /* V1 Beta                    0.1 */
 /* V1 RC < 2008/1/31          1.0 */
 /* V1 RC > 2008/1/31          2.0 */
-#define VMSTOR_PROTOCOL_VERSION_CURRENT VMSTOR_PROTOCOL_VERSION(2, 0)
+#define VMSTOR_PROTOCOL_VERSION_CURRENT VMSTOR_PROTOCOL_VERSION(4, 2)
 
 
 
@@ -104,7 +106,8 @@ enum vstor_packet_operation {
 	VSTOR_OPERATION_END_INITIALIZATION	= 8,
 	VSTOR_OPERATION_QUERY_PROTOCOL_VERSION	= 9,
 	VSTOR_OPERATION_QUERY_PROPERTIES	= 10,
-	VSTOR_OPERATION_MAXIMUM			= 10
+	VSTOR_OPERATION_ENUMERATE_BUS		= 11,
+	VSTOR_OPERATION_MAXIMUM			= 11
 };
 
 /*
@@ -234,8 +237,6 @@ struct vstor_packet {
 #define STORVSC_MAX_CHANNELS				1
 #define STORVSC_MAX_CMD_LEN				16
 
-struct hv_storvsc_request;
-
 /* Matches Windows-end */
 enum storvsc_request_type {
 	WRITE_TYPE,
@@ -284,9 +285,13 @@ struct storvsc_device {
 	struct hv_storvsc_request reset_request;
 };
 
+struct stor_mem_pools {
+	struct kmem_cache *request_pool;
+	mempool_t *request_mempool;
+};
+
 struct hv_host_device {
 	struct hv_device *dev;
-	struct kmem_cache *request_pool;
 	unsigned int port;
 	unsigned char path;
 	unsigned char target;
@@ -302,6 +307,51 @@ struct storvsc_cmd_request {
 	struct hv_storvsc_request request;
 };
 
+struct storvsc_scan_work {
+	struct work_struct work;
+	struct Scsi_Host *host;
+	uint lun;
+};
+
+static void storvsc_bus_scan(struct work_struct *work)
+{
+	struct storvsc_scan_work *wrk;
+	int id, order_id;
+
+	wrk = container_of(work, struct storvsc_scan_work, work);
+	for (id = 0; id < wrk->host->max_id; ++id) {
+		if (wrk->host->reverse_ordering)
+			order_id = wrk->host->max_id - id - 1;
+		else
+			order_id = id;
+
+		scsi_scan_target(&wrk->host->shost_gendev, 0,
+				order_id, SCAN_WILD_CARD, 1);
+	}
+	kfree(wrk);
+}
+
+static void storvsc_remove_lun(struct work_struct *work)
+{
+	struct storvsc_scan_work *wrk;
+	struct scsi_device *sdev;
+
+	wrk = container_of(work, struct storvsc_scan_work, work);
+	if (!scsi_host_get(wrk->host))
+		goto done;
+
+	sdev = scsi_device_lookup(wrk->host, 0, 0, wrk->lun);
+
+	if (sdev) {
+		scsi_remove_device(sdev);
+		scsi_device_put(sdev);
+	}
+	scsi_host_put(wrk->host);
+
+done:
+	kfree(wrk);
+}
+
 static inline struct storvsc_device *get_out_stor_device(
 					struct hv_device *device)
 {
@@ -549,11 +599,25 @@ static void storvsc_on_receive(struct hv_device *device,
 			     struct vstor_packet *vstor_packet,
 			     struct hv_storvsc_request *request)
 {
+	struct storvsc_scan_work *work;
+	struct storvsc_device *stor_device;
+
 	switch (vstor_packet->operation) {
 	case VSTOR_OPERATION_COMPLETE_IO:
 		storvsc_on_io_completion(device, vstor_packet, request);
 		break;
+
 	case VSTOR_OPERATION_REMOVE_DEVICE:
+	case VSTOR_OPERATION_ENUMERATE_BUS:
+		stor_device = get_in_stor_device(device);
+		work = kmalloc(sizeof(struct storvsc_scan_work), GFP_ATOMIC);
+		if (!work)
+			return;
+
+		INIT_WORK(&work->work, storvsc_bus_scan);
+		work->host = stor_device->host;
+		schedule_work(&work->work);
+		break;
 
 	default:
 		break;
@@ -729,19 +793,48 @@ static void storvsc_get_ide_info(struct hv_device *dev, int *target, int *path)
 
 static int storvsc_device_alloc(struct scsi_device *sdevice)
 {
-	/*
-	 * This enables luns to be located sparsely. Otherwise, we may not
-	 * discovered them.
-	 */
-	sdevice->sdev_bflags |= BLIST_SPARSELUN | BLIST_LARGELUN;
+	struct stor_mem_pools *memp;
+	int number = STORVSC_MIN_BUF_NR;
+
+	memp = kzalloc(sizeof(struct stor_mem_pools), GFP_KERNEL);
+	if (!memp)
+		return -ENOMEM;
+
+	memp->request_pool =
+		kmem_cache_create(dev_name(&sdevice->sdev_dev),
+				sizeof(struct storvsc_cmd_request), 0,
+				SLAB_HWCACHE_ALIGN, NULL);
+
+	if (!memp->request_pool)
+		goto err0;
+
+	memp->request_mempool = mempool_create(number, mempool_alloc_slab,
+						mempool_free_slab,
+						memp->request_pool);
+
+	if (!memp->request_mempool)
+		goto err1;
+
+	sdevice->hostdata = memp;
+
 	return 0;
+
+err1:
+	kmem_cache_destroy(memp->request_pool);
+
+err0:
+	kfree(memp);
+	return -ENOMEM;
 }
 
-static int storvsc_merge_bvec(struct request_queue *q,
-			      struct bvec_merge_data *bmd, struct bio_vec *bvec)
+static void storvsc_device_destroy(struct scsi_device *sdevice)
 {
-	/* checking done by caller. */
-	return bvec->bv_len;
+	struct stor_mem_pools *memp = sdevice->hostdata;
+
+	mempool_destroy(memp->request_mempool);
+	kmem_cache_destroy(memp->request_pool);
+	kfree(memp);
+	sdevice->hostdata = NULL;
 }
 
 static int storvsc_device_configure(struct scsi_device *sdevice)
@@ -751,8 +844,6 @@ static int storvsc_device_configure(struct scsi_device *sdevice)
 
 	blk_queue_max_segment_size(sdevice->request_queue, PAGE_SIZE);
 
-	blk_queue_merge_bvec(sdevice->request_queue, storvsc_merge_bvec);
-
 	blk_queue_bounce_limit(sdevice->request_queue, BLK_BOUNCE_ANY);
 
 	return 0;
@@ -802,12 +893,14 @@ static int do_bounce_buffer(struct scatterlist *sgl, unsigned int sg_count)
 
 static struct scatterlist *create_bounce_buffer(struct scatterlist *sgl,
 						unsigned int sg_count,
-						unsigned int len)
+						unsigned int len,
+						int write)
 {
 	int i;
 	int num_pages;
 	struct scatterlist *bounce_sgl;
 	struct page *page_buf;
+	unsigned int buf_len = ((write == WRITE_TYPE) ? 0 : PAGE_SIZE);
 
 	num_pages = ALIGN(len, PAGE_SIZE) >> PAGE_SHIFT;
 
@@ -819,7 +912,7 @@ static struct scatterlist *create_bounce_buffer(struct scatterlist *sgl,
 		page_buf = alloc_page(GFP_ATOMIC);
 		if (!page_buf)
 			goto cleanup;
-		sg_set_page(&bounce_sgl[i], page_buf, 0, 0);
+		sg_set_page(&bounce_sgl[i], page_buf, buf_len, 0);
 	}
 
 	return bounce_sgl;
@@ -833,7 +926,8 @@ cleanup:
 /* Assume the original sgl has enough room */
 static unsigned int copy_from_bounce_buffer(struct scatterlist *orig_sgl,
 					    struct scatterlist *bounce_sgl,
-					    unsigned int orig_sgl_count)
+					    unsigned int orig_sgl_count,
+					    unsigned int bounce_sgl_count)
 {
 	int i;
 	int j = 0;
@@ -874,6 +968,24 @@ static unsigned int copy_from_bounce_buffer(struct scatterlist *orig_sgl,
 				kunmap_atomic((void *)bounce_addr, KM_IRQ0);
 				j++;
 
+				/*
+				 * It is possible that the number of elements
+				 * in the bounce buffer may not be equal to
+				 * the number of elements in the original
+				 * scatter list. Handle this correctly.
+				 */
+
+				if (j == bounce_sgl_count) {
+					/*
+					 * We are done; cleanup and return.
+					 */
+					kunmap_atomic((void *)(dest_addr -
+							orig_sgl[i].offset),
+							KM_IRQ0);
+					local_irq_restore(flags);
+					return total_copied;
+				}
+
 				/* if we need to use another bounce buffer */
 				if (destlen || i != orig_sgl_count - 1)
 					bounce_addr =
@@ -965,18 +1077,13 @@ static int storvsc_remove(struct hv_device *dev)
 {
 	struct storvsc_device *stor_device = hv_get_drvdata(dev);
 	struct Scsi_Host *host = stor_device->host;
-	struct hv_host_device *host_dev =
-			(struct hv_host_device *)host->hostdata;
 
 	scsi_remove_host(host);
 
 	scsi_host_put(host);
 
 	storvsc_dev_remove(dev);
-	if (host_dev->request_pool) {
-		kmem_cache_destroy(host_dev->request_pool);
-		host_dev->request_pool = NULL;
-	}
+
 	return 0;
 }
 
@@ -1014,7 +1121,7 @@ static int storvsc_host_reset(struct hv_device *device)
 
 	stor_device = get_out_stor_device(device);
 	if (!stor_device)
-		return -ENODEV;
+		return FAILED;
 
 	request = &stor_device->reset_request;
 	vstor_packet = &request->vstor_packet;
@@ -1031,13 +1138,11 @@ static int storvsc_host_reset(struct hv_device *device)
 			       VM_PKT_DATA_INBAND,
 			       VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
 	if (ret != 0)
-		goto cleanup;
+		return FAILED;
 
 	t = wait_for_completion_timeout(&request->wait_event, 5*HZ);
-	if (t == 0) {
-		ret = -ETIMEDOUT;
-		goto cleanup;
-	}
+	if (t == 0)
+		return TIMEOUT_ERROR;
 
 
 	/*
@@ -1045,8 +1150,7 @@ static int storvsc_host_reset(struct hv_device *device)
 	 * should have been flushed out and return to us
 	 */
 
-cleanup:
-	return ret;
+	return SUCCESS;
 }
 
 
@@ -1055,16 +1159,10 @@ cleanup:
  */
 static int storvsc_host_reset_handler(struct scsi_cmnd *scmnd)
 {
-	int ret;
-	struct hv_host_device *host_dev =
-		(struct hv_host_device *)scmnd->device->host->hostdata;
+	struct hv_host_device *host_dev = shost_priv(scmnd->device->host);
 	struct hv_device *dev = host_dev->dev;
 
-	ret = storvsc_host_reset(dev);
-	if (ret != 0)
-		return ret;
-
-	return ret;
+	return storvsc_host_reset(dev);
 }
 
 
@@ -1076,21 +1174,22 @@ static void storvsc_command_completion(struct hv_storvsc_request *request)
 	struct storvsc_cmd_request *cmd_request =
 		(struct storvsc_cmd_request *)request->context;
 	struct scsi_cmnd *scmnd = cmd_request->cmd;
-	struct hv_host_device *host_dev =
-		(struct hv_host_device *)scmnd->device->host->hostdata;
+	struct hv_host_device *host_dev = shost_priv(scmnd->device->host);
 	void (*scsi_done_fn)(struct scsi_cmnd *);
 	struct scsi_sense_hdr sense_hdr;
 	struct vmscsi_request *vm_srb;
+	struct storvsc_scan_work *wrk;
+	struct stor_mem_pools *memp = scmnd->device->hostdata;
 
 	vm_srb = &request->vstor_packet.vm_srb;
 	if (cmd_request->bounce_sgl_count) {
-		if (vm_srb->data_in == READ_TYPE) {
+		if (vm_srb->data_in == READ_TYPE)
 			copy_from_bounce_buffer(scsi_sglist(scmnd),
 					cmd_request->bounce_sgl,
-					scsi_sg_count(scmnd));
-			destroy_bounce_buffer(cmd_request->bounce_sgl,
+					scsi_sg_count(scmnd),
+					cmd_request->bounce_sgl_count);
+		destroy_bounce_buffer(cmd_request->bounce_sgl,
 					cmd_request->bounce_sgl_count);
-		}
 	}
 
 	/*
@@ -1103,6 +1202,29 @@ static void storvsc_command_completion(struct hv_storvsc_request *request)
 	else
 		scmnd->result = vm_srb->scsi_status;
 
+	/*
+	 * If the LUN is invalid; remove the device.
+	 */
+	if (vm_srb->srb_status == 0x20) {
+		struct storvsc_device *stor_dev;
+		struct hv_device *dev = host_dev->dev;
+		struct Scsi_Host *host;
+
+		stor_dev = get_in_stor_device(dev);
+		host = stor_dev->host;
+
+		wrk = kmalloc(sizeof(struct storvsc_scan_work),
+				GFP_ATOMIC);
+		if (!wrk) {
+			scmnd->result = DID_TARGET_FAILURE << 16;
+		} else {
+			wrk->host = host;
+			wrk->lun = vm_srb->lun;
+			INIT_WORK(&wrk->work, storvsc_remove_lun);
+			schedule_work(&wrk->work);
+		}
+	}
+
 	if (scmnd->result) {
 		if (scsi_normalize_sense(scmnd->sense_buffer,
 				SCSI_SENSE_BUFFERSIZE, &sense_hdr))
@@ -1120,7 +1242,7 @@ static void storvsc_command_completion(struct hv_storvsc_request *request)
 
 	scsi_done_fn(scmnd);
 
-	kmem_cache_free(host_dev->request_pool, cmd_request);
+	mempool_free(cmd_request, memp->request_mempool);
 }
 
 static bool storvsc_check_scsi_cmd(struct scsi_cmnd *scmnd)
@@ -1131,7 +1253,7 @@ static bool storvsc_check_scsi_cmd(struct scsi_cmnd *scmnd)
 	switch (scsi_op) {
 	/* smartd sends this command, which will offline the device */
 	case SET_WINDOW:
-		scmnd->result = DID_ERROR << 16;
+		scmnd->result = ILLEGAL_REQUEST << 16;
 		allowed = false;
 		break;
 	default:
@@ -1143,12 +1265,10 @@ static bool storvsc_check_scsi_cmd(struct scsi_cmnd *scmnd)
 /*
  * storvsc_queuecommand - Initiate command processing
  */
-static int storvsc_queuecommand_lck(struct scsi_cmnd *scmnd,
-				void (*done)(struct scsi_cmnd *))
+static int storvsc_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *scmnd)
 {
 	int ret;
-	struct hv_host_device *host_dev =
-		(struct hv_host_device *)scmnd->device->host->hostdata;
+	struct hv_host_device *host_dev = shost_priv(host);
 	struct hv_device *dev = host_dev->dev;
 	struct hv_storvsc_request *request;
 	struct storvsc_cmd_request *cmd_request;
@@ -1157,9 +1277,10 @@ static int storvsc_queuecommand_lck(struct scsi_cmnd *scmnd,
 	struct scatterlist *sgl;
 	unsigned int sg_count = 0;
 	struct vmscsi_request *vm_srb;
+	struct stor_mem_pools *memp = scmnd->device->hostdata;
 
 	if (storvsc_check_scsi_cmd(scmnd) == false) {
-		done(scmnd);
+		scmnd->scsi_done(scmnd);
 		return 0;
 	}
 
@@ -1172,16 +1293,14 @@ static int storvsc_queuecommand_lck(struct scsi_cmnd *scmnd,
 		goto retry_request;
 	}
 
-	scmnd->scsi_done = done;
-
 	request_size = sizeof(struct storvsc_cmd_request);
 
-	cmd_request = kmem_cache_zalloc(host_dev->request_pool,
+	cmd_request = mempool_alloc(memp->request_mempool,
 				       GFP_ATOMIC);
-	if (!cmd_request) {
-		scmnd->scsi_done = NULL;
+	if (!cmd_request)
 		return SCSI_MLQUEUE_DEVICE_BUSY;
-	}
+
+	memset(cmd_request, 0, sizeof(struct storvsc_cmd_request));
 
 	/* Setup the cmd request */
 	cmd_request->bounce_sgl_count = 0;
@@ -1231,12 +1350,12 @@ static int storvsc_queuecommand_lck(struct scsi_cmnd *scmnd,
 		if (do_bounce_buffer(sgl, scsi_sg_count(scmnd)) != -1) {
 			cmd_request->bounce_sgl =
 				create_bounce_buffer(sgl, scsi_sg_count(scmnd),
-						     scsi_bufflen(scmnd));
+						     scsi_bufflen(scmnd),
+						     vm_srb->data_in);
 			if (!cmd_request->bounce_sgl) {
-				scmnd->scsi_done = NULL;
 				scmnd->host_scribble = NULL;
-				kmem_cache_free(host_dev->request_pool,
-						cmd_request);
+				mempool_free(cmd_request,
+						memp->request_mempool);
 
 				return SCSI_MLQUEUE_HOST_BUSY;
 			}
@@ -1278,9 +1397,8 @@ retry_request:
 			destroy_bounce_buffer(cmd_request->bounce_sgl,
 					cmd_request->bounce_sgl_count);
 
-		kmem_cache_free(host_dev->request_pool, cmd_request);
+		mempool_free(cmd_request, memp->request_mempool);
 
-		scmnd->scsi_done = NULL;
 		scmnd->host_scribble = NULL;
 
 		ret = SCSI_MLQUEUE_DEVICE_BUSY;
@@ -1289,9 +1407,6 @@ retry_request:
 	return ret;
 }
 
-static DEF_SCSI_QCMD(storvsc_queuecommand)
-
-
 /* Scsi driver */
 static struct scsi_host_template scsi_driver = {
 	.module	=		THIS_MODULE,
@@ -1300,6 +1415,7 @@ static struct scsi_host_template scsi_driver = {
 	.queuecommand =		storvsc_queuecommand,
 	.eh_host_reset_handler =	storvsc_host_reset_handler,
 	.slave_alloc =		storvsc_device_alloc,
+	.slave_destroy =	storvsc_device_destroy,
 	.slave_configure =	storvsc_device_configure,
 	.cmd_per_lun =		1,
 	/* 64 max_queue * 1 target */
@@ -1308,14 +1424,7 @@ static struct scsi_host_template scsi_driver = {
 	/* no use setting to 0 since ll_blk_rw reset it to 1 */
 	/* currently 32 */
 	.sg_tablesize =		MAX_MULTIPAGE_BUFFER_COUNT,
-	/*
-	 * ENABLE_CLUSTERING allows mutiple physically contig bio_vecs to merge
-	 * into 1 sg element. If set, we must limit the max_segment_size to
-	 * PAGE_SIZE, otherwise we may get 1 sg element that represents
-	 * multiple
-	 */
-	/* physically contig pfns (ie sg[x].length > PAGE_SIZE). */
-	.use_clustering =	ENABLE_CLUSTERING,
+	.use_clustering =	DISABLE_CLUSTERING,
 	/* Make sure we dont get a sg segment crosses a page boundary */
 	.dma_boundary =		PAGE_SIZE-1,
 };
@@ -1360,27 +1469,17 @@ static int storvsc_probe(struct hv_device *device,
 	if (!host)
 		return -ENOMEM;
 
-	host_dev = (struct hv_host_device *)host->hostdata;
+	host_dev = shost_priv(host);
 	memset(host_dev, 0, sizeof(struct hv_host_device));
 
 	host_dev->port = host->host_no;
 	host_dev->dev = device;
 
-	host_dev->request_pool =
-				kmem_cache_create(dev_name(&device->device),
-					sizeof(struct storvsc_cmd_request), 0,
-					SLAB_HWCACHE_ALIGN, NULL);
-
-	if (!host_dev->request_pool) {
-		scsi_host_put(host);
-		return -ENOMEM;
-	}
 
 	stor_device = kzalloc(sizeof(struct storvsc_device), GFP_KERNEL);
 	if (!stor_device) {
-		kmem_cache_destroy(host_dev->request_pool);
-		scsi_host_put(host);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto err_out0;
 	}
 
 	stor_device->destroy = false;
@@ -1391,12 +1490,8 @@ static int storvsc_probe(struct hv_device *device,
 
 	stor_device->port_number = host->host_no;
 	ret = storvsc_connect_to_vsp(device, storvsc_ringbuffer_size);
-	if (ret) {
-		kmem_cache_destroy(host_dev->request_pool);
-		scsi_host_put(host);
-		kfree(stor_device);
-		return ret;
-	}
+	if (ret)
+		goto err_out1;
 
 	if (dev_is_ide)
 		storvsc_get_ide_info(device, &target, &path);
@@ -1416,7 +1511,7 @@ static int storvsc_probe(struct hv_device *device,
 	/* Register the HBA and start the scsi bus scan */
 	ret = scsi_add_host(host, &device->device);
 	if (ret != 0)
-		goto err_out;
+		goto err_out2;
 
 	if (!dev_is_ide) {
 		scsi_scan_host(host);
@@ -1425,21 +1520,32 @@ static int storvsc_probe(struct hv_device *device,
 	ret = scsi_add_device(host, 0, target, 0);
 	if (ret) {
 		scsi_remove_host(host);
-		goto err_out;
+		goto err_out2;
 	}
 	return 0;
 
-err_out:
+err_out2:
+	/*
+	 * Once we have connected with the host, we would need to
+	 * to invoke storvsc_dev_remove() to rollback this state and
+	 * this call also frees up the stor_device; hence the jump around
+	 * err_out1 label.
+	 */
 	storvsc_dev_remove(device);
-	kmem_cache_destroy(host_dev->request_pool);
+	goto err_out0;
+
+err_out1:
+	kfree(stor_device);
+
+err_out0:
 	scsi_host_put(host);
-	return -ENODEV;
+	return ret;
 }
 
 /* The one and only one */
 
 static struct hv_driver storvsc_drv = {
-	.name = "storvsc",
+	.name = KBUILD_MODNAME,
 	.id_table = id_table,
 	.probe = storvsc_probe,
 	.remove = storvsc_remove,
diff --git a/drivers/staging/iio/Documentation/generic_buffer.c b/drivers/staging/iio/Documentation/generic_buffer.c
index d58095321491..69a05b9456d6 100644
--- a/drivers/staging/iio/Documentation/generic_buffer.c
+++ b/drivers/staging/iio/Documentation/generic_buffer.c
@@ -28,6 +28,7 @@
 #include <linux/types.h>
 #include <string.h>
 #include <poll.h>
+#include <endian.h>
 #include "iio_utils.h"
 
 /**
@@ -56,6 +57,13 @@ int size_from_channelarray(struct iio_channel_info *channels, int num_channels)
 
 void print2byte(int input, struct iio_channel_info *info)
 {
+	/* First swap if incorrect endian */
+
+	if (info->be)
+		input = be16toh((uint_16t)input);
+	else
+		input = le16toh((uint_16t)input);
+
 	/* shift before conversion to avoid sign extension
 	   of left aligned data */
 	input = input >> info->shift;
diff --git a/drivers/staging/iio/Documentation/iio_utils.h b/drivers/staging/iio/Documentation/iio_utils.h
index 75938b2412ff..6f3a392297ec 100644
--- a/drivers/staging/iio/Documentation/iio_utils.h
+++ b/drivers/staging/iio/Documentation/iio_utils.h
@@ -13,6 +13,7 @@
 #include <ctype.h>
 #include <stdio.h>
 #include <stdint.h>
+#include <dirent.h>
 
 #define IIO_MAX_NAME_LENGTH 30
 
@@ -73,6 +74,7 @@ struct iio_channel_info {
 	unsigned bits_used;
 	unsigned shift;
 	uint64_t mask;
+	unsigned be;
 	unsigned is_signed;
 	unsigned enabled;
 	unsigned location;
@@ -83,6 +85,7 @@ struct iio_channel_info {
  * @is_signed: output whether channel is signed
  * @bytes: output how many bytes the channel storage occupies
  * @mask: output a bit mask for the raw data
+ * @be: big endian
  * @device_dir: the iio device directory
  * @name: the channel name
  * @generic_name: the channel type name
@@ -92,6 +95,7 @@ inline int iioutils_get_type(unsigned *is_signed,
 			     unsigned *bits_used,
 			     unsigned *shift,
 			     uint64_t *mask,
+			     unsigned *be,
 			     const char *device_dir,
 			     const char *name,
 			     const char *generic_name)
@@ -100,7 +104,7 @@ inline int iioutils_get_type(unsigned *is_signed,
 	int ret;
 	DIR *dp;
 	char *scan_el_dir, *builtname, *builtname_generic, *filename = 0;
-	char signchar;
+	char signchar, endianchar;
 	unsigned padint;
 	const struct dirent *ent;
 
@@ -144,9 +148,18 @@ inline int iioutils_get_type(unsigned *is_signed,
 				ret = -errno;
 				goto error_free_filename;
 			}
-			fscanf(sysfsfp,
-			       "%c%u/%u>>%u", &signchar, bits_used,
-			       &padint, shift);
+
+			ret = fscanf(sysfsfp,
+				     "%ce:%c%u/%u>>%u",
+				     &endianchar,
+				     &signchar,
+				     bits_used,
+				     &padint, shift);
+			if (ret < 0) {
+				printf("failed to pass scan type description\n");
+				return ret;
+			}
+			*be = (endianchar == 'b');
 			*bytes = padint / 8;
 			if (*bits_used == 64)
 				*mask = ~0;
@@ -156,6 +169,10 @@ inline int iioutils_get_type(unsigned *is_signed,
 				*is_signed = 1;
 			else
 				*is_signed = 0;
+			fclose(sysfsfp);
+			free(filename);
+
+			filename = 0;
 		}
 error_free_filename:
 	if (filename)
@@ -386,6 +403,7 @@ inline int build_channel_array(const char *device_dir,
 						&current->bits_used,
 						&current->shift,
 						&current->mask,
+						&current->be,
 						device_dir,
 						current->name,
 						current->generic_name);
diff --git a/drivers/staging/iio/Documentation/ring.txt b/drivers/staging/iio/Documentation/ring.txt
index 7e99ef2b7bc0..e33807761cd7 100644
--- a/drivers/staging/iio/Documentation/ring.txt
+++ b/drivers/staging/iio/Documentation/ring.txt
@@ -23,10 +23,6 @@ The function pointers within here are used to allow the core to handle
 as much buffer functionality as possible. Note almost all of these
 are optional.
 
-mark_in_use, unmark_in_use
-  Basically indicate that not changes should be made to the buffer state that
-  will effect the form of the data being captures (e.g. scan elements or length)
-
 store_to
   If possible, push data to the buffer.
 
@@ -39,8 +35,6 @@ rip_first_n
   The primary buffer reading function. Note that it may well not return
   as much data as requested.
 
-mark_param_changed
-  Used to indicate that something has changed. Used in conjunction with
 request_update
   If parameters have changed that require reinitialization or configuration of
   the buffer this will trigger it.
@@ -51,7 +45,3 @@ get_bytes_per_datum, set_bytes_per_datum
 get_length / set_length
   Get/set the number of complete scans that may be held by the buffer.
 
-is_enabled
-  Query if ring buffer is in use
-enable
-  Start the ring buffer.
diff --git a/drivers/staging/iio/Documentation/sysfs-bus-iio b/drivers/staging/iio/Documentation/sysfs-bus-iio
index 0d6823d19b61..46a995d6d261 100644
--- a/drivers/staging/iio/Documentation/sysfs-bus-iio
+++ b/drivers/staging/iio/Documentation/sysfs-bus-iio
@@ -276,6 +276,16 @@ Description:
 		If a discrete set of scale values are available, they
 		are listed in this attribute.
 
+What:		/sys/.../in_accel_filter_low_pass_3db_frequency
+What:		/sys/.../in_magn_filter_low_pass_3db_frequency
+What:		/sys/.../in_anglvel_filter_low_pass_3db_frequency
+KernelVersion:	3.2
+Contact:	linux-iio@vger.kernel.org
+Description:
+		If a known or controllable low pass filter is applied
+		to the underlying data channel, then this parameter
+		gives the 3dB frequency of the filter in Hz.
+
 What:		/sys/bus/iio/devices/iio:deviceX/out_voltageY_raw
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
diff --git a/drivers/staging/iio/Kconfig b/drivers/staging/iio/Kconfig
index 4ec9118955f9..90162aa8b2df 100644
--- a/drivers/staging/iio/Kconfig
+++ b/drivers/staging/iio/Kconfig
@@ -76,7 +76,6 @@ config IIO_DUMMY_EVGEN
 
 config IIO_SIMPLE_DUMMY
        tristate "An example driver with no hardware requirements"
-       select IIO_SIMPLE_DUMMY_EVGEN if IIO_SIMPLE_DUMMY_EVENTS
        help
 	 Driver intended mainly as documentation for how to write
 	 a driver. May also be useful for testing userspace code
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index 97f747eac647..d439e45d07fa 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -17,7 +17,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "adis16201.h"
 
@@ -322,8 +322,7 @@ static int adis16201_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_VOLTAGE:
 			*val = 0;
@@ -348,10 +347,10 @@ static int adis16201_read_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 		}
 		break;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		*val = 25;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
 		case IIO_ACCEL:
 			bits = 12;
@@ -388,7 +387,7 @@ static int adis16201_write_raw(struct iio_dev *indio_dev,
 	s16 val16;
 	u8 addr;
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
 		case IIO_ACCEL:
 			bits = 12;
@@ -408,36 +407,36 @@ static int adis16201_write_raw(struct iio_dev *indio_dev,
 
 static struct iio_chan_spec adis16201_channels[] = {
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "supply", 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_supply, ADIS16201_SCAN_SUPPLY,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE) |
-		 (1 << IIO_CHAN_INFO_OFFSET_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
 		 temp, ADIS16201_SCAN_TEMP,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 accel_x, ADIS16201_SCAN_ACC_X,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 accel_y, ADIS16201_SCAN_ACC_Y,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_aux, ADIS16201_SCAN_AUX_ADC,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 incli_x, ADIS16201_SCAN_INCLI_X,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 incli_y, ADIS16201_SCAN_INCLI_Y,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN_SOFT_TIMESTAMP(7)
@@ -554,3 +553,4 @@ module_spi_driver(adis16201_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADIS16201 Programmable Digital Vibration Sensor driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16201");
diff --git a/drivers/staging/iio/accel/adis16201_ring.c b/drivers/staging/iio/accel/adis16201_ring.c
index 0016ed378e3a..26c610faee3f 100644
--- a/drivers/staging/iio/accel/adis16201_ring.c
+++ b/drivers/staging/iio/accel/adis16201_ring.c
@@ -74,11 +74,11 @@ static irqreturn_t adis16201_trigger_handler(int irq, void *p)
 		return -ENOMEM;
 	}
 
-	if (ring->scan_count)
-		if (adis16201_read_ring_data(indio_dev, st->rx) >= 0)
-			for (; i < ring->scan_count; i++)
-				data[i] = be16_to_cpup(
-					(__be16 *)&(st->rx[i*2]));
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)
+	    && adis16201_read_ring_data(indio_dev, st->rx) >= 0)
+		for (; i < bitmap_weight(indio_dev->active_scan_mask,
+					 indio_dev->masklength); i++)
+			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
 	if (ring->scan_timestamp)
@@ -116,11 +116,9 @@ int adis16201_configure_ring(struct iio_dev *indio_dev)
 	}
 	indio_dev->buffer = ring;
 	/* Effectively select the ring buffer implementation */
-	ring->bpe = 2;
 	ring->scan_timestamp = true;
 	ring->access = &ring_sw_access_funcs;
-	ring->setup_ops = &adis16201_ring_setup_ops;
-	ring->owner = THIS_MODULE;
+	indio_dev->setup_ops = &adis16201_ring_setup_ops;
 
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 						 &adis16201_trigger_handler,
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index a6d6d27f3c97..1a5140f9e3f4 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -17,7 +17,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "adis16203.h"
 
@@ -329,8 +329,7 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_VOLTAGE:
 			*val = 0;
@@ -350,10 +349,10 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
 		default:
 			return -EINVAL;
 		}
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		*val = 25;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		bits = 14;
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16203_addresses[chan->address][1];
@@ -374,26 +373,26 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
 
 static struct iio_chan_spec adis16203_channels[] = {
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "supply", 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_supply, ADIS16203_SCAN_SUPPLY,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_aux, ADIS16203_SCAN_AUX_ADC,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 incli_x, ADIS16203_SCAN_INCLI_X,
 		 IIO_ST('s', 14, 16, 0), 0),
 	/* Fixme: Not what it appears to be - see data sheet */
 	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 incli_y, ADIS16203_SCAN_INCLI_Y,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE) |
-		 (1 << IIO_CHAN_INFO_OFFSET_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
 		 temp, ADIS16203_SCAN_TEMP,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN_SOFT_TIMESTAMP(5),
@@ -509,3 +508,4 @@ module_spi_driver(adis16203_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADIS16203 Programmable Digital Vibration Sensor driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16203");
diff --git a/drivers/staging/iio/accel/adis16203_ring.c b/drivers/staging/iio/accel/adis16203_ring.c
index 1fdfe6f6ac6e..064640d15e41 100644
--- a/drivers/staging/iio/accel/adis16203_ring.c
+++ b/drivers/staging/iio/accel/adis16203_ring.c
@@ -74,11 +74,11 @@ static irqreturn_t adis16203_trigger_handler(int irq, void *p)
 		return -ENOMEM;
 	}
 
-	if (ring->scan_count)
-		if (adis16203_read_ring_data(&indio_dev->dev, st->rx) >= 0)
-			for (; i < ring->scan_count; i++)
-				data[i] = be16_to_cpup(
-					(__be16 *)&(st->rx[i*2]));
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
+	    adis16203_read_ring_data(&indio_dev->dev, st->rx) >= 0)
+		for (; i < bitmap_weight(indio_dev->active_scan_mask,
+					 indio_dev->masklength); i++)
+			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
 	if (ring->scan_timestamp)
@@ -118,11 +118,9 @@ int adis16203_configure_ring(struct iio_dev *indio_dev)
 	}
 	indio_dev->buffer = ring;
 	/* Effectively select the ring buffer implementation */
-	ring->bpe = 2;
 	ring->scan_timestamp = true;
 	ring->access = &ring_sw_access_funcs;
-	ring->setup_ops = &adis16203_ring_setup_ops;
-	ring->owner = THIS_MODULE;
+	indio_dev->setup_ops = &adis16203_ring_setup_ops;
 
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 						 &adis16203_trigger_handler,
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index 7ac5b4c533d8..fa89364b841e 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -20,7 +20,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "adis16204.h"
 
@@ -366,7 +366,7 @@ static int adis16204_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_VOLTAGE:
 			*val = 0;
@@ -390,12 +390,12 @@ static int adis16204_read_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 		}
 		break;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		*val = 25;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
-	case (1 << IIO_CHAN_INFO_PEAK_SEPARATE):
-		if (mask == (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE)) {
+	case IIO_CHAN_INFO_CALIBBIAS:
+	case IIO_CHAN_INFO_PEAK:
+		if (mask == IIO_CHAN_INFO_CALIBBIAS) {
 			bits = 12;
 			addrind = 1;
 		} else { /* PEAK_SEPARATE */
@@ -428,7 +428,7 @@ static int adis16204_write_raw(struct iio_dev *indio_dev,
 	s16 val16;
 	u8 addr;
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
 		case IIO_ACCEL:
 			bits = 12;
@@ -445,28 +445,28 @@ static int adis16204_write_raw(struct iio_dev *indio_dev,
 
 static struct iio_chan_spec adis16204_channels[] = {
 	IIO_CHAN(IIO_VOLTAGE, 0, 0, 0, "supply", 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_supply, ADIS16204_SCAN_SUPPLY,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_aux, ADIS16204_SCAN_AUX_ADC,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE) |
-		 (1 << IIO_CHAN_INFO_OFFSET_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
 		 temp, ADIS16204_SCAN_TEMP,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		 (1 << IIO_CHAN_INFO_PEAK_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		 IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
 		 accel_x, ADIS16204_SCAN_ACC_X,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		 (1 << IIO_CHAN_INFO_PEAK_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		 IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
 		 accel_y, ADIS16204_SCAN_ACC_Y,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN_SOFT_TIMESTAMP(5),
@@ -582,3 +582,4 @@ module_spi_driver(adis16204_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("ADIS16204 High-g Digital Impact Sensor and Recorder");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16204");
diff --git a/drivers/staging/iio/accel/adis16204_ring.c b/drivers/staging/iio/accel/adis16204_ring.c
index 6fd3d8f51f2c..4081179dfa5c 100644
--- a/drivers/staging/iio/accel/adis16204_ring.c
+++ b/drivers/staging/iio/accel/adis16204_ring.c
@@ -71,11 +71,11 @@ static irqreturn_t adis16204_trigger_handler(int irq, void *p)
 		return -ENOMEM;
 	}
 
-	if (ring->scan_count)
-		if (adis16204_read_ring_data(&indio_dev->dev, st->rx) >= 0)
-			for (; i < ring->scan_count; i++)
-				data[i] = be16_to_cpup(
-					(__be16 *)&(st->rx[i*2]));
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
+	    adis16204_read_ring_data(&indio_dev->dev, st->rx) >= 0)
+		for (; i < bitmap_weight(indio_dev->active_scan_mask,
+					 indio_dev->masklength); i++)
+			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
 	if (ring->scan_timestamp)
@@ -114,10 +114,8 @@ int adis16204_configure_ring(struct iio_dev *indio_dev)
 	indio_dev->buffer = ring;
 	/* Effectively select the ring buffer implementation */
 	ring->access = &ring_sw_access_funcs;
-	ring->bpe = 2;
 	ring->scan_timestamp = true;
-	ring->setup_ops = &adis16204_ring_setup_ops;
-	ring->owner = THIS_MODULE;
+	indio_dev->setup_ops = &adis16204_ring_setup_ops;
 
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 						 &adis16204_trigger_handler,
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index c03afbf5bbdc..a98715f6bd6d 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -18,7 +18,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "adis16209.h"
 
@@ -304,7 +304,7 @@ static int adis16209_write_raw(struct iio_dev *indio_dev,
 	s16 val16;
 	u8 addr;
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
 		case IIO_ACCEL:
 		case IIO_INCLI:
@@ -355,8 +355,7 @@ static int adis16209_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_VOLTAGE:
 			*val = 0;
@@ -381,10 +380,10 @@ static int adis16209_read_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 		}
 		break;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		*val = 25;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
 		case IIO_ACCEL:
 			bits = 14;
@@ -410,34 +409,34 @@ static int adis16209_read_raw(struct iio_dev *indio_dev,
 
 static struct iio_chan_spec adis16209_channels[] = {
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_supply, ADIS16209_SCAN_SUPPLY,
 		 IIO_ST('u', 14, 16, 0), 0),
 	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE) |
-		 (1 << IIO_CHAN_INFO_OFFSET_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT,
 		 temp, ADIS16209_SCAN_TEMP,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 accel_x, ADIS16209_SCAN_ACC_X,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 accel_y, ADIS16209_SCAN_ACC_Y,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_aux, ADIS16209_SCAN_AUX_ADC,
 		 IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 incli_x, ADIS16209_SCAN_INCLI_X,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_INCLI, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 incli_y, ADIS16209_SCAN_INCLI_Y,
 		 IIO_ST('s', 14, 16, 0), 0),
 	IIO_CHAN(IIO_ROT, 0, 1, 0, NULL, 0, IIO_MOD_X,
@@ -558,3 +557,4 @@ module_spi_driver(adis16209_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADIS16209 Digital Vibration Sensor driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16209");
diff --git a/drivers/staging/iio/accel/adis16209_ring.c b/drivers/staging/iio/accel/adis16209_ring.c
index d17e39d95459..2a6fd334f5f1 100644
--- a/drivers/staging/iio/accel/adis16209_ring.c
+++ b/drivers/staging/iio/accel/adis16209_ring.c
@@ -72,9 +72,10 @@ static irqreturn_t adis16209_trigger_handler(int irq, void *p)
 		return -ENOMEM;
 	}
 
-	if (ring->scan_count &&
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
 	    adis16209_read_ring_data(&indio_dev->dev, st->rx) >= 0)
-		for (; i < ring->scan_count; i++)
+		for (; i < bitmap_weight(indio_dev->active_scan_mask,
+					 indio_dev->masklength); i++)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
@@ -114,10 +115,8 @@ int adis16209_configure_ring(struct iio_dev *indio_dev)
 	indio_dev->buffer = ring;
 	/* Effectively select the ring buffer implementation */
 	ring->access = &ring_sw_access_funcs;
-	ring->bpe = 2;
 	ring->scan_timestamp = true;
-	ring->setup_ops = &adis16209_ring_setup_ops;
-	ring->owner = THIS_MODULE;
+	indio_dev->setup_ops = &adis16209_ring_setup_ops;
 
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 						 &adis16209_trigger_handler,
diff --git a/drivers/staging/iio/accel/adis16220_core.c b/drivers/staging/iio/accel/adis16220_core.c
index 73298e7849e6..51a852d45482 100644
--- a/drivers/staging/iio/accel/adis16220_core.c
+++ b/drivers/staging/iio/accel/adis16220_core.c
@@ -167,9 +167,9 @@ static ssize_t adis16220_write_16bit(struct device *dev,
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
-	long val;
+	u16 val;
 
-	ret = strict_strtol(buf, 10, &val);
+	ret = kstrtou16(buf, 10, &val);
 	if (ret)
 		goto error_ret;
 	ret = adis16220_spi_write_reg_16(indio_dev, this_attr->address, val);
@@ -510,17 +510,17 @@ static int adis16220_read_raw(struct iio_dev *indio_dev,
 	case 0:
 		addrind = 0;
 		break;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		if (chan->type == IIO_TEMP) {
 			*val = 25;
 			return IIO_VAL_INT;
 		}
 		addrind = 1;
 		break;
-	case (1 << IIO_CHAN_INFO_PEAK_SEPARATE):
+	case IIO_CHAN_INFO_PEAK:
 		addrind = 2;
 		break;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		*val = 0;
 		switch (chan->type) {
 		case IIO_TEMP:
@@ -575,27 +575,27 @@ static const struct iio_chan_spec adis16220_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_supply,
 	}, {
 		.type = IIO_ACCEL,
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-			     (1 << IIO_CHAN_INFO_SCALE_SEPARATE) |
-			     (1 << IIO_CHAN_INFO_PEAK_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+			     IIO_CHAN_INFO_PEAK_SEPARATE_BIT,
 		.address = accel,
 	}, {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-			     (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = temp,
 	}, {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-			     (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+			     IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_1,
 	}, {
 		.type = IIO_VOLTAGE,
@@ -713,3 +713,4 @@ module_spi_driver(adis16220_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADIS16220 Digital Vibration Sensor");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16220");
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index 88881b9919ef..17f77fef7f2b 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -21,7 +21,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "adis16240.h"
 
@@ -389,8 +389,7 @@ static int adis16240_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_VOLTAGE:
 			*val = 0;
@@ -411,14 +410,14 @@ static int adis16240_read_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 		}
 		break;
-	case (1 << IIO_CHAN_INFO_PEAK_SCALE_SHARED):
+	case IIO_CHAN_INFO_PEAK_SCALE:
 		*val = 6;
 		*val2 = 629295;
 		return IIO_VAL_INT_PLUS_MICRO;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		*val = 25;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		bits = 10;
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16240_addresses[chan->address][1];
@@ -432,7 +431,7 @@ static int adis16240_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_PEAK_SEPARATE):
+	case IIO_CHAN_INFO_PEAK:
 		bits = 10;
 		mutex_lock(&indio_dev->mlock);
 		addr = adis16240_addresses[chan->address][2];
@@ -460,7 +459,7 @@ static int adis16240_write_raw(struct iio_dev *indio_dev,
 	s16 val16;
 	u8 addr;
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		val16 = val & ((1 << bits) - 1);
 		addr = adis16240_addresses[chan->address][1];
 		return adis16240_spi_write_reg_16(indio_dev, addr, val16);
@@ -470,7 +469,7 @@ static int adis16240_write_raw(struct iio_dev *indio_dev,
 
 static struct iio_chan_spec adis16240_channels[] = {
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "supply", 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 in_supply, ADIS16240_SCAN_SUPPLY,
 		 IIO_ST('u', 10, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
@@ -478,22 +477,22 @@ static struct iio_chan_spec adis16240_channels[] = {
 		 in_aux, ADIS16240_SCAN_AUX_ADC,
 		 IIO_ST('u', 10, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_X,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 accel_x, ADIS16240_SCAN_ACC_X,
 		 IIO_ST('s', 10, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Y,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 accel_y, ADIS16240_SCAN_ACC_Y,
 		 IIO_ST('s', 10, 16, 0), 0),
 	IIO_CHAN(IIO_ACCEL, 1, 0, 0, NULL, 0, IIO_MOD_Z,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED) |
-		 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		 accel_z, ADIS16240_SCAN_ACC_Z,
 		 IIO_ST('s', 10, 16, 0), 0),
 	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 temp, ADIS16240_SCAN_TEMP,
 		 IIO_ST('u', 10, 16, 0), 0),
 	IIO_CHAN_SOFT_TIMESTAMP(6)
@@ -611,3 +610,4 @@ module_spi_driver(adis16240_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices Programmable Impact Sensor and Recorder");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16240");
diff --git a/drivers/staging/iio/accel/adis16240_ring.c b/drivers/staging/iio/accel/adis16240_ring.c
index b907ca3f4fdf..e23622d96f9f 100644
--- a/drivers/staging/iio/accel/adis16240_ring.c
+++ b/drivers/staging/iio/accel/adis16240_ring.c
@@ -69,9 +69,10 @@ static irqreturn_t adis16240_trigger_handler(int irq, void *p)
 		return -ENOMEM;
 	}
 
-	if (ring->scan_count &&
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
 	    adis16240_read_ring_data(&indio_dev->dev, st->rx) >= 0)
-		for (; i < ring->scan_count; i++)
+		for (; i < bitmap_weight(indio_dev->active_scan_mask,
+					 indio_dev->masklength); i++)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
@@ -111,10 +112,8 @@ int adis16240_configure_ring(struct iio_dev *indio_dev)
 	indio_dev->buffer = ring;
 	/* Effectively select the ring buffer implementation */
 	ring->access = &ring_sw_access_funcs;
-	ring->bpe = 2;
 	ring->scan_timestamp = true;
-	ring->setup_ops = &adis16240_ring_setup_ops;
-	ring->owner = THIS_MODULE;
+	indio_dev->setup_ops = &adis16240_ring_setup_ops;
 
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 						 &adis16240_trigger_handler,
diff --git a/drivers/staging/iio/accel/kxsd9.c b/drivers/staging/iio/accel/kxsd9.c
index cfce21c2eddc..d13d7215ff6e 100644
--- a/drivers/staging/iio/accel/kxsd9.c
+++ b/drivers/staging/iio/accel/kxsd9.c
@@ -140,7 +140,7 @@ static int kxsd9_write_raw(struct iio_dev *indio_dev,
 {
 	int ret = -EINVAL;
 
-	if (mask == (1 << IIO_CHAN_INFO_SCALE_SHARED)) {
+	if (mask == IIO_CHAN_INFO_SCALE) {
 		/* Check no integer component */
 		if (val)
 			return -EINVAL;
@@ -164,7 +164,7 @@ static int kxsd9_read_raw(struct iio_dev *indio_dev,
 			goto error_ret;
 		*val = ret;
 		break;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		ret = spi_w8r8(st->us, KXSD9_READ(KXSD9_REG_CTRL_C));
 		if (ret)
 			goto error_ret;
@@ -181,7 +181,7 @@ error_ret:
 		.type = IIO_ACCEL,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = 1 << IIO_CHAN_INFO_SCALE_SHARED,		\
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
 		.address = KXSD9_REG_##axis,				\
 	}
 
@@ -268,8 +268,10 @@ static int __devexit kxsd9_remove(struct spi_device *spi)
 }
 
 static const struct spi_device_id kxsd9_id[] = {
-	{"kxsd9", 0}
+	{"kxsd9", 0},
+	{ },
 };
+MODULE_DEVICE_TABLE(spi, kxsd9_id);
 
 static struct spi_driver kxsd9_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/accel/lis3l02dq.h b/drivers/staging/iio/accel/lis3l02dq.h
index 7237a9ab61a8..2db383fc2743 100644
--- a/drivers/staging/iio/accel/lis3l02dq.h
+++ b/drivers/staging/iio/accel/lis3l02dq.h
@@ -181,11 +181,6 @@ int lis3l02dq_disable_all_events(struct iio_dev *indio_dev);
 void lis3l02dq_remove_trigger(struct iio_dev *indio_dev);
 int lis3l02dq_probe_trigger(struct iio_dev *indio_dev);
 
-ssize_t lis3l02dq_read_accel_from_buffer(struct iio_buffer *buffer,
-				       int index,
-				       int *val);
-
-
 int lis3l02dq_configure_buffer(struct iio_dev *indio_dev);
 void lis3l02dq_unconfigure_buffer(struct iio_dev *indio_dev);
 
@@ -212,13 +207,6 @@ static inline int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
 {
 	return 0;
 }
-static inline ssize_t
-lis3l02dq_read_accel_from_buffer(struct iio_buffer *buffer,
-				 int index,
-				 int *val)
-{
-	return 0;
-}
 
 static int lis3l02dq_configure_buffer(struct iio_dev *indio_dev)
 {
diff --git a/drivers/staging/iio/accel/lis3l02dq_core.c b/drivers/staging/iio/accel/lis3l02dq_core.c
index 6877521ec173..376da5137967 100644
--- a/drivers/staging/iio/accel/lis3l02dq_core.c
+++ b/drivers/staging/iio/accel/lis3l02dq_core.c
@@ -25,7 +25,8 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../events.h"
+#include "../buffer.h"
 
 #include "lis3l02dq.h"
 
@@ -226,14 +227,14 @@ static int lis3l02dq_write_raw(struct iio_dev *indio_dev,
 	u8 uval;
 	s8 sval;
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		if (val > 255 || val < -256)
 			return -EINVAL;
 		sval = val;
 		reg = lis3l02dq_axis_map[LIS3L02DQ_BIAS][chan->address];
 		ret = lis3l02dq_spi_write_reg_8(indio_dev, reg, sval);
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		if (val & ~0xFF)
 			return -EINVAL;
 		uval = val;
@@ -259,23 +260,20 @@ static int lis3l02dq_read_raw(struct iio_dev *indio_dev,
 	case 0:
 		/* Take the iio_dev status lock */
 		mutex_lock(&indio_dev->mlock);
-		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED)
-			ret = lis3l02dq_read_accel_from_buffer(indio_dev->
-							       buffer,
-							       chan->scan_index,
-							       val);
-		else {
+		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
+			ret = -EBUSY;
+		} else {
 			reg = lis3l02dq_axis_map
 				[LIS3L02DQ_ACCEL][chan->address];
 			ret = lis3l02dq_read_reg_s16(indio_dev, reg, val);
 		}
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		*val = 0;
 		*val2 = 9580;
 		return IIO_VAL_INT_PLUS_MICRO;
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		reg = lis3l02dq_axis_map[LIS3L02DQ_GAIN][chan->address];
 		ret = lis3l02dq_spi_read_reg_8(indio_dev, reg, &utemp);
 		if (ret)
@@ -284,7 +282,7 @@ static int lis3l02dq_read_raw(struct iio_dev *indio_dev,
 		*val = utemp;
 		return IIO_VAL_INT;
 
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		reg = lis3l02dq_axis_map[LIS3L02DQ_BIAS][chan->address];
 		ret = lis3l02dq_spi_read_reg_8(indio_dev, reg, (u8 *)&stemp);
 		/* to match with what previous code does */
@@ -331,11 +329,11 @@ static ssize_t lis3l02dq_write_frequency(struct device *dev,
 					 size_t len)
 {
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	long val;
+	unsigned long val;
 	int ret;
 	u8 t;
 
-	ret = strict_strtol(buf, 10, &val);
+	ret = kstrtoul(buf, 10, &val);
 	if (ret)
 		return ret;
 
@@ -515,9 +513,9 @@ static irqreturn_t lis3l02dq_event_handler(int irq, void *private)
 }
 
 #define LIS3L02DQ_INFO_MASK				\
-	((1 << IIO_CHAN_INFO_SCALE_SHARED) |		\
-	 (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |	\
-	 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE))
+	(IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
+	 IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |	\
+	 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT)
 
 #define LIS3L02DQ_EVENT_MASK					\
 	(IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |	\
@@ -534,7 +532,7 @@ static struct iio_chan_spec lis3l02dq_channels[] = {
 };
 
 
-static ssize_t lis3l02dq_read_event_config(struct iio_dev *indio_dev,
+static int lis3l02dq_read_event_config(struct iio_dev *indio_dev,
 					   u64 event_code)
 {
 
@@ -809,3 +807,4 @@ module_spi_driver(lis3l02dq_driver);
 MODULE_AUTHOR("Jonathan Cameron <jic23@cam.ac.uk>");
 MODULE_DESCRIPTION("ST LIS3L02DQ Accelerometer SPI driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:lis3l02dq");
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c
index 89527af8f4c5..98c5c92d3450 100644
--- a/drivers/staging/iio/accel/lis3l02dq_ring.c
+++ b/drivers/staging/iio/accel/lis3l02dq_ring.c
@@ -38,38 +38,6 @@ irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private)
 		return IRQ_WAKE_THREAD;
 }
 
-/**
- * lis3l02dq_read_accel_from_buffer() individual acceleration read from buffer
- **/
-ssize_t lis3l02dq_read_accel_from_buffer(struct iio_buffer *buffer,
-					 int index,
-					 int *val)
-{
-	int ret;
-	s16 *data;
-
-	if (!iio_scan_mask_query(buffer, index))
-		return -EINVAL;
-
-	if (!buffer->access->read_last)
-		return -EBUSY;
-
-	data = kmalloc(buffer->access->get_bytes_per_datum(buffer),
-		       GFP_KERNEL);
-	if (data == NULL)
-		return -ENOMEM;
-
-	ret = buffer->access->read_last(buffer, (u8 *)data);
-	if (ret)
-		goto error_free_data;
-	*val = data[bitmap_weight(buffer->scan_mask, index)];
-error_free_data:
-
-	kfree(data);
-
-	return ret;
-}
-
 static const u8 read_all_tx_array[] = {
 	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_L_ADDR), 0,
 	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_H_ADDR), 0,
@@ -87,21 +55,21 @@ static const u8 read_all_tx_array[] = {
  **/
 static int lis3l02dq_read_all(struct iio_dev *indio_dev, u8 *rx_array)
 {
-	struct iio_buffer *buffer = indio_dev->buffer;
 	struct lis3l02dq_state *st = iio_priv(indio_dev);
 	struct spi_transfer *xfers;
 	struct spi_message msg;
 	int ret, i, j = 0;
 
-	xfers = kzalloc((buffer->scan_count) * 2
-			* sizeof(*xfers), GFP_KERNEL);
+	xfers = kcalloc(bitmap_weight(indio_dev->active_scan_mask,
+				      indio_dev->masklength) * 2,
+			sizeof(*xfers), GFP_KERNEL);
 	if (!xfers)
 		return -ENOMEM;
 
 	mutex_lock(&st->buf_lock);
 
 	for (i = 0; i < ARRAY_SIZE(read_all_tx_array)/4; i++)
-		if (test_bit(i, buffer->scan_mask)) {
+		if (test_bit(i, indio_dev->active_scan_mask)) {
 			/* lower byte */
 			xfers[j].tx_buf = st->tx + 2*j;
 			st->tx[2*j] = read_all_tx_array[i*4];
@@ -129,7 +97,8 @@ static int lis3l02dq_read_all(struct iio_dev *indio_dev, u8 *rx_array)
 	 * values in alternate bytes
 	 */
 	spi_message_init(&msg);
-	for (j = 0; j < buffer->scan_count * 2; j++)
+	for (j = 0; j < bitmap_weight(indio_dev->active_scan_mask,
+				      indio_dev->masklength) * 2; j++)
 		spi_message_add_tail(&xfers[j], &msg);
 
 	ret = spi_sync(st->us, &msg);
@@ -145,14 +114,16 @@ static int lis3l02dq_get_buffer_element(struct iio_dev *indio_dev,
 	int ret, i;
 	u8 *rx_array ;
 	s16 *data = (s16 *)buf;
+	int scan_count = bitmap_weight(indio_dev->active_scan_mask,
+				       indio_dev->masklength);
 
-	rx_array = kzalloc(4 * (indio_dev->buffer->scan_count), GFP_KERNEL);
+	rx_array = kzalloc(4 * scan_count, GFP_KERNEL);
 	if (rx_array == NULL)
 		return -ENOMEM;
 	ret = lis3l02dq_read_all(indio_dev, rx_array);
 	if (ret < 0)
 		return ret;
-	for (i = 0; i < indio_dev->buffer->scan_count; i++)
+	for (i = 0; i < scan_count; i++)
 		data[i] = combine_8_to_16(rx_array[i*4+1],
 					rx_array[i*4+3]);
 	kfree(rx_array);
@@ -175,7 +146,7 @@ static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 		return -ENOMEM;
 	}
 
-	if (buffer->scan_count)
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		len = lis3l02dq_get_buffer_element(indio_dev, data);
 
 	  /* Guaranteed to be aligned with 8 byte boundary */
@@ -363,17 +334,17 @@ static int lis3l02dq_buffer_postenable(struct iio_dev *indio_dev)
 	if (ret)
 		goto error_ret;
 
-	if (iio_scan_mask_query(indio_dev->buffer, 0)) {
+	if (test_bit(0, indio_dev->active_scan_mask)) {
 		t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
 		oneenabled = true;
 	} else
 		t &= ~LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
-	if (iio_scan_mask_query(indio_dev->buffer, 1)) {
+	if (test_bit(1, indio_dev->active_scan_mask)) {
 		t |= LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
 		oneenabled = true;
 	} else
 		t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
-	if (iio_scan_mask_query(indio_dev->buffer, 2)) {
+	if (test_bit(2, indio_dev->active_scan_mask)) {
 		t |= LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
 		oneenabled = true;
 	} else
@@ -437,11 +408,9 @@ int lis3l02dq_configure_buffer(struct iio_dev *indio_dev)
 	indio_dev->buffer = buffer;
 	/* Effectively select the buffer implementation */
 	indio_dev->buffer->access = &lis3l02dq_access_funcs;
-	buffer->bpe = 2;
 
 	buffer->scan_timestamp = true;
-	buffer->setup_ops = &lis3l02dq_buffer_setup_ops;
-	buffer->owner = THIS_MODULE;
+	indio_dev->setup_ops = &lis3l02dq_buffer_setup_ops;
 
 	/* Functions are NULL as we set handler below */
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
diff --git a/drivers/staging/iio/accel/sca3000_core.c b/drivers/staging/iio/accel/sca3000_core.c
index 6c359074a06d..49764fb7181c 100644
--- a/drivers/staging/iio/accel/sca3000_core.c
+++ b/drivers/staging/iio/accel/sca3000_core.c
@@ -20,7 +20,8 @@
 #include <linux/module.h>
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../events.h"
+#include "../buffer.h"
 
 #include "sca3000.h"
 
@@ -381,13 +382,17 @@ sca3000_store_measurement_mode(struct device *dev,
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int ret;
-	int mask = 0x03;
-	long val;
+	u8 mask = 0x03;
+	u8 val;
 
 	mutex_lock(&st->lock);
-	ret = strict_strtol(buf, 10, &val);
+	ret = kstrtou8(buf, 10, &val);
 	if (ret)
 		goto error_ret;
+	if (val > 3) {
+		ret = -EINVAL;
+		goto error_ret;
+	}
 	ret = sca3000_read_data_short(st, SCA3000_REG_ADDR_MODE, 1);
 	if (ret)
 		goto error_ret;
@@ -424,7 +429,7 @@ static IIO_DEVICE_ATTR(measurement_mode, S_IRUGO | S_IWUSR,
 static IIO_DEVICE_ATTR(revision, S_IRUGO, sca3000_show_rev, NULL, 0);
 
 #define SCA3000_INFO_MASK			\
-	(1 << IIO_CHAN_INFO_SCALE_SHARED)
+	IIO_CHAN_INFO_SCALE_SHARED_BIT
 #define SCA3000_EVENT_MASK					\
 	(IIO_EV_BIT(IIO_EV_TYPE_MAG, IIO_EV_DIR_RISING))
 
@@ -474,7 +479,7 @@ static int sca3000_read_raw(struct iio_dev *indio_dev,
 			(sizeof(*val)*8 - 13);
 		mutex_unlock(&st->lock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		*val = 0;
 		if (chan->type == IIO_ACCEL)
 			*val2 = st->info->scale;
@@ -1161,9 +1166,9 @@ static int __devinit sca3000_probe(struct spi_device *spi)
 	if (ret < 0)
 		goto error_unregister_dev;
 	if (indio_dev->buffer) {
-		iio_scan_mask_set(indio_dev->buffer, 0);
-		iio_scan_mask_set(indio_dev->buffer, 1);
-		iio_scan_mask_set(indio_dev->buffer, 2);
+		iio_scan_mask_set(indio_dev, indio_dev->buffer, 0);
+		iio_scan_mask_set(indio_dev, indio_dev->buffer, 1);
+		iio_scan_mask_set(indio_dev, indio_dev->buffer, 2);
 	}
 
 	if (spi->irq) {
@@ -1240,6 +1245,7 @@ static const struct spi_device_id sca3000_id[] = {
 	{"sca3000_e05", e05},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, sca3000_id);
 
 static struct spi_driver sca3000_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/accel/sca3000_ring.c b/drivers/staging/iio/accel/sca3000_ring.c
index 4a9a01dccd0c..6b824a11f7f4 100644
--- a/drivers/staging/iio/accel/sca3000_ring.c
+++ b/drivers/staging/iio/accel/sca3000_ring.c
@@ -20,7 +20,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_hw.h"
 #include "sca3000.h"
 
@@ -146,7 +146,6 @@ static int sca3000_ring_get_bytes_per_datum(struct iio_buffer *r)
 }
 
 static IIO_BUFFER_ENABLE_ATTR;
-static IIO_BUFFER_BYTES_PER_DATUM_ATTR;
 static IIO_BUFFER_LENGTH_ATTR;
 
 /**
@@ -158,8 +157,7 @@ static ssize_t sca3000_query_ring_int(struct device *dev,
 {
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret, val;
-	struct iio_buffer *ring = dev_get_drvdata(dev);
-	struct iio_dev *indio_dev = ring->indio_dev;
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->lock);
@@ -180,8 +178,7 @@ static ssize_t sca3000_set_ring_int(struct device *dev,
 				      const char *buf,
 				      size_t len)
 {
-	struct iio_buffer *ring = dev_get_drvdata(dev);
-	struct iio_dev *indio_dev = ring->indio_dev;
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	long val;
@@ -222,8 +219,7 @@ static ssize_t sca3000_show_buffer_scale(struct device *dev,
 					 struct device_attribute *attr,
 					 char *buf)
 {
-	struct iio_buffer *ring = dev_get_drvdata(dev);
-	struct iio_dev *indio_dev = ring->indio_dev;
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct sca3000_state *st = iio_priv(indio_dev);
 
 	return sprintf(buf, "0.%06d\n", 4*st->info->scale);
@@ -243,7 +239,6 @@ static IIO_DEVICE_ATTR(in_accel_scale,
  */
 static struct attribute *sca3000_ring_attributes[] = {
 	&dev_attr_length.attr,
-	&dev_attr_bytes_per_datum.attr,
 	&dev_attr_enable.attr,
 	&iio_dev_attr_50_percent.dev_attr.attr,
 	&iio_dev_attr_75_percent.dev_attr.attr,
@@ -269,7 +264,7 @@ static struct iio_buffer *sca3000_rb_allocate(struct iio_dev *indio_dev)
 	buf = &ring->buf;
 	buf->stufftoread = 0;
 	buf->attrs = &sca3000_ring_attr;
-	iio_buffer_init(buf, indio_dev);
+	iio_buffer_init(buf);
 
 	return buf;
 }
@@ -350,7 +345,7 @@ static const struct iio_buffer_setup_ops sca3000_ring_setup_ops = {
 
 void sca3000_register_ring_funcs(struct iio_dev *indio_dev)
 {
-	indio_dev->buffer->setup_ops = &sca3000_ring_setup_ops;
+	indio_dev->setup_ops = &sca3000_ring_setup_ops;
 }
 
 /**
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index 797e65cd03e8..45f4504ed927 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -19,7 +19,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 #include "../trigger.h"
 #include "../trigger_consumer.h"
@@ -453,25 +453,6 @@ out:
 	return ret;
 }
 
-static int ad7192_scan_from_ring(struct ad7192_state *st, unsigned ch, int *val)
-{
-	struct iio_buffer *ring = iio_priv_to_dev(st)->buffer;
-	int ret;
-	s64 dat64[2];
-	u32 *dat32 = (u32 *)dat64;
-
-	if (!(test_bit(ch, ring->scan_mask)))
-		return  -EBUSY;
-
-	ret = ring->access->read_last(ring, (u8 *) &dat64);
-	if (ret)
-		return ret;
-
-	*val = *dat32;
-
-	return 0;
-}
-
 static int ad7192_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ad7192_state *st = iio_priv(indio_dev);
@@ -479,12 +460,14 @@ static int ad7192_ring_preenable(struct iio_dev *indio_dev)
 	size_t d_size;
 	unsigned channel;
 
-	if (!ring->scan_count)
+	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		return -EINVAL;
 
-	channel = find_first_bit(ring->scan_mask, indio_dev->masklength);
+	channel = find_first_bit(indio_dev->active_scan_mask,
+				 indio_dev->masklength);
 
-	d_size = ring->scan_count *
+	d_size = bitmap_weight(indio_dev->active_scan_mask,
+			       indio_dev->masklength) *
 		 indio_dev->channels[0].scan_type.storagebits / 8;
 
 	if (ring->scan_timestamp) {
@@ -544,7 +527,7 @@ static irqreturn_t ad7192_trigger_handler(int irq, void *p)
 	s64 dat64[2];
 	s32 *dat32 = (s32 *)dat64;
 
-	if (ring->scan_count)
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		__ad7192_read_reg(st, 1, 1, AD7192_REG_DATA,
 				  dat32,
 				  indio_dev->channels[0].scan_type.realbits/8);
@@ -592,7 +575,7 @@ static int ad7192_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	}
 
 	/* Ring buffer functions - here trigger setup related */
-	indio_dev->buffer->setup_ops = &ad7192_ring_setup_ops;
+	indio_dev->setup_ops = &ad7192_ring_setup_ops;
 
 	/* Flag that polled ring buffering is possible */
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
@@ -626,6 +609,10 @@ static irqreturn_t ad7192_data_rdy_trig_poll(int irq, void *private)
 	return IRQ_HANDLED;
 }
 
+static struct iio_trigger_ops ad7192_trigger_ops = {
+	.owner = THIS_MODULE,
+};
+
 static int ad7192_probe_trigger(struct iio_dev *indio_dev)
 {
 	struct ad7192_state *st = iio_priv(indio_dev);
@@ -638,7 +625,7 @@ static int ad7192_probe_trigger(struct iio_dev *indio_dev)
 		ret = -ENOMEM;
 		goto error_ret;
 	}
-
+	st->trig->ops = &ad7192_trigger_ops;
 	ret = request_irq(st->spi->irq,
 			  ad7192_data_rdy_trig_poll,
 			  IRQF_TRIGGER_LOW,
@@ -650,7 +637,6 @@ static int ad7192_probe_trigger(struct iio_dev *indio_dev)
 	disable_irq_nosync(st->spi->irq);
 	st->irq_dis = true;
 	st->trig->dev.parent = &st->spi->dev;
-	st->trig->owner = THIS_MODULE;
 	st->trig->private_data = indio_dev;
 
 	ret = iio_trigger_register(st->trig);
@@ -795,7 +781,7 @@ static ssize_t ad7192_set(struct device *dev,
 		return -EBUSY;
 	}
 
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case AD7192_REG_GPOCON:
 		if (val)
 			st->gpocon |= AD7192_GPOCON_BPDSW;
@@ -873,8 +859,7 @@ static int ad7192_read_raw(struct iio_dev *indio_dev,
 	case 0:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
-			ret = ad7192_scan_from_ring(st,
-					chan->scan_index, &smpl);
+			ret = -EBUSY;
 		else
 			ret = ad7192_read(st, chan->address,
 					chan->scan_type.realbits / 8, &smpl);
@@ -901,18 +886,20 @@ static int ad7192_read_raw(struct iio_dev *indio_dev,
 		}
 		return IIO_VAL_INT;
 
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
-		mutex_lock(&indio_dev->mlock);
-		*val = st->scale_avail[AD7192_CONF_GAIN(st->conf)][0];
-		*val2 = st->scale_avail[AD7192_CONF_GAIN(st->conf)][1];
-		mutex_unlock(&indio_dev->mlock);
-
-		return IIO_VAL_INT_PLUS_NANO;
-
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-		*val =  1000;
-
-		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_VOLTAGE:
+			mutex_lock(&indio_dev->mlock);
+			*val = st->scale_avail[AD7192_CONF_GAIN(st->conf)][0];
+			*val2 = st->scale_avail[AD7192_CONF_GAIN(st->conf)][1];
+			mutex_unlock(&indio_dev->mlock);
+			return IIO_VAL_INT_PLUS_NANO;
+		case IIO_TEMP:
+			*val =  1000;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
 	}
 
 	return -EINVAL;
@@ -935,7 +922,7 @@ static int ad7192_write_raw(struct iio_dev *indio_dev,
 	}
 
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		ret = -EINVAL;
 		for (i = 0; i < ARRAY_SIZE(st->scale_avail); i++)
 			if (val2 == st->scale_avail[i][1]) {
@@ -992,7 +979,7 @@ static const struct iio_info ad7192_info = {
 	  .extend_name = _name,						\
 	  .channel = _chan,						\
 	  .channel2 = _chan2,						\
-	  .info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),		\
+	  .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
 	  .address = _address,						\
 	  .scan_index = _si,						\
 	  .scan_type =  IIO_ST('s', 24, 32, 0)}
@@ -1001,7 +988,7 @@ static const struct iio_info ad7192_info = {
 	{ .type = IIO_VOLTAGE,						\
 	  .indexed = 1,							\
 	  .channel = _chan,						\
-	  .info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),		\
+	  .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
 	  .address = _address,						\
 	  .scan_index = _si,						\
 	  .scan_type =  IIO_ST('s', 24, 32, 0)}
@@ -1010,7 +997,7 @@ static const struct iio_info ad7192_info = {
 	{ .type = IIO_TEMP,						\
 	  .indexed = 1,							\
 	  .channel = _chan,						\
-	  .info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),		\
+	  .info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
 	  .address = _address,						\
 	  .scan_index = _si,						\
 	  .scan_type =  IIO_ST('s', 24, 32, 0)}
@@ -1151,6 +1138,7 @@ static const struct spi_device_id ad7192_id[] = {
 	{"ad7195", ID_AD7195},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad7192_id);
 
 static struct spi_driver ad7192_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index dbaeae81e873..7dbd6812c240 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -18,6 +18,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
+#include "../events.h"
 
 #include "ad7280a.h"
 
@@ -455,10 +456,10 @@ static ssize_t ad7280_store_balance_timer(struct device *dev,
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad7280_state *st = iio_priv(indio_dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	long val;
+	unsigned long val;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &val);
+	ret = kstrtoul(buf, 10, &val);
 	if (ret)
 		return ret;
 
@@ -487,8 +488,8 @@ static int ad7280_channel_init(struct ad7280_state *st)
 {
 	int dev, ch, cnt;
 
-	st->channels = kzalloc(sizeof(*st->channels) *
-				((st->slave_num + 1) * 12 + 2), GFP_KERNEL);
+	st->channels = kcalloc((st->slave_num + 1) * 12 + 2,
+			       sizeof(*st->channels), GFP_KERNEL);
 	if (st->channels == NULL)
 		return -ENOMEM;
 
@@ -507,7 +508,7 @@ static int ad7280_channel_init(struct ad7280_state *st)
 			}
 			st->channels[cnt].indexed = 1;
 			st->channels[cnt].info_mask =
-				(1 << IIO_CHAN_INFO_SCALE_SHARED);
+				IIO_CHAN_INFO_SCALE_SHARED_BIT;
 			st->channels[cnt].address =
 				AD7280A_DEVADDR(dev) << 8 | ch;
 			st->channels[cnt].scan_index = cnt;
@@ -523,7 +524,7 @@ static int ad7280_channel_init(struct ad7280_state *st)
 	st->channels[cnt].channel2 = dev * 6;
 	st->channels[cnt].address = AD7280A_ALL_CELLS;
 	st->channels[cnt].indexed = 1;
-	st->channels[cnt].info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED);
+	st->channels[cnt].info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT;
 	st->channels[cnt].scan_index = cnt;
 	st->channels[cnt].scan_type.sign = 'u';
 	st->channels[cnt].scan_type.realbits = 32;
@@ -600,7 +601,7 @@ static ssize_t ad7280_read_channel_config(struct device *dev,
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	unsigned val;
 
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case AD7280A_CELL_OVERVOLTAGE:
 		val = 1000 + (st->cell_threshhigh * 1568) / 100;
 		break;
@@ -636,7 +637,7 @@ static ssize_t ad7280_write_channel_config(struct device *dev,
 	if (ret)
 		return ret;
 
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case AD7280A_CELL_OVERVOLTAGE:
 	case AD7280A_CELL_UNDERVOLTAGE:
 		val = ((val - 1000) * 100) / 1568; /* LSB 15.68mV */
@@ -652,7 +653,7 @@ static ssize_t ad7280_write_channel_config(struct device *dev,
 	val = clamp(val, 0L, 0xFFL);
 
 	mutex_lock(&indio_dev->mlock);
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case AD7280A_CELL_OVERVOLTAGE:
 		st->cell_threshhigh = val;
 		break;
@@ -682,13 +683,13 @@ static irqreturn_t ad7280_event_handler(int irq, void *private)
 	unsigned *channels;
 	int i, ret;
 
-	channels = kzalloc(sizeof(*channels) * st->scan_cnt, GFP_KERNEL);
+	channels = kcalloc(st->scan_cnt, sizeof(*channels), GFP_KERNEL);
 	if (channels == NULL)
 		return IRQ_HANDLED;
 
 	ret = ad7280_read_all_channels(st, st->scan_cnt, channels);
 	if (ret < 0)
-		return IRQ_HANDLED;
+		goto out;
 
 	for (i = 0; i < st->scan_cnt; i++) {
 		if (((channels[i] >> 23) & 0xF) <= AD7280A_CELL_VOLTAGE_6) {
@@ -731,6 +732,7 @@ static irqreturn_t ad7280_event_handler(int irq, void *private)
 		}
 	}
 
+out:
 	kfree(channels);
 
 	return IRQ_HANDLED;
@@ -801,7 +803,7 @@ static int ad7280_read_raw(struct iio_dev *indio_dev,
 		*val = ret;
 
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		if ((chan->address & 0xFF) <= AD7280A_CELL_VOLTAGE_6)
 			scale_uv = (4000 * 1000) >> AD7280A_BITS;
 		else
@@ -968,11 +970,11 @@ static const struct spi_device_id ad7280_id[] = {
 	{"ad7280a", 0},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad7280_id);
 
 static struct spi_driver ad7280_driver = {
 	.driver = {
 		.name	= "ad7280",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad7280_probe,
diff --git a/drivers/staging/iio/adc/ad7291.c b/drivers/staging/iio/adc/ad7291.c
index aa44a52a7adb..0a13616e3db9 100644
--- a/drivers/staging/iio/adc/ad7291.c
+++ b/drivers/staging/iio/adc/ad7291.c
@@ -19,6 +19,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
+#include "../events.h"
 
 /*
  * Simplified handling
@@ -500,7 +501,7 @@ static int ad7291_read_raw(struct iio_dev *indio_dev,
 		default:
 			return -EINVAL;
 		}
-	case (1 << IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE):
+	case IIO_CHAN_INFO_AVERAGE_RAW:
 		ret = i2c_smbus_read_word_data(chip->client,
 					       AD7291_T_AVERAGE);
 			if (ret < 0)
@@ -509,18 +510,24 @@ static int ad7291_read_raw(struct iio_dev *indio_dev,
 				AD7291_VALUE_MASK) << 4) >> 4;
 			*val = signval;
 			return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
-		scale_uv = (chip->int_vref_mv * 1000) >> AD7291_BITS;
-		*val =  scale_uv / 1000;
-		*val2 = (scale_uv % 1000) * 1000;
-		return IIO_VAL_INT_PLUS_MICRO;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-		/*
-		* One LSB of the ADC corresponds to 0.25 deg C.
-		* The temperature reading is in 12-bit twos complement format
-		*/
-		*val = 250;
-		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_VOLTAGE:
+			scale_uv = (chip->int_vref_mv * 1000) >> AD7291_BITS;
+			*val =  scale_uv / 1000;
+			*val2 = (scale_uv % 1000) * 1000;
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_TEMP:
+			/*
+			 * One LSB of the ADC corresponds to 0.25 deg C.
+			 * The temperature reading is in 12-bit twos
+			 * complement format
+			 */
+			*val = 250;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
 	default:
 		return -EINVAL;
 	}
@@ -529,7 +536,7 @@ static int ad7291_read_raw(struct iio_dev *indio_dev,
 #define AD7291_VOLTAGE_CHAN(_chan)					\
 {									\
 	.type = IIO_VOLTAGE,						\
-	.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),			\
+	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,			\
 	.indexed = 1,							\
 	.channel = _chan,						\
 	.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING)|\
@@ -547,8 +554,8 @@ static const struct iio_chan_spec ad7291_channels[] = {
 	AD7291_VOLTAGE_CHAN(7),
 	{
 		.type = IIO_TEMP,
-		.info_mask = (1 << IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE) |
-				(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT |
+				IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.indexed = 1,
 		.channel = 0,
 		.event_mask =
diff --git a/drivers/staging/iio/adc/ad7298.h b/drivers/staging/iio/adc/ad7298.h
index 9ddf5cf0e519..a0e5dea415ef 100644
--- a/drivers/staging/iio/adc/ad7298.h
+++ b/drivers/staging/iio/adc/ad7298.h
@@ -54,14 +54,9 @@ struct ad7298_state {
 };
 
 #ifdef CONFIG_IIO_BUFFER
-int ad7298_scan_from_ring(struct iio_dev *indio_dev, long ch);
 int ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev);
 void ad7298_ring_cleanup(struct iio_dev *indio_dev);
 #else /* CONFIG_IIO_BUFFER */
-static inline int ad7298_scan_from_ring(struct iio_dev *indio_dev, long ch)
-{
-	return 0;
-}
 
 static inline int
 ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/adc/ad7298_core.c b/drivers/staging/iio/adc/ad7298_core.c
index a799bd1922dc..8dd6aa9cf928 100644
--- a/drivers/staging/iio/adc/ad7298_core.c
+++ b/drivers/staging/iio/adc/ad7298_core.c
@@ -18,37 +18,37 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "ad7298.h"
 
 static struct iio_chan_spec ad7298_channels[] = {
 	IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 9, AD7298_CH_TEMP, IIO_ST('s', 32, 32, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 0, 0, IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 1, 1, IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 2, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 2, 2, IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 3, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 3, 3, IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 4, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 4, 4, IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 5, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 5, 5, IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 6, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 6, 6, IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 7, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		 IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		 7, 7, IIO_ST('u', 12, 16, 0), 0),
 	IIO_CHAN_SOFT_TIMESTAMP(8),
 };
@@ -69,27 +69,28 @@ static int ad7298_scan_direct(struct ad7298_state *st, unsigned ch)
 static int ad7298_scan_temp(struct ad7298_state *st, int *val)
 {
 	int tmp, ret;
+	__be16 buf;
 
-	tmp = cpu_to_be16(AD7298_WRITE | AD7298_TSENSE |
+	buf = cpu_to_be16(AD7298_WRITE | AD7298_TSENSE |
 			  AD7298_TAVG | st->ext_ref);
 
-	ret = spi_write(st->spi, (u8 *)&tmp, 2);
+	ret = spi_write(st->spi, (u8 *)&buf, 2);
 	if (ret)
 		return ret;
 
-	tmp = 0;
+	buf = cpu_to_be16(0);
 
-	ret = spi_write(st->spi, (u8 *)&tmp, 2);
+	ret = spi_write(st->spi, (u8 *)&buf, 2);
 	if (ret)
 		return ret;
 
 	usleep_range(101, 1000); /* sleep > 100us */
 
-	ret = spi_read(st->spi, (u8 *)&tmp, 2);
+	ret = spi_read(st->spi, (u8 *)&buf, 2);
 	if (ret)
 		return ret;
 
-	tmp = be16_to_cpu(tmp) & RES_MASK(AD7298_BITS);
+	tmp = be16_to_cpu(buf) & RES_MASK(AD7298_BITS);
 
 	/*
 	 * One LSB of the ADC corresponds to 0.25 deg C.
@@ -122,12 +123,8 @@ static int ad7298_read_raw(struct iio_dev *indio_dev,
 	switch (m) {
 	case 0:
 		mutex_lock(&indio_dev->mlock);
-		if (iio_buffer_enabled(indio_dev)) {
-			if (chan->address == AD7298_CH_TEMP)
-				ret = -ENODEV;
-			else
-				ret = ad7298_scan_from_ring(indio_dev,
-							    chan->address);
+		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
+			ret = -EBUSY;
 		} else {
 			if (chan->address == AD7298_CH_TEMP)
 				ret = ad7298_scan_temp(st, val);
@@ -143,15 +140,20 @@ static int ad7298_read_raw(struct iio_dev *indio_dev,
 			*val = ret & RES_MASK(AD7298_BITS);
 
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
-		scale_uv = (st->int_vref_mv * 1000) >> AD7298_BITS;
-		*val =  scale_uv / 1000;
-		*val2 = (scale_uv % 1000) * 1000;
-		return IIO_VAL_INT_PLUS_MICRO;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-		*val =  1;
-		*val2 = 0;
-		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_VOLTAGE:
+			scale_uv = (st->int_vref_mv * 1000) >> AD7298_BITS;
+			*val =  scale_uv / 1000;
+			*val2 = (scale_uv % 1000) * 1000;
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_TEMP:
+			*val =  1;
+			*val2 = 0;
+			return IIO_VAL_INT_PLUS_MICRO;
+		default:
+			return -EINVAL;
+		}
 	}
 	return -EINVAL;
 }
@@ -265,11 +267,11 @@ static const struct spi_device_id ad7298_id[] = {
 	{"ad7298", 0},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad7298_id);
 
 static struct spi_driver ad7298_driver = {
 	.driver = {
 		.name	= "ad7298",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad7298_probe,
@@ -281,4 +283,3 @@ module_spi_driver(ad7298_driver);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD7298 ADC");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:ad7298");
diff --git a/drivers/staging/iio/adc/ad7298_ring.c b/drivers/staging/iio/adc/ad7298_ring.c
index 47630d506a63..d1a12dd015e2 100644
--- a/drivers/staging/iio/adc/ad7298_ring.c
+++ b/drivers/staging/iio/adc/ad7298_ring.c
@@ -12,41 +12,12 @@
 #include <linux/spi/spi.h>
 
 #include "../iio.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 #include "../trigger_consumer.h"
 
 #include "ad7298.h"
 
-int ad7298_scan_from_ring(struct iio_dev *indio_dev, long ch)
-{
-	struct iio_buffer *ring = indio_dev->buffer;
-	int ret;
-	u16 *ring_data;
-
-	if (!(test_bit(ch, ring->scan_mask))) {
-		ret = -EBUSY;
-		goto error_ret;
-	}
-
-	ring_data = kmalloc(ring->access->get_bytes_per_datum(ring),
-			    GFP_KERNEL);
-	if (ring_data == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = ring->access->read_last(ring, (u8 *) ring_data);
-	if (ret)
-		goto error_free_ring_data;
-
-	ret = be16_to_cpu(ring_data[ch]);
-
-error_free_ring_data:
-	kfree(ring_data);
-error_ret:
-	return ret;
-}
-
 /**
  * ad7298_ring_preenable() setup the parameters of the ring before enabling
  *
@@ -61,8 +32,9 @@ static int ad7298_ring_preenable(struct iio_dev *indio_dev)
 	size_t d_size;
 	int i, m;
 	unsigned short command;
-
-	d_size = ring->scan_count * (AD7298_STORAGE_BITS / 8);
+	int scan_count = bitmap_weight(indio_dev->active_scan_mask,
+				       indio_dev->masklength);
+	d_size = scan_count * (AD7298_STORAGE_BITS / 8);
 
 	if (ring->scan_timestamp) {
 		d_size += sizeof(s64);
@@ -79,7 +51,7 @@ static int ad7298_ring_preenable(struct iio_dev *indio_dev)
 	command = AD7298_WRITE | st->ext_ref;
 
 	for (i = 0, m = AD7298_CH(0); i < AD7298_MAX_CHAN; i++, m >>= 1)
-		if (test_bit(i, ring->scan_mask))
+		if (test_bit(i, indio_dev->active_scan_mask))
 			command |= m;
 
 	st->tx_buf[0] = cpu_to_be16(command);
@@ -96,7 +68,7 @@ static int ad7298_ring_preenable(struct iio_dev *indio_dev)
 	spi_message_add_tail(&st->ring_xfer[0], &st->ring_msg);
 	spi_message_add_tail(&st->ring_xfer[1], &st->ring_msg);
 
-	for (i = 0; i < ring->scan_count; i++) {
+	for (i = 0; i < scan_count; i++) {
 		st->ring_xfer[i + 2].rx_buf = &st->rx_buf[i];
 		st->ring_xfer[i + 2].len = 2;
 		st->ring_xfer[i + 2].cs_change = 1;
@@ -134,7 +106,8 @@ static irqreturn_t ad7298_trigger_handler(int irq, void *p)
 			&time_ns, sizeof(time_ns));
 	}
 
-	for (i = 0; i < ring->scan_count; i++)
+	for (i = 0; i < bitmap_weight(indio_dev->active_scan_mask,
+						 indio_dev->masklength); i++)
 		buf[i] = be16_to_cpu(st->rx_buf[i]);
 
 	indio_dev->buffer->access->store_to(ring, (u8 *)buf, time_ns);
@@ -174,7 +147,7 @@ int ad7298_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	}
 
 	/* Ring buffer functions - here trigger setup related */
-	indio_dev->buffer->setup_ops = &ad7298_ring_setup_ops;
+	indio_dev->setup_ops = &ad7298_ring_setup_ops;
 	indio_dev->buffer->scan_timestamp = true;
 
 	/* Flag that polled ring buffering is possible */
diff --git a/drivers/staging/iio/adc/ad7476.h b/drivers/staging/iio/adc/ad7476.h
index 60142926565f..27f696c75cc4 100644
--- a/drivers/staging/iio/adc/ad7476.h
+++ b/drivers/staging/iio/adc/ad7476.h
@@ -50,14 +50,9 @@ enum ad7476_supported_device_ids {
 };
 
 #ifdef CONFIG_IIO_BUFFER
-int ad7476_scan_from_ring(struct iio_dev *indio_dev);
 int ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev);
 void ad7476_ring_cleanup(struct iio_dev *indio_dev);
 #else /* CONFIG_IIO_BUFFER */
-static inline int ad7476_scan_from_ring(struct iio_dev *indio_dev)
-{
-	return 0;
-}
 
 static inline int
 ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/adc/ad7476_core.c b/drivers/staging/iio/adc/ad7476_core.c
index 0b5852030ab6..0c064d1c3927 100644
--- a/drivers/staging/iio/adc/ad7476_core.c
+++ b/drivers/staging/iio/adc/ad7476_core.c
@@ -17,7 +17,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "ad7476.h"
 
@@ -46,7 +46,7 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 	case 0:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
-			ret = ad7476_scan_from_ring(indio_dev);
+			ret = -EBUSY;
 		else
 			ret = ad7476_scan_direct(st);
 		mutex_unlock(&indio_dev->mlock);
@@ -56,7 +56,7 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 		*val = (ret >> st->chip_info->channel[0].scan_type.shift) &
 			RES_MASK(st->chip_info->channel[0].scan_type.realbits);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		scale_uv = (st->int_vref_mv * 1000)
 			>> st->chip_info->channel[0].scan_type.realbits;
 		*val =  scale_uv/1000;
@@ -69,49 +69,49 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 static const struct ad7476_chip_info ad7476_chip_info_tbl[] = {
 	[ID_AD7466] = {
 		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				       0, 0, IIO_ST('u', 12, 16, 0), 0),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7467] = {
 		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				       0, 0, IIO_ST('u', 10, 16, 2), 0),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7468] = {
 		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1 , 0, NULL, 0, 0,
-				       (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				       0, 0, IIO_ST('u', 8, 16, 4), 0),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7475] = {
 		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				       0, 0, IIO_ST('u', 12, 16, 0), 0),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7476] = {
 		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				       0, 0, IIO_ST('u', 12, 16, 0), 0),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7477] = {
 		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				       0, 0, IIO_ST('u', 10, 16, 2), 0),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7478] = {
 		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				       0, 0, IIO_ST('u', 8, 16, 4), 0),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 	},
 	[ID_AD7495] = {
 		.channel[0] = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				       (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				       IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				       0, 0, IIO_ST('u', 12, 16, 0), 0),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 		.int_vref_mv = 2500,
@@ -237,11 +237,11 @@ static const struct spi_device_id ad7476_id[] = {
 	{"ad7495", ID_AD7495},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad7476_id);
 
 static struct spi_driver ad7476_driver = {
 	.driver = {
 		.name	= "ad7476",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad7476_probe,
@@ -253,4 +253,3 @@ module_spi_driver(ad7476_driver);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD7475/6/7/8(A) AD7466/7/8 ADC");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:ad7476");
diff --git a/drivers/staging/iio/adc/ad7476_ring.c b/drivers/staging/iio/adc/ad7476_ring.c
index e82c1a433f4f..4e298b2a05b2 100644
--- a/drivers/staging/iio/adc/ad7476_ring.c
+++ b/drivers/staging/iio/adc/ad7476_ring.c
@@ -14,36 +14,12 @@
 #include <linux/spi/spi.h>
 
 #include "../iio.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 #include "../trigger_consumer.h"
 
 #include "ad7476.h"
 
-int ad7476_scan_from_ring(struct iio_dev *indio_dev)
-{
-	struct iio_buffer *ring = indio_dev->buffer;
-	int ret;
-	u8 *ring_data;
-
-	ring_data = kmalloc(ring->access->get_bytes_per_datum(ring),
-			    GFP_KERNEL);
-	if (ring_data == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = ring->access->read_last(ring, ring_data);
-	if (ret)
-		goto error_free_ring_data;
-
-	ret = (ring_data[0] << 8) | ring_data[1];
-
-error_free_ring_data:
-	kfree(ring_data);
-error_ret:
-	return ret;
-}
-
 /**
  * ad7476_ring_preenable() setup the parameters of the ring before enabling
  *
@@ -56,7 +32,8 @@ static int ad7476_ring_preenable(struct iio_dev *indio_dev)
 	struct ad7476_state *st = iio_priv(indio_dev);
 	struct iio_buffer *ring = indio_dev->buffer;
 
-	st->d_size = ring->scan_count *
+	st->d_size = bitmap_weight(indio_dev->active_scan_mask,
+				   indio_dev->masklength) *
 		st->chip_info->channel[0].scan_type.storagebits / 8;
 
 	if (ring->scan_timestamp) {
@@ -137,7 +114,7 @@ int ad7476_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	}
 
 	/* Ring buffer functions - here trigger setup related */
-	indio_dev->buffer->setup_ops = &ad7476_ring_setup_ops;
+	indio_dev->setup_ops = &ad7476_ring_setup_ops;
 	indio_dev->buffer->scan_timestamp = true;
 
 	/* Flag that polled ring buffering is possible */
diff --git a/drivers/staging/iio/adc/ad7606.h b/drivers/staging/iio/adc/ad7606.h
index 35018c3f518e..10f59896597f 100644
--- a/drivers/staging/iio/adc/ad7606.h
+++ b/drivers/staging/iio/adc/ad7606.h
@@ -99,7 +99,6 @@ enum ad7606_supported_device_ids {
 	ID_AD7606_4
 };
 
-int ad7606_scan_from_ring(struct iio_dev *indio_dev, unsigned ch);
 int ad7606_register_ring_funcs_and_init(struct iio_dev *indio_dev);
 void ad7606_ring_cleanup(struct iio_dev *indio_dev);
 #endif /* IIO_ADC_AD7606_H_ */
diff --git a/drivers/staging/iio/adc/ad7606_core.c b/drivers/staging/iio/adc/ad7606_core.c
index e3ecd3d2ef3a..ddb7ef92f5c1 100644
--- a/drivers/staging/iio/adc/ad7606_core.c
+++ b/drivers/staging/iio/adc/ad7606_core.c
@@ -20,7 +20,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "ad7606.h"
 
@@ -91,7 +91,7 @@ static int ad7606_read_raw(struct iio_dev *indio_dev,
 	case 0:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
-			ret = ad7606_scan_from_ring(indio_dev, chan->address);
+			ret = -EBUSY;
 		else
 			ret = ad7606_scan_direct(indio_dev, chan->address);
 		mutex_unlock(&indio_dev->mlock);
@@ -100,7 +100,7 @@ static int ad7606_read_raw(struct iio_dev *indio_dev,
 			return ret;
 		*val = (short) ret;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		scale_uv = (st->range * 1000 * 2)
 			>> st->chip_info->channels[0].scan_type.realbits;
 		*val =  scale_uv / 1000;
diff --git a/drivers/staging/iio/adc/ad7606_par.c b/drivers/staging/iio/adc/ad7606_par.c
index 688632edd3d7..cff97568189e 100644
--- a/drivers/staging/iio/adc/ad7606_par.c
+++ b/drivers/staging/iio/adc/ad7606_par.c
@@ -189,4 +189,3 @@ module_exit(ad7606_cleanup);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD7606 ADC");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:ad7606_par");
diff --git a/drivers/staging/iio/adc/ad7606_ring.c b/drivers/staging/iio/adc/ad7606_ring.c
index 20927fd53728..e8f94a18a943 100644
--- a/drivers/staging/iio/adc/ad7606_ring.c
+++ b/drivers/staging/iio/adc/ad7606_ring.c
@@ -12,36 +12,12 @@
 #include <linux/slab.h>
 
 #include "../iio.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 #include "../trigger_consumer.h"
 
 #include "ad7606.h"
 
-int ad7606_scan_from_ring(struct iio_dev *indio_dev, unsigned ch)
-{
-	struct iio_buffer *ring = indio_dev->buffer;
-	int ret;
-	u16 *ring_data;
-
-	ring_data = kmalloc(ring->access->get_bytes_per_datum(ring),
-			    GFP_KERNEL);
-	if (ring_data == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = ring->access->read_last(ring, (u8 *) ring_data);
-	if (ret)
-		goto error_free_ring_data;
-
-	ret = ring_data[ch];
-
-error_free_ring_data:
-	kfree(ring_data);
-error_ret:
-	return ret;
-}
-
 /**
  * ad7606_trigger_handler_th() th/bh of trigger launched polling to ring buffer
  *
@@ -136,8 +112,6 @@ int ad7606_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 
 	/* Effectively select the ring buffer implementation */
 	indio_dev->buffer->access = &ring_sw_access_funcs;
-	indio_dev->buffer->bpe =
-		st->chip_info->channels[0].scan_type.storagebits / 8;
 	indio_dev->pollfunc = iio_alloc_pollfunc(&ad7606_trigger_handler_th_bh,
 						 &ad7606_trigger_handler_th_bh,
 						 0,
@@ -152,7 +126,7 @@ int ad7606_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 
 	/* Ring buffer functions - here trigger setup related */
 
-	indio_dev->buffer->setup_ops = &ad7606_ring_setup_ops;
+	indio_dev->setup_ops = &ad7606_ring_setup_ops;
 	indio_dev->buffer->scan_timestamp = true ;
 
 	INIT_WORK(&st->poll_work, &ad7606_poll_bh_to_ring);
diff --git a/drivers/staging/iio/adc/ad7606_spi.c b/drivers/staging/iio/adc/ad7606_spi.c
index b984bd2048b6..237f1c44d296 100644
--- a/drivers/staging/iio/adc/ad7606_spi.c
+++ b/drivers/staging/iio/adc/ad7606_spi.c
@@ -97,11 +97,11 @@ static const struct spi_device_id ad7606_id[] = {
 	{"ad7606-4", ID_AD7606_4},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad7606_id);
 
 static struct spi_driver ad7606_driver = {
 	.driver = {
 		.name = "ad7606",
-		.bus = &spi_bus_type,
 		.owner = THIS_MODULE,
 		.pm    = AD7606_SPI_PM_OPS,
 	},
@@ -114,4 +114,3 @@ module_spi_driver(ad7606_driver);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD7606 ADC");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:ad7606_spi");
diff --git a/drivers/staging/iio/adc/ad7780.c b/drivers/staging/iio/adc/ad7780.c
index ec90261cbc3c..a13e58c814e6 100644
--- a/drivers/staging/iio/adc/ad7780.c
+++ b/drivers/staging/iio/adc/ad7780.c
@@ -114,7 +114,7 @@ static int ad7780_read_raw(struct iio_dev *indio_dev,
 			*val *= 128;
 
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		scale_uv = (st->int_vref_mv * 100000)
 			>> (channel.scan_type.realbits - 1);
 		*val =  scale_uv / 100000;
@@ -127,12 +127,12 @@ static int ad7780_read_raw(struct iio_dev *indio_dev,
 static const struct ad7780_chip_info ad7780_chip_info_tbl[] = {
 	[ID_AD7780] = {
 		.channel = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				    (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				    IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				    0, 0, IIO_ST('s', 24, 32, 8), 0),
 	},
 	[ID_AD7781] = {
 		.channel = IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 0, 0,
-				    (1 << IIO_CHAN_INFO_SCALE_SHARED),
+				    IIO_CHAN_INFO_SCALE_SHARED_BIT,
 				    0, 0, IIO_ST('s', 20, 32, 12), 0),
 	},
 };
@@ -272,11 +272,11 @@ static const struct spi_device_id ad7780_id[] = {
 	{"ad7781", ID_AD7781},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad7780_id);
 
 static struct spi_driver ad7780_driver = {
 	.driver = {
 		.name	= "ad7780",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad7780_probe,
diff --git a/drivers/staging/iio/adc/ad7793.c b/drivers/staging/iio/adc/ad7793.c
index 1c5588e88cdf..6a058b19c49a 100644
--- a/drivers/staging/iio/adc/ad7793.c
+++ b/drivers/staging/iio/adc/ad7793.c
@@ -20,7 +20,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 #include "../trigger.h"
 #include "../trigger_consumer.h"
@@ -316,25 +316,6 @@ out:
 	return ret;
 }
 
-static int ad7793_scan_from_ring(struct ad7793_state *st, unsigned ch, int *val)
-{
-	struct iio_buffer *ring = iio_priv_to_dev(st)->buffer;
-	int ret;
-	s64 dat64[2];
-	u32 *dat32 = (u32 *)dat64;
-
-	if (!(test_bit(ch, ring->scan_mask)))
-		return  -EBUSY;
-
-	ret = ring->access->read_last(ring, (u8 *) &dat64);
-	if (ret)
-		return ret;
-
-	*val = *dat32;
-
-	return 0;
-}
-
 static int ad7793_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ad7793_state *st = iio_priv(indio_dev);
@@ -342,14 +323,15 @@ static int ad7793_ring_preenable(struct iio_dev *indio_dev)
 	size_t d_size;
 	unsigned channel;
 
-	if (!ring->scan_count)
+	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		return -EINVAL;
 
-	channel = find_first_bit(ring->scan_mask,
+	channel = find_first_bit(indio_dev->active_scan_mask,
 				 indio_dev->masklength);
 
-	d_size = ring->scan_count *
-		 indio_dev->channels[0].scan_type.storagebits / 8;
+	d_size = bitmap_weight(indio_dev->active_scan_mask,
+			       indio_dev->masklength) *
+		indio_dev->channels[0].scan_type.storagebits / 8;
 
 	if (ring->scan_timestamp) {
 		d_size += sizeof(s64);
@@ -411,7 +393,7 @@ static irqreturn_t ad7793_trigger_handler(int irq, void *p)
 	s64 dat64[2];
 	s32 *dat32 = (s32 *)dat64;
 
-	if (ring->scan_count)
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		__ad7793_read_reg(st, 1, 1, AD7793_REG_DATA,
 				  dat32,
 				  indio_dev->channels[0].scan_type.realbits/8);
@@ -459,7 +441,7 @@ static int ad7793_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	}
 
 	/* Ring buffer functions - here trigger setup related */
-	indio_dev->buffer->setup_ops = &ad7793_ring_setup_ops;
+	indio_dev->setup_ops = &ad7793_ring_setup_ops;
 
 	/* Flag that polled ring buffering is possible */
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
@@ -493,6 +475,10 @@ static irqreturn_t ad7793_data_rdy_trig_poll(int irq, void *private)
 	return IRQ_HANDLED;
 }
 
+static struct iio_trigger_ops ad7793_trigger_ops = {
+	.owner = THIS_MODULE,
+};
+
 static int ad7793_probe_trigger(struct iio_dev *indio_dev)
 {
 	struct ad7793_state *st = iio_priv(indio_dev);
@@ -505,6 +491,7 @@ static int ad7793_probe_trigger(struct iio_dev *indio_dev)
 		ret = -ENOMEM;
 		goto error_ret;
 	}
+	st->trig->ops = &ad7793_trigger_ops;
 
 	ret = request_irq(st->spi->irq,
 			  ad7793_data_rdy_trig_poll,
@@ -517,7 +504,6 @@ static int ad7793_probe_trigger(struct iio_dev *indio_dev)
 	disable_irq_nosync(st->spi->irq);
 	st->irq_dis = true;
 	st->trig->dev.parent = &st->spi->dev;
-	st->trig->owner = THIS_MODULE;
 	st->trig->private_data = indio_dev;
 
 	ret = iio_trigger_register(st->trig);
@@ -649,8 +635,7 @@ static int ad7793_read_raw(struct iio_dev *indio_dev,
 	case 0:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
-			ret = ad7793_scan_from_ring(st,
-					chan->scan_index, &smpl);
+			ret = -EBUSY;
 		else
 			ret = ad7793_read(st, chan->address,
 					chan->scan_type.realbits / 8, &smpl);
@@ -667,19 +652,21 @@ static int ad7793_read_raw(struct iio_dev *indio_dev,
 
 		return IIO_VAL_INT;
 
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
-		*val = st->scale_avail[(st->conf >> 8) & 0x7][0];
-		*val2 = st->scale_avail[(st->conf >> 8) & 0x7][1];
-
-		return IIO_VAL_INT_PLUS_NANO;
-
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_VOLTAGE:
-			/* 1170mV / 2^23 * 6 */
-			scale_uv = (1170ULL * 100000000ULL * 6ULL)
-				>> (chan->scan_type.realbits -
-				(unipolar ? 0 : 1));
+			if (chan->differential) {
+				*val = st->
+					scale_avail[(st->conf >> 8) & 0x7][0];
+				*val2 = st->
+					scale_avail[(st->conf >> 8) & 0x7][1];
+				return IIO_VAL_INT_PLUS_NANO;
+			} else {
+				/* 1170mV / 2^23 * 6 */
+				scale_uv = (1170ULL * 100000000ULL * 6ULL)
+					>> (chan->scan_type.realbits -
+					    (unipolar ? 0 : 1));
+			}
 			break;
 		case IIO_TEMP:
 			/* Always uses unity gain and internal ref */
@@ -716,7 +703,7 @@ static int ad7793_write_raw(struct iio_dev *indio_dev,
 	}
 
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		ret = -EINVAL;
 		for (i = 0; i < ARRAY_SIZE(st->scale_avail); i++)
 			if (val2 == st->scale_avail[i][1]) {
@@ -775,7 +762,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 0,
 			.channel2 = 0,
 			.address = AD7793_CH_AIN1P_AIN1M,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 0,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
@@ -786,7 +773,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 1,
 			.channel2 = 1,
 			.address = AD7793_CH_AIN2P_AIN2M,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 1,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
@@ -797,7 +784,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 2,
 			.channel2 = 2,
 			.address = AD7793_CH_AIN3P_AIN3M,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 2,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
@@ -809,7 +796,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 2,
 			.channel2 = 2,
 			.address = AD7793_CH_AIN1M_AIN1M,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 2,
 			.scan_type = IIO_ST('s', 24, 32, 0)
 		},
@@ -818,7 +805,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.indexed = 1,
 			.channel = 0,
 			.address = AD7793_CH_TEMP,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 			.scan_index = 4,
 			.scan_type = IIO_ST('s', 24, 32, 0),
 		},
@@ -828,7 +815,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.indexed = 1,
 			.channel = 4,
 			.address = AD7793_CH_AVDD_MONITOR,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 			.scan_index = 5,
 			.scan_type = IIO_ST('s', 24, 32, 0),
 		},
@@ -842,7 +829,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 0,
 			.channel2 = 0,
 			.address = AD7793_CH_AIN1P_AIN1M,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 0,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
@@ -853,7 +840,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 1,
 			.channel2 = 1,
 			.address = AD7793_CH_AIN2P_AIN2M,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 1,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
@@ -864,7 +851,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 2,
 			.channel2 = 2,
 			.address = AD7793_CH_AIN3P_AIN3M,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 2,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
@@ -876,7 +863,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.channel = 2,
 			.channel2 = 2,
 			.address = AD7793_CH_AIN1M_AIN1M,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.scan_index = 2,
 			.scan_type = IIO_ST('s', 16, 32, 0)
 		},
@@ -885,7 +872,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.indexed = 1,
 			.channel = 0,
 			.address = AD7793_CH_TEMP,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 			.scan_index = 4,
 			.scan_type = IIO_ST('s', 16, 32, 0),
 		},
@@ -895,7 +882,7 @@ static const struct ad7793_chip_info ad7793_chip_info_tbl[] = {
 			.indexed = 1,
 			.channel = 4,
 			.address = AD7793_CH_AVDD_MONITOR,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 			.scan_index = 5,
 			.scan_type = IIO_ST('s', 16, 32, 0),
 		},
@@ -1034,11 +1021,11 @@ static const struct spi_device_id ad7793_id[] = {
 	{"ad7793", ID_AD7793},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad7793_id);
 
 static struct spi_driver ad7793_driver = {
 	.driver = {
 		.name	= "ad7793",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad7793_probe,
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
index acbf9363132b..52b720e2b03a 100644
--- a/drivers/staging/iio/adc/ad7816.c
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -18,6 +18,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
+#include "../events.h"
 
 /*
  * AD7816 config masks
@@ -459,7 +460,6 @@ MODULE_DEVICE_TABLE(spi, ad7816_id);
 static struct spi_driver ad7816_driver = {
 	.driver = {
 		.name = "ad7816",
-		.bus = &spi_bus_type,
 		.owner = THIS_MODULE,
 	},
 	.probe = ad7816_probe,
diff --git a/drivers/staging/iio/adc/ad7887.h b/drivers/staging/iio/adc/ad7887.h
index 3452d1819077..bc53b6532121 100644
--- a/drivers/staging/iio/adc/ad7887.h
+++ b/drivers/staging/iio/adc/ad7887.h
@@ -83,14 +83,9 @@ enum ad7887_supported_device_ids {
 };
 
 #ifdef CONFIG_IIO_BUFFER
-int ad7887_scan_from_ring(struct ad7887_state *st, int channum);
 int ad7887_register_ring_funcs_and_init(struct iio_dev *indio_dev);
 void ad7887_ring_cleanup(struct iio_dev *indio_dev);
 #else /* CONFIG_IIO_BUFFER */
-static inline int ad7887_scan_from_ring(struct ad7887_state *st, int channum)
-{
-	return 0;
-}
 
 static inline int
 ad7887_register_ring_funcs_and_init(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/adc/ad7887_core.c b/drivers/staging/iio/adc/ad7887_core.c
index 91b8fb09d92b..e9bbc3eed15d 100644
--- a/drivers/staging/iio/adc/ad7887_core.c
+++ b/drivers/staging/iio/adc/ad7887_core.c
@@ -17,7 +17,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 
 #include "ad7887.h"
@@ -45,7 +45,7 @@ static int ad7887_read_raw(struct iio_dev *indio_dev,
 	case 0:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
-			ret = ad7887_scan_from_ring(st, 1 << chan->address);
+			ret = -EBUSY;
 		else
 			ret = ad7887_scan_direct(st, chan->address);
 		mutex_unlock(&indio_dev->mlock);
@@ -55,7 +55,7 @@ static int ad7887_read_raw(struct iio_dev *indio_dev,
 		*val = (ret >> st->chip_info->channel[0].scan_type.shift) &
 			RES_MASK(st->chip_info->channel[0].scan_type.realbits);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		scale_uv = (st->int_vref_mv * 1000)
 			>> st->chip_info->channel[0].scan_type.realbits;
 		*val =  scale_uv/1000;
@@ -75,7 +75,7 @@ static const struct ad7887_chip_info ad7887_chip_info_tbl[] = {
 			.type = IIO_VOLTAGE,
 			.indexed = 1,
 			.channel = 1,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.address = 1,
 			.scan_index = 1,
 			.scan_type = IIO_ST('u', 12, 16, 0),
@@ -84,7 +84,7 @@ static const struct ad7887_chip_info ad7887_chip_info_tbl[] = {
 			.type = IIO_VOLTAGE,
 			.indexed = 1,
 			.channel = 0,
-			.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+			.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 			.address = 0,
 			.scan_index = 0,
 			.scan_type = IIO_ST('u', 12, 16, 0),
@@ -246,11 +246,11 @@ static const struct spi_device_id ad7887_id[] = {
 	{"ad7887", ID_AD7887},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad7887_id);
 
 static struct spi_driver ad7887_driver = {
 	.driver = {
 		.name	= "ad7887",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad7887_probe,
@@ -262,4 +262,3 @@ module_spi_driver(ad7887_driver);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD7887 ADC");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:ad7887");
diff --git a/drivers/staging/iio/adc/ad7887_ring.c b/drivers/staging/iio/adc/ad7887_ring.c
index cb74cada5611..85076cd962e7 100644
--- a/drivers/staging/iio/adc/ad7887_ring.c
+++ b/drivers/staging/iio/adc/ad7887_ring.c
@@ -13,46 +13,12 @@
 #include <linux/spi/spi.h>
 
 #include "../iio.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 #include "../trigger_consumer.h"
 
 #include "ad7887.h"
 
-int ad7887_scan_from_ring(struct ad7887_state *st, int channum)
-{
-	struct iio_buffer *ring = iio_priv_to_dev(st)->buffer;
-	int count = 0, ret;
-	u16 *ring_data;
-
-	if (!(test_bit(channum, ring->scan_mask))) {
-		ret = -EBUSY;
-		goto error_ret;
-	}
-
-	ring_data = kmalloc(ring->access->get_bytes_per_datum(ring),
-			    GFP_KERNEL);
-	if (ring_data == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = ring->access->read_last(ring, (u8 *) ring_data);
-	if (ret)
-		goto error_free_ring_data;
-
-	/* for single channel scan the result is stored with zero offset */
-	if ((test_bit(1, ring->scan_mask) || test_bit(0, ring->scan_mask)) &&
-	    (channum == 1))
-		count = 1;
-
-	ret = be16_to_cpu(ring_data[count]);
-
-error_free_ring_data:
-	kfree(ring_data);
-error_ret:
-	return ret;
-}
-
 /**
  * ad7887_ring_preenable() setup the parameters of the ring before enabling
  *
@@ -65,7 +31,8 @@ static int ad7887_ring_preenable(struct iio_dev *indio_dev)
 	struct ad7887_state *st = iio_priv(indio_dev);
 	struct iio_buffer *ring = indio_dev->buffer;
 
-	st->d_size = ring->scan_count *
+	st->d_size = bitmap_weight(indio_dev->active_scan_mask,
+				   indio_dev->masklength) *
 		st->chip_info->channel[0].scan_type.storagebits / 8;
 
 	if (ring->scan_timestamp) {
@@ -80,7 +47,7 @@ static int ad7887_ring_preenable(struct iio_dev *indio_dev)
 			set_bytes_per_datum(indio_dev->buffer, st->d_size);
 
 	/* We know this is a single long so can 'cheat' */
-	switch (*ring->scan_mask) {
+	switch (*indio_dev->active_scan_mask) {
 	case (1 << 0):
 		st->ring_msg = &st->msg[AD7887_CH0];
 		break;
@@ -121,7 +88,8 @@ static irqreturn_t ad7887_trigger_handler(int irq, void *p)
 	__u8 *buf;
 	int b_sent;
 
-	unsigned int bytes = ring->scan_count *
+	unsigned int bytes = bitmap_weight(indio_dev->active_scan_mask,
+					   indio_dev->masklength) *
 		st->chip_info->channel[0].scan_type.storagebits / 8;
 
 	buf = kzalloc(st->d_size, GFP_KERNEL);
@@ -176,7 +144,7 @@ int ad7887_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 		goto error_deallocate_sw_rb;
 	}
 	/* Ring buffer functions - here trigger setup related */
-	indio_dev->buffer->setup_ops = &ad7887_ring_setup_ops;
+	indio_dev->setup_ops = &ad7887_ring_setup_ops;
 
 	/* Flag that polled ring buffering is possible */
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
diff --git a/drivers/staging/iio/adc/ad799x.h b/drivers/staging/iio/adc/ad799x.h
index eda01d5bab7d..356f690a76fb 100644
--- a/drivers/staging/iio/adc/ad799x.h
+++ b/drivers/staging/iio/adc/ad799x.h
@@ -124,15 +124,9 @@ struct ad799x_platform_data {
 int ad7997_8_set_scan_mode(struct ad799x_state *st, unsigned mask);
 
 #ifdef CONFIG_AD799X_RING_BUFFER
-int ad799x_single_channel_from_ring(struct iio_dev *indio_dev, int channum);
 int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev);
 void ad799x_ring_cleanup(struct iio_dev *indio_dev);
 #else /* CONFIG_AD799X_RING_BUFFER */
-int ad799x_single_channel_from_ring(struct iio_dev *indio_dev, int channum)
-{
-	return -EINVAL;
-}
-
 
 static inline int
 ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/adc/ad799x_core.c b/drivers/staging/iio/adc/ad799x_core.c
index c0d2f886ea2c..d5b581d8bc2b 100644
--- a/drivers/staging/iio/adc/ad799x_core.c
+++ b/drivers/staging/iio/adc/ad799x_core.c
@@ -35,7 +35,8 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../events.h"
+#include "../buffer.h"
 
 #include "ad799x.h"
 
@@ -150,8 +151,7 @@ static int ad799x_read_raw(struct iio_dev *indio_dev,
 	case 0:
 		mutex_lock(&indio_dev->mlock);
 		if (iio_buffer_enabled(indio_dev))
-			ret = ad799x_single_channel_from_ring(indio_dev,
-							      chan->scan_index);
+			ret = -EBUSY;
 		else
 			ret = ad799x_scan_direct(st, chan->scan_index);
 		mutex_unlock(&indio_dev->mlock);
@@ -161,7 +161,7 @@ static int ad799x_read_raw(struct iio_dev *indio_dev,
 		*val = (ret >> chan->scan_type.shift) &
 			RES_MASK(chan->scan_type.realbits);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		scale_uv = (st->int_vref_mv * 1000) >> chan->scan_type.realbits;
 		*val =  scale_uv / 1000;
 		*val2 = (scale_uv % 1000) * 1000;
@@ -934,4 +934,3 @@ module_i2c_driver(ad799x_driver);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD799x ADC");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("i2c:ad799x");
diff --git a/drivers/staging/iio/adc/ad799x_ring.c b/drivers/staging/iio/adc/ad799x_ring.c
index e3f4698d7815..5dded9e7820a 100644
--- a/drivers/staging/iio/adc/ad799x_ring.c
+++ b/drivers/staging/iio/adc/ad799x_ring.c
@@ -17,42 +17,12 @@
 #include <linux/bitops.h>
 
 #include "../iio.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 #include "../trigger_consumer.h"
 
 #include "ad799x.h"
 
-int ad799x_single_channel_from_ring(struct iio_dev *indio_dev, int channum)
-{
-	struct iio_buffer *ring = indio_dev->buffer;
-	int count = 0, ret;
-	u16 *ring_data;
-
-	if (!(test_bit(channum, ring->scan_mask))) {
-		ret = -EBUSY;
-		goto error_ret;
-	}
-
-	ring_data = kmalloc(ring->access->get_bytes_per_datum(ring),
-			    GFP_KERNEL);
-	if (ring_data == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = ring->access->read_last(ring, (u8 *) ring_data);
-	if (ret)
-		goto error_free_ring_data;
-	/* Need a count of channels prior to this one */
-	count = bitmap_weight(ring->scan_mask, channum);
-	ret = be16_to_cpu(ring_data[count]);
-
-error_free_ring_data:
-	kfree(ring_data);
-error_ret:
-	return ret;
-}
-
 /**
  * ad799x_ring_preenable() setup the parameters of the ring before enabling
  *
@@ -71,9 +41,10 @@ static int ad799x_ring_preenable(struct iio_dev *indio_dev)
 	 */
 
 	if (st->id == ad7997 || st->id == ad7998)
-		ad7997_8_set_scan_mode(st, *ring->scan_mask);
+		ad7997_8_set_scan_mode(st, *indio_dev->active_scan_mask);
 
-	st->d_size = ring->scan_count * 2;
+	st->d_size = bitmap_weight(indio_dev->active_scan_mask,
+				   indio_dev->masklength) * 2;
 
 	if (ring->scan_timestamp) {
 		st->d_size += sizeof(s64);
@@ -115,12 +86,13 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 	case ad7991:
 	case ad7995:
 	case ad7999:
-		cmd = st->config | (*ring->scan_mask << AD799X_CHANNEL_SHIFT);
+		cmd = st->config |
+			(*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT);
 		break;
 	case ad7992:
 	case ad7993:
 	case ad7994:
-		cmd = (*ring->scan_mask << AD799X_CHANNEL_SHIFT) |
+		cmd = (*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT) |
 			AD7998_CONV_RES_REG;
 		break;
 	case ad7997:
@@ -132,7 +104,8 @@ static irqreturn_t ad799x_trigger_handler(int irq, void *p)
 	}
 
 	b_sent = i2c_smbus_read_i2c_block_data(st->client,
-			cmd, ring->scan_count * 2, rxbuf);
+			cmd, bitmap_weight(indio_dev->active_scan_mask,
+					   indio_dev->masklength) * 2, rxbuf);
 	if (b_sent < 0)
 		goto done;
 
@@ -183,7 +156,7 @@ int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	}
 
 	/* Ring buffer functions - here trigger setup related */
-	indio_dev->buffer->setup_ops = &ad799x_buf_setup_ops;
+	indio_dev->setup_ops = &ad799x_buf_setup_ops;
 	indio_dev->buffer->scan_timestamp = true;
 
 	/* Flag that polled ring buffering is possible */
diff --git a/drivers/staging/iio/adc/adt7310.c b/drivers/staging/iio/adc/adt7310.c
index bc307f3b024e..eec2f325d549 100644
--- a/drivers/staging/iio/adc/adt7310.c
+++ b/drivers/staging/iio/adc/adt7310.c
@@ -17,7 +17,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-
+#include "../events.h"
 /*
  * ADT7310 registers definition
  */
@@ -877,7 +877,6 @@ MODULE_DEVICE_TABLE(spi, adt7310_id);
 static struct spi_driver adt7310_driver = {
 	.driver = {
 		.name = "adt7310",
-		.bus = &spi_bus_type,
 		.owner = THIS_MODULE,
 	},
 	.probe = adt7310_probe,
diff --git a/drivers/staging/iio/adc/adt7410.c b/drivers/staging/iio/adc/adt7410.c
index 3481cf6f7574..c62248ceb37a 100644
--- a/drivers/staging/iio/adc/adt7410.c
+++ b/drivers/staging/iio/adc/adt7410.c
@@ -17,6 +17,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
+#include "../events.h"
 
 /*
  * ADT7410 registers definition
diff --git a/drivers/staging/iio/adc/max1363.h b/drivers/staging/iio/adc/max1363.h
index cbcb08a2cb50..2cd0112067b2 100644
--- a/drivers/staging/iio/adc/max1363.h
+++ b/drivers/staging/iio/adc/max1363.h
@@ -146,22 +146,22 @@ struct max1363_state {
 };
 
 const struct max1363_mode
-*max1363_match_mode(unsigned long *mask, const struct max1363_chip_info *ci);
+*max1363_match_mode(const unsigned long *mask,
+		    const struct max1363_chip_info *ci);
 
 int max1363_set_scan_mode(struct max1363_state *st);
 
 #ifdef CONFIG_MAX1363_RING_BUFFER
-
-int max1363_single_channel_from_ring(const long *mask,
-				     struct max1363_state *st);
+int max1363_update_scan_mode(struct iio_dev *indio_dev,
+			     const unsigned long *scan_mask);
 int max1363_register_ring_funcs_and_init(struct iio_dev *indio_dev);
 void max1363_ring_cleanup(struct iio_dev *indio_dev);
 
 #else /* CONFIG_MAX1363_RING_BUFFER */
-
-int max1363_single_channel_from_ring(long mask, struct max1363_state *st)
+int max1363_update_scan_mode(struct iio_dev *indio_dev,
+			     const long *scan_mask)
 {
-	return -EINVAL;
+	return 0;
 }
 
 static inline int
diff --git a/drivers/staging/iio/adc/max1363_core.c b/drivers/staging/iio/adc/max1363_core.c
index 3f28f1ab52a2..b92cb4af18ce 100644
--- a/drivers/staging/iio/adc/max1363_core.c
+++ b/drivers/staging/iio/adc/max1363_core.c
@@ -34,7 +34,8 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../events.h"
+#include "../buffer.h"
 
 #include "max1363.h"
 
@@ -147,7 +148,8 @@ static const struct max1363_mode max1363_mode_table[] = {
 };
 
 const struct max1363_mode
-*max1363_match_mode(unsigned long *mask, const struct max1363_chip_info *ci)
+*max1363_match_mode(const unsigned long *mask,
+const struct max1363_chip_info *ci)
 {
 	int i;
 	if (mask)
@@ -189,7 +191,6 @@ static int max1363_read_single_chan(struct iio_dev *indio_dev,
 	int ret = 0;
 	s32 data;
 	char rxbuf[2];
-	const unsigned long *mask;
 	struct max1363_state *st = iio_priv(indio_dev);
 	struct i2c_client *client = st->client;
 
@@ -198,46 +199,38 @@ static int max1363_read_single_chan(struct iio_dev *indio_dev,
 	 * If monitor mode is enabled, the method for reading a single
 	 * channel will have to be rather different and has not yet
 	 * been implemented.
+	 *
+	 * Also, cannot read directly if buffered capture enabled.
 	 */
-	if (st->monitor_on) {
+	if (st->monitor_on || iio_buffer_enabled(indio_dev)) {
 		ret = -EBUSY;
 		goto error_ret;
 	}
 
-	/* If ring buffer capture is occurring, query the buffer */
-	if (iio_buffer_enabled(indio_dev)) {
-		mask = max1363_mode_table[chan->address].modemask;
-		data = max1363_single_channel_from_ring(mask, st);
+	/* Check to see if current scan mode is correct */
+	if (st->current_mode != &max1363_mode_table[chan->address]) {
+		/* Update scan mode if needed */
+		st->current_mode = &max1363_mode_table[chan->address];
+		ret = max1363_set_scan_mode(st);
+		if (ret < 0)
+			goto error_ret;
+	}
+	if (st->chip_info->bits != 8) {
+		/* Get reading */
+		data = i2c_master_recv(client, rxbuf, 2);
 		if (data < 0) {
 			ret = data;
 			goto error_ret;
 		}
+		data = (s32)(rxbuf[1]) | ((s32)(rxbuf[0] & 0x0F)) << 8;
 	} else {
-		/* Check to see if current scan mode is correct */
-		if (st->current_mode != &max1363_mode_table[chan->address]) {
-			/* Update scan mode if needed */
-			st->current_mode = &max1363_mode_table[chan->address];
-			ret = max1363_set_scan_mode(st);
-			if (ret < 0)
-				goto error_ret;
-		}
-		if (st->chip_info->bits != 8) {
-			/* Get reading */
-			data = i2c_master_recv(client, rxbuf, 2);
-			if (data < 0) {
-				ret = data;
-				goto error_ret;
-			}
-			data = (s32)(rxbuf[1]) | ((s32)(rxbuf[0] & 0x0F)) << 8;
-		} else {
-			/* Get reading */
-			data = i2c_master_recv(client, rxbuf, 1);
-			if (data < 0) {
-				ret = data;
-				goto error_ret;
-			}
-			data = rxbuf[0];
+		/* Get reading */
+		data = i2c_master_recv(client, rxbuf, 1);
+		if (data < 0) {
+			ret = data;
+			goto error_ret;
 		}
+		data = rxbuf[0];
 	}
 	*val = data;
 error_ret:
@@ -260,7 +253,7 @@ static int max1363_read_raw(struct iio_dev *indio_dev,
 		if (ret < 0)
 			return ret;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		if ((1 << (st->chip_info->bits + 1)) >
 		    st->chip_info->int_vref_mv) {
 			*val = 0;
@@ -288,7 +281,7 @@ static const enum max1363_modes max1363_mode_list[] = {
 #define MAX1363_EV_M						\
 	(IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING)	\
 	 | IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING))
-#define MAX1363_INFO_MASK (1 << IIO_CHAN_INFO_SCALE_SHARED)
+#define MAX1363_INFO_MASK IIO_CHAN_INFO_SCALE_SHARED_BIT
 #define MAX1363_CHAN_U(num, addr, si, bits, evmask)			\
 	{								\
 		.type = IIO_VOLTAGE,					\
@@ -296,7 +289,13 @@ static const enum max1363_modes max1363_mode_list[] = {
 		.channel = num,						\
 		.address = addr,					\
 		.info_mask = MAX1363_INFO_MASK,				\
-		.scan_type = IIO_ST('u', bits, (bits > 8) ? 16 : 8, 0), \
+		.datasheet_name = "AIN"#num,				\
+		.scan_type = {						\
+			.sign = 'u',					\
+			.realbits = bits,				\
+			.storagebits = (bits > 8) ? 16 : 8,		\
+			.endianness = IIO_BE,				\
+		},							\
 		.scan_index = si,					\
 		.event_mask = evmask,					\
 	}
@@ -311,7 +310,13 @@ static const enum max1363_modes max1363_mode_list[] = {
 		.channel2 = num2,					\
 		.address = addr,					\
 		.info_mask = MAX1363_INFO_MASK,				\
-		.scan_type = IIO_ST('u', bits, (bits > 8) ? 16 : 8, 0), \
+		.datasheet_name = "AIN"#num"-AIN"#num2,			\
+		.scan_type = {						\
+			.sign = 's',					\
+			.realbits = bits,				\
+			.storagebits = (bits > 8) ? 16 : 8,		\
+			.endianness = IIO_BE,				\
+		},							\
 		.scan_index = si,					\
 		.event_mask = evmask,					\
 	}
@@ -833,6 +838,7 @@ static const struct iio_info max1363_info = {
 	.read_event_config = &max1363_read_event_config,
 	.write_event_config = &max1363_write_event_config,
 	.read_raw = &max1363_read_raw,
+	.update_scan_mode = &max1363_update_scan_mode,
 	.driver_module = THIS_MODULE,
 	.event_attrs = &max1363_event_attribute_group,
 };
diff --git a/drivers/staging/iio/adc/max1363_ring.c b/drivers/staging/iio/adc/max1363_ring.c
index df6893e058cc..f730b3fb971a 100644
--- a/drivers/staging/iio/adc/max1363_ring.c
+++ b/drivers/staging/iio/adc/max1363_ring.c
@@ -15,88 +15,25 @@
 #include <linux/bitops.h>
 
 #include "../iio.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 #include "../trigger_consumer.h"
 
 #include "max1363.h"
 
-int max1363_single_channel_from_ring(const long *mask, struct max1363_state *st)
-{
-	struct iio_buffer *ring = iio_priv_to_dev(st)->buffer;
-	int count = 0, ret, index;
-	u8 *ring_data;
-	index = find_first_bit(mask, MAX1363_MAX_CHANNELS);
-
-	if (!(test_bit(index, st->current_mode->modemask))) {
-		ret = -EBUSY;
-		goto error_ret;
-	}
-
-	ring_data = kmalloc(ring->access->get_bytes_per_datum(ring),
-			    GFP_KERNEL);
-	if (ring_data == NULL) {
-		ret = -ENOMEM;
-		goto error_ret;
-	}
-	ret = ring->access->read_last(ring, ring_data);
-	if (ret)
-		goto error_free_ring_data;
-	/* Need a count of channels prior to this one */
-
-	count = bitmap_weight(mask, index - 1);
-	if (st->chip_info->bits != 8)
-		ret = ((int)(ring_data[count*2 + 0] & 0x0F) << 8)
-			+ (int)(ring_data[count*2 + 1]);
-	else
-		ret = ring_data[count];
-
-error_free_ring_data:
-	kfree(ring_data);
-error_ret:
-	return ret;
-}
-
-
-/**
- * max1363_ring_preenable() - setup the parameters of the ring before enabling
- *
- * The complex nature of the setting of the nuber of bytes per datum is due
- * to this driver currently ensuring that the timestamp is stored at an 8
- * byte boundary.
- **/
-static int max1363_ring_preenable(struct iio_dev *indio_dev)
+int max1363_update_scan_mode(struct iio_dev *indio_dev,
+			     const unsigned long *scan_mask)
 {
 	struct max1363_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
-	size_t d_size = 0;
-	unsigned long numvals;
 
 	/*
 	 * Need to figure out the current mode based upon the requested
 	 * scan mask in iio_dev
 	 */
-	st->current_mode = max1363_match_mode(ring->scan_mask,
-					st->chip_info);
+	st->current_mode = max1363_match_mode(scan_mask, st->chip_info);
 	if (!st->current_mode)
 		return -EINVAL;
-
 	max1363_set_scan_mode(st);
-
-	numvals = bitmap_weight(st->current_mode->modemask,
-				indio_dev->masklength);
-	if (ring->access->set_bytes_per_datum) {
-		if (ring->scan_timestamp)
-			d_size += sizeof(s64);
-		if (st->chip_info->bits != 8)
-			d_size += numvals*2;
-		else
-			d_size += numvals;
-		if (ring->scan_timestamp && (d_size % 8))
-			d_size += 8 - (d_size % 8);
-		ring->access->set_bytes_per_datum(ring, d_size);
-	}
-
 	return 0;
 }
 
@@ -114,12 +51,14 @@ static irqreturn_t max1363_trigger_handler(int irq, void *p)
 
 	/* Ensure the timestamp is 8 byte aligned */
 	if (st->chip_info->bits != 8)
-		d_size = numvals*2 + sizeof(s64);
+		d_size = numvals*2;
 	else
-		d_size = numvals + sizeof(s64);
-	if (d_size % sizeof(s64))
-		d_size += sizeof(s64) - (d_size % sizeof(s64));
-
+		d_size = numvals;
+	if (indio_dev->buffer->scan_timestamp) {
+		d_size += sizeof(s64);
+		if (d_size % sizeof(s64))
+			d_size += sizeof(s64) - (d_size % sizeof(s64));
+	}
 	/* Monitor mode prevents reading. Whilst not currently implemented
 	 * might as well have this test in here in the meantime as it does
 	 * no harm.
@@ -139,9 +78,10 @@ static irqreturn_t max1363_trigger_handler(int irq, void *p)
 
 	time_ns = iio_get_time_ns();
 
-	memcpy(rxbuf + d_size - sizeof(s64), &time_ns, sizeof(time_ns));
+	if (indio_dev->buffer->scan_timestamp)
+		memcpy(rxbuf + d_size - sizeof(s64), &time_ns, sizeof(time_ns));
+	iio_push_to_buffer(indio_dev->buffer, rxbuf, time_ns);
 
-	indio_dev->buffer->access->store_to(indio_dev->buffer, rxbuf, time_ns);
 done:
 	iio_trigger_notify_done(indio_dev->trig);
 	kfree(rxbuf);
@@ -151,7 +91,7 @@ done:
 
 static const struct iio_buffer_setup_ops max1363_ring_setup_ops = {
 	.postenable = &iio_triggered_buffer_postenable,
-	.preenable = &max1363_ring_preenable,
+	.preenable = &iio_sw_buffer_preenable,
 	.predisable = &iio_triggered_buffer_predisable,
 };
 
@@ -179,7 +119,7 @@ int max1363_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	/* Effectively select the ring buffer implementation */
 	indio_dev->buffer->access = &ring_sw_access_funcs;
 	/* Ring buffer functions - here trigger setup related */
-	indio_dev->buffer->setup_ops = &max1363_ring_setup_ops;
+	indio_dev->setup_ops = &max1363_ring_setup_ops;
 
 	/* Flag that polled ring buffering is possible */
 	indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
diff --git a/drivers/staging/iio/addac/adt7316-spi.c b/drivers/staging/iio/addac/adt7316-spi.c
index 1e93c7b7aed9..1ea3cd06299d 100644
--- a/drivers/staging/iio/addac/adt7316-spi.c
+++ b/drivers/staging/iio/addac/adt7316-spi.c
@@ -151,7 +151,6 @@ static int adt7316_spi_resume(struct spi_device *spi_dev)
 static struct spi_driver adt7316_driver = {
 	.driver = {
 		.name = "adt7316",
-		.bus = &spi_bus_type,
 		.owner = THIS_MODULE,
 	},
 	.probe = adt7316_spi_probe,
diff --git a/drivers/staging/iio/addac/adt7316.c b/drivers/staging/iio/addac/adt7316.c
index 8df24704ff26..13c39292d3f2 100644
--- a/drivers/staging/iio/addac/adt7316.c
+++ b/drivers/staging/iio/addac/adt7316.c
@@ -20,6 +20,7 @@
 #include <linux/module.h>
 
 #include "../iio.h"
+#include "../events.h"
 #include "../sysfs.h"
 #include "adt7316.h"
 
diff --git a/drivers/staging/iio/buffer_generic.h b/drivers/staging/iio/buffer.h
index 9e8f01009979..6fb6e64181a5 100644
--- a/drivers/staging/iio/buffer_generic.h
+++ b/drivers/staging/iio/buffer.h
@@ -11,7 +11,6 @@
 #define _IIO_BUFFER_GENERIC_H_
 #include <linux/sysfs.h>
 #include "iio.h"
-#include "chrdev.h"
 
 #ifdef CONFIG_IIO_BUFFER
 
@@ -19,22 +18,14 @@ struct iio_buffer;
 
 /**
  * struct iio_buffer_access_funcs - access functions for buffers.
- * @mark_in_use:	reference counting, typically to prevent module removal
- * @unmark_in_use:	reduce reference count when no longer using buffer
  * @store_to:		actually store stuff to the buffer
- * @read_last:		get the last element stored
- * @read_first_n:	try to get a specified number of elements (must exist)
- * @mark_param_change:	notify buffer that some relevant parameter has changed
- *			Often this means the underlying storage may need to
- *			change.
+ * @read_first_n:	try to get a specified number of bytes (must exist)
  * @request_update:	if a parameter change has been marked, update underlying
  *			storage.
  * @get_bytes_per_datum:get current bytes per datum
  * @set_bytes_per_datum:set number of bytes per datum
  * @get_length:		get number of datums in buffer
  * @set_length:		set number of datums in buffer
- * @is_enabled:		query if buffer is currently being used
- * @enable:		enable the buffer
  *
  * The purpose of this structure is to make the buffer element
  * modular as event for a given driver, different usecases may require
@@ -45,85 +36,60 @@ struct iio_buffer;
  * any of them not existing.
  **/
 struct iio_buffer_access_funcs {
-	void (*mark_in_use)(struct iio_buffer *buffer);
-	void (*unmark_in_use)(struct iio_buffer *buffer);
-
 	int (*store_to)(struct iio_buffer *buffer, u8 *data, s64 timestamp);
-	int (*read_last)(struct iio_buffer *buffer, u8 *data);
 	int (*read_first_n)(struct iio_buffer *buffer,
 			    size_t n,
 			    char __user *buf);
 
-	int (*mark_param_change)(struct iio_buffer *buffer);
 	int (*request_update)(struct iio_buffer *buffer);
 
 	int (*get_bytes_per_datum)(struct iio_buffer *buffer);
 	int (*set_bytes_per_datum)(struct iio_buffer *buffer, size_t bpd);
 	int (*get_length)(struct iio_buffer *buffer);
 	int (*set_length)(struct iio_buffer *buffer, int length);
-
-	int (*is_enabled)(struct iio_buffer *buffer);
-	int (*enable)(struct iio_buffer *buffer);
-};
-
-/**
- * struct iio_buffer_setup_ops - buffer setup related callbacks
- * @preenable:		[DRIVER] function to run prior to marking buffer enabled
- * @postenable:		[DRIVER] function to run after marking buffer enabled
- * @predisable:		[DRIVER] function to run prior to marking buffer
- *			disabled
- * @postdisable:	[DRIVER] function to run after marking buffer disabled
- */
-struct iio_buffer_setup_ops {
-	int				(*preenable)(struct iio_dev *);
-	int				(*postenable)(struct iio_dev *);
-	int				(*predisable)(struct iio_dev *);
-	int				(*postdisable)(struct iio_dev *);
 };
 
 /**
  * struct iio_buffer - general buffer structure
- * @indio_dev:		industrial I/O device structure
- * @owner:		module that owns the buffer (for ref counting)
  * @length:		[DEVICE] number of datums in buffer
  * @bytes_per_datum:	[DEVICE] size of individual datum including timestamp
- * @bpe:		[DEVICE] size of individual channel value
  * @scan_el_attrs:	[DRIVER] control of scan elements if that scan mode
  *			control method is used
- * @scan_count:	[INTERN] the number of elements in the current scan mode
  * @scan_mask:		[INTERN] bitmask used in masking scan mode elements
+ * @scan_index_timestamp:[INTERN] cache of the index to the timestamp
  * @scan_timestamp:	[INTERN] does the scan mode include a timestamp
  * @access:		[DRIVER] buffer access functions associated with the
  *			implementation.
- * @flags:		[INTERN] file ops related flags including busy flag.
+ * @scan_el_dev_attr_list:[INTERN] list of scan element related attributes.
+ * @scan_el_group:	[DRIVER] attribute group for those attributes not
+ *			created from the iio_chan_info array.
+ * @pollq:		[INTERN] wait queue to allow for polling on the buffer.
+ * @stufftoread:	[INTERN] flag to indicate new data.
+ * @demux_list:		[INTERN] list of operations required to demux the scan.
+ * @demux_bounce:	[INTERN] buffer for doing gather from incoming scan.
  **/
 struct iio_buffer {
-	struct iio_dev				*indio_dev;
-	struct module				*owner;
 	int					length;
 	int					bytes_per_datum;
-	int					bpe;
 	struct attribute_group			*scan_el_attrs;
-	int					scan_count;
 	long					*scan_mask;
 	bool					scan_timestamp;
+	unsigned				scan_index_timestamp;
 	const struct iio_buffer_access_funcs	*access;
-	const struct iio_buffer_setup_ops		*setup_ops;
 	struct list_head			scan_el_dev_attr_list;
 	struct attribute_group			scan_el_group;
 	wait_queue_head_t			pollq;
 	bool					stufftoread;
-	unsigned long				flags;
 	const struct attribute_group *attrs;
+	struct list_head			demux_list;
+	unsigned char				*demux_bounce;
 };
 
 /**
  * iio_buffer_init() - Initialize the buffer structure
  * @buffer: buffer to be initialized
- * @indio_dev: the iio device the buffer is assocated with
  **/
-void iio_buffer_init(struct iio_buffer *buffer,
-			  struct iio_dev *indio_dev);
+void iio_buffer_init(struct iio_buffer *buffer);
 
 void iio_buffer_deinit(struct iio_buffer *buffer);
 
@@ -140,17 +106,27 @@ static inline void __iio_update_buffer(struct iio_buffer *buffer,
 	buffer->length = length;
 }
 
-int iio_scan_mask_query(struct iio_buffer *buffer, int bit);
+int iio_scan_mask_query(struct iio_dev *indio_dev,
+			struct iio_buffer *buffer, int bit);
 
 /**
  * iio_scan_mask_set() - set particular bit in the scan mask
  * @buffer: the buffer whose scan mask we are interested in
  * @bit: the bit to be set.
  **/
-int iio_scan_mask_set(struct iio_buffer *buffer, int bit);
+int iio_scan_mask_set(struct iio_dev *indio_dev,
+		      struct iio_buffer *buffer, int bit);
 
-#define to_iio_buffer(d)				\
-	container_of(d, struct iio_buffer, dev)
+/**
+ * iio_push_to_buffer() - push to a registered buffer.
+ * @buffer:		IIO buffer structure for device
+ * @scan:		Full scan.
+ * @timestamp:
+ */
+int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data,
+		       s64 timestamp);
+
+int iio_update_demux(struct iio_dev *indio_dev);
 
 /**
  * iio_buffer_register() - register the buffer with IIO core
@@ -180,12 +156,6 @@ ssize_t iio_buffer_write_length(struct device *dev,
 			      const char *buf,
 			      size_t len);
 /**
- * iio_buffer_read_bytes_per_datum() - attr for number of bytes in whole datum
- **/
-ssize_t iio_buffer_read_bytes_per_datum(struct device *dev,
-					struct device_attribute *attr,
-					char *buf);
-/**
  * iio_buffer_store_enable() - attr to turn the buffer on
  **/
 ssize_t iio_buffer_store_enable(struct device *dev,
@@ -201,9 +171,6 @@ ssize_t iio_buffer_show_enable(struct device *dev,
 #define IIO_BUFFER_LENGTH_ATTR DEVICE_ATTR(length, S_IRUGO | S_IWUSR,	\
 					   iio_buffer_read_length,	\
 					   iio_buffer_write_length)
-#define IIO_BUFFER_BYTES_PER_DATUM_ATTR					\
-	DEVICE_ATTR(bytes_per_datum, S_IRUGO | S_IWUSR,			\
-		    iio_buffer_read_bytes_per_datum, NULL)
 
 #define IIO_BUFFER_ENABLE_ATTR DEVICE_ATTR(enable, S_IRUGO | S_IWUSR,	\
 					   iio_buffer_show_enable,	\
diff --git a/drivers/staging/iio/cdc/ad7150.c b/drivers/staging/iio/cdc/ad7150.c
index 47181875e113..b73007dcf4b3 100644
--- a/drivers/staging/iio/cdc/ad7150.c
+++ b/drivers/staging/iio/cdc/ad7150.c
@@ -15,7 +15,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-
+#include "../events.h"
 /*
  * AD7150 registers definition
  */
@@ -111,7 +111,7 @@ static int ad7150_read_raw(struct iio_dev *indio_dev,
 			return ret;
 		*val = swab16(ret);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE):
+	case IIO_CHAN_INFO_AVERAGE_RAW:
 		ret = i2c_smbus_read_word_data(chip->client,
 					ad7150_addresses[chan->channel][1]);
 		if (ret < 0)
@@ -429,7 +429,7 @@ static const struct iio_chan_spec ad7150_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT,
 		.event_mask =
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING) |
@@ -441,7 +441,7 @@ static const struct iio_chan_spec ad7150_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = (1 << IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT,
 		.event_mask =
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |
 		IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING) |
diff --git a/drivers/staging/iio/cdc/ad7152.c b/drivers/staging/iio/cdc/ad7152.c
index 152d3be6d97d..fdb83c35e6dd 100644
--- a/drivers/staging/iio/cdc/ad7152.c
+++ b/drivers/staging/iio/cdc/ad7152.c
@@ -259,7 +259,7 @@ static int ad7152_write_raw(struct iio_dev *indio_dev,
 	mutex_lock(&indio_dev->mlock);
 
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		if (val != 1) {
 			ret = -EINVAL;
 			goto out;
@@ -276,7 +276,7 @@ static int ad7152_write_raw(struct iio_dev *indio_dev,
 		ret = 0;
 		break;
 
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		if ((val < 0) | (val > 0xFFFF)) {
 			ret = -EINVAL;
 			goto out;
@@ -289,7 +289,7 @@ static int ad7152_write_raw(struct iio_dev *indio_dev,
 
 		ret = 0;
 		break;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		if (val != 0) {
 			ret = -EINVAL;
 			goto out;
@@ -372,7 +372,7 @@ static int ad7152_read_raw(struct iio_dev *indio_dev,
 
 		ret = IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 
 		ret = i2c_smbus_read_word_data(chip->client,
 				ad7152_addresses[chan->channel][AD7152_GAIN]);
@@ -384,7 +384,7 @@ static int ad7152_read_raw(struct iio_dev *indio_dev,
 
 		ret = IIO_VAL_INT_PLUS_MICRO;
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		ret = i2c_smbus_read_word_data(chip->client,
 				ad7152_addresses[chan->channel][AD7152_OFFS]);
 		if (ret < 0)
@@ -393,7 +393,7 @@ static int ad7152_read_raw(struct iio_dev *indio_dev,
 
 		ret = IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		ret = i2c_smbus_read_byte_data(chip->client,
 				ad7152_addresses[chan->channel][AD7152_SETUP]);
 		if (ret < 0)
@@ -416,7 +416,7 @@ static int ad7152_write_raw_get_fmt(struct iio_dev *indio_dev,
 			       long mask)
 {
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		return IIO_VAL_INT_PLUS_NANO;
 	default:
 		return IIO_VAL_INT_PLUS_MICRO;
@@ -436,34 +436,34 @@ static const struct iio_chan_spec ad7152_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 	}, {
 		.type = IIO_CAPACITANCE,
 		.differential = 1,
 		.indexed = 1,
 		.channel = 0,
 		.channel2 = 2,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 	}, {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 	}, {
 		.type = IIO_CAPACITANCE,
 		.differential = 1,
 		.indexed = 1,
 		.channel = 1,
 		.channel2 = 3,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 	}
 };
 /*
diff --git a/drivers/staging/iio/cdc/ad7746.c b/drivers/staging/iio/cdc/ad7746.c
index 9df590891a69..40b8512cbc32 100644
--- a/drivers/staging/iio/cdc/ad7746.c
+++ b/drivers/staging/iio/cdc/ad7746.c
@@ -123,7 +123,7 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_EXT_VIN,
 	},
@@ -132,7 +132,7 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "supply",
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = AD7746_REG_VT_DATA_HIGH << 8 |
 			AD7746_VTSETUP_VTMD_VDD_MON,
 	},
@@ -156,10 +156,10 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SHARED) |
-		(1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = AD7746_REG_CAP_DATA_HIGH << 8,
 	},
 	[CIN1_DIFF] = {
@@ -168,10 +168,10 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.channel2 = 2,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SHARED) |
-		(1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = AD7746_REG_CAP_DATA_HIGH << 8 |
 			AD7746_CAPSETUP_CAPDIFF
 	},
@@ -179,10 +179,10 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.type = IIO_CAPACITANCE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SHARED) |
-		(1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = AD7746_REG_CAP_DATA_HIGH << 8 |
 			AD7746_CAPSETUP_CIN2,
 	},
@@ -192,10 +192,10 @@ static const struct iio_chan_spec ad7746_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.channel2 = 3,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SHARED) |
-		(1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = AD7746_REG_CAP_DATA_HIGH << 8 |
 			AD7746_CAPSETUP_CAPDIFF | AD7746_CAPSETUP_CIN2,
 	}
@@ -477,7 +477,7 @@ static int ad7746_write_raw(struct iio_dev *indio_dev,
 	mutex_lock(&indio_dev->mlock);
 
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		if (val != 1) {
 			ret = -EINVAL;
 			goto out;
@@ -503,7 +503,7 @@ static int ad7746_write_raw(struct iio_dev *indio_dev,
 
 		ret = 0;
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SHARED):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		if ((val < 0) | (val > 0xFFFF)) {
 			ret = -EINVAL;
 			goto out;
@@ -515,7 +515,7 @@ static int ad7746_write_raw(struct iio_dev *indio_dev,
 
 		ret = 0;
 		break;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		if ((val < 0) | (val > 43008000)) { /* 21pF */
 			ret = -EINVAL;
 			goto out;
@@ -612,7 +612,7 @@ static int ad7746_read_raw(struct iio_dev *indio_dev,
 
 		ret = IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		switch (chan->type) {
 		case IIO_CAPACITANCE:
 			reg = AD7746_REG_CAP_GAINH;
@@ -634,7 +634,7 @@ static int ad7746_read_raw(struct iio_dev *indio_dev,
 
 		ret = IIO_VAL_INT_PLUS_MICRO;
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SHARED):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		ret = i2c_smbus_read_word_data(chip->client,
 					       AD7746_REG_CAP_OFFH);
 		if (ret < 0)
@@ -643,13 +643,13 @@ static int ad7746_read_raw(struct iio_dev *indio_dev,
 
 		ret = IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		*val = AD7746_CAPDAC_DACP(chip->capdac[chan->channel]
 			[chan->differential]) * 338646;
 
 		ret = IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_CAPACITANCE:
 			/* 8.192pf / 2^24 */
diff --git a/drivers/staging/iio/chrdev.h b/drivers/staging/iio/chrdev.h
deleted file mode 100644
index d8e736f60522..000000000000
--- a/drivers/staging/iio/chrdev.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/* The industrial I/O core - character device related
- *
- * Copyright (c) 2008 Jonathan Cameron
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License version 2 as published by
- * the Free Software Foundation.
- */
-
-#ifndef _IIO_CHRDEV_H_
-#define _IIO_CHRDEV_H_
-
-/**
- * struct iio_event_data - The actual event being pushed to userspace
- * @id:		event identifier
- * @timestamp:	best estimate of time of event occurrence (often from
- *		the interrupt handler)
- */
-struct iio_event_data {
-	u64	id;
-	s64	timestamp;
-};
-
-#define IIO_GET_EVENT_FD_IOCTL _IOR('i', 0x90, int)
-#endif
diff --git a/drivers/staging/iio/dac/Kconfig b/drivers/staging/iio/dac/Kconfig
index fac8549dc8e7..13e27979df24 100644
--- a/drivers/staging/iio/dac/Kconfig
+++ b/drivers/staging/iio/dac/Kconfig
@@ -24,6 +24,29 @@ config AD5360
 	  To compile this driver as module choose M here: the module will be called
 	  ad5360.
 
+config AD5380
+	tristate "Analog Devices AD5380/81/82/83/84/90/91/92 DAC driver"
+	depends on (SPI_MASTER || I2C)
+	select REGMAP_I2C if I2C
+	select REGMAP_SPI if SPI_MASTER
+	help
+	  Say yes here to build support for Analog Devices AD5380, AD5381,
+	  AD5382, AD5383, AD5384, AD5390, AD5391, AD5392 multi-channel
+	  Digital to Analog Converters (DAC).
+
+	  To compile this driver as module choose M here: the module will be called
+	  ad5380.
+
+config AD5421
+	tristate "Analog Devices AD5421 DAC driver"
+	depends on SPI
+	help
+	  Say yes here to build support for Analog Devices AD5421 loop-powered
+	  digital-to-analog convertors (DAC).
+
+	  To compile this driver as module choose M here: the module will be called
+	  ad5421.
+
 config AD5624R_SPI
 	tristate "Analog Devices AD5624/44/64R DAC spi driver"
 	depends on SPI
@@ -37,7 +60,7 @@ config AD5446
 	help
 	  Say yes here to build support for Analog Devices AD5444, AD5446,
 	  AD5512A, AD5542A, AD5543, AD5553, AD5601, AD5611, AD5620, AD5621,
-	  AD5640, AD5660 DACs.
+	  AD5640, AD5660, AD5662 DACs.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad5446.
@@ -52,12 +75,22 @@ config AD5504
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad5504.
 
+config AD5764
+	tristate "Analog Devices AD5764/64R/44/44R DAC driver"
+	depends on SPI_MASTER
+	help
+	  Say yes here to build support for Analog Devices AD5764, AD5764R, AD5744,
+	  AD5744R Digital to Analog Converter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad5764.
+
 config AD5791
-	tristate "Analog Devices AD5760/AD5780/AD5781/AD5791 DAC SPI driver"
+	tristate "Analog Devices AD5760/AD5780/AD5781/AD5790/AD5791 DAC SPI driver"
 	depends on SPI
 	help
 	  Say yes here to build support for Analog Devices AD5760, AD5780,
-	  AD5781, AD5791 High Resolution Voltage Output Digital to
+	  AD5781, AD5790, AD5791 High Resolution Voltage Output Digital to
 	  Analog Converter.
 
 	  To compile this driver as a module, choose M here: the
diff --git a/drivers/staging/iio/dac/Makefile b/drivers/staging/iio/dac/Makefile
index 07b6f5ebdb52..8ab1d264aab7 100644
--- a/drivers/staging/iio/dac/Makefile
+++ b/drivers/staging/iio/dac/Makefile
@@ -3,10 +3,13 @@
 #
 
 obj-$(CONFIG_AD5360) += ad5360.o
+obj-$(CONFIG_AD5380) += ad5380.o
+obj-$(CONFIG_AD5421) += ad5421.o
 obj-$(CONFIG_AD5624R_SPI) += ad5624r_spi.o
 obj-$(CONFIG_AD5064) += ad5064.o
 obj-$(CONFIG_AD5504) += ad5504.o
 obj-$(CONFIG_AD5446) += ad5446.o
+obj-$(CONFIG_AD5764) += ad5764.o
 obj-$(CONFIG_AD5791) += ad5791.o
 obj-$(CONFIG_AD5686) += ad5686.o
 obj-$(CONFIG_MAX517) += max517.o
diff --git a/drivers/staging/iio/dac/ad5064.c b/drivers/staging/iio/dac/ad5064.c
index 39cfe6cb02a2..049a855039c2 100644
--- a/drivers/staging/iio/dac/ad5064.c
+++ b/drivers/staging/iio/dac/ad5064.c
@@ -91,7 +91,7 @@ enum ad5064_type {
 	.indexed = 1,						\
 	.output = 1,						\
 	.channel = (chan),					\
-	.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),	\
+	.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,	\
 	.address = AD5064_ADDR_DAC(chan),			\
 	.scan_type = IIO_ST('u', (bits), 16, 20 - (bits))	\
 }
@@ -280,14 +280,14 @@ static int ad5064_read_raw(struct iio_dev *indio_dev,
 			   long m)
 {
 	struct ad5064_state *st = iio_priv(indio_dev);
-	unsigned long scale_uv;
 	unsigned int vref;
+	int scale_uv;
 
 	switch (m) {
 	case 0:
 		*val = st->dac_cache[chan->channel];
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		vref = st->chip_info->shared_vref ? 0 : chan->channel;
 		scale_uv = regulator_get_voltage(st->vref_reg[vref].consumer);
 		if (scale_uv < 0)
diff --git a/drivers/staging/iio/dac/ad5360.c b/drivers/staging/iio/dac/ad5360.c
index bc0459e32d04..710b256affcc 100644
--- a/drivers/staging/iio/dac/ad5360.c
+++ b/drivers/staging/iio/dac/ad5360.c
@@ -103,10 +103,10 @@ enum ad5360_type {
 	.type = IIO_VOLTAGE,					\
 	.indexed = 1,						\
 	.output = 1,						\
-	.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE) |	\
-		(1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |		\
-		(1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |	\
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),	\
+	.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT |	\
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |	\
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,	\
 	.scan_type = IIO_ST('u', (bits), 16, 16 - (bits))	\
 }
 
@@ -326,21 +326,21 @@ static int ad5360_write_raw(struct iio_dev *indio_dev,
 		return ad5360_write(indio_dev, AD5360_CMD_WRITE_DATA,
 				 chan->address, val, chan->scan_type.shift);
 
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		if (val >= max_val || val < 0)
 			return -EINVAL;
 
 		return ad5360_write(indio_dev, AD5360_CMD_WRITE_OFFSET,
 				 chan->address, val, chan->scan_type.shift);
 
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		if (val >= max_val || val < 0)
 			return -EINVAL;
 
 		return ad5360_write(indio_dev, AD5360_CMD_WRITE_GAIN,
 				 chan->address, val, chan->scan_type.shift);
 
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		if (val <= -max_val || val > 0)
 			return -EINVAL;
 
@@ -371,8 +371,8 @@ static int ad5360_read_raw(struct iio_dev *indio_dev,
 			   long m)
 {
 	struct ad5360_state *st = iio_priv(indio_dev);
-	unsigned long scale_uv;
 	unsigned int ofs_index;
+	int scale_uv;
 	int ret;
 
 	switch (m) {
@@ -383,7 +383,7 @@ static int ad5360_read_raw(struct iio_dev *indio_dev,
 			return ret;
 		*val = ret >> chan->scan_type.shift;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		/* vout = 4 * vref * dac_code */
 		scale_uv = ad5360_get_channel_vref(st, chan->channel) * 4 * 100;
 		if (scale_uv < 0)
@@ -393,21 +393,21 @@ static int ad5360_read_raw(struct iio_dev *indio_dev,
 		*val =  scale_uv / 100000;
 		*val2 = (scale_uv % 100000) * 10;
 		return IIO_VAL_INT_PLUS_MICRO;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		ret = ad5360_read(indio_dev, AD5360_READBACK_OFFSET,
 			chan->address);
 		if (ret < 0)
 			return ret;
 		*val = ret;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		ret = ad5360_read(indio_dev, AD5360_READBACK_GAIN,
 			chan->address);
 		if (ret < 0)
 			return ret;
 		*val = ret;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		ofs_index = ad5360_get_channel_vref_index(st, chan->channel);
 		ret = ad5360_read(indio_dev, AD5360_READBACK_SF,
 			AD5360_REG_SF_OFS(ofs_index));
diff --git a/drivers/staging/iio/dac/ad5380.c b/drivers/staging/iio/dac/ad5380.c
new file mode 100644
index 000000000000..eff97ae05c4b
--- /dev/null
+++ b/drivers/staging/iio/dac/ad5380.c
@@ -0,0 +1,676 @@
+/*
+ * Analog devices AD5380, AD5381, AD5382, AD5383, AD5390, AD5391, AD5392
+ * multi-channel Digital to Analog Converters driver
+ *
+ * Copyright 2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+#include "../iio.h"
+#include "../sysfs.h"
+#include "dac.h"
+
+
+#define AD5380_REG_DATA(x)	(((x) << 2) | 3)
+#define AD5380_REG_OFFSET(x)	(((x) << 2) | 2)
+#define AD5380_REG_GAIN(x)	(((x) << 2) | 1)
+#define AD5380_REG_SF_PWR_DOWN	(8 << 2)
+#define AD5380_REG_SF_PWR_UP	(9 << 2)
+#define AD5380_REG_SF_CTRL	(12 << 2)
+
+#define AD5380_CTRL_PWR_DOWN_MODE_OFFSET	13
+#define AD5380_CTRL_INT_VREF_2V5		BIT(12)
+#define AD5380_CTRL_INT_VREF_EN			BIT(10)
+
+/**
+ * struct ad5380_chip_info - chip specific information
+ * @channel_template:	channel specification template
+ * @num_channels:	number of channels
+ * @int_vref:		internal vref in uV
+*/
+
+struct ad5380_chip_info {
+	struct iio_chan_spec	channel_template;
+	unsigned int		num_channels;
+	unsigned int		int_vref;
+};
+
+/**
+ * struct ad5380_state - driver instance specific data
+ * @regmap:		regmap instance used by the device
+ * @chip_info:		chip model specific constants, available modes etc
+ * @vref_reg:		vref supply regulator
+ * @vref:		actual reference voltage used in uA
+ * @pwr_down:		whether the chip is currently in power down mode
+ */
+
+struct ad5380_state {
+	struct regmap			*regmap;
+	const struct ad5380_chip_info	*chip_info;
+	struct regulator		*vref_reg;
+	int				vref;
+	bool				pwr_down;
+};
+
+enum ad5380_type {
+	ID_AD5380_3,
+	ID_AD5380_5,
+	ID_AD5381_3,
+	ID_AD5381_5,
+	ID_AD5382_3,
+	ID_AD5382_5,
+	ID_AD5383_3,
+	ID_AD5383_5,
+	ID_AD5390_3,
+	ID_AD5390_5,
+	ID_AD5391_3,
+	ID_AD5391_5,
+	ID_AD5392_3,
+	ID_AD5392_5,
+};
+
+#define AD5380_CHANNEL(_bits) {					\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.output = 1,						\
+	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |		\
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
+	.scan_type = IIO_ST('u', (_bits), 16, 14 - (_bits))	\
+}
+
+static const struct ad5380_chip_info ad5380_chip_info_tbl[] = {
+	[ID_AD5380_3] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 40,
+		.int_vref = 1250000,
+	},
+	[ID_AD5380_5] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 40,
+		.int_vref = 2500000,
+	},
+	[ID_AD5381_3] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 16,
+		.int_vref = 1250000,
+	},
+	[ID_AD5381_5] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 16,
+		.int_vref = 2500000,
+	},
+	[ID_AD5382_3] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 32,
+		.int_vref = 1250000,
+	},
+	[ID_AD5382_5] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 32,
+		.int_vref = 2500000,
+	},
+	[ID_AD5383_3] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 32,
+		.int_vref = 1250000,
+	},
+	[ID_AD5383_5] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 32,
+		.int_vref = 2500000,
+	},
+	[ID_AD5390_3] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 16,
+		.int_vref = 1250000,
+	},
+	[ID_AD5390_5] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 16,
+		.int_vref = 2500000,
+	},
+	[ID_AD5391_3] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 16,
+		.int_vref = 1250000,
+	},
+	[ID_AD5391_5] = {
+		.channel_template = AD5380_CHANNEL(12),
+		.num_channels = 16,
+		.int_vref = 2500000,
+	},
+	[ID_AD5392_3] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 8,
+		.int_vref = 1250000,
+	},
+	[ID_AD5392_5] = {
+		.channel_template = AD5380_CHANNEL(14),
+		.num_channels = 8,
+		.int_vref = 2500000,
+	},
+};
+
+static ssize_t ad5380_read_dac_powerdown(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ad5380_state *st = iio_priv(indio_dev);
+
+	return sprintf(buf, "%d\n", st->pwr_down);
+}
+
+static ssize_t ad5380_write_dac_powerdown(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ad5380_state *st = iio_priv(indio_dev);
+	bool pwr_down;
+	int ret;
+
+	ret = strtobool(buf, &pwr_down);
+	if (ret)
+		return ret;
+
+	mutex_lock(&indio_dev->mlock);
+
+	if (pwr_down)
+		ret = regmap_write(st->regmap, AD5380_REG_SF_PWR_DOWN, 0);
+	else
+		ret = regmap_write(st->regmap, AD5380_REG_SF_PWR_UP, 0);
+
+	st->pwr_down = pwr_down;
+
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret ? ret : len;
+}
+
+static IIO_DEVICE_ATTR(out_voltage_powerdown,
+			S_IRUGO | S_IWUSR,
+			ad5380_read_dac_powerdown,
+			ad5380_write_dac_powerdown, 0);
+
+static const char ad5380_powerdown_modes[][15] = {
+	[0]	= "100kohm_to_gnd",
+	[1]	= "three_state",
+};
+
+static ssize_t ad5380_read_powerdown_mode(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ad5380_state *st = iio_priv(indio_dev);
+	unsigned int mode;
+	int ret;
+
+	ret = regmap_read(st->regmap, AD5380_REG_SF_CTRL, &mode);
+	if (ret)
+		return ret;
+
+	mode = (mode >> AD5380_CTRL_PWR_DOWN_MODE_OFFSET) & 1;
+
+	return sprintf(buf, "%s\n", ad5380_powerdown_modes[mode]);
+}
+
+static ssize_t ad5380_write_powerdown_mode(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ad5380_state *st = iio_priv(indio_dev);
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(ad5380_powerdown_modes); ++i) {
+		if (sysfs_streq(buf, ad5380_powerdown_modes[i]))
+			break;
+	}
+
+	if (i == ARRAY_SIZE(ad5380_powerdown_modes))
+		return -EINVAL;
+
+	ret = regmap_update_bits(st->regmap, AD5380_REG_SF_CTRL,
+		1 << AD5380_CTRL_PWR_DOWN_MODE_OFFSET,
+		i << AD5380_CTRL_PWR_DOWN_MODE_OFFSET);
+
+	return ret ? ret : len;
+}
+
+static IIO_DEVICE_ATTR(out_voltage_powerdown_mode,
+			S_IRUGO | S_IWUSR,
+			ad5380_read_powerdown_mode,
+			ad5380_write_powerdown_mode, 0);
+
+static IIO_CONST_ATTR(out_voltage_powerdown_mode_available,
+			"100kohm_to_gnd three_state");
+
+static struct attribute *ad5380_attributes[] = {
+	&iio_dev_attr_out_voltage_powerdown.dev_attr.attr,
+	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
+	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ad5380_attribute_group = {
+	.attrs = ad5380_attributes,
+};
+
+static unsigned int ad5380_info_to_reg(struct iio_chan_spec const *chan,
+	long info)
+{
+	switch (info) {
+	case 0:
+		return AD5380_REG_DATA(chan->address);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		return AD5380_REG_OFFSET(chan->address);
+	case IIO_CHAN_INFO_CALIBSCALE:
+		return AD5380_REG_GAIN(chan->address);
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int ad5380_write_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int val, int val2, long info)
+{
+	const unsigned int max_val = (1 << chan->scan_type.realbits);
+	struct ad5380_state *st = iio_priv(indio_dev);
+
+	switch (info) {
+	case 0:
+	case IIO_CHAN_INFO_CALIBSCALE:
+		if (val >= max_val || val < 0)
+			return -EINVAL;
+
+		return regmap_write(st->regmap,
+			ad5380_info_to_reg(chan, info),
+			val << chan->scan_type.shift);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		val += (1 << chan->scan_type.realbits) / 2;
+		if (val >= max_val || val < 0)
+			return -EINVAL;
+
+		return regmap_write(st->regmap,
+			AD5380_REG_OFFSET(chan->address),
+			val << chan->scan_type.shift);
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static int ad5380_read_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int *val, int *val2, long info)
+{
+	struct ad5380_state *st = iio_priv(indio_dev);
+	unsigned long scale_uv;
+	int ret;
+
+	switch (info) {
+	case 0:
+	case IIO_CHAN_INFO_CALIBSCALE:
+		ret = regmap_read(st->regmap, ad5380_info_to_reg(chan, info),
+					val);
+		if (ret)
+			return ret;
+		*val >>= chan->scan_type.shift;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		ret = regmap_read(st->regmap, AD5380_REG_OFFSET(chan->address),
+					val);
+		if (ret)
+			return ret;
+		*val >>= chan->scan_type.shift;
+		val -= (1 << chan->scan_type.realbits) / 2;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		scale_uv = ((2 * st->vref) >> chan->scan_type.realbits) * 100;
+		*val =  scale_uv / 100000;
+		*val2 = (scale_uv % 100000) * 10;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info ad5380_info = {
+	.read_raw = ad5380_read_raw,
+	.write_raw = ad5380_write_raw,
+	.attrs = &ad5380_attribute_group,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit ad5380_alloc_channels(struct iio_dev *indio_dev)
+{
+	struct ad5380_state *st = iio_priv(indio_dev);
+	struct iio_chan_spec *channels;
+	unsigned int i;
+
+	channels = kcalloc(sizeof(struct iio_chan_spec),
+			st->chip_info->num_channels, GFP_KERNEL);
+
+	if (!channels)
+		return -ENOMEM;
+
+	for (i = 0; i < st->chip_info->num_channels; ++i) {
+		channels[i] = st->chip_info->channel_template;
+		channels[i].channel = i;
+		channels[i].address = i;
+	}
+
+	indio_dev->channels = channels;
+
+	return 0;
+}
+
+static int __devinit ad5380_probe(struct device *dev, struct regmap *regmap,
+	enum ad5380_type type, const char *name)
+{
+	struct iio_dev *indio_dev;
+	struct ad5380_state *st;
+	unsigned int ctrl = 0;
+	int ret;
+
+	indio_dev = iio_allocate_device(sizeof(*st));
+	if (indio_dev == NULL) {
+		dev_err(dev, "Failed to allocate iio device\n");
+		ret = -ENOMEM;
+		goto error_regmap_exit;
+	}
+
+	st = iio_priv(indio_dev);
+	dev_set_drvdata(dev, indio_dev);
+
+	st->chip_info = &ad5380_chip_info_tbl[type];
+	st->regmap = regmap;
+
+	indio_dev->dev.parent = dev;
+	indio_dev->name = name;
+	indio_dev->info = &ad5380_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->num_channels = st->chip_info->num_channels;
+
+	ret = ad5380_alloc_channels(indio_dev);
+	if (ret) {
+		dev_err(dev, "Failed to allocate channel spec: %d\n", ret);
+		goto error_free;
+	}
+
+	if (st->chip_info->int_vref == 2500000)
+		ctrl |= AD5380_CTRL_INT_VREF_2V5;
+
+	st->vref_reg = regulator_get(dev, "vref");
+	if (!IS_ERR(st->vref_reg)) {
+		ret = regulator_enable(st->vref_reg);
+		if (ret) {
+			dev_err(dev, "Failed to enable vref regulators: %d\n",
+				ret);
+			goto error_free_reg;
+		}
+
+		st->vref = regulator_get_voltage(st->vref_reg);
+	} else {
+		st->vref = st->chip_info->int_vref;
+		ctrl |= AD5380_CTRL_INT_VREF_EN;
+	}
+
+	ret = regmap_write(st->regmap, AD5380_REG_SF_CTRL, ctrl);
+	if (ret) {
+		dev_err(dev, "Failed to write to device: %d\n", ret);
+		goto error_disable_reg;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(dev, "Failed to register iio device: %d\n", ret);
+		goto error_disable_reg;
+	}
+
+	return 0;
+
+error_disable_reg:
+	if (!IS_ERR(st->vref_reg))
+		regulator_disable(st->vref_reg);
+error_free_reg:
+	if (!IS_ERR(st->vref_reg))
+		regulator_put(st->vref_reg);
+
+	kfree(indio_dev->channels);
+error_free:
+	iio_free_device(indio_dev);
+error_regmap_exit:
+	regmap_exit(regmap);
+
+	return ret;
+}
+
+static int __devexit ad5380_remove(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ad5380_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	kfree(indio_dev->channels);
+
+	if (!IS_ERR(st->vref_reg)) {
+		regulator_disable(st->vref_reg);
+		regulator_put(st->vref_reg);
+	}
+
+	regmap_exit(st->regmap);
+	iio_free_device(indio_dev);
+
+	return 0;
+}
+
+static bool ad5380_reg_false(struct device *dev, unsigned int reg)
+{
+	return false;
+}
+
+static const struct regmap_config ad5380_regmap_config = {
+	.reg_bits = 10,
+	.val_bits = 14,
+
+	.max_register = AD5380_REG_DATA(40),
+	.cache_type = REGCACHE_RBTREE,
+
+	.volatile_reg = ad5380_reg_false,
+	.readable_reg = ad5380_reg_false,
+};
+
+#if IS_ENABLED(CONFIG_SPI_MASTER)
+
+static int __devinit ad5380_spi_probe(struct spi_device *spi)
+{
+	const struct spi_device_id *id = spi_get_device_id(spi);
+	struct regmap *regmap;
+
+	regmap = regmap_init_spi(spi, &ad5380_regmap_config);
+
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return ad5380_probe(&spi->dev, regmap, id->driver_data, id->name);
+}
+
+static int __devexit ad5380_spi_remove(struct spi_device *spi)
+{
+	return ad5380_remove(&spi->dev);
+}
+
+static const struct spi_device_id ad5380_spi_ids[] = {
+	{ "ad5380-3", ID_AD5380_3 },
+	{ "ad5380-5", ID_AD5380_5 },
+	{ "ad5381-3", ID_AD5381_3 },
+	{ "ad5381-5", ID_AD5381_5 },
+	{ "ad5382-3", ID_AD5382_3 },
+	{ "ad5382-5", ID_AD5382_5 },
+	{ "ad5383-3", ID_AD5383_3 },
+	{ "ad5383-5", ID_AD5383_5 },
+	{ "ad5384-3", ID_AD5380_3 },
+	{ "ad5384-5", ID_AD5380_5 },
+	{ "ad5390-3", ID_AD5390_3 },
+	{ "ad5390-5", ID_AD5390_5 },
+	{ "ad5391-3", ID_AD5391_3 },
+	{ "ad5391-5", ID_AD5391_5 },
+	{ "ad5392-3", ID_AD5392_3 },
+	{ "ad5392-5", ID_AD5392_5 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, ad5380_spi_ids);
+
+static struct spi_driver ad5380_spi_driver = {
+	.driver = {
+		   .name = "ad5380",
+		   .owner = THIS_MODULE,
+	},
+	.probe = ad5380_spi_probe,
+	.remove = __devexit_p(ad5380_spi_remove),
+	.id_table = ad5380_spi_ids,
+};
+
+static inline int ad5380_spi_register_driver(void)
+{
+	return spi_register_driver(&ad5380_spi_driver);
+}
+
+static inline void ad5380_spi_unregister_driver(void)
+{
+	spi_unregister_driver(&ad5380_spi_driver);
+}
+
+#else
+
+static inline int ad5380_spi_register_driver(void)
+{
+	return 0;
+}
+
+static inline void ad5380_spi_unregister_driver(void)
+{
+}
+
+#endif
+
+#if IS_ENABLED(CONFIG_I2C)
+
+static int __devinit ad5380_i2c_probe(struct i2c_client *i2c,
+	const struct i2c_device_id *id)
+{
+	struct regmap *regmap;
+
+	regmap = regmap_init_i2c(i2c, &ad5380_regmap_config);
+
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return ad5380_probe(&i2c->dev, regmap, id->driver_data, id->name);
+}
+
+static int __devexit ad5380_i2c_remove(struct i2c_client *i2c)
+{
+	return ad5380_remove(&i2c->dev);
+}
+
+static const struct i2c_device_id ad5380_i2c_ids[] = {
+	{ "ad5380-3", ID_AD5380_3 },
+	{ "ad5380-5", ID_AD5380_5 },
+	{ "ad5381-3", ID_AD5381_3 },
+	{ "ad5381-5", ID_AD5381_5 },
+	{ "ad5382-3", ID_AD5382_3 },
+	{ "ad5382-5", ID_AD5382_5 },
+	{ "ad5383-3", ID_AD5383_3 },
+	{ "ad5383-5", ID_AD5383_5 },
+	{ "ad5384-3", ID_AD5380_3 },
+	{ "ad5384-5", ID_AD5380_5 },
+	{ "ad5390-3", ID_AD5390_3 },
+	{ "ad5390-5", ID_AD5390_5 },
+	{ "ad5391-3", ID_AD5391_3 },
+	{ "ad5391-5", ID_AD5391_5 },
+	{ "ad5392-3", ID_AD5392_3 },
+	{ "ad5392-5", ID_AD5392_5 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ad5380_i2c_ids);
+
+static struct i2c_driver ad5380_i2c_driver = {
+	.driver = {
+		   .name = "ad5380",
+		   .owner = THIS_MODULE,
+	},
+	.probe = ad5380_i2c_probe,
+	.remove = __devexit_p(ad5380_i2c_remove),
+	.id_table = ad5380_i2c_ids,
+};
+
+static inline int ad5380_i2c_register_driver(void)
+{
+	return i2c_add_driver(&ad5380_i2c_driver);
+}
+
+static inline void ad5380_i2c_unregister_driver(void)
+{
+	i2c_del_driver(&ad5380_i2c_driver);
+}
+
+#else
+
+static inline int ad5380_i2c_register_driver(void)
+{
+	return 0;
+}
+
+static inline void ad5380_i2c_unregister_driver(void)
+{
+}
+
+#endif
+
+static int __init ad5380_spi_init(void)
+{
+	int ret;
+
+	ret = ad5380_spi_register_driver();
+	if (ret)
+		return ret;
+
+	ret = ad5380_i2c_register_driver();
+	if (ret) {
+		ad5380_spi_unregister_driver();
+		return ret;
+	}
+
+	return 0;
+}
+module_init(ad5380_spi_init);
+
+static void __exit ad5380_spi_exit(void)
+{
+	ad5380_i2c_unregister_driver();
+	ad5380_spi_unregister_driver();
+
+}
+module_exit(ad5380_spi_exit);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Analog Devices AD5380/81/82/83/84/90/91/92 DAC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/dac/ad5421.c b/drivers/staging/iio/dac/ad5421.c
new file mode 100644
index 000000000000..71ee86824763
--- /dev/null
+++ b/drivers/staging/iio/dac/ad5421.c
@@ -0,0 +1,555 @@
+/*
+ * AD5421 Digital to analog converters  driver
+ *
+ * Copyright 2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/spi/spi.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+
+#include "../iio.h"
+#include "../sysfs.h"
+#include "../events.h"
+#include "dac.h"
+#include "ad5421.h"
+
+
+#define AD5421_REG_DAC_DATA		0x1
+#define AD5421_REG_CTRL			0x2
+#define AD5421_REG_OFFSET		0x3
+#define AD5421_REG_GAIN			0x4
+/* load dac and fault shared the same register number. Writing to it will cause
+ * a dac load command, reading from it will return the fault status register */
+#define AD5421_REG_LOAD_DAC		0x5
+#define AD5421_REG_FAULT		0x5
+#define AD5421_REG_FORCE_ALARM_CURRENT	0x6
+#define AD5421_REG_RESET		0x7
+#define AD5421_REG_START_CONVERSION	0x8
+#define AD5421_REG_NOOP			0x9
+
+#define AD5421_CTRL_WATCHDOG_DISABLE	BIT(12)
+#define AD5421_CTRL_AUTO_FAULT_READBACK	BIT(11)
+#define AD5421_CTRL_MIN_CURRENT		BIT(9)
+#define AD5421_CTRL_ADC_SOURCE_TEMP	BIT(8)
+#define AD5421_CTRL_ADC_ENABLE		BIT(7)
+#define AD5421_CTRL_PWR_DOWN_INT_VREF	BIT(6)
+
+#define AD5421_FAULT_SPI			BIT(15)
+#define AD5421_FAULT_PEC			BIT(14)
+#define AD5421_FAULT_OVER_CURRENT		BIT(13)
+#define AD5421_FAULT_UNDER_CURRENT		BIT(12)
+#define AD5421_FAULT_TEMP_OVER_140		BIT(11)
+#define AD5421_FAULT_TEMP_OVER_100		BIT(10)
+#define AD5421_FAULT_UNDER_VOLTAGE_6V		BIT(9)
+#define AD5421_FAULT_UNDER_VOLTAGE_12V		BIT(8)
+
+/* These bits will cause the fault pin to go high */
+#define AD5421_FAULT_TRIGGER_IRQ \
+	(AD5421_FAULT_SPI | AD5421_FAULT_PEC | AD5421_FAULT_OVER_CURRENT | \
+	AD5421_FAULT_UNDER_CURRENT | AD5421_FAULT_TEMP_OVER_140)
+
+/**
+ * struct ad5421_state - driver instance specific data
+ * @spi:		spi_device
+ * @ctrl:		control register cache
+ * @current_range:	current range which the device is configured for
+ * @data:		spi transfer buffers
+ * @fault_mask:		software masking of events
+ */
+struct ad5421_state {
+	struct spi_device		*spi;
+	unsigned int			ctrl;
+	enum ad5421_current_range	current_range;
+	unsigned int			fault_mask;
+
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	union {
+		u32 d32;
+		u8 d8[4];
+	} data[2] ____cacheline_aligned;
+};
+
+static const struct iio_chan_spec ad5421_channels[] = {
+	{
+		.type = IIO_CURRENT,
+		.indexed = 1,
+		.output = 1,
+		.channel = 0,
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+			IIO_CHAN_INFO_OFFSET_SHARED_BIT |
+			IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |
+			IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
+		.scan_type = IIO_ST('u', 16, 16, 0),
+		.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING) |
+			IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING),
+	},
+	{
+		.type = IIO_TEMP,
+		.channel = -1,
+		.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING),
+	},
+};
+
+static int ad5421_write_unlocked(struct iio_dev *indio_dev,
+	unsigned int reg, unsigned int val)
+{
+	struct ad5421_state *st = iio_priv(indio_dev);
+
+	st->data[0].d32 = cpu_to_be32((reg << 16) | val);
+
+	return spi_write(st->spi, &st->data[0].d8[1], 3);
+}
+
+static int ad5421_write(struct iio_dev *indio_dev, unsigned int reg,
+	unsigned int val)
+{
+	int ret;
+
+	mutex_lock(&indio_dev->mlock);
+	ret = ad5421_write_unlocked(indio_dev, reg, val);
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static int ad5421_read(struct iio_dev *indio_dev, unsigned int reg)
+{
+	struct ad5421_state *st = iio_priv(indio_dev);
+	struct spi_message m;
+	int ret;
+	struct spi_transfer t[] = {
+		{
+			.tx_buf = &st->data[0].d8[1],
+			.len = 3,
+			.cs_change = 1,
+		}, {
+			.rx_buf = &st->data[1].d8[1],
+			.len = 3,
+		},
+	};
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+
+	mutex_lock(&indio_dev->mlock);
+
+	st->data[0].d32 = cpu_to_be32((1 << 23) | (reg << 16));
+
+	ret = spi_sync(st->spi, &m);
+	if (ret >= 0)
+		ret = be32_to_cpu(st->data[1].d32) & 0xffff;
+
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static int ad5421_update_ctrl(struct iio_dev *indio_dev, unsigned int set,
+	unsigned int clr)
+{
+	struct ad5421_state *st = iio_priv(indio_dev);
+	unsigned int ret;
+
+	mutex_lock(&indio_dev->mlock);
+
+	st->ctrl &= ~clr;
+	st->ctrl |= set;
+
+	ret = ad5421_write_unlocked(indio_dev, AD5421_REG_CTRL, st->ctrl);
+
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static irqreturn_t ad5421_fault_handler(int irq, void *data)
+{
+	struct iio_dev *indio_dev = data;
+	struct ad5421_state *st = iio_priv(indio_dev);
+	unsigned int fault;
+	unsigned int old_fault = 0;
+	unsigned int events;
+
+	fault = ad5421_read(indio_dev, AD5421_REG_FAULT);
+	if (!fault)
+		return IRQ_NONE;
+
+	/* If we had a fault, this might mean that the DAC has lost its state
+	 * and has been reset. Make sure that the control register actually
+	 * contains what we expect it to contain. Otherwise the watchdog might
+	 * be enabled and we get watchdog timeout faults, which will render the
+	 * DAC unusable. */
+	ad5421_update_ctrl(indio_dev, 0, 0);
+
+
+	/* The fault pin stays high as long as a fault condition is present and
+	 * it is not possible to mask fault conditions. For certain fault
+	 * conditions for example like over-temperature it takes some time
+	 * until the fault condition disappears. If we would exit the interrupt
+	 * handler immediately after handling the event it would be entered
+	 * again instantly. Thus we fall back to polling in case we detect that
+	 * a interrupt condition is still present.
+	 */
+	do {
+		/* 0xffff is a invalid value for the register and will only be
+		 * read if there has been a communication error */
+		if (fault == 0xffff)
+			fault = 0;
+
+		/* we are only interested in new events */
+		events = (old_fault ^ fault) & fault;
+		events &= st->fault_mask;
+
+		if (events & AD5421_FAULT_OVER_CURRENT) {
+			iio_push_event(indio_dev,
+				IIO_UNMOD_EVENT_CODE(IIO_CURRENT,
+					0,
+					IIO_EV_TYPE_THRESH,
+					IIO_EV_DIR_RISING),
+			iio_get_time_ns());
+		}
+
+		if (events & AD5421_FAULT_UNDER_CURRENT) {
+			iio_push_event(indio_dev,
+				IIO_UNMOD_EVENT_CODE(IIO_CURRENT,
+					0,
+					IIO_EV_TYPE_THRESH,
+					IIO_EV_DIR_FALLING),
+				iio_get_time_ns());
+		}
+
+		if (events & AD5421_FAULT_TEMP_OVER_140) {
+			iio_push_event(indio_dev,
+				IIO_UNMOD_EVENT_CODE(IIO_TEMP,
+					0,
+					IIO_EV_TYPE_MAG,
+					IIO_EV_DIR_RISING),
+				iio_get_time_ns());
+		}
+
+		old_fault = fault;
+		fault = ad5421_read(indio_dev, AD5421_REG_FAULT);
+
+		/* still active? go to sleep for some time */
+		if (fault & AD5421_FAULT_TRIGGER_IRQ)
+			msleep(1000);
+
+	} while (fault & AD5421_FAULT_TRIGGER_IRQ);
+
+
+	return IRQ_HANDLED;
+}
+
+static void ad5421_get_current_min_max(struct ad5421_state *st,
+	unsigned int *min, unsigned int *max)
+{
+	/* The current range is configured using external pins, which are
+	 * usually hard-wired and not run-time switchable. */
+	switch (st->current_range) {
+	case AD5421_CURRENT_RANGE_4mA_20mA:
+		*min = 4000;
+		*max = 20000;
+		break;
+	case AD5421_CURRENT_RANGE_3mA8_21mA:
+		*min = 3800;
+		*max = 21000;
+		break;
+	case AD5421_CURRENT_RANGE_3mA2_24mA:
+		*min = 3200;
+		*max = 24000;
+		break;
+	default:
+		*min = 0;
+		*max = 1;
+		break;
+	}
+}
+
+static inline unsigned int ad5421_get_offset(struct ad5421_state *st)
+{
+	unsigned int min, max;
+
+	ad5421_get_current_min_max(st, &min, &max);
+	return (min * (1 << 16)) / (max - min);
+}
+
+static inline unsigned int ad5421_get_scale(struct ad5421_state *st)
+{
+	unsigned int min, max;
+
+	ad5421_get_current_min_max(st, &min, &max);
+	return ((max - min) * 1000) / (1 << 16);
+}
+
+static int ad5421_read_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int *val, int *val2, long m)
+{
+	struct ad5421_state *st = iio_priv(indio_dev);
+	int ret;
+
+	if (chan->type != IIO_CURRENT)
+		return -EINVAL;
+
+	switch (m) {
+	case 0:
+		ret = ad5421_read(indio_dev, AD5421_REG_DAC_DATA);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = ad5421_get_scale(st);
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = ad5421_get_offset(st);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		ret = ad5421_read(indio_dev, AD5421_REG_OFFSET);
+		if (ret < 0)
+			return ret;
+		*val = ret - 32768;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBSCALE:
+		ret = ad5421_read(indio_dev, AD5421_REG_GAIN);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		return IIO_VAL_INT;
+	}
+
+	return -EINVAL;
+}
+
+static int ad5421_write_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int val, int val2, long mask)
+{
+	const unsigned int max_val = 1 << 16;
+
+	switch (mask) {
+	case 0:
+		if (val >= max_val || val < 0)
+			return -EINVAL;
+
+		return ad5421_write(indio_dev, AD5421_REG_DAC_DATA, val);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		val += 32768;
+		if (val >= max_val || val < 0)
+			return -EINVAL;
+
+		return ad5421_write(indio_dev, AD5421_REG_OFFSET, val);
+	case IIO_CHAN_INFO_CALIBSCALE:
+		if (val >= max_val || val < 0)
+			return -EINVAL;
+
+		return ad5421_write(indio_dev, AD5421_REG_GAIN, val);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static int ad5421_write_event_config(struct iio_dev *indio_dev,
+	u64 event_code, int state)
+{
+	struct ad5421_state *st = iio_priv(indio_dev);
+	unsigned int mask;
+
+	switch (IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event_code)) {
+	case IIO_CURRENT:
+		if (IIO_EVENT_CODE_EXTRACT_DIR(event_code) ==
+			IIO_EV_DIR_RISING)
+			mask = AD5421_FAULT_OVER_CURRENT;
+		else
+			mask = AD5421_FAULT_UNDER_CURRENT;
+		break;
+	case IIO_TEMP:
+		mask = AD5421_FAULT_TEMP_OVER_140;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mutex_lock(&indio_dev->mlock);
+	if (state)
+		st->fault_mask |= mask;
+	else
+		st->fault_mask &= ~mask;
+	mutex_unlock(&indio_dev->mlock);
+
+	return 0;
+}
+
+static int ad5421_read_event_config(struct iio_dev *indio_dev,
+	u64 event_code)
+{
+	struct ad5421_state *st = iio_priv(indio_dev);
+	unsigned int mask;
+
+	switch (IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event_code)) {
+	case IIO_CURRENT:
+		if (IIO_EVENT_CODE_EXTRACT_DIR(event_code) ==
+			IIO_EV_DIR_RISING)
+			mask = AD5421_FAULT_OVER_CURRENT;
+		else
+			mask = AD5421_FAULT_UNDER_CURRENT;
+		break;
+	case IIO_TEMP:
+		mask = AD5421_FAULT_TEMP_OVER_140;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return (bool)(st->fault_mask & mask);
+}
+
+static int ad5421_read_event_value(struct iio_dev *indio_dev, u64 event_code,
+	int *val)
+{
+	int ret;
+
+	switch (IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(event_code)) {
+	case IIO_CURRENT:
+		ret = ad5421_read(indio_dev, AD5421_REG_DAC_DATA);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		break;
+	case IIO_TEMP:
+		*val = 140000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct iio_info ad5421_info = {
+	.read_raw =		ad5421_read_raw,
+	.write_raw =		ad5421_write_raw,
+	.read_event_config =	ad5421_read_event_config,
+	.write_event_config =	ad5421_write_event_config,
+	.read_event_value =	ad5421_read_event_value,
+	.driver_module =	THIS_MODULE,
+};
+
+static int __devinit ad5421_probe(struct spi_device *spi)
+{
+	struct ad5421_platform_data *pdata = dev_get_platdata(&spi->dev);
+	struct iio_dev *indio_dev;
+	struct ad5421_state *st;
+	int ret;
+
+	indio_dev = iio_allocate_device(sizeof(*st));
+	if (indio_dev == NULL) {
+		dev_err(&spi->dev, "Failed to allocate iio device\n");
+		return  -ENOMEM;
+	}
+
+	st = iio_priv(indio_dev);
+	spi_set_drvdata(spi, indio_dev);
+
+	st->spi = spi;
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = "ad5421";
+	indio_dev->info = &ad5421_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = ad5421_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ad5421_channels);
+
+	st->ctrl = AD5421_CTRL_WATCHDOG_DISABLE |
+			AD5421_CTRL_AUTO_FAULT_READBACK;
+
+	if (pdata) {
+		st->current_range = pdata->current_range;
+		if (pdata->external_vref)
+			st->ctrl |= AD5421_CTRL_PWR_DOWN_INT_VREF;
+	} else {
+		st->current_range = AD5421_CURRENT_RANGE_4mA_20mA;
+	}
+
+	/* write initial ctrl register value */
+	ad5421_update_ctrl(indio_dev, 0, 0);
+
+	if (spi->irq) {
+		ret = request_threaded_irq(spi->irq,
+					   NULL,
+					   ad5421_fault_handler,
+					   IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+					   "ad5421 fault",
+					   indio_dev);
+		if (ret)
+			goto error_free;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to register iio device: %d\n", ret);
+		goto error_free_irq;
+	}
+
+	return 0;
+
+error_free_irq:
+	if (spi->irq)
+		free_irq(spi->irq, indio_dev);
+error_free:
+	iio_free_device(indio_dev);
+
+	return ret;
+}
+
+static int __devexit ad5421_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+
+	iio_device_unregister(indio_dev);
+	if (spi->irq)
+		free_irq(spi->irq, indio_dev);
+	iio_free_device(indio_dev);
+
+	return 0;
+}
+
+static struct spi_driver ad5421_driver = {
+	.driver = {
+		   .name = "ad5421",
+		   .owner = THIS_MODULE,
+	},
+	.probe = ad5421_probe,
+	.remove = __devexit_p(ad5421_remove),
+};
+
+static __init int ad5421_init(void)
+{
+	return spi_register_driver(&ad5421_driver);
+}
+module_init(ad5421_init);
+
+static __exit void ad5421_exit(void)
+{
+	spi_unregister_driver(&ad5421_driver);
+}
+module_exit(ad5421_exit);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Analog Devices AD5421 DAC");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:ad5421");
diff --git a/drivers/staging/iio/dac/ad5421.h b/drivers/staging/iio/dac/ad5421.h
new file mode 100644
index 000000000000..cd2bb84ff1b0
--- /dev/null
+++ b/drivers/staging/iio/dac/ad5421.h
@@ -0,0 +1,32 @@
+#ifndef __IIO_DAC_AD5421_H__
+#define __IIO_DAC_AD5421_H__
+
+/*
+ * TODO: This file needs to go into include/linux/iio
+ */
+
+/**
+ * enum ad5421_current_range - Current range the AD5421 is configured for.
+ * @AD5421_CURRENT_RANGE_4mA_20mA: 4 mA to 20 mA (RANGE1,0 pins = 00)
+ * @AD5421_CURRENT_RANGE_3mA8_21mA: 3.8 mA to 21 mA (RANGE1,0 pins = x1)
+ * @AD5421_CURRENT_RANGE_3mA2_24mA: 3.2 mA to 24 mA (RANGE1,0 pins = 10)
+ */
+
+enum ad5421_current_range {
+	AD5421_CURRENT_RANGE_4mA_20mA,
+	AD5421_CURRENT_RANGE_3mA8_21mA,
+	AD5421_CURRENT_RANGE_3mA2_24mA,
+};
+
+/**
+ * struct ad5421_platform_data - AD5421 DAC driver platform data
+ * @external_vref: whether an external reference voltage is used or not
+ * @current_range: Current range the AD5421 is configured for
+ */
+
+struct ad5421_platform_data {
+	bool external_vref;
+	enum ad5421_current_range current_range;
+};
+
+#endif
diff --git a/drivers/staging/iio/dac/ad5446.c b/drivers/staging/iio/dac/ad5446.c
index ec701e939b6b..693e7482524c 100644
--- a/drivers/staging/iio/dac/ad5446.c
+++ b/drivers/staging/iio/dac/ad5446.c
@@ -26,19 +26,17 @@
 
 static void ad5446_store_sample(struct ad5446_state *st, unsigned val)
 {
-	st->data.d16 = cpu_to_be16(AD5446_LOAD |
-					(val << st->chip_info->left_shift));
+	st->data.d16 = cpu_to_be16(AD5446_LOAD | val);
 }
 
 static void ad5542_store_sample(struct ad5446_state *st, unsigned val)
 {
-	st->data.d16 = cpu_to_be16(val << st->chip_info->left_shift);
+	st->data.d16 = cpu_to_be16(val);
 }
 
 static void ad5620_store_sample(struct ad5446_state *st, unsigned val)
 {
-	st->data.d16 = cpu_to_be16(AD5620_LOAD |
-					(val << st->chip_info->left_shift));
+	st->data.d16 = cpu_to_be16(AD5620_LOAD | val);
 }
 
 static void ad5660_store_sample(struct ad5446_state *st, unsigned val)
@@ -63,50 +61,6 @@ static void ad5660_store_pwr_down(struct ad5446_state *st, unsigned mode)
 	st->data.d24[2] = val & 0xFF;
 }
 
-static ssize_t ad5446_write(struct device *dev,
-		struct device_attribute *attr,
-		const char *buf,
-		size_t len)
-{
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	struct ad5446_state *st = iio_priv(indio_dev);
-	int ret;
-	long val;
-
-	ret = strict_strtol(buf, 10, &val);
-	if (ret)
-		goto error_ret;
-
-	if (val > RES_MASK(st->chip_info->bits)) {
-		ret = -EINVAL;
-		goto error_ret;
-	}
-
-	mutex_lock(&indio_dev->mlock);
-	st->cached_val = val;
-	st->chip_info->store_sample(st, val);
-	ret = spi_sync(st->spi, &st->msg);
-	mutex_unlock(&indio_dev->mlock);
-
-error_ret:
-	return ret ? ret : len;
-}
-
-static IIO_DEV_ATTR_OUT_RAW(0, ad5446_write, 0);
-
-static ssize_t ad5446_show_scale(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
-{
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	struct ad5446_state *st = iio_priv(indio_dev);
-	/* Corresponds to Vref / 2^(bits) */
-	unsigned int scale_uv = (st->vref_mv * 1000) >> st->chip_info->bits;
-
-	return sprintf(buf, "%d.%03d\n", scale_uv / 1000, scale_uv % 1000);
-}
-static IIO_DEVICE_ATTR(out_voltage_scale, S_IRUGO, ad5446_show_scale, NULL, 0);
-
 static ssize_t ad5446_write_powerdown_mode(struct device *dev,
 				       struct device_attribute *attr,
 				       const char *buf, size_t len)
@@ -189,8 +143,6 @@ static IIO_DEVICE_ATTR(out_voltage0_powerdown, S_IRUGO | S_IWUSR,
 			ad5446_write_dac_powerdown, 0);
 
 static struct attribute *ad5446_attributes[] = {
-	&iio_dev_attr_out_voltage0_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage_scale.dev_attr.attr,
 	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
 	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
@@ -223,121 +175,148 @@ static const struct attribute_group ad5446_attribute_group = {
 	.is_visible = ad5446_attr_is_visible,
 };
 
+#define AD5446_CHANNEL(bits, storage, shift) { \
+	.type = IIO_VOLTAGE, \
+	.indexed = 1, \
+	.output = 1, \
+	.channel = 0, \
+	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.scan_type = IIO_ST('u', (bits), (storage), (shift)) \
+}
+
 static const struct ad5446_chip_info ad5446_chip_info_tbl[] = {
 	[ID_AD5444] = {
-		.bits = 12,
-		.storagebits = 16,
-		.left_shift = 2,
+		.channel = AD5446_CHANNEL(12, 16, 2),
 		.store_sample = ad5446_store_sample,
 	},
 	[ID_AD5446] = {
-		.bits = 14,
-		.storagebits = 16,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(14, 16, 0),
 		.store_sample = ad5446_store_sample,
 	},
 	[ID_AD5541A] = {
-		.bits = 16,
-		.storagebits = 16,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(16, 16, 0),
 		.store_sample = ad5542_store_sample,
 	},
 	[ID_AD5542A] = {
-		.bits = 16,
-		.storagebits = 16,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(16, 16, 0),
 		.store_sample = ad5542_store_sample,
 	},
 	[ID_AD5543] = {
-		.bits = 16,
-		.storagebits = 16,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(16, 16, 0),
 		.store_sample = ad5542_store_sample,
 	},
 	[ID_AD5512A] = {
-		.bits = 12,
-		.storagebits = 16,
-		.left_shift = 4,
+		.channel = AD5446_CHANNEL(12, 16, 4),
 		.store_sample = ad5542_store_sample,
 	},
 	[ID_AD5553] = {
-		.bits = 14,
-		.storagebits = 16,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(14, 16, 0),
 		.store_sample = ad5542_store_sample,
 	},
 	[ID_AD5601] = {
-		.bits = 8,
-		.storagebits = 16,
-		.left_shift = 6,
+		.channel = AD5446_CHANNEL(8, 16, 6),
 		.store_sample = ad5542_store_sample,
 		.store_pwr_down = ad5620_store_pwr_down,
 	},
 	[ID_AD5611] = {
-		.bits = 10,
-		.storagebits = 16,
-		.left_shift = 4,
+		.channel = AD5446_CHANNEL(10, 16, 4),
 		.store_sample = ad5542_store_sample,
 		.store_pwr_down = ad5620_store_pwr_down,
 	},
 	[ID_AD5621] = {
-		.bits = 12,
-		.storagebits = 16,
-		.left_shift = 2,
+		.channel = AD5446_CHANNEL(12, 16, 2),
 		.store_sample = ad5542_store_sample,
 		.store_pwr_down = ad5620_store_pwr_down,
 	},
 	[ID_AD5620_2500] = {
-		.bits = 12,
-		.storagebits = 16,
-		.left_shift = 2,
+		.channel = AD5446_CHANNEL(12, 16, 2),
 		.int_vref_mv = 2500,
 		.store_sample = ad5620_store_sample,
 		.store_pwr_down = ad5620_store_pwr_down,
 	},
 	[ID_AD5620_1250] = {
-		.bits = 12,
-		.storagebits = 16,
-		.left_shift = 2,
+		.channel = AD5446_CHANNEL(12, 16, 2),
 		.int_vref_mv = 1250,
 		.store_sample = ad5620_store_sample,
 		.store_pwr_down = ad5620_store_pwr_down,
 	},
 	[ID_AD5640_2500] = {
-		.bits = 14,
-		.storagebits = 16,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(14, 16, 0),
 		.int_vref_mv = 2500,
 		.store_sample = ad5620_store_sample,
 		.store_pwr_down = ad5620_store_pwr_down,
 	},
 	[ID_AD5640_1250] = {
-		.bits = 14,
-		.storagebits = 16,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(14, 16, 0),
 		.int_vref_mv = 1250,
 		.store_sample = ad5620_store_sample,
 		.store_pwr_down = ad5620_store_pwr_down,
 	},
 	[ID_AD5660_2500] = {
-		.bits = 16,
-		.storagebits = 24,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(16, 16, 0),
 		.int_vref_mv = 2500,
 		.store_sample = ad5660_store_sample,
 		.store_pwr_down = ad5660_store_pwr_down,
 	},
 	[ID_AD5660_1250] = {
-		.bits = 16,
-		.storagebits = 24,
-		.left_shift = 0,
+		.channel = AD5446_CHANNEL(16, 16, 0),
 		.int_vref_mv = 1250,
 		.store_sample = ad5660_store_sample,
 		.store_pwr_down = ad5660_store_pwr_down,
 	},
 };
 
+static int ad5446_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
+{
+	struct ad5446_state *st = iio_priv(indio_dev);
+	unsigned long scale_uv;
+
+	switch (m) {
+	case IIO_CHAN_INFO_SCALE:
+		scale_uv = (st->vref_mv * 1000) >> chan->scan_type.realbits;
+		*val =  scale_uv / 1000;
+		*val2 = (scale_uv % 1000) * 1000;
+		return IIO_VAL_INT_PLUS_MICRO;
+
+	}
+	return -EINVAL;
+}
+
+static int ad5446_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct ad5446_state *st = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case 0:
+		if (val >= (1 << chan->scan_type.realbits) || val < 0)
+			return -EINVAL;
+
+		val <<= chan->scan_type.shift;
+		mutex_lock(&indio_dev->mlock);
+		st->cached_val = val;
+		st->chip_info->store_sample(st, val);
+		ret = spi_sync(st->spi, &st->msg);
+		mutex_unlock(&indio_dev->mlock);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
 static const struct iio_info ad5446_info = {
+	.read_raw = ad5446_read_raw,
+	.write_raw = ad5446_write_raw,
 	.attrs = &ad5446_attribute_group,
 	.driver_module = THIS_MODULE,
 };
@@ -376,11 +355,13 @@ static int __devinit ad5446_probe(struct spi_device *spi)
 	indio_dev->name = spi_get_device_id(spi)->name;
 	indio_dev->info = &ad5446_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = &st->chip_info->channel;
+	indio_dev->num_channels = 1;
 
 	/* Setup default message */
 
 	st->xfer.tx_buf = &st->data;
-	st->xfer.len = st->chip_info->storagebits / 8;
+	st->xfer.len = st->chip_info->channel.scan_type.storagebits / 8;
 
 	spi_message_init(&st->msg);
 	spi_message_add_tail(&st->xfer, &st->msg);
@@ -454,11 +435,11 @@ static const struct spi_device_id ad5446_id[] = {
 	{"ad5660-1250", ID_AD5660_1250},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad5446_id);
 
 static struct spi_driver ad5446_driver = {
 	.driver = {
 		.name	= "ad5446",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad5446_probe,
@@ -470,4 +451,3 @@ module_spi_driver(ad5446_driver);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD5444/AD5446 DAC");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:ad5446");
diff --git a/drivers/staging/iio/dac/ad5446.h b/drivers/staging/iio/dac/ad5446.h
index 7118d653ac3e..4ea3476fb065 100644
--- a/drivers/staging/iio/dac/ad5446.h
+++ b/drivers/staging/iio/dac/ad5446.h
@@ -25,8 +25,6 @@
 #define AD5660_PWRDWN_100k	(0x2 << 16) /* Power-down: 100kOhm to GND */
 #define AD5660_PWRDWN_TRISTATE	(0x3 << 16) /* Power-down: Three-state */
 
-#define RES_MASK(bits)	((1 << (bits)) - 1)
-
 #define MODE_PWRDWN_1k		0x1
 #define MODE_PWRDWN_100k	0x2
 #define MODE_PWRDWN_TRISTATE	0x3
@@ -62,18 +60,14 @@ struct ad5446_state {
 
 /**
  * struct ad5446_chip_info - chip specific information
- * @bits:		accuracy of the DAC in bits
- * @storagebits:	number of bits written to the DAC
- * @left_shift:		number of bits the datum must be shifted
+ * @channel:		channel spec for the DAC
  * @int_vref_mv:	AD5620/40/60: the internal reference voltage
  * @store_sample:	chip specific helper function to store the datum
  * @store_sample:	chip specific helper function to store the powerpown cmd
  */
 
 struct ad5446_chip_info {
-	u8			bits;
-	u8			storagebits;
-	u8			left_shift;
+	struct iio_chan_spec	channel;
 	u16			int_vref_mv;
 	void (*store_sample)	(struct ad5446_state *st, unsigned val);
 	void (*store_pwr_down)	(struct ad5446_state *st, unsigned mode);
diff --git a/drivers/staging/iio/dac/ad5504.c b/drivers/staging/iio/dac/ad5504.c
index 57539ce8e6cf..bc17205fe722 100644
--- a/drivers/staging/iio/dac/ad5504.c
+++ b/drivers/staging/iio/dac/ad5504.c
@@ -18,9 +18,27 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
+#include "../events.h"
 #include "dac.h"
 #include "ad5504.h"
 
+#define AD5504_CHANNEL(_chan) { \
+	.type = IIO_VOLTAGE, \
+	.indexed = 1, \
+	.output = 1, \
+	.channel = (_chan), \
+	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.address = AD5504_ADDR_DAC(_chan), \
+	.scan_type = IIO_ST('u', 12, 16, 0), \
+}
+
+static const struct iio_chan_spec ad5504_channels[] = {
+	AD5504_CHANNEL(0),
+	AD5504_CHANNEL(1),
+	AD5504_CHANNEL(2),
+	AD5504_CHANNEL(3),
+};
+
 static int ad5504_spi_write(struct spi_device *spi, u8 addr, u16 val)
 {
 	u16 tmp = cpu_to_be16(AD5504_CMD_WRITE |
@@ -30,13 +48,14 @@ static int ad5504_spi_write(struct spi_device *spi, u8 addr, u16 val)
 	return spi_write(spi, (u8 *)&tmp, 2);
 }
 
-static int ad5504_spi_read(struct spi_device *spi, u8 addr, u16 *val)
+static int ad5504_spi_read(struct spi_device *spi, u8 addr)
 {
 	u16 tmp = cpu_to_be16(AD5504_CMD_READ | AD5504_ADDR(addr));
+	u16 val;
 	int ret;
 	struct spi_transfer	t = {
 			.tx_buf		= &tmp,
-			.rx_buf		= val,
+			.rx_buf		= &val,
 			.len		= 2,
 		};
 	struct spi_message	m;
@@ -45,44 +64,61 @@ static int ad5504_spi_read(struct spi_device *spi, u8 addr, u16 *val)
 	spi_message_add_tail(&t, &m);
 	ret = spi_sync(spi, &m);
 
-	*val = be16_to_cpu(*val) & AD5504_RES_MASK;
+	if (ret < 0)
+		return ret;
 
-	return ret;
+	return be16_to_cpu(val) & AD5504_RES_MASK;
 }
 
-static ssize_t ad5504_write_dac(struct device *dev,
-				 struct device_attribute *attr,
-				 const char *buf, size_t len)
+static int ad5504_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	long readin;
+	unsigned long scale_uv;
 	int ret;
 
-	ret = strict_strtol(buf, 10, &readin);
-	if (ret)
-		return ret;
+	switch (m) {
+	case 0:
+		ret = ad5504_spi_read(st->spi, chan->address);
+		if (ret < 0)
+			return ret;
 
-	ret = ad5504_spi_write(st->spi, this_attr->address, readin);
-	return ret ? ret : len;
+		*val = ret;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		scale_uv = (st->vref_mv * 1000) >> chan->scan_type.realbits;
+		*val =  scale_uv / 1000;
+		*val2 = (scale_uv % 1000) * 1000;
+		return IIO_VAL_INT_PLUS_MICRO;
+
+	}
+	return -EINVAL;
 }
 
-static ssize_t ad5504_read_dac(struct device *dev,
-					   struct device_attribute *attr,
-					   char *buf)
+static int ad5504_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
 {
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5504_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret;
-	u16 val;
 
-	ret = ad5504_spi_read(st->spi, this_attr->address, &val);
-	if (ret)
-		return ret;
+	switch (mask) {
+	case 0:
+		if (val >= (1 << chan->scan_type.realbits) || val < 0)
+			return -EINVAL;
 
-	return sprintf(buf, "%d\n", val);
+		return ad5504_spi_write(st->spi, chan->address, val);
+	default:
+		ret = -EINVAL;
+	}
+
+	return -EINVAL;
 }
 
 static ssize_t ad5504_read_powerdown_mode(struct device *dev,
@@ -157,32 +193,6 @@ static ssize_t ad5504_write_dac_powerdown(struct device *dev,
 	return ret ? ret : len;
 }
 
-static ssize_t ad5504_show_scale(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
-{
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	struct ad5504_state *st = iio_priv(indio_dev);
-	/* Corresponds to Vref / 2^(bits) */
-	unsigned int scale_uv = (st->vref_mv * 1000) >> AD5505_BITS;
-
-	return sprintf(buf, "%d.%03d\n", scale_uv / 1000, scale_uv % 1000);
-}
-static IIO_DEVICE_ATTR(out_voltage_scale, S_IRUGO, ad5504_show_scale, NULL, 0);
-
-#define IIO_DEV_ATTR_OUT_RW_RAW(_num, _show, _store, _addr)		\
-	IIO_DEVICE_ATTR(out_voltage##_num##_raw,			\
-			S_IRUGO | S_IWUSR, _show, _store, _addr)
-
-static IIO_DEV_ATTR_OUT_RW_RAW(0, ad5504_read_dac,
-	ad5504_write_dac, AD5504_ADDR_DAC0);
-static IIO_DEV_ATTR_OUT_RW_RAW(1, ad5504_read_dac,
-	ad5504_write_dac, AD5504_ADDR_DAC1);
-static IIO_DEV_ATTR_OUT_RW_RAW(2, ad5504_read_dac,
-	ad5504_write_dac, AD5504_ADDR_DAC2);
-static IIO_DEV_ATTR_OUT_RW_RAW(3, ad5504_read_dac,
-	ad5504_write_dac, AD5504_ADDR_DAC3);
-
 static IIO_DEVICE_ATTR(out_voltage_powerdown_mode, S_IRUGO |
 			S_IWUSR, ad5504_read_powerdown_mode,
 			ad5504_write_powerdown_mode, 0);
@@ -203,17 +213,12 @@ static IIO_DEV_ATTR_DAC_POWERDOWN(3, ad5504_read_dac_powerdown,
 				   ad5504_write_dac_powerdown, 3);
 
 static struct attribute *ad5504_attributes[] = {
-	&iio_dev_attr_out_voltage0_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage1_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage2_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage3_raw.dev_attr.attr,
 	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage1_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage2_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage3_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
 	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	&iio_dev_attr_out_voltage_scale.dev_attr.attr,
 	NULL,
 };
 
@@ -222,11 +227,9 @@ static const struct attribute_group ad5504_attribute_group = {
 };
 
 static struct attribute *ad5501_attributes[] = {
-	&iio_dev_attr_out_voltage0_raw.dev_attr.attr,
 	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
 	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	&iio_dev_attr_out_voltage_scale.dev_attr.attr,
 	NULL,
 };
 
@@ -261,12 +264,16 @@ static irqreturn_t ad5504_event_handler(int irq, void *private)
 }
 
 static const struct iio_info ad5504_info = {
+	.write_raw = ad5504_write_raw,
+	.read_raw = ad5504_read_raw,
 	.attrs = &ad5504_attribute_group,
 	.event_attrs = &ad5504_ev_attribute_group,
 	.driver_module = THIS_MODULE,
 };
 
 static const struct iio_info ad5501_info = {
+	.write_raw = ad5504_write_raw,
+	.read_raw = ad5504_read_raw,
 	.attrs = &ad5501_attribute_group,
 	.event_attrs = &ad5504_ev_attribute_group,
 	.driver_module = THIS_MODULE,
@@ -307,10 +314,14 @@ static int __devinit ad5504_probe(struct spi_device *spi)
 	st->spi = spi;
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi_get_device_id(st->spi)->name;
-	if (spi_get_device_id(st->spi)->driver_data == ID_AD5501)
+	if (spi_get_device_id(st->spi)->driver_data == ID_AD5501) {
 		indio_dev->info = &ad5501_info;
-	else
+		indio_dev->num_channels = 1;
+	} else {
 		indio_dev->info = &ad5504_info;
+		indio_dev->num_channels = 4;
+	}
+	indio_dev->channels = ad5504_channels;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
 	if (spi->irq) {
@@ -367,6 +378,7 @@ static const struct spi_device_id ad5504_id[] = {
 	{"ad5501", ID_AD5501},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad5504_id);
 
 static struct spi_driver ad5504_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/dac/ad5504.h b/drivers/staging/iio/dac/ad5504.h
index 85beb1dd29b9..afe09522f53c 100644
--- a/drivers/staging/iio/dac/ad5504.h
+++ b/drivers/staging/iio/dac/ad5504.h
@@ -18,10 +18,7 @@
 
 /* Registers */
 #define AD5504_ADDR_NOOP		0
-#define AD5504_ADDR_DAC0		1
-#define AD5504_ADDR_DAC1		2
-#define AD5504_ADDR_DAC2		3
-#define AD5504_ADDR_DAC3		4
+#define AD5504_ADDR_DAC(x)		((x) + 1)
 #define AD5504_ADDR_ALL_DAC		5
 #define AD5504_ADDR_CTRL		7
 
diff --git a/drivers/staging/iio/dac/ad5624r.h b/drivers/staging/iio/dac/ad5624r.h
index b71c6a03e780..5dca3028cdfd 100644
--- a/drivers/staging/iio/dac/ad5624r.h
+++ b/drivers/staging/iio/dac/ad5624r.h
@@ -32,12 +32,12 @@
 
 /**
  * struct ad5624r_chip_info - chip specific information
- * @bits:		accuracy of the DAC in bits
+ * @channels:		channel spec for the DAC
  * @int_vref_mv:	AD5620/40/60: the internal reference voltage
  */
 
 struct ad5624r_chip_info {
-	u8				bits;
+	const struct iio_chan_spec	*channels;
 	u16				int_vref_mv;
 };
 
diff --git a/drivers/staging/iio/dac/ad5624r_spi.c b/drivers/staging/iio/dac/ad5624r_spi.c
index 6e05f0dbae0b..10c7484366ef 100644
--- a/drivers/staging/iio/dac/ad5624r_spi.c
+++ b/drivers/staging/iio/dac/ad5624r_spi.c
@@ -21,29 +21,51 @@
 #include "dac.h"
 #include "ad5624r.h"
 
+#define AD5624R_CHANNEL(_chan, _bits) { \
+	.type = IIO_VOLTAGE, \
+	.indexed = 1, \
+	.output = 1, \
+	.channel = (_chan), \
+	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, \
+	.address = (_chan), \
+	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits)), \
+}
+
+#define DECLARE_AD5624R_CHANNELS(_name, _bits) \
+	const struct iio_chan_spec _name##_channels[] = { \
+		AD5624R_CHANNEL(0, _bits), \
+		AD5624R_CHANNEL(1, _bits), \
+		AD5624R_CHANNEL(2, _bits), \
+		AD5624R_CHANNEL(3, _bits), \
+}
+
+static DECLARE_AD5624R_CHANNELS(ad5624r, 12);
+static DECLARE_AD5624R_CHANNELS(ad5644r, 14);
+static DECLARE_AD5624R_CHANNELS(ad5664r, 16);
+
 static const struct ad5624r_chip_info ad5624r_chip_info_tbl[] = {
 	[ID_AD5624R3] = {
-		.bits = 12,
-		.int_vref_mv = 1250,
-	},
-	[ID_AD5644R3] = {
-		.bits = 14,
-		.int_vref_mv = 1250,
-	},
-	[ID_AD5664R3] = {
-		.bits = 16,
+		.channels = ad5624r_channels,
 		.int_vref_mv = 1250,
 	},
 	[ID_AD5624R5] = {
-		.bits = 12,
+		.channels = ad5624r_channels,
 		.int_vref_mv = 2500,
 	},
+	[ID_AD5644R3] = {
+		.channels = ad5644r_channels,
+		.int_vref_mv = 1250,
+	},
 	[ID_AD5644R5] = {
-		.bits = 14,
+		.channels = ad5644r_channels,
 		.int_vref_mv = 2500,
 	},
+	[ID_AD5664R3] = {
+		.channels = ad5664r_channels,
+		.int_vref_mv = 1250,
+	},
 	[ID_AD5664R5] = {
-		.bits = 16,
+		.channels = ad5664r_channels,
 		.int_vref_mv = 2500,
 	},
 };
@@ -70,24 +92,49 @@ static int ad5624r_spi_write(struct spi_device *spi,
 	return spi_write(spi, msg, 3);
 }
 
-static ssize_t ad5624r_write_dac(struct device *dev,
-				 struct device_attribute *attr,
-				 const char *buf, size_t len)
+static int ad5624r_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
 {
-	long readin;
-	int ret;
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct ad5624r_state *st = iio_priv(indio_dev);
-	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	unsigned long scale_uv;
 
-	ret = strict_strtol(buf, 10, &readin);
-	if (ret)
-		return ret;
+	switch (m) {
+	case IIO_CHAN_INFO_SCALE:
+		scale_uv = (st->vref_mv * 1000) >> chan->scan_type.realbits;
+		*val =  scale_uv / 1000;
+		*val2 = (scale_uv % 1000) * 1000;
+		return IIO_VAL_INT_PLUS_MICRO;
 
-	ret = ad5624r_spi_write(st->us, AD5624R_CMD_WRITE_INPUT_N_UPDATE_N,
-				this_attr->address, readin,
-				st->chip_info->bits);
-	return ret ? ret : len;
+	}
+	return -EINVAL;
+}
+
+static int ad5624r_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct ad5624r_state *st = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case 0:
+		if (val >= (1 << chan->scan_type.realbits) || val < 0)
+			return -EINVAL;
+
+		return ad5624r_spi_write(st->us,
+				AD5624R_CMD_WRITE_INPUT_N_UPDATE_N,
+				chan->address, val,
+				chan->scan_type.shift);
+	default:
+		ret = -EINVAL;
+	}
+
+	return -EINVAL;
 }
 
 static ssize_t ad5624r_read_powerdown_mode(struct device *dev,
@@ -161,24 +208,6 @@ static ssize_t ad5624r_write_dac_powerdown(struct device *dev,
 	return ret ? ret : len;
 }
 
-static ssize_t ad5624r_show_scale(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
-{
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	struct ad5624r_state *st = iio_priv(indio_dev);
-	/* Corresponds to Vref / 2^(bits) */
-	unsigned int scale_uv = (st->vref_mv * 1000) >> st->chip_info->bits;
-
-	return sprintf(buf, "%d.%03d\n", scale_uv / 1000, scale_uv % 1000);
-}
-static IIO_DEVICE_ATTR(out_voltage_scale, S_IRUGO, ad5624r_show_scale, NULL, 0);
-
-static IIO_DEV_ATTR_OUT_RAW(0, ad5624r_write_dac, AD5624R_ADDR_DAC0);
-static IIO_DEV_ATTR_OUT_RAW(1, ad5624r_write_dac, AD5624R_ADDR_DAC1);
-static IIO_DEV_ATTR_OUT_RAW(2, ad5624r_write_dac, AD5624R_ADDR_DAC2);
-static IIO_DEV_ATTR_OUT_RAW(3, ad5624r_write_dac, AD5624R_ADDR_DAC3);
-
 static IIO_DEVICE_ATTR(out_voltage_powerdown_mode, S_IRUGO |
 			S_IWUSR, ad5624r_read_powerdown_mode,
 			ad5624r_write_powerdown_mode, 0);
@@ -200,17 +229,12 @@ static IIO_DEV_ATTR_DAC_POWERDOWN(3, ad5624r_read_dac_powerdown,
 				   ad5624r_write_dac_powerdown, 3);
 
 static struct attribute *ad5624r_attributes[] = {
-	&iio_dev_attr_out_voltage0_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage1_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage2_raw.dev_attr.attr,
-	&iio_dev_attr_out_voltage3_raw.dev_attr.attr,
 	&iio_dev_attr_out_voltage0_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage1_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage2_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage3_powerdown.dev_attr.attr,
 	&iio_dev_attr_out_voltage_powerdown_mode.dev_attr.attr,
 	&iio_const_attr_out_voltage_powerdown_mode_available.dev_attr.attr,
-	&iio_dev_attr_out_voltage_scale.dev_attr.attr,
 	NULL,
 };
 
@@ -219,6 +243,8 @@ static const struct attribute_group ad5624r_attribute_group = {
 };
 
 static const struct iio_info ad5624r_info = {
+	.write_raw = ad5624r_write_raw,
+	.read_raw = ad5624r_read_raw,
 	.attrs = &ad5624r_attribute_group,
 	.driver_module = THIS_MODULE,
 };
@@ -259,6 +285,8 @@ static int __devinit ad5624r_probe(struct spi_device *spi)
 	indio_dev->name = spi_get_device_id(spi)->name;
 	indio_dev->info = &ad5624r_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = st->chip_info->channels;
+	indio_dev->num_channels = AD5624R_DAC_CHANNELS;
 
 	ret = ad5624r_spi_write(spi, AD5624R_CMD_INTERNAL_REFER_SETUP, 0,
 				!!voltage_uv, 16);
@@ -307,6 +335,7 @@ static const struct spi_device_id ad5624r_id[] = {
 	{"ad5664r5", ID_AD5664R5},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad5624r_id);
 
 static struct spi_driver ad5624r_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/dac/ad5686.c b/drivers/staging/iio/dac/ad5686.c
index e72db2fbfedf..ce2d6193dd89 100644
--- a/drivers/staging/iio/dac/ad5686.c
+++ b/drivers/staging/iio/dac/ad5686.c
@@ -99,7 +99,7 @@ enum ad5686_supported_device_ids {
 		.indexed = 1,					\
 		.output = 1,					\
 		.channel = chan,				\
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),	\
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,	\
 		.address = AD5686_ADDR_DAC(chan),			\
 		.scan_type = IIO_ST('u', bits, 16, shift)	\
 }
@@ -306,7 +306,7 @@ static int ad5686_read_raw(struct iio_dev *indio_dev,
 		*val = ret;
 		return IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		scale_uv = (st->vref_mv * 100000)
 			>> (chan->scan_type.realbits);
 		*val =  scale_uv / 100000;
@@ -437,6 +437,7 @@ static const struct spi_device_id ad5686_id[] = {
 	{"ad5686", ID_AD5686},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad5686_id);
 
 static struct spi_driver ad5686_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/dac/ad5764.c b/drivers/staging/iio/dac/ad5764.c
new file mode 100644
index 000000000000..ff91480ae65c
--- /dev/null
+++ b/drivers/staging/iio/dac/ad5764.c
@@ -0,0 +1,393 @@
+/*
+ * Analog devices AD5764, AD5764R, AD5744, AD5744R quad-channel
+ * Digital to Analog Converters driver
+ *
+ * Copyright 2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/spi/spi.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/regulator/consumer.h>
+
+#include "../iio.h"
+#include "../sysfs.h"
+#include "dac.h"
+
+#define AD5764_REG_SF_NOP			0x0
+#define AD5764_REG_SF_CONFIG			0x1
+#define AD5764_REG_SF_CLEAR			0x4
+#define AD5764_REG_SF_LOAD			0x5
+#define AD5764_REG_DATA(x)			((2 << 3) | (x))
+#define AD5764_REG_COARSE_GAIN(x)		((3 << 3) | (x))
+#define AD5764_REG_FINE_GAIN(x)			((4 << 3) | (x))
+#define AD5764_REG_OFFSET(x)			((5 << 3) | (x))
+
+#define AD5764_NUM_CHANNELS 4
+
+/**
+ * struct ad5764_chip_info - chip specific information
+ * @int_vref:	Value of the internal reference voltage in uV - 0 if external
+ *		reference voltage is used
+ * @channel	channel specification
+*/
+
+struct ad5764_chip_info {
+	unsigned long int_vref;
+	const struct iio_chan_spec *channels;
+};
+
+/**
+ * struct ad5764_state - driver instance specific data
+ * @spi:		spi_device
+ * @chip_info:		chip info
+ * @vref_reg:		vref supply regulators
+ * @data:		spi transfer buffers
+ */
+
+struct ad5764_state {
+	struct spi_device		*spi;
+	const struct ad5764_chip_info	*chip_info;
+	struct regulator_bulk_data	vref_reg[2];
+
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	union {
+		__be32 d32;
+		u8 d8[4];
+	} data[2] ____cacheline_aligned;
+};
+
+enum ad5764_type {
+	ID_AD5744,
+	ID_AD5744R,
+	ID_AD5764,
+	ID_AD5764R,
+};
+
+#define AD5764_CHANNEL(_chan, _bits) {				\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.output = 1,						\
+	.channel = (_chan),					\
+	.address = (_chan),					\
+	.info_mask = IIO_CHAN_INFO_OFFSET_SHARED_BIT |		\
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |		\
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,		\
+	.scan_type = IIO_ST('u', (_bits), 16, 16 - (_bits))	\
+}
+
+#define DECLARE_AD5764_CHANNELS(_name, _bits) \
+const struct iio_chan_spec _name##_channels[] = { \
+	AD5764_CHANNEL(0, (_bits)), \
+	AD5764_CHANNEL(1, (_bits)), \
+	AD5764_CHANNEL(2, (_bits)), \
+	AD5764_CHANNEL(3, (_bits)), \
+};
+
+static DECLARE_AD5764_CHANNELS(ad5764, 16);
+static DECLARE_AD5764_CHANNELS(ad5744, 14);
+
+static const struct ad5764_chip_info ad5764_chip_infos[] = {
+	[ID_AD5744] = {
+		.int_vref = 0,
+		.channels = ad5744_channels,
+	},
+	[ID_AD5744R] = {
+		.int_vref = 5000000,
+		.channels = ad5744_channels,
+	},
+	[ID_AD5764] = {
+		.int_vref = 0,
+		.channels = ad5764_channels,
+	},
+	[ID_AD5764R] = {
+		.int_vref = 5000000,
+		.channels = ad5764_channels,
+	},
+};
+
+static int ad5764_write(struct iio_dev *indio_dev, unsigned int reg,
+	unsigned int val)
+{
+	struct ad5764_state *st = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&indio_dev->mlock);
+	st->data[0].d32 = cpu_to_be32((reg << 16) | val);
+
+	ret = spi_write(st->spi, &st->data[0].d8[1], 3);
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static int ad5764_read(struct iio_dev *indio_dev, unsigned int reg,
+	unsigned int *val)
+{
+	struct ad5764_state *st = iio_priv(indio_dev);
+	struct spi_message m;
+	int ret;
+	struct spi_transfer t[] = {
+		{
+			.tx_buf = &st->data[0].d8[1],
+			.len = 3,
+			.cs_change = 1,
+		}, {
+			.rx_buf = &st->data[1].d8[1],
+			.len = 3,
+		},
+	};
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+
+	mutex_lock(&indio_dev->mlock);
+
+	st->data[0].d32 = cpu_to_be32((1 << 23) | (reg << 16));
+
+	ret = spi_sync(st->spi, &m);
+	if (ret >= 0)
+		*val = be32_to_cpu(st->data[1].d32) & 0xffff;
+
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static int ad5764_chan_info_to_reg(struct iio_chan_spec const *chan, long info)
+{
+	switch (info) {
+	case 0:
+		return AD5764_REG_DATA(chan->address);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		return AD5764_REG_OFFSET(chan->address);
+	case IIO_CHAN_INFO_CALIBSCALE:
+		return AD5764_REG_FINE_GAIN(chan->address);
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int ad5764_write_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int val, int val2, long info)
+{
+	const int max_val = (1 << chan->scan_type.realbits);
+	unsigned int reg;
+
+	switch (info) {
+	case 0:
+		if (val >= max_val || val < 0)
+			return -EINVAL;
+		val <<= chan->scan_type.shift;
+		break;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		if (val >= 128 || val < -128)
+			return -EINVAL;
+		break;
+	case IIO_CHAN_INFO_CALIBSCALE:
+		if (val >= 32 || val < -32)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	reg = ad5764_chan_info_to_reg(chan, info);
+	return ad5764_write(indio_dev, reg, (u16)val);
+}
+
+static int ad5764_get_channel_vref(struct ad5764_state *st,
+	unsigned int channel)
+{
+	if (st->chip_info->int_vref)
+		return st->chip_info->int_vref;
+	else
+		return regulator_get_voltage(st->vref_reg[channel / 2].consumer);
+}
+
+static int ad5764_read_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int *val, int *val2, long info)
+{
+	struct ad5764_state *st = iio_priv(indio_dev);
+	unsigned long scale_uv;
+	unsigned int reg;
+	int vref;
+	int ret;
+
+	switch (info) {
+	case 0:
+		reg = AD5764_REG_DATA(chan->address);
+		ret = ad5764_read(indio_dev, reg, val);
+		if (ret < 0)
+			return ret;
+		*val >>= chan->scan_type.shift;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		reg = AD5764_REG_OFFSET(chan->address);
+		ret = ad5764_read(indio_dev, reg, val);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(*val, 7);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBSCALE:
+		reg = AD5764_REG_FINE_GAIN(chan->address);
+		ret = ad5764_read(indio_dev, reg, val);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(*val, 5);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		/* vout = 4 * vref + ((dac_code / 65535) - 0.5) */
+		vref = ad5764_get_channel_vref(st, chan->channel);
+		if (vref < 0)
+			return vref;
+
+		scale_uv = (vref * 4 * 100) >> chan->scan_type.realbits;
+		*val = scale_uv / 100000;
+		*val2 = (scale_uv % 100000) * 10;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = -(1 << chan->scan_type.realbits) / 2;
+		return IIO_VAL_INT;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info ad5764_info = {
+	.read_raw = ad5764_read_raw,
+	.write_raw = ad5764_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit ad5764_probe(struct spi_device *spi)
+{
+	enum ad5764_type type = spi_get_device_id(spi)->driver_data;
+	struct iio_dev *indio_dev;
+	struct ad5764_state *st;
+	int ret;
+
+	indio_dev = iio_allocate_device(sizeof(*st));
+	if (indio_dev == NULL) {
+		dev_err(&spi->dev, "Failed to allocate iio device\n");
+		return -ENOMEM;
+	}
+
+	st = iio_priv(indio_dev);
+	spi_set_drvdata(spi, indio_dev);
+
+	st->spi = spi;
+	st->chip_info = &ad5764_chip_infos[type];
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->info = &ad5764_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->num_channels = AD5764_NUM_CHANNELS;
+	indio_dev->channels = st->chip_info->channels;
+
+	if (st->chip_info->int_vref == 0) {
+		st->vref_reg[0].supply = "vrefAB";
+		st->vref_reg[1].supply = "vrefCD";
+
+		ret = regulator_bulk_get(&st->spi->dev,
+			ARRAY_SIZE(st->vref_reg), st->vref_reg);
+		if (ret) {
+			dev_err(&spi->dev, "Failed to request vref regulators: %d\n",
+				ret);
+			goto error_free;
+		}
+
+		ret = regulator_bulk_enable(ARRAY_SIZE(st->vref_reg),
+			st->vref_reg);
+		if (ret) {
+			dev_err(&spi->dev, "Failed to enable vref regulators: %d\n",
+				ret);
+			goto error_free_reg;
+		}
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&spi->dev, "Failed to register iio device: %d\n", ret);
+		goto error_disable_reg;
+	}
+
+	return 0;
+
+error_disable_reg:
+	if (st->chip_info->int_vref == 0)
+		regulator_bulk_disable(ARRAY_SIZE(st->vref_reg), st->vref_reg);
+error_free_reg:
+	if (st->chip_info->int_vref == 0)
+		regulator_bulk_free(ARRAY_SIZE(st->vref_reg), st->vref_reg);
+error_free:
+	iio_free_device(indio_dev);
+
+	return ret;
+}
+
+static int __devexit ad5764_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct ad5764_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	if (st->chip_info->int_vref == 0) {
+		regulator_bulk_disable(ARRAY_SIZE(st->vref_reg), st->vref_reg);
+		regulator_bulk_free(ARRAY_SIZE(st->vref_reg), st->vref_reg);
+	}
+
+	iio_free_device(indio_dev);
+
+	return 0;
+}
+
+static const struct spi_device_id ad5764_ids[] = {
+	{ "ad5744", ID_AD5744 },
+	{ "ad5744r", ID_AD5744R },
+	{ "ad5764", ID_AD5764 },
+	{ "ad5764r", ID_AD5764R },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, ad5764_ids);
+
+static struct spi_driver ad5764_driver = {
+	.driver = {
+		.name = "ad5764",
+		.owner = THIS_MODULE,
+	},
+	.probe = ad5764_probe,
+	.remove = __devexit_p(ad5764_remove),
+	.id_table = ad5764_ids,
+};
+
+static int __init ad5764_spi_init(void)
+{
+	return spi_register_driver(&ad5764_driver);
+}
+module_init(ad5764_spi_init);
+
+static void __exit ad5764_spi_exit(void)
+{
+	spi_unregister_driver(&ad5764_driver);
+}
+module_exit(ad5764_spi_exit);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Analog Devices AD5744/AD5744R/AD5764/AD5764R DAC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/dac/ad5791.c b/drivers/staging/iio/dac/ad5791.c
index 4a80fd822231..ac45636a8d72 100644
--- a/drivers/staging/iio/dac/ad5791.c
+++ b/drivers/staging/iio/dac/ad5791.c
@@ -1,5 +1,6 @@
 /*
- * AD5760, AD5780, AD5781, AD5791 Voltage Output Digital to Analog Converter
+ * AD5760, AD5780, AD5781, AD5790, AD5791 Voltage Output Digital to Analog
+ * Converter
  *
  * Copyright 2011 Analog Devices Inc.
  *
@@ -77,8 +78,8 @@ static int ad5791_spi_read(struct spi_device *spi, u8 addr, u32 *val)
 	.indexed = 1,					\
 	.address = AD5791_ADDR_DAC0,			\
 	.channel = 0,					\
-	.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED) | \
-		(1 << IIO_CHAN_INFO_OFFSET_SHARED),	\
+	.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT | \
+		IIO_CHAN_INFO_OFFSET_SHARED_BIT,	\
 	.scan_type = IIO_ST('u', bits, 24, shift)	\
 }
 
@@ -237,11 +238,11 @@ static int ad5791_read_raw(struct iio_dev *indio_dev,
 		*val &= AD5791_DAC_MASK;
 		*val >>= chan->scan_type.shift;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		*val = 0;
 		*val2 = (((u64)st->vref_mv) * 1000000ULL) >> chan->scan_type.realbits;
 		return IIO_VAL_INT_PLUS_MICRO;
-	case (1 << IIO_CHAN_INFO_OFFSET_SHARED):
+	case IIO_CHAN_INFO_OFFSET:
 		val64 = (((u64)st->vref_neg_mv) << chan->scan_type.realbits);
 		do_div(val64, st->vref_mv);
 		*val = -val64;
@@ -397,9 +398,11 @@ static const struct spi_device_id ad5791_id[] = {
 	{"ad5760", ID_AD5760},
 	{"ad5780", ID_AD5780},
 	{"ad5781", ID_AD5781},
+	{"ad5790", ID_AD5791},
 	{"ad5791", ID_AD5791},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad5791_id);
 
 static struct spi_driver ad5791_driver = {
 	.driver = {
@@ -413,5 +416,5 @@ static struct spi_driver ad5791_driver = {
 module_spi_driver(ad5791_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
-MODULE_DESCRIPTION("Analog Devices AD5760/AD5780/AD5781/AD5791 DAC");
+MODULE_DESCRIPTION("Analog Devices AD5760/AD5780/AD5781/AD5790/AD5791 DAC");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/dds/ad5930.c b/drivers/staging/iio/dds/ad5930.c
index 4a360d044a36..9c32d1beae25 100644
--- a/drivers/staging/iio/dds/ad5930.c
+++ b/drivers/staging/iio/dds/ad5930.c
@@ -148,3 +148,4 @@ module_spi_driver(ad5930_driver);
 MODULE_AUTHOR("Cliff Cai");
 MODULE_DESCRIPTION("Analog Devices ad5930 driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/dds/ad9832.c b/drivers/staging/iio/dds/ad9832.c
index cc32fd65b8b3..2ccf25dd9289 100644
--- a/drivers/staging/iio/dds/ad9832.c
+++ b/drivers/staging/iio/dds/ad9832.c
@@ -88,7 +88,7 @@ static ssize_t ad9832_write(struct device *dev,
 		goto error_ret;
 
 	mutex_lock(&indio_dev->mlock);
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case AD9832_FREQ0HM:
 	case AD9832_FREQ1HM:
 		ret = ad9832_write_frequency(st, this_attr->address, val);
@@ -344,11 +344,11 @@ static const struct spi_device_id ad9832_id[] = {
 	{"ad9835", 0},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad9832_id);
 
 static struct spi_driver ad9832_driver = {
 	.driver = {
 		.name	= "ad9832",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad9832_probe,
@@ -360,4 +360,3 @@ module_spi_driver(ad9832_driver);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD9832/AD9835 DDS");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:ad9832");
diff --git a/drivers/staging/iio/dds/ad9834.c b/drivers/staging/iio/dds/ad9834.c
index 51fda6f69815..5e67104fea18 100644
--- a/drivers/staging/iio/dds/ad9834.c
+++ b/drivers/staging/iio/dds/ad9834.c
@@ -77,7 +77,7 @@ static ssize_t ad9834_write(struct device *dev,
 		goto error_ret;
 
 	mutex_lock(&indio_dev->mlock);
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case AD9834_REG_FREQ0:
 	case AD9834_REG_FREQ1:
 		ret = ad9834_write_frequency(st, this_attr->address, val);
@@ -153,7 +153,7 @@ static ssize_t ad9834_store_wavetype(struct device *dev,
 
 	mutex_lock(&indio_dev->mlock);
 
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case 0:
 		if (sysfs_streq(buf, "sine")) {
 			st->control &= ~AD9834_MODE;
@@ -435,11 +435,11 @@ static const struct spi_device_id ad9834_id[] = {
 	{"ad9838", ID_AD9838},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad9834_id);
 
 static struct spi_driver ad9834_driver = {
 	.driver = {
 		.name	= "ad9834",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe		= ad9834_probe,
@@ -451,4 +451,3 @@ module_spi_driver(ad9834_driver);
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD9833/AD9834/AD9837/AD9838 DDS");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:ad9834");
diff --git a/drivers/staging/iio/dds/ad9850.c b/drivers/staging/iio/dds/ad9850.c
index f9c96afcb996..f4f731bb2191 100644
--- a/drivers/staging/iio/dds/ad9850.c
+++ b/drivers/staging/iio/dds/ad9850.c
@@ -134,3 +134,4 @@ module_spi_driver(ad9850_driver);
 MODULE_AUTHOR("Cliff Cai");
 MODULE_DESCRIPTION("Analog Devices ad9850 driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/dds/ad9852.c b/drivers/staging/iio/dds/ad9852.c
index 9fc73fdc9c3b..554266c615a8 100644
--- a/drivers/staging/iio/dds/ad9852.c
+++ b/drivers/staging/iio/dds/ad9852.c
@@ -285,3 +285,4 @@ module_spi_driver(ad9852_driver);
 MODULE_AUTHOR("Cliff Cai");
 MODULE_DESCRIPTION("Analog Devices ad9852 driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/dds/ad9910.c b/drivers/staging/iio/dds/ad9910.c
index 57046b03121f..3985766d6f87 100644
--- a/drivers/staging/iio/dds/ad9910.c
+++ b/drivers/staging/iio/dds/ad9910.c
@@ -418,3 +418,4 @@ module_spi_driver(ad9910_driver);
 MODULE_AUTHOR("Cliff Cai");
 MODULE_DESCRIPTION("Analog Devices ad9910 driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/dds/ad9951.c b/drivers/staging/iio/dds/ad9951.c
index d29130e9acdf..4d150048002a 100644
--- a/drivers/staging/iio/dds/ad9951.c
+++ b/drivers/staging/iio/dds/ad9951.c
@@ -229,3 +229,4 @@ module_spi_driver(ad9951_driver);
 MODULE_AUTHOR("Cliff Cai");
 MODULE_DESCRIPTION("Analog Devices ad9951 driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/events.h b/drivers/staging/iio/events.h
new file mode 100644
index 000000000000..bfb63400fa60
--- /dev/null
+++ b/drivers/staging/iio/events.h
@@ -0,0 +1,103 @@
+/* The industrial I/O - event passing to userspace
+ *
+ * Copyright (c) 2008-2011 Jonathan Cameron
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+#ifndef _IIO_EVENTS_H_
+#define _IIO_EVENTS_H_
+
+#include <linux/ioctl.h>
+#include <linux/types.h>
+#include "types.h"
+
+/**
+ * struct iio_event_data - The actual event being pushed to userspace
+ * @id:		event identifier
+ * @timestamp:	best estimate of time of event occurrence (often from
+ *		the interrupt handler)
+ */
+struct iio_event_data {
+	__u64	id;
+	__s64	timestamp;
+};
+
+#define IIO_GET_EVENT_FD_IOCTL _IOR('i', 0x90, int)
+
+enum iio_event_type {
+	IIO_EV_TYPE_THRESH,
+	IIO_EV_TYPE_MAG,
+	IIO_EV_TYPE_ROC,
+	IIO_EV_TYPE_THRESH_ADAPTIVE,
+	IIO_EV_TYPE_MAG_ADAPTIVE,
+};
+
+enum iio_event_direction {
+	IIO_EV_DIR_EITHER,
+	IIO_EV_DIR_RISING,
+	IIO_EV_DIR_FALLING,
+};
+
+/**
+ * IIO_EVENT_CODE() - create event identifier
+ * @chan_type:	Type of the channel. Should be one of enum iio_chan_type.
+ * @diff:	Whether the event is for an differential channel or not.
+ * @modifier:	Modifier for the channel. Should be one of enum iio_modifier.
+ * @direction:	Direction of the event. One of enum iio_event_direction.
+ * @type:	Type of the event. Should be one enum iio_event_type.
+ * @chan:	Channel number for non-differential channels.
+ * @chan1:	First channel number for differential channels.
+ * @chan2:	Second channel number for differential channels.
+ */
+
+#define IIO_EVENT_CODE(chan_type, diff, modifier, direction,		\
+		       type, chan, chan1, chan2)			\
+	(((u64)type << 56) | ((u64)diff << 55) |			\
+	 ((u64)direction << 48) | ((u64)modifier << 40) |		\
+	 ((u64)chan_type << 32) | (((u16)chan2) << 16) | ((u16)chan1) | \
+	 ((u16)chan))
+
+
+#define IIO_EV_DIR_MAX 4
+#define IIO_EV_BIT(type, direction)			\
+	(1 << (type*IIO_EV_DIR_MAX + direction))
+
+/**
+ * IIO_MOD_EVENT_CODE() - create event identifier for modified channels
+ * @chan_type:	Type of the channel. Should be one of enum iio_chan_type.
+ * @number:	Channel number.
+ * @modifier:	Modifier for the channel. Should be one of enum iio_modifier.
+ * @type:	Type of the event. Should be one enum iio_event_type.
+ * @direction:	Direction of the event. One of enum iio_event_direction.
+ */
+
+#define IIO_MOD_EVENT_CODE(chan_type, number, modifier,		\
+			   type, direction)				\
+	IIO_EVENT_CODE(chan_type, 0, modifier, direction, type, number, 0, 0)
+
+/**
+ * IIO_UNMOD_EVENT_CODE() - create event identifier for unmodified channels
+ * @chan_type:	Type of the channel. Should be one of enum iio_chan_type.
+ * @number:	Channel number.
+ * @type:	Type of the event. Should be one enum iio_event_type.
+ * @direction:	Direction of the event. One of enum iio_event_direction.
+ */
+
+#define IIO_UNMOD_EVENT_CODE(chan_type, number, type, direction)	\
+	IIO_EVENT_CODE(chan_type, 0, 0, direction, type, number, 0, 0)
+
+#define IIO_EVENT_CODE_EXTRACT_TYPE(mask) ((mask >> 56) & 0xFF)
+
+#define IIO_EVENT_CODE_EXTRACT_DIR(mask) ((mask >> 48) & 0xCF)
+
+#define IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(mask) ((mask >> 32) & 0xFF)
+
+/* Event code number extraction depends on which type of event we have.
+ * Perhaps review this function in the future*/
+#define IIO_EVENT_CODE_EXTRACT_NUM(mask) ((__s16)(mask & 0xFFFF))
+
+#define IIO_EVENT_CODE_EXTRACT_MODIFIER(mask) ((mask >> 40) & 0xFF)
+
+#endif
diff --git a/drivers/staging/iio/gyro/Kconfig b/drivers/staging/iio/gyro/Kconfig
index 22aea5b4e61d..ea295b25308c 100644
--- a/drivers/staging/iio/gyro/Kconfig
+++ b/drivers/staging/iio/gyro/Kconfig
@@ -37,11 +37,11 @@ config ADIS16260
 	  will be called adis16260.
 
 config ADXRS450
-	tristate "Analog Devices ADXRS450 Digital Output Gyroscope SPI driver"
+	tristate "Analog Devices ADXRS450/3 Digital Output Gyroscope SPI driver"
 	depends on SPI
 	help
-	  Say yes here to build support for Analog Devices ADXRS450 programmable
-	  digital output gyroscope.
+	  Say yes here to build support for Analog Devices ADXRS450 and ADXRS453
+	  programmable digital output gyroscope.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called adxrs450.
diff --git a/drivers/staging/iio/gyro/adis16060_core.c b/drivers/staging/iio/gyro/adis16060_core.c
index ff1b5a82b3d6..c0ca7093e0ed 100644
--- a/drivers/staging/iio/gyro/adis16060_core.c
+++ b/drivers/staging/iio/gyro/adis16060_core.c
@@ -98,11 +98,11 @@ static int adis16060_read_raw(struct iio_dev *indio_dev,
 		mutex_unlock(&indio_dev->mlock);
 		*val = tval;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		*val = -7;
 		*val2 = 461117;
 		return IIO_VAL_INT_PLUS_MICRO;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		*val = 0;
 		*val2 = 34000;
 		return IIO_VAL_INT_PLUS_MICRO;
@@ -136,8 +136,8 @@ static const struct iio_chan_spec adis16060_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = ADIS16060_TEMP_OUT,
 	}
 };
diff --git a/drivers/staging/iio/gyro/adis16080_core.c b/drivers/staging/iio/gyro/adis16080_core.c
index 9405f2d368ee..1815490db8b4 100644
--- a/drivers/staging/iio/gyro/adis16080_core.c
+++ b/drivers/staging/iio/gyro/adis16080_core.c
@@ -194,3 +194,4 @@ module_spi_driver(adis16080_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADIS16080/100 Yaw Rate Gyroscope Driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16080");
diff --git a/drivers/staging/iio/gyro/adis16130_core.c b/drivers/staging/iio/gyro/adis16130_core.c
index c9aaca9631f4..947eb86f05d8 100644
--- a/drivers/staging/iio/gyro/adis16130_core.c
+++ b/drivers/staging/iio/gyro/adis16130_core.c
@@ -173,3 +173,4 @@ module_spi_driver(adis16130_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADIS16130 High Precision Angular Rate");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:adis16130");
diff --git a/drivers/staging/iio/gyro/adis16260_core.c b/drivers/staging/iio/gyro/adis16260_core.c
index 886dddf867ef..8f6af47e9559 100644
--- a/drivers/staging/iio/gyro/adis16260_core.c
+++ b/drivers/staging/iio/gyro/adis16260_core.c
@@ -20,7 +20,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 
 #include "adis16260.h"
 
@@ -390,9 +390,9 @@ enum adis16260_channel {
 #define ADIS16260_GYRO_CHANNEL_SET(axis, mod)				\
 	struct iio_chan_spec adis16260_channels_##axis[] = {		\
 		IIO_CHAN(IIO_ANGL_VEL, 1, 0, 0, NULL, 0, mod,		\
-			 (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |	\
-			 (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) |	\
-			 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),		\
+			 IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |	\
+			 IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT |	\
+			 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
 			 gyro, ADIS16260_SCAN_GYRO,			\
 			 IIO_ST('s', 14, 16, 0), 0),			\
 		IIO_CHAN(IIO_ANGL, 1, 0, 0, NULL, 0, mod,		\
@@ -400,16 +400,16 @@ enum adis16260_channel {
 			 angle, ADIS16260_SCAN_ANGL,			\
 			 IIO_ST('u', 14, 16, 0), 0),			\
 		IIO_CHAN(IIO_TEMP, 0, 1, 0, NULL, 0, 0,			\
-			 (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |		\
-			 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),		\
+			 IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |		\
+			 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
 			 temp, ADIS16260_SCAN_TEMP,			\
 			 IIO_ST('u', 12, 16, 0), 0),			\
 		IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "supply", 0, 0,		\
-			 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),		\
+			 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
 			 in_supply, ADIS16260_SCAN_SUPPLY,		\
 			 IIO_ST('u', 12, 16, 0), 0),			\
 		IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, NULL, 1, 0,		\
-			 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),		\
+			 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,		\
 			 in_aux, ADIS16260_SCAN_AUX_ADC,		\
 			 IIO_ST('u', 12, 16, 0), 0),			\
 		IIO_CHAN_SOFT_TIMESTAMP(5)				\
@@ -464,8 +464,7 @@ static int adis16260_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
 			*val = 0;
@@ -489,10 +488,10 @@ static int adis16260_read_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 		}
 		break;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		*val = 25;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
 			bits = 12;
@@ -512,7 +511,7 @@ static int adis16260_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
 			bits = 12;
@@ -544,11 +543,11 @@ static int adis16260_write_raw(struct iio_dev *indio_dev,
 	s16 val16;
 	u8 addr;
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		val16 = val & ((1 << bits) - 1);
 		addr = adis16260_addresses[chan->address][1];
 		return adis16260_spi_write_reg_16(indio_dev, addr, val16);
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		val16 = val & ((1 << bits) - 1);
 		addr = adis16260_addresses[chan->address][2];
 		return adis16260_spi_write_reg_16(indio_dev, addr, val16);
@@ -633,11 +632,16 @@ static int __devinit adis16260_probe(struct spi_device *spi)
 	}
 	if (indio_dev->buffer) {
 		/* Set default scan mode */
-		iio_scan_mask_set(indio_dev->buffer, ADIS16260_SCAN_SUPPLY);
-		iio_scan_mask_set(indio_dev->buffer, ADIS16260_SCAN_GYRO);
-		iio_scan_mask_set(indio_dev->buffer, ADIS16260_SCAN_AUX_ADC);
-		iio_scan_mask_set(indio_dev->buffer, ADIS16260_SCAN_TEMP);
-		iio_scan_mask_set(indio_dev->buffer, ADIS16260_SCAN_ANGL);
+		iio_scan_mask_set(indio_dev, indio_dev->buffer,
+				  ADIS16260_SCAN_SUPPLY);
+		iio_scan_mask_set(indio_dev, indio_dev->buffer,
+				  ADIS16260_SCAN_GYRO);
+		iio_scan_mask_set(indio_dev, indio_dev->buffer,
+				  ADIS16260_SCAN_AUX_ADC);
+		iio_scan_mask_set(indio_dev, indio_dev->buffer,
+				  ADIS16260_SCAN_TEMP);
+		iio_scan_mask_set(indio_dev, indio_dev->buffer,
+				  ADIS16260_SCAN_ANGL);
 	}
 	if (spi->irq) {
 		ret = adis16260_probe_trigger(indio_dev);
@@ -701,6 +705,7 @@ static const struct spi_device_id adis16260_id[] = {
 	{"adis16251", 1},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, adis16260_id);
 
 static struct spi_driver adis16260_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/gyro/adis16260_ring.c b/drivers/staging/iio/gyro/adis16260_ring.c
index 52a9e784e7c8..699a6152c409 100644
--- a/drivers/staging/iio/gyro/adis16260_ring.c
+++ b/drivers/staging/iio/gyro/adis16260_ring.c
@@ -74,9 +74,10 @@ static irqreturn_t adis16260_trigger_handler(int irq, void *p)
 		return -ENOMEM;
 	}
 
-	if (ring->scan_count &&
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
 	    adis16260_read_ring_data(&indio_dev->dev, st->rx) >= 0)
-		for (; i < ring->scan_count; i++)
+		for (; i < bitmap_weight(indio_dev->active_scan_mask,
+					 indio_dev->masklength); i++)
 			data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
 
 	/* Guaranteed to be aligned with 8 byte boundary */
@@ -116,10 +117,8 @@ int adis16260_configure_ring(struct iio_dev *indio_dev)
 	indio_dev->buffer = ring;
 	/* Effectively select the ring buffer implementation */
 	ring->access = &ring_sw_access_funcs;
-	ring->bpe = 2;
 	ring->scan_timestamp = true;
-	ring->setup_ops = &adis16260_ring_setup_ops;
-	ring->owner = THIS_MODULE;
+	indio_dev->setup_ops = &adis16260_ring_setup_ops;
 
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 						 &adis16260_trigger_handler,
diff --git a/drivers/staging/iio/gyro/adxrs450.h b/drivers/staging/iio/gyro/adxrs450.h
index b6b682876406..af0c870100b6 100644
--- a/drivers/staging/iio/gyro/adxrs450.h
+++ b/drivers/staging/iio/gyro/adxrs450.h
@@ -39,6 +39,11 @@
 
 #define ADXRS450_GET_ST(a)	((a >> 26) & 0x3)
 
+enum {
+	ID_ADXRS450,
+	ID_ADXRS453,
+};
+
 /**
  * struct adxrs450_state - device instance specific data
  * @us:			actual spi_device
diff --git a/drivers/staging/iio/gyro/adxrs450_core.c b/drivers/staging/iio/gyro/adxrs450_core.c
index 70fd468b6850..15e2496f70c8 100644
--- a/drivers/staging/iio/gyro/adxrs450_core.c
+++ b/drivers/staging/iio/gyro/adxrs450_core.c
@@ -1,5 +1,5 @@
 /*
- * ADXRS450 Digital Output Gyroscope Driver
+ * ADXRS450/ADXRS453 Digital Output Gyroscope Driver
  *
  * Copyright 2011 Analog Devices Inc.
  *
@@ -243,7 +243,7 @@ static int adxrs450_write_raw(struct iio_dev *indio_dev,
 {
 	int ret;
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		ret = adxrs450_spi_write_reg_16(indio_dev,
 						ADXRS450_DNC1,
 						val & 0x3FF);
@@ -263,7 +263,7 @@ static int adxrs450_read_raw(struct iio_dev *indio_dev,
 {
 	int ret;
 	s16 t;
-	u16 ut;
+
 	switch (mask) {
 	case 0:
 		switch (chan->type) {
@@ -276,10 +276,10 @@ static int adxrs450_read_raw(struct iio_dev *indio_dev,
 			break;
 		case IIO_TEMP:
 			ret = adxrs450_spi_read_reg_16(indio_dev,
-						       ADXRS450_TEMP1, &ut);
+						       ADXRS450_TEMP1, &t);
 			if (ret)
 				break;
-			*val = ut;
+			*val = (t >> 6) + 225;
 			ret = IIO_VAL_INT;
 			break;
 		default:
@@ -287,13 +287,34 @@ static int adxrs450_read_raw(struct iio_dev *indio_dev,
 			break;
 		}
 		break;
-	case (1 << IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_ANGL_VEL:
+			*val = 0;
+			*val2 = 218166;
+			return IIO_VAL_INT_PLUS_NANO;
+		case IIO_TEMP:
+			*val = 200;
+			*val2 = 0;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW:
 		ret = adxrs450_spi_read_reg_16(indio_dev, ADXRS450_QUAD1, &t);
 		if (ret)
 			break;
 		*val = t;
 		ret = IIO_VAL_INT;
 		break;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		ret = adxrs450_spi_read_reg_16(indio_dev, ADXRS450_DNC1, &t);
+		if (ret)
+			break;
+		*val = t;
+		ret = IIO_VAL_INT;
+		break;
 	default:
 		ret = -EINVAL;
 		break;
@@ -302,18 +323,36 @@ static int adxrs450_read_raw(struct iio_dev *indio_dev,
 	return ret;
 }
 
-static const struct iio_chan_spec adxrs450_channels[] = {
-	{
-		.type = IIO_ANGL_VEL,
-		.modified = 1,
-		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE)
-	}, {
-		.type = IIO_TEMP,
-		.indexed = 1,
-		.channel = 0,
-	}
+static const struct iio_chan_spec adxrs450_channels[2][2] = {
+	[ID_ADXRS450] = {
+		{
+			.type = IIO_ANGL_VEL,
+			.modified = 1,
+			.channel2 = IIO_MOD_Z,
+			.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+			IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT |
+			IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		}, {
+			.type = IIO_TEMP,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		}
+	},
+	[ID_ADXRS453] = {
+		{
+			.type = IIO_ANGL_VEL,
+			.modified = 1,
+			.channel2 = IIO_MOD_Z,
+			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+			IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT,
+		}, {
+			.type = IIO_TEMP,
+			.indexed = 1,
+			.channel = 0,
+			.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
+		}
+	},
 };
 
 static const struct iio_info adxrs450_info = {
@@ -343,7 +382,8 @@ static int __devinit adxrs450_probe(struct spi_device *spi)
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->info = &adxrs450_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
-	indio_dev->channels = adxrs450_channels;
+	indio_dev->channels =
+		adxrs450_channels[spi_get_device_id(spi)->driver_data];
 	indio_dev->num_channels = ARRAY_SIZE(adxrs450_channels);
 	indio_dev->name = spi->dev.driver->name;
 
@@ -373,6 +413,13 @@ static int adxrs450_remove(struct spi_device *spi)
 	return 0;
 }
 
+static const struct spi_device_id adxrs450_id[] = {
+	{"adxrs450", ID_ADXRS450},
+	{"adxrs453", ID_ADXRS453},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, adxrs450_id);
+
 static struct spi_driver adxrs450_driver = {
 	.driver = {
 		.name = "adxrs450",
@@ -380,9 +427,10 @@ static struct spi_driver adxrs450_driver = {
 	},
 	.probe = adxrs450_probe,
 	.remove = __devexit_p(adxrs450_remove),
+	.id_table	= adxrs450_id,
 };
 module_spi_driver(adxrs450_driver);
 
 MODULE_AUTHOR("Cliff Cai <cliff.cai@xxxxxxxxxx>");
-MODULE_DESCRIPTION("Analog Devices ADXRS450 Gyroscope SPI driver");
+MODULE_DESCRIPTION("Analog Devices ADXRS450/ADXRS453 Gyroscope SPI driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/iio.h b/drivers/staging/iio/iio.h
index f3d88cd7e8a0..be6ced31f65e 100644
--- a/drivers/staging/iio/iio.h
+++ b/drivers/staging/iio/iio.h
@@ -7,13 +7,12 @@
  * under the terms of the GNU General Public License version 2 as published by
  * the Free Software Foundation.
  */
-
 #ifndef _INDUSTRIAL_IO_H_
 #define _INDUSTRIAL_IO_H_
 
 #include <linux/device.h>
 #include <linux/cdev.h>
-
+#include "types.h"
 /* IIO TODO LIST */
 /*
  * Provide means of adjusting timer accuracy.
@@ -25,62 +24,64 @@ enum iio_data_type {
 	IIO_PROCESSED,
 };
 
-enum iio_chan_type {
-	/* real channel types */
-	IIO_VOLTAGE,
-	IIO_CURRENT,
-	IIO_POWER,
-	IIO_ACCEL,
-	IIO_ANGL_VEL,
-	IIO_MAGN,
-	IIO_LIGHT,
-	IIO_INTENSITY,
-	IIO_PROXIMITY,
-	IIO_TEMP,
-	IIO_INCLI,
-	IIO_ROT,
-	IIO_ANGL,
-	IIO_TIMESTAMP,
-	IIO_CAPACITANCE,
-};
-
-enum iio_modifier {
-	IIO_NO_MOD,
-	IIO_MOD_X,
-	IIO_MOD_Y,
-	IIO_MOD_Z,
-	IIO_MOD_X_AND_Y,
-	IIO_MOD_X_ANX_Z,
-	IIO_MOD_Y_AND_Z,
-	IIO_MOD_X_AND_Y_AND_Z,
-	IIO_MOD_X_OR_Y,
-	IIO_MOD_X_OR_Z,
-	IIO_MOD_Y_OR_Z,
-	IIO_MOD_X_OR_Y_OR_Z,
-	IIO_MOD_LIGHT_BOTH,
-	IIO_MOD_LIGHT_IR,
-};
-
 /* Could add the raw attributes as well - allowing buffer only devices */
 enum iio_chan_info_enum {
-	IIO_CHAN_INFO_SCALE_SHARED,
-	IIO_CHAN_INFO_SCALE_SEPARATE,
-	IIO_CHAN_INFO_OFFSET_SHARED,
-	IIO_CHAN_INFO_OFFSET_SEPARATE,
-	IIO_CHAN_INFO_CALIBSCALE_SHARED,
-	IIO_CHAN_INFO_CALIBSCALE_SEPARATE,
-	IIO_CHAN_INFO_CALIBBIAS_SHARED,
-	IIO_CHAN_INFO_CALIBBIAS_SEPARATE,
-	IIO_CHAN_INFO_PEAK_SHARED,
-	IIO_CHAN_INFO_PEAK_SEPARATE,
-	IIO_CHAN_INFO_PEAK_SCALE_SHARED,
-	IIO_CHAN_INFO_PEAK_SCALE_SEPARATE,
-	IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SHARED,
-	IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE,
-	IIO_CHAN_INFO_AVERAGE_RAW_SHARED,
-	IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE,
+	/* 0 is reserverd for raw attributes */
+	IIO_CHAN_INFO_SCALE = 1,
+	IIO_CHAN_INFO_OFFSET,
+	IIO_CHAN_INFO_CALIBSCALE,
+	IIO_CHAN_INFO_CALIBBIAS,
+	IIO_CHAN_INFO_PEAK,
+	IIO_CHAN_INFO_PEAK_SCALE,
+	IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW,
+	IIO_CHAN_INFO_AVERAGE_RAW,
+	IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY,
 };
 
+#define IIO_CHAN_INFO_SHARED_BIT(type) BIT(type*2)
+#define IIO_CHAN_INFO_SEPARATE_BIT(type) BIT(type*2 + 1)
+
+#define IIO_CHAN_INFO_SCALE_SEPARATE_BIT		\
+	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_SCALE)
+#define IIO_CHAN_INFO_SCALE_SHARED_BIT			\
+	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_SCALE)
+#define IIO_CHAN_INFO_OFFSET_SEPARATE_BIT			\
+	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_OFFSET)
+#define IIO_CHAN_INFO_OFFSET_SHARED_BIT			\
+	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_OFFSET)
+#define IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT			\
+	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_CALIBSCALE)
+#define IIO_CHAN_INFO_CALIBSCALE_SHARED_BIT			\
+	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_CALIBSCALE)
+#define IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT			\
+	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_CALIBBIAS)
+#define IIO_CHAN_INFO_CALIBBIAS_SHARED_BIT			\
+	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_CALIBBIAS)
+#define IIO_CHAN_INFO_PEAK_SEPARATE_BIT			\
+	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_PEAK)
+#define IIO_CHAN_INFO_PEAK_SHARED_BIT			\
+	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_PEAK)
+#define IIO_CHAN_INFO_PEAKSCALE_SEPARATE_BIT			\
+	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_PEAKSCALE)
+#define IIO_CHAN_INFO_PEAKSCALE_SHARED_BIT			\
+	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_PEAKSCALE)
+#define IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SEPARATE_BIT	\
+	IIO_CHAN_INFO_SEPARATE_BIT(				\
+		IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW)
+#define IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SHARED_BIT	\
+	IIO_CHAN_INFO_SHARED_BIT(				\
+		IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW)
+#define IIO_CHAN_INFO_AVERAGE_RAW_SEPARATE_BIT			\
+	IIO_CHAN_INFO_SEPARATE_BIT(IIO_CHAN_INFO_AVERAGE_RAW)
+#define IIO_CHAN_INFO_AVERAGE_RAW_SHARED_BIT			\
+	IIO_CHAN_INFO_SHARED_BIT(IIO_CHAN_INFO_AVERAGE_RAW)
+#define IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT \
+	IIO_CHAN_INFO_SHARED_BIT(			       \
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)
+#define IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SEPARATE_BIT \
+	IIO_CHAN_INFO_SEPARATE_BIT(			       \
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)
+
 enum iio_endian {
 	IIO_CPU,
 	IIO_BE,
@@ -109,6 +110,10 @@ enum iio_endian {
  * @extend_name:	Allows labeling of channel attributes with an
  *			informative name. Note this has no effect codes etc,
  *			unlike modifiers.
+ * @datasheet_name:	A name used in in kernel mapping of channels. It should
+ *			corrspond to the first name that the channel is referred
+ *			to by in the datasheet (e.g. IND), or the nearest
+ *			possible compound name (e.g. IND-INC).
  * @processed_val:	Flag to specify the data access attribute should be
  *			*_input rather than *_raw.
  * @modified:		Does a modifier apply to this channel. What these are
@@ -137,6 +142,7 @@ struct iio_chan_spec {
 	long			info_mask;
 	long			event_mask;
 	char			*extend_name;
+	const char		*datasheet_name;
 	unsigned		processed_val:1;
 	unsigned		modified:1;
 	unsigned		indexed:1;
@@ -261,7 +267,23 @@ struct iio_info {
 				 int val);
 	int (*validate_trigger)(struct iio_dev *indio_dev,
 				struct iio_trigger *trig);
+	int (*update_scan_mode)(struct iio_dev *indio_dev,
+				const unsigned long *scan_mask);
+};
 
+/**
+ * struct iio_buffer_setup_ops - buffer setup related callbacks
+ * @preenable:		[DRIVER] function to run prior to marking buffer enabled
+ * @postenable:		[DRIVER] function to run after marking buffer enabled
+ * @predisable:		[DRIVER] function to run prior to marking buffer
+ *			disabled
+ * @postdisable:	[DRIVER] function to run after marking buffer disabled
+ */
+struct iio_buffer_setup_ops {
+	int				(*preenable)(struct iio_dev *);
+	int				(*postenable)(struct iio_dev *);
+	int				(*predisable)(struct iio_dev *);
+	int				(*postdisable)(struct iio_dev *);
 };
 
 /**
@@ -278,6 +300,7 @@ struct iio_info {
  * @available_scan_masks: [DRIVER] optional array of allowed bitmasks
  * @masklength:		[INTERN] the length of the mask established from
  *			channels
+ * @active_scan_mask:	[INTERN] union of all scan masks requested by buffers
  * @trig:		[INTERN] current device trigger (buffer modes)
  * @pollfunc:		[DRIVER] function run on trigger being received
  * @channels:		[DRIVER] channel specification structure table
@@ -290,6 +313,7 @@ struct iio_info {
  * @chrdev:		[INTERN] associated character device
  * @groups:		[INTERN] attribute groups
  * @groupcounter:	[INTERN] index of next attribute group
+ * @flags:		[INTERN] file ops related flags including busy flag.
  **/
 struct iio_dev {
 	int				id;
@@ -305,6 +329,7 @@ struct iio_dev {
 
 	unsigned long			*available_scan_masks;
 	unsigned			masklength;
+	unsigned long			*active_scan_mask;
 	struct iio_trigger		*trig;
 	struct iio_poll_func		*pollfunc;
 
@@ -315,13 +340,24 @@ struct iio_dev {
 	struct attribute_group		chan_attr_group;
 	const char			*name;
 	const struct iio_info		*info;
+	const struct iio_buffer_setup_ops	*setup_ops;
 	struct cdev			chrdev;
 #define IIO_MAX_GROUPS 6
 	const struct attribute_group	*groups[IIO_MAX_GROUPS + 1];
 	int				groupcounter;
+
+	unsigned long			flags;
 };
 
 /**
+ * iio_find_channel_from_si() - get channel from its scan index
+ * @indio_dev:		device
+ * @si:			scan index to match
+ */
+const struct iio_chan_spec
+*iio_find_channel_from_si(struct iio_dev *indio_dev, int si);
+
+/**
  * iio_device_register() - register a device with the IIO subsystem
  * @indio_dev:		Device structure filled by the device driver
  **/
diff --git a/drivers/staging/iio/iio_core.h b/drivers/staging/iio/iio_core.h
index 36159e0dbfc3..107cfb1cbb01 100644
--- a/drivers/staging/iio/iio_core.h
+++ b/drivers/staging/iio/iio_core.h
@@ -33,9 +33,6 @@ int __iio_add_chan_devattr(const char *postfix,
 #ifdef CONFIG_IIO_BUFFER
 struct poll_table_struct;
 
-int iio_chrdev_buffer_open(struct iio_dev *indio_dev);
-void iio_chrdev_buffer_release(struct iio_dev *indio_dev);
-
 unsigned int iio_buffer_poll(struct file *filp,
 			     struct poll_table_struct *wait);
 ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
@@ -47,14 +44,6 @@ ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
 
 #else
 
-static inline int iio_chrdev_buffer_open(struct iio_dev *indio_dev)
-{
-	return -EINVAL;
-}
-
-static inline void iio_chrdev_buffer_release(struct iio_dev *indio_dev)
-{}
-
 #define iio_buffer_poll_addr NULL
 #define iio_buffer_read_first_n_outer_addr NULL
 
diff --git a/drivers/staging/iio/iio_core_trigger.h b/drivers/staging/iio/iio_core_trigger.h
index 523c288b776b..6f7c56fcbe78 100644
--- a/drivers/staging/iio/iio_core_trigger.h
+++ b/drivers/staging/iio/iio_core_trigger.h
@@ -13,8 +13,7 @@
  * iio_device_register_trigger_consumer() - set up an iio_dev to use triggers
  * @indio_dev: iio_dev associated with the device that will consume the trigger
  **/
-
-int iio_device_register_trigger_consumer(struct iio_dev *indio_dev);
+void iio_device_register_trigger_consumer(struct iio_dev *indio_dev);
 
 /**
  * iio_device_unregister_trigger_consumer() - reverse the registration process
diff --git a/drivers/staging/iio/iio_dummy_evgen.c b/drivers/staging/iio/iio_dummy_evgen.c
index da657d10471d..cdbf289bfe2d 100644
--- a/drivers/staging/iio/iio_dummy_evgen.c
+++ b/drivers/staging/iio/iio_dummy_evgen.c
@@ -102,6 +102,10 @@ static int iio_dummy_evgen_create(void)
 int iio_dummy_evgen_get_irq(void)
 {
 	int i, ret = 0;
+
+	if (iio_evgen == NULL)
+		return -ENODEV;
+
 	mutex_lock(&iio_evgen->lock);
 	for (i = 0; i < IIO_EVENTGEN_NO; i++)
 		if (iio_evgen->inuse[i] == false) {
diff --git a/drivers/staging/iio/iio_simple_dummy.c b/drivers/staging/iio/iio_simple_dummy.c
index af0c99236d4f..e3a94572bb40 100644
--- a/drivers/staging/iio/iio_simple_dummy.c
+++ b/drivers/staging/iio/iio_simple_dummy.c
@@ -21,7 +21,8 @@
 
 #include "iio.h"
 #include "sysfs.h"
-#include "buffer_generic.h"
+#include "events.h"
+#include "buffer.h"
 #include "iio_simple_dummy.h"
 
 /*
@@ -76,13 +77,13 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		 * Offset for userspace to apply prior to scale
 		 * when converting to standard units (microvolts)
 		 */
-		(1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
+		IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
 		/*
 		 * in_voltage0_scale
 		 * Multipler for userspace to apply post offset
 		 * when converting to standard units (microvolts)
 		 */
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		/* The ordering of elements in the buffer via an enum */
 		.scan_index = voltage0,
 		.scan_type = { /* Description of storage in buffer */
@@ -117,7 +118,7 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		 * Shared version of scale - shared by differential
 		 * input channels of type IIO_VOLTAGE.
 		 */
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.scan_index = diffvoltage1m2,
 		.scan_type = { /* Description of storage in buffer */
 			.sign = 's', /* signed */
@@ -134,7 +135,7 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		.channel = 3,
 		.channel2 = 4,
 		.info_mask =
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.scan_index = diffvoltage3m4,
 		.scan_type = {
 			.sign = 's',
@@ -159,7 +160,7 @@ static struct iio_chan_spec iio_dummy_channels[] = {
 		 * seeing the readings. Typically part of hardware
 		 * calibration.
 		 */
-		(1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE),
+		IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT,
 		.scan_index = accelx,
 		.scan_type = { /* Description of storage in buffer */
 			.sign = 's', /* signed */
@@ -228,29 +229,32 @@ static int iio_dummy_read_raw(struct iio_dev *indio_dev,
 			break;
 		}
 		break;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		/* only single ended adc -> 7 */
 		*val = 7;
 		ret = IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
-		/* only single ended adc -> 0.001333 */
-		*val = 0;
-		*val2 = 1333;
-		ret = IIO_VAL_INT_PLUS_MICRO;
-		break;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
-		/* all differential adc channels -> 0.000001344 */
-		*val = 0;
-		*val2 = 1344;
-		ret = IIO_VAL_INT_PLUS_NANO;
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->differential) {
+		case 0:
+			/* only single ended adc -> 0.001333 */
+			*val = 0;
+			*val2 = 1333;
+			ret = IIO_VAL_INT_PLUS_MICRO;
+			break;
+		case 1:
+			/* all differential adc channels -> 0.000001344 */
+			*val = 0;
+			*val2 = 1344;
+			ret = IIO_VAL_INT_PLUS_NANO;
+		}
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		/* only the acceleration axis - read from cache */
 		*val = st->accel_calibbias;
 		ret = IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		*val = st->accel_calibscale->val;
 		*val2 = st->accel_calibscale->val2;
 		ret = IIO_VAL_INT_PLUS_MICRO;
@@ -295,7 +299,7 @@ static int iio_dummy_write_raw(struct iio_dev *indio_dev,
 		st->dac_val = val;
 		mutex_unlock(&st->lock);
 		return 0;
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		mutex_lock(&st->lock);
 		/* Compare against table - hard matching here */
 		for (i = 0; i < ARRAY_SIZE(dummy_scales); i++)
@@ -514,7 +518,8 @@ static __init int iio_dummy_init(void)
 		return -EINVAL;
 	}
 	/* Fake a bus */
-	iio_dummy_devs = kzalloc(sizeof(*iio_dummy_devs)*instances, GFP_KERNEL);
+	iio_dummy_devs = kcalloc(instances, sizeof(*iio_dummy_devs),
+				 GFP_KERNEL);
 	/* Here we have no actual device so call probe */
 	for (i = 0; i < instances; i++) {
 		ret = iio_dummy_probe(i);
diff --git a/drivers/staging/iio/iio_simple_dummy_buffer.c b/drivers/staging/iio/iio_simple_dummy_buffer.c
index edad0e7b4f4d..d6a1c0e82a5b 100644
--- a/drivers/staging/iio/iio_simple_dummy_buffer.c
+++ b/drivers/staging/iio/iio_simple_dummy_buffer.c
@@ -57,7 +57,7 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 	if (data == NULL)
 		return -ENOMEM;
 
-	if (buffer->scan_count) {
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
 		/*
 		 * Three common options here:
 		 * hardware scans: certain combinations of channels make
@@ -75,7 +75,10 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 		 * in the constant table fakedata.
 		 */
 		int i, j;
-		for (i = 0, j = 0; i < buffer->scan_count; i++) {
+		for (i = 0, j = 0;
+		     i < bitmap_weight(indio_dev->active_scan_mask,
+				       indio_dev->masklength);
+		     i++) {
 			j = find_next_bit(buffer->scan_mask,
 					  indio_dev->masklength, j + 1);
 			/* random access read form the 'device' */
@@ -142,8 +145,6 @@ int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev)
 	/* Tell the core how to access the buffer */
 	buffer->access = &kfifo_access_funcs;
 
-	/* Number of bytes per element */
-	buffer->bpe = 2;
 	/* Enable timestamps by default */
 	buffer->scan_timestamp = true;
 
@@ -151,8 +152,7 @@ int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev)
 	 * Tell the core what device type specific functions should
 	 * be run on either side of buffer capture enable / disable.
 	 */
-	buffer->setup_ops = &iio_simple_dummy_buffer_setup_ops;
-	buffer->owner = THIS_MODULE;
+	indio_dev->setup_ops = &iio_simple_dummy_buffer_setup_ops;
 
 	/*
 	 * Configure a polling function.
diff --git a/drivers/staging/iio/iio_simple_dummy_events.c b/drivers/staging/iio/iio_simple_dummy_events.c
index 9f00cff7ddd5..449c7a5ece80 100644
--- a/drivers/staging/iio/iio_simple_dummy_events.c
+++ b/drivers/staging/iio/iio_simple_dummy_events.c
@@ -14,6 +14,7 @@
 
 #include "iio.h"
 #include "sysfs.h"
+#include "events.h"
 #include "iio_simple_dummy.h"
 
 /* Evgen 'fakes' interrupt events for this example */
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
index 454d131455de..9a2ca55625f4 100644
--- a/drivers/staging/iio/impedance-analyzer/ad5933.c
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -21,7 +21,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "../ring_sw.h"
 
 #include "ad5933.h"
@@ -113,10 +113,10 @@ static struct iio_chan_spec ad5933_channels[] = {
 		 0, AD5933_REG_TEMP_DATA, IIO_ST('s', 14, 16, 0), 0),
 	/* Ring Channels */
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "real_raw", 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 AD5933_REG_REAL_DATA, 0, IIO_ST('s', 16, 16, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "imag_raw", 0, 0,
-		 (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		 IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		 AD5933_REG_IMAG_DATA, 1, IIO_ST('s', 16, 16, 0), 0),
 };
 
@@ -329,7 +329,7 @@ static ssize_t ad5933_show(struct device *dev,
 	int ret = 0, len = 0;
 
 	mutex_lock(&indio_dev->mlock);
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case AD5933_OUT_RANGE:
 		len = sprintf(buf, "%d\n",
 			      st->range_avail[(st->ctrl_hb >> 1) & 0x3]);
@@ -380,7 +380,7 @@ static ssize_t ad5933_store(struct device *dev,
 	}
 
 	mutex_lock(&indio_dev->mlock);
-	switch (this_attr->address) {
+	switch ((u32) this_attr->address) {
 	case AD5933_OUT_RANGE:
 		for (i = 0; i < 4; i++)
 			if (val == st->range_avail[i]) {
@@ -537,14 +537,14 @@ static const struct iio_info ad5933_info = {
 static int ad5933_ring_preenable(struct iio_dev *indio_dev)
 {
 	struct ad5933_state *st = iio_priv(indio_dev);
-	struct iio_buffer *ring = indio_dev->buffer;
 	size_t d_size;
 	int ret;
 
-	if (!ring->scan_count)
+	if (bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		return -EINVAL;
 
-	d_size = ring->scan_count *
+	d_size = bitmap_weight(indio_dev->active_scan_mask,
+			       indio_dev->masklength) *
 		 ad5933_channels[1].scan_type.storagebits / 8;
 
 	if (indio_dev->buffer->access->set_bytes_per_datum)
@@ -611,7 +611,7 @@ static int ad5933_register_ring_funcs_and_init(struct iio_dev *indio_dev)
 	indio_dev->buffer->access = &ring_sw_access_funcs;
 
 	/* Ring buffer functions - here trigger setup related */
-	indio_dev->buffer->setup_ops = &ad5933_ring_setup_ops;
+	indio_dev->setup_ops = &ad5933_ring_setup_ops;
 
 	indio_dev->modes |= INDIO_BUFFER_HARDWARE;
 
@@ -640,12 +640,14 @@ static void ad5933_work(struct work_struct *work)
 	ad5933_i2c_read(st->client, AD5933_REG_STATUS, 1, &status);
 
 	if (status & AD5933_STAT_DATA_VALID) {
+		int scan_count = bitmap_weight(indio_dev->active_scan_mask,
+					       indio_dev->masklength);
 		ad5933_i2c_read(st->client,
-				test_bit(1, ring->scan_mask) ?
+				test_bit(1, indio_dev->active_scan_mask) ?
 				AD5933_REG_REAL_DATA : AD5933_REG_IMAG_DATA,
-				ring->scan_count * 2, (u8 *)buf);
+				scan_count * 2, (u8 *)buf);
 
-		if (ring->scan_count == 2) {
+		if (scan_count == 2) {
 			buf[0] = be16_to_cpu(buf[0]);
 			buf[1] = be16_to_cpu(buf[1]);
 		} else {
@@ -734,8 +736,8 @@ static int __devinit ad5933_probe(struct i2c_client *client,
 		goto error_unreg_ring;
 
 	/* enable both REAL and IMAG channels by default */
-	iio_scan_mask_set(indio_dev->buffer, 0);
-	iio_scan_mask_set(indio_dev->buffer, 1);
+	iio_scan_mask_set(indio_dev, indio_dev->buffer, 0);
+	iio_scan_mask_set(indio_dev, indio_dev->buffer, 1);
 
 	ret = ad5933_setup(st);
 	if (ret)
diff --git a/drivers/staging/iio/imu/adis16400.h b/drivers/staging/iio/imu/adis16400.h
index f3546ee910a2..83d133efaac6 100644
--- a/drivers/staging/iio/imu/adis16400.h
+++ b/drivers/staging/iio/imu/adis16400.h
@@ -148,12 +148,14 @@ struct adis16400_chip_info {
  * @tx:			transmit buffer
  * @rx:			receive buffer
  * @buf_lock:		mutex to protect tx and rx
+ * @filt_int:		integer part of requested filter frequency
  **/
 struct adis16400_state {
 	struct spi_device		*us;
 	struct iio_trigger		*trig;
 	struct mutex			buf_lock;
 	struct adis16400_chip_info	*variant;
+	int				filt_int;
 
 	u8	tx[ADIS16400_MAX_TX] ____cacheline_aligned;
 	u8	rx[ADIS16400_MAX_RX] ____cacheline_aligned;
diff --git a/drivers/staging/iio/imu/adis16400_core.c b/drivers/staging/iio/imu/adis16400_core.c
index efc0f6529008..e73ad7818d85 100644
--- a/drivers/staging/iio/imu/adis16400_core.c
+++ b/drivers/staging/iio/imu/adis16400_core.c
@@ -28,7 +28,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "adis16400.h"
 
 enum adis16400_chip_variant {
@@ -161,25 +161,65 @@ error_ret:
 	return ret;
 }
 
+static int adis16400_get_freq(struct iio_dev *indio_dev)
+{
+	u16 t;
+	int sps, ret;
+
+	ret = adis16400_spi_read_reg_16(indio_dev, ADIS16400_SMPL_PRD, &t);
+	if (ret < 0)
+		return ret;
+	sps =  (t & ADIS16400_SMPL_PRD_TIME_BASE) ? 53 : 1638;
+	sps /= (t & ADIS16400_SMPL_PRD_DIV_MASK) + 1;
+
+	return sps;
+}
+
 static ssize_t adis16400_read_frequency(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	int ret, len = 0;
-	u16 t;
-	int sps;
-	ret = adis16400_spi_read_reg_16(indio_dev,
-			ADIS16400_SMPL_PRD,
-			&t);
-	if (ret)
+	ret = adis16400_get_freq(indio_dev);
+	if (ret < 0)
 		return ret;
-	sps =  (t & ADIS16400_SMPL_PRD_TIME_BASE) ? 53 : 1638;
-	sps /= (t & ADIS16400_SMPL_PRD_DIV_MASK) + 1;
-	len = sprintf(buf, "%d SPS\n", sps);
+	len = sprintf(buf, "%d SPS\n", ret);
 	return len;
 }
 
+static const unsigned adis16400_3db_divisors[] = {
+	[0] = 2, /* Special case */
+	[1] = 5,
+	[2] = 10,
+	[3] = 50,
+	[4] = 200,
+};
+
+static int adis16400_set_filter(struct iio_dev *indio_dev, int sps, int val)
+{
+	int i, ret;
+	u16 val16;
+	for (i = ARRAY_SIZE(adis16400_3db_divisors) - 1; i >= 0; i--)
+		if (sps/adis16400_3db_divisors[i] > val)
+			break;
+	if (i == -1)
+		ret = -EINVAL;
+	else {
+		ret = adis16400_spi_read_reg_16(indio_dev,
+						ADIS16400_SENS_AVG,
+						&val16);
+		if (ret < 0)
+			goto error_ret;
+
+		ret = adis16400_spi_write_reg_16(indio_dev,
+						 ADIS16400_SENS_AVG,
+						 (val16 & ~0x03) | i);
+	}
+error_ret:
+	return ret;
+}
+
 static ssize_t adis16400_write_frequency(struct device *dev,
 		struct device_attribute *attr,
 		const char *buf,
@@ -210,6 +250,7 @@ static ssize_t adis16400_write_frequency(struct device *dev,
 			ADIS16400_SMPL_PRD,
 			t);
 
+	/* Also update the filter */
 	mutex_unlock(&indio_dev->mlock);
 
 	return ret ? ret : len;
@@ -455,22 +496,39 @@ static u8 adis16400_addresses[17][2] = {
 	[incli_y] = { ADIS16300_ROLL_OUT }
 };
 
+
 static int adis16400_write_raw(struct iio_dev *indio_dev,
 			       struct iio_chan_spec const *chan,
 			       int val,
 			       int val2,
 			       long mask)
 {
-	int ret;
+	struct adis16400_state *st = iio_priv(indio_dev);
+	int ret, sps;
 
 	switch (mask) {
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		mutex_lock(&indio_dev->mlock);
 		ret = adis16400_spi_write_reg_16(indio_dev,
 				adis16400_addresses[chan->address][1],
 				val);
 		mutex_unlock(&indio_dev->mlock);
 		return ret;
+	case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
+		/* Need to cache values so we can update if the frequency
+		   changes */
+		mutex_lock(&indio_dev->mlock);
+		st->filt_int = val;
+		/* Work out update to current value */
+		sps = adis16400_get_freq(indio_dev);
+		if (sps < 0) {
+			mutex_unlock(&indio_dev->mlock);
+			return sps;
+		}
+
+		ret = adis16400_set_filter(indio_dev, sps, val);
+		mutex_unlock(&indio_dev->mlock);
+		return ret;
 	default:
 		return -EINVAL;
 	}
@@ -504,8 +562,7 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 		*val = val16;
 		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_ANGL_VEL:
 			*val = 0;
@@ -533,7 +590,7 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 		default:
 			return -EINVAL;
 		}
-	case (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE):
+	case IIO_CHAN_INFO_CALIBBIAS:
 		mutex_lock(&indio_dev->mlock);
 		ret = adis16400_spi_read_reg_16(indio_dev,
 				adis16400_addresses[chan->address][1],
@@ -544,11 +601,29 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 		val16 = ((val16 & 0xFFF) << 4) >> 4;
 		*val = val16;
 		return IIO_VAL_INT;
-	case (1 << IIO_CHAN_INFO_OFFSET_SEPARATE):
+	case IIO_CHAN_INFO_OFFSET:
 		/* currently only temperature */
 		*val = 198;
 		*val2 = 160000;
 		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
+		mutex_lock(&indio_dev->mlock);
+		/* Need both the number of taps and the sampling frequency */
+		ret = adis16400_spi_read_reg_16(indio_dev,
+						ADIS16400_SENS_AVG,
+						&val16);
+		if (ret < 0) {
+			mutex_unlock(&indio_dev->mlock);
+			return ret;
+		}
+		ret = adis16400_get_freq(indio_dev);
+		if (ret > 0)
+			*val = ret/adis16400_3db_divisors[val16 & 0x03];
+		*val2 = 0;
+		mutex_unlock(&indio_dev->mlock);
+		if (ret < 0)
+			return ret;
+		return IIO_VAL_INT_PLUS_MICRO;
 	default:
 		return -EINVAL;
 	}
@@ -560,7 +635,7 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_supply,
 		.scan_index = ADIS16400_SCAN_SUPPLY,
 		.scan_type = IIO_ST('u', 14, 16, 0)
@@ -568,8 +643,9 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_x,
 		.scan_index = ADIS16400_SCAN_GYRO_X,
 		.scan_type = IIO_ST('s', 14, 16, 0)
@@ -577,8 +653,9 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_y,
 		.scan_index = ADIS16400_SCAN_GYRO_Y,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -586,8 +663,9 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_z,
 		.scan_index = ADIS16400_SCAN_GYRO_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -595,8 +673,9 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_x,
 		.scan_index = ADIS16400_SCAN_ACC_X,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -604,8 +683,9 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_y,
 		.scan_index = ADIS16400_SCAN_ACC_Y,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -613,8 +693,9 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_z,
 		.scan_index = ADIS16400_SCAN_ACC_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -622,7 +703,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = magn_x,
 		.scan_index = ADIS16400_SCAN_MAGN_X,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -630,7 +712,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = magn_y,
 		.scan_index = ADIS16400_SCAN_MAGN_Y,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -638,7 +721,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = magn_z,
 		.scan_index = ADIS16400_SCAN_MAGN_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -646,8 +730,8 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = temp,
 		.scan_index = ADIS16400_SCAN_TEMP,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -655,7 +739,7 @@ static struct iio_chan_spec adis16400_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in1,
 		.scan_index = ADIS16400_SCAN_ADC_0,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -669,7 +753,7 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_supply,
 		.scan_index = ADIS16400_SCAN_SUPPLY,
 		.scan_type = IIO_ST('u', 12, 16, 0)
@@ -677,8 +761,9 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_x,
 		.scan_index = ADIS16400_SCAN_GYRO_X,
 		.scan_type = IIO_ST('s', 14, 16, 0)
@@ -686,8 +771,9 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_y,
 		.scan_index = ADIS16400_SCAN_GYRO_Y,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -695,8 +781,9 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_z,
 		.scan_index = ADIS16400_SCAN_GYRO_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -704,8 +791,9 @@ static struct iio_chan_spec adis16350_channels[] = {
 	.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_x,
 		.scan_index = ADIS16400_SCAN_ACC_X,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -713,8 +801,9 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_y,
 		.scan_index = ADIS16400_SCAN_ACC_Y,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -722,8 +811,9 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_z,
 		.scan_index = ADIS16400_SCAN_ACC_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -732,8 +822,9 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "x",
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = temp0,
 		.scan_index = ADIS16350_SCAN_TEMP_X,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -742,8 +833,9 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.indexed = 1,
 		.channel = 1,
 		.extend_name = "y",
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = temp1,
 		.scan_index = ADIS16350_SCAN_TEMP_Y,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -752,8 +844,8 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.indexed = 1,
 		.channel = 2,
 		.extend_name = "z",
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = temp2,
 		.scan_index = ADIS16350_SCAN_TEMP_Z,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -761,7 +853,7 @@ static struct iio_chan_spec adis16350_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in1,
 		.scan_index = ADIS16350_SCAN_ADC_0,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -775,7 +867,7 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.extend_name = "supply",
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in_supply,
 		.scan_index = ADIS16400_SCAN_SUPPLY,
 		.scan_type = IIO_ST('u', 12, 16, 0)
@@ -783,8 +875,9 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_x,
 		.scan_index = ADIS16400_SCAN_GYRO_X,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -792,8 +885,9 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_x,
 		.scan_index = ADIS16400_SCAN_ACC_X,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -801,8 +895,9 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_y,
 		.scan_index = ADIS16400_SCAN_ACC_Y,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -810,8 +905,9 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_z,
 		.scan_index = ADIS16400_SCAN_ACC_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -819,8 +915,8 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_OFFSET_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_OFFSET_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = temp,
 		.scan_index = ADIS16400_SCAN_TEMP,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -828,7 +924,7 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_VOLTAGE,
 		.indexed = 1,
 		.channel = 1,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,
 		.address = in1,
 		.scan_index = ADIS16350_SCAN_ADC_0,
 		.scan_type = IIO_ST('s', 12, 16, 0),
@@ -836,7 +932,7 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_INCLI,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = incli_x,
 		.scan_index = ADIS16300_SCAN_INCLI_X,
 		.scan_type = IIO_ST('s', 13, 16, 0),
@@ -844,7 +940,7 @@ static struct iio_chan_spec adis16300_channels[] = {
 		.type = IIO_INCLI,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = incli_y,
 		.scan_index = ADIS16300_SCAN_INCLI_Y,
 		.scan_type = IIO_ST('s', 13, 16, 0),
@@ -857,8 +953,9 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_x,
 		.scan_index = ADIS16400_SCAN_GYRO_X,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -866,8 +963,9 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_y,
 		.scan_index = ADIS16400_SCAN_GYRO_Y,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -875,8 +973,9 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = gyro_z,
 		.scan_index = ADIS16400_SCAN_GYRO_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -884,8 +983,9 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_x,
 		.scan_index = ADIS16400_SCAN_ACC_X,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -893,8 +993,9 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_y,
 		.scan_index = ADIS16400_SCAN_ACC_Y,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -902,8 +1003,9 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT |
+		IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY_SHARED_BIT,
 		.address = accel_z,
 		.scan_index = ADIS16400_SCAN_ACC_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -911,8 +1013,8 @@ static const struct iio_chan_spec adis16334_channels[] = {
 		.type = IIO_TEMP,
 		.indexed = 1,
 		.channel = 0,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBBIAS_SEPARATE) |
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		.info_mask = IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT |
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		.address = accel_z,
 		.scan_index = ADIS16400_SCAN_ACC_Z,
 		.scan_type = IIO_ST('s', 14, 16, 0),
@@ -1118,6 +1220,7 @@ static const struct spi_device_id adis16400_id[] = {
 	{"adis16405", ADIS16400},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, adis16400_id);
 
 static struct spi_driver adis16400_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/imu/adis16400_ring.c b/drivers/staging/iio/imu/adis16400_ring.c
index fd886bf51a6d..ac22de573f3e 100644
--- a/drivers/staging/iio/imu/adis16400_ring.c
+++ b/drivers/staging/iio/imu/adis16400_ring.c
@@ -79,14 +79,16 @@ static int adis16350_spi_read_all(struct device *dev, u8 *rx)
 	struct spi_message msg;
 	int i, j = 0, ret;
 	struct spi_transfer *xfers;
+	int scan_count = bitmap_weight(indio_dev->active_scan_mask,
+				       indio_dev->masklength);
 
-	xfers = kzalloc(sizeof(*xfers)*indio_dev->buffer->scan_count + 1,
+	xfers = kzalloc(sizeof(*xfers)*(scan_count + 1),
 			GFP_KERNEL);
 	if (xfers == NULL)
 		return -ENOMEM;
 
 	for (i = 0; i < ARRAY_SIZE(read_all_tx_array); i++)
-		if (test_bit(i, indio_dev->buffer->scan_mask)) {
+		if (test_bit(i, indio_dev->active_scan_mask)) {
 			xfers[j].tx_buf = &read_all_tx_array[i];
 			xfers[j].bits_per_word = 16;
 			xfers[j].len = 2;
@@ -97,7 +99,7 @@ static int adis16350_spi_read_all(struct device *dev, u8 *rx)
 	xfers[j].len = 2;
 
 	spi_message_init(&msg);
-	for (j = 0; j < indio_dev->buffer->scan_count + 1; j++)
+	for (j = 0; j < scan_count + 1; j++)
 		spi_message_add_tail(&xfers[j], &msg);
 
 	ret = spi_sync(st->us, &msg);
@@ -119,26 +121,27 @@ static irqreturn_t adis16400_trigger_handler(int irq, void *p)
 	s16 *data;
 	size_t datasize = ring->access->get_bytes_per_datum(ring);
 	/* Asumption that long is enough for maximum channels */
-	unsigned long mask = *ring->scan_mask;
-
+	unsigned long mask = *indio_dev->active_scan_mask;
+	int scan_count = bitmap_weight(indio_dev->active_scan_mask,
+				       indio_dev->masklength);
 	data = kmalloc(datasize , GFP_KERNEL);
 	if (data == NULL) {
 		dev_err(&st->us->dev, "memory alloc failed in ring bh");
 		return -ENOMEM;
 	}
 
-	if (ring->scan_count) {
+	if (scan_count) {
 		if (st->variant->flags & ADIS16400_NO_BURST) {
 			ret = adis16350_spi_read_all(&indio_dev->dev, st->rx);
 			if (ret < 0)
 				goto err;
-			for (; i < ring->scan_count; i++)
+			for (; i < scan_count; i++)
 				data[i]	= *(s16 *)(st->rx + i*2);
 		} else {
 			ret = adis16400_spi_read_burst(&indio_dev->dev, st->rx);
 			if (ret < 0)
 				goto err;
-			for (; i < indio_dev->buffer->scan_count; i++) {
+			for (; i < scan_count; i++) {
 				j = __ffs(mask);
 				mask &= ~(1 << j);
 				data[i] = be16_to_cpup(
@@ -186,10 +189,8 @@ int adis16400_configure_ring(struct iio_dev *indio_dev)
 	indio_dev->buffer = ring;
 	/* Effectively select the ring buffer implementation */
 	ring->access = &ring_sw_access_funcs;
-	ring->bpe = 2;
 	ring->scan_timestamp = true;
-	ring->setup_ops = &adis16400_ring_setup_ops;
-	ring->owner = THIS_MODULE;
+	indio_dev->setup_ops = &adis16400_ring_setup_ops;
 
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 						 &adis16400_trigger_handler,
diff --git a/drivers/staging/iio/industrialio-buffer.c b/drivers/staging/iio/industrialio-buffer.c
index 9df0ce81dade..d7b1e9e435ae 100644
--- a/drivers/staging/iio/industrialio-buffer.c
+++ b/drivers/staging/iio/industrialio-buffer.c
@@ -24,7 +24,7 @@
 #include "iio.h"
 #include "iio_core.h"
 #include "sysfs.h"
-#include "buffer_generic.h"
+#include "buffer.h"
 
 static const char * const iio_endian_prefix[] = {
 	[IIO_BE] = "be",
@@ -43,7 +43,7 @@ ssize_t iio_buffer_read_first_n_outer(struct file *filp, char __user *buf,
 	struct iio_dev *indio_dev = filp->private_data;
 	struct iio_buffer *rb = indio_dev->buffer;
 
-	if (!rb->access->read_first_n)
+	if (!rb || !rb->access->read_first_n)
 		return -EINVAL;
 	return rb->access->read_first_n(rb, n, buf);
 }
@@ -64,28 +64,9 @@ unsigned int iio_buffer_poll(struct file *filp,
 	return 0;
 }
 
-int iio_chrdev_buffer_open(struct iio_dev *indio_dev)
+void iio_buffer_init(struct iio_buffer *buffer)
 {
-	struct iio_buffer *rb = indio_dev->buffer;
-	if (!rb)
-		return -EINVAL;
-	if (rb->access->mark_in_use)
-		rb->access->mark_in_use(rb);
-	return 0;
-}
-
-void iio_chrdev_buffer_release(struct iio_dev *indio_dev)
-{
-	struct iio_buffer *rb = indio_dev->buffer;
-
-	clear_bit(IIO_BUSY_BIT_POS, &rb->flags);
-	if (rb->access->unmark_in_use)
-		rb->access->unmark_in_use(rb);
-}
-
-void iio_buffer_init(struct iio_buffer *buffer, struct iio_dev *indio_dev)
-{
-	buffer->indio_dev = indio_dev;
+	INIT_LIST_HEAD(&buffer->demux_list);
 	init_waitqueue_head(&buffer->pollq);
 }
 EXPORT_SYMBOL(iio_buffer_init);
@@ -126,17 +107,15 @@ static ssize_t iio_scan_el_show(struct device *dev,
 	int ret;
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 
-	ret = iio_scan_mask_query(indio_dev->buffer,
-				  to_iio_dev_attr(attr)->address);
-	if (ret < 0)
-		return ret;
+	ret = test_bit(to_iio_dev_attr(attr)->address,
+		       indio_dev->buffer->scan_mask);
+
 	return sprintf(buf, "%d\n", ret);
 }
 
 static int iio_scan_mask_clear(struct iio_buffer *buffer, int bit)
 {
 	clear_bit(bit, buffer->scan_mask);
-	buffer->scan_count--;
 	return 0;
 }
 
@@ -153,11 +132,11 @@ static ssize_t iio_scan_el_store(struct device *dev,
 
 	state = !(buf[0] == '0');
 	mutex_lock(&indio_dev->mlock);
-	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
+	if (iio_buffer_enabled(indio_dev)) {
 		ret = -EBUSY;
 		goto error_ret;
 	}
-	ret = iio_scan_mask_query(buffer, this_attr->address);
+	ret = iio_scan_mask_query(indio_dev, buffer, this_attr->address);
 	if (ret < 0)
 		goto error_ret;
 	if (!state && ret) {
@@ -165,7 +144,7 @@ static ssize_t iio_scan_el_store(struct device *dev,
 		if (ret)
 			goto error_ret;
 	} else if (state && !ret) {
-		ret = iio_scan_mask_set(buffer, this_attr->address);
+		ret = iio_scan_mask_set(indio_dev, buffer, this_attr->address);
 		if (ret)
 			goto error_ret;
 	}
@@ -173,7 +152,7 @@ static ssize_t iio_scan_el_store(struct device *dev,
 error_ret:
 	mutex_unlock(&indio_dev->mlock);
 
-	return ret ? ret : len;
+	return ret < 0 ? ret : len;
 
 }
 
@@ -196,7 +175,7 @@ static ssize_t iio_scan_el_ts_store(struct device *dev,
 
 	state = !(buf[0] == '0');
 	mutex_lock(&indio_dev->mlock);
-	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
+	if (iio_buffer_enabled(indio_dev)) {
 		ret = -EBUSY;
 		goto error_ret;
 	}
@@ -311,12 +290,14 @@ int iio_buffer_register(struct iio_dev *indio_dev,
 			if (ret < 0)
 				goto error_cleanup_dynamic;
 			attrcount += ret;
+			if (channels[i].type == IIO_TIMESTAMP)
+				buffer->scan_index_timestamp =
+					channels[i].scan_index;
 		}
 		if (indio_dev->masklength && buffer->scan_mask == NULL) {
-			buffer->scan_mask
-				= kzalloc(sizeof(*buffer->scan_mask)*
-					  BITS_TO_LONGS(indio_dev->masklength),
-					  GFP_KERNEL);
+			buffer->scan_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
+						    sizeof(*buffer->scan_mask),
+						    GFP_KERNEL);
 			if (buffer->scan_mask == NULL) {
 				ret = -ENOMEM;
 				goto error_cleanup_dynamic;
@@ -326,10 +307,9 @@ int iio_buffer_register(struct iio_dev *indio_dev,
 
 	buffer->scan_el_group.name = iio_scan_elements_group_name;
 
-	buffer->scan_el_group.attrs
-		= kzalloc(sizeof(buffer->scan_el_group.attrs[0])*
-			  (attrcount + 1),
-			  GFP_KERNEL);
+	buffer->scan_el_group.attrs = kcalloc(attrcount + 1,
+					      sizeof(buffer->scan_el_group.attrs[0]),
+					      GFP_KERNEL);
 	if (buffer->scan_el_group.attrs == NULL) {
 		ret = -ENOMEM;
 		goto error_free_scan_mask;
@@ -395,31 +375,20 @@ ssize_t iio_buffer_write_length(struct device *dev,
 		if (val == buffer->access->get_length(buffer))
 			return len;
 
-	if (buffer->access->set_length) {
-		buffer->access->set_length(buffer, val);
-		if (buffer->access->mark_param_change)
-			buffer->access->mark_param_change(buffer);
+	mutex_lock(&indio_dev->mlock);
+	if (iio_buffer_enabled(indio_dev)) {
+		ret = -EBUSY;
+	} else {
+		if (buffer->access->set_length)
+			buffer->access->set_length(buffer, val);
+		ret = 0;
 	}
+	mutex_unlock(&indio_dev->mlock);
 
-	return len;
+	return ret ? ret : len;
 }
 EXPORT_SYMBOL(iio_buffer_write_length);
 
-ssize_t iio_buffer_read_bytes_per_datum(struct device *dev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	struct iio_buffer *buffer = indio_dev->buffer;
-
-	if (buffer->access->get_bytes_per_datum)
-		return sprintf(buf, "%d\n",
-			       buffer->access->get_bytes_per_datum(buffer));
-
-	return 0;
-}
-EXPORT_SYMBOL(iio_buffer_read_bytes_per_datum);
-
 ssize_t iio_buffer_store_enable(struct device *dev,
 				struct device_attribute *attr,
 				const char *buf,
@@ -434,14 +403,14 @@ ssize_t iio_buffer_store_enable(struct device *dev,
 	mutex_lock(&indio_dev->mlock);
 	previous_mode = indio_dev->currentmode;
 	requested_state = !(buf[0] == '0');
-	current_state = !!(previous_mode & INDIO_ALL_BUFFER_MODES);
+	current_state = iio_buffer_enabled(indio_dev);
 	if (current_state == requested_state) {
 		printk(KERN_INFO "iio-buffer, current state requested again\n");
 		goto done;
 	}
 	if (requested_state) {
-		if (buffer->setup_ops->preenable) {
-			ret = buffer->setup_ops->preenable(indio_dev);
+		if (indio_dev->setup_ops->preenable) {
+			ret = indio_dev->setup_ops->preenable(indio_dev);
 			if (ret) {
 				printk(KERN_ERR
 				       "Buffer not started:"
@@ -458,16 +427,12 @@ ssize_t iio_buffer_store_enable(struct device *dev,
 				goto error_ret;
 			}
 		}
-		if (buffer->access->mark_in_use)
-			buffer->access->mark_in_use(buffer);
 		/* Definitely possible for devices to support both of these.*/
 		if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) {
 			if (!indio_dev->trig) {
 				printk(KERN_INFO
 				       "Buffer not started: no trigger\n");
 				ret = -EINVAL;
-				if (buffer->access->unmark_in_use)
-					buffer->access->unmark_in_use(buffer);
 				goto error_ret;
 			}
 			indio_dev->currentmode = INDIO_BUFFER_TRIGGERED;
@@ -478,32 +443,28 @@ ssize_t iio_buffer_store_enable(struct device *dev,
 			goto error_ret;
 		}
 
-		if (buffer->setup_ops->postenable) {
-			ret = buffer->setup_ops->postenable(indio_dev);
+		if (indio_dev->setup_ops->postenable) {
+			ret = indio_dev->setup_ops->postenable(indio_dev);
 			if (ret) {
 				printk(KERN_INFO
 				       "Buffer not started:"
 				       "postenable failed\n");
-				if (buffer->access->unmark_in_use)
-					buffer->access->unmark_in_use(buffer);
 				indio_dev->currentmode = previous_mode;
-				if (buffer->setup_ops->postdisable)
-					buffer->setup_ops->
+				if (indio_dev->setup_ops->postdisable)
+					indio_dev->setup_ops->
 						postdisable(indio_dev);
 				goto error_ret;
 			}
 		}
 	} else {
-		if (buffer->setup_ops->predisable) {
-			ret = buffer->setup_ops->predisable(indio_dev);
+		if (indio_dev->setup_ops->predisable) {
+			ret = indio_dev->setup_ops->predisable(indio_dev);
 			if (ret)
 				goto error_ret;
 		}
-		if (buffer->access->unmark_in_use)
-			buffer->access->unmark_in_use(buffer);
 		indio_dev->currentmode = INDIO_DIRECT_MODE;
-		if (buffer->setup_ops->postdisable) {
-			ret = buffer->setup_ops->postdisable(indio_dev);
+		if (indio_dev->setup_ops->postdisable) {
+			ret = indio_dev->setup_ops->postdisable(indio_dev);
 			if (ret)
 				goto error_ret;
 		}
@@ -523,37 +484,10 @@ ssize_t iio_buffer_show_enable(struct device *dev,
 			       char *buf)
 {
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
-	return sprintf(buf, "%d\n", !!(indio_dev->currentmode
-				       & INDIO_ALL_BUFFER_MODES));
+	return sprintf(buf, "%d\n", iio_buffer_enabled(indio_dev));
 }
 EXPORT_SYMBOL(iio_buffer_show_enable);
 
-int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
-{
-	struct iio_buffer *buffer = indio_dev->buffer;
-	size_t size;
-	dev_dbg(&indio_dev->dev, "%s\n", __func__);
-	/* Check if there are any scan elements enabled, if not fail*/
-	if (!(buffer->scan_count || buffer->scan_timestamp))
-		return -EINVAL;
-	if (buffer->scan_timestamp)
-		if (buffer->scan_count)
-			/* Timestamp (aligned to s64) and data */
-			size = (((buffer->scan_count * buffer->bpe)
-					+ sizeof(s64) - 1)
-				& ~(sizeof(s64) - 1))
-				+ sizeof(s64);
-		else /* Timestamp only  */
-			size = sizeof(s64);
-	else /* Data only */
-		size = buffer->scan_count * buffer->bpe;
-	buffer->access->set_bytes_per_datum(buffer, size);
-
-	return 0;
-}
-EXPORT_SYMBOL(iio_sw_buffer_preenable);
-
-
 /* note NULL used as error indicator as it doesn't make sense. */
 static unsigned long *iio_scan_mask_match(unsigned long *av_masks,
 					  unsigned int masklength,
@@ -569,14 +503,57 @@ static unsigned long *iio_scan_mask_match(unsigned long *av_masks,
 	return NULL;
 }
 
+int iio_sw_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct iio_buffer *buffer = indio_dev->buffer;
+	const struct iio_chan_spec *ch;
+	unsigned bytes = 0;
+	int length, i;
+	dev_dbg(&indio_dev->dev, "%s\n", __func__);
+
+	/* How much space will the demuxed element take? */
+	for_each_set_bit(i, buffer->scan_mask,
+			 indio_dev->masklength) {
+		ch = iio_find_channel_from_si(indio_dev, i);
+		length = ch->scan_type.storagebits/8;
+		bytes = ALIGN(bytes, length);
+		bytes += length;
+	}
+	if (buffer->scan_timestamp) {
+		ch = iio_find_channel_from_si(indio_dev,
+					      buffer->scan_index_timestamp);
+		length = ch->scan_type.storagebits/8;
+		bytes = ALIGN(bytes, length);
+		bytes += length;
+	}
+	buffer->access->set_bytes_per_datum(buffer, bytes);
+
+	/* What scan mask do we actually have ?*/
+	if (indio_dev->available_scan_masks)
+		indio_dev->active_scan_mask =
+			iio_scan_mask_match(indio_dev->available_scan_masks,
+					    indio_dev->masklength,
+					    buffer->scan_mask);
+	else
+		indio_dev->active_scan_mask = buffer->scan_mask;
+	iio_update_demux(indio_dev);
+
+	if (indio_dev->info->update_scan_mode)
+		return indio_dev->info
+			->update_scan_mode(indio_dev,
+					   indio_dev->active_scan_mask);
+	return 0;
+}
+EXPORT_SYMBOL(iio_sw_buffer_preenable);
+
 /**
  * iio_scan_mask_set() - set particular bit in the scan mask
  * @buffer: the buffer whose scan mask we are interested in
  * @bit: the bit to be set.
  **/
-int iio_scan_mask_set(struct iio_buffer *buffer, int bit)
+int iio_scan_mask_set(struct iio_dev *indio_dev,
+		      struct iio_buffer *buffer, int bit)
 {
-	struct iio_dev *indio_dev = buffer->indio_dev;
 	unsigned long *mask;
 	unsigned long *trialmask;
 
@@ -604,7 +581,6 @@ int iio_scan_mask_set(struct iio_buffer *buffer, int bit)
 		}
 	}
 	bitmap_copy(buffer->scan_mask, trialmask, indio_dev->masklength);
-	buffer->scan_count++;
 
 	kfree(trialmask);
 
@@ -612,25 +588,147 @@ int iio_scan_mask_set(struct iio_buffer *buffer, int bit)
 };
 EXPORT_SYMBOL_GPL(iio_scan_mask_set);
 
-int iio_scan_mask_query(struct iio_buffer *buffer, int bit)
+int iio_scan_mask_query(struct iio_dev *indio_dev,
+			struct iio_buffer *buffer, int bit)
 {
-	struct iio_dev *indio_dev = buffer->indio_dev;
-	long *mask;
-
 	if (bit > indio_dev->masklength)
 		return -EINVAL;
 
 	if (!buffer->scan_mask)
 		return 0;
-	if (indio_dev->available_scan_masks)
-		mask = iio_scan_mask_match(indio_dev->available_scan_masks,
-					   indio_dev->masklength,
-					   buffer->scan_mask);
-	else
-		mask = buffer->scan_mask;
-	if (!mask)
-		return 0;
 
-	return test_bit(bit, mask);
+	return test_bit(bit, buffer->scan_mask);
 };
 EXPORT_SYMBOL_GPL(iio_scan_mask_query);
+
+/**
+ * struct iio_demux_table() - table describing demux memcpy ops
+ * @from:	index to copy from
+ * @to:	index to copy to
+ * @length:	how many bytes to copy
+ * @l:		list head used for management
+ */
+struct iio_demux_table {
+	unsigned from;
+	unsigned to;
+	unsigned length;
+	struct list_head l;
+};
+
+static unsigned char *iio_demux(struct iio_buffer *buffer,
+				 unsigned char *datain)
+{
+	struct iio_demux_table *t;
+
+	if (list_empty(&buffer->demux_list))
+		return datain;
+	list_for_each_entry(t, &buffer->demux_list, l)
+		memcpy(buffer->demux_bounce + t->to,
+		       datain + t->from, t->length);
+
+	return buffer->demux_bounce;
+}
+
+int iio_push_to_buffer(struct iio_buffer *buffer, unsigned char *data,
+		       s64 timestamp)
+{
+	unsigned char *dataout = iio_demux(buffer, data);
+
+	return buffer->access->store_to(buffer, dataout, timestamp);
+}
+EXPORT_SYMBOL_GPL(iio_push_to_buffer);
+
+int iio_update_demux(struct iio_dev *indio_dev)
+{
+	const struct iio_chan_spec *ch;
+	struct iio_buffer *buffer = indio_dev->buffer;
+	int ret, in_ind = -1, out_ind, length;
+	unsigned in_loc = 0, out_loc = 0;
+	struct iio_demux_table *p, *q;
+
+	/* Clear out any old demux */
+	list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
+		list_del(&p->l);
+		kfree(p);
+	}
+	kfree(buffer->demux_bounce);
+	buffer->demux_bounce = NULL;
+
+	/* First work out which scan mode we will actually have */
+	if (bitmap_equal(indio_dev->active_scan_mask,
+			 buffer->scan_mask,
+			 indio_dev->masklength))
+		return 0;
+
+	/* Now we have the two masks, work from least sig and build up sizes */
+	for_each_set_bit(out_ind,
+			 indio_dev->active_scan_mask,
+			 indio_dev->masklength) {
+		in_ind = find_next_bit(indio_dev->active_scan_mask,
+				       indio_dev->masklength,
+				       in_ind + 1);
+		while (in_ind != out_ind) {
+			in_ind = find_next_bit(indio_dev->active_scan_mask,
+					       indio_dev->masklength,
+					       in_ind + 1);
+			ch = iio_find_channel_from_si(indio_dev, in_ind);
+			length = ch->scan_type.storagebits/8;
+			/* Make sure we are aligned */
+			in_loc += length;
+			if (in_loc % length)
+				in_loc += length - in_loc % length;
+		}
+		p = kmalloc(sizeof(*p), GFP_KERNEL);
+		if (p == NULL) {
+			ret = -ENOMEM;
+			goto error_clear_mux_table;
+		}
+		ch = iio_find_channel_from_si(indio_dev, in_ind);
+		length = ch->scan_type.storagebits/8;
+		if (out_loc % length)
+			out_loc += length - out_loc % length;
+		if (in_loc % length)
+			in_loc += length - in_loc % length;
+		p->from = in_loc;
+		p->to = out_loc;
+		p->length = length;
+		list_add_tail(&p->l, &buffer->demux_list);
+		out_loc += length;
+		in_loc += length;
+	}
+	/* Relies on scan_timestamp being last */
+	if (buffer->scan_timestamp) {
+		p = kmalloc(sizeof(*p), GFP_KERNEL);
+		if (p == NULL) {
+			ret = -ENOMEM;
+			goto error_clear_mux_table;
+		}
+		ch = iio_find_channel_from_si(indio_dev,
+			buffer->scan_index_timestamp);
+		length = ch->scan_type.storagebits/8;
+		if (out_loc % length)
+			out_loc += length - out_loc % length;
+		if (in_loc % length)
+			in_loc += length - in_loc % length;
+		p->from = in_loc;
+		p->to = out_loc;
+		p->length = length;
+		list_add_tail(&p->l, &buffer->demux_list);
+		out_loc += length;
+		in_loc += length;
+	}
+	buffer->demux_bounce = kzalloc(out_loc, GFP_KERNEL);
+	if (buffer->demux_bounce == NULL) {
+		ret = -ENOMEM;
+		goto error_clear_mux_table;
+	}
+	return 0;
+
+error_clear_mux_table:
+	list_for_each_entry_safe(p, q, &buffer->demux_list, l) {
+		list_del(&p->l);
+		kfree(p);
+	}
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iio_update_demux);
diff --git a/drivers/staging/iio/industrialio-core.c b/drivers/staging/iio/industrialio-core.c
index aec9311b108c..19f897f3c85e 100644
--- a/drivers/staging/iio/industrialio-core.c
+++ b/drivers/staging/iio/industrialio-core.c
@@ -25,8 +25,8 @@
 #include "iio.h"
 #include "iio_core.h"
 #include "iio_core_trigger.h"
-#include "chrdev.h"
 #include "sysfs.h"
+#include "events.h"
 
 /* IDA to assign each registered device a unique id*/
 static DEFINE_IDA(iio_ida);
@@ -77,17 +77,29 @@ static const char * const iio_modifier_names[] = {
 
 /* relies on pairs of these shared then separate */
 static const char * const iio_chan_info_postfix[] = {
-	[IIO_CHAN_INFO_SCALE_SHARED/2] = "scale",
-	[IIO_CHAN_INFO_OFFSET_SHARED/2] = "offset",
-	[IIO_CHAN_INFO_CALIBSCALE_SHARED/2] = "calibscale",
-	[IIO_CHAN_INFO_CALIBBIAS_SHARED/2] = "calibbias",
-	[IIO_CHAN_INFO_PEAK_SHARED/2] = "peak_raw",
-	[IIO_CHAN_INFO_PEAK_SCALE_SHARED/2] = "peak_scale",
-	[IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW_SHARED/2]
-	= "quadrature_correction_raw",
-	[IIO_CHAN_INFO_AVERAGE_RAW_SHARED/2] = "mean_raw",
+	[IIO_CHAN_INFO_SCALE] = "scale",
+	[IIO_CHAN_INFO_OFFSET] = "offset",
+	[IIO_CHAN_INFO_CALIBSCALE] = "calibscale",
+	[IIO_CHAN_INFO_CALIBBIAS] = "calibbias",
+	[IIO_CHAN_INFO_PEAK] = "peak_raw",
+	[IIO_CHAN_INFO_PEAK_SCALE] = "peak_scale",
+	[IIO_CHAN_INFO_QUADRATURE_CORRECTION_RAW] = "quadrature_correction_raw",
+	[IIO_CHAN_INFO_AVERAGE_RAW] = "mean_raw",
+	[IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY]
+	= "filter_low_pass_3db_frequency",
 };
 
+const struct iio_chan_spec
+*iio_find_channel_from_si(struct iio_dev *indio_dev, int si)
+{
+	int i;
+
+	for (i = 0; i < indio_dev->num_channels; i++)
+		if (indio_dev->channels[i].scan_index == si)
+			return &indio_dev->channels[i];
+	return NULL;
+}
+
 /**
  * struct iio_detected_event_list - list element for events that have occurred
  * @list:		linked list header
@@ -169,8 +181,11 @@ static ssize_t iio_event_chrdev_read(struct file *filep,
 {
 	struct iio_event_interface *ev_int = filep->private_data;
 	struct iio_detected_event_list *el;
+	size_t len = sizeof(el->ev);
 	int ret;
-	size_t len;
+
+	if (count < len)
+		return -EINVAL;
 
 	mutex_lock(&ev_int->event_list_lock);
 	if (list_empty(&ev_int->det_events)) {
@@ -192,7 +207,6 @@ static ssize_t iio_event_chrdev_read(struct file *filep,
 	el = list_first_entry(&ev_int->det_events,
 			      struct iio_detected_event_list,
 			      list);
-	len = sizeof el->ev;
 	if (copy_to_user(buf, &(el->ev), len)) {
 		ret = -EFAULT;
 		goto error_mutex_unlock;
@@ -415,7 +429,7 @@ int __iio_device_attr_init(struct device_attribute *dev_attr,
 	sysfs_attr_init(&dev_attr->attr);
 
 	/* Build up postfix of <extend_name>_<modifier>_postfix */
-	if (chan->modified) {
+	if (chan->modified && !generic) {
 		if (chan->extend_name)
 			full_postfix = kasprintf(GFP_KERNEL, "%s_%s_%s",
 						 iio_modifier_names[chan
@@ -600,7 +614,7 @@ static int iio_device_add_channel_sysfs(struct iio_dev *indio_dev,
 					     chan,
 					     &iio_read_channel_info,
 					     &iio_write_channel_info,
-					     (1 << i),
+					     i/2,
 					     !(i%2),
 					     &indio_dev->dev,
 					     &indio_dev->channel_attr_list);
@@ -665,10 +679,9 @@ static int iio_device_register_sysfs(struct iio_dev *indio_dev)
 	if (indio_dev->name)
 		attrcount++;
 
-	indio_dev->chan_attr_group.attrs
-		= kzalloc(sizeof(indio_dev->chan_attr_group.attrs[0])*
-			  (attrcount + 1),
-			  GFP_KERNEL);
+	indio_dev->chan_attr_group.attrs = kcalloc(attrcount + 1,
+						   sizeof(indio_dev->chan_attr_group.attrs[0]),
+						   GFP_KERNEL);
 	if (indio_dev->chan_attr_group.attrs == NULL) {
 		ret = -ENOMEM;
 		goto error_clear_attrs;
@@ -788,6 +801,9 @@ static ssize_t iio_ev_value_store(struct device *dev,
 	unsigned long val;
 	int ret;
 
+	if (!indio_dev->info->write_event_value)
+		return -EINVAL;
+
 	ret = strict_strtoul(buf, 10, &val);
 	if (ret)
 		return ret;
@@ -958,10 +974,9 @@ static int iio_device_register_eventset(struct iio_dev *indio_dev)
 	}
 
 	indio_dev->event_interface->group.name = iio_event_group_name;
-	indio_dev->event_interface->group.attrs =
-		kzalloc(sizeof(indio_dev->event_interface->group.attrs[0])
-			*(attrcount + 1),
-			GFP_KERNEL);
+	indio_dev->event_interface->group.attrs = kcalloc(attrcount + 1,
+							  sizeof(indio_dev->event_interface->group.attrs[0]),
+							  GFP_KERNEL);
 	if (indio_dev->event_interface->group.attrs == NULL) {
 		ret = -ENOMEM;
 		goto error_free_setup_event_lines;
@@ -1068,9 +1083,13 @@ static int iio_chrdev_open(struct inode *inode, struct file *filp)
 {
 	struct iio_dev *indio_dev = container_of(inode->i_cdev,
 						struct iio_dev, chrdev);
+
+	if (test_and_set_bit(IIO_BUSY_BIT_POS, &indio_dev->flags))
+		return -EBUSY;
+
 	filp->private_data = indio_dev;
 
-	return iio_chrdev_buffer_open(indio_dev);
+	return 0;
 }
 
 /**
@@ -1078,8 +1097,9 @@ static int iio_chrdev_open(struct inode *inode, struct file *filp)
  **/
 static int iio_chrdev_release(struct inode *inode, struct file *filp)
 {
-	iio_chrdev_buffer_release(container_of(inode->i_cdev,
-					     struct iio_dev, chrdev));
+	struct iio_dev *indio_dev = container_of(inode->i_cdev,
+						struct iio_dev, chrdev);
+	clear_bit(IIO_BUSY_BIT_POS, &indio_dev->flags);
 	return 0;
 }
 
diff --git a/drivers/staging/iio/industrialio-trigger.c b/drivers/staging/iio/industrialio-trigger.c
index 68a4d4e8c635..47ecadd4818d 100644
--- a/drivers/staging/iio/industrialio-trigger.c
+++ b/drivers/staging/iio/industrialio-trigger.c
@@ -159,13 +159,12 @@ EXPORT_SYMBOL(iio_trigger_generic_data_rdy_poll);
 void iio_trigger_poll_chained(struct iio_trigger *trig, s64 time)
 {
 	int i;
-	if (!trig->use_count) {
+	if (!trig->use_count)
 		for (i = 0; i < CONFIG_IIO_CONSUMERS_PER_TRIGGER; i++)
 			if (trig->subirqs[i].enabled) {
 				trig->use_count++;
 				handle_nested_irq(trig->subirq_base + i);
 			}
-	}
 }
 EXPORT_SYMBOL(iio_trigger_poll_chained);
 
@@ -173,10 +172,9 @@ void iio_trigger_notify_done(struct iio_trigger *trig)
 {
 	trig->use_count--;
 	if (trig->use_count == 0 && trig->ops && trig->ops->try_reenable)
-		if (trig->ops->try_reenable(trig)) {
+		if (trig->ops->try_reenable(trig))
 			/* Missed and interrupt so launch new poll now */
 			iio_trigger_poll(trig, 0);
-		}
 }
 EXPORT_SYMBOL(iio_trigger_notify_done);
 
@@ -222,8 +220,16 @@ static int iio_trigger_attach_poll_func(struct iio_trigger *trig,
 	ret = request_threaded_irq(pf->irq, pf->h, pf->thread,
 				   pf->type, pf->name,
 				   pf);
-	if (trig->ops && trig->ops->set_trigger_state && notinuse)
+	if (ret < 0) {
+		module_put(pf->indio_dev->info->driver_module);
+		return ret;
+	}
+
+	if (trig->ops && trig->ops->set_trigger_state && notinuse) {
 		ret = trig->ops->set_trigger_state(trig, true);
+		if (ret < 0)
+			module_put(pf->indio_dev->info->driver_module);
+	}
 
 	return ret;
 }
@@ -336,6 +342,8 @@ static ssize_t iio_trigger_write_current(struct device *dev,
 	mutex_unlock(&indio_dev->mlock);
 
 	trig = iio_trigger_find_by_name(buf, len);
+	if (oldtrig == trig)
+		return len;
 
 	if (trig && indio_dev->info->validate_trigger) {
 		ret = indio_dev->info->validate_trigger(indio_dev, trig);
@@ -473,12 +481,10 @@ void iio_free_trigger(struct iio_trigger *trig)
 }
 EXPORT_SYMBOL(iio_free_trigger);
 
-int iio_device_register_trigger_consumer(struct iio_dev *indio_dev)
+void iio_device_register_trigger_consumer(struct iio_dev *indio_dev)
 {
 	indio_dev->groups[indio_dev->groupcounter++] =
 		&iio_trigger_consumer_attr_group;
-
-	return 0;
 }
 
 void iio_device_unregister_trigger_consumer(struct iio_dev *indio_dev)
@@ -490,18 +496,14 @@ void iio_device_unregister_trigger_consumer(struct iio_dev *indio_dev)
 
 int iio_triggered_buffer_postenable(struct iio_dev *indio_dev)
 {
-	return indio_dev->trig
-		? iio_trigger_attach_poll_func(indio_dev->trig,
-					       indio_dev->pollfunc)
-		: 0;
+	return iio_trigger_attach_poll_func(indio_dev->trig,
+					    indio_dev->pollfunc);
 }
 EXPORT_SYMBOL(iio_triggered_buffer_postenable);
 
 int iio_triggered_buffer_predisable(struct iio_dev *indio_dev)
 {
-	return indio_dev->trig
-		? iio_trigger_dettach_poll_func(indio_dev->trig,
-						indio_dev->pollfunc)
-		: 0;
+	return iio_trigger_dettach_poll_func(indio_dev->trig,
+					     indio_dev->pollfunc);
 }
 EXPORT_SYMBOL(iio_triggered_buffer_predisable);
diff --git a/drivers/staging/iio/kfifo_buf.c b/drivers/staging/iio/kfifo_buf.c
index e8c234bb18f0..e1e9c06cde4a 100644
--- a/drivers/staging/iio/kfifo_buf.c
+++ b/drivers/staging/iio/kfifo_buf.c
@@ -11,9 +11,7 @@
 struct iio_kfifo {
 	struct iio_buffer buffer;
 	struct kfifo kf;
-	int use_count;
 	int update_needed;
-	struct mutex use_lock;
 };
 
 #define iio_to_kfifo(r) container_of(r, struct iio_kfifo, buffer)
@@ -33,54 +31,25 @@ static int iio_request_update_kfifo(struct iio_buffer *r)
 	int ret = 0;
 	struct iio_kfifo *buf = iio_to_kfifo(r);
 
-	mutex_lock(&buf->use_lock);
 	if (!buf->update_needed)
 		goto error_ret;
-	if (buf->use_count) {
-		ret = -EAGAIN;
-		goto error_ret;
-	}
 	kfifo_free(&buf->kf);
 	ret = __iio_allocate_kfifo(buf, buf->buffer.bytes_per_datum,
 				   buf->buffer.length);
 error_ret:
-	mutex_unlock(&buf->use_lock);
 	return ret;
 }
 
-static void iio_mark_kfifo_in_use(struct iio_buffer *r)
-{
-	struct iio_kfifo *buf = iio_to_kfifo(r);
-	mutex_lock(&buf->use_lock);
-	buf->use_count++;
-	mutex_unlock(&buf->use_lock);
-}
-
-static void iio_unmark_kfifo_in_use(struct iio_buffer *r)
-{
-	struct iio_kfifo *buf = iio_to_kfifo(r);
-	mutex_lock(&buf->use_lock);
-	buf->use_count--;
-	mutex_unlock(&buf->use_lock);
-}
-
 static int iio_get_length_kfifo(struct iio_buffer *r)
 {
 	return r->length;
 }
 
-static inline void __iio_init_kfifo(struct iio_kfifo *kf)
-{
-	mutex_init(&kf->use_lock);
-}
-
 static IIO_BUFFER_ENABLE_ATTR;
-static IIO_BUFFER_BYTES_PER_DATUM_ATTR;
 static IIO_BUFFER_LENGTH_ATTR;
 
 static struct attribute *iio_kfifo_attributes[] = {
 	&dev_attr_length.attr,
-	&dev_attr_bytes_per_datum.attr,
 	&dev_attr_enable.attr,
 	NULL,
 };
@@ -98,9 +67,8 @@ struct iio_buffer *iio_kfifo_allocate(struct iio_dev *indio_dev)
 	if (!kf)
 		return NULL;
 	kf->update_needed = true;
-	iio_buffer_init(&kf->buffer, indio_dev);
+	iio_buffer_init(&kf->buffer);
 	kf->buffer.attrs = &iio_kfifo_attribute_group;
-	__iio_init_kfifo(kf);
 
 	return &kf->buffer;
 }
@@ -111,20 +79,19 @@ static int iio_get_bytes_per_datum_kfifo(struct iio_buffer *r)
 	return r->bytes_per_datum;
 }
 
-static int iio_set_bytes_per_datum_kfifo(struct iio_buffer *r, size_t bpd)
+static int iio_mark_update_needed_kfifo(struct iio_buffer *r)
 {
-	if (r->bytes_per_datum != bpd) {
-		r->bytes_per_datum = bpd;
-		if (r->access->mark_param_change)
-			r->access->mark_param_change(r);
-	}
+	struct iio_kfifo *kf = iio_to_kfifo(r);
+	kf->update_needed = true;
 	return 0;
 }
 
-static int iio_mark_update_needed_kfifo(struct iio_buffer *r)
+static int iio_set_bytes_per_datum_kfifo(struct iio_buffer *r, size_t bpd)
 {
-	struct iio_kfifo *kf = iio_to_kfifo(r);
-	kf->update_needed = true;
+	if (r->bytes_per_datum != bpd) {
+		r->bytes_per_datum = bpd;
+		iio_mark_update_needed_kfifo(r);
+	}
 	return 0;
 }
 
@@ -132,8 +99,7 @@ static int iio_set_length_kfifo(struct iio_buffer *r, int length)
 {
 	if (r->length != length) {
 		r->length = length;
-		if (r->access->mark_param_change)
-			r->access->mark_param_change(r);
+		iio_mark_update_needed_kfifo(r);
 	}
 	return 0;
 }
@@ -150,16 +116,9 @@ static int iio_store_to_kfifo(struct iio_buffer *r,
 {
 	int ret;
 	struct iio_kfifo *kf = iio_to_kfifo(r);
-	u8 *datal = kmalloc(r->bytes_per_datum, GFP_KERNEL);
-	memcpy(datal, data, r->bytes_per_datum - sizeof(timestamp));
-	memcpy(datal + r->bytes_per_datum - sizeof(timestamp),
-		&timestamp, sizeof(timestamp));
 	ret = kfifo_in(&kf->kf, data, r->bytes_per_datum);
-	if (ret != r->bytes_per_datum) {
-		kfree(datal);
+	if (ret != r->bytes_per_datum)
 		return -EBUSY;
-	}
-	kfree(datal);
 	return 0;
 }
 
@@ -169,17 +128,18 @@ static int iio_read_first_n_kfifo(struct iio_buffer *r,
 	int ret, copied;
 	struct iio_kfifo *kf = iio_to_kfifo(r);
 
-	ret = kfifo_to_user(&kf->kf, buf, r->bytes_per_datum*n, &copied);
+	if (n < r->bytes_per_datum)
+		return -EINVAL;
+
+	n = rounddown(n, r->bytes_per_datum);
+	ret = kfifo_to_user(&kf->kf, buf, n, &copied);
 
 	return copied;
 }
 
 const struct iio_buffer_access_funcs kfifo_access_funcs = {
-	.mark_in_use = &iio_mark_kfifo_in_use,
-	.unmark_in_use = &iio_unmark_kfifo_in_use,
 	.store_to = &iio_store_to_kfifo,
 	.read_first_n = &iio_read_first_n_kfifo,
-	.mark_param_change = &iio_mark_update_needed_kfifo,
 	.request_update = &iio_request_update_kfifo,
 	.get_bytes_per_datum = &iio_get_bytes_per_datum_kfifo,
 	.set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo,
diff --git a/drivers/staging/iio/kfifo_buf.h b/drivers/staging/iio/kfifo_buf.h
index a15598bb9fdb..cc2bd9a1ccfe 100644
--- a/drivers/staging/iio/kfifo_buf.h
+++ b/drivers/staging/iio/kfifo_buf.h
@@ -1,7 +1,7 @@
 
 #include <linux/kfifo.h>
 #include "iio.h"
-#include "buffer_generic.h"
+#include "buffer.h"
 
 extern const struct iio_buffer_access_funcs kfifo_access_funcs;
 
diff --git a/drivers/staging/iio/light/isl29018.c b/drivers/staging/iio/light/isl29018.c
index 47638362224b..849d6a564afa 100644
--- a/drivers/staging/iio/light/isl29018.c
+++ b/drivers/staging/iio/light/isl29018.c
@@ -362,8 +362,7 @@ static int isl29018_write_raw(struct iio_dev *indio_dev,
 	int ret = -EINVAL;
 
 	mutex_lock(&chip->lock);
-	if (mask == (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE) &&
-	    chan->type == IIO_LIGHT) {
+	if (mask == IIO_CHAN_INFO_CALIBSCALE && chan->type == IIO_LIGHT) {
 		chip->lux_scale = val;
 		ret = 0;
 	}
@@ -402,7 +401,7 @@ static int isl29018_read_raw(struct iio_dev *indio_dev,
 		if (!ret)
 			ret = IIO_VAL_INT;
 		break;
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		if (chan->type == IIO_LIGHT) {
 			*val = chip->lux_scale;
 			ret = IIO_VAL_INT;
@@ -421,7 +420,7 @@ static const struct iio_chan_spec isl29018_channels[] = {
 		.indexed = 1,
 		.channel = 0,
 		.processed_val = IIO_PROCESSED,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
 	}, {
 		.type = IIO_INTENSITY,
 		.modified = 1,
diff --git a/drivers/staging/iio/light/tsl2563.c b/drivers/staging/iio/light/tsl2563.c
index 1942db13b03b..ffca85e81ef5 100644
--- a/drivers/staging/iio/light/tsl2563.c
+++ b/drivers/staging/iio/light/tsl2563.c
@@ -37,6 +37,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
+#include "../events.h"
 #include "tsl2563.h"
 
 /* Use this many bits for fraction part. */
@@ -226,6 +227,8 @@ static int tsl2563_read_id(struct tsl2563_chip *chip, u8 *id)
 	if (ret < 0)
 		return ret;
 
+	*id = ret;
+
 	return 0;
 }
 
@@ -510,7 +513,7 @@ static int tsl2563_read_raw(struct iio_dev *indio_dev,
 		}
 		break;
 
-	case (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE):
+	case IIO_CHAN_INFO_CALIBSCALE:
 		if (chan->channel == 0)
 			*val = calib_to_sysfs(chip->calib0);
 		else
@@ -536,7 +539,7 @@ static const struct iio_chan_spec tsl2563_channels[] = {
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.channel2 = IIO_MOD_LIGHT_BOTH,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE),
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
 		.event_mask = (IIO_EV_BIT(IIO_EV_TYPE_THRESH,
 					  IIO_EV_DIR_RISING) |
 			       IIO_EV_BIT(IIO_EV_TYPE_THRESH,
@@ -544,8 +547,8 @@ static const struct iio_chan_spec tsl2563_channels[] = {
 	}, {
 		.type = IIO_INTENSITY,
 		.modified = 1,
-		.channel2 = IIO_MOD_LIGHT_BOTH,
-		.info_mask = (1 << IIO_CHAN_INFO_CALIBSCALE_SEPARATE),
+		.channel2 = IIO_MOD_LIGHT_IR,
+		.info_mask = IIO_CHAN_INFO_CALIBSCALE_SEPARATE_BIT,
 	}
 };
 
diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c
index 3836f73a5296..5b6455a238d8 100644
--- a/drivers/staging/iio/light/tsl2583.c
+++ b/drivers/staging/iio/light/tsl2583.c
@@ -194,6 +194,7 @@ static int taos_get_lux(struct iio_dev *indio_dev)
 {
 	u16 ch0, ch1; /* separated ch0/ch1 data from device */
 	u32 lux; /* raw lux calculated from device data */
+	u64 lux64;
 	u32 ratio;
 	u8 buf[5];
 	struct taos_lux *p;
@@ -297,9 +298,19 @@ static int taos_get_lux(struct iio_dev *indio_dev)
 		lux = (lux + (chip->als_time_scale >> 1)) /
 			chip->als_time_scale;
 
-	/* adjust for active gain scale */
-	lux >>= 13; /* tables have factor of 8192 builtin for accuracy */
-	lux = (lux * chip->taos_settings.als_gain_trim + 500) / 1000;
+	/* Adjust for active gain scale.
+	 * The taos_device_lux tables above have a factor of 8192 built in,
+	 * so we need to shift right.
+	 * User-specified gain provides a multiplier.
+	 * Apply user-specified gain before shifting right to retain precision.
+	 * Use 64 bits to avoid overflow on multiplication.
+	 * Then go back to 32 bits before division to avoid using div_u64().
+	 */
+	lux64 = lux;
+	lux64 = lux64 * chip->taos_settings.als_gain_trim;
+	lux64 >>= 13;
+	lux = lux64;
+	lux = (lux + 500) / 1000;
 	if (lux > TSL258X_LUX_CALC_OVER_FLOW) { /* check for overflow */
 return_max:
 		lux = TSL258X_LUX_CALC_OVER_FLOW;
diff --git a/drivers/staging/iio/magnetometer/ak8975.c b/drivers/staging/iio/magnetometer/ak8975.c
index db31d6d0e5b6..3158f12cb051 100644
--- a/drivers/staging/iio/magnetometer/ak8975.c
+++ b/drivers/staging/iio/magnetometer/ak8975.c
@@ -431,7 +431,7 @@ static int ak8975_read_raw(struct iio_dev *indio_dev,
 	switch (mask) {
 	case 0:
 		return ak8975_read_axis(indio_dev, chan->address, val);
-	case (1 << IIO_CHAN_INFO_SCALE_SEPARATE):
+	case IIO_CHAN_INFO_SCALE:
 		*val = data->raw_to_gauss[chan->address];
 		return IIO_VAL_INT;
 	}
@@ -443,7 +443,7 @@ static int ak8975_read_raw(struct iio_dev *indio_dev,
 		.type = IIO_MAGN,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SEPARATE),	\
+		.info_mask = IIO_CHAN_INFO_SCALE_SEPARATE_BIT,	\
 		.address = index,					\
 	}
 
diff --git a/drivers/staging/iio/magnetometer/hmc5843.c b/drivers/staging/iio/magnetometer/hmc5843.c
index 7bb1bc605136..f2e85a9cf196 100644
--- a/drivers/staging/iio/magnetometer/hmc5843.c
+++ b/drivers/staging/iio/magnetometer/hmc5843.c
@@ -463,7 +463,7 @@ static int hmc5843_read_raw(struct iio_dev *indio_dev,
 		return hmc5843_read_measurement(indio_dev,
 						chan->address,
 						val);
-	case (1 << IIO_CHAN_INFO_SCALE_SHARED):
+	case IIO_CHAN_INFO_SCALE:
 		*val = 0;
 		*val2 = hmc5843_regval_to_nanoscale[data->range];
 		return IIO_VAL_INT_PLUS_NANO;
@@ -476,7 +476,7 @@ static int hmc5843_read_raw(struct iio_dev *indio_dev,
 		.type = IIO_MAGN,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask = (1 << IIO_CHAN_INFO_SCALE_SHARED),		\
+		.info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT,		\
 		.address = add						\
 	}
 
@@ -605,6 +605,7 @@ static const struct i2c_device_id hmc5843_id[] = {
 	{ "hmc5843", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, hmc5843_id);
 
 static struct i2c_driver hmc5843_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/meter/ade7753.c b/drivers/staging/iio/meter/ade7753.c
index 4c7b0cbf49fa..57baac6c0d40 100644
--- a/drivers/staging/iio/meter/ade7753.c
+++ b/drivers/staging/iio/meter/ade7753.c
@@ -582,3 +582,4 @@ module_spi_driver(ade7753_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADE7753/6 Single-Phase Multifunction Meter");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:ade7753");
diff --git a/drivers/staging/iio/meter/ade7754.c b/drivers/staging/iio/meter/ade7754.c
index 15c98cde76d1..8d81c92007e9 100644
--- a/drivers/staging/iio/meter/ade7754.c
+++ b/drivers/staging/iio/meter/ade7754.c
@@ -605,3 +605,4 @@ module_spi_driver(ade7754_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADE7754 Polyphase Multifunction Energy Metering IC Driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:ad7754");
diff --git a/drivers/staging/iio/meter/ade7758_core.c b/drivers/staging/iio/meter/ade7758_core.c
index 39338bcb1872..dcb20294dfe8 100644
--- a/drivers/staging/iio/meter/ade7758_core.c
+++ b/drivers/staging/iio/meter/ade7758_core.c
@@ -20,7 +20,7 @@
 
 #include "../iio.h"
 #include "../sysfs.h"
-#include "../buffer_generic.h"
+#include "../buffer.h"
 #include "meter.h"
 #include "ade7758.h"
 
@@ -663,63 +663,63 @@ static const struct attribute_group ade7758_attribute_group = {
 
 static struct iio_chan_spec ade7758_channels[] = {
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "raw", 0, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_A, AD7758_VOLTAGE),
 		0, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_CURRENT, 0, 1, 0, "raw", 0, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_A, AD7758_CURRENT),
 		1, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "apparent_raw", 0, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_A, AD7758_APP_PWR),
 		2, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "active_raw", 0, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_A, AD7758_ACT_PWR),
 		3, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "reactive_raw", 0, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_A, AD7758_REACT_PWR),
 		4, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "raw", 1, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_B, AD7758_VOLTAGE),
 		5, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_CURRENT, 0, 1, 0, "raw", 1, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_B, AD7758_CURRENT),
 		6, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "apparent_raw", 1, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_B, AD7758_APP_PWR),
 		7, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "active_raw", 1, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_B, AD7758_ACT_PWR),
 		8, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "reactive_raw", 1, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_B, AD7758_REACT_PWR),
 		9, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_VOLTAGE, 0, 1, 0, "raw", 2, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_C, AD7758_VOLTAGE),
 		10, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_CURRENT, 0, 1, 0, "raw", 2, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_C, AD7758_CURRENT),
 		11, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "apparent_raw", 2, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_C, AD7758_APP_PWR),
 		12, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "active_raw", 2, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_C, AD7758_ACT_PWR),
 		13, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN(IIO_POWER, 0, 1, 0, "reactive_raw", 2, 0,
-		(1 << IIO_CHAN_INFO_SCALE_SHARED),
+		IIO_CHAN_INFO_SCALE_SHARED_BIT,
 		AD7758_WT(AD7758_PHASE_C, AD7758_REACT_PWR),
 		14, IIO_ST('s', 24, 32, 0), 0),
 	IIO_CHAN_SOFT_TIMESTAMP(15),
@@ -746,12 +746,12 @@ static int __devinit ade7758_probe(struct spi_device *spi)
 	spi_set_drvdata(spi, indio_dev);
 
 	/* Allocate the comms buffers */
-	st->rx = kzalloc(sizeof(*st->rx)*ADE7758_MAX_RX, GFP_KERNEL);
+	st->rx = kcalloc(ADE7758_MAX_RX, sizeof(*st->rx), GFP_KERNEL);
 	if (st->rx == NULL) {
 		ret = -ENOMEM;
 		goto error_free_dev;
 	}
-	st->tx = kzalloc(sizeof(*st->tx)*ADE7758_MAX_TX, GFP_KERNEL);
+	st->tx = kcalloc(ADE7758_MAX_TX, sizeof(*st->tx), GFP_KERNEL);
 	if (st->tx == NULL) {
 		ret = -ENOMEM;
 		goto error_free_rx;
@@ -843,6 +843,7 @@ static const struct spi_device_id ade7758_id[] = {
 	{"ade7758", 0},
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ade7758_id);
 
 static struct spi_driver ade7758_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/meter/ade7758_ring.c b/drivers/staging/iio/meter/ade7758_ring.c
index 00fa2ac5c459..f29f2b278fe4 100644
--- a/drivers/staging/iio/meter/ade7758_ring.c
+++ b/drivers/staging/iio/meter/ade7758_ring.c
@@ -67,7 +67,7 @@ static irqreturn_t ade7758_trigger_handler(int irq, void *p)
 	s64 dat64[2];
 	u32 *dat32 = (u32 *)dat64;
 
-	if (ring->scan_count)
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		if (ade7758_spi_read_burst(&indio_dev->dev) >= 0)
 			*dat32 = get_unaligned_be32(&st->rx_buf[5]) & 0xFFFFFF;
 
@@ -96,10 +96,11 @@ static int ade7758_ring_preenable(struct iio_dev *indio_dev)
 	size_t d_size;
 	unsigned channel;
 
-	if (!ring->scan_count)
+	if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 		return -EINVAL;
 
-	channel = find_first_bit(ring->scan_mask, indio_dev->masklength);
+	channel = find_first_bit(indio_dev->active_scan_mask,
+				 indio_dev->masklength);
 
 	d_size = st->ade7758_ring_channels[channel].scan_type.storagebits / 8;
 
@@ -145,8 +146,7 @@ int ade7758_configure_ring(struct iio_dev *indio_dev)
 
 	/* Effectively select the ring buffer implementation */
 	indio_dev->buffer->access = &ring_sw_access_funcs;
-	indio_dev->buffer->setup_ops = &ade7758_ring_setup_ops;
-	indio_dev->buffer->owner = THIS_MODULE;
+	indio_dev->setup_ops = &ade7758_ring_setup_ops;
 
 	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 						 &ade7758_trigger_handler,
diff --git a/drivers/staging/iio/meter/ade7759.c b/drivers/staging/iio/meter/ade7759.c
index cfa2a5eff122..0beab478dcd9 100644
--- a/drivers/staging/iio/meter/ade7759.c
+++ b/drivers/staging/iio/meter/ade7759.c
@@ -526,3 +526,4 @@ module_spi_driver(ade7759_driver);
 MODULE_AUTHOR("Barry Song <21cnbao@gmail.com>");
 MODULE_DESCRIPTION("Analog Devices ADE7759 Active Energy Metering IC Driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:ad7759");
diff --git a/drivers/staging/iio/meter/ade7854-spi.c b/drivers/staging/iio/meter/ade7854-spi.c
index c485a79aeec3..81121862c1bd 100644
--- a/drivers/staging/iio/meter/ade7854-spi.c
+++ b/drivers/staging/iio/meter/ade7854-spi.c
@@ -343,6 +343,7 @@ static const struct spi_device_id ade7854_id[] = {
 	{ "ade7878", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(spi, ade7854_id);
 
 static struct spi_driver ade7854_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/resolver/ad2s1200.c b/drivers/staging/iio/resolver/ad2s1200.c
index 1c6a02bfd45d..d8ce854c1897 100644
--- a/drivers/staging/iio/resolver/ad2s1200.c
+++ b/drivers/staging/iio/resolver/ad2s1200.c
@@ -160,6 +160,7 @@ static const struct spi_device_id ad2s1200_id[] = {
 	{ "ad2s1205" },
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad2s1200_id);
 
 static struct spi_driver ad2s1200_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/resolver/ad2s1210.c b/drivers/staging/iio/resolver/ad2s1210.c
index ff1b3316d016..c439fcf72be7 100644
--- a/drivers/staging/iio/resolver/ad2s1210.c
+++ b/drivers/staging/iio/resolver/ad2s1210.c
@@ -749,6 +749,7 @@ static const struct spi_device_id ad2s1210_id[] = {
 	{ "ad2s1210" },
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad2s1210_id);
 
 static struct spi_driver ad2s1210_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/resolver/ad2s90.c b/drivers/staging/iio/resolver/ad2s90.c
index 6d0794389e74..2a86f582ddf1 100644
--- a/drivers/staging/iio/resolver/ad2s90.c
+++ b/drivers/staging/iio/resolver/ad2s90.c
@@ -109,6 +109,7 @@ static const struct spi_device_id ad2s90_id[] = {
 	{ "ad2s90" },
 	{}
 };
+MODULE_DEVICE_TABLE(spi, ad2s90_id);
 
 static struct spi_driver ad2s90_driver = {
 	.driver = {
diff --git a/drivers/staging/iio/ring_sw.c b/drivers/staging/iio/ring_sw.c
index 66a34addb75f..3e24ec455854 100644
--- a/drivers/staging/iio/ring_sw.c
+++ b/drivers/staging/iio/ring_sw.c
@@ -23,11 +23,8 @@
  * @data:		the ring buffer memory
  * @read_p:		read pointer (oldest available)
  * @write_p:		write pointer
- * @last_written_p:	read pointer (newest available)
  * @half_p:		half buffer length behind write_p (event generation)
- * @use_count:		reference count to prevent resizing when in use
  * @update_needed:	flag to indicated change in size requested
- * @use_lock:		lock to prevent change in size when in use
  *
  * Note that the first element of all ring buffers must be a
  * struct iio_buffer.
@@ -37,12 +34,9 @@ struct iio_sw_ring_buffer {
 	unsigned char		*data;
 	unsigned char		*read_p;
 	unsigned char		*write_p;
-	unsigned char		*last_written_p;
 	/* used to act as a point at which to signal an event */
 	unsigned char		*half_p;
-	int			use_count;
 	int			update_needed;
-	spinlock_t		use_lock;
 };
 
 #define iio_to_sw_ring(r) container_of(r, struct iio_sw_ring_buffer, buf)
@@ -56,38 +50,15 @@ static inline int __iio_allocate_sw_ring_buffer(struct iio_sw_ring_buffer *ring,
 	ring->data = kmalloc(length*ring->buf.bytes_per_datum, GFP_ATOMIC);
 	ring->read_p = NULL;
 	ring->write_p = NULL;
-	ring->last_written_p = NULL;
 	ring->half_p = NULL;
 	return ring->data ? 0 : -ENOMEM;
 }
 
-static inline void __iio_init_sw_ring_buffer(struct iio_sw_ring_buffer *ring)
-{
-	spin_lock_init(&ring->use_lock);
-}
-
 static inline void __iio_free_sw_ring_buffer(struct iio_sw_ring_buffer *ring)
 {
 	kfree(ring->data);
 }
 
-static void iio_mark_sw_rb_in_use(struct iio_buffer *r)
-{
-	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
-	spin_lock(&ring->use_lock);
-	ring->use_count++;
-	spin_unlock(&ring->use_lock);
-}
-
-static void iio_unmark_sw_rb_in_use(struct iio_buffer *r)
-{
-	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
-	spin_lock(&ring->use_lock);
-	ring->use_count--;
-	spin_unlock(&ring->use_lock);
-}
-
-
 /* Ring buffer related functionality */
 /* Store to ring is typically called in the bh of a data ready interrupt handler
  * in the device driver */
@@ -115,7 +86,6 @@ static int iio_store_to_sw_ring(struct iio_sw_ring_buffer *ring,
 	 * Always valid as either points to latest or second latest value.
 	 * Before this runs it is null and read attempts fail with -EAGAIN.
 	 */
-	ring->last_written_p = ring->write_p;
 	barrier();
 	/* temp_ptr used to ensure we never have an invalid pointer
 	 * it may be slightly lagging, but never invalid
@@ -174,6 +144,7 @@ static int iio_read_first_n_sw_rb(struct iio_buffer *r,
 	u8 *initial_read_p, *initial_write_p, *current_read_p, *end_read_p;
 	u8 *data;
 	int ret, max_copied, bytes_to_rip, dead_offset;
+	size_t data_available, buffer_size;
 
 	/* A userspace program has probably made an error if it tries to
 	 *  read something that is not a whole number of bpds.
@@ -186,9 +157,11 @@ static int iio_read_first_n_sw_rb(struct iio_buffer *r,
 		       n, ring->buf.bytes_per_datum);
 		goto error_ret;
 	}
+
+	buffer_size = ring->buf.bytes_per_datum*ring->buf.length;
+
 	/* Limit size to whole of ring buffer */
-	bytes_to_rip = min((size_t)(ring->buf.bytes_per_datum*ring->buf.length),
-			   n);
+	bytes_to_rip = min_t(size_t, buffer_size, n);
 
 	data = kmalloc(bytes_to_rip, GFP_KERNEL);
 	if (data == NULL) {
@@ -217,38 +190,24 @@ static int iio_read_first_n_sw_rb(struct iio_buffer *r,
 		goto error_free_data_cpy;
 	}
 
-	if (initial_write_p >= initial_read_p + bytes_to_rip) {
-		/* write_p is greater than necessary, all is easy */
-		max_copied = bytes_to_rip;
-		memcpy(data, initial_read_p, max_copied);
-		end_read_p = initial_read_p + max_copied;
-	} else if (initial_write_p > initial_read_p) {
-		/*not enough data to cpy */
-		max_copied = initial_write_p - initial_read_p;
+	if (initial_write_p >= initial_read_p)
+		data_available = initial_write_p - initial_read_p;
+	else
+		data_available = buffer_size - (initial_read_p - initial_write_p);
+
+	if (data_available < bytes_to_rip)
+		bytes_to_rip = data_available;
+
+	if (initial_read_p + bytes_to_rip >= ring->data + buffer_size) {
+		max_copied = ring->data + buffer_size - initial_read_p;
 		memcpy(data, initial_read_p, max_copied);
-		end_read_p = initial_write_p;
+		memcpy(data + max_copied, ring->data, bytes_to_rip - max_copied);
+		end_read_p = ring->data + bytes_to_rip - max_copied;
 	} else {
-		/* going through 'end' of ring buffer */
-		max_copied = ring->data
-			+ ring->buf.length*ring->buf.bytes_per_datum - initial_read_p;
-		memcpy(data, initial_read_p, max_copied);
-		/* possible we are done if we align precisely with end */
-		if (max_copied == bytes_to_rip)
-			end_read_p = ring->data;
-		else if (initial_write_p
-			 > ring->data + bytes_to_rip - max_copied) {
-			/* enough data to finish */
-			memcpy(data + max_copied, ring->data,
-			       bytes_to_rip - max_copied);
-			max_copied = bytes_to_rip;
-			end_read_p = ring->data + (bytes_to_rip - max_copied);
-		} else {  /* not enough data */
-			memcpy(data + max_copied, ring->data,
-			       initial_write_p - ring->data);
-			max_copied += initial_write_p - ring->data;
-			end_read_p = initial_write_p;
-		}
+		memcpy(data, initial_read_p, bytes_to_rip);
+		end_read_p = initial_read_p + bytes_to_rip;
 	}
+
 	/* Now to verify which section was cleanly copied - i.e. how far
 	 * read pointer has been pushed */
 	current_read_p = ring->read_p;
@@ -256,15 +215,14 @@ static int iio_read_first_n_sw_rb(struct iio_buffer *r,
 	if (initial_read_p <= current_read_p)
 		dead_offset = current_read_p - initial_read_p;
 	else
-		dead_offset = ring->buf.length*ring->buf.bytes_per_datum
-			- (initial_read_p - current_read_p);
+		dead_offset = buffer_size - (initial_read_p - current_read_p);
 
 	/* possible issue if the initial write has been lapped or indeed
 	 * the point we were reading to has been passed */
 	/* No valid data read.
 	 * In this case the read pointer is already correct having been
 	 * pushed further than we would look. */
-	if (max_copied - dead_offset < 0) {
+	if (bytes_to_rip - dead_offset < 0) {
 		ret = 0;
 		goto error_free_data_cpy;
 	}
@@ -280,7 +238,7 @@ static int iio_read_first_n_sw_rb(struct iio_buffer *r,
 	while (ring->read_p != end_read_p)
 		ring->read_p = end_read_p;
 
-	ret = max_copied - dead_offset;
+	ret = bytes_to_rip - dead_offset;
 
 	if (copy_to_user(buf, data + dead_offset, ret))  {
 		ret =  -EFAULT;
@@ -305,52 +263,18 @@ static int iio_store_to_sw_rb(struct iio_buffer *r,
 	return iio_store_to_sw_ring(ring, data, timestamp);
 }
 
-static int iio_read_last_from_sw_ring(struct iio_sw_ring_buffer *ring,
-				      unsigned char *data)
-{
-	unsigned char *last_written_p_copy;
-
-	iio_mark_sw_rb_in_use(&ring->buf);
-again:
-	barrier();
-	last_written_p_copy = ring->last_written_p;
-	barrier(); /*unnessecary? */
-	/* Check there is anything here */
-	if (last_written_p_copy == NULL)
-		return -EAGAIN;
-	memcpy(data, last_written_p_copy, ring->buf.bytes_per_datum);
-
-	if (unlikely(ring->last_written_p != last_written_p_copy))
-		goto again;
-
-	iio_unmark_sw_rb_in_use(&ring->buf);
-	return 0;
-}
-
-static int iio_read_last_from_sw_rb(struct iio_buffer *r,
-			     unsigned char *data)
-{
-	return iio_read_last_from_sw_ring(iio_to_sw_ring(r), data);
-}
-
 static int iio_request_update_sw_rb(struct iio_buffer *r)
 {
 	int ret = 0;
 	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
 
 	r->stufftoread = false;
-	spin_lock(&ring->use_lock);
 	if (!ring->update_needed)
 		goto error_ret;
-	if (ring->use_count) {
-		ret = -EAGAIN;
-		goto error_ret;
-	}
 	__iio_free_sw_ring_buffer(ring);
 	ret = __iio_allocate_sw_ring_buffer(ring, ring->buf.bytes_per_datum,
 					    ring->buf.length);
 error_ret:
-	spin_unlock(&ring->use_lock);
 	return ret;
 }
 
@@ -360,12 +284,18 @@ static int iio_get_bytes_per_datum_sw_rb(struct iio_buffer *r)
 	return ring->buf.bytes_per_datum;
 }
 
+static int iio_mark_update_needed_sw_rb(struct iio_buffer *r)
+{
+	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
+	ring->update_needed = true;
+	return 0;
+}
+
 static int iio_set_bytes_per_datum_sw_rb(struct iio_buffer *r, size_t bpd)
 {
 	if (r->bytes_per_datum != bpd) {
 		r->bytes_per_datum = bpd;
-		if (r->access->mark_param_change)
-			r->access->mark_param_change(r);
+		iio_mark_update_needed_sw_rb(r);
 	}
 	return 0;
 }
@@ -379,27 +309,17 @@ static int iio_set_length_sw_rb(struct iio_buffer *r, int length)
 {
 	if (r->length != length) {
 		r->length = length;
-		if (r->access->mark_param_change)
-			r->access->mark_param_change(r);
+		iio_mark_update_needed_sw_rb(r);
 	}
 	return 0;
 }
 
-static int iio_mark_update_needed_sw_rb(struct iio_buffer *r)
-{
-	struct iio_sw_ring_buffer *ring = iio_to_sw_ring(r);
-	ring->update_needed = true;
-	return 0;
-}
-
 static IIO_BUFFER_ENABLE_ATTR;
-static IIO_BUFFER_BYTES_PER_DATUM_ATTR;
 static IIO_BUFFER_LENGTH_ATTR;
 
 /* Standard set of ring buffer attributes */
 static struct attribute *iio_ring_attributes[] = {
 	&dev_attr_length.attr,
-	&dev_attr_bytes_per_datum.attr,
 	&dev_attr_enable.attr,
 	NULL,
 };
@@ -419,8 +339,7 @@ struct iio_buffer *iio_sw_rb_allocate(struct iio_dev *indio_dev)
 		return NULL;
 	ring->update_needed = true;
 	buf = &ring->buf;
-	iio_buffer_init(buf, indio_dev);
-	__iio_init_sw_ring_buffer(ring);
+	iio_buffer_init(buf);
 	buf->attrs = &iio_ring_attribute_group;
 
 	return buf;
@@ -434,12 +353,8 @@ void iio_sw_rb_free(struct iio_buffer *r)
 EXPORT_SYMBOL(iio_sw_rb_free);
 
 const struct iio_buffer_access_funcs ring_sw_access_funcs = {
-	.mark_in_use = &iio_mark_sw_rb_in_use,
-	.unmark_in_use = &iio_unmark_sw_rb_in_use,
 	.store_to = &iio_store_to_sw_rb,
-	.read_last = &iio_read_last_from_sw_rb,
 	.read_first_n = &iio_read_first_n_sw_rb,
-	.mark_param_change = &iio_mark_update_needed_sw_rb,
 	.request_update = &iio_request_update_sw_rb,
 	.get_bytes_per_datum = &iio_get_bytes_per_datum_sw_rb,
 	.set_bytes_per_datum = &iio_set_bytes_per_datum_sw_rb,
diff --git a/drivers/staging/iio/ring_sw.h b/drivers/staging/iio/ring_sw.h
index a3e15784c225..e6a6e2c40960 100644
--- a/drivers/staging/iio/ring_sw.h
+++ b/drivers/staging/iio/ring_sw.h
@@ -23,7 +23,7 @@
 
 #ifndef _IIO_RING_SW_H_
 #define _IIO_RING_SW_H_
-#include "buffer_generic.h"
+#include "buffer.h"
 
 /**
  * ring_sw_access_funcs - access functions for a software ring buffer
diff --git a/drivers/staging/iio/sysfs.h b/drivers/staging/iio/sysfs.h
index 868952b5ba63..bfedb73b850e 100644
--- a/drivers/staging/iio/sysfs.h
+++ b/drivers/staging/iio/sysfs.h
@@ -114,47 +114,4 @@ struct iio_const_attr {
 #define IIO_CONST_ATTR_TEMP_SCALE(_string)		\
 	IIO_CONST_ATTR(in_temp_scale, _string)
 
-enum iio_event_type {
-	IIO_EV_TYPE_THRESH,
-	IIO_EV_TYPE_MAG,
-	IIO_EV_TYPE_ROC,
-	IIO_EV_TYPE_THRESH_ADAPTIVE,
-	IIO_EV_TYPE_MAG_ADAPTIVE,
-};
-
-enum iio_event_direction {
-	IIO_EV_DIR_EITHER,
-	IIO_EV_DIR_RISING,
-	IIO_EV_DIR_FALLING,
-};
-
-#define IIO_EVENT_CODE(chan_type, diff, modifier, direction,		\
-		       type, chan, chan1, chan2)			\
-	(((u64)type << 56) | ((u64)diff << 55) |			\
-	 ((u64)direction << 48) | ((u64)modifier << 40) |		\
-	 ((u64)chan_type << 32) | (chan2 << 16) | chan1 | chan)
-
-#define IIO_EV_DIR_MAX 4
-#define IIO_EV_BIT(type, direction)			\
-	(1 << (type*IIO_EV_DIR_MAX + direction))
-
-#define IIO_MOD_EVENT_CODE(channelclass, number, modifier,		\
-			   type, direction)				\
-	IIO_EVENT_CODE(channelclass, 0, modifier, direction, type, number, 0, 0)
-
-#define IIO_UNMOD_EVENT_CODE(channelclass, number, type, direction)	\
-	IIO_EVENT_CODE(channelclass, 0, 0, direction, type, number, 0, 0)
-
-#define IIO_EVENT_CODE_EXTRACT_TYPE(mask) ((mask >> 56) & 0xFF)
-
-#define IIO_EVENT_CODE_EXTRACT_DIR(mask) ((mask >> 48) & 0xCF)
-
-#define IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(mask) ((mask >> 32) & 0xFF)
-
-/* Event code number extraction depends on which type of event we have.
- * Perhaps review this function in the future*/
-#define IIO_EVENT_CODE_EXTRACT_NUM(mask) (mask & 0xFFFF)
-
-#define IIO_EVENT_CODE_EXTRACT_MODIFIER(mask) ((mask >> 40) & 0xFF)
-
 #endif /* _INDUSTRIAL_IO_SYSFS_H_ */
diff --git a/drivers/staging/iio/trigger.h b/drivers/staging/iio/trigger.h
index 5cc42a655c88..1cfca231db8f 100644
--- a/drivers/staging/iio/trigger.h
+++ b/drivers/staging/iio/trigger.h
@@ -46,7 +46,6 @@ struct iio_trigger_ops {
  * @private_data:	[DRIVER] device specific data
  * @list:		[INTERN] used in maintenance of global trigger list
  * @alloc_list:		[DRIVER] used for driver specific trigger list
- * @owner:		[DRIVER] used to monitor usage count of the trigger.
  * @use_count:		use count for the trigger
  * @subirq_chip:	[INTERN] associate 'virtual' irq chip.
  * @subirq_base:	[INTERN] base number for irqs provided by trigger.
@@ -63,7 +62,6 @@ struct iio_trigger {
 	void				*private_data;
 	struct list_head		list;
 	struct list_head		alloc_list;
-	struct module			*owner;
 	int use_count;
 
 	struct irq_chip			subirq_chip;
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index d35d085da949..bd7416b2c561 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -125,7 +125,6 @@ static int iio_trig_periodic_rtc_probe(struct platform_device *dev)
 			goto error_put_trigger_and_remove_from_list;
 		}
 		trig->private_data = trig_info;
-		trig->owner = THIS_MODULE;
 		trig->ops = &iio_prtc_trigger_ops;
 		/* RTC access */
 		trig_info->rtc
diff --git a/drivers/staging/iio/types.h b/drivers/staging/iio/types.h
new file mode 100644
index 000000000000..b7d26474ad06
--- /dev/null
+++ b/drivers/staging/iio/types.h
@@ -0,0 +1,49 @@
+/* industrial I/O data types needed both in and out of kernel
+ *
+ * Copyright (c) 2008 Jonathan Cameron
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#ifndef _IIO_TYPES_H_
+#define _IIO_TYPES_H_
+
+enum iio_chan_type {
+	/* real channel types */
+	IIO_VOLTAGE,
+	IIO_CURRENT,
+	IIO_POWER,
+	IIO_ACCEL,
+	IIO_ANGL_VEL,
+	IIO_MAGN,
+	IIO_LIGHT,
+	IIO_INTENSITY,
+	IIO_PROXIMITY,
+	IIO_TEMP,
+	IIO_INCLI,
+	IIO_ROT,
+	IIO_ANGL,
+	IIO_TIMESTAMP,
+	IIO_CAPACITANCE,
+};
+
+enum iio_modifier {
+	IIO_NO_MOD,
+	IIO_MOD_X,
+	IIO_MOD_Y,
+	IIO_MOD_Z,
+	IIO_MOD_X_AND_Y,
+	IIO_MOD_X_AND_Z,
+	IIO_MOD_Y_AND_Z,
+	IIO_MOD_X_AND_Y_AND_Z,
+	IIO_MOD_X_OR_Y,
+	IIO_MOD_X_OR_Z,
+	IIO_MOD_Y_OR_Z,
+	IIO_MOD_X_OR_Y_OR_Z,
+	IIO_MOD_LIGHT_BOTH,
+	IIO_MOD_LIGHT_IR,
+};
+
+#endif /* _IIO_TYPES_H_ */
diff --git a/drivers/staging/intel_sst/Kconfig b/drivers/staging/intel_sst/Kconfig
deleted file mode 100644
index 82391077b384..000000000000
--- a/drivers/staging/intel_sst/Kconfig
+++ /dev/null
@@ -1,19 +0,0 @@
-config SND_INTEL_SST
-	tristate "Intel SST (LPE) Driver"
-	depends on X86 && INTEL_SCU_IPC
-	default n
-	help
-	  Say Y here to include support for the Intel(R) MID SST DSP driver
-	  On other PC platforms if you are unsure answer 'N'
-
-config SND_INTELMID
-	tristate "Intel MID sound card driver"
-	depends on SOUND && SND
-	select SND_PCM
-	select SND_SEQUENCER
-	select SND_JACK
-	depends on SND_INTEL_SST
-	default n
-	help
-	  Say Y here to include support for the Intel(R) MID sound card driver
-	  On other PC platforms if you are unsure answer 'N'
diff --git a/drivers/staging/intel_sst/Makefile b/drivers/staging/intel_sst/Makefile
deleted file mode 100644
index 9eb7c158bb65..000000000000
--- a/drivers/staging/intel_sst/Makefile
+++ /dev/null
@@ -1,7 +0,0 @@
-#
-# Makefile for Intel MID Audio drivers
-#
-snd-intel-sst-y := intel_sst.o intel_sst_ipc.o intel_sst_stream.o intel_sst_drv_interface.o intel_sst_dsp.o intel_sst_pvt.o intel_sst_stream_encoded.o intel_sst_app_interface.o
-snd-intelmid-y := intelmid.o intelmid_msic_control.o intelmid_ctrl.o intelmid_pvt.o intelmid_v0_control.o intelmid_v1_control.o intelmid_v2_control.o
-obj-$(CONFIG_SND_INTEL_SST) += snd-intel-sst.o
-obj-$(CONFIG_SND_INTELMID) += snd-intelmid.o
diff --git a/drivers/staging/intel_sst/TODO b/drivers/staging/intel_sst/TODO
deleted file mode 100644
index c733d7011091..000000000000
--- a/drivers/staging/intel_sst/TODO
+++ /dev/null
@@ -1,13 +0,0 @@
-TODO
-----
-
-Get the memrar driver cleaned up and upstream (dependency blocking SST)
-Replace long/short press with two virtual buttons
-Review the printks and kill off any left over ST_ERR: messages
-Review the misc device ioctls for 32/64bit safety and sanity
-Review the misc device ioctls for size safety depending on config and decide
-	if space/unused areas should be left
-What the sound folks turn up on full review
-Using the ALSA frameworks properly
-
-
diff --git a/drivers/staging/intel_sst/intel_sst.c b/drivers/staging/intel_sst/intel_sst.c
deleted file mode 100644
index ff9aaec0557f..000000000000
--- a/drivers/staging/intel_sst/intel_sst.c
+++ /dev/null
@@ -1,649 +0,0 @@
-/*
- *  intel_sst.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10	Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This driver exposes the audio engine functionalities to the ALSA
- *	 and middleware.
- *
- *  This file contains all init functions
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/fs.h>
-#include <linux/interrupt.h>
-#include <linux/firmware.h>
-#include <linux/miscdevice.h>
-#include <linux/pm_runtime.h>
-#include <linux/module.h>
-#include <asm/mrst.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-
-
-MODULE_AUTHOR("Vinod Koul <vinod.koul@intel.com>");
-MODULE_AUTHOR("Harsha Priya <priya.harsha@intel.com>");
-MODULE_AUTHOR("Dharageswari R <dharageswari.r@intel.com>");
-MODULE_AUTHOR("KP Jeeja <jeeja.kp@intel.com>");
-MODULE_DESCRIPTION("Intel (R) SST(R) Audio Engine Driver");
-MODULE_LICENSE("GPL v2");
-MODULE_VERSION(SST_DRIVER_VERSION);
-
-struct intel_sst_drv *sst_drv_ctx;
-static struct mutex drv_ctx_lock;
-struct class *sst_class;
-
-/* fops Routines */
-static const struct file_operations intel_sst_fops = {
-	.owner = THIS_MODULE,
-	.open = intel_sst_open,
-	.release = intel_sst_release,
-	.read = intel_sst_read,
-	.write = intel_sst_write,
-	.unlocked_ioctl = intel_sst_ioctl,
-	.mmap = intel_sst_mmap,
-	.aio_read = intel_sst_aio_read,
-	.aio_write = intel_sst_aio_write,
-};
-static const struct file_operations intel_sst_fops_cntrl = {
-	.owner = THIS_MODULE,
-	.open = intel_sst_open_cntrl,
-	.release = intel_sst_release_cntrl,
-	.unlocked_ioctl = intel_sst_ioctl,
-};
-
-static struct miscdevice lpe_dev = {
-	.minor = MISC_DYNAMIC_MINOR,/* dynamic allocation */
-	.name = "intel_sst",/* /dev/intel_sst */
-	.fops = &intel_sst_fops
-};
-
-
-static struct miscdevice lpe_ctrl = {
-	.minor = MISC_DYNAMIC_MINOR,/* dynamic allocation */
-	.name = "intel_sst_ctrl",/* /dev/intel_sst_ctrl */
-	.fops = &intel_sst_fops_cntrl
-};
-
-/**
-* intel_sst_interrupt - Interrupt service routine for SST
-*
-* @irq:	irq number of interrupt
-* @context: pointer to device structre
-*
-* This function is called by OS when SST device raises
-* an interrupt. This will be result of write in IPC register
-* Source can be busy or done interrupt
-*/
-static irqreturn_t intel_sst_interrupt(int irq, void *context)
-{
-	union interrupt_reg isr;
-	union ipc_header header;
-	union interrupt_reg imr;
-	struct intel_sst_drv *drv = (struct intel_sst_drv *) context;
-	unsigned int size = 0, str_id;
-	struct stream_info *stream ;
-
-	/* Do not handle interrupt in suspended state */
-	if (drv->sst_state == SST_SUSPENDED)
-		return IRQ_NONE;
-	/* Interrupt arrived, check src */
-	isr.full = sst_shim_read(drv->shim, SST_ISRX);
-
-	if (isr.part.busy_interrupt) {
-		header.full = sst_shim_read(drv->shim, SST_IPCD);
-		if (header.part.msg_id == IPC_SST_PERIOD_ELAPSED) {
-			sst_clear_interrupt();
-			str_id = header.part.str_id;
-			stream = &sst_drv_ctx->streams[str_id];
-			if (stream->period_elapsed)
-				stream->period_elapsed(stream->pcm_substream);
-			return IRQ_HANDLED;
-		}
-		if (header.part.large)
-			size = header.part.data;
-		if (header.part.msg_id & REPLY_MSG) {
-			sst_drv_ctx->ipc_process_msg.header = header;
-			memcpy_fromio(sst_drv_ctx->ipc_process_msg.mailbox,
-				drv->mailbox + SST_MAILBOX_RCV, size);
-			queue_work(sst_drv_ctx->process_msg_wq,
-					&sst_drv_ctx->ipc_process_msg.wq);
-		} else {
-			sst_drv_ctx->ipc_process_reply.header = header;
-			memcpy_fromio(sst_drv_ctx->ipc_process_reply.mailbox,
-				drv->mailbox + SST_MAILBOX_RCV, size);
-			queue_work(sst_drv_ctx->process_reply_wq,
-					&sst_drv_ctx->ipc_process_reply.wq);
-		}
-		/* mask busy inetrrupt */
-		imr.full = sst_shim_read(drv->shim, SST_IMRX);
-		imr.part.busy_interrupt = 1;
-		sst_shim_write(sst_drv_ctx->shim, SST_IMRX, imr.full);
-		return IRQ_HANDLED;
-	} else if (isr.part.done_interrupt) {
-		/* Clear done bit */
-		header.full = sst_shim_read(drv->shim, SST_IPCX);
-		header.part.done = 0;
-		sst_shim_write(sst_drv_ctx->shim, SST_IPCX, header.full);
-		/* write 1 to clear status register */;
-		isr.part.done_interrupt = 1;
-		/* dummy register for shim workaround */
-		sst_shim_write(sst_drv_ctx->shim, SST_ISRX, isr.full);
-		queue_work(sst_drv_ctx->post_msg_wq,
-			&sst_drv_ctx->ipc_post_msg.wq);
-		return IRQ_HANDLED;
-	} else
-		return IRQ_NONE;
-
-}
-
-
-/*
-* intel_sst_probe - PCI probe function
-*
-* @pci:	PCI device structure
-* @pci_id: PCI device ID structure
-*
-* This function is called by OS when a device is found
-* This enables the device, interrupt etc
-*/
-static int __devinit intel_sst_probe(struct pci_dev *pci,
-			const struct pci_device_id *pci_id)
-{
-	int i, ret = 0;
-
-	pr_debug("Probe for DID %x\n", pci->device);
-	mutex_lock(&drv_ctx_lock);
-	if (sst_drv_ctx) {
-		pr_err("Only one sst handle is supported\n");
-		mutex_unlock(&drv_ctx_lock);
-		return -EBUSY;
-	}
-
-	sst_drv_ctx = kzalloc(sizeof(*sst_drv_ctx), GFP_KERNEL);
-	if (!sst_drv_ctx) {
-		pr_err("malloc fail\n");
-		mutex_unlock(&drv_ctx_lock);
-		return -ENOMEM;
-	}
-	mutex_unlock(&drv_ctx_lock);
-
-	sst_drv_ctx->pci_id = pci->device;
-
-	mutex_init(&sst_drv_ctx->stream_lock);
-	mutex_init(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->pmic_state = SND_MAD_UN_INIT;
-
-	sst_drv_ctx->stream_cnt = 0;
-	sst_drv_ctx->encoded_cnt = 0;
-	sst_drv_ctx->am_cnt = 0;
-	sst_drv_ctx->pb_streams = 0;
-	sst_drv_ctx->cp_streams = 0;
-	sst_drv_ctx->unique_id = 0;
-	sst_drv_ctx->pmic_port_instance = SST_DEFAULT_PMIC_PORT;
-
-	INIT_LIST_HEAD(&sst_drv_ctx->ipc_dispatch_list);
-	INIT_WORK(&sst_drv_ctx->ipc_post_msg.wq, sst_post_message);
-	INIT_WORK(&sst_drv_ctx->ipc_process_msg.wq, sst_process_message);
-	INIT_WORK(&sst_drv_ctx->ipc_process_reply.wq, sst_process_reply);
-	INIT_WORK(&sst_drv_ctx->mad_ops.wq, sst_process_mad_ops);
-	init_waitqueue_head(&sst_drv_ctx->wait_queue);
-
-	sst_drv_ctx->mad_wq = create_workqueue("sst_mad_wq");
-	if (!sst_drv_ctx->mad_wq)
-		goto do_free_drv_ctx;
-	sst_drv_ctx->post_msg_wq = create_workqueue("sst_post_msg_wq");
-	if (!sst_drv_ctx->post_msg_wq)
-		goto free_mad_wq;
-	sst_drv_ctx->process_msg_wq = create_workqueue("sst_process_msg_wqq");
-	if (!sst_drv_ctx->process_msg_wq)
-		goto free_post_msg_wq;
-	sst_drv_ctx->process_reply_wq = create_workqueue("sst_proces_reply_wq");
-	if (!sst_drv_ctx->process_reply_wq)
-		goto free_process_msg_wq;
-
-	for (i = 0; i < MAX_ACTIVE_STREAM; i++) {
-		sst_drv_ctx->alloc_block[i].sst_id = BLOCK_UNINIT;
-		sst_drv_ctx->alloc_block[i].ops_block.condition = false;
-	}
-	spin_lock_init(&sst_drv_ctx->list_spin_lock);
-
-	sst_drv_ctx->max_streams = pci_id->driver_data;
-	pr_debug("Got drv data max stream %d\n",
-				sst_drv_ctx->max_streams);
-	for (i = 1; i <= sst_drv_ctx->max_streams; i++) {
-		struct stream_info *stream = &sst_drv_ctx->streams[i];
-		INIT_LIST_HEAD(&stream->bufs);
-		mutex_init(&stream->lock);
-		spin_lock_init(&stream->pcm_lock);
-	}
-	if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID) {
-		sst_drv_ctx->mmap_mem = NULL;
-		sst_drv_ctx->mmap_len = SST_MMAP_PAGES * PAGE_SIZE;
-		while (sst_drv_ctx->mmap_len > 0) {
-			sst_drv_ctx->mmap_mem =
-				kzalloc(sst_drv_ctx->mmap_len, GFP_KERNEL);
-			if (sst_drv_ctx->mmap_mem) {
-				pr_debug("Got memory %p size 0x%x\n",
-					sst_drv_ctx->mmap_mem,
-					sst_drv_ctx->mmap_len);
-				break;
-			}
-			if (sst_drv_ctx->mmap_len < (SST_MMAP_STEP*PAGE_SIZE)) {
-				pr_err("mem alloc fail...abort!!\n");
-				ret = -ENOMEM;
-				goto free_process_reply_wq;
-			}
-			sst_drv_ctx->mmap_len -= (SST_MMAP_STEP * PAGE_SIZE);
-			pr_debug("mem alloc failed...trying %d\n",
-						sst_drv_ctx->mmap_len);
-		}
-	}
-
-	/* Init the device */
-	ret = pci_enable_device(pci);
-	if (ret) {
-		pr_err("device can't be enabled\n");
-		goto do_free_mem;
-	}
-	sst_drv_ctx->pci = pci_dev_get(pci);
-	ret = pci_request_regions(pci, SST_DRV_NAME);
-	if (ret)
-		goto do_disable_device;
-	/* map registers */
-	/* SST Shim */
-	sst_drv_ctx->shim_phy_add = pci_resource_start(pci, 1);
-	sst_drv_ctx->shim = pci_ioremap_bar(pci, 1);
-	if (!sst_drv_ctx->shim)
-		goto do_release_regions;
-	pr_debug("SST Shim Ptr %p\n", sst_drv_ctx->shim);
-
-	/* Shared SRAM */
-	sst_drv_ctx->mailbox = pci_ioremap_bar(pci, 2);
-	if (!sst_drv_ctx->mailbox)
-		goto do_unmap_shim;
-	pr_debug("SRAM Ptr %p\n", sst_drv_ctx->mailbox);
-
-	/* IRAM */
-	sst_drv_ctx->iram = pci_ioremap_bar(pci, 3);
-	if (!sst_drv_ctx->iram)
-		goto do_unmap_sram;
-	pr_debug("IRAM Ptr %p\n", sst_drv_ctx->iram);
-
-	/* DRAM */
-	sst_drv_ctx->dram = pci_ioremap_bar(pci, 4);
-	if (!sst_drv_ctx->dram)
-		goto do_unmap_iram;
-	pr_debug("DRAM Ptr %p\n", sst_drv_ctx->dram);
-
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_UN_INIT;
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	/* Register the ISR */
-	ret = request_irq(pci->irq, intel_sst_interrupt,
-		IRQF_SHARED, SST_DRV_NAME, sst_drv_ctx);
-	if (ret)
-		goto do_unmap_dram;
-	pr_debug("Registered IRQ 0x%x\n", pci->irq);
-
-	/*Register LPE Control as misc driver*/
-	ret = misc_register(&lpe_ctrl);
-	if (ret) {
-		pr_err("couldn't register control device\n");
-		goto do_free_irq;
-	}
-
-	if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID) {
-		ret = misc_register(&lpe_dev);
-		if (ret) {
-			pr_err("couldn't register LPE device\n");
-			goto do_free_misc;
-		}
-	} else if (sst_drv_ctx->pci_id == SST_MFLD_PCI_ID) {
-		u32 csr;
-
-		/*allocate mem for fw context save during suspend*/
-		sst_drv_ctx->fw_cntx = kzalloc(FW_CONTEXT_MEM, GFP_KERNEL);
-		if (!sst_drv_ctx->fw_cntx) {
-			ret = -ENOMEM;
-			goto do_free_misc;
-		}
-		/*setting zero as that is valid mem to restore*/
-		sst_drv_ctx->fw_cntx_size = 0;
-
-		/*set lpe start clock and ram size*/
-		csr = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-		csr |= 0x30060; /*remove the clock ratio after fw fix*/
-		sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr);
-	}
-	sst_drv_ctx->lpe_stalled = 0;
-	pci_set_drvdata(pci, sst_drv_ctx);
-	pm_runtime_allow(&pci->dev);
-	pm_runtime_put_noidle(&pci->dev);
-	pr_debug("...successfully done!!!\n");
-	return ret;
-
-do_free_misc:
-	misc_deregister(&lpe_ctrl);
-do_free_irq:
-	free_irq(pci->irq, sst_drv_ctx);
-do_unmap_dram:
-	iounmap(sst_drv_ctx->dram);
-do_unmap_iram:
-	iounmap(sst_drv_ctx->iram);
-do_unmap_sram:
-	iounmap(sst_drv_ctx->mailbox);
-do_unmap_shim:
-	iounmap(sst_drv_ctx->shim);
-do_release_regions:
-	pci_release_regions(pci);
-do_disable_device:
-	pci_disable_device(pci);
-do_free_mem:
-	kfree(sst_drv_ctx->mmap_mem);
-free_process_reply_wq:
-	destroy_workqueue(sst_drv_ctx->process_reply_wq);
-free_process_msg_wq:
-	destroy_workqueue(sst_drv_ctx->process_msg_wq);
-free_post_msg_wq:
-	destroy_workqueue(sst_drv_ctx->post_msg_wq);
-free_mad_wq:
-	destroy_workqueue(sst_drv_ctx->mad_wq);
-do_free_drv_ctx:
-	kfree(sst_drv_ctx);
-	sst_drv_ctx = NULL;
-	pr_err("Probe failed with %d\n", ret);
-	return ret;
-}
-
-/**
-* intel_sst_remove - PCI remove function
-*
-* @pci:	PCI device structure
-*
-* This function is called by OS when a device is unloaded
-* This frees the interrupt etc
-*/
-static void __devexit intel_sst_remove(struct pci_dev *pci)
-{
-	pm_runtime_get_noresume(&pci->dev);
-	pm_runtime_forbid(&pci->dev);
-	pci_dev_put(sst_drv_ctx->pci);
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_UN_INIT;
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	misc_deregister(&lpe_ctrl);
-	free_irq(pci->irq, sst_drv_ctx);
-	iounmap(sst_drv_ctx->dram);
-	iounmap(sst_drv_ctx->iram);
-	iounmap(sst_drv_ctx->mailbox);
-	iounmap(sst_drv_ctx->shim);
-	sst_drv_ctx->pmic_state = SND_MAD_UN_INIT;
-	if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID) {
-		misc_deregister(&lpe_dev);
-		kfree(sst_drv_ctx->mmap_mem);
-	} else
-		kfree(sst_drv_ctx->fw_cntx);
-	flush_scheduled_work();
-	destroy_workqueue(sst_drv_ctx->process_reply_wq);
-	destroy_workqueue(sst_drv_ctx->process_msg_wq);
-	destroy_workqueue(sst_drv_ctx->post_msg_wq);
-	destroy_workqueue(sst_drv_ctx->mad_wq);
-	kfree(pci_get_drvdata(pci));
-	sst_drv_ctx = NULL;
-	pci_release_regions(pci);
-	pci_disable_device(pci);
-	pci_set_drvdata(pci, NULL);
-}
-
-void sst_save_dsp_context(void)
-{
-	struct snd_sst_ctxt_params fw_context;
-	unsigned int pvt_id, i;
-	struct ipc_post *msg = NULL;
-
-	/*check cpu type*/
-	if (sst_drv_ctx->pci_id != SST_MFLD_PCI_ID)
-		return;
-		/*not supported for rest*/
-	if (sst_drv_ctx->sst_state != SST_FW_RUNNING) {
-		pr_debug("fw not running no context save ...\n");
-		return;
-	}
-
-	/*send msg to fw*/
-	if (sst_create_large_msg(&msg))
-		return;
-	pvt_id = sst_assign_pvt_id(sst_drv_ctx);
-	i = sst_get_block_stream(sst_drv_ctx);
-	sst_drv_ctx->alloc_block[i].sst_id = pvt_id;
-	sst_fill_header(&msg->header, IPC_IA_GET_FW_CTXT, 1, pvt_id);
-	msg->header.part.data = sizeof(fw_context) + sizeof(u32);
-	fw_context.address = virt_to_phys((void *)sst_drv_ctx->fw_cntx);
-	fw_context.size = FW_CONTEXT_MEM;
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32),
-				&fw_context, sizeof(fw_context));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	/*wait for reply*/
-	if (sst_wait_timeout(sst_drv_ctx, &sst_drv_ctx->alloc_block[i]))
-		pr_debug("err fw context save timeout  ...\n");
-	sst_drv_ctx->alloc_block[i].sst_id = BLOCK_UNINIT;
-	pr_debug("fw context saved  ...\n");
-	return;
-}
-
-/* Power Management */
-/*
-* intel_sst_suspend - PCI suspend function
-*
-* @pci: PCI device structure
-* @state: PM message
-*
-* This function is called by OS when a power event occurs
-*/
-int intel_sst_suspend(struct pci_dev *pci, pm_message_t state)
-{
-	union config_status_reg csr;
-
-	pr_debug("intel_sst_suspend called\n");
-
-	if (sst_drv_ctx->stream_cnt) {
-		pr_err("active streams,not able to suspend\n");
-		return -EBUSY;
-	}
-	/*save fw context*/
-	sst_save_dsp_context();
-	/*Assert RESET on LPE Processor*/
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.full = csr.full | 0x2;
-	/* Move the SST state to Suspended */
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_SUSPENDED;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	pci_set_drvdata(pci, sst_drv_ctx);
-	pci_save_state(pci);
-	pci_disable_device(pci);
-	pci_set_power_state(pci, PCI_D3hot);
-	return 0;
-}
-
-/**
-* intel_sst_resume - PCI resume function
-*
-* @pci:	PCI device structure
-*
-* This function is called by OS when a power event occurs
-*/
-int intel_sst_resume(struct pci_dev *pci)
-{
-	int ret = 0;
-
-	pr_debug("intel_sst_resume called\n");
-	if (sst_drv_ctx->sst_state != SST_SUSPENDED) {
-		pr_err("SST is not in suspended state\n");
-		return 0;
-	}
-	sst_drv_ctx = pci_get_drvdata(pci);
-	pci_set_power_state(pci, PCI_D0);
-	pci_restore_state(pci);
-	ret = pci_enable_device(pci);
-	if (ret)
-		pr_err("device can't be enabled\n");
-
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_UN_INIT;
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	return 0;
-}
-
-/* The runtime_suspend/resume is pretty much similar to the legacy
- * suspend/resume with the noted exception below:
- * The PCI core takes care of taking the system through D3hot and
- * restoring it back to D0 and so there is no need to duplicate
- * that here.
- */
-static int intel_sst_runtime_suspend(struct device *dev)
-{
-	union config_status_reg csr;
-
-	pr_debug("intel_sst_runtime_suspend called\n");
-	if (sst_drv_ctx->stream_cnt) {
-		pr_err("active streams,not able to suspend\n");
-		return -EBUSY;
-	}
-	/*save fw context*/
-	sst_save_dsp_context();
-	/*Assert RESET on LPE Processor*/
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.full = csr.full | 0x2;
-	/* Move the SST state to Suspended */
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_SUSPENDED;
-
-	/* Only needed by Medfield */
-	if (sst_drv_ctx->pci_id != SST_MRST_PCI_ID)
-		sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	return 0;
-}
-
-static int intel_sst_runtime_resume(struct device *dev)
-{
-
-	pr_debug("intel_sst_runtime_resume called\n");
-	if (sst_drv_ctx->sst_state != SST_SUSPENDED) {
-		pr_err("SST is not in suspended state\n");
-		return 0;
-	}
-
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_UN_INIT;
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	return 0;
-}
-
-static int intel_sst_runtime_idle(struct device *dev)
-{
-	pr_debug("runtime_idle called\n");
-	if (sst_drv_ctx->stream_cnt == 0 && sst_drv_ctx->am_cnt == 0)
-		pm_schedule_suspend(dev, SST_SUSPEND_DELAY);
-	return -EBUSY;
-}
-
-static const struct dev_pm_ops intel_sst_pm = {
-	.runtime_suspend = intel_sst_runtime_suspend,
-	.runtime_resume = intel_sst_runtime_resume,
-	.runtime_idle = intel_sst_runtime_idle,
-};
-
-/* PCI Routines */
-static struct pci_device_id intel_sst_ids[] = {
-	{ PCI_VDEVICE(INTEL, SST_MRST_PCI_ID), 3},
-	{ PCI_VDEVICE(INTEL, SST_MFLD_PCI_ID), 6},
-	{ 0, }
-};
-MODULE_DEVICE_TABLE(pci, intel_sst_ids);
-
-static struct pci_driver driver = {
-	.name = SST_DRV_NAME,
-	.id_table = intel_sst_ids,
-	.probe = intel_sst_probe,
-	.remove = __devexit_p(intel_sst_remove),
-#ifdef CONFIG_PM
-	.suspend = intel_sst_suspend,
-	.resume = intel_sst_resume,
-	.driver = {
-		.pm = &intel_sst_pm,
-	},
-#endif
-};
-
-/**
-* intel_sst_init - Module init function
-*
-* Registers with PCI
-* Registers with /dev
-* Init all data strutures
-*/
-static int __init intel_sst_init(void)
-{
-	/* Init all variables, data structure etc....*/
-	int ret = 0;
-	pr_debug("INFO: ******** SST DRIVER loading.. Ver: %s\n",
-				       SST_DRIVER_VERSION);
-
-	mutex_init(&drv_ctx_lock);
-	/* Register with PCI */
-	ret = pci_register_driver(&driver);
-	if (ret)
-		pr_err("PCI register failed\n");
-	return ret;
-}
-
-/**
-* intel_sst_exit - Module exit function
-*
-* Unregisters with PCI
-* Unregisters with /dev
-* Frees all data strutures
-*/
-static void __exit intel_sst_exit(void)
-{
-	pci_unregister_driver(&driver);
-
-	pr_debug("driver unloaded\n");
-	sst_drv_ctx = NULL;
-	return;
-}
-
-module_init(intel_sst_init);
-module_exit(intel_sst_exit);
diff --git a/drivers/staging/intel_sst/intel_sst.h b/drivers/staging/intel_sst/intel_sst.h
deleted file mode 100644
index 4ad2829105a7..000000000000
--- a/drivers/staging/intel_sst/intel_sst.h
+++ /dev/null
@@ -1,162 +0,0 @@
-#ifndef __INTEL_SST_H__
-#define __INTEL_SST_H__
-/*
- *  intel_sst.h - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corporation
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This driver exposes the audio engine functionalities to the ALSA
- *	and middleware.
- *  This file is shared between the SST and MAD drivers
- */
-#include "intel_sst_ioctl.h"
-#include <sound/jack.h>
-
-#define SST_CARD_NAMES "intel_mid_card"
-
-#define MFLD_MAX_HW_CH 4
-/* control list Pmic & Lpe */
-/* Input controls */
-enum port_status {
-	ACTIVATE = 1,
-	DEACTIVATE,
-};
-
-/* Card states */
-enum sst_card_states {
-	SND_CARD_UN_INIT = 0,
-	SND_CARD_INIT_DONE,
-};
-
-enum sst_controls {
-	SST_SND_ALLOC =			0x1000,
-	SST_SND_PAUSE =			0x1001,
-	SST_SND_RESUME =		0x1002,
-	SST_SND_DROP =			0x1003,
-	SST_SND_FREE =			0x1004,
-	SST_SND_BUFFER_POINTER =	0x1005,
-	SST_SND_STREAM_INIT =		0x1006,
-	SST_SND_START	 =		0x1007,
-	SST_SND_STREAM_PROCESS =	0x1008,
-	SST_MAX_CONTROLS =		0x1008,
-	SST_CONTROL_BASE =		0x1000,
-	SST_ENABLE_RX_TIME_SLOT =	0x1009,
-};
-
-enum SND_CARDS {
-	SND_FS = 0,
-	SND_MX,
-	SND_NC,
-	SND_MSIC
-};
-
-struct pcm_stream_info {
-	int str_id;
-	void *mad_substream;
-	void (*period_elapsed) (void *mad_substream);
-	unsigned long long buffer_ptr;
-	int sfreq;
-};
-
-struct snd_pmic_ops {
-	int card_status;
-	int master_mute;
-	int num_channel;
-	int input_dev_id;
-	int mute_status;
-	struct mutex lock;
-	int pb_on, pbhs_on;
-	int cap_on;
-	int output_dev_id;
-	int lineout_dev_id, line_out_names_cnt;
-	int prev_lineout_dev_id;
-	bool jack_interrupt_status;
-	int (*set_input_dev) (u8 value);
-	int (*set_output_dev) (u8 value);
-	int (*set_lineout_dev) (u8 value);
-	int (*set_mute) (int dev_id, u8 value);
-	int (*get_mute) (int dev_id, u8 *value);
-
-	int (*set_vol) (int dev_id, int value);
-	int (*get_vol) (int dev_id, int *value);
-
-	int (*init_card) (void);
-	int (*set_pcm_audio_params)
-		(int sfreq, int word_size , int num_channel);
-	int (*set_pcm_voice_params) (void);
-	int (*set_voice_port) (int status);
-	int (*set_audio_port) (int status);
-
-	int (*power_up_pmic_pb) (unsigned int port);
-	int (*power_up_pmic_cp) (unsigned int port);
-	int (*power_down_pmic_pb) (unsigned int device);
-	int (*power_down_pmic_cp) (unsigned int device);
-	int (*power_down_pmic) (void);
-	void (*pmic_irq_cb) (void *cb_data, u8 value);
-	void (*pmic_irq_enable)(void *data);
-	int (*pmic_jack_enable) (void);
-	int (*pmic_get_mic_bias)(void *intelmaddata);
-	int (*pmic_set_headset_state)(int state);
-
-	unsigned int hw_dmic_map[MFLD_MAX_HW_CH];
-	unsigned int available_dmics;
-	int (*set_hw_dmic_route) (u8 index);
-
-	int gpio_amp;
-};
-
-extern void sst_mad_send_jack_report(struct snd_jack *jack,
-				     int buttonpressevent,
-				     int status);
-
-
-int intemad_set_headset_state(int state);
-int intelmad_get_mic_bias(void);
-
-struct intel_sst_pcm_control {
-	int (*open) (struct snd_sst_params *str_param);
-	int (*device_control) (int cmd, void *arg);
-	int (*close) (unsigned int str_id);
-};
-struct intel_sst_card_ops {
-	char *module_name;
-	unsigned int  vendor_id;
-	struct intel_sst_pcm_control *pcm_control;
-	struct snd_pmic_ops *scard_ops;
-};
-
-/* modified for generic access */
-struct sc_reg_access {
-	u16 reg_addr;
-	u8 value;
-	u8 mask;
-};
-enum sc_reg_access_type {
-	PMIC_READ = 0,
-	PMIC_WRITE,
-	PMIC_READ_MODIFY,
-};
-
-int register_sst_card(struct intel_sst_card_ops *card);
-void unregister_sst_card(struct intel_sst_card_ops *card);
-#endif /* __INTEL_SST_H__ */
diff --git a/drivers/staging/intel_sst/intel_sst_app_interface.c b/drivers/staging/intel_sst/intel_sst_app_interface.c
deleted file mode 100644
index 93b41a284d83..000000000000
--- a/drivers/staging/intel_sst/intel_sst_app_interface.c
+++ /dev/null
@@ -1,1460 +0,0 @@
-/*
- *  intel_sst_interface.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *  Harsha Priya <priya.harsha@intel.com>
- *  Dharageswari R <dharageswari.r@intel.com>
- *  Jeeja KP <jeeja.kp@intel.com>
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *  This driver exposes the audio engine functionalities to the ALSA
- *	and middleware.
- *  Upper layer interfaces (MAD driver, MMF) to SST driver
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/fs.h>
-#include <linux/uio.h>
-#include <linux/aio.h>
-#include <linux/uaccess.h>
-#include <linux/firmware.h>
-#include <linux/pm_runtime.h>
-#include <linux/ioctl.h>
-#ifdef CONFIG_MRST_RAR_HANDLER
-#include <linux/rar_register.h>
-#include "../../../drivers/staging/memrar/memrar.h"
-#endif
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-
-#define AM_MODULE 1
-#define STREAM_MODULE 0
-
-
-/**
-* intel_sst_check_device - checks SST device
-*
-* This utility function checks the state of SST device and downlaods FW if
-* not done, or resumes the device if suspended
-*/
-
-static int intel_sst_check_device(void)
-{
-	int retval = 0;
-	if (sst_drv_ctx->pmic_state != SND_MAD_INIT_DONE) {
-		pr_warn("Sound card not available\n");
-		return -EIO;
-	}
-	if (sst_drv_ctx->sst_state == SST_SUSPENDED) {
-		pr_debug("Resuming from Suspended state\n");
-		retval = intel_sst_resume(sst_drv_ctx->pci);
-		if (retval) {
-			pr_debug("Resume Failed= %#x,abort\n", retval);
-			return retval;
-		}
-	}
-
-	if (sst_drv_ctx->sst_state == SST_UN_INIT) {
-		/* FW is not downloaded */
-		retval = sst_download_fw();
-		if (retval)
-			return -ENODEV;
-		if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID) {
-			retval = sst_drv_ctx->rx_time_slot_status;
-			if (retval != RX_TIMESLOT_UNINIT
-					&& sst_drv_ctx->pmic_vendor != SND_NC)
-				sst_enable_rx_timeslot(retval);
-		}
-	}
-	return 0;
-}
-
-/**
- * intel_sst_open - opens a handle to driver
- *
- * @i_node:	inode structure
- * @file_ptr:pointer to file
- *
- * This function is called by OS when a user space component
- * tries to get a driver handle. Only one handle at a time
- * will be allowed
- */
-int intel_sst_open(struct inode *i_node, struct file *file_ptr)
-{
-	unsigned int retval;
-
-	mutex_lock(&sst_drv_ctx->stream_lock);
-	pm_runtime_get_sync(&sst_drv_ctx->pci->dev);
-	retval = intel_sst_check_device();
-	if (retval) {
-		pm_runtime_put(&sst_drv_ctx->pci->dev);
-		mutex_unlock(&sst_drv_ctx->stream_lock);
-		return retval;
-	}
-
-	if (sst_drv_ctx->encoded_cnt < MAX_ENC_STREAM) {
-		struct ioctl_pvt_data *data =
-			kzalloc(sizeof(struct ioctl_pvt_data), GFP_KERNEL);
-		if (!data) {
-			pm_runtime_put(&sst_drv_ctx->pci->dev);
-			mutex_unlock(&sst_drv_ctx->stream_lock);
-			return -ENOMEM;
-		}
-
-		sst_drv_ctx->encoded_cnt++;
-		mutex_unlock(&sst_drv_ctx->stream_lock);
-		data->pvt_id = sst_assign_pvt_id(sst_drv_ctx);
-		data->str_id = 0;
-		file_ptr->private_data = (void *)data;
-		pr_debug("pvt_id handle = %d!\n", data->pvt_id);
-	} else {
-		retval = -EUSERS;
-		pm_runtime_put(&sst_drv_ctx->pci->dev);
-		mutex_unlock(&sst_drv_ctx->stream_lock);
-	}
-	return retval;
-}
-
-/**
- * intel_sst_open_cntrl - opens a handle to driver
- *
- * @i_node:	inode structure
- * @file_ptr:pointer to file
- *
- * This function is called by OS when a user space component
- * tries to get a driver handle to /dev/intel_sst_control.
- * Only one handle at a time will be allowed
- * This is for control operations only
- */
-int intel_sst_open_cntrl(struct inode *i_node, struct file *file_ptr)
-{
-	unsigned int retval;
-
-	/* audio manager open */
-	mutex_lock(&sst_drv_ctx->stream_lock);
-	pm_runtime_get_sync(&sst_drv_ctx->pci->dev);
-	retval = intel_sst_check_device();
-	if (retval) {
-		pm_runtime_put(&sst_drv_ctx->pci->dev);
-		mutex_unlock(&sst_drv_ctx->stream_lock);
-		return retval;
-	}
-
-	if (sst_drv_ctx->am_cnt < MAX_AM_HANDLES) {
-		sst_drv_ctx->am_cnt++;
-		pr_debug("AM handle opened...\n");
-		file_ptr->private_data = NULL;
-	} else {
-		retval = -EACCES;
-		pm_runtime_put(&sst_drv_ctx->pci->dev);
-	}
-
-	mutex_unlock(&sst_drv_ctx->stream_lock);
-	return retval;
-}
-
-/**
- * intel_sst_release - releases a handle to driver
- *
- * @i_node:	inode structure
- * @file_ptr:	pointer to file
- *
- * This function is called by OS when a user space component
- * tries to release a driver handle.
- */
-int intel_sst_release(struct inode *i_node, struct file *file_ptr)
-{
-	struct ioctl_pvt_data *data = file_ptr->private_data;
-
-	pr_debug("Release called, closing app handle\n");
-	mutex_lock(&sst_drv_ctx->stream_lock);
-	sst_drv_ctx->encoded_cnt--;
-	sst_drv_ctx->stream_cnt--;
-	pm_runtime_put(&sst_drv_ctx->pci->dev);
-	mutex_unlock(&sst_drv_ctx->stream_lock);
-	free_stream_context(data->str_id);
-	kfree(data);
-	return 0;
-}
-
-int intel_sst_release_cntrl(struct inode *i_node, struct file *file_ptr)
-{
-	/* audio manager close */
-	mutex_lock(&sst_drv_ctx->stream_lock);
-	sst_drv_ctx->am_cnt--;
-	pm_runtime_put(&sst_drv_ctx->pci->dev);
-	mutex_unlock(&sst_drv_ctx->stream_lock);
-	pr_debug("AM handle closed\n");
-	return 0;
-}
-
-/**
-* intel_sst_mmap - mmaps a kernel buffer to user space for copying data
-*
-* @vma:		vm area structure instance
-* @file_ptr:	pointer to file
-*
-* This function is called by OS when a user space component
-* tries to get mmap memory from driver
-*/
-int intel_sst_mmap(struct file *file_ptr, struct vm_area_struct *vma)
-{
-	int retval, length;
-	struct ioctl_pvt_data *data =
-		(struct ioctl_pvt_data *)file_ptr->private_data;
-	int str_id = data->str_id;
-	void *mem_area;
-
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return -EINVAL;
-
-	length = vma->vm_end - vma->vm_start;
-	pr_debug("called for stream %d length 0x%x\n", str_id, length);
-
-	if (length > sst_drv_ctx->mmap_len)
-		return -ENOMEM;
-	if (!sst_drv_ctx->mmap_mem)
-		return -EIO;
-
-	/* round it up to the page boundary  */
-	/*mem_area = (void *)((((unsigned long)sst_drv_ctx->mmap_mem)
-				+ PAGE_SIZE - 1) & PAGE_MASK);*/
-	mem_area = (void *) PAGE_ALIGN((unsigned int) sst_drv_ctx->mmap_mem);
-
-	/* map the whole physically contiguous area in one piece  */
-	retval = remap_pfn_range(vma,
-			vma->vm_start,
-			virt_to_phys((void *)mem_area) >> PAGE_SHIFT,
-			length,
-			vma->vm_page_prot);
-	if (retval)
-		sst_drv_ctx->streams[str_id].mmapped = false;
-	else
-		sst_drv_ctx->streams[str_id].mmapped = true;
-
-	pr_debug("mmap ret 0x%x\n", retval);
-	return retval;
-}
-
-/* sets mmap data buffers to play/capture*/
-static int intel_sst_mmap_play_capture(u32 str_id,
-		struct snd_sst_mmap_buffs *mmap_buf)
-{
-	struct sst_stream_bufs *bufs;
-	int retval, i;
-	struct stream_info *stream;
-	struct snd_sst_mmap_buff_entry *buf_entry;
-	struct snd_sst_mmap_buff_entry *tmp_buf;
-
-	pr_debug("called for str_id %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return -EINVAL;
-
-	stream = &sst_drv_ctx->streams[str_id];
-	if (stream->mmapped != true)
-		return -EIO;
-
-	if (stream->status == STREAM_UN_INIT ||
-		stream->status == STREAM_DECODE) {
-		return -EBADRQC;
-	}
-	stream->curr_bytes = 0;
-	stream->cumm_bytes = 0;
-
-	tmp_buf = kcalloc(mmap_buf->entries, sizeof(*tmp_buf), GFP_KERNEL);
-	if (!tmp_buf)
-		return -ENOMEM;
-	if (copy_from_user(tmp_buf, (void __user *)mmap_buf->buff,
-			mmap_buf->entries * sizeof(*tmp_buf))) {
-		retval = -EFAULT;
-		goto out_free;
-	}
-
-	pr_debug("new buffers count %d status %d\n",
-			mmap_buf->entries, stream->status);
-	buf_entry = tmp_buf;
-	for (i = 0; i < mmap_buf->entries; i++) {
-		bufs = kzalloc(sizeof(*bufs), GFP_KERNEL);
-		if (!bufs) {
-			retval = -ENOMEM;
-			goto out_free;
-		}
-		bufs->size = buf_entry->size;
-		bufs->offset = buf_entry->offset;
-		bufs->addr = sst_drv_ctx->mmap_mem;
-		bufs->in_use = false;
-		buf_entry++;
-		/* locking here */
-		mutex_lock(&stream->lock);
-		list_add_tail(&bufs->node, &stream->bufs);
-		mutex_unlock(&stream->lock);
-	}
-
-	mutex_lock(&stream->lock);
-	stream->data_blk.condition = false;
-	stream->data_blk.ret_code = 0;
-	if (stream->status == STREAM_INIT &&
-			stream->prev != STREAM_UN_INIT &&
-			stream->need_draining != true) {
-		stream->prev = stream->status;
-		stream->status = STREAM_RUNNING;
-		if (stream->ops == STREAM_OPS_PLAYBACK) {
-			if (sst_play_frame(str_id) < 0) {
-				pr_warn("play frames fail\n");
-				mutex_unlock(&stream->lock);
-				retval = -EIO;
-				goto out_free;
-			}
-		} else if (stream->ops == STREAM_OPS_CAPTURE) {
-			if (sst_capture_frame(str_id) < 0) {
-				pr_warn("capture frame fail\n");
-				mutex_unlock(&stream->lock);
-				retval = -EIO;
-				goto out_free;
-			}
-		}
-	}
-	mutex_unlock(&stream->lock);
-	/* Block the call for reply */
-	if (!list_empty(&stream->bufs)) {
-		stream->data_blk.on = true;
-		retval = sst_wait_interruptible(sst_drv_ctx,
-					&stream->data_blk);
-	}
-
-	if (retval >= 0)
-		retval = stream->cumm_bytes;
-	pr_debug("end of play/rec ioctl bytes = %d!!\n", retval);
-
-out_free:
-	kfree(tmp_buf);
-	return retval;
-}
-
-/*sets user data buffers to play/capture*/
-static int intel_sst_play_capture(struct stream_info *stream, int str_id)
-{
-	int retval;
-
-	stream->data_blk.ret_code = 0;
-	stream->data_blk.on = true;
-	stream->data_blk.condition = false;
-
-	mutex_lock(&stream->lock);
-	if (stream->status == STREAM_INIT && stream->prev != STREAM_UN_INIT) {
-		/* stream is started */
-		stream->prev = stream->status;
-		stream->status = STREAM_RUNNING;
-	}
-
-	if (stream->status == STREAM_INIT && stream->prev == STREAM_UN_INIT) {
-		/* stream is not started yet */
-		pr_debug("Stream isn't in started state %d, prev %d\n",
-			stream->status, stream->prev);
-	} else if ((stream->status == STREAM_RUNNING ||
-			stream->status == STREAM_PAUSED) &&
-			stream->need_draining != true) {
-		/* stream is started */
-		if (stream->ops == STREAM_OPS_PLAYBACK ||
-				stream->ops == STREAM_OPS_PLAYBACK_DRM) {
-			if (sst_play_frame(str_id) < 0) {
-				pr_warn("play frames failed\n");
-				mutex_unlock(&stream->lock);
-				return -EIO;
-			}
-		} else if (stream->ops == STREAM_OPS_CAPTURE) {
-			if (sst_capture_frame(str_id) < 0) {
-				pr_warn("capture frames failed\n");
-				mutex_unlock(&stream->lock);
-				return -EIO;
-			}
-		}
-	} else {
-		mutex_unlock(&stream->lock);
-		return -EIO;
-	}
-	mutex_unlock(&stream->lock);
-	/* Block the call for reply */
-
-	retval = sst_wait_interruptible(sst_drv_ctx, &stream->data_blk);
-	if (retval) {
-		stream->status = STREAM_INIT;
-		pr_debug("wait returned error...\n");
-	}
-	return retval;
-}
-
-/* fills kernel list with buffer addresses for SST DSP driver to process*/
-static int snd_sst_fill_kernel_list(struct stream_info *stream,
-			const struct iovec *iovec, unsigned long nr_segs,
-			struct list_head *copy_to_list)
-{
-	struct sst_stream_bufs *stream_bufs;
-	unsigned long index, mmap_len;
-	unsigned char __user *bufp;
-	unsigned long size, copied_size;
-	int retval = 0, add_to_list = 0;
-	static int sent_offset;
-	static unsigned long sent_index;
-
-#ifdef CONFIG_MRST_RAR_HANDLER
-	if (stream->ops == STREAM_OPS_PLAYBACK_DRM) {
-		for (index = stream->sg_index; index < nr_segs; index++) {
-			__u32 rar_handle;
-			struct sst_stream_bufs *stream_bufs =
-				kzalloc(sizeof(*stream_bufs), GFP_KERNEL);
-
-			stream->sg_index = index;
-			if (!stream_bufs)
-				return -ENOMEM;
-			if (copy_from_user((void *) &rar_handle,
-					iovec[index].iov_base,
-					sizeof(__u32))) {
-				kfree(stream_bufs);
-				return -EFAULT;
-			}
-			stream_bufs->addr = (char *)rar_handle;
-			stream_bufs->in_use = false;
-			stream_bufs->size = iovec[0].iov_len;
-			/* locking here */
-			mutex_lock(&stream->lock);
-			list_add_tail(&stream_bufs->node, &stream->bufs);
-			mutex_unlock(&stream->lock);
-		}
-		stream->sg_index = index;
-		return retval;
-	}
-#endif
-	stream_bufs = kzalloc(sizeof(*stream_bufs), GFP_KERNEL);
-	if (!stream_bufs)
-		return -ENOMEM;
-	stream_bufs->addr = sst_drv_ctx->mmap_mem;
-	mmap_len = sst_drv_ctx->mmap_len;
-	stream_bufs->addr = sst_drv_ctx->mmap_mem;
-	bufp = stream->cur_ptr;
-
-	copied_size = 0;
-
-	if (!stream->sg_index)
-		sent_index = sent_offset = 0;
-
-	for (index = stream->sg_index; index < nr_segs; index++) {
-		stream->sg_index = index;
-		if (!stream->cur_ptr)
-			bufp = iovec[index].iov_base;
-
-		size = ((unsigned long)iovec[index].iov_base
-			+ iovec[index].iov_len) - (unsigned long) bufp;
-
-		if ((copied_size + size) > mmap_len)
-			size = mmap_len - copied_size;
-
-
-		if (stream->ops == STREAM_OPS_PLAYBACK) {
-			if (copy_from_user((void *)
-					(stream_bufs->addr + copied_size),
-					bufp, size)) {
-				/* Clean up the list and return error code */
-				retval = -EFAULT;
-				break;
-			}
-		} else if (stream->ops == STREAM_OPS_CAPTURE) {
-			struct snd_sst_user_cap_list *entry =
-				kzalloc(sizeof(*entry), GFP_KERNEL);
-
-			if (!entry) {
-				kfree(stream_bufs);
-				return -ENOMEM;
-			}
-			entry->iov_index = index;
-			entry->iov_offset = (unsigned long) bufp -
-					(unsigned long)iovec[index].iov_base;
-			entry->offset = copied_size;
-			entry->size = size;
-			list_add_tail(&entry->node, copy_to_list);
-		}
-
-		stream->cur_ptr = bufp + size;
-
-		if (((unsigned long)iovec[index].iov_base
-				+ iovec[index].iov_len) <
-				((unsigned long)iovec[index].iov_base)) {
-			pr_debug("Buffer overflows\n");
-			kfree(stream_bufs);
-			return -EINVAL;
-		}
-
-		if (((unsigned long)iovec[index].iov_base
-					+ iovec[index].iov_len) ==
-					(unsigned long)stream->cur_ptr) {
-			stream->cur_ptr = NULL;
-			stream->sg_index++;
-		}
-
-		copied_size += size;
-		pr_debug("copied_size - %lx\n", copied_size);
-		if ((copied_size >= mmap_len) ||
-				(stream->sg_index == nr_segs)) {
-			add_to_list = 1;
-		}
-
-		if (add_to_list) {
-			stream_bufs->in_use = false;
-			stream_bufs->size = copied_size;
-			/* locking here */
-			mutex_lock(&stream->lock);
-			list_add_tail(&stream_bufs->node, &stream->bufs);
-			mutex_unlock(&stream->lock);
-			break;
-		}
-	}
-	return retval;
-}
-
-/* This function copies the captured data returned from SST DSP engine
- * to the user buffers*/
-static int snd_sst_copy_userbuf_capture(struct stream_info *stream,
-			const struct iovec *iovec,
-			struct list_head *copy_to_list)
-{
-	struct snd_sst_user_cap_list *entry, *_entry;
-	struct sst_stream_bufs *kbufs = NULL, *_kbufs;
-	int retval = 0;
-
-	/* copy sent buffers */
-	pr_debug("capture stream copying to user now...\n");
-	list_for_each_entry_safe(kbufs, _kbufs, &stream->bufs, node) {
-		if (kbufs->in_use == true) {
-			/* copy to user */
-			list_for_each_entry_safe(entry, _entry,
-						copy_to_list, node) {
-				if (copy_to_user(iovec[entry->iov_index].iov_base + entry->iov_offset,
-					     kbufs->addr + entry->offset,
-					     entry->size)) {
-					/* Clean up the list and return error */
-					retval = -EFAULT;
-					break;
-				}
-				list_del(&entry->node);
-				kfree(entry);
-			}
-		}
-	}
-	pr_debug("end of cap copy\n");
-	return retval;
-}
-
-/*
- * snd_sst_userbufs_play_cap - constructs the list from user buffers
- *
- * @iovec:pointer to iovec structure
- * @nr_segs:number entries in the iovec structure
- * @str_id:stream id
- * @stream:pointer to stream_info structure
- *
- * This function will traverse the user list and copy the data to the kernel
- * space buffers.
- */
-static int snd_sst_userbufs_play_cap(const struct iovec *iovec,
-			unsigned long nr_segs, unsigned int str_id,
-			struct stream_info *stream)
-{
-	int retval;
-	LIST_HEAD(copy_to_list);
-
-
-	retval = snd_sst_fill_kernel_list(stream, iovec, nr_segs,
-		       &copy_to_list);
-
-	retval = intel_sst_play_capture(stream, str_id);
-	if (retval < 0)
-		return retval;
-
-	if (stream->ops == STREAM_OPS_CAPTURE) {
-		retval = snd_sst_copy_userbuf_capture(stream, iovec,
-				&copy_to_list);
-	}
-	return retval;
-}
-
-/* This function is common function across read/write
-  for user buffers called from system calls*/
-static int intel_sst_read_write(unsigned int str_id, char __user *buf,
-					size_t count)
-{
-	int retval;
-	struct stream_info *stream;
-	struct iovec iovec;
-	unsigned long nr_segs;
-
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return -EINVAL;
-	stream = &sst_drv_ctx->streams[str_id];
-	if (stream->mmapped == true) {
-		pr_warn("user write and stream is mapped\n");
-		return -EIO;
-	}
-	if (!count)
-		return -EINVAL;
-	stream->curr_bytes = 0;
-	stream->cumm_bytes = 0;
-	/* copy user buf details */
-	pr_debug("new buffers %p, copy size %d, status %d\n" ,
-			buf, (int) count, (int) stream->status);
-
-	stream->buf_type = SST_BUF_USER_STATIC;
-	iovec.iov_base = buf;
-	iovec.iov_len  = count;
-	nr_segs = 1;
-
-	do {
-		retval = snd_sst_userbufs_play_cap(
-				&iovec, nr_segs, str_id, stream);
-		if (retval < 0)
-			break;
-
-	} while (stream->sg_index < nr_segs);
-
-	stream->sg_index = 0;
-	stream->cur_ptr = NULL;
-	if (retval >= 0)
-		retval = stream->cumm_bytes;
-	pr_debug("end of play/rec bytes = %d!!\n", retval);
-	return retval;
-}
-
-/***
- * intel_sst_write - This function is called when user tries to play out data
- *
- * @file_ptr:pointer to file
- * @buf:user buffer to be played out
- * @count:size of tthe buffer
- * @offset:offset to start from
- *
- * writes the encoded data into DSP
- */
-int intel_sst_write(struct file *file_ptr, const char __user *buf,
-			size_t count, loff_t *offset)
-{
-	struct ioctl_pvt_data *data = file_ptr->private_data;
-	int str_id = data->str_id;
-	struct stream_info *stream = &sst_drv_ctx->streams[str_id];
-
-	pr_debug("called for %d\n", str_id);
-	if (stream->status == STREAM_UN_INIT ||
-		stream->status == STREAM_DECODE) {
-		return -EBADRQC;
-	}
-	return intel_sst_read_write(str_id, (char __user *)buf, count);
-}
-
-/*
- * intel_sst_aio_write - write buffers
- *
- * @kiocb:pointer to a structure containing file pointer
- * @iov:list of user buffer to be played out
- * @nr_segs:number of entries
- * @offset:offset to start from
- *
- * This function is called when user tries to play out multiple data buffers
- */
-ssize_t intel_sst_aio_write(struct kiocb *kiocb, const struct iovec *iov,
-			unsigned long nr_segs, loff_t  offset)
-{
-	int retval;
-	struct ioctl_pvt_data *data = kiocb->ki_filp->private_data;
-	int str_id = data->str_id;
-	struct stream_info *stream;
-
-	pr_debug("entry - %ld\n", nr_segs);
-
-	if (is_sync_kiocb(kiocb) == false)
-		return -EINVAL;
-
-	pr_debug("called for str_id %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return -EINVAL;
-	stream = &sst_drv_ctx->streams[str_id];
-	if (stream->mmapped == true)
-		return -EIO;
-	if (stream->status == STREAM_UN_INIT ||
-		stream->status == STREAM_DECODE) {
-		return -EBADRQC;
-	}
-	stream->curr_bytes = 0;
-	stream->cumm_bytes = 0;
-	pr_debug("new segs %ld, offset %d, status %d\n" ,
-			nr_segs, (int) offset, (int) stream->status);
-	stream->buf_type = SST_BUF_USER_STATIC;
-	do {
-		retval = snd_sst_userbufs_play_cap(iov, nr_segs,
-						str_id, stream);
-		if (retval < 0)
-			break;
-
-	} while (stream->sg_index < nr_segs);
-
-	stream->sg_index = 0;
-	stream->cur_ptr = NULL;
-	if (retval >= 0)
-		retval = stream->cumm_bytes;
-	pr_debug("end of play/rec bytes = %d!!\n", retval);
-	return retval;
-}
-
-/*
- * intel_sst_read - read the encoded data
- *
- * @file_ptr: pointer to file
- * @buf: user buffer to be filled with captured data
- * @count: size of tthe buffer
- * @offset: offset to start from
- *
- * This function is called when user tries to capture data
- */
-int intel_sst_read(struct file *file_ptr, char __user *buf,
-			size_t count, loff_t *offset)
-{
-	struct ioctl_pvt_data *data = file_ptr->private_data;
-	int str_id = data->str_id;
-	struct stream_info *stream = &sst_drv_ctx->streams[str_id];
-
-	pr_debug("called for %d\n", str_id);
-	if (stream->status == STREAM_UN_INIT ||
-			stream->status == STREAM_DECODE)
-		return -EBADRQC;
-	return intel_sst_read_write(str_id, buf, count);
-}
-
-/*
- * intel_sst_aio_read - aio read
- *
- * @kiocb: pointer to a structure containing file pointer
- * @iov: list of user buffer to be filled with captured
- * @nr_segs: number of entries
- * @offset: offset to start from
- *
- * This function is called when user tries to capture out multiple data buffers
- */
-ssize_t intel_sst_aio_read(struct kiocb *kiocb, const struct iovec *iov,
-			 unsigned long nr_segs, loff_t offset)
-{
-	int retval;
-	struct ioctl_pvt_data *data = kiocb->ki_filp->private_data;
-	int str_id = data->str_id;
-	struct stream_info *stream;
-
-	pr_debug("entry - %ld\n", nr_segs);
-
-	if (is_sync_kiocb(kiocb) == false) {
-		pr_debug("aio_read from user space is not allowed\n");
-		return -EINVAL;
-	}
-
-	pr_debug("called for str_id %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return -EINVAL;
-	stream = &sst_drv_ctx->streams[str_id];
-	if (stream->mmapped == true)
-		return -EIO;
-	if (stream->status == STREAM_UN_INIT ||
-			stream->status == STREAM_DECODE)
-		return -EBADRQC;
-	stream->curr_bytes = 0;
-	stream->cumm_bytes = 0;
-
-	pr_debug("new segs %ld, offset %d, status %d\n" ,
-			nr_segs, (int) offset, (int) stream->status);
-	stream->buf_type = SST_BUF_USER_STATIC;
-	do {
-		retval = snd_sst_userbufs_play_cap(iov, nr_segs,
-						str_id, stream);
-		if (retval < 0)
-			break;
-
-	} while (stream->sg_index < nr_segs);
-
-	stream->sg_index = 0;
-	stream->cur_ptr = NULL;
-	if (retval >= 0)
-		retval = stream->cumm_bytes;
-	pr_debug("end of play/rec bytes = %d!!\n", retval);
-	return retval;
-}
-
-/* sst_print_stream_params - prints the stream parameters (debug fn)*/
-static void sst_print_stream_params(struct snd_sst_get_stream_params *get_prm)
-{
-	pr_debug("codec params:result = %d\n",
-				get_prm->codec_params.result);
-	pr_debug("codec params:stream = %d\n",
-				get_prm->codec_params.stream_id);
-	pr_debug("codec params:codec = %d\n",
-				get_prm->codec_params.codec);
-	pr_debug("codec params:ops = %d\n",
-				get_prm->codec_params.ops);
-	pr_debug("codec params:stream_type = %d\n",
-				get_prm->codec_params.stream_type);
-	pr_debug("pcmparams:sfreq = %d\n",
-				get_prm->pcm_params.sfreq);
-	pr_debug("pcmparams:num_chan = %d\n",
-				get_prm->pcm_params.num_chan);
-	pr_debug("pcmparams:pcm_wd_sz = %d\n",
-				get_prm->pcm_params.pcm_wd_sz);
-	return;
-}
-
-/**
- * sst_create_algo_ipc - create ipc msg for algorithm parameters
- *
- * @algo_params: Algorithm parameters
- * @msg: post msg pointer
- *
- * This function is called to create ipc msg
- */
-int sst_create_algo_ipc(struct snd_ppp_params *algo_params,
-					struct ipc_post **msg)
-{
-	if (sst_create_large_msg(msg))
-		return -ENOMEM;
-	sst_fill_header(&(*msg)->header,
-			IPC_IA_ALG_PARAMS, 1, algo_params->str_id);
-	(*msg)->header.part.data = sizeof(u32) +
-			sizeof(*algo_params) + algo_params->size;
-	memcpy((*msg)->mailbox_data, &(*msg)->header, sizeof(u32));
-	memcpy((*msg)->mailbox_data + sizeof(u32),
-				algo_params, sizeof(*algo_params));
-	return 0;
-}
-
-/**
- * sst_send_algo_ipc - send ipc msg for algorithm parameters
- *
- * @msg: post msg pointer
- *
- * This function is called to send ipc msg
- */
-int sst_send_algo_ipc(struct ipc_post **msg)
-{
-	sst_drv_ctx->ppp_params_blk.condition = false;
-	sst_drv_ctx->ppp_params_blk.ret_code = 0;
-	sst_drv_ctx->ppp_params_blk.on = true;
-	sst_drv_ctx->ppp_params_blk.data = NULL;
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&(*msg)->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	return sst_wait_interruptible_timeout(sst_drv_ctx,
-			&sst_drv_ctx->ppp_params_blk, SST_BLOCK_TIMEOUT);
-}
-
-/**
- * intel_sst_ioctl_dsp - receives the device ioctl's
- *
- * @cmd:Ioctl cmd
- * @arg:data
- *
- * This function is called when a user space component
- * sends a DSP Ioctl to SST driver
- */
-long intel_sst_ioctl_dsp(unsigned int cmd, unsigned long arg)
-{
-	int retval = 0;
-	struct snd_ppp_params algo_params;
-	struct snd_ppp_params *algo_params_copied;
-	struct ipc_post *msg;
-
-	switch (_IOC_NR(cmd)) {
-	case _IOC_NR(SNDRV_SST_SET_ALGO):
-		if (copy_from_user(&algo_params, (void __user *)arg,
-							sizeof(algo_params)))
-			return -EFAULT;
-		if (algo_params.size > SST_MAILBOX_SIZE)
-			return -EMSGSIZE;
-
-		pr_debug("Algo ID %d Str id %d Enable %d Size %d\n",
-			algo_params.algo_id, algo_params.str_id,
-			algo_params.enable, algo_params.size);
-		retval = sst_create_algo_ipc(&algo_params, &msg);
-		if (retval)
-			break;
-		algo_params.reserved = 0;
-		if (copy_from_user(msg->mailbox_data + sizeof(algo_params),
-				algo_params.params, algo_params.size))
-			return -EFAULT;
-
-		retval = sst_send_algo_ipc(&msg);
-		if (retval) {
-			pr_debug("Error in sst_set_algo = %d\n", retval);
-			retval = -EIO;
-		}
-		break;
-
-	case _IOC_NR(SNDRV_SST_GET_ALGO):
-		if (copy_from_user(&algo_params, (void __user *)arg,
-							sizeof(algo_params)))
-			return -EFAULT;
-		pr_debug("Algo ID %d Str id %d Enable %d Size %d\n",
-			algo_params.algo_id, algo_params.str_id,
-			algo_params.enable, algo_params.size);
-		retval = sst_create_algo_ipc(&algo_params, &msg);
-		if (retval)
-			break;
-		algo_params.reserved = 1;
-		retval = sst_send_algo_ipc(&msg);
-		if (retval) {
-			pr_debug("Error in sst_get_algo = %d\n", retval);
-			retval = -EIO;
-			break;
-		}
-		algo_params_copied = (struct snd_ppp_params *)
-					sst_drv_ctx->ppp_params_blk.data;
-		if (algo_params_copied->size > algo_params.size) {
-			pr_debug("mem insufficient to copy\n");
-			retval = -EMSGSIZE;
-			goto free_mem;
-		} else {
-			char __user *tmp;
-
-			if (copy_to_user(algo_params.params,
-					algo_params_copied->params,
-					algo_params_copied->size)) {
-				retval = -EFAULT;
-				goto free_mem;
-			}
-			tmp = (char __user *)arg + offsetof(
-					struct snd_ppp_params, size);
-			if (copy_to_user(tmp, &algo_params_copied->size,
-						 sizeof(__u32))) {
-				retval = -EFAULT;
-				goto free_mem;
-			}
-
-		}
-free_mem:
-		kfree(algo_params_copied->params);
-		kfree(algo_params_copied);
-		break;
-	}
-	return retval;
-}
-
-
-int sst_ioctl_tuning_params(unsigned long arg)
-{
-	struct snd_sst_tuning_params params;
-	struct ipc_post *msg;
-
-	if (copy_from_user(&params, (void __user *)arg, sizeof(params)))
-		return -EFAULT;
-	if (params.size > SST_MAILBOX_SIZE)
-		return -ENOMEM;
-	pr_debug("Parameter %d, Stream %d, Size %d\n", params.type,
-			params.str_id, params.size);
-	if (sst_create_large_msg(&msg))
-		return -ENOMEM;
-
-	sst_fill_header(&msg->header, IPC_IA_TUNING_PARAMS, 1, params.str_id);
-	msg->header.part.data = sizeof(u32) + sizeof(params) + params.size;
-	memcpy(msg->mailbox_data, &msg->header.full, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), &params, sizeof(params));
-	if (copy_from_user(msg->mailbox_data + sizeof(params),
-			(void __user *)(unsigned long)params.addr,
-			params.size)) {
-		kfree(msg->mailbox_data);
-		kfree(msg);
-		return -EFAULT;
-	}
-	return sst_send_algo_ipc(&msg);
-}
-/**
- * intel_sst_ioctl - receives the device ioctl's
- * @file_ptr:pointer to file
- * @cmd:Ioctl cmd
- * @arg:data
- *
- * This function is called by OS when a user space component
- * sends an Ioctl to SST driver
- */
-long intel_sst_ioctl(struct file *file_ptr, unsigned int cmd, unsigned long arg)
-{
-	int retval = 0;
-	struct ioctl_pvt_data *data = NULL;
-	int str_id = 0, minor = 0;
-
-	data = file_ptr->private_data;
-	if (data) {
-		minor = 0;
-		str_id = data->str_id;
-	} else
-		minor = 1;
-
-	if (sst_drv_ctx->sst_state != SST_FW_RUNNING)
-		return -EBUSY;
-
-	switch (_IOC_NR(cmd)) {
-	case _IOC_NR(SNDRV_SST_STREAM_PAUSE):
-		pr_debug("IOCTL_PAUSE received for %d!\n", str_id);
-		if (minor != STREAM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-		retval = sst_pause_stream(str_id);
-		break;
-
-	case _IOC_NR(SNDRV_SST_STREAM_RESUME):
-		pr_debug("SNDRV_SST_IOCTL_RESUME received!\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-		retval = sst_resume_stream(str_id);
-		break;
-
-	case _IOC_NR(SNDRV_SST_STREAM_SET_PARAMS): {
-		struct snd_sst_params str_param;
-
-		pr_debug("IOCTL_SET_PARAMS received!\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-
-		if (copy_from_user(&str_param, (void __user *)arg,
-				sizeof(str_param))) {
-			retval = -EFAULT;
-			break;
-		}
-
-		if (!str_id) {
-
-			retval = sst_get_stream(&str_param);
-			if (retval > 0) {
-				struct stream_info *str_info;
-				char __user *dest;
-
-				sst_drv_ctx->stream_cnt++;
-				data->str_id = retval;
-				str_info = &sst_drv_ctx->streams[retval];
-				str_info->src = SST_DRV;
-				dest = (char __user *)arg + offsetof(struct snd_sst_params, stream_id);
-				retval = copy_to_user(dest, &retval, sizeof(__u32));
-				if (retval)
-					retval = -EFAULT;
-			} else {
-				if (retval == -SST_ERR_INVALID_PARAMS)
-					retval = -EINVAL;
-			}
-		} else {
-			pr_debug("SET_STREAM_PARAMS received!\n");
-			/* allocated set params only */
-			retval = sst_set_stream_param(str_id, &str_param);
-			/* Block the call for reply */
-			if (!retval) {
-				int sfreq = 0, word_size = 0, num_channel = 0;
-				sfreq =	str_param.sparams.uc.pcm_params.sfreq;
-				word_size = str_param.sparams.uc.pcm_params.pcm_wd_sz;
-				num_channel = str_param.sparams.uc.pcm_params.num_chan;
-				if (str_param.ops == STREAM_OPS_CAPTURE) {
-					sst_drv_ctx->scard_ops->\
-					set_pcm_audio_params(sfreq,
-						word_size, num_channel);
-				}
-			}
-		}
-		break;
-	}
-	case _IOC_NR(SNDRV_SST_SET_VOL): {
-		struct snd_sst_vol set_vol;
-
-		if (copy_from_user(&set_vol, (void __user *)arg,
-				sizeof(set_vol))) {
-			pr_debug("copy failed\n");
-			retval = -EFAULT;
-			break;
-		}
-		pr_debug("SET_VOLUME received for %d!\n",
-				set_vol.stream_id);
-		if (minor == STREAM_MODULE && set_vol.stream_id == 0) {
-			pr_debug("invalid operation!\n");
-			retval = -EPERM;
-			break;
-		}
-		retval = sst_set_vol(&set_vol);
-		break;
-	}
-	case _IOC_NR(SNDRV_SST_GET_VOL): {
-		struct snd_sst_vol get_vol;
-
-		if (copy_from_user(&get_vol, (void __user *)arg,
-				sizeof(get_vol))) {
-			retval = -EFAULT;
-			break;
-		}
-		pr_debug("IOCTL_GET_VOLUME received for stream = %d!\n",
-				get_vol.stream_id);
-		if (minor == STREAM_MODULE && get_vol.stream_id == 0) {
-			pr_debug("invalid operation!\n");
-			retval = -EPERM;
-			break;
-		}
-		retval = sst_get_vol(&get_vol);
-		if (retval) {
-			retval = -EIO;
-			break;
-		}
-		pr_debug("id:%d\n, vol:%d, ramp_dur:%d, ramp_type:%d\n",
-				get_vol.stream_id, get_vol.volume,
-				get_vol.ramp_duration, get_vol.ramp_type);
-		if (copy_to_user((struct snd_sst_vol __user *)arg,
-				&get_vol, sizeof(get_vol))) {
-			retval = -EFAULT;
-			break;
-		}
-		/*sst_print_get_vol_info(str_id, &get_vol);*/
-		break;
-	}
-
-	case _IOC_NR(SNDRV_SST_MUTE): {
-		struct snd_sst_mute set_mute;
-
-		if (copy_from_user(&set_mute, (void __user *)arg,
-				sizeof(set_mute))) {
-			retval = -EFAULT;
-			break;
-		}
-		pr_debug("SNDRV_SST_SET_VOLUME received for %d!\n",
-			set_mute.stream_id);
-		if (minor == STREAM_MODULE && set_mute.stream_id == 0) {
-			retval = -EPERM;
-			break;
-		}
-		retval = sst_set_mute(&set_mute);
-		break;
-	}
-	case _IOC_NR(SNDRV_SST_STREAM_GET_PARAMS): {
-		struct snd_sst_get_stream_params get_params;
-
-		pr_debug("IOCTL_GET_PARAMS received!\n");
-		if (minor != 0) {
-			retval = -EBADRQC;
-			break;
-		}
-
-		retval = sst_get_stream_params(str_id, &get_params);
-		if (retval) {
-			retval = -EIO;
-			break;
-		}
-		if (copy_to_user((struct snd_sst_get_stream_params __user *)arg,
-					&get_params, sizeof(get_params))) {
-			retval = -EFAULT;
-			break;
-		}
-		sst_print_stream_params(&get_params);
-		break;
-	}
-
-	case _IOC_NR(SNDRV_SST_MMAP_PLAY):
-	case _IOC_NR(SNDRV_SST_MMAP_CAPTURE): {
-		struct snd_sst_mmap_buffs mmap_buf;
-
-		pr_debug("SNDRV_SST_MMAP_PLAY/CAPTURE received!\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-		if (copy_from_user(&mmap_buf, (void __user *)arg,
-				sizeof(mmap_buf))) {
-			retval = -EFAULT;
-			break;
-		}
-		retval = intel_sst_mmap_play_capture(str_id, &mmap_buf);
-		break;
-	}
-	case _IOC_NR(SNDRV_SST_STREAM_DROP):
-		pr_debug("SNDRV_SST_IOCTL_DROP received!\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EINVAL;
-			break;
-		}
-		retval = sst_drop_stream(str_id);
-		break;
-
-	case _IOC_NR(SNDRV_SST_STREAM_GET_TSTAMP): {
-		struct snd_sst_tstamp tstamp = {0};
-		unsigned long long time, freq, mod;
-
-		pr_debug("SNDRV_SST_STREAM_GET_TSTAMP received!\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-		memcpy_fromio(&tstamp,
-			sst_drv_ctx->mailbox + SST_TIME_STAMP + str_id * sizeof(tstamp),
-			sizeof(tstamp));
-		time = tstamp.samples_rendered;
-		freq = (unsigned long long) tstamp.sampling_frequency;
-		time = time * 1000; /* converting it to ms */
-		mod = do_div(time, freq);
-		if (copy_to_user((void __user *)arg, &time,
-				sizeof(unsigned long long)))
-			retval = -EFAULT;
-		break;
-	}
-
-	case _IOC_NR(SNDRV_SST_STREAM_START):{
-		struct stream_info *stream;
-
-		pr_debug("SNDRV_SST_STREAM_START received!\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EINVAL;
-			break;
-		}
-		retval = sst_validate_strid(str_id);
-		if (retval)
-			break;
-		stream = &sst_drv_ctx->streams[str_id];
-		mutex_lock(&stream->lock);
-		if (stream->status == STREAM_INIT &&
-			stream->need_draining != true) {
-			stream->prev = stream->status;
-			stream->status = STREAM_RUNNING;
-			if (stream->ops == STREAM_OPS_PLAYBACK ||
-				stream->ops == STREAM_OPS_PLAYBACK_DRM) {
-				retval = sst_play_frame(str_id);
-			} else if (stream->ops == STREAM_OPS_CAPTURE)
-				retval = sst_capture_frame(str_id);
-			else {
-				retval = -EINVAL;
-				mutex_unlock(&stream->lock);
-				break;
-			}
-			if (retval < 0) {
-				stream->status = STREAM_INIT;
-				mutex_unlock(&stream->lock);
-				break;
-			}
-		} else {
-			retval = -EINVAL;
-		}
-		mutex_unlock(&stream->lock);
-		break;
-	}
-
-	case _IOC_NR(SNDRV_SST_SET_TARGET_DEVICE): {
-		struct snd_sst_target_device target_device;
-
-		pr_debug("SET_TARGET_DEVICE received!\n");
-		if (copy_from_user(&target_device, (void __user *)arg,
-				sizeof(target_device))) {
-			retval = -EFAULT;
-			break;
-		}
-		if (minor != AM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-		retval = sst_target_device_select(&target_device);
-		break;
-	}
-
-	case _IOC_NR(SNDRV_SST_DRIVER_INFO): {
-		struct snd_sst_driver_info info;
-
-		pr_debug("SNDRV_SST_DRIVER_INFO received\n");
-		info.version = SST_VERSION_NUM;
-		/* hard coding, shud get sumhow later */
-		info.active_pcm_streams = sst_drv_ctx->stream_cnt -
-						sst_drv_ctx->encoded_cnt;
-		info.active_enc_streams = sst_drv_ctx->encoded_cnt;
-		info.max_pcm_streams = MAX_ACTIVE_STREAM - MAX_ENC_STREAM;
-		info.max_enc_streams = MAX_ENC_STREAM;
-		info.buf_per_stream = sst_drv_ctx->mmap_len;
-		if (copy_to_user((void __user *)arg, &info,
-				sizeof(info)))
-			retval = -EFAULT;
-		break;
-	}
-
-	case _IOC_NR(SNDRV_SST_STREAM_DECODE): {
-		struct snd_sst_dbufs param;
-		struct snd_sst_dbufs dbufs_local;
-		struct snd_sst_buffs ibufs, obufs;
-		struct snd_sst_buff_entry *ibuf_tmp, *obuf_tmp;
-		char __user *dest;
-
-		pr_debug("SNDRV_SST_STREAM_DECODE received\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-		if (copy_from_user(&param, (void __user *)arg,
-				sizeof(param))) {
-			retval = -EFAULT;
-			break;
-		}
-
-		dbufs_local.input_bytes_consumed = param.input_bytes_consumed;
-		dbufs_local.output_bytes_produced =
-					param.output_bytes_produced;
-
-		if (copy_from_user(&ibufs, (void __user *)param.ibufs, sizeof(ibufs))) {
-			retval = -EFAULT;
-			break;
-		}
-		if (copy_from_user(&obufs, (void __user *)param.obufs, sizeof(obufs))) {
-			retval = -EFAULT;
-			break;
-		}
-
-		ibuf_tmp = kcalloc(ibufs.entries, sizeof(*ibuf_tmp), GFP_KERNEL);
-		obuf_tmp = kcalloc(obufs.entries, sizeof(*obuf_tmp), GFP_KERNEL);
-		if (!ibuf_tmp || !obuf_tmp) {
-			retval = -ENOMEM;
-			goto free_iobufs;
-		}
-
-		if (copy_from_user(ibuf_tmp, (void __user *)ibufs.buff_entry,
-				ibufs.entries * sizeof(*ibuf_tmp))) {
-			retval = -EFAULT;
-			goto free_iobufs;
-		}
-		ibufs.buff_entry = ibuf_tmp;
-		dbufs_local.ibufs = &ibufs;
-
-		if (copy_from_user(obuf_tmp, (void __user *)obufs.buff_entry,
-				obufs.entries * sizeof(*obuf_tmp))) {
-			retval = -EFAULT;
-			goto free_iobufs;
-		}
-		obufs.buff_entry = obuf_tmp;
-		dbufs_local.obufs = &obufs;
-
-		retval = sst_decode(str_id, &dbufs_local);
-		if (retval) {
-			retval = -EAGAIN;
-			goto free_iobufs;
-		}
-
-		dest = (char __user *)arg + offsetof(struct snd_sst_dbufs, input_bytes_consumed);
-		if (copy_to_user(dest,
-				&dbufs_local.input_bytes_consumed,
-				sizeof(unsigned long long))) {
-			retval = -EFAULT;
-			goto free_iobufs;
-		}
-
-		dest = (char __user *)arg + offsetof(struct snd_sst_dbufs, input_bytes_consumed);
-		if (copy_to_user(dest,
-				&dbufs_local.output_bytes_produced,
-				sizeof(unsigned long long))) {
-			retval = -EFAULT;
-			goto free_iobufs;
-		}
-free_iobufs:
-		kfree(ibuf_tmp);
-		kfree(obuf_tmp);
-		break;
-	}
-
-	case _IOC_NR(SNDRV_SST_STREAM_DRAIN):
-		pr_debug("SNDRV_SST_STREAM_DRAIN received\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EINVAL;
-			break;
-		}
-		retval = sst_drain_stream(str_id);
-		break;
-
-	case _IOC_NR(SNDRV_SST_STREAM_BYTES_DECODED): {
-		unsigned long long __user *bytes = (unsigned long long __user *)arg;
-		struct snd_sst_tstamp tstamp = {0};
-
-		pr_debug("STREAM_BYTES_DECODED received!\n");
-		if (minor != STREAM_MODULE) {
-			retval = -EINVAL;
-			break;
-		}
-		memcpy_fromio(&tstamp,
-			sst_drv_ctx->mailbox + SST_TIME_STAMP + str_id * sizeof(tstamp),
-			sizeof(tstamp));
-		if (copy_to_user(bytes, &tstamp.bytes_processed,
-				sizeof(*bytes)))
-			retval = -EFAULT;
-		break;
-	}
-	case _IOC_NR(SNDRV_SST_FW_INFO): {
-		struct snd_sst_fw_info *fw_info;
-
-		pr_debug("SNDRV_SST_FW_INFO received\n");
-
-		fw_info = kzalloc(sizeof(*fw_info), GFP_ATOMIC);
-		if (!fw_info) {
-			retval = -ENOMEM;
-			break;
-		}
-		retval = sst_get_fw_info(fw_info);
-		if (retval) {
-			retval = -EIO;
-			kfree(fw_info);
-			break;
-		}
-		if (copy_to_user((struct snd_sst_dbufs __user *)arg,
-				fw_info, sizeof(*fw_info))) {
-			kfree(fw_info);
-			retval = -EFAULT;
-			break;
-		}
-		/*sst_print_fw_info(fw_info);*/
-		kfree(fw_info);
-		break;
-	}
-	case _IOC_NR(SNDRV_SST_GET_ALGO):
-	case _IOC_NR(SNDRV_SST_SET_ALGO):
-		if (minor != AM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-		retval = intel_sst_ioctl_dsp(cmd, arg);
-		break;
-
-	case _IOC_NR(SNDRV_SST_TUNING_PARAMS):
-		if (minor != AM_MODULE) {
-			retval = -EBADRQC;
-			break;
-		}
-		retval = sst_ioctl_tuning_params(arg);
-		break;
-
-	default:
-		retval = -EINVAL;
-	}
-	pr_debug("intel_sst_ioctl:complete ret code = %d\n", retval);
-	return retval;
-}
-
diff --git a/drivers/staging/intel_sst/intel_sst_common.h b/drivers/staging/intel_sst/intel_sst_common.h
deleted file mode 100644
index 870981ba3c97..000000000000
--- a/drivers/staging/intel_sst/intel_sst_common.h
+++ /dev/null
@@ -1,623 +0,0 @@
-#ifndef __INTEL_SST_COMMON_H__
-#define __INTEL_SST_COMMON_H__
-/*
- *  intel_sst_common.h - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corporation
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  Common private declarations for SST
- */
-
-#define SST_DRIVER_VERSION "1.2.17"
-#define SST_VERSION_NUM 0x1217
-
-/* driver names */
-#define SST_DRV_NAME "intel_sst_driver"
-#define SST_MRST_PCI_ID 0x080A
-#define SST_MFLD_PCI_ID 0x082F
-#define PCI_ID_LENGTH 4
-#define SST_SUSPEND_DELAY 2000
-#define FW_CONTEXT_MEM (64*1024)
-
-enum sst_states {
-	SST_FW_LOADED = 1,
-	SST_FW_RUNNING,
-	SST_UN_INIT,
-	SST_ERROR,
-	SST_SUSPENDED
-};
-
-#define MAX_ACTIVE_STREAM	3
-#define MAX_ENC_STREAM		1
-#define MAX_AM_HANDLES		1
-#define ALLOC_TIMEOUT		5000
-/* SST numbers */
-#define SST_BLOCK_TIMEOUT	5000
-#define TARGET_DEV_BLOCK_TIMEOUT	5000
-
-#define BLOCK_UNINIT		-1
-#define RX_TIMESLOT_UNINIT	-1
-
-/* SST register map */
-#define SST_CSR			0x00
-#define SST_PISR		0x08
-#define SST_PIMR		0x10
-#define SST_ISRX		0x18
-#define SST_IMRX		0x28
-#define SST_IPCX		0x38 /* IPC IA-SST */
-#define SST_IPCD		0x40 /* IPC SST-IA */
-#define SST_ISRD		0x20 /* dummy register for shim workaround */
-#define SST_SHIM_SIZE		0X44
-
-#define SPI_MODE_ENABLE_BASE_ADDR 0xffae4000
-#define FW_SIGNATURE_SIZE	4
-
-/* PMIC and SST hardware states */
-enum sst_mad_states {
-	SND_MAD_UN_INIT = 0,
-	SND_MAD_INIT_DONE,
-};
-
-/* stream states */
-enum sst_stream_states {
-	STREAM_UN_INIT	= 0,	/* Freed/Not used stream */
-	STREAM_RUNNING	= 1,	/* Running */
-	STREAM_PAUSED	= 2,	/* Paused stream */
-	STREAM_DECODE	= 3,	/* stream is in decoding only state */
-	STREAM_INIT	= 4,	/* stream init, waiting for data */
-};
-
-
-enum sst_ram_type {
-	SST_IRAM	= 1,
-	SST_DRAM	= 2,
-};
-/* SST shim registers to structure mapping  */
-union config_status_reg {
-	struct {
-		u32 mfld_strb:1;
-		u32 sst_reset:1;
-		u32 hw_rsvd:3;
-		u32 sst_clk:2;
-		u32 bypass:3;
-		u32 run_stall:1;
-		u32 rsvd1:2;
-		u32 strb_cntr_rst:1;
-		u32 rsvd:18;
-	} part;
-	u32 full;
-};
-
-union interrupt_reg {
-	struct {
-		u32 done_interrupt:1;
-		u32 busy_interrupt:1;
-		u32 rsvd:30;
-	} part;
-	u32 full;
-};
-
-union sst_pisr_reg {
-	struct {
-		u32 pssp0:1;
-		u32 pssp1:1;
-		u32 rsvd0:3;
-		u32 dmac:1;
-		u32 rsvd1:26;
-	} part;
-	u32 full;
-};
-
-union sst_pimr_reg {
-	struct {
-		u32 ssp0:1;
-		u32 ssp1:1;
-		u32 rsvd0:3;
-		u32 dmac:1;
-		u32 rsvd1:10;
-		u32 ssp0_sc:1;
-		u32 ssp1_sc:1;
-		u32 rsvd2:3;
-		u32 dmac_sc:1;
-		u32 rsvd3:10;
-	} part;
-	u32 full;
-};
-
-
-struct sst_stream_bufs {
-	struct list_head	node;
-	u32			size;
-	const char		*addr;
-	u32			data_copied;
-	bool			in_use;
-	u32			offset;
-};
-
-struct snd_sst_user_cap_list {
-	unsigned int iov_index; /* index of iov */
-	unsigned long iov_offset; /* offset in iov */
-	unsigned long offset; /* offset in kmem */
-	unsigned long size; /* size copied */
-	struct list_head node;
-};
-/*
-This structure is used to block a user/fw data call to another
-fw/user call
-*/
-struct sst_block {
-	bool	condition; /* condition for blocking check */
-	int	ret_code; /* ret code when block is released */
-	void	*data; /* data to be appsed for block if any */
-	bool	on;
-};
-
-enum snd_sst_buf_type {
-	SST_BUF_USER_STATIC = 1,
-	SST_BUF_USER_DYNAMIC,
-	SST_BUF_MMAP_STATIC,
-	SST_BUF_MMAP_DYNAMIC,
-};
-
-enum snd_src {
-	SST_DRV = 1,
-	MAD_DRV = 2
-};
-
-/**
- * struct stream_info - structure that holds the stream information
- *
- * @status : stream current state
- * @prev : stream prev state
- * @codec : stream codec
- * @sst_id : stream id
- * @ops : stream operation pb/cp/drm...
- * @bufs: stream buffer list
- * @lock : stream mutex for protecting state
- * @pcm_lock : spinlock for pcm path only
- * @mmapped : is stream mmapped
- * @sg_index : current stream user buffer index
- * @cur_ptr : stream user buffer pointer
- * @buf_entry : current user buffer
- * @data_blk : stream block for data operations
- * @ctrl_blk : stream block for ctrl operations
- * @buf_type : stream user buffer type
- * @pcm_substream : PCM substream
- * @period_elapsed : PCM period elapsed callback
- * @sfreq : stream sampling freq
- * @decode_ibuf : Decoded i/p buffers pointer
- * @decode_obuf : Decoded o/p buffers pointer
- * @decode_isize : Decoded i/p buffers size
- * @decode_osize : Decoded o/p buffers size
- * @decode_ibuf_type : Decoded i/p buffer type
- * @decode_obuf_type : Decoded o/p buffer type
- * @idecode_alloc : Decode alloc index
- * @need_draining : stream set for drain
- * @str_type : stream type
- * @curr_bytes : current bytes decoded
- * @cumm_bytes : cummulative bytes decoded
- * @str_type : stream type
- * @src : stream source
- * @device : output device type (medfield only)
- * @pcm_slot : pcm slot value
- */
-struct stream_info {
-	unsigned int		status;
-	unsigned int		prev;
-	u8			codec;
-	unsigned int		sst_id;
-	unsigned int		ops;
-	struct list_head	bufs;
-	struct mutex		lock; /* mutex */
-	spinlock_t          pcm_lock;
-	bool			mmapped;
-	unsigned int		sg_index; /*  current buf Index  */
-	unsigned char __user 	*cur_ptr; /*  Current static bufs  */
-	struct snd_sst_buf_entry __user *buf_entry;
-	struct sst_block	data_blk; /* stream ops block */
-	struct sst_block	ctrl_blk; /* stream control cmd block */
-	enum snd_sst_buf_type   buf_type;
-	void			*pcm_substream;
-	void (*period_elapsed) (void *pcm_substream);
-	unsigned int		sfreq;
-	void			*decode_ibuf, *decode_obuf;
-	unsigned int		decode_isize, decode_osize;
-	u8 decode_ibuf_type, decode_obuf_type;
-	unsigned int		idecode_alloc;
-	unsigned int		need_draining;
-	unsigned int		str_type;
-	u32			curr_bytes;
-	u32			cumm_bytes;
-	u32			src;
-	enum snd_sst_audio_device_type device;
-	u8			pcm_slot;
-};
-
-/*
- * struct stream_alloc_bloc - this structure is used for blocking the user's
- * alloc calls to fw's response to alloc calls
- *
- * @sst_id : session id of blocked stream
- * @ops_block : ops block struture
- */
-struct stream_alloc_block {
-	int			sst_id; /* session id of blocked stream */
-	struct sst_block	ops_block; /* ops block struture */
-};
-
-#define SST_FW_SIGN "$SST"
-#define SST_FW_LIB_SIGN "$LIB"
-
-/*
- * struct fw_header - FW file headers
- *
- * @signature : FW signature
- * @modules : # of modules
- * @file_format : version of header format
- * @reserved : reserved fields
- */
-struct fw_header {
-	unsigned char signature[FW_SIGNATURE_SIZE]; /* FW signature */
-	u32 file_size; /* size of fw minus this header */
-	u32 modules; /*  # of modules */
-	u32 file_format; /* version of header format */
-	u32 reserved[4];
-};
-
-struct fw_module_header {
-	unsigned char signature[FW_SIGNATURE_SIZE]; /* module signature */
-	u32 mod_size; /* size of module */
-	u32 blocks; /* # of blocks */
-	u32 type; /* codec type, pp lib */
-	u32 entry_point;
-};
-
-struct dma_block_info {
-	enum sst_ram_type	type;	/* IRAM/DRAM */
-	u32			size;	/* Bytes */
-	u32			ram_offset; /* Offset in I/DRAM */
-	u32			rsvd;	/* Reserved field */
-};
-
-struct ioctl_pvt_data {
-	int			str_id;
-	int			pvt_id;
-};
-
-struct sst_ipc_msg_wq {
-	union ipc_header	header;
-	char mailbox[SST_MAILBOX_SIZE];
-	struct work_struct	wq;
-};
-
-struct mad_ops_wq {
-	int stream_id;
-	enum sst_controls control_op;
-	struct work_struct	wq;
-
-};
-
-#define SST_MMAP_PAGES	(640*1024 / PAGE_SIZE)
-#define SST_MMAP_STEP	(40*1024 / PAGE_SIZE)
-
-/***
- * struct intel_sst_drv - driver ops
- *
- * @pmic_state : pmic state
- * @pmic_vendor : pmic vendor detected
- * @sst_state : current sst device state
- * @pci_id : PCI device id loaded
- * @shim : SST shim pointer
- * @mailbox : SST mailbox pointer
- * @iram : SST IRAM pointer
- * @dram : SST DRAM pointer
- * @shim_phy_add : SST shim phy addr
- * @ipc_dispatch_list : ipc messages dispatched
- * @ipc_post_msg_wq : wq to post IPC messages context
- * @ipc_process_msg : wq to process msgs from FW context
- * @ipc_process_reply : wq to process reply from FW context
- * @ipc_post_msg : wq to post reply from FW context
- * @mad_ops : MAD driver operations registered
- * @mad_wq : MAD driver wq
- * @post_msg_wq : wq to post IPC messages
- * @process_msg_wq : wq to process msgs from FW
- * @process_reply_wq : wq to process reply from FW
- * @streams : sst stream contexts
- * @alloc_block : block structure for alloc
- * @tgt_dev_blk : block structure for target device
- * @fw_info_blk : block structure for fw info block
- * @vol_info_blk : block structure for vol info block
- * @mute_info_blk : block structure for mute info block
- * @hs_info_blk : block structure for hs info block
- * @list_lock : sst driver list lock (deprecated)
- * @list_spin_lock : sst driver spin lock block
- * @scard_ops : sst card ops
- * @pci : sst pci device struture
- * @active_streams : sst active streams
- * @sst_lock : sst device lock
- * @stream_lock : sst stream lock
- * @unique_id : sst unique id
- * @stream_cnt : total sst active stream count
- * @pb_streams : total active pb streams
- * @cp_streams : total active cp streams
- * @lpe_stalled : lpe stall status
- * @pmic_port_instance : active pmic port instance
- * @rx_time_slot_status : active rx slot
- * @lpaudio_start : lpaudio status
- * @audio_start : audio status
- * @devt_d : pointer to /dev/lpe node
- * @devt_c : pointer to /dev/lpe_ctrl node
- * @max_streams : max streams allowed
- */
-struct intel_sst_drv {
-	bool			pmic_state;
-	int			pmic_vendor;
-	int			sst_state;
-	unsigned int		pci_id;
-	void __iomem		*shim;
-	void __iomem		*mailbox;
-	void __iomem		*iram;
-	void __iomem		*dram;
-	unsigned int		shim_phy_add;
-	struct list_head	ipc_dispatch_list;
-	struct work_struct	ipc_post_msg_wq;
-	struct sst_ipc_msg_wq	ipc_process_msg;
-	struct sst_ipc_msg_wq	ipc_process_reply;
-	struct sst_ipc_msg_wq	ipc_post_msg;
-	struct mad_ops_wq	mad_ops;
-	wait_queue_head_t	wait_queue;
-	struct workqueue_struct *mad_wq;
-	struct workqueue_struct *post_msg_wq;
-	struct workqueue_struct *process_msg_wq;
-	struct workqueue_struct *process_reply_wq;
-
-	struct stream_info	streams[MAX_NUM_STREAMS];
-	struct stream_alloc_block alloc_block[MAX_ACTIVE_STREAM];
-	struct sst_block	tgt_dev_blk, fw_info_blk, ppp_params_blk,
-				vol_info_blk, mute_info_blk, hs_info_blk;
-	struct mutex		list_lock;/* mutex for IPC list locking */
-	spinlock_t	list_spin_lock; /* mutex for IPC list locking */
-	struct snd_pmic_ops	*scard_ops;
-	struct pci_dev		*pci;
-	int active_streams[MAX_NUM_STREAMS];
-	void			*mmap_mem;
-	struct mutex            sst_lock;
-	struct mutex		stream_lock;
-	unsigned int		mmap_len;
-	unsigned int		unique_id;
-	unsigned int		stream_cnt;	/* total streams */
-	unsigned int		encoded_cnt;	/* enocded streams only */
-	unsigned int		am_cnt;
-	unsigned int		pb_streams;	/* pb streams active */
-	unsigned int		cp_streams;	/* cp streams active */
-	unsigned int		lpe_stalled; /* LPE is stalled or not */
-	unsigned int		pmic_port_instance; /*pmic port instance*/
-	int			rx_time_slot_status;
-	unsigned int		lpaudio_start;
-		/* 1 - LPA stream(MP3 pb) in progress*/
-	unsigned int		audio_start;
-	dev_t			devt_d, devt_c;
-	unsigned int		max_streams;
-	unsigned int		*fw_cntx;
-	unsigned int		fw_cntx_size;
-
-	unsigned int		fw_downloaded;
-};
-
-extern struct intel_sst_drv *sst_drv_ctx;
-
-#define CHIP_REV_REG 0xff108000
-#define CHIP_REV_ADDR 0x78
-
-/* misc definitions */
-#define FW_DWNL_ID 0xFF
-#define LOOP1 0x11111111
-#define LOOP2 0x22222222
-#define LOOP3 0x33333333
-#define LOOP4 0x44444444
-
-#define SST_DEFAULT_PMIC_PORT 1 /*audio port*/
-/* NOTE: status will have +ve for good cases and -ve for error ones */
-#define MAX_STREAM_FIELD 255
-
-int sst_alloc_stream(char *params, unsigned int stream_ops, u8 codec,
-						unsigned int session_id);
-int sst_alloc_stream_response(unsigned int str_id,
-				struct snd_sst_alloc_response *response);
-int sst_stalled(void);
-int sst_pause_stream(int id);
-int sst_resume_stream(int id);
-int sst_enable_rx_timeslot(int status);
-int sst_drop_stream(int id);
-int sst_free_stream(int id);
-int sst_start_stream(int streamID);
-int sst_play_frame(int streamID);
-int sst_pcm_play_frame(int str_id, struct sst_stream_bufs *sst_buf);
-int sst_capture_frame(int streamID);
-int sst_set_stream_param(int streamID, struct snd_sst_params *str_param);
-int sst_target_device_select(struct snd_sst_target_device *target_device);
-int sst_decode(int str_id, struct snd_sst_dbufs *dbufs);
-int sst_get_decoded_bytes(int str_id, unsigned long long *bytes);
-int sst_get_fw_info(struct snd_sst_fw_info *info);
-int sst_get_stream_params(int str_id,
-		struct snd_sst_get_stream_params *get_params);
-int sst_get_stream(struct snd_sst_params *str_param);
-int sst_get_stream_allocated(struct snd_sst_params *str_param,
-				struct snd_sst_lib_download **lib_dnld);
-int sst_drain_stream(int str_id);
-int sst_get_vol(struct snd_sst_vol *set_vol);
-int sst_set_vol(struct snd_sst_vol *set_vol);
-int sst_set_mute(struct snd_sst_mute *set_mute);
-
-
-void sst_post_message(struct work_struct *work);
-void sst_process_message(struct work_struct *work);
-void sst_process_reply(struct work_struct *work);
-void sst_process_mad_ops(struct work_struct *work);
-void sst_process_mad_jack_detection(struct work_struct *work);
-
-long intel_sst_ioctl(struct file *file_ptr, unsigned int cmd,
-			unsigned long arg);
-int intel_sst_open(struct inode *i_node, struct file *file_ptr);
-int intel_sst_open_cntrl(struct inode *i_node, struct file *file_ptr);
-int intel_sst_release(struct inode *i_node, struct file *file_ptr);
-int intel_sst_release_cntrl(struct inode *i_node, struct file *file_ptr);
-int intel_sst_read(struct file *file_ptr, char __user *buf,
-			size_t count, loff_t *ppos);
-int intel_sst_write(struct file *file_ptr, const char __user *buf,
-			size_t count, loff_t *ppos);
-int intel_sst_mmap(struct file *fp, struct vm_area_struct *vma);
-ssize_t intel_sst_aio_write(struct kiocb *kiocb, const struct iovec *iov,
-			unsigned long nr_segs, loff_t  offset);
-ssize_t intel_sst_aio_read(struct kiocb *kiocb, const struct iovec *iov,
-			unsigned long nr_segs, loff_t offset);
-
-int sst_load_fw(const struct firmware *fw, void *context);
-int sst_load_library(struct snd_sst_lib_download *lib, u8 ops);
-int sst_spi_mode_enable(void);
-int sst_get_block_stream(struct intel_sst_drv *sst_drv_ctx);
-
-int sst_wait_interruptible(struct intel_sst_drv *sst_drv_ctx,
-				struct sst_block *block);
-int sst_wait_interruptible_timeout(struct intel_sst_drv *sst_drv_ctx,
-		struct sst_block *block, int timeout);
-int sst_wait_timeout(struct intel_sst_drv *sst_drv_ctx,
-		struct stream_alloc_block *block);
-int sst_create_large_msg(struct ipc_post **arg);
-int sst_create_short_msg(struct ipc_post **arg);
-void sst_wake_up_alloc_block(struct intel_sst_drv *sst_drv_ctx,
-		u8 sst_id, int status, void *data);
-void sst_clear_interrupt(void);
-int intel_sst_resume(struct pci_dev *pci);
-int sst_download_fw(void);
-void free_stream_context(unsigned int str_id);
-void sst_clean_stream(struct stream_info *stream);
-
-/*
- * sst_fill_header - inline to fill sst header
- *
- * @header : ipc header
- * @msg : IPC message to be sent
- * @large : is ipc large msg
- * @str_id : stream id
- *
- * this function is an inline function that sets the headers before
- * sending a message
- */
-static inline void sst_fill_header(union ipc_header *header,
-				int msg, int large, int str_id)
-{
-	header->part.msg_id = msg;
-	header->part.str_id = str_id;
-	header->part.large = large;
-	header->part.done = 0;
-	header->part.busy = 1;
-	header->part.data = 0;
-}
-
-/*
- * sst_assign_pvt_id - assign a pvt id for stream
- *
- * @sst_drv_ctx : driver context
- *
- * this inline function assigns a private id for calls that dont have stream
- * context yet, should be called with lock held
- */
-static inline unsigned int sst_assign_pvt_id(struct intel_sst_drv *sst_drv_ctx)
-{
-	sst_drv_ctx->unique_id++;
-	if (sst_drv_ctx->unique_id >= MAX_NUM_STREAMS)
-		sst_drv_ctx->unique_id = 1;
-	return sst_drv_ctx->unique_id;
-}
-
-/*
- * sst_init_stream - this function initialzes stream context
- *
- * @stream : stream struture
- * @codec : codec for stream
- * @sst_id : stream id
- * @ops : stream operation
- * @slot : stream pcm slot
- * @device : device type
- *
- * this inline function initialzes stream context for allocated stream
- */
-static inline void sst_init_stream(struct stream_info *stream,
-		int codec, int sst_id, int ops, u8 slot,
-		enum snd_sst_audio_device_type device)
-{
-	stream->status = STREAM_INIT;
-	stream->prev = STREAM_UN_INIT;
-	stream->codec = codec;
-	stream->sst_id = sst_id;
-	stream->str_type = 0;
-	stream->ops = ops;
-	stream->data_blk.on = false;
-	stream->data_blk.condition = false;
-	stream->data_blk.ret_code = 0;
-	stream->data_blk.data = NULL;
-	stream->ctrl_blk.on = false;
-	stream->ctrl_blk.condition = false;
-	stream->ctrl_blk.ret_code = 0;
-	stream->ctrl_blk.data = NULL;
-	stream->need_draining = false;
-	stream->decode_ibuf = NULL;
-	stream->decode_isize = 0;
-	stream->mmapped = false;
-	stream->pcm_slot = slot;
-	stream->device = device;
-}
-
-
-/*
- * sst_validate_strid - this function validates the stream id
- *
- * @str_id : stream id to be validated
- *
- * returns 0 if valid stream
- */
-static inline int sst_validate_strid(int str_id)
-{
-	if (str_id <= 0 || str_id > sst_drv_ctx->max_streams) {
-		pr_err("SST ERR: invalid stream id : %d MAX_STREAMS:%d\n",
-					str_id, sst_drv_ctx->max_streams);
-		return -EINVAL;
-	} else
-		return 0;
-}
-
-static inline int sst_shim_write(void __iomem *addr, int offset, int value)
-{
-
-	if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID)
-		writel(value, addr + SST_ISRD);	/*dummy*/
-	writel(value, addr + offset);
-	return 0;
-}
-
-static inline int sst_shim_read(void __iomem *addr, int offset)
-{
-	return readl(addr + offset);
-}
-#endif /* __INTEL_SST_COMMON_H__ */
diff --git a/drivers/staging/intel_sst/intel_sst_drv_interface.c b/drivers/staging/intel_sst/intel_sst_drv_interface.c
deleted file mode 100644
index 22bd29c0c439..000000000000
--- a/drivers/staging/intel_sst/intel_sst_drv_interface.c
+++ /dev/null
@@ -1,564 +0,0 @@
-/*
- *  intel_sst_interface.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com)
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *  This driver exposes the audio engine functionalities to the ALSA
- *	and middleware.
- *  Upper layer interfaces (MAD driver, MMF) to SST driver
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/delay.h>
-#include <linux/pci.h>
-#include <linux/fs.h>
-#include <linux/firmware.h>
-#include <linux/pm_runtime.h>
-#include <linux/export.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-
-
-/*
- * sst_download_fw - download the audio firmware to DSP
- *
- * This function is called when the FW needs to be downloaded to SST DSP engine
- */
-int sst_download_fw(void)
-{
-	int retval;
-	const struct firmware *fw_sst;
-	char name[20];
-
-	if (sst_drv_ctx->sst_state != SST_UN_INIT)
-		return -EPERM;
-
-	/* Reload firmware is not needed for MRST */
-	if ( (sst_drv_ctx->pci_id == SST_MRST_PCI_ID) && sst_drv_ctx->fw_downloaded) {
-		pr_debug("FW already downloaded, skip for MRST platform\n");
-		sst_drv_ctx->sst_state = SST_FW_RUNNING;
-		return 0;
-	}
-
-	snprintf(name, sizeof(name), "%s%04x%s", "fw_sst_",
-					sst_drv_ctx->pci_id, ".bin");
-
-	pr_debug("Downloading %s FW now...\n", name);
-	retval = request_firmware(&fw_sst, name, &sst_drv_ctx->pci->dev);
-	if (retval) {
-		pr_err("request fw failed %d\n", retval);
-		return retval;
-	}
-	sst_drv_ctx->alloc_block[0].sst_id = FW_DWNL_ID;
-	sst_drv_ctx->alloc_block[0].ops_block.condition = false;
-	retval = sst_load_fw(fw_sst, NULL);
-	if (retval)
-		goto end_restore;
-
-	retval = sst_wait_timeout(sst_drv_ctx, &sst_drv_ctx->alloc_block[0]);
-	if (retval)
-		pr_err("fw download failed %d\n" , retval);
-	else
-		sst_drv_ctx->fw_downloaded = 1;
-
-end_restore:
-	release_firmware(fw_sst);
-	sst_drv_ctx->alloc_block[0].sst_id = BLOCK_UNINIT;
-	return retval;
-}
-
-
-/*
- * sst_stalled - this function checks if the lpe is in stalled state
- */
-int sst_stalled(void)
-{
-	int retry = 1000;
-	int retval = -1;
-
-	while (retry) {
-		if (!sst_drv_ctx->lpe_stalled)
-			return 0;
-		/*wait for time and re-check*/
-		msleep(1);
-
-		retry--;
-	}
-	pr_debug("in Stalled State\n");
-	return retval;
-}
-
-void free_stream_context(unsigned int str_id)
-{
-	struct stream_info *stream;
-
-	if (!sst_validate_strid(str_id)) {
-		/* str_id is valid, so stream is alloacted */
-		stream = &sst_drv_ctx->streams[str_id];
-		if (sst_free_stream(str_id))
-			sst_clean_stream(&sst_drv_ctx->streams[str_id]);
-		if (stream->ops == STREAM_OPS_PLAYBACK ||
-				stream->ops == STREAM_OPS_PLAYBACK_DRM) {
-			sst_drv_ctx->pb_streams--;
-			if (sst_drv_ctx->pci_id == SST_MFLD_PCI_ID)
-				sst_drv_ctx->scard_ops->power_down_pmic_pb(
-						stream->device);
-			else {
-				if (sst_drv_ctx->pb_streams == 0)
-					sst_drv_ctx->scard_ops->
-					power_down_pmic_pb(stream->device);
-			}
-		} else if (stream->ops == STREAM_OPS_CAPTURE) {
-			sst_drv_ctx->cp_streams--;
-			if (sst_drv_ctx->cp_streams == 0)
-				sst_drv_ctx->scard_ops->power_down_pmic_cp(
-						stream->device);
-		}
-		if (sst_drv_ctx->pb_streams == 0
-				&& sst_drv_ctx->cp_streams == 0)
-			sst_drv_ctx->scard_ops->power_down_pmic();
-	}
-}
-
-/*
- * sst_get_stream_allocated - this function gets a stream allocated with
- * the given params
- *
- * @str_param : stream params
- * @lib_dnld : pointer to pointer of lib downlaod struct
- *
- * This creates new stream id for a stream, in case lib is to be downloaded to
- * DSP, it downloads that
- */
-int sst_get_stream_allocated(struct snd_sst_params *str_param,
-		struct snd_sst_lib_download **lib_dnld)
-{
-	int retval, str_id;
-	struct stream_info *str_info;
-
-	retval = sst_alloc_stream((char *) &str_param->sparams, str_param->ops,
-				str_param->codec, str_param->device_type);
-	if (retval < 0) {
-		pr_err("sst_alloc_stream failed %d\n", retval);
-		return retval;
-	}
-	pr_debug("Stream allocated %d\n", retval);
-	str_id = retval;
-	str_info = &sst_drv_ctx->streams[str_id];
-	/* Block the call for reply */
-	retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-			&str_info->ctrl_blk, SST_BLOCK_TIMEOUT);
-	if ((retval != 0) || (str_info->ctrl_blk.ret_code != 0)) {
-		pr_debug("FW alloc failed retval %d, ret_code %d\n",
-				retval, str_info->ctrl_blk.ret_code);
-		str_id = -str_info->ctrl_blk.ret_code; /*return error*/
-		*lib_dnld = str_info->ctrl_blk.data;
-		sst_clean_stream(str_info);
-	} else
-		pr_debug("FW Stream allocated success\n");
-	return str_id; /*will ret either error (in above if) or correct str id*/
-}
-
-/*
- * sst_get_sfreq - this function returns the frequency of the stream
- *
- * @str_param : stream params
- */
-static int sst_get_sfreq(struct snd_sst_params *str_param)
-{
-	switch (str_param->codec) {
-	case SST_CODEC_TYPE_PCM:
-		return 48000; /*str_param->sparams.uc.pcm_params.sfreq;*/
-	case SST_CODEC_TYPE_MP3:
-		return str_param->sparams.uc.mp3_params.sfreq;
-	case SST_CODEC_TYPE_AAC:
-		return str_param->sparams.uc.aac_params.sfreq;
-	case SST_CODEC_TYPE_WMA9:
-		return str_param->sparams.uc.wma_params.sfreq;
-	default:
-		return 0;
-	}
-}
-
-/*
- * sst_get_stream - this function prepares for stream allocation
- *
- * @str_param : stream param
- */
-int sst_get_stream(struct snd_sst_params *str_param)
-{
-	int i, retval;
-	struct stream_info *str_info;
-	struct snd_sst_lib_download *lib_dnld;
-
-	/* stream is not allocated, we are allocating */
-	retval = sst_get_stream_allocated(str_param, &lib_dnld);
-	if (retval == -(SST_LIB_ERR_LIB_DNLD_REQUIRED)) {
-		/* codec download is required */
-		struct snd_sst_alloc_response *response;
-
-		pr_debug("Codec is required.... trying that\n");
-		if (lib_dnld == NULL) {
-			pr_err("lib download null!!! abort\n");
-			return -EIO;
-		}
-		i = sst_get_block_stream(sst_drv_ctx);
-		response = sst_drv_ctx->alloc_block[i].ops_block.data;
-		pr_debug("alloc block allocated = %d\n", i);
-		if (i < 0) {
-			kfree(lib_dnld);
-			return -ENOMEM;
-		}
-		retval = sst_load_library(lib_dnld, str_param->ops);
-		kfree(lib_dnld);
-
-		sst_drv_ctx->alloc_block[i].sst_id = BLOCK_UNINIT;
-		if (!retval) {
-			pr_debug("codec was downloaded successfully\n");
-
-			retval = sst_get_stream_allocated(str_param, &lib_dnld);
-			if (retval <= 0)
-				goto err;
-
-			pr_debug("Alloc done stream id %d\n", retval);
-		} else {
-			pr_debug("codec download failed\n");
-			retval = -EIO;
-			goto err;
-		}
-	} else if  (retval <= 0)
-		goto err;
-	/*else
-		set_port_params(str_param, str_param->ops);*/
-
-	/* store sampling freq */
-	str_info = &sst_drv_ctx->streams[retval];
-	str_info->sfreq = sst_get_sfreq(str_param);
-
-	/* power on the analog, if reqd */
-	if (str_param->ops == STREAM_OPS_PLAYBACK ||
-			str_param->ops == STREAM_OPS_PLAYBACK_DRM) {
-		if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID)
-			sst_drv_ctx->scard_ops->power_up_pmic_pb(
-					sst_drv_ctx->pmic_port_instance);
-		else
-			sst_drv_ctx->scard_ops->power_up_pmic_pb(
-							str_info->device);
-		/*Only if the playback is MP3 - Send a message*/
-		sst_drv_ctx->pb_streams++;
-	} else if (str_param->ops == STREAM_OPS_CAPTURE) {
-
-		sst_drv_ctx->scard_ops->power_up_pmic_cp(
-				sst_drv_ctx->pmic_port_instance);
-		/*Send a messageif not sent already*/
-		sst_drv_ctx->cp_streams++;
-	}
-
-err:
-	return retval;
-}
-
-void sst_process_mad_ops(struct work_struct *work)
-{
-
-	struct mad_ops_wq *mad_ops =
-			container_of(work, struct mad_ops_wq, wq);
-	int retval = 0;
-
-	switch (mad_ops->control_op) {
-	case SST_SND_PAUSE:
-		retval = sst_pause_stream(mad_ops->stream_id);
-		break;
-	case SST_SND_RESUME:
-		retval = sst_resume_stream(mad_ops->stream_id);
-		break;
-	case SST_SND_DROP:
-		retval = sst_drop_stream(mad_ops->stream_id);
-		break;
-	case SST_SND_START:
-			pr_debug("SST Debug: start stream\n");
-		retval = sst_start_stream(mad_ops->stream_id);
-		break;
-	case SST_SND_STREAM_PROCESS:
-		pr_debug("play/capt frames...\n");
-		break;
-	default:
-		pr_err(" wrong control_ops reported\n");
-	}
-	return;
-}
-
-void send_intial_rx_timeslot(void)
-{
-	if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID &&
-			sst_drv_ctx->rx_time_slot_status != RX_TIMESLOT_UNINIT
-			&& sst_drv_ctx->pmic_vendor != SND_NC)
-		sst_enable_rx_timeslot(sst_drv_ctx->rx_time_slot_status);
-}
-
-/*
- * sst_open_pcm_stream - Open PCM interface
- *
- * @str_param: parameters of pcm stream
- *
- * This function is called by MID sound card driver to open
- * a new pcm interface
- */
-int sst_open_pcm_stream(struct snd_sst_params *str_param)
-{
-	struct stream_info *str_info;
-	int retval;
-
-	pm_runtime_get_sync(&sst_drv_ctx->pci->dev);
-
-	if (sst_drv_ctx->sst_state == SST_SUSPENDED) {
-		/* LPE is suspended, resume it before proceeding*/
-		pr_debug("Resuming from Suspended state\n");
-		retval = intel_sst_resume(sst_drv_ctx->pci);
-		if (retval) {
-			pr_err("Resume Failed = %#x, abort\n", retval);
-			pm_runtime_put(&sst_drv_ctx->pci->dev);
-			return retval;
-		}
-	}
-	if (sst_drv_ctx->sst_state == SST_UN_INIT) {
-		/* FW is not downloaded */
-		pr_debug("DSP Downloading FW now...\n");
-		retval = sst_download_fw();
-		if (retval) {
-			pr_err("FW download fail %x, abort\n", retval);
-			pm_runtime_put(&sst_drv_ctx->pci->dev);
-			return retval;
-		}
-		send_intial_rx_timeslot();
-	}
-
-	if (!str_param) {
-		pm_runtime_put(&sst_drv_ctx->pci->dev);
-		return -EINVAL;
-	}
-
-	retval = sst_get_stream(str_param);
-	if (retval > 0) {
-		sst_drv_ctx->stream_cnt++;
-		str_info = &sst_drv_ctx->streams[retval];
-		str_info->src = MAD_DRV;
-	} else
-		pm_runtime_put(&sst_drv_ctx->pci->dev);
-
-	return retval;
-}
-
-/*
- * sst_close_pcm_stream - Close PCM interface
- *
- * @str_id: stream id to be closed
- *
- * This function is called by MID sound card driver to close
- * an existing pcm interface
- */
-int sst_close_pcm_stream(unsigned int str_id)
-{
-	struct stream_info *stream;
-
-	pr_debug("sst: stream free called\n");
-	if (sst_validate_strid(str_id))
-		return -EINVAL;
-	stream = &sst_drv_ctx->streams[str_id];
-	free_stream_context(str_id);
-	stream->pcm_substream = NULL;
-	stream->status = STREAM_UN_INIT;
-	stream->period_elapsed = NULL;
-	sst_drv_ctx->stream_cnt--;
-	pr_debug("sst: will call runtime put now\n");
-	pm_runtime_put(&sst_drv_ctx->pci->dev);
-	return 0;
-}
-
-/*
- * sst_device_control - Set Control params
- *
- * @cmd: control cmd to be set
- * @arg: command argument
- *
- * This function is called by MID sound card driver to set
- * SST/Sound card controls for an opened stream.
- * This is registered with MID driver
- */
-int sst_device_control(int cmd, void *arg)
-{
-	int retval = 0, str_id = 0;
-
-	switch (cmd) {
-	case SST_SND_PAUSE:
-	case SST_SND_RESUME:
-	case SST_SND_DROP:
-	case SST_SND_START:
-		sst_drv_ctx->mad_ops.control_op = cmd;
-		sst_drv_ctx->mad_ops.stream_id = *(int *)arg;
-		queue_work(sst_drv_ctx->mad_wq, &sst_drv_ctx->mad_ops.wq);
-		break;
-
-	case SST_SND_STREAM_INIT: {
-		struct pcm_stream_info *str_info;
-		struct stream_info *stream;
-
-		pr_debug("stream init called\n");
-		str_info = (struct pcm_stream_info *)arg;
-		str_id = str_info->str_id;
-		retval = sst_validate_strid(str_id);
-		if (retval)
-			break;
-
-		stream = &sst_drv_ctx->streams[str_id];
-		pr_debug("setting the period ptrs\n");
-		stream->pcm_substream = str_info->mad_substream;
-		stream->period_elapsed = str_info->period_elapsed;
-		stream->sfreq = str_info->sfreq;
-		stream->prev = stream->status;
-		stream->status = STREAM_INIT;
-		break;
-	}
-
-	case SST_SND_BUFFER_POINTER: {
-		struct pcm_stream_info *stream_info;
-		struct snd_sst_tstamp fw_tstamp = {0,};
-		struct stream_info *stream;
-
-
-		stream_info = (struct pcm_stream_info *)arg;
-		str_id = stream_info->str_id;
-		retval = sst_validate_strid(str_id);
-		if (retval)
-			break;
-		stream = &sst_drv_ctx->streams[str_id];
-
-		if (!stream->pcm_substream)
-			break;
-		memcpy_fromio(&fw_tstamp,
-			((void *)(sst_drv_ctx->mailbox + SST_TIME_STAMP)
-			+(str_id * sizeof(fw_tstamp))),
-			sizeof(fw_tstamp));
-
-		pr_debug("Pointer Query on strid = %d ops %d\n",
-						str_id, stream->ops);
-
-		if (stream->ops == STREAM_OPS_PLAYBACK)
-			stream_info->buffer_ptr = fw_tstamp.samples_rendered;
-		else
-			stream_info->buffer_ptr = fw_tstamp.samples_processed;
-		pr_debug("Samples rendered = %llu, buffer ptr %llu\n",
-			fw_tstamp.samples_rendered, stream_info->buffer_ptr);
-		break;
-	}
-	case SST_ENABLE_RX_TIME_SLOT: {
-		int status = *(int *)arg;
-		sst_drv_ctx->rx_time_slot_status = status ;
-		sst_enable_rx_timeslot(status);
-		break;
-	}
-	default:
-		/* Illegal case */
-		pr_warn("illegal req\n");
-		return -EINVAL;
-	}
-
-	return retval;
-}
-
-
-struct intel_sst_pcm_control pcm_ops = {
-	.open = sst_open_pcm_stream,
-	.device_control = sst_device_control,
-	.close = sst_close_pcm_stream,
-};
-
-struct intel_sst_card_ops sst_pmic_ops = {
-	.pcm_control = &pcm_ops,
-};
-
-/*
- * register_sst_card - function for sound card to register
- *
- * @card: pointer to structure of operations
- *
- * This function is called card driver loads and is ready for registration
- */
-int register_sst_card(struct intel_sst_card_ops *card)
-{
-	if (!sst_drv_ctx) {
-		pr_err("No SST driver register card reject\n");
-		return -ENODEV;
-	}
-
-	if (!card || !card->module_name) {
-		pr_err("Null Pointer Passed\n");
-		return -EINVAL;
-	}
-	if (sst_drv_ctx->pmic_state == SND_MAD_UN_INIT) {
-		/* register this driver */
-		if ((strncmp(SST_CARD_NAMES, card->module_name,
-				strlen(SST_CARD_NAMES))) == 0) {
-			sst_drv_ctx->pmic_vendor = card->vendor_id;
-			sst_drv_ctx->scard_ops =  card->scard_ops;
-			sst_pmic_ops.module_name = card->module_name;
-			sst_drv_ctx->pmic_state = SND_MAD_INIT_DONE;
-			sst_drv_ctx->rx_time_slot_status = 0; /*default AMIC*/
-			card->pcm_control = sst_pmic_ops.pcm_control;
-			return 0;
-		} else {
-			pr_err("strcmp fail %s\n", card->module_name);
-			return -EINVAL;
-		}
-
-	} else {
-		/* already registered a driver */
-		pr_err("Repeat for registration..denied\n");
-		return -EBADRQC;
-	}
-	/* The ASoC code doesn't set scard_ops */
-	if (sst_drv_ctx->scard_ops)
-		sst_drv_ctx->scard_ops->card_status = SND_CARD_UN_INIT;
-	return 0;
-}
-EXPORT_SYMBOL_GPL(register_sst_card);
-
-/*
- * unregister_sst_card- function for sound card to un-register
- *
- * @card: pointer to structure of operations
- *
- * This function is called when card driver unloads
- */
-void unregister_sst_card(struct intel_sst_card_ops *card)
-{
-	if (sst_pmic_ops.pcm_control == card->pcm_control) {
-		/* unreg */
-		sst_pmic_ops.module_name = "";
-		sst_drv_ctx->pmic_state = SND_MAD_UN_INIT;
-		pr_debug("Unregistered %s\n", card->module_name);
-	}
-	return;
-}
-EXPORT_SYMBOL_GPL(unregister_sst_card);
diff --git a/drivers/staging/intel_sst/intel_sst_dsp.c b/drivers/staging/intel_sst/intel_sst_dsp.c
deleted file mode 100644
index 426d2b92073a..000000000000
--- a/drivers/staging/intel_sst/intel_sst_dsp.c
+++ /dev/null
@@ -1,496 +0,0 @@
-/*
- *  intel_sst_dsp.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10	Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This driver exposes the audio engine functionalities to the ALSA
- *	and middleware.
- *
- *  This file contains all dsp controlling functions like firmware download,
- * setting/resetting dsp cores, etc
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/fs.h>
-#include <linux/firmware.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-
-
-/**
- * intel_sst_reset_dsp_mrst - Resetting SST DSP
- *
- * This resets DSP in case of MRST platfroms
- */
-static int intel_sst_reset_dsp_mrst(void)
-{
-	union config_status_reg csr;
-
-	pr_debug("Resetting the DSP in mrst\n");
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.full |= 0x382;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.part.strb_cntr_rst = 0;
-	csr.part.run_stall = 0x1;
-	csr.part.bypass = 0x7;
-	csr.part.sst_reset = 0x1;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-	return 0;
-}
-
-/**
- * intel_sst_reset_dsp_medfield - Resetting SST DSP
- *
- * This resets DSP in case of Medfield platfroms
- */
-static int intel_sst_reset_dsp_medfield(void)
-{
-	union config_status_reg csr;
-
-	pr_debug("Resetting the DSP in medfield\n");
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.full |= 0x382;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-
-	return 0;
-}
-
-/**
- * sst_start_mrst - Start the SST DSP processor
- *
- * This starts the DSP in MRST platfroms
- */
-static int sst_start_mrst(void)
-{
-	union config_status_reg csr;
-
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.part.bypass = 0;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-	csr.part.run_stall = 0;
-	csr.part.sst_reset = 0;
-	csr.part.strb_cntr_rst = 1;
-	pr_debug("Setting SST to execute_mrst 0x%x\n", csr.full);
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-
-	return 0;
-}
-
-/**
- * sst_start_medfield - Start the SST DSP processor
- *
- * This starts the DSP in Medfield platfroms
- */
-static int sst_start_medfield(void)
-{
-	union config_status_reg csr;
-
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.part.bypass = 0;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.part.mfld_strb = 1;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.part.run_stall = 0;
-	csr.part.sst_reset = 0;
-	pr_debug("Starting the DSP_medfld %x\n", csr.full);
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-	pr_debug("Starting the DSP_medfld\n");
-
-	return 0;
-}
-
-/**
- * sst_parse_module - Parse audio FW modules
- *
- * @module: FW module header
- *
- * Parses modules that need to be placed in SST IRAM and DRAM
- * returns error or 0 if module sizes are proper
- */
-static int sst_parse_module(struct fw_module_header *module)
-{
-	struct dma_block_info *block;
-	u32 count;
-	void __iomem *ram;
-
-	pr_debug("module sign %s size %x blocks %x type %x\n",
-			module->signature, module->mod_size,
-			module->blocks, module->type);
-	pr_debug("module entrypoint 0x%x\n", module->entry_point);
-
-	block = (void *)module + sizeof(*module);
-
-	for (count = 0; count < module->blocks; count++) {
-		if (block->size <= 0) {
-			pr_err("block size invalid\n");
-			return -EINVAL;
-		}
-		switch (block->type) {
-		case SST_IRAM:
-			ram = sst_drv_ctx->iram;
-			break;
-		case SST_DRAM:
-			ram = sst_drv_ctx->dram;
-			break;
-		default:
-			pr_err("wrong ram type0x%x in block0x%x\n",
-					block->type, count);
-			return -EINVAL;
-		}
-		memcpy_toio(ram + block->ram_offset,
-				(void *)block + sizeof(*block), block->size);
-		block = (void *)block + sizeof(*block) + block->size;
-	}
-	return 0;
-}
-
-/**
- * sst_parse_fw_image - parse and load FW
- *
- * @sst_fw: pointer to audio fw
- *
- * This function is called to parse and download the FW image
- */
-static int sst_parse_fw_image(const struct firmware *sst_fw)
-{
-	struct fw_header *header;
-	u32 count;
-	int ret_val;
-	struct fw_module_header *module;
-
-	BUG_ON(!sst_fw);
-
-	/* Read the header information from the data pointer */
-	header = (struct fw_header *)sst_fw->data;
-
-	/* verify FW */
-	if ((strncmp(header->signature, SST_FW_SIGN, 4) != 0) ||
-			(sst_fw->size != header->file_size + sizeof(*header))) {
-		/* Invalid FW signature */
-		pr_err("Invalid FW sign/filesize mismatch\n");
-		return -EINVAL;
-	}
-	pr_debug("header sign=%s size=%x modules=%x fmt=%x size=%x\n",
-			header->signature, header->file_size, header->modules,
-			header->file_format, sizeof(*header));
-	module = (void *)sst_fw->data + sizeof(*header);
-	for (count = 0; count < header->modules; count++) {
-		/* module */
-		ret_val = sst_parse_module(module);
-		if (ret_val)
-			return ret_val;
-		module = (void *)module + sizeof(*module) + module->mod_size ;
-	}
-
-	return 0;
-}
-
-/**
- * sst_load_fw - function to load FW into DSP
- *
- * @fw: Pointer to driver loaded FW
- * @context: driver context
- *
- * This function is called by OS when the FW is loaded into kernel
- */
-int sst_load_fw(const struct firmware *fw, void *context)
-{
-	int ret_val;
-
-	pr_debug("load_fw called\n");
-	BUG_ON(!fw);
-
-	if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID)
-		ret_val = intel_sst_reset_dsp_mrst();
-	else if (sst_drv_ctx->pci_id == SST_MFLD_PCI_ID)
-		ret_val = intel_sst_reset_dsp_medfield();
-	if (ret_val)
-		return ret_val;
-
-	ret_val = sst_parse_fw_image(fw);
-	if (ret_val)
-		return ret_val;
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_FW_LOADED;
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	/*  7. ask scu to reset the bypass bits */
-	/*  8.bring sst out of reset  */
-	if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID)
-		ret_val = sst_start_mrst();
-	else if (sst_drv_ctx->pci_id == SST_MFLD_PCI_ID)
-		ret_val = sst_start_medfield();
-	if (ret_val)
-		return ret_val;
-
-	pr_debug("fw loaded successful!!!\n");
-	return ret_val;
-}
-
-/*This function is called when any codec/post processing library
- needs to be downloaded*/
-static int sst_download_library(const struct firmware *fw_lib,
-				struct snd_sst_lib_download_info *lib)
-{
-	/* send IPC message and wait */
-	int i;
-	u8 pvt_id;
-	struct ipc_post *msg = NULL;
-	union config_status_reg csr;
-	struct snd_sst_str_type str_type = {0};
-	int retval = 0;
-
-	if (sst_create_large_msg(&msg))
-		return -ENOMEM;
-
-	pvt_id = sst_assign_pvt_id(sst_drv_ctx);
-	i = sst_get_block_stream(sst_drv_ctx);
-	pr_debug("alloc block allocated = %d, pvt_id %d\n", i, pvt_id);
-	if (i < 0) {
-		kfree(msg);
-		return -ENOMEM;
-	}
-	sst_drv_ctx->alloc_block[i].sst_id = pvt_id;
-	sst_fill_header(&msg->header, IPC_IA_PREP_LIB_DNLD, 1, pvt_id);
-	msg->header.part.data = sizeof(u32) + sizeof(str_type);
-	str_type.codec_type = lib->dload_lib.lib_info.lib_type;
-	/*str_type.pvt_id = pvt_id;*/
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), &str_type, sizeof(str_type));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	retval = sst_wait_timeout(sst_drv_ctx, &sst_drv_ctx->alloc_block[i]);
-	if (retval) {
-		/* error */
-		sst_drv_ctx->alloc_block[i].sst_id = BLOCK_UNINIT;
-		pr_err("Prep codec downloaded failed %d\n",
-				retval);
-		return -EIO;
-	}
-	pr_debug("FW responded, ready for download now...\n");
-	/* downloading on success */
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_FW_LOADED;
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	csr.full = readl(sst_drv_ctx->shim + SST_CSR);
-	csr.part.run_stall = 1;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.part.bypass = 0x7;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-
-	sst_parse_fw_image(fw_lib);
-
-	/* set the FW to running again */
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.part.bypass = 0x0;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-
-	csr.full = sst_shim_read(sst_drv_ctx->shim, SST_CSR);
-	csr.part.run_stall = 0;
-	sst_shim_write(sst_drv_ctx->shim, SST_CSR, csr.full);
-
-	/* send download complete and wait */
-	if (sst_create_large_msg(&msg)) {
-		sst_drv_ctx->alloc_block[i].sst_id = BLOCK_UNINIT;
-		return -ENOMEM;
-	}
-
-	sst_fill_header(&msg->header, IPC_IA_LIB_DNLD_CMPLT, 1, pvt_id);
-	sst_drv_ctx->alloc_block[i].sst_id = pvt_id;
-	msg->header.part.data = sizeof(u32) + sizeof(*lib);
-	lib->pvt_id = pvt_id;
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), lib, sizeof(*lib));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	pr_debug("Waiting for FW response Download complete\n");
-	sst_drv_ctx->alloc_block[i].ops_block.condition = false;
-	retval = sst_wait_timeout(sst_drv_ctx, &sst_drv_ctx->alloc_block[i]);
-	if (retval) {
-		/* error */
-		mutex_lock(&sst_drv_ctx->sst_lock);
-		sst_drv_ctx->sst_state = SST_UN_INIT;
-		mutex_unlock(&sst_drv_ctx->sst_lock);
-		sst_drv_ctx->alloc_block[i].sst_id = BLOCK_UNINIT;
-		return -EIO;
-	}
-
-	pr_debug("FW success on Download complete\n");
-	sst_drv_ctx->alloc_block[i].sst_id = BLOCK_UNINIT;
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_FW_RUNNING;
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	return 0;
-
-}
-
-/* This function is called before downloading the codec/postprocessing
-library is set for download to SST DSP*/
-static int sst_validate_library(const struct firmware *fw_lib,
-		struct lib_slot_info *slot,
-		u32 *entry_point)
-{
-	struct fw_header *header;
-	struct fw_module_header *module;
-	struct dma_block_info *block;
-	unsigned int n_blk, isize = 0, dsize = 0;
-	int err = 0;
-
-	header = (struct fw_header *)fw_lib->data;
-	if (header->modules != 1) {
-		pr_err("Module no mismatch found\n");
-		err = -EINVAL;
-		goto exit;
-	}
-	module = (void *)fw_lib->data + sizeof(*header);
-	*entry_point = module->entry_point;
-	pr_debug("Module entry point 0x%x\n", *entry_point);
-	pr_debug("Module Sign %s, Size 0x%x, Blocks 0x%x Type 0x%x\n",
-			module->signature, module->mod_size,
-			module->blocks, module->type);
-
-	block = (void *)module + sizeof(*module);
-	for (n_blk = 0; n_blk < module->blocks; n_blk++) {
-		switch (block->type) {
-		case SST_IRAM:
-			isize += block->size;
-			break;
-		case SST_DRAM:
-			dsize += block->size;
-			break;
-		default:
-			pr_err("Invalid block type for 0x%x\n", n_blk);
-			err = -EINVAL;
-			goto exit;
-		}
-		block = (void *)block + sizeof(*block) + block->size;
-	}
-	if (isize > slot->iram_size || dsize > slot->dram_size) {
-		pr_err("library exceeds size allocated\n");
-		err = -EINVAL;
-		goto exit;
-	} else
-		pr_debug("Library is safe for download...\n");
-
-	pr_debug("iram 0x%x, dram 0x%x, iram 0x%x, dram 0x%x\n",
-			isize, dsize, slot->iram_size, slot->dram_size);
-exit:
-	return err;
-
-}
-
-/* This function is called when FW requests for a particular library download
-This function prepares the library to download*/
-int sst_load_library(struct snd_sst_lib_download *lib, u8 ops)
-{
-	char buf[20];
-	const char *type, *dir;
-	int len = 0, error = 0;
-	u32 entry_point;
-	const struct firmware *fw_lib;
-	struct snd_sst_lib_download_info dload_info = {{{0},},};
-
-	memset(buf, 0, sizeof(buf));
-
-	pr_debug("Lib Type 0x%x, Slot 0x%x, ops 0x%x\n",
-			lib->lib_info.lib_type, lib->slot_info.slot_num, ops);
-	pr_debug("Version 0x%x, name %s, caps 0x%x media type 0x%x\n",
-		lib->lib_info.lib_version, lib->lib_info.lib_name,
-		lib->lib_info.lib_caps, lib->lib_info.media_type);
-
-	pr_debug("IRAM Size 0x%x, offset 0x%x\n",
-		lib->slot_info.iram_size, lib->slot_info.iram_offset);
-	pr_debug("DRAM Size 0x%x, offset 0x%x\n",
-		lib->slot_info.dram_size, lib->slot_info.dram_offset);
-
-	switch (lib->lib_info.lib_type) {
-	case SST_CODEC_TYPE_MP3:
-		type = "mp3_";
-		break;
-	case SST_CODEC_TYPE_AAC:
-		type = "aac_";
-		break;
-	case SST_CODEC_TYPE_AACP:
-		type = "aac_v1_";
-		break;
-	case SST_CODEC_TYPE_eAACP:
-		type = "aac_v2_";
-		break;
-	case SST_CODEC_TYPE_WMA9:
-		type = "wma9_";
-		break;
-	default:
-		pr_err("Invalid codec type\n");
-		error = -EINVAL;
-		goto wake;
-	}
-
-	if (ops == STREAM_OPS_CAPTURE)
-		dir = "enc_";
-	else
-		dir = "dec_";
-	len = strlen(type) + strlen(dir);
-	strncpy(buf, type, sizeof(buf)-1);
-	strncpy(buf + strlen(type), dir, sizeof(buf)-strlen(type)-1);
-	len += snprintf(buf + len, sizeof(buf) - len, "%d",
-			lib->slot_info.slot_num);
-	len += snprintf(buf + len, sizeof(buf) - len, ".bin");
-
-	pr_debug("Requesting %s\n", buf);
-
-	error = request_firmware(&fw_lib, buf, &sst_drv_ctx->pci->dev);
-	if (error) {
-		pr_err("library load failed %d\n", error);
-		goto wake;
-	}
-	error = sst_validate_library(fw_lib, &lib->slot_info, &entry_point);
-	if (error)
-		goto wake_free;
-
-	lib->mod_entry_pt = entry_point;
-	memcpy(&dload_info.dload_lib, lib, sizeof(*lib));
-	error = sst_download_library(fw_lib, &dload_info);
-	if (error)
-		goto wake_free;
-
-	/* lib is downloaded and init send alloc again */
-	pr_debug("Library is downloaded now...\n");
-wake_free:
-	/* sst_wake_up_alloc_block(sst_drv_ctx, pvt_id, error, NULL); */
-	release_firmware(fw_lib);
-wake:
-	return error;
-}
-
diff --git a/drivers/staging/intel_sst/intel_sst_fw_ipc.h b/drivers/staging/intel_sst/intel_sst_fw_ipc.h
deleted file mode 100644
index 5d0cc56aaef9..000000000000
--- a/drivers/staging/intel_sst/intel_sst_fw_ipc.h
+++ /dev/null
@@ -1,416 +0,0 @@
-#ifndef __INTEL_SST_FW_IPC_H__
-#define __INTEL_SST_FW_IPC_H__
-/*
-*  intel_sst_fw_ipc.h - Intel SST Driver for audio engine
-*
-*  Copyright (C) 2008-10 Intel Corporation
-*  Author:	Vinod Koul <vinod.koul@intel.com>
-*		Harsha Priya <priya.harsha@intel.com>
-*		Dharageswari R <dharageswari.r@intel.com>
-*		KP Jeeja <jeeja.kp@intel.com>
-*  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-*
-*  This program is free software; you can redistribute it and/or modify
-*  it under the terms of the GNU General Public License as published by
-*  the Free Software Foundation; version 2 of the License.
-*
-*  This program is distributed in the hope that it will be useful, but
-*  WITHOUT ANY WARRANTY; without even the implied warranty of
-*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-*  General Public License for more details.
-*
-*  You should have received a copy of the GNU General Public License along
-*  with this program; if not, write to the Free Software Foundation, Inc.,
-*  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
-*
-* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-*
-*  This driver exposes the audio engine functionalities to the ALSA
-*	and middleware.
-*  This file has definitions shared between the firmware and driver
-*/
-
-#define MAX_NUM_STREAMS_MRST 3
-#define MAX_NUM_STREAMS_MFLD 6
-#define MAX_NUM_STREAMS 6
-#define MAX_DBG_RW_BYTES 80
-#define MAX_NUM_SCATTER_BUFFERS 8
-#define MAX_LOOP_BACK_DWORDS 8
-/* IPC base address and mailbox, timestamp offsets */
-#define SST_MAILBOX_SIZE 0x0400
-#define SST_MAILBOX_SEND 0x0000
-#define SST_MAILBOX_RCV 0x0804
-#define SST_TIME_STAMP 0x1800
-#define SST_RESERVED_OFFSET 0x1A00
-#define SST_CHEKPOINT_OFFSET 0x1C00
-#define REPLY_MSG 0x80
-
-/* Message ID's for IPC messages */
-/* Bits B7: SST or IA/SC ; B6-B4: Msg Category; B3-B0: Msg Type */
-
-/* I2L Firmware/Codec Download msgs */
-#define IPC_IA_PREP_LIB_DNLD 0x01
-#define IPC_IA_LIB_DNLD_CMPLT 0x02
-
-#define IPC_IA_SET_PMIC_TYPE 0x03
-#define IPC_IA_GET_FW_VERSION 0x04
-#define IPC_IA_GET_FW_BUILD_INF 0x05
-#define IPC_IA_GET_FW_INFO 0x06
-#define IPC_IA_GET_FW_CTXT 0x07
-#define IPC_IA_SET_FW_CTXT 0x08
-
-/* I2L Codec Config/control msgs */
-#define IPC_IA_SET_CODEC_PARAMS 0x10
-#define IPC_IA_GET_CODEC_PARAMS 0x11
-#define IPC_IA_SET_PPP_PARAMS 0x12
-#define IPC_IA_GET_PPP_PARAMS 0x13
-#define IPC_IA_PLAY_FRAMES 0x14
-#define IPC_IA_CAPT_FRAMES 0x15
-#define IPC_IA_PLAY_VOICE 0x16
-#define IPC_IA_CAPT_VOICE 0x17
-#define IPC_IA_DECODE_FRAMES 0x18
-
-#define IPC_IA_ALG_PARAMS 0x1A
-#define IPC_IA_TUNING_PARAMS 0x1B
-
-/* I2L Stream config/control msgs */
-#define IPC_IA_ALLOC_STREAM 0x20 /* Allocate a stream ID */
-#define IPC_IA_FREE_STREAM 0x21 /* Free the stream ID */
-#define IPC_IA_SET_STREAM_PARAMS 0x22
-#define IPC_IA_GET_STREAM_PARAMS 0x23
-#define IPC_IA_PAUSE_STREAM 0x24
-#define IPC_IA_RESUME_STREAM 0x25
-#define IPC_IA_DROP_STREAM 0x26
-#define IPC_IA_DRAIN_STREAM 0x27 /* Short msg with str_id */
-#define IPC_IA_TARGET_DEV_SELECT 0x28
-#define IPC_IA_CONTROL_ROUTING 0x29
-
-#define IPC_IA_SET_STREAM_VOL 0x2A /*Vol for stream, pre mixer */
-#define IPC_IA_GET_STREAM_VOL 0x2B
-#define IPC_IA_SET_STREAM_MUTE 0x2C
-#define IPC_IA_GET_STREAM_MUTE 0x2D
-#define IPC_IA_ENABLE_RX_TIME_SLOT 0x2E /* Enable Rx time slot 0 or 1 */
-
-#define IPC_IA_START_STREAM 0x30 /* Short msg with str_id */
-
-/* Debug msgs */
-#define IPC_IA_DBG_MEM_READ 0x40
-#define IPC_IA_DBG_MEM_WRITE 0x41
-#define IPC_IA_DBG_LOOP_BACK 0x42
-
-/* L2I Firmware/Codec Download msgs */
-#define IPC_IA_FW_INIT_CMPLT 0x81
-#define IPC_IA_LPE_GETTING_STALLED 0x82
-#define IPC_IA_LPE_UNSTALLED 0x83
-
-/* L2I Codec Config/control msgs */
-#define IPC_SST_GET_PLAY_FRAMES 0x90 /* Request IA more data */
-#define IPC_SST_GET_CAPT_FRAMES 0x91 /* Request IA more data */
-#define IPC_SST_BUF_UNDER_RUN 0x92 /* PB Under run and stopped */
-#define IPC_SST_BUF_OVER_RUN 0x93 /* CAP Under run and stopped */
-#define IPC_SST_DRAIN_END 0x94 /* PB Drain complete and stopped */
-#define IPC_SST_CHNGE_SSP_PARAMS 0x95 /* PB SSP parameters changed */
-#define IPC_SST_STREAM_PROCESS_FATAL_ERR 0x96/* error in processing a stream */
-#define IPC_SST_PERIOD_ELAPSED 0x97 /* period elapsed */
-#define IPC_IA_TARGET_DEV_CHNGD 0x98 /* error in processing a stream */
-
-#define IPC_SST_ERROR_EVENT 0x99 /* Buffer over run occurred */
-/* L2S messages */
-#define IPC_SC_DDR_LINK_UP 0xC0
-#define IPC_SC_DDR_LINK_DOWN 0xC1
-#define IPC_SC_SET_LPECLK_REQ 0xC2
-#define IPC_SC_SSP_BIT_BANG 0xC3
-
-/* L2I Error reporting msgs */
-#define IPC_IA_MEM_ALLOC_FAIL 0xE0
-#define IPC_IA_PROC_ERR 0xE1 /* error in processing a
-					stream can be used by playback and
-					capture modules */
-
-/* L2I Debug msgs */
-#define IPC_IA_PRINT_STRING 0xF0
-
-
-
-/* Command Response or Acknowledge message to any IPC message will have
- * same message ID and stream ID information which is sent.
- * There is no specific Ack message ID. The data field is used as response
- * meaning.
- */
-enum ackData {
-	IPC_ACK_SUCCESS = 0,
-	IPC_ACK_FAILURE
-};
-
-
-enum sst_error_codes {
-	/* Error code,response to msgId: Description */
-	/* Common error codes */
-	SST_SUCCESS = 0,	/* Success */
-	SST_ERR_INVALID_STREAM_ID = 1,
-	SST_ERR_INVALID_MSG_ID = 2,
-	SST_ERR_INVALID_STREAM_OP = 3,
-	SST_ERR_INVALID_PARAMS = 4,
-	SST_ERR_INVALID_CODEC = 5,
-	SST_ERR_INVALID_MEDIA_TYPE = 6,
-	SST_ERR_STREAM_ERR = 7,
-
-	/* IPC specific error codes */
-	SST_IPC_ERR_CALL_BACK_NOT_REGD = 8,
-	SST_IPC_ERR_STREAM_NOT_ALLOCATED = 9,
-	SST_IPC_ERR_STREAM_ALLOC_FAILED = 10,
-	SST_IPC_ERR_GET_STREAM_FAILED = 11,
-	SST_ERR_MOD_NOT_AVAIL = 12,
-	SST_ERR_MOD_DNLD_RQD = 13,
-	SST_ERR_STREAM_STOPPED = 14,
-	SST_ERR_STREAM_IN_USE = 15,
-
-	/* Capture specific error codes */
-	SST_CAP_ERR_INCMPLTE_CAPTURE_MSG = 16,
-	SST_CAP_ERR_CAPTURE_FAIL = 17,
-	SST_CAP_ERR_GET_DDR_NEW_SGLIST = 18,
-	SST_CAP_ERR_UNDER_RUN = 19,
-	SST_CAP_ERR_OVERFLOW = 20,
-
-	/* Playback specific error codes*/
-	SST_PB_ERR_INCMPLTE_PLAY_MSG = 21,
-	SST_PB_ERR_PLAY_FAIL = 22,
-	SST_PB_ERR_GET_DDR_NEW_SGLIST = 23,
-
-	/* Codec manager specific error codes */
-	SST_LIB_ERR_LIB_DNLD_REQUIRED = 24,
-	SST_LIB_ERR_LIB_NOT_SUPPORTED = 25,
-
-	/* Library manager specific error codes */
-	SST_SCC_ERR_PREP_DNLD_FAILED = 26,
-	SST_SCC_ERR_LIB_DNLD_RES_FAILED = 27,
-	/* Scheduler specific error codes */
-	SST_SCH_ERR_FAIL = 28,
-
-	/* DMA specific error codes */
-	SST_DMA_ERR_NO_CHNL_AVAILABLE = 29,
-	SST_DMA_ERR_INVALID_INPUT_PARAMS = 30,
-	SST_DMA_ERR_CHNL_ALREADY_SUSPENDED = 31,
-	SST_DMA_ERR_CHNL_ALREADY_STARTED = 32,
-	SST_DMA_ERR_CHNL_NOT_ENABLED = 33,
-	SST_DMA_ERR_TRANSFER_FAILED = 34,
-
-	SST_SSP_ERR_ALREADY_ENABLED = 35,
-	SST_SSP_ERR_ALREADY_DISABLED = 36,
-	SST_SSP_ERR_NOT_INITIALIZED = 37,
-	SST_SSP_ERR_SRAM_NO_DMA_DATA = 38,
-
-	/* Other error codes */
-	SST_ERR_MOD_INIT_FAIL = 39,
-
-	/* FW init error codes */
-	SST_RDR_ERR_IO_DEV_SEL_NOT_ALLOWED = 40,
-	SST_RDR_ERR_ROUTE_ALREADY_STARTED = 41,
-	SST_RDR_ERR_IO_DEV_SEL_FAILED = 42,
-	SST_RDR_PREP_CODEC_DNLD_FAILED = 43,
-
-	/* Memory debug error codes */
-	SST_ERR_DBG_MEM_READ_FAIL = 44,
-	SST_ERR_DBG_MEM_WRITE_FAIL = 45,
-	SST_ERR_INSUFFICIENT_INPUT_SG_LIST = 46,
-	SST_ERR_INSUFFICIENT_OUTPUT_SG_LIST = 47,
-
-	SST_ERR_BUFFER_NOT_AVAILABLE = 48,
-	SST_ERR_BUFFER_NOT_ALLOCATED = 49,
-	SST_ERR_INVALID_REGION_TYPE = 50,
-	SST_ERR_NULL_PTR = 51,
-	SST_ERR_INVALID_BUFFER_SIZE = 52,
-	SST_ERR_INVALID_BUFFER_INDEX = 53,
-
-	/*IIPC specific error codes */
-	SST_IIPC_QUEUE_FULL = 54,
-	SST_IIPC_ERR_MSG_SND_FAILED = 55,
-	SST_PB_ERR_UNDERRUN_OCCURED = 56,
-	SST_RDR_INSUFFICIENT_MIXER_BUFFER = 57,
-	SST_INVALID_TIME_SLOTS = 58,
-};
-
-enum dbg_mem_data_type {
-	/* Data type of debug read/write */
-	DATA_TYPE_U32,
-	DATA_TYPE_U16,
-	DATA_TYPE_U8,
-};
-
-/* CAUTION NOTE: All IPC message body must be multiple of 32 bits.*/
-
-/* IPC Header */
-union ipc_header {
-	struct {
-		u32  msg_id:8; /* Message ID - Max 256 Message Types */
-		u32  str_id:5;
-		u32  large:1;	/* Large Message if large = 1 */
-		u32  reserved:2;	/* Reserved for future use */
-		u32  data:14;	/* Ack/Info for msg, size of msg in Mailbox */
-		u32  done:1; /* bit 30 */
-		u32  busy:1; /* bit 31 */
-	} part;
-	u32 full;
-} __attribute__ ((packed));
-
-/* Firmware build info */
-struct sst_fw_build_info {
-	unsigned char  date[16]; /* Firmware build date */
-	unsigned char  time[16]; /* Firmware build time */
-} __attribute__ ((packed));
-
-struct ipc_header_fw_init {
-	struct snd_sst_fw_version fw_version;/* Firmware version details */
-	struct sst_fw_build_info build_info;
-	u16 result;	/* Fw init result */
-	u8 module_id; /* Module ID in case of error */
-	u8 debug_info; /* Debug info from Module ID in case of fail */
-} __attribute__ ((packed));
-
-/* Address and size info of a frame buffer in DDR */
-struct sst_address_info {
-	u32 addr; /* Address at IA */
-	u32 size; /* Size of the buffer */
-} __attribute__ ((packed));
-
-/* Time stamp */
-struct snd_sst_tstamp {
-	u64 samples_processed;/* capture - data in DDR */
-	u64 samples_rendered;/* playback - data rendered */
-	u64 bytes_processed;/* bytes decoded or encoded */
-	u32 sampling_frequency;/* eg: 48000, 44100 */
-	u32 dma_base_address;/* DMA base address */
-	u16	dma_channel_no;/* DMA Channel used for the data transfer*/
-	u16	reserved;/* 32 bit alignment */
-};
-
-/* Frame info to play or capture */
-struct sst_frame_info {
-	u16  num_entries; /* number of entries to follow */
-	u16  rsrvd;
-	struct sst_address_info  addr[MAX_NUM_SCATTER_BUFFERS];
-} __attribute__ ((packed));
-
-/* Frames info for decode */
-struct snd_sst_decode_info {
-	unsigned long long input_bytes_consumed;
-	unsigned long long output_bytes_produced;
-	struct sst_frame_info frames_in;
-	struct sst_frame_info frames_out;
-} __attribute__ ((packed));
-
-/* SST to IA print debug message*/
-struct ipc_sst_ia_print_params {
-	u32 string_size;/* Max value is 160 */
-	u8 prt_string[160];/* Null terminated Char string */
-} __attribute__ ((packed));
-
-/* Voice data message  */
-struct snd_sst_voice_data {
-	u16 num_bytes;/* Number of valid voice data bytes */
-	u8  pcm_wd_size;/* 0=8 bit, 1=16 bit 2=32 bit */
-	u8  reserved;/* Reserved */
-	u8  voice_data_buf[0];/* Voice data buffer in bytes, little endian */
-} __attribute__ ((packed));
-
-/* SST to IA memory read debug message  */
-struct ipc_sst_ia_dbg_mem_rw  {
-	u16  num_bytes;/* Maximum of MAX_DBG_RW_BYTES */
-	u16  data_type;/* enum: dbg_mem_data_type */
-	u32  address;	/* Memory address of data memory of data_type */
-	u8	rw_bytes[MAX_DBG_RW_BYTES];/* Maximum of 64 bytes can be RW */
-} __attribute__ ((packed));
-
-struct ipc_sst_ia_dbg_loop_back {
-	u16 num_dwords; /* Maximum of MAX_DBG_RW_BYTES */
-	u16 increment_val;/* Increments dwords by this value, 0- no increment */
-	u32 lpbk_dwords[MAX_LOOP_BACK_DWORDS];/* Maximum of 8 dwords loopback */
-} __attribute__ ((packed));
-
-/* Stream type params struture for Alloc stream */
-struct snd_sst_str_type {
-	u8 codec_type;		/* Codec type */
-	u8 str_type;		/* 1 = voice 2 = music */
-	u8 operation;		/* Playback or Capture */
-	u8 protected_str;	/* 0=Non DRM, 1=DRM */
-	u8 time_slots;
-	u8 reserved;		/* Reserved */
-	u16 result;		/* Result used for acknowledgment */
-} __attribute__ ((packed));
-
-/* Library info structure */
-struct module_info {
-	u32 lib_version;
-	u32 lib_type;/*TBD- KLOCKWORK u8 lib_type;*/
-	u32 media_type;
-	u8  lib_name[12];
-	u32 lib_caps;
-	unsigned char  b_date[16]; /* Lib build date */
-	unsigned char  b_time[16]; /* Lib build time */
-} __attribute__ ((packed));
-
-/* Library slot info */
-struct lib_slot_info {
-	u8  slot_num; /* 1 or 2 */
-	u8  reserved1;
-	u16 reserved2;
-	u32 iram_size; /* slot size in IRAM */
-	u32 dram_size; /* slot size in DRAM */
-	u32 iram_offset; /* starting offset of slot in IRAM */
-	u32 dram_offset; /* starting offset of slot in DRAM */
-} __attribute__ ((packed));
-
-struct snd_sst_lib_download {
-	struct module_info lib_info; /* library info type, capabilities etc */
-	struct lib_slot_info slot_info; /* slot info to be downloaded */
-	u32 mod_entry_pt;
-};
-
-struct snd_sst_lib_download_info {
-	struct snd_sst_lib_download dload_lib;
-	u16 result;	/* Result used for acknowledgment */
-	u8 pvt_id; /* Private ID */
-	u8 reserved;  /* for alignment */
-};
-
-/* Alloc stream params structure */
-struct snd_sst_alloc_params {
-	struct snd_sst_str_type str_type;
-	struct snd_sst_stream_params stream_params;
-};
-
-struct snd_sst_fw_get_stream_params {
-	struct snd_sst_stream_params codec_params;
-	struct snd_sst_pmic_config pcm_params;
-};
-
-/* Alloc stream response message */
-struct snd_sst_alloc_response {
-	struct snd_sst_str_type str_type; /* Stream type for allocation */
-	struct snd_sst_lib_download lib_dnld; /* Valid only for codec dnld */
-};
-
-/* Drop response */
-struct snd_sst_drop_response {
-	u32 result;
-	u32 bytes;
-};
-
-/* CSV Voice call routing structure */
-struct snd_sst_control_routing {
-	u8 control; /* 0=start, 1=Stop */
-	u8 reserved[3];	/* Reserved- for 32 bit alignment */
-};
-
-
-struct ipc_post {
-	struct list_head node;
-	union ipc_header header; /* driver specific */
-	char *mailbox_data;
-};
-
-struct snd_sst_ctxt_params {
-	u32 address; /* Physical Address in DDR where the context is stored */
-	u32 size; /* size of the context */
-};
-#endif /* __INTEL_SST_FW_IPC_H__ */
diff --git a/drivers/staging/intel_sst/intel_sst_ioctl.h b/drivers/staging/intel_sst/intel_sst_ioctl.h
deleted file mode 100644
index 5da5ee092c69..000000000000
--- a/drivers/staging/intel_sst/intel_sst_ioctl.h
+++ /dev/null
@@ -1,440 +0,0 @@
-#ifndef __INTEL_SST_IOCTL_H__
-#define __INTEL_SST_IOCTL_H__
-/*
- *  intel_sst_ioctl.h - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corporation
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This file defines all sst ioctls
- */
-
-/* codec and post/pre processing related info */
-
-#include <linux/types.h>
-
-enum sst_codec_types {
-/*  AUDIO/MUSIC	CODEC Type Definitions */
-	SST_CODEC_TYPE_UNKNOWN = 0,
-	SST_CODEC_TYPE_PCM,	/* Pass through Audio codec */
-	SST_CODEC_TYPE_MP3,
-	SST_CODEC_TYPE_MP24,
-	SST_CODEC_TYPE_AAC,
-	SST_CODEC_TYPE_AACP,
-	SST_CODEC_TYPE_eAACP,
-	SST_CODEC_TYPE_WMA9,
-	SST_CODEC_TYPE_WMA10,
-	SST_CODEC_TYPE_WMA10P,
-	SST_CODEC_TYPE_RA,
-	SST_CODEC_TYPE_DDAC3,
-	SST_CODEC_TYPE_STEREO_TRUE_HD,
-	SST_CODEC_TYPE_STEREO_HD_PLUS,
-
-	/*  VOICE CODEC Type Definitions */
-	SST_CODEC_TYPE_VOICE_PCM = 0x21, /* Pass through voice codec */
-};
-
-enum sst_algo_types {
-	SST_CODEC_SRC = 0x64,
-	SST_CODEC_MIXER = 0x65,
-	SST_CODEC_DOWN_MIXER = 0x66,
-	SST_CODEC_VOLUME_CONTROL = 0x67,
-	SST_CODEC_OEM1 = 0xC8,
-	SST_CODEC_OEM2 = 0xC9,
-};
-
-enum snd_sst_stream_ops {
-	STREAM_OPS_PLAYBACK = 0,	/* Decode */
-	STREAM_OPS_CAPTURE,		/* Encode */
-	STREAM_OPS_PLAYBACK_DRM,	/* Play Audio/Voice */
-	STREAM_OPS_PLAYBACK_ALERT,	/* Play Audio/Voice */
-	STREAM_OPS_CAPTURE_VOICE_CALL,	/* CSV Voice recording */
-};
-
-enum stream_mode {
-	SST_STREAM_MODE_NONE = 0,
-	SST_STREAM_MODE_DNR = 1,
-	SST_STREAM_MODE_FNF = 2,
-	SST_STREAM_MODE_CAPTURE = 3
-};
-
-enum stream_type {
-	SST_STREAM_TYPE_NONE = 0,
-	SST_STREAM_TYPE_MUSIC = 1,
-	SST_STREAM_TYPE_NORMAL = 2,
-	SST_STREAM_TYPE_LONG_PB = 3,
-	SST_STREAM_TYPE_LOW_LATENCY = 4,
-};
-
-enum snd_sst_audio_device_type {
-	SND_SST_DEVICE_HEADSET = 1,
-	SND_SST_DEVICE_IHF,
-	SND_SST_DEVICE_VIBRA,
-	SND_SST_DEVICE_HAPTIC,
-	SND_SST_DEVICE_CAPTURE,
-};
-
-/* Firmware Version info */
-struct snd_sst_fw_version {
-	__u8 build;	/* build number*/
-	__u8 minor;	/* minor number*/
-	__u8 major;	/* major number*/
-	__u8 type; /* build type */
-};
-
-/* Port info structure */
-struct snd_sst_port_info {
-	__u16 port_type;
-	__u16 reserved;
-};
-
-/* Mixer info structure */
-struct snd_sst_mix_info {
-	__u16 max_streams;
-	__u16 reserved;
-};
-
-/* PCM Parameters */
-struct snd_pcm_params {
-	__u16 codec;	/* codec type */
-	__u8 num_chan;	/* 1=Mono, 2=Stereo */
-	__u8 pcm_wd_sz;	/* 16/24 - bit*/
-	__u32 reserved;	/* Bitrate in bits per second */
-	__u32 sfreq;	/* Sampling rate in Hz */
-	__u32 ring_buffer_size;
-	__u32 period_count;	/* period elapsed in samples*/
-	__u32 ring_buffer_addr;
-};
-
-/* MP3 Music Parameters Message */
-struct snd_mp3_params {
-	__u16 codec;
-	__u8  num_chan;	/* 1=Mono, 2=Stereo	*/
-	__u8  pcm_wd_sz; /* 16/24 - bit*/
-	__u32 brate; /* Use the hard coded value. */
-	__u32 sfreq; /* Sampling freq eg. 8000, 441000, 48000 */
-	__u8  crc_check; /* crc_check - disable (0) or enable (1) */
-	__u8  op_align; /* op align 0- 16 bit, 1- MSB, 2 LSB*/
-	__u16 reserved;	/* Unused */
-};
-
-#define AAC_BIT_STREAM_ADTS		0
-#define AAC_BIT_STREAM_ADIF		1
-#define AAC_BIT_STREAM_RAW		2
-
-/* AAC Music Parameters Message */
-struct snd_aac_params {
-	__u16 codec;
-	__u8 num_chan; /* 1=Mono, 2=Stereo*/
-	__u8 pcm_wd_sz; /* 16/24 - bit*/
-	__u32 brate;
-	__u32 sfreq; /* Sampling freq eg. 8000, 441000, 48000 */
-	__u32 aac_srate;	/* Plain AAC decoder operating sample rate */
-	__u8 mpg_id; /* 0=MPEG-2, 1=MPEG-4 */
-	__u8 bs_format; /* input bit stream format adts=0, adif=1, raw=2 */
-	__u8 aac_profile; /* 0=Main Profile, 1=LC profile, 3=SSR profile */
-	__u8 ext_chl; /* No.of external channels */
-	__u8 aot; /* Audio object type. 1=Main , 2=LC , 3=SSR, 4=SBR*/
-	__u8 op_align; /* output alignment 0=16 bit , 1=MSB, 2= LSB align */
-	__u8 brate_type; /* 0=CBR, 1=VBR */
-	__u8 crc_check; /* crc check 0= disable, 1=enable */
-	__s8 bit_stream_format[8]; /* input bit stream format adts/adif/raw */
-	__u8 jstereo; /* Joint stereo Flag */
-	__u8 sbr_present; /* 1 = SBR Present, 0 = SBR absent, for RAW */
-	__u8 downsample;       /* 1 = Downsampling ON, 0 = Downsampling OFF */
-	__u8 num_syntc_elems; /* 1- Mono/stereo, 0 - Dual Mono, 0 - for raw */
-	__s8 syntc_id[2]; /* 0 for ID_SCE(Dula Mono), -1 for raw */
-	__s8 syntc_tag[2]; /* raw - -1 and 0 -16 for rest of the streams */
-	__u8 pce_present; /* Flag. 1- present 0 - not present, for RAW */
-	__u8 sbr_type;		/* sbr_type: 0-plain aac, 1-aac-v1, 2-aac-v2 */
-	__u8 outchmode;  /*0- mono, 1-stereo, 2-dual mono 3-Parametric stereo */
-	__u8 ps_present;
-};
-
-/* WMA Music Parameters Message */
-struct snd_wma_params {
-	__u16 codec;
-	__u8  num_chan;	/* 1=Mono, 2=Stereo */
-	__u8  pcm_wd_sz;	/* 16/24 - bit*/
-	__u32 brate;	/* Use the hard coded value. */
-	__u32 sfreq;	/* Sampling freq eg. 8000, 441000, 48000 */
-	__u32 channel_mask;  /* Channel Mask */
-	__u16 format_tag;	/* Format Tag */
-	__u16 block_align;	/* packet size */
-	__u16 wma_encode_opt;/* Encoder option */
-	__u8 op_align;	/* op align 0- 16 bit, 1- MSB, 2 LSB */
-	__u8 pcm_src;	/* input pcm bit width */
-};
-
-/* Pre processing param structure */
-struct snd_prp_params {
-	__u32 reserved;	/* No pre-processing defined yet */
-};
-
-/* Pre and post processing params structure */
-struct snd_ppp_params {
-	__u8			algo_id;/* Post/Pre processing algorithm ID  */
-	__u8			str_id;	/*Only 5 bits used 0 - 31 are valid*/
-	__u8			enable;	/* 0= disable, 1= enable*/
-	__u8			reserved;
-	__u32			size;	/*Size of parameters for all blocks*/
-	void			*params;
-} __attribute__ ((packed));
-
-struct snd_sst_postproc_info {
-	__u32 src_min;		/* Supported SRC Min sampling freq */
-	__u32 src_max;		/* Supported SRC Max sampling freq */
-	__u8  src;		/* 0=Not supported, 1=Supported */
-	__u8  bass_boost;		/* 0=Not Supported, 1=Supported */
-	__u8  stereo_widening;	/* 0=Not Supported, 1=Supported */
-	__u8  volume_control;	/* 0=Not Supported, 1=Supported */
-	__s16 min_vol;		/* Minimum value of Volume in dB */
-	__s16 max_vol;		/* Maximum value of Volume in dB */
-	__u8 mute_control;	/* 0=No Mute, 1=Mute */
-	__u8 reserved1;
-	__u16 reserved2;
-};
-
-/* pre processing Capability info structure */
-struct snd_sst_prp_info {
-	__s16 min_vol;			/* Minimum value of Volume in dB */
-	__s16 max_vol;			/* Maximum value of Volume in dB */
-	__u8 volume_control;		/* 0=Not Supported, 1=Supported */
-	__u8 reserved1;			/* for 32 bit alignment */
-	__u16 reserved2;		/* for 32 bit alignment */
-} __attribute__ ((packed));
-
-/*Pre / Post processing algorithms support*/
-struct snd_sst_ppp_info {
-	__u32 src:1;		/* 0=Not supported, 1=Supported */
-	__u32 mixer:1;		/* 0=Not supported, 1=Supported */
-	__u32 volume_control:1;	/* 0=Not Supported, 1=Supported */
-	__u32 mute_control:1;	/* 0=Not Supported, 1=Supported */
-	__u32 anc:1;		/* 0=Not Supported, 1=Supported */
-	__u32 side_tone:1;	/* 0=Not Supported, 1=Supported */
-	__u32 dc_removal:1;	/* 0=Not Supported, 1=Supported */
-	__u32 equalizer:1;	/* 0=Not Supported, 1=Supported */
-	__u32 spkr_prot:1;	/* 0=Not Supported, 1=Supported */
-	__u32 bass_boost:1;	/* 0=Not Supported, 1=Supported */
-	__u32 stereo_widening:1;/* 0=Not Supported, 1=Supported */
-	__u32 rsvd1:21;
-	__u32 rsvd2;
-};
-
-/* Firmware capabilities info */
-struct snd_sst_fw_info {
-	struct snd_sst_fw_version fw_version; /* Firmware version */
-	__u8 audio_codecs_supported[8];	/* Codecs supported by FW */
-	__u32 recommend_min_duration; /* Min duration for Lowpower Playback */
-	__u8 max_pcm_streams_supported; /* Max num of PCM streams supported */
-	__u8 max_enc_streams_supported;	/* Max number of Encoded streams  */
-	__u16 reserved;		/* 32 bit alignment*/
-	struct snd_sst_ppp_info ppp_info; /* pre_processing mod cap info */
-	struct snd_sst_postproc_info pop_info; /* Post processing cap info*/
-	struct snd_sst_port_info port_info[3]; /* Port info */
-	struct snd_sst_mix_info mix_info;/* Mixer info */
-	__u32 min_input_buf; /* minmum i/p buffer for decode */
-};
-
-/* Codec params struture */
-union  snd_sst_codec_params {
-	struct snd_pcm_params pcm_params;
-	struct snd_mp3_params mp3_params;
-	struct snd_aac_params aac_params;
-	struct snd_wma_params wma_params;
-};
-
-
-struct snd_sst_stream_params {
-	union snd_sst_codec_params uc;
-} __attribute__ ((packed));
-
-struct snd_sst_params {
-	__u32 result;
-	__u32 stream_id;
-	__u8 codec;
-	__u8 ops;
-	__u8 stream_type;
-	__u8 device_type;
-	struct snd_sst_stream_params sparams;
-};
-
-struct snd_sst_vol {
-	__u32	stream_id;
-	__s32	volume;
-	__u32	ramp_duration;
-	__u32	ramp_type;		/* Ramp type, default=0 */
-};
-
-struct snd_sst_mute {
-	__u32	stream_id;
-	__u32	mute;
-};
-
-/* ioctl related stuff here */
-struct snd_sst_pmic_config {
-	__u32  sfreq;                /* Sampling rate in Hz */
-	__u16  num_chan;             /* Mono =1 or Stereo =2 */
-	__u16  pcm_wd_sz;            /* Number of bits per sample */
-} __attribute__ ((packed));
-
-struct snd_sst_get_stream_params {
-	struct snd_sst_params codec_params;
-	struct snd_sst_pmic_config pcm_params;
-};
-
-enum snd_sst_target_type {
-	SND_SST_TARGET_PMIC = 1,
-	SND_SST_TARGET_LPE,
-	SND_SST_TARGET_MODEM,
-	SND_SST_TARGET_BT,
-	SND_SST_TARGET_FM,
-	SND_SST_TARGET_NONE,
-};
-
-enum snd_sst_device_type {
-	SND_SST_DEVICE_SSP = 1,
-	SND_SST_DEVICE_PCM,
-	SND_SST_DEVICE_OTHER,
-};
-
-enum snd_sst_device_mode {
-
-	SND_SST_DEV_MODE_PCM_MODE1 = 1, /*(16-bit word, bit-length frame sync)*/
-	SND_SST_DEV_MODE_PCM_MODE2,
-	SND_SST_DEV_MODE_PCM_MODE3,
-	SND_SST_DEV_MODE_PCM_MODE4_RIGHT_JUSTIFIED,
-	SND_SST_DEV_MODE_PCM_MODE4_LEFT_JUSTIFIED,
-	SND_SST_DEV_MODE_PCM_MODE4_I2S, /*(I2S mode, 16-bit words)*/
-	SND_SST_DEV_MODE_PCM_MODE5,
-	SND_SST_DEV_MODE_PCM_MODE6,
-};
-
-enum snd_sst_port_action {
-	SND_SST_PORT_PREPARE = 1,
-	SND_SST_PORT_ACTIVATE,
-};
-
-/* Target selection per device structure */
-struct snd_sst_slot_info {
-	__u8 mix_enable;	/* Mixer enable or disable */
-	__u8 device_type;
-	__u8 device_instance;	/* 0, 1, 2 */
-	__u8 target_device;
-	__u16 target_sink;
-	__u8 slot[2];
-	__u8 master;
-	__u8 action;
-	__u8 device_mode;
-	__u8 reserved;
-	struct snd_sst_pmic_config pcm_params;
-} __attribute__ ((packed));
-
-#define SST_MAX_TARGET_DEVICES 3
-/* Target device list structure */
-struct snd_sst_target_device  {
-	__u32 device_route;
-	struct snd_sst_slot_info devices[SST_MAX_TARGET_DEVICES];
-} __attribute__ ((packed));
-
-struct snd_sst_driver_info {
-	__u32 version;	/* Version of the driver */
-	__u32 active_pcm_streams;
-	__u32 active_enc_streams;
-	__u32 max_pcm_streams;
-	__u32 max_enc_streams;
-	__u32 buf_per_stream;
-};
-
-enum snd_sst_buff_type {
-	SST_BUF_USER = 1,
-	SST_BUF_MMAP,
-	SST_BUF_RAR,
-};
-
-struct snd_sst_mmap_buff_entry {
-	unsigned int offset;
-	unsigned int size;
-};
-
-struct snd_sst_mmap_buffs {
-	unsigned int entries;
-	enum snd_sst_buff_type type;
-	struct snd_sst_mmap_buff_entry *buff;
-};
-
-struct snd_sst_buff_entry {
-	void *buffer;
-	unsigned int size;
-};
-
-struct snd_sst_buffs {
-	unsigned int entries;
-	__u8 type;
-	struct snd_sst_buff_entry *buff_entry;
-};
-
-struct snd_sst_dbufs  {
-	unsigned long long input_bytes_consumed;
-	unsigned long long output_bytes_produced;
-	struct snd_sst_buffs *ibufs;
-	struct snd_sst_buffs *obufs;
-};
-
-struct snd_sst_tuning_params {
-	__u8 type;
-	__u8 str_id;
-	__u8 size;
-	__u8 rsvd;
-	__aligned_u64 addr;
-} __attribute__ ((packed));
-/*IOCTL defined here */
-/*SST MMF IOCTLS only */
-#define SNDRV_SST_STREAM_SET_PARAMS _IOR('L', 0x00, \
-					struct snd_sst_stream_params *)
-#define SNDRV_SST_STREAM_GET_PARAMS _IOWR('L', 0x01, \
-					struct snd_sst_get_stream_params *)
-#define SNDRV_SST_STREAM_GET_TSTAMP _IOWR('L', 0x02, __u64 *)
-#define	SNDRV_SST_STREAM_DECODE	_IOWR('L', 0x03, struct snd_sst_dbufs *)
-#define SNDRV_SST_STREAM_BYTES_DECODED _IOWR('L', 0x04, __u64 *)
-#define SNDRV_SST_STREAM_START	_IO('A', 0x42)
-#define SNDRV_SST_STREAM_DROP	_IO('A', 0x43)
-#define SNDRV_SST_STREAM_DRAIN	_IO('A', 0x44)
-#define SNDRV_SST_STREAM_PAUSE	_IOW('A', 0x45, int)
-#define SNDRV_SST_STREAM_RESUME _IO('A', 0x47)
-#define SNDRV_SST_MMAP_PLAY	_IOW('L', 0x05, struct snd_sst_mmap_buffs *)
-#define SNDRV_SST_MMAP_CAPTURE _IOW('L', 0x06, struct snd_sst_mmap_buffs *)
-/*SST common ioctls */
-#define SNDRV_SST_DRIVER_INFO	_IOR('L', 0x10, struct snd_sst_driver_info *)
-#define SNDRV_SST_SET_VOL	_IOW('L', 0x11, struct snd_sst_vol *)
-#define SNDRV_SST_GET_VOL	_IOW('L', 0x12, struct snd_sst_vol *)
-#define SNDRV_SST_MUTE		_IOW('L', 0x13, struct snd_sst_mute *)
-/*AM Ioctly only */
-#define SNDRV_SST_FW_INFO	_IOR('L', 0x20,  struct snd_sst_fw_info *)
-#define SNDRV_SST_SET_TARGET_DEVICE _IOW('L', 0x21, \
-					struct snd_sst_target_device *)
-/*DSP Ioctls on /dev/intel_sst_ctrl only*/
-#define SNDRV_SST_SET_ALGO	_IOW('L', 0x30,  struct snd_ppp_params *)
-#define SNDRV_SST_GET_ALGO	_IOWR('L', 0x31,  struct snd_ppp_params *)
-#define SNDRV_SST_TUNING_PARAMS	_IOW('L', 0x32,  struct snd_sst_tuning_params *)
-
-#endif /* __INTEL_SST_IOCTL_H__ */
diff --git a/drivers/staging/intel_sst/intel_sst_ipc.c b/drivers/staging/intel_sst/intel_sst_ipc.c
deleted file mode 100644
index 5c3444f6ab41..000000000000
--- a/drivers/staging/intel_sst/intel_sst_ipc.c
+++ /dev/null
@@ -1,774 +0,0 @@
-/*
- *  intel_sst_ipc.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corporation
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This file defines all ipc functions
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/firmware.h>
-#include <linux/sched.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-
-/*
- * sst_send_sound_card_type - send sound card type
- *
- * this function sends the sound card type to sst dsp engine
- */
-static void sst_send_sound_card_type(void)
-{
-	struct ipc_post *msg = NULL;
-
-	if (sst_create_short_msg(&msg))
-		return;
-
-	sst_fill_header(&msg->header, IPC_IA_SET_PMIC_TYPE, 0, 0);
-	msg->header.part.data = sst_drv_ctx->pmic_vendor;
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	return;
-}
-
-/**
-* sst_post_message - Posts message to SST
-*
-* @work: Pointer to work structure
-*
-* This function is called by any component in driver which
-* wants to send an IPC message. This will post message only if
-* busy bit is free
-*/
-void sst_post_message(struct work_struct *work)
-{
-	struct ipc_post *msg;
-	union ipc_header header;
-	union  interrupt_reg imr;
-	int retval = 0;
-	imr.full = 0;
-
-	/*To check if LPE is in stalled state.*/
-	retval = sst_stalled();
-	if (retval < 0) {
-		pr_err("in stalled state\n");
-		return;
-	}
-	pr_debug("post message called\n");
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-
-	/* check list */
-	if (list_empty(&sst_drv_ctx->ipc_dispatch_list)) {
-		/* list is empty, mask imr */
-		pr_debug("Empty msg queue... masking\n");
-		imr.full = readl(sst_drv_ctx->shim + SST_IMRX);
-		imr.part.done_interrupt = 1;
-		/* dummy register for shim workaround */
-		sst_shim_write(sst_drv_ctx->shim, SST_IMRX, imr.full);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		return;
-	}
-
-	/* check busy bit */
-	header.full = sst_shim_read(sst_drv_ctx->shim, SST_IPCX);
-	if (header.part.busy) {
-		/* busy, unmask */
-		pr_debug("Busy not free... unmasking\n");
-		imr.full = readl(sst_drv_ctx->shim + SST_IMRX);
-		imr.part.done_interrupt = 0;
-		/* dummy register for shim workaround */
-		sst_shim_write(sst_drv_ctx->shim, SST_IMRX, imr.full);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		return;
-	}
-	/* copy msg from list */
-	msg = list_entry(sst_drv_ctx->ipc_dispatch_list.next,
-			struct ipc_post, node);
-	list_del(&msg->node);
-	pr_debug("Post message: header = %x\n", msg->header.full);
-	pr_debug("size: = %x\n", msg->header.part.data);
-	if (msg->header.part.large)
-		memcpy_toio(sst_drv_ctx->mailbox + SST_MAILBOX_SEND,
-			msg->mailbox_data, msg->header.part.data);
-	/* dummy register for shim workaround */
-
-	sst_shim_write(sst_drv_ctx->shim, SST_IPCX, msg->header.full);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-
-	kfree(msg->mailbox_data);
-	kfree(msg);
-	return;
-}
-
-/*
- * sst_clear_interrupt - clear the SST FW interrupt
- *
- * This function clears the interrupt register after the interrupt
- * bottom half is complete allowing next interrupt to arrive
- */
-void sst_clear_interrupt(void)
-{
-	union interrupt_reg isr;
-	union interrupt_reg imr;
-	union ipc_header clear_ipc;
-
-	imr.full = sst_shim_read(sst_drv_ctx->shim, SST_IMRX);
-	isr.full = sst_shim_read(sst_drv_ctx->shim, SST_ISRX);
-	/*  write 1 to clear  */;
-	isr.part.busy_interrupt = 1;
-	sst_shim_write(sst_drv_ctx->shim, SST_ISRX, isr.full);
-	/* Set IA done bit */
-	clear_ipc.full = sst_shim_read(sst_drv_ctx->shim, SST_IPCD);
-	clear_ipc.part.busy = 0;
-	clear_ipc.part.done = 1;
-	clear_ipc.part.data = IPC_ACK_SUCCESS;
-	sst_shim_write(sst_drv_ctx->shim, SST_IPCD, clear_ipc.full);
-	/* un mask busy interrupt */
-	imr.part.busy_interrupt = 0;
-	sst_shim_write(sst_drv_ctx->shim, SST_IMRX, imr.full);
-}
-
-void sst_restore_fw_context(void)
-{
-	struct snd_sst_ctxt_params fw_context;
-	struct ipc_post *msg = NULL;
-
-	pr_debug("restore_fw_context\n");
-	/*check cpu type*/
-	if (sst_drv_ctx->pci_id != SST_MFLD_PCI_ID)
-		return;
-		/*not supported for rest*/
-	if (!sst_drv_ctx->fw_cntx_size)
-		return;
-		/*nothing to restore*/
-	pr_debug("restoring context......\n");
-	/*send msg to fw*/
-	if (sst_create_large_msg(&msg))
-		return;
-
-	sst_fill_header(&msg->header, IPC_IA_SET_FW_CTXT, 1, 0);
-	msg->header.part.data = sizeof(fw_context) + sizeof(u32);
-	fw_context.address = virt_to_phys((void *)sst_drv_ctx->fw_cntx);
-	fw_context.size = sst_drv_ctx->fw_cntx_size;
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32),
-				&fw_context, sizeof(fw_context));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	return;
-}
-/*
- * process_fw_init - process the FW init msg
- *
- * @msg: IPC message from FW
- *
- * This function processes the FW init msg from FW
- * marks FW state and prints debug info of loaded FW
- */
-int process_fw_init(struct sst_ipc_msg_wq *msg)
-{
-	struct ipc_header_fw_init *init =
-		(struct ipc_header_fw_init *)msg->mailbox;
-	int retval = 0;
-
-	pr_debug("*** FW Init msg came***\n");
-	if (init->result) {
-		mutex_lock(&sst_drv_ctx->sst_lock);
-		sst_drv_ctx->sst_state = SST_ERROR;
-		mutex_unlock(&sst_drv_ctx->sst_lock);
-		pr_debug("FW Init failed, Error %x\n", init->result);
-		pr_err("FW Init failed, Error %x\n", init->result);
-		retval = -init->result;
-		return retval;
-	}
-	if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID)
-		sst_send_sound_card_type();
-	mutex_lock(&sst_drv_ctx->sst_lock);
-	sst_drv_ctx->sst_state = SST_FW_RUNNING;
-	sst_drv_ctx->lpe_stalled = 0;
-	mutex_unlock(&sst_drv_ctx->sst_lock);
-	pr_debug("FW Version %02x.%02x.%02x\n", init->fw_version.major,
-			init->fw_version.minor, init->fw_version.build);
-	pr_debug("Build Type %x\n", init->fw_version.type);
-	pr_debug(" Build date %s Time %s\n",
-			init->build_info.date, init->build_info.time);
-	sst_wake_up_alloc_block(sst_drv_ctx, FW_DWNL_ID, retval, NULL);
-	sst_restore_fw_context();
-	return retval;
-}
-/**
-* sst_process_message - Processes message from SST
-*
-* @work:	Pointer to work structure
-*
-* This function is scheduled by ISR
-* It take a msg from process_queue and does action based on msg
-*/
-void sst_process_message(struct work_struct *work)
-{
-	struct sst_ipc_msg_wq *msg =
-			container_of(work, struct sst_ipc_msg_wq, wq);
-	int str_id = msg->header.part.str_id;
-
-	pr_debug("IPC process for %x\n", msg->header.full);
-
-	/* based on msg in list call respective handler */
-	switch (msg->header.part.msg_id) {
-	case IPC_SST_BUF_UNDER_RUN:
-	case IPC_SST_BUF_OVER_RUN:
-		if (sst_validate_strid(str_id)) {
-			pr_err("stream id %d invalid\n", str_id);
-			break;
-		}
-		pr_err("Buffer under/overrun for %d\n",
-				msg->header.part.str_id);
-		pr_err("Got Underrun & not to send data...ignore\n");
-		break;
-
-	case IPC_SST_GET_PLAY_FRAMES:
-		if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID) {
-			struct stream_info *stream ;
-
-			if (sst_validate_strid(str_id)) {
-				pr_err("strid %d invalid\n", str_id);
-				break;
-			}
-			/* call sst_play_frame */
-			stream = &sst_drv_ctx->streams[str_id];
-			pr_debug("sst_play_frames for %d\n",
-					msg->header.part.str_id);
-			mutex_lock(&sst_drv_ctx->streams[str_id].lock);
-			sst_play_frame(msg->header.part.str_id);
-			mutex_unlock(&sst_drv_ctx->streams[str_id].lock);
-			break;
-		} else
-			pr_err("sst_play_frames for Penwell!!\n");
-
-	case IPC_SST_GET_CAPT_FRAMES:
-		if (sst_drv_ctx->pci_id == SST_MRST_PCI_ID) {
-			struct stream_info *stream;
-			/* call sst_capture_frame */
-			if (sst_validate_strid(str_id)) {
-				pr_err("str id %d invalid\n", str_id);
-				break;
-			}
-			stream = &sst_drv_ctx->streams[str_id];
-			pr_debug("sst_capture_frames for %d\n",
-					msg->header.part.str_id);
-			mutex_lock(&stream->lock);
-			if (stream->mmapped == false &&
-					stream->src == SST_DRV) {
-				pr_debug("waking up block for copy.\n");
-				stream->data_blk.ret_code = 0;
-				stream->data_blk.condition = true;
-				stream->data_blk.on = false;
-				wake_up(&sst_drv_ctx->wait_queue);
-			} else
-				sst_capture_frame(msg->header.part.str_id);
-			mutex_unlock(&stream->lock);
-		} else
-			pr_err("sst_play_frames for Penwell!!\n");
-		break;
-
-	case IPC_IA_PRINT_STRING:
-		pr_debug("been asked to print something by fw\n");
-		/* TBD */
-		break;
-
-	case IPC_IA_FW_INIT_CMPLT: {
-		/* send next data to FW */
-		process_fw_init(msg);
-		break;
-	}
-
-	case IPC_SST_STREAM_PROCESS_FATAL_ERR:
-		if (sst_validate_strid(str_id)) {
-			pr_err("stream id %d invalid\n", str_id);
-			break;
-		}
-		pr_err("codec fatal error %x stream %d...\n",
-				msg->header.full, msg->header.part.str_id);
-		pr_err("Dropping the stream\n");
-		sst_drop_stream(msg->header.part.str_id);
-		break;
-	case IPC_IA_LPE_GETTING_STALLED:
-		sst_drv_ctx->lpe_stalled = 1;
-		break;
-	case IPC_IA_LPE_UNSTALLED:
-		sst_drv_ctx->lpe_stalled = 0;
-		break;
-	default:
-		/* Illegal case */
-		pr_err("Unhandled msg %x header %x\n",
-		msg->header.part.msg_id, msg->header.full);
-	}
-	sst_clear_interrupt();
-	return;
-}
-
-/**
-* sst_process_reply - Processes reply message from SST
-*
-* @work:	Pointer to work structure
-*
-* This function is scheduled by ISR
-* It take a reply msg from response_queue and
-* does action based on msg
-*/
-void sst_process_reply(struct work_struct *work)
-{
-	struct sst_ipc_msg_wq *msg =
-			container_of(work, struct sst_ipc_msg_wq, wq);
-
-	int str_id = msg->header.part.str_id;
-	struct stream_info *str_info;
-
-	switch (msg->header.part.msg_id) {
-	case IPC_IA_TARGET_DEV_SELECT:
-		if (!msg->header.part.data) {
-			sst_drv_ctx->tgt_dev_blk.ret_code = 0;
-		} else {
-			pr_err(" Msg %x reply error %x\n",
-			msg->header.part.msg_id, msg->header.part.data);
-			sst_drv_ctx->tgt_dev_blk.ret_code =
-					-msg->header.part.data;
-		}
-
-		if (sst_drv_ctx->tgt_dev_blk.on == true) {
-				sst_drv_ctx->tgt_dev_blk.condition = true;
-				wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	case IPC_IA_ALG_PARAMS: {
-		pr_debug("sst:IPC_ALG_PARAMS response %x\n", msg->header.full);
-		pr_debug("sst: data value %x\n", msg->header.part.data);
-		pr_debug("sst: large value %x\n", msg->header.part.large);
-
-		if (!msg->header.part.large) {
-			if (!msg->header.part.data) {
-				pr_debug("sst: alg set success\n");
-				sst_drv_ctx->ppp_params_blk.ret_code = 0;
-			} else {
-				pr_debug("sst: alg set failed\n");
-				sst_drv_ctx->ppp_params_blk.ret_code =
-							-msg->header.part.data;
-			}
-
-		} else if (msg->header.part.data) {
-			struct snd_ppp_params *mailbox_params, *get_params;
-			char *params;
-
-			pr_debug("sst: alg get success\n");
-			mailbox_params = (struct snd_ppp_params *)msg->mailbox;
-			get_params = kzalloc(sizeof(*get_params), GFP_KERNEL);
-			if (get_params == NULL) {
-				pr_err("sst: out of memory for ALG PARAMS");
-				break;
-			}
-			memcpy_fromio(get_params, mailbox_params,
-							sizeof(*get_params));
-			get_params->params = kzalloc(mailbox_params->size,
-							GFP_KERNEL);
-			if (get_params->params == NULL) {
-				kfree(get_params);
-				pr_err("sst: out of memory for ALG PARAMS block");
-				break;
-			}
-			params = msg->mailbox;
-			params = params + sizeof(*mailbox_params) - sizeof(u32);
-			memcpy_fromio(get_params->params, params,
-							get_params->size);
-			sst_drv_ctx->ppp_params_blk.ret_code = 0;
-			sst_drv_ctx->ppp_params_blk.data = get_params;
-		}
-
-		if (sst_drv_ctx->ppp_params_blk.on == true) {
-			sst_drv_ctx->ppp_params_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	}
-
-	case IPC_IA_TUNING_PARAMS: {
-		pr_debug("sst:IPC_TUNING_PARAMS resp: %x\n", msg->header.full);
-		pr_debug("data value %x\n", msg->header.part.data);
-		if (msg->header.part.large) {
-			pr_debug("alg set failed\n");
-			sst_drv_ctx->ppp_params_blk.ret_code =
-							-msg->header.part.data;
-		} else {
-			pr_debug("alg set success\n");
-			sst_drv_ctx->ppp_params_blk.ret_code = 0;
-		}
-		if (sst_drv_ctx->ppp_params_blk.on == true) {
-			sst_drv_ctx->ppp_params_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-	}
-
-	case IPC_IA_GET_FW_INFO: {
-		struct snd_sst_fw_info *fw_info =
-			(struct snd_sst_fw_info *)msg->mailbox;
-		if (msg->header.part.large) {
-			int major = fw_info->fw_version.major;
-			int minor = fw_info->fw_version.minor;
-			int build = fw_info->fw_version.build;
-			pr_debug("Msg succeeded %x\n",
-				       msg->header.part.msg_id);
-			pr_debug("INFO: ***FW*** = %02d.%02d.%02d\n",
-					major, minor, build);
-			memcpy_fromio(sst_drv_ctx->fw_info_blk.data,
-				((struct snd_sst_fw_info *)(msg->mailbox)),
-				sizeof(struct snd_sst_fw_info));
-			sst_drv_ctx->fw_info_blk.ret_code = 0;
-		} else {
-			pr_err(" Msg %x reply error %x\n",
-			msg->header.part.msg_id, msg->header.part.data);
-			sst_drv_ctx->fw_info_blk.ret_code =
-					-msg->header.part.data;
-		}
-		if (sst_drv_ctx->fw_info_blk.on == true) {
-			pr_debug("Memcopy succeeded\n");
-			sst_drv_ctx->fw_info_blk.on = false;
-			sst_drv_ctx->fw_info_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	}
-	case IPC_IA_SET_STREAM_MUTE:
-		if (!msg->header.part.data) {
-			pr_debug("Msg succeeded %x\n",
-				       msg->header.part.msg_id);
-			sst_drv_ctx->mute_info_blk.ret_code = 0;
-		} else {
-			pr_err(" Msg %x reply error %x\n",
-			msg->header.part.msg_id, msg->header.part.data);
-			sst_drv_ctx->mute_info_blk.ret_code =
-					-msg->header.part.data;
-
-		}
-		if (sst_drv_ctx->mute_info_blk.on == true) {
-			sst_drv_ctx->mute_info_blk.on = false;
-			sst_drv_ctx->mute_info_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	case IPC_IA_SET_STREAM_VOL:
-		if (!msg->header.part.data) {
-			pr_debug("Msg succeeded %x\n",
-				       msg->header.part.msg_id);
-			sst_drv_ctx->vol_info_blk.ret_code = 0;
-		} else {
-			pr_err(" Msg %x reply error %x\n",
-					msg->header.part.msg_id,
-			msg->header.part.data);
-			sst_drv_ctx->vol_info_blk.ret_code =
-					-msg->header.part.data;
-
-		}
-
-		if (sst_drv_ctx->vol_info_blk.on == true) {
-			sst_drv_ctx->vol_info_blk.on = false;
-			sst_drv_ctx->vol_info_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	case IPC_IA_GET_STREAM_VOL:
-		if (msg->header.part.large) {
-			pr_debug("Large Msg Received Successfully\n");
-			pr_debug("Msg succeeded %x\n",
-				       msg->header.part.msg_id);
-			memcpy_fromio(sst_drv_ctx->vol_info_blk.data,
-				(void *) msg->mailbox,
-				sizeof(struct snd_sst_vol));
-			sst_drv_ctx->vol_info_blk.ret_code = 0;
-		} else {
-			pr_err("Msg %x reply error %x\n",
-			msg->header.part.msg_id, msg->header.part.data);
-			sst_drv_ctx->vol_info_blk.ret_code =
-					-msg->header.part.data;
-		}
-		if (sst_drv_ctx->vol_info_blk.on == true) {
-			sst_drv_ctx->vol_info_blk.on = false;
-			sst_drv_ctx->vol_info_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-
-	case IPC_IA_GET_STREAM_PARAMS:
-		if (sst_validate_strid(str_id)) {
-			pr_err("stream id %d invalid\n", str_id);
-			break;
-		}
-		str_info = &sst_drv_ctx->streams[str_id];
-		if (msg->header.part.large) {
-			pr_debug("Get stream large success\n");
-			memcpy_fromio(str_info->ctrl_blk.data,
-				((void *)(msg->mailbox)),
-				sizeof(struct snd_sst_fw_get_stream_params));
-			str_info->ctrl_blk.ret_code = 0;
-		} else {
-			pr_err("Msg %x reply error %x\n",
-				msg->header.part.msg_id, msg->header.part.data);
-			str_info->ctrl_blk.ret_code = -msg->header.part.data;
-		}
-		if (str_info->ctrl_blk.on == true) {
-			str_info->ctrl_blk.on = false;
-			str_info->ctrl_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	case IPC_IA_DECODE_FRAMES:
-		if (sst_validate_strid(str_id)) {
-			pr_err("stream id %d invalid\n", str_id);
-			break;
-		}
-		str_info = &sst_drv_ctx->streams[str_id];
-		if (msg->header.part.large) {
-			pr_debug("Msg succeeded %x\n",
-				       msg->header.part.msg_id);
-			memcpy_fromio(str_info->data_blk.data,
-					((void *)(msg->mailbox)),
-					sizeof(struct snd_sst_decode_info));
-			str_info->data_blk.ret_code = 0;
-		} else {
-			pr_err("Msg %x reply error %x\n",
-				msg->header.part.msg_id, msg->header.part.data);
-			str_info->data_blk.ret_code = -msg->header.part.data;
-		}
-		if (str_info->data_blk.on == true) {
-			str_info->data_blk.on = false;
-			str_info->data_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	case IPC_IA_DRAIN_STREAM:
-		if (sst_validate_strid(str_id)) {
-			pr_err("stream id %d invalid\n", str_id);
-			break;
-		}
-		str_info = &sst_drv_ctx->streams[str_id];
-		if (!msg->header.part.data) {
-			pr_debug("Msg succeeded %x\n",
-					msg->header.part.msg_id);
-			str_info->ctrl_blk.ret_code = 0;
-
-		} else {
-			pr_err(" Msg %x reply error %x\n",
-				msg->header.part.msg_id, msg->header.part.data);
-			str_info->ctrl_blk.ret_code = -msg->header.part.data;
-
-		}
-		str_info = &sst_drv_ctx->streams[str_id];
-		if (str_info->data_blk.on == true) {
-			str_info->data_blk.on = false;
-			str_info->data_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-
-	case IPC_IA_DROP_STREAM:
-		if (sst_validate_strid(str_id)) {
-			pr_err("str id %d invalid\n", str_id);
-			break;
-		}
-		str_info = &sst_drv_ctx->streams[str_id];
-		if (msg->header.part.large) {
-			struct snd_sst_drop_response *drop_resp =
-				(struct snd_sst_drop_response *)msg->mailbox;
-
-			pr_debug("Drop ret bytes %x\n", drop_resp->bytes);
-
-			str_info->curr_bytes = drop_resp->bytes;
-			str_info->ctrl_blk.ret_code =  0;
-		} else {
-			pr_err(" Msg %x reply error %x\n",
-				msg->header.part.msg_id, msg->header.part.data);
-			str_info->ctrl_blk.ret_code = -msg->header.part.data;
-		}
-		if (str_info->ctrl_blk.on == true) {
-			str_info->ctrl_blk.on = false;
-			str_info->ctrl_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	case IPC_IA_ENABLE_RX_TIME_SLOT:
-		if (!msg->header.part.data) {
-			pr_debug("RX_TIME_SLOT success\n");
-			sst_drv_ctx->hs_info_blk.ret_code = 0;
-		} else {
-			pr_err(" Msg %x reply error %x\n",
-				msg->header.part.msg_id,
-				msg->header.part.data);
-			sst_drv_ctx->hs_info_blk.ret_code =
-				-msg->header.part.data;
-		}
-		if (sst_drv_ctx->hs_info_blk.on == true) {
-			sst_drv_ctx->hs_info_blk.on = false;
-			sst_drv_ctx->hs_info_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	case IPC_IA_PAUSE_STREAM:
-	case IPC_IA_RESUME_STREAM:
-	case IPC_IA_SET_STREAM_PARAMS:
-		str_info = &sst_drv_ctx->streams[str_id];
-		if (!msg->header.part.data) {
-			pr_debug("Msg succeeded %x\n",
-					msg->header.part.msg_id);
-			str_info->ctrl_blk.ret_code = 0;
-		} else {
-			pr_err(" Msg %x reply error %x\n",
-					msg->header.part.msg_id,
-					msg->header.part.data);
-			str_info->ctrl_blk.ret_code = -msg->header.part.data;
-		}
-		if (sst_validate_strid(str_id)) {
-			pr_err(" stream id %d invalid\n", str_id);
-			break;
-		}
-
-		if (str_info->ctrl_blk.on == true) {
-			str_info->ctrl_blk.on = false;
-			str_info->ctrl_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-
-	case IPC_IA_FREE_STREAM:
-		str_info = &sst_drv_ctx->streams[str_id];
-		if (!msg->header.part.data) {
-			pr_debug("Stream %d freed\n", str_id);
-		} else {
-			pr_err("Free for %d ret error %x\n",
-				       str_id, msg->header.part.data);
-		}
-		if (str_info->ctrl_blk.on == true) {
-			str_info->ctrl_blk.on = false;
-			str_info->ctrl_blk.condition = true;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		break;
-	case IPC_IA_ALLOC_STREAM: {
-		/* map to stream, call play */
-		struct snd_sst_alloc_response *resp =
-				(struct snd_sst_alloc_response *)msg->mailbox;
-		if (resp->str_type.result)
-			pr_err("error alloc stream = %x\n",
-				       resp->str_type.result);
-		sst_alloc_stream_response(str_id, resp);
-		break;
-	}
-
-	case IPC_IA_PLAY_FRAMES:
-	case IPC_IA_CAPT_FRAMES:
-		if (sst_validate_strid(str_id)) {
-			pr_err("stream id %d invalid\n", str_id);
-			break;
-		}
-		pr_debug("Ack for play/capt frames received\n");
-		break;
-
-	case IPC_IA_PREP_LIB_DNLD: {
-		struct snd_sst_str_type *str_type =
-			(struct snd_sst_str_type *)msg->mailbox;
-		pr_debug("Prep Lib download %x\n",
-				msg->header.part.msg_id);
-		if (str_type->result)
-			pr_err("Prep lib download %x\n", str_type->result);
-		else
-			pr_debug("Can download codec now...\n");
-		sst_wake_up_alloc_block(sst_drv_ctx, str_id,
-				str_type->result, NULL);
-		break;
-	}
-
-	case IPC_IA_LIB_DNLD_CMPLT: {
-		struct snd_sst_lib_download_info *resp =
-			(struct snd_sst_lib_download_info *)msg->mailbox;
-		int retval = resp->result;
-
-		pr_debug("Lib downloaded %x\n", msg->header.part.msg_id);
-		if (resp->result) {
-			pr_err("err in lib dload %x\n", resp->result);
-		} else {
-			pr_debug("Codec download complete...\n");
-			pr_debug("codec Type %d Ver %d Built %s: %s\n",
-				resp->dload_lib.lib_info.lib_type,
-				resp->dload_lib.lib_info.lib_version,
-				resp->dload_lib.lib_info.b_date,
-				resp->dload_lib.lib_info.b_time);
-		}
-		sst_wake_up_alloc_block(sst_drv_ctx, str_id,
-						retval, NULL);
-		break;
-	}
-
-	case IPC_IA_GET_FW_VERSION: {
-		struct ipc_header_fw_init *version =
-				(struct ipc_header_fw_init *)msg->mailbox;
-		int major = version->fw_version.major;
-		int minor = version->fw_version.minor;
-		int build = version->fw_version.build;
-		dev_info(&sst_drv_ctx->pci->dev,
-			"INFO: ***LOADED SST FW VERSION*** = %02d.%02d.%02d\n",
-		major, minor, build);
-		break;
-	}
-	case IPC_IA_GET_FW_BUILD_INF: {
-		struct sst_fw_build_info *build =
-			(struct sst_fw_build_info *)msg->mailbox;
-		pr_debug("Build date:%sTime:%s", build->date, build->time);
-		break;
-	}
-	case IPC_IA_SET_PMIC_TYPE:
-		break;
-	case IPC_IA_START_STREAM:
-		pr_debug("reply for START STREAM %x\n", msg->header.full);
-		break;
-
-	case IPC_IA_GET_FW_CTXT:
-		pr_debug("reply for get fw ctxt  %x\n", msg->header.full);
-		if (msg->header.part.data)
-			sst_drv_ctx->fw_cntx_size = 0;
-		else
-			sst_drv_ctx->fw_cntx_size = *sst_drv_ctx->fw_cntx;
-		pr_debug("fw copied data %x\n", sst_drv_ctx->fw_cntx_size);
-		sst_wake_up_alloc_block(
-			sst_drv_ctx, str_id, msg->header.part.data, NULL);
-		break;
-	default:
-		/* Illegal case */
-		pr_err("process reply:default = %x\n", msg->header.full);
-	}
-	sst_clear_interrupt();
-	return;
-}
diff --git a/drivers/staging/intel_sst/intel_sst_pvt.c b/drivers/staging/intel_sst/intel_sst_pvt.c
deleted file mode 100644
index e034bea56f14..000000000000
--- a/drivers/staging/intel_sst/intel_sst_pvt.c
+++ /dev/null
@@ -1,313 +0,0 @@
-/*
- *  intel_sst_pvt.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10	Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This driver exposes the audio engine functionalities to the ALSA
- *	and middleware.
- *
- *  This file contains all private functions
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/fs.h>
-#include <linux/firmware.h>
-#include <linux/sched.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-
-/*
- * sst_get_block_stream - get a new block stream
- *
- * @sst_drv_ctx: Driver context structure
- *
- * This function assigns a block for the calls that dont have stream context yet
- * the blocks are used for waiting on Firmware's response for any operation
- * Should be called with stream lock held
- */
-int sst_get_block_stream(struct intel_sst_drv *sst_drv_ctx)
-{
-	int i;
-
-	for (i = 0; i < MAX_ACTIVE_STREAM; i++) {
-		if (sst_drv_ctx->alloc_block[i].sst_id == BLOCK_UNINIT) {
-			sst_drv_ctx->alloc_block[i].ops_block.condition = false;
-			sst_drv_ctx->alloc_block[i].ops_block.ret_code = 0;
-			sst_drv_ctx->alloc_block[i].sst_id = 0;
-			break;
-		}
-	}
-	if (i == MAX_ACTIVE_STREAM) {
-		pr_err("max alloc_stream reached\n");
-		i = -EBUSY; /* active stream limit reached */
-	}
-	return i;
-}
-
-/*
- * sst_wait_interruptible - wait on event
- *
- * @sst_drv_ctx: Driver context
- * @block: Driver block to wait on
- *
- * This function waits without a timeout (and is interruptable) for a
- * given block event
- */
-int sst_wait_interruptible(struct intel_sst_drv *sst_drv_ctx,
-				struct sst_block *block)
-{
-	int retval = 0;
-
-	if (!wait_event_interruptible(sst_drv_ctx->wait_queue,
-				block->condition)) {
-		/* event wake */
-		if (block->ret_code < 0) {
-			pr_err("stream failed %d\n", block->ret_code);
-			retval = -EBUSY;
-		} else {
-			pr_debug("event up\n");
-			retval = 0;
-		}
-	} else {
-		pr_err("signal interrupted\n");
-		retval = -EINTR;
-	}
-	return retval;
-
-}
-
-
-/*
- * sst_wait_interruptible_timeout - wait on event interruptable
- *
- * @sst_drv_ctx: Driver context
- * @block: Driver block to wait on
- * @timeout: time for wait on
- *
- * This function waits with a timeout value (and is interruptible) on a
- * given block event
- */
-int sst_wait_interruptible_timeout(
-			struct intel_sst_drv *sst_drv_ctx,
-			struct sst_block *block, int timeout)
-{
-	int retval = 0;
-
-	pr_debug("sst_wait_interruptible_timeout - waiting....\n");
-	if (wait_event_interruptible_timeout(sst_drv_ctx->wait_queue,
-						block->condition,
-						msecs_to_jiffies(timeout))) {
-		if (block->ret_code < 0)
-			pr_err("stream failed %d\n", block->ret_code);
-		else
-			pr_debug("event up\n");
-		retval = block->ret_code;
-	} else {
-		block->on = false;
-		pr_err("timeout occurred...\n");
-		/*setting firmware state as uninit so that the
-		firmware will get re-downloaded on next request
-		this is because firmare not responding for 5 sec
-		is equalant to some unrecoverable error of FW
-		sst_drv_ctx->sst_state = SST_UN_INIT;*/
-		retval = -EBUSY;
-	}
-	return retval;
-
-}
-
-
-/*
- * sst_wait_timeout - wait on event for timeout
- *
- * @sst_drv_ctx: Driver context
- * @block: Driver block to wait on
- *
- * This function waits with a timeout value (and is not interruptible) on a
- * given block event
- */
-int sst_wait_timeout(struct intel_sst_drv *sst_drv_ctx,
-		struct stream_alloc_block *block)
-{
-	int retval = 0;
-
-	/* NOTE:
-	Observed that FW processes the alloc msg and replies even
-	before the alloc thread has finished execution */
-	pr_debug("waiting for %x, condition %x\n",
-		       block->sst_id, block->ops_block.condition);
-	if (wait_event_interruptible_timeout(sst_drv_ctx->wait_queue,
-				block->ops_block.condition,
-				msecs_to_jiffies(SST_BLOCK_TIMEOUT))) {
-		/* event wake */
-		pr_debug("Event wake %x\n", block->ops_block.condition);
-		pr_debug("message ret: %d\n", block->ops_block.ret_code);
-		retval = block->ops_block.ret_code;
-	} else {
-		block->ops_block.on = false;
-		pr_err("Wait timed-out %x\n", block->ops_block.condition);
-		/* settign firmware state as uninit so that the
-		firmware will get redownloaded on next request
-		this is because firmare not responding for 5 sec
-		is equalant to some unrecoverable error of FW
-		sst_drv_ctx->sst_state = SST_UN_INIT;*/
-		retval = -EBUSY;
-	}
-	return retval;
-
-}
-
-/*
- * sst_create_large_msg - create a large IPC message
- *
- * @arg: ipc message
- *
- * this function allocates structures to send a large message to the firmware
- */
-int sst_create_large_msg(struct ipc_post **arg)
-{
-	struct ipc_post *msg;
-
-	msg = kzalloc(sizeof(struct ipc_post), GFP_ATOMIC);
-	if (!msg) {
-		pr_err("kzalloc msg failed\n");
-		return -ENOMEM;
-	}
-
-	msg->mailbox_data = kzalloc(SST_MAILBOX_SIZE, GFP_ATOMIC);
-	if (!msg->mailbox_data) {
-		kfree(msg);
-		pr_err("kzalloc mailbox_data failed");
-		return -ENOMEM;
-	}
-	*arg = msg;
-	return 0;
-}
-
-/*
- * sst_create_short_msg - create a short IPC message
- *
- * @arg: ipc message
- *
- * this function allocates structures to send a short message to the firmware
- */
-int sst_create_short_msg(struct ipc_post **arg)
-{
-	struct ipc_post *msg;
-
-	msg = kzalloc(sizeof(*msg), GFP_ATOMIC);
-	if (!msg) {
-		pr_err("kzalloc msg failed\n");
-		return -ENOMEM;
-	}
-	msg->mailbox_data = NULL;
-	*arg = msg;
-	return 0;
-}
-
-/*
- * sst_clean_stream - clean the stream context
- *
- * @stream: stream structure
- *
- * this function resets the stream contexts
- * should be called in free
- */
-void sst_clean_stream(struct stream_info *stream)
-{
-	struct sst_stream_bufs *bufs = NULL, *_bufs;
-	stream->status = STREAM_UN_INIT;
-	stream->prev = STREAM_UN_INIT;
-	mutex_lock(&stream->lock);
-	list_for_each_entry_safe(bufs, _bufs, &stream->bufs, node) {
-		list_del(&bufs->node);
-		kfree(bufs);
-	}
-	mutex_unlock(&stream->lock);
-
-	if (stream->ops != STREAM_OPS_PLAYBACK_DRM)
-		kfree(stream->decode_ibuf);
-}
-
-/*
- * sst_wake_up_alloc_block - wake up waiting block
- *
- * @sst_drv_ctx: Driver context
- * @sst_id: stream id
- * @status: status of wakeup
- * @data: data pointer of wakeup
- *
- * This function wakes up a sleeping block event based on the response
- */
-void sst_wake_up_alloc_block(struct intel_sst_drv *sst_drv_ctx,
-		u8 sst_id, int status, void *data)
-{
-	int i;
-
-	/* Unblock with retval code */
-	for (i = 0; i < MAX_ACTIVE_STREAM; i++) {
-		if (sst_id == sst_drv_ctx->alloc_block[i].sst_id) {
-			sst_drv_ctx->alloc_block[i].ops_block.condition = true;
-			sst_drv_ctx->alloc_block[i].ops_block.ret_code = status;
-			sst_drv_ctx->alloc_block[i].ops_block.data = data;
-			wake_up(&sst_drv_ctx->wait_queue);
-			break;
-		}
-	}
-}
-
-/*
- * sst_enable_rx_timeslot - Send msg to query for stream parameters
- * @status: rx timeslot to be enabled
- *
- * This function is called when the RX timeslot is required to be enabled
- */
-int sst_enable_rx_timeslot(int status)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-
-	if (sst_create_short_msg(&msg)) {
-		pr_err("mem allocation failed\n");
-			return -ENOMEM;
-	}
-	pr_debug("ipc message sending: ENABLE_RX_TIME_SLOT\n");
-	sst_fill_header(&msg->header, IPC_IA_ENABLE_RX_TIME_SLOT, 0, 0);
-	msg->header.part.data = status;
-	sst_drv_ctx->hs_info_blk.condition = false;
-	sst_drv_ctx->hs_info_blk.ret_code = 0;
-	sst_drv_ctx->hs_info_blk.on = true;
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node,
-			&sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-				&sst_drv_ctx->hs_info_blk, SST_BLOCK_TIMEOUT);
-	return retval;
-}
-
diff --git a/drivers/staging/intel_sst/intel_sst_stream.c b/drivers/staging/intel_sst/intel_sst_stream.c
deleted file mode 100644
index be4565e74f8c..000000000000
--- a/drivers/staging/intel_sst/intel_sst_stream.c
+++ /dev/null
@@ -1,583 +0,0 @@
-/*
- *  intel_sst_stream.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This file contains the stream operations of SST driver
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/firmware.h>
-#include <linux/sched.h>
-#include <linux/delay.h>
-#include "intel_sst_ioctl.h"
-#include "intel_sst.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-
-/*
- * sst_check_device_type - Check the medfield device type
- *
- * @device: Device to be checked
- * @num_ch: Number of channels queried
- * @pcm_slot: slot to be enabled for this device
- *
- * This checks the deivce against the map and calculates pcm_slot value
- */
-int sst_check_device_type(u32 device, u32 num_chan, u32 *pcm_slot)
-{
-	if (device >= MAX_NUM_STREAMS_MFLD) {
-		pr_debug("device type invalid %d\n", device);
-		return -EINVAL;
-	}
-	if (sst_drv_ctx->streams[device].status == STREAM_UN_INIT) {
-		if (device == SND_SST_DEVICE_VIBRA && num_chan == 1)
-			*pcm_slot = 0x10;
-		else if (device == SND_SST_DEVICE_HAPTIC && num_chan == 1)
-			*pcm_slot = 0x20;
-		else if (device == SND_SST_DEVICE_IHF && num_chan == 1)
-			*pcm_slot = 0x04;
-		else if (device == SND_SST_DEVICE_IHF && num_chan == 2)
-			*pcm_slot = 0x0C;
-		else if (device == SND_SST_DEVICE_HEADSET && num_chan == 1)
-			*pcm_slot = 0x01;
-		else if (device == SND_SST_DEVICE_HEADSET && num_chan == 2)
-			*pcm_slot = 0x03;
-		else if (device == SND_SST_DEVICE_CAPTURE && num_chan == 1)
-			*pcm_slot = 0x01;
-		else if (device == SND_SST_DEVICE_CAPTURE && num_chan == 2)
-			*pcm_slot = 0x03;
-		else if (device == SND_SST_DEVICE_CAPTURE && num_chan == 3)
-			*pcm_slot = 0x07;
-		else if (device == SND_SST_DEVICE_CAPTURE && num_chan == 4)
-			*pcm_slot = 0x0F;
-		else if (device == SND_SST_DEVICE_CAPTURE && num_chan > 4)
-			*pcm_slot = 0x1F;
-		else {
-			pr_debug("No condition satisfied.. ret err\n");
-			return -EINVAL;
-		}
-	} else {
-		pr_debug("this stream state is not uni-init, is %d\n",
-				sst_drv_ctx->streams[device].status);
-		return -EBADRQC;
-	}
-	pr_debug("returning slot %x\n", *pcm_slot);
-	return 0;
-}
-/**
- * get_mrst_stream_id	-	gets a new stream id for use
- *
- * This functions searches the current streams and allocated an empty stream
- * lock stream_lock required to be held before calling this
- */
-static unsigned int get_mrst_stream_id(void)
-{
-	int i;
-
-	for (i = 1; i <= MAX_NUM_STREAMS_MRST; i++) {
-		if (sst_drv_ctx->streams[i].status == STREAM_UN_INIT)
-			return i;
-	}
-	pr_debug("Didn't find empty stream for mrst\n");
-	return -EBUSY;
-}
-
-/**
- * sst_alloc_stream - Send msg for a new stream ID
- *
- * @params:	stream params
- * @stream_ops:	operation of stream PB/capture
- * @codec:	codec for stream
- * @device:	device stream to be allocated for
- *
- * This function is called by any function which wants to start
- * a new stream. This also check if a stream exists which is idle
- * it initializes idle stream id to this request
- */
-int sst_alloc_stream(char *params, unsigned int stream_ops,
-	       u8 codec, unsigned int device)
-{
-	struct ipc_post *msg = NULL;
-	struct snd_sst_alloc_params alloc_param;
-	unsigned int pcm_slot = 0, num_ch;
-	int str_id;
-	struct snd_sst_stream_params *sparams;
-	struct stream_info *str_info;
-
-	pr_debug("SST DBG:entering sst_alloc_stream\n");
-	pr_debug("SST DBG:%d %d %d\n", stream_ops, codec, device);
-
-	BUG_ON(!params);
-	sparams = (struct snd_sst_stream_params *)params;
-	num_ch = sparams->uc.pcm_params.num_chan;
-	/*check the device type*/
-	if (sst_drv_ctx->pci_id == SST_MFLD_PCI_ID) {
-		if (sst_check_device_type(device, num_ch, &pcm_slot))
-			return -EINVAL;
-		mutex_lock(&sst_drv_ctx->stream_lock);
-		str_id = device;
-		mutex_unlock(&sst_drv_ctx->stream_lock);
-		pr_debug("SST_DBG: slot %x\n", pcm_slot);
-	} else {
-		mutex_lock(&sst_drv_ctx->stream_lock);
-		str_id = get_mrst_stream_id();
-		mutex_unlock(&sst_drv_ctx->stream_lock);
-		if (str_id <= 0)
-			return -EBUSY;
-	}
-	/*allocate device type context*/
-	sst_init_stream(&sst_drv_ctx->streams[str_id], codec,
-			str_id, stream_ops, pcm_slot, device);
-	/* send msg to FW to allocate a stream */
-	if (sst_create_large_msg(&msg))
-		return -ENOMEM;
-
-	sst_fill_header(&msg->header, IPC_IA_ALLOC_STREAM, 1, str_id);
-	msg->header.part.data = sizeof(alloc_param) + sizeof(u32);
-	alloc_param.str_type.codec_type = codec;
-	alloc_param.str_type.str_type = SST_STREAM_TYPE_MUSIC;
-	alloc_param.str_type.operation = stream_ops;
-	alloc_param.str_type.protected_str = 0; /* non drm */
-	alloc_param.str_type.time_slots = pcm_slot;
-	alloc_param.str_type.result = alloc_param.str_type.reserved = 0;
-	memcpy(&alloc_param.stream_params, params,
-			sizeof(struct snd_sst_stream_params));
-
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), &alloc_param,
-			sizeof(alloc_param));
-	str_info = &sst_drv_ctx->streams[str_id];
-	str_info->ctrl_blk.condition = false;
-	str_info->ctrl_blk.ret_code = 0;
-	str_info->ctrl_blk.on = true;
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	pr_debug("SST DBG:alloc stream done\n");
-	return str_id;
-}
-
-
-/*
- * sst_alloc_stream_response - process alloc reply
- *
- * @str_id:	stream id for which the stream has been allocated
- * @resp		the stream response from firware
- *
- * This function is called by firmware as a response to stream allcoation
- * request
- */
-int sst_alloc_stream_response(unsigned int str_id,
-				struct snd_sst_alloc_response *resp)
-{
-	int retval = 0;
-	struct stream_info *str_info;
-	struct snd_sst_lib_download *lib_dnld;
-
-	pr_debug("SST DEBUG: stream number given = %d\n", str_id);
-	str_info = &sst_drv_ctx->streams[str_id];
-	if (resp->str_type.result == SST_LIB_ERR_LIB_DNLD_REQUIRED) {
-		lib_dnld = kzalloc(sizeof(*lib_dnld), GFP_KERNEL);
-		memcpy(lib_dnld, &resp->lib_dnld, sizeof(*lib_dnld));
-	} else
-		lib_dnld = NULL;
-	if (str_info->ctrl_blk.on == true) {
-		str_info->ctrl_blk.on = false;
-		str_info->ctrl_blk.data = lib_dnld;
-		str_info->ctrl_blk.condition = true;
-		str_info->ctrl_blk.ret_code = resp->str_type.result;
-		pr_debug("SST DEBUG: sst_alloc_stream_response: waking up.\n");
-		wake_up(&sst_drv_ctx->wait_queue);
-	}
-	return retval;
-}
-
-
-/**
-* sst_get_fw_info - Send msg to query for firmware configurations
-* @info: out param that holds the firmare configurations
-*
-* This function is called when the firmware configurations are queiried for
-*/
-int sst_get_fw_info(struct snd_sst_fw_info *info)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-
-	pr_debug("SST DBG:sst_get_fw_info called\n");
-
-	if (sst_create_short_msg(&msg)) {
-		pr_err("SST ERR: message creation failed\n");
-		return -ENOMEM;
-	}
-
-	sst_fill_header(&msg->header, IPC_IA_GET_FW_INFO, 0, 0);
-	sst_drv_ctx->fw_info_blk.condition = false;
-	sst_drv_ctx->fw_info_blk.ret_code = 0;
-	sst_drv_ctx->fw_info_blk.on = true;
-	sst_drv_ctx->fw_info_blk.data = info;
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-			&sst_drv_ctx->fw_info_blk, SST_BLOCK_TIMEOUT);
-	if (retval) {
-		pr_err("SST ERR: error in fw_info = %d\n", retval);
-		retval = -EIO;
-	}
-	return retval;
-}
-
-
-/**
-* sst_pause_stream - Send msg for a pausing stream
-* @str_id:	 stream ID
-*
-* This function is called by any function which wants to pause
-* an already running stream.
-*/
-int sst_start_stream(int str_id)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct stream_info *str_info;
-
-	pr_debug("sst_start_stream for %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-	str_info = &sst_drv_ctx->streams[str_id];
-	if (str_info->status != STREAM_INIT)
-		return -EBADRQC;
-	if (sst_create_short_msg(&msg))
-		return -ENOMEM;
-
-	sst_fill_header(&msg->header, IPC_IA_START_STREAM, 0, str_id);
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	return retval;
-}
-
-/*
- * sst_pause_stream - Send msg for a pausing stream
- * @str_id:	 stream ID
- *
- * This function is called by any function which wants to pause
- * an already running stream.
- */
-int sst_pause_stream(int str_id)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct stream_info *str_info;
-
-	pr_debug("SST DBG:sst_pause_stream for %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-	str_info = &sst_drv_ctx->streams[str_id];
-	if (str_info->status == STREAM_PAUSED)
-		return 0;
-	if (str_info->status == STREAM_RUNNING ||
-		str_info->status == STREAM_INIT) {
-		if (str_info->prev == STREAM_UN_INIT)
-			return -EBADRQC;
-		if (str_info->ctrl_blk.on == true) {
-			pr_err("SST ERR: control path is in use\n");
-			return -EINVAL;
-		}
-		if (sst_create_short_msg(&msg))
-			return -ENOMEM;
-
-		sst_fill_header(&msg->header, IPC_IA_PAUSE_STREAM, 0, str_id);
-		str_info->ctrl_blk.condition = false;
-		str_info->ctrl_blk.ret_code = 0;
-		str_info->ctrl_blk.on = true;
-		spin_lock(&sst_drv_ctx->list_spin_lock);
-		list_add_tail(&msg->node,
-				&sst_drv_ctx->ipc_dispatch_list);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-		retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-				&str_info->ctrl_blk, SST_BLOCK_TIMEOUT);
-		if (retval == 0) {
-			str_info->prev = str_info->status;
-			str_info->status = STREAM_PAUSED;
-		} else if (retval == SST_ERR_INVALID_STREAM_ID) {
-			retval = -EINVAL;
-			mutex_lock(&sst_drv_ctx->stream_lock);
-			sst_clean_stream(str_info);
-			mutex_unlock(&sst_drv_ctx->stream_lock);
-		}
-	} else {
-		retval = -EBADRQC;
-		pr_err("SST ERR: BADQRC for stream\n");
-	}
-
-	return retval;
-}
-
-/**
- * sst_resume_stream - Send msg for resuming stream
- * @str_id:		stream ID
- *
- * This function is called by any function which wants to resume
- * an already paused stream.
- */
-int sst_resume_stream(int str_id)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct stream_info *str_info;
-
-	pr_debug("SST DBG:sst_resume_stream for %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-	str_info = &sst_drv_ctx->streams[str_id];
-	if (str_info->status == STREAM_RUNNING)
-			return 0;
-	if (str_info->status == STREAM_PAUSED) {
-		if (str_info->ctrl_blk.on == true) {
-			pr_err("SST ERR: control path in use\n");
-			return -EINVAL;
-		}
-		if (sst_create_short_msg(&msg)) {
-			pr_err("SST ERR: mem allocation failed\n");
-			return -ENOMEM;
-		}
-		sst_fill_header(&msg->header, IPC_IA_RESUME_STREAM, 0, str_id);
-		str_info->ctrl_blk.condition = false;
-		str_info->ctrl_blk.ret_code = 0;
-		str_info->ctrl_blk.on = true;
-		spin_lock(&sst_drv_ctx->list_spin_lock);
-		list_add_tail(&msg->node,
-				&sst_drv_ctx->ipc_dispatch_list);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-		retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-				&str_info->ctrl_blk, SST_BLOCK_TIMEOUT);
-		if (!retval) {
-			if (str_info->prev == STREAM_RUNNING)
-				str_info->status = STREAM_RUNNING;
-			else
-				str_info->status = STREAM_INIT;
-			str_info->prev = STREAM_PAUSED;
-		} else if (retval == -SST_ERR_INVALID_STREAM_ID) {
-			retval = -EINVAL;
-			mutex_lock(&sst_drv_ctx->stream_lock);
-			sst_clean_stream(str_info);
-			mutex_unlock(&sst_drv_ctx->stream_lock);
-		}
-	} else {
-		retval = -EBADRQC;
-		pr_err("SST ERR: BADQRC for stream\n");
-	}
-
-	return retval;
-}
-
-
-/**
- * sst_drop_stream - Send msg for stopping stream
- * @str_id:		stream ID
- *
- * This function is called by any function which wants to stop
- * a stream.
- */
-int sst_drop_stream(int str_id)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct sst_stream_bufs *bufs = NULL, *_bufs;
-	struct stream_info *str_info;
-
-	pr_debug("SST DBG:sst_drop_stream for %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-	str_info = &sst_drv_ctx->streams[str_id];
-
-	if (str_info->status != STREAM_UN_INIT &&
-		str_info->status != STREAM_DECODE) {
-		if (str_info->ctrl_blk.on == true) {
-			pr_err("SST ERR: control path in use\n");
-			return -EINVAL;
-		}
-		if (sst_create_short_msg(&msg)) {
-			pr_err("SST ERR: mem allocation failed\n");
-			return -ENOMEM;
-		}
-		sst_fill_header(&msg->header, IPC_IA_DROP_STREAM, 0, str_id);
-		str_info->ctrl_blk.condition = false;
-		str_info->ctrl_blk.ret_code = 0;
-		str_info->ctrl_blk.on = true;
-		spin_lock(&sst_drv_ctx->list_spin_lock);
-		list_add_tail(&msg->node,
-				&sst_drv_ctx->ipc_dispatch_list);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-		retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-				&str_info->ctrl_blk, SST_BLOCK_TIMEOUT);
-		if (!retval) {
-			pr_debug("SST DBG:drop success\n");
-			str_info->prev = STREAM_UN_INIT;
-			str_info->status = STREAM_INIT;
-			if (str_info->src != MAD_DRV) {
-				mutex_lock(&str_info->lock);
-				list_for_each_entry_safe(bufs, _bufs,
-							&str_info->bufs, node) {
-					list_del(&bufs->node);
-					kfree(bufs);
-				}
-				mutex_unlock(&str_info->lock);
-			}
-			str_info->cumm_bytes += str_info->curr_bytes;
-		} else if (retval == -SST_ERR_INVALID_STREAM_ID) {
-			retval = -EINVAL;
-			mutex_lock(&sst_drv_ctx->stream_lock);
-			sst_clean_stream(str_info);
-			mutex_unlock(&sst_drv_ctx->stream_lock);
-		}
-		if (str_info->data_blk.on == true) {
-			str_info->data_blk.condition = true;
-			str_info->data_blk.ret_code = retval;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-	} else {
-		retval = -EBADRQC;
-		pr_err("SST ERR: BADQRC for stream\n");
-	}
-	return retval;
-}
-
-/**
-* sst_drain_stream - Send msg for draining stream
-* @str_id:		stream ID
-*
-* This function is called by any function which wants to drain
-* a stream.
-*/
-int sst_drain_stream(int str_id)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct stream_info *str_info;
-
-	pr_debug("SST DBG:sst_drain_stream for %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-	str_info = &sst_drv_ctx->streams[str_id];
-
-	if (str_info->status != STREAM_RUNNING &&
-		str_info->status != STREAM_INIT &&
-		str_info->status != STREAM_PAUSED) {
-			pr_err("SST ERR: BADQRC for stream = %d\n",
-				       str_info->status);
-			return -EBADRQC;
-	}
-
-	if (str_info->status == STREAM_INIT) {
-		if (sst_create_short_msg(&msg)) {
-			pr_err("SST ERR: mem allocation failed\n");
-			return -ENOMEM;
-		}
-		sst_fill_header(&msg->header, IPC_IA_DRAIN_STREAM, 0, str_id);
-		spin_lock(&sst_drv_ctx->list_spin_lock);
-		list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	} else
-		str_info->need_draining = true;
-	str_info->data_blk.condition = false;
-	str_info->data_blk.ret_code = 0;
-	str_info->data_blk.on = true;
-	retval = sst_wait_interruptible(sst_drv_ctx, &str_info->data_blk);
-	str_info->need_draining = false;
-	return retval;
-}
-
-/**
- * sst_free_stream - Frees a stream
- * @str_id:		stream ID
- *
- * This function is called by any function which wants to free
- * a stream.
- */
-int sst_free_stream(int str_id)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct stream_info *str_info;
-
-	pr_debug("SST DBG:sst_free_stream for %d\n", str_id);
-
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-	str_info = &sst_drv_ctx->streams[str_id];
-
-	if (str_info->status != STREAM_UN_INIT) {
-		if (sst_create_short_msg(&msg)) {
-			pr_err("SST ERR: mem allocation failed\n");
-			return -ENOMEM;
-		}
-		sst_fill_header(&msg->header, IPC_IA_FREE_STREAM, 0, str_id);
-		spin_lock(&sst_drv_ctx->list_spin_lock);
-		list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-		str_info->prev =  str_info->status;
-		str_info->status = STREAM_UN_INIT;
-		if (str_info->data_blk.on == true) {
-			str_info->data_blk.condition = true;
-			str_info->data_blk.ret_code = 0;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		str_info->data_blk.on = true;
-		str_info->data_blk.condition = false;
-		retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-				&str_info->ctrl_blk, SST_BLOCK_TIMEOUT);
-		pr_debug("wait for free returned %d\n", retval);
-		msleep(100);
-		mutex_lock(&sst_drv_ctx->stream_lock);
-		sst_clean_stream(str_info);
-		mutex_unlock(&sst_drv_ctx->stream_lock);
-		pr_debug("SST DBG:Stream freed\n");
-	} else {
-		retval = -EBADRQC;
-		pr_debug("SST DBG:BADQRC for stream\n");
-	}
-
-	return retval;
-}
-
-
diff --git a/drivers/staging/intel_sst/intel_sst_stream_encoded.c b/drivers/staging/intel_sst/intel_sst_stream_encoded.c
deleted file mode 100644
index 2be58c5cba02..000000000000
--- a/drivers/staging/intel_sst/intel_sst_stream_encoded.c
+++ /dev/null
@@ -1,1273 +0,0 @@
-/*
- *  intel_sst_stream.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This file contains the stream operations of SST driver
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/syscalls.h>
-#include <linux/firmware.h>
-#include <linux/sched.h>
-#ifdef CONFIG_MRST_RAR_HANDLER
-#include <linux/rar_register.h>
-#include "../memrar/memrar.h"
-#endif
-#include "intel_sst_ioctl.h"
-#include "intel_sst.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-/**
-* sst_get_stream_params - Send msg to query for stream parameters
-* @str_id:	stream id for which the parameters are queried for
-* @get_params:	out parameters to which the parameters are copied to
-*
-* This function is called when the stream parameters are queiried for
-*/
-int sst_get_stream_params(int str_id,
-			struct snd_sst_get_stream_params *get_params)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct stream_info *str_info;
-	struct snd_sst_fw_get_stream_params *fw_params;
-
-	pr_debug("get_stream for %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-
-	str_info = &sst_drv_ctx->streams[str_id];
-	if (str_info->status != STREAM_UN_INIT) {
-		if (str_info->ctrl_blk.on == true) {
-			pr_err("control path in use\n");
-			return -EINVAL;
-		}
-		if (sst_create_short_msg(&msg)) {
-			pr_err("message creation failed\n");
-			return -ENOMEM;
-		}
-		fw_params = kzalloc(sizeof(*fw_params), GFP_ATOMIC);
-		if (!fw_params) {
-			pr_err("mem allocation failed\n");
-			kfree(msg);
-			return -ENOMEM;
-		}
-
-		sst_fill_header(&msg->header, IPC_IA_GET_STREAM_PARAMS,
-					0, str_id);
-		str_info->ctrl_blk.condition = false;
-		str_info->ctrl_blk.ret_code = 0;
-		str_info->ctrl_blk.on = true;
-		str_info->ctrl_blk.data = (void *) fw_params;
-		spin_lock(&sst_drv_ctx->list_spin_lock);
-		list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-		retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-				&str_info->ctrl_blk, SST_BLOCK_TIMEOUT);
-		if (retval) {
-			get_params->codec_params.result = retval;
-			kfree(fw_params);
-			return -EIO;
-		}
-		memcpy(&get_params->pcm_params, &fw_params->pcm_params,
-				sizeof(fw_params->pcm_params));
-		memcpy(&get_params->codec_params.sparams,
-				&fw_params->codec_params,
-				sizeof(fw_params->codec_params));
-		get_params->codec_params.result = 0;
-		get_params->codec_params.stream_id = str_id;
-		get_params->codec_params.codec = str_info->codec;
-		get_params->codec_params.ops = str_info->ops;
-		get_params->codec_params.stream_type = str_info->str_type;
-		kfree(fw_params);
-	} else {
-		pr_debug("Stream is not in the init state\n");
-	}
-	return retval;
-}
-
-/**
- * sst_set_stream_param - Send msg for setting stream parameters
- *
- * @str_id: stream id
- * @str_param: stream params
- *
- * This function sets stream params during runtime
- */
-int sst_set_stream_param(int str_id, struct snd_sst_params *str_param)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct stream_info *str_info;
-
-	BUG_ON(!str_param);
-	if (sst_drv_ctx->streams[str_id].ops != str_param->ops) {
-		pr_err("Invalid operation\n");
-		return -EINVAL;
-	}
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-	pr_debug("set_stream for %d\n", str_id);
-	str_info =  &sst_drv_ctx->streams[str_id];
-	if (sst_drv_ctx->streams[str_id].status == STREAM_INIT) {
-		if (str_info->ctrl_blk.on == true) {
-			pr_err("control path in use\n");
-			return -EAGAIN;
-		}
-		if (sst_create_large_msg(&msg))
-			return -ENOMEM;
-
-		sst_fill_header(&msg->header,
-				IPC_IA_SET_STREAM_PARAMS, 1, str_id);
-		str_info->ctrl_blk.condition = false;
-		str_info->ctrl_blk.ret_code = 0;
-		str_info->ctrl_blk.on = true;
-		msg->header.part.data = sizeof(u32) +
-				sizeof(str_param->sparams);
-		memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-		memcpy(msg->mailbox_data + sizeof(u32), &str_param->sparams,
-				sizeof(str_param->sparams));
-		spin_lock(&sst_drv_ctx->list_spin_lock);
-		list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-		spin_unlock(&sst_drv_ctx->list_spin_lock);
-		sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-		retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-				&str_info->ctrl_blk, SST_BLOCK_TIMEOUT);
-		if (retval < 0) {
-			retval = -EIO;
-			sst_clean_stream(str_info);
-		}
-	} else {
-		retval = -EBADRQC;
-		pr_err("BADQRC for stream\n");
-	}
-	return retval;
-}
-
-/**
-* sst_get_vol - This function allows to get the premix gain or gain of a stream
-*
-* @get_vol: this is an output param through which the volume
-*	structure is passed back to user
-*
-* This function is called when the premix gain or stream gain is queried for
-*/
-int sst_get_vol(struct snd_sst_vol *get_vol)
-{
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-	struct snd_sst_vol *fw_get_vol;
-	int str_id = get_vol->stream_id;
-
-	pr_debug("get vol called\n");
-
-	if (sst_create_short_msg(&msg))
-		return -ENOMEM;
-
-	sst_fill_header(&msg->header,
-				IPC_IA_GET_STREAM_VOL, 0, str_id);
-	sst_drv_ctx->vol_info_blk.condition = false;
-	sst_drv_ctx->vol_info_blk.ret_code = 0;
-	sst_drv_ctx->vol_info_blk.on = true;
-	fw_get_vol = kzalloc(sizeof(*fw_get_vol), GFP_ATOMIC);
-	if (!fw_get_vol) {
-		pr_err("mem allocation failed\n");
-		kfree(msg);
-		return -ENOMEM;
-	}
-	sst_drv_ctx->vol_info_blk.data = (void *)fw_get_vol;
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-			&sst_drv_ctx->vol_info_blk, SST_BLOCK_TIMEOUT);
-	if (retval)
-		retval = -EIO;
-	else {
-		pr_debug("stream id %d\n", fw_get_vol->stream_id);
-		pr_debug("volume %d\n", fw_get_vol->volume);
-		pr_debug("ramp duration %d\n", fw_get_vol->ramp_duration);
-		pr_debug("ramp_type %d\n", fw_get_vol->ramp_type);
-		memcpy(get_vol, fw_get_vol, sizeof(*fw_get_vol));
-	}
-	return retval;
-}
-
-/**
-* sst_set_vol - This function allows to set the premix gain or gain of a stream
-*
-* @set_vol:	this holds the volume structure that needs to be set
-*
-* This function is called when premix gain or stream gain is requested to be set
-*/
-int sst_set_vol(struct snd_sst_vol *set_vol)
-{
-
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-
-	pr_debug("set vol called\n");
-
-	if (sst_create_large_msg(&msg)) {
-		pr_err("message creation failed\n");
-		return -ENOMEM;
-	}
-	sst_fill_header(&msg->header, IPC_IA_SET_STREAM_VOL, 1,
-				set_vol->stream_id);
-
-	msg->header.part.data = sizeof(u32) + sizeof(*set_vol);
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), set_vol, sizeof(*set_vol));
-	sst_drv_ctx->vol_info_blk.condition = false;
-	sst_drv_ctx->vol_info_blk.ret_code = 0;
-	sst_drv_ctx->vol_info_blk.on = true;
-	sst_drv_ctx->vol_info_blk.data = set_vol;
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-			&sst_drv_ctx->vol_info_blk, SST_BLOCK_TIMEOUT);
-	if (retval) {
-		pr_err("error in set_vol = %d\n", retval);
-		retval = -EIO;
-	}
-	return retval;
-}
-
-/**
-* sst_set_mute - This function sets premix mute or soft mute of a stream
-*
-* @set_mute:	this holds the mute structure that needs to be set
-*
-* This function is called when premix mute or stream mute requested to be set
-*/
-int sst_set_mute(struct snd_sst_mute *set_mute)
-{
-
-	int retval = 0;
-	struct ipc_post *msg = NULL;
-
-	pr_debug("set mute called\n");
-
-	if (sst_create_large_msg(&msg)) {
-		pr_err("message creation failed\n");
-		return -ENOMEM;
-	}
-	sst_fill_header(&msg->header, IPC_IA_SET_STREAM_MUTE, 1,
-				set_mute->stream_id);
-	sst_drv_ctx->mute_info_blk.condition = false;
-	sst_drv_ctx->mute_info_blk.ret_code = 0;
-	sst_drv_ctx->mute_info_blk.on = true;
-	sst_drv_ctx->mute_info_blk.data = set_mute;
-
-	msg->header.part.data = sizeof(u32) + sizeof(*set_mute);
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), set_mute,
-			sizeof(*set_mute));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-			&sst_drv_ctx->mute_info_blk, SST_BLOCK_TIMEOUT);
-	if (retval) {
-		pr_err("error in set_mute = %d\n", retval);
-		retval = -EIO;
-	}
-	return retval;
-}
-
-int sst_prepare_target(struct snd_sst_slot_info *slot)
-{
-	if (slot->target_device == SND_SST_TARGET_PMIC
-		&& slot->device_instance == 1) {
-			/*music mode*/
-			if (sst_drv_ctx->pmic_port_instance == 0)
-				sst_drv_ctx->scard_ops->set_voice_port(
-					DEACTIVATE);
-	} else if ((slot->target_device == SND_SST_TARGET_PMIC ||
-			slot->target_device == SND_SST_TARGET_MODEM) &&
-			slot->device_instance == 0) {
-				/*voip mode where pcm0 is active*/
-				if (sst_drv_ctx->pmic_port_instance == 1)
-					sst_drv_ctx->scard_ops->set_audio_port(
-						DEACTIVATE);
-	}
-	return 0;
-}
-
-int sst_activate_target(struct snd_sst_slot_info *slot)
-{
-	if (slot->target_device == SND_SST_TARGET_PMIC &&
-		slot->device_instance == 1) {
-			/*music mode*/
-			sst_drv_ctx->pmic_port_instance = 1;
-			sst_drv_ctx->scard_ops->set_audio_port(ACTIVATE);
-			sst_drv_ctx->scard_ops->set_pcm_audio_params(
-				slot->pcm_params.sfreq,
-				slot->pcm_params.pcm_wd_sz,
-				slot->pcm_params.num_chan);
-			if (sst_drv_ctx->pb_streams)
-				sst_drv_ctx->scard_ops->power_up_pmic_pb(1);
-			if (sst_drv_ctx->cp_streams)
-				sst_drv_ctx->scard_ops->power_up_pmic_cp(1);
-	} else if ((slot->target_device == SND_SST_TARGET_PMIC ||
-			slot->target_device == SND_SST_TARGET_MODEM) &&
-			slot->device_instance == 0) {
-				/*voip mode where pcm0 is active*/
-				sst_drv_ctx->pmic_port_instance = 0;
-				sst_drv_ctx->scard_ops->set_voice_port(
-					ACTIVATE);
-				sst_drv_ctx->scard_ops->power_up_pmic_pb(0);
-				/*sst_drv_ctx->scard_ops->power_up_pmic_cp(0);*/
-	}
-	return 0;
-}
-
-int sst_parse_target(struct snd_sst_slot_info *slot)
-{
-	int retval = 0;
-
-	if (slot->action == SND_SST_PORT_ACTIVATE &&
-		slot->device_type == SND_SST_DEVICE_PCM) {
-			retval = sst_activate_target(slot);
-			if (retval)
-				pr_err("SST_Activate_target_fail\n");
-			else
-				pr_err("SST_Activate_target_pass\n");
-	} else if (slot->action == SND_SST_PORT_PREPARE &&
-			slot->device_type == SND_SST_DEVICE_PCM) {
-				retval = sst_prepare_target(slot);
-			if (retval)
-				pr_err("SST_prepare_target_fail\n");
-			else
-				pr_err("SST_prepare_target_pass\n");
-	} else {
-		pr_err("slot_action : %d, device_type: %d\n",
-				slot->action, slot->device_type);
-	}
-	return retval;
-}
-
-int sst_send_target(struct snd_sst_target_device *target)
-{
-	int retval;
-	struct ipc_post *msg;
-
-	if (sst_create_large_msg(&msg)) {
-		pr_err("message creation failed\n");
-		return -ENOMEM;
-	}
-	sst_fill_header(&msg->header, IPC_IA_TARGET_DEV_SELECT, 1, 0);
-	sst_drv_ctx->tgt_dev_blk.condition = false;
-	sst_drv_ctx->tgt_dev_blk.ret_code = 0;
-	sst_drv_ctx->tgt_dev_blk.on = true;
-
-	msg->header.part.data = sizeof(u32) + sizeof(*target);
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), target,
-			sizeof(*target));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	pr_debug("message sent- waiting\n");
-	retval = sst_wait_interruptible_timeout(sst_drv_ctx,
-			&sst_drv_ctx->tgt_dev_blk, TARGET_DEV_BLOCK_TIMEOUT);
-	if (retval)
-		pr_err("target device ipc failed = 0x%x\n", retval);
-	return retval;
-
-}
-
-int sst_target_device_validate(struct snd_sst_target_device *target)
-{
-	int retval = 0;
-       int i;
-
-	for (i = 0; i < SST_MAX_TARGET_DEVICES; i++) {
-		if (target->devices[i].device_type == SND_SST_DEVICE_PCM) {
-			/*pcm device, check params*/
-			if (target->devices[i].device_instance == 1) {
-				if ((target->devices[i].device_mode !=
-				SND_SST_DEV_MODE_PCM_MODE4_I2S) &&
-				(target->devices[i].device_mode !=
-				SND_SST_DEV_MODE_PCM_MODE4_RIGHT_JUSTIFIED)
-				&& (target->devices[i].device_mode !=
-				SND_SST_DEV_MODE_PCM_MODE1))
-					goto err;
-			} else if (target->devices[i].device_instance == 0) {
-				if ((target->devices[i].device_mode !=
-						SND_SST_DEV_MODE_PCM_MODE2)
-					&& (target->devices[i].device_mode !=
-						SND_SST_DEV_MODE_PCM_MODE4_I2S)
-					&& (target->devices[i].device_mode !=
-						SND_SST_DEV_MODE_PCM_MODE1))
-					goto err;
-				if (target->devices[i].pcm_params.sfreq != 8000
-				|| target->devices[i].pcm_params.num_chan != 1
-				|| target->devices[i].pcm_params.pcm_wd_sz !=
-									16)
-					goto err;
-			} else {
-err:
-				pr_err("i/p params incorrect\n");
-				return -EINVAL;
-			}
-		}
-	}
-    return retval;
-}
-
-/**
- * sst_target_device_select - This function sets the target device configurations
- *
- * @target: this parameter holds the configurations to be set
- *
- * This function is called when the user layer wants to change the target
- * device's configurations
- */
-
-int sst_target_device_select(struct snd_sst_target_device *target)
-{
-	int retval, i, prepare_count = 0;
-
-	pr_debug("Target Device Select\n");
-
-	if (target->device_route < 0 || target->device_route > 2) {
-		pr_err("device route is invalid\n");
-		return -EINVAL;
-	}
-
-	if (target->device_route != 0) {
-		pr_err("Unsupported config\n");
-		return -EIO;
-	}
-	retval = sst_target_device_validate(target);
-	if (retval)
-		return retval;
-
-	retval = sst_send_target(target);
-	if (retval)
-		return retval;
-	for (i = 0; i < SST_MAX_TARGET_DEVICES; i++) {
-		if (target->devices[i].action == SND_SST_PORT_ACTIVATE) {
-			pr_debug("activate called in %d\n", i);
-			retval = sst_parse_target(&target->devices[i]);
-			if (retval)
-				return retval;
-		} else if (target->devices[i].action == SND_SST_PORT_PREPARE) {
-			pr_debug("PREPARE in %d, Forwarding\n", i);
-			retval = sst_parse_target(&target->devices[i]);
-			if (retval) {
-				pr_err("Parse Target fail %d\n", retval);
-				return retval;
-			}
-			pr_debug("Parse Target successful %d\n", retval);
-			if (target->devices[i].device_type ==
-						SND_SST_DEVICE_PCM)
-				prepare_count++;
-		}
-	}
-	if (target->devices[0].action == SND_SST_PORT_PREPARE &&
-		prepare_count == 0)
-			sst_drv_ctx->scard_ops->power_down_pmic();
-
-	return retval;
-}
-#ifdef CONFIG_MRST_RAR_HANDLER
-/*This function gets the physical address of the secure memory from the handle*/
-static inline int sst_get_RAR(struct RAR_buffer *buffers, int count)
-{
-	int retval = 0, rar_status = 0;
-
-	rar_status = rar_handle_to_bus(buffers, count);
-
-	if (count != rar_status) {
-		pr_err("The rar CALL Failed");
-		retval = -EIO;
-	}
-	if (buffers->info.type != RAR_TYPE_AUDIO) {
-		pr_err("Invalid RAR type\n");
-		return -EINVAL;
-	}
-	return retval;
-}
-
-#endif
-
-/* This function creates the scatter gather list to be sent to firmware to
-capture/playback data*/
-static int sst_create_sg_list(struct stream_info *stream,
-		struct sst_frame_info *sg_list)
-{
-	struct sst_stream_bufs *kbufs = NULL;
-#ifdef CONFIG_MRST_RAR_HANDLER
-	struct RAR_buffer rar_buffers;
-	int retval = 0;
-#endif
-	int i = 0;
-	list_for_each_entry(kbufs, &stream->bufs, node) {
-		if (kbufs->in_use == false) {
-#ifdef CONFIG_MRST_RAR_HANDLER
-			if (stream->ops == STREAM_OPS_PLAYBACK_DRM) {
-				pr_debug("DRM playback handling\n");
-				rar_buffers.info.handle = (__u32)kbufs->addr;
-				rar_buffers.info.size = kbufs->size;
-				pr_debug("rar handle 0x%x size=0x%x\n",
-					rar_buffers.info.handle,
-					rar_buffers.info.size);
-				retval =  sst_get_RAR(&rar_buffers, 1);
-
-				if (retval)
-					return retval;
-				sg_list->addr[i].addr = rar_buffers.bus_address;
-				/* rar_buffers.info.size; */
-				sg_list->addr[i].size = (__u32)kbufs->size;
-				pr_debug("phyaddr[%d] 0x%x Size:0x%x\n"
-					, i, sg_list->addr[i].addr,
-					sg_list->addr[i].size);
-			}
-#endif
-			if (stream->ops != STREAM_OPS_PLAYBACK_DRM) {
-				sg_list->addr[i].addr =
-					virt_to_phys((void *)
-						kbufs->addr + kbufs->offset);
-				sg_list->addr[i].size = kbufs->size;
-				pr_debug("phyaddr[%d]:0x%x Size:0x%x\n"
-				, i , sg_list->addr[i].addr, kbufs->size);
-			}
-			stream->curr_bytes += sg_list->addr[i].size;
-			kbufs->in_use = true;
-			i++;
-		}
-		if (i >= MAX_NUM_SCATTER_BUFFERS)
-			break;
-	}
-
-	sg_list->num_entries = i;
-	pr_debug("sg list entries = %d\n", sg_list->num_entries);
-	return i;
-}
-
-
-/**
- * sst_play_frame - Send msg for sending stream frames
- *
- * @str_id:	ID of stream
- *
- * This function is called to send data to be played out
- * to the firmware
- */
-int sst_play_frame(int str_id)
-{
-	int i = 0, retval = 0;
-	struct ipc_post *msg = NULL;
-	struct sst_frame_info sg_list = {0};
-	struct sst_stream_bufs *kbufs = NULL, *_kbufs;
-	struct stream_info *stream;
-
-	pr_debug("play frame for %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-
-	stream = &sst_drv_ctx->streams[str_id];
-	/* clear prev sent buffers */
-	list_for_each_entry_safe(kbufs, _kbufs, &stream->bufs, node) {
-		if (kbufs->in_use == true) {
-			spin_lock(&stream->pcm_lock);
-			list_del(&kbufs->node);
-			spin_unlock(&stream->pcm_lock);
-			kfree(kbufs);
-		}
-	}
-	/* update bytes sent */
-	stream->cumm_bytes += stream->curr_bytes;
-	stream->curr_bytes = 0;
-	if (list_empty(&stream->bufs)) {
-		/* no user buffer available */
-		pr_debug("Null buffer stream status %d\n", stream->status);
-		stream->prev = stream->status;
-		stream->status = STREAM_INIT;
-		pr_debug("new stream status = %d\n", stream->status);
-		if (stream->need_draining == true) {
-			pr_debug("draining stream\n");
-			if (sst_create_short_msg(&msg)) {
-				pr_err("mem allocation failed\n");
-				return -ENOMEM;
-			}
-			sst_fill_header(&msg->header, IPC_IA_DRAIN_STREAM,
-						0, str_id);
-			spin_lock(&sst_drv_ctx->list_spin_lock);
-			list_add_tail(&msg->node,
-					&sst_drv_ctx->ipc_dispatch_list);
-			spin_unlock(&sst_drv_ctx->list_spin_lock);
-			sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-		} else if (stream->data_blk.on == true) {
-			pr_debug("user list empty.. wake\n");
-			/* unblock */
-			stream->data_blk.ret_code = 0;
-			stream->data_blk.condition = true;
-			stream->data_blk.on = false;
-			wake_up(&sst_drv_ctx->wait_queue);
-		}
-		return 0;
-	}
-
-	/* create list */
-	i = sst_create_sg_list(stream, &sg_list);
-
-	/* post msg */
-	if (sst_create_large_msg(&msg))
-		return -ENOMEM;
-
-	sst_fill_header(&msg->header, IPC_IA_PLAY_FRAMES, 1, str_id);
-	msg->header.part.data = sizeof(u32) + sizeof(sg_list);
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), &sg_list, sizeof(sg_list));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	return 0;
-
-}
-
-/**
- * sst_capture_frame - Send msg for sending stream frames
- *
- * @str_id:	ID of stream
- *
- * This function is called to capture data from the firmware
- */
-int sst_capture_frame(int str_id)
-{
-	int i = 0, retval = 0;
-	struct ipc_post *msg = NULL;
-	struct sst_frame_info sg_list = {0};
-	struct sst_stream_bufs *kbufs = NULL, *_kbufs;
-	struct stream_info *stream;
-
-
-	pr_debug("capture frame for %d\n", str_id);
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-	stream = &sst_drv_ctx->streams[str_id];
-	/* clear prev sent buffers */
-	list_for_each_entry_safe(kbufs, _kbufs, &stream->bufs, node) {
-		if (kbufs->in_use == true) {
-			list_del(&kbufs->node);
-			kfree(kbufs);
-			pr_debug("del node\n");
-		}
-	}
-	if (list_empty(&stream->bufs)) {
-		/* no user buffer available */
-		pr_debug("Null buffer!!!!stream status %d\n",
-			       stream->status);
-		stream->prev = stream->status;
-		stream->status = STREAM_INIT;
-		pr_debug("new stream status = %d\n",
-			       stream->status);
-		if (stream->data_blk.on == true) {
-			pr_debug("user list empty.. wake\n");
-			/* unblock */
-			stream->data_blk.ret_code = 0;
-			stream->data_blk.condition = true;
-			stream->data_blk.on = false;
-			wake_up(&sst_drv_ctx->wait_queue);
-
-		}
-		return 0;
-	}
-	/* create new sg list */
-	i = sst_create_sg_list(stream, &sg_list);
-
-	/* post msg */
-	if (sst_create_large_msg(&msg))
-		return -ENOMEM;
-
-	sst_fill_header(&msg->header, IPC_IA_CAPT_FRAMES, 1, str_id);
-	msg->header.part.data = sizeof(u32) + sizeof(sg_list);
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), &sg_list, sizeof(sg_list));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-
-
-	/*update bytes recevied*/
-	stream->cumm_bytes += stream->curr_bytes;
-	stream->curr_bytes = 0;
-
-    pr_debug("Cum bytes  = %d\n", stream->cumm_bytes);
-	return 0;
-}
-
-/*This function is used to calculate the minimum size of input buffers given*/
-static unsigned int calculate_min_size(struct snd_sst_buffs *bufs)
-{
-	int i, min_val = bufs->buff_entry[0].size;
-	for (i = 1 ; i < bufs->entries; i++) {
-		if (bufs->buff_entry[i].size < min_val)
-			min_val = bufs->buff_entry[i].size;
-	}
-	pr_debug("min_val = %d\n", min_val);
-	return min_val;
-}
-
-static unsigned int calculate_max_size(struct snd_sst_buffs *bufs)
-{
-	int i, max_val = bufs->buff_entry[0].size;
-	for (i = 1 ; i < bufs->entries; i++) {
-		if (bufs->buff_entry[i].size > max_val)
-			max_val = bufs->buff_entry[i].size;
-	}
-	pr_debug("max_val = %d\n", max_val);
-	return max_val;
-}
-
-/*This function is used to allocate input and output buffers to be sent to
-the firmware that will take encoded data and return decoded data*/
-static int sst_allocate_decode_buf(struct stream_info *str_info,
-				struct snd_sst_dbufs *dbufs,
-				unsigned int cum_input_given,
-				unsigned int cum_output_given)
-{
-#ifdef CONFIG_MRST_RAR_HANDLER
-	if (str_info->ops == STREAM_OPS_PLAYBACK_DRM) {
-
-		if (dbufs->ibufs->type == SST_BUF_RAR  &&
-			dbufs->obufs->type == SST_BUF_RAR) {
-			if (dbufs->ibufs->entries == dbufs->obufs->entries)
-				return 0;
-			else {
-				pr_err("RAR entries dont match\n");
-				 return -EINVAL;
-			}
-		} else
-			str_info->decode_osize = cum_output_given;
-		return 0;
-
-	}
-#endif
-	if (!str_info->decode_ibuf) {
-		pr_debug("no i/p buffers, trying full size\n");
-		str_info->decode_isize = cum_input_given;
-		str_info->decode_ibuf = kzalloc(str_info->decode_isize,
-						GFP_KERNEL);
-		str_info->idecode_alloc = str_info->decode_isize;
-	}
-	if (!str_info->decode_ibuf) {
-		pr_debug("buff alloc failed, try max size\n");
-		str_info->decode_isize = calculate_max_size(dbufs->ibufs);
-		str_info->decode_ibuf = kzalloc(
-				str_info->decode_isize, GFP_KERNEL);
-		str_info->idecode_alloc = str_info->decode_isize;
-	}
-	if (!str_info->decode_ibuf) {
-		pr_debug("buff alloc failed, try min size\n");
-		str_info->decode_isize = calculate_min_size(dbufs->ibufs);
-		str_info->decode_ibuf = kzalloc(str_info->decode_isize,
-						GFP_KERNEL);
-		if (!str_info->decode_ibuf) {
-			pr_err("mem allocation failed\n");
-			return -ENOMEM;
-		}
-		str_info->idecode_alloc = str_info->decode_isize;
-	}
-	str_info->decode_osize = cum_output_given;
-	if (str_info->decode_osize > sst_drv_ctx->mmap_len)
-		str_info->decode_osize = sst_drv_ctx->mmap_len;
-	return 0;
-}
-
-/*This function is used to send the message to firmware to decode the data*/
-static int sst_send_decode_mess(int str_id, struct stream_info *str_info,
-				struct snd_sst_decode_info *dec_info)
-{
-	struct ipc_post *msg = NULL;
-	int retval = 0;
-
-	pr_debug("SST DBG:sst_set_mute:called\n");
-
-	if (str_info->decode_ibuf_type == SST_BUF_RAR) {
-#ifdef CONFIG_MRST_RAR_HANDLER
-			dec_info->frames_in.addr[0].addr =
-				(unsigned long)str_info->decode_ibuf;
-			dec_info->frames_in.addr[0].size =
-							str_info->decode_isize;
-#endif
-
-	} else {
-		dec_info->frames_in.addr[0].addr = virt_to_phys((void *)
-					str_info->decode_ibuf);
-		dec_info->frames_in.addr[0].size = str_info->decode_isize;
-	}
-
-
-	if (str_info->decode_obuf_type == SST_BUF_RAR) {
-#ifdef CONFIG_MRST_RAR_HANDLER
-		dec_info->frames_out.addr[0].addr =
-				(unsigned long)str_info->decode_obuf;
-		dec_info->frames_out.addr[0].size = str_info->decode_osize;
-#endif
-
-	} else {
-		dec_info->frames_out.addr[0].addr = virt_to_phys((void *)
-						str_info->decode_obuf) ;
-		dec_info->frames_out.addr[0].size = str_info->decode_osize;
-	}
-
-	dec_info->frames_in.num_entries = 1;
-	dec_info->frames_out.num_entries = 1;
-	dec_info->frames_in.rsrvd = 0;
-	dec_info->frames_out.rsrvd = 0;
-	dec_info->input_bytes_consumed = 0;
-	dec_info->output_bytes_produced = 0;
-	if (sst_create_large_msg(&msg)) {
-		pr_err("message creation failed\n");
-		return -ENOMEM;
-	}
-
-	sst_fill_header(&msg->header, IPC_IA_DECODE_FRAMES, 1, str_id);
-	msg->header.part.data = sizeof(u32) + sizeof(*dec_info);
-	memcpy(msg->mailbox_data, &msg->header, sizeof(u32));
-	memcpy(msg->mailbox_data + sizeof(u32), dec_info,
-			sizeof(*dec_info));
-	spin_lock(&sst_drv_ctx->list_spin_lock);
-	list_add_tail(&msg->node, &sst_drv_ctx->ipc_dispatch_list);
-	spin_unlock(&sst_drv_ctx->list_spin_lock);
-	str_info->data_blk.condition = false;
-	str_info->data_blk.ret_code = 0;
-	str_info->data_blk.on = true;
-	str_info->data_blk.data = dec_info;
-	sst_post_message(&sst_drv_ctx->ipc_post_msg_wq);
-	retval = sst_wait_interruptible(sst_drv_ctx, &str_info->data_blk);
-	return retval;
-}
-
-#ifdef CONFIG_MRST_RAR_HANDLER
-static int sst_prepare_input_buffers_rar(struct stream_info *str_info,
-			struct snd_sst_dbufs *dbufs,
-			int *input_index, int *in_copied,
-			int *input_index_valid_size, int *new_entry_flag)
-{
-	int retval = 0, i;
-
-	if (str_info->ops == STREAM_OPS_PLAYBACK_DRM) {
-		struct RAR_buffer rar_buffers;
-		__u32 info;
-		retval = copy_from_user((void *) &info,
-				dbufs->ibufs->buff_entry[i].buffer,
-				sizeof(__u32));
-		if (retval) {
-			pr_err("cpy from user fail\n");
-			return -EAGAIN;
-		}
-		rar_buffers.info.type = dbufs->ibufs->type;
-		rar_buffers.info.size = dbufs->ibufs->buff_entry[i].size;
-		rar_buffers.info.handle =  info;
-		pr_debug("rar in DnR(input buffer function)=0x%x size=0x%x",
-				rar_buffers.info.handle,
-				rar_buffers.info.size);
-		retval =  sst_get_RAR(&rar_buffers, 1);
-		if (retval) {
-			pr_debug("SST ERR: RAR API failed\n");
-			return retval;
-		}
-		str_info->decode_ibuf =
-		(void *) ((unsigned long) rar_buffers.bus_address);
-		pr_debug("RAR buf addr in DnR (input buffer function)0x%lu",
-				 (unsigned long) str_info->decode_ibuf);
-		pr_debug("rar in DnR decode function/output b_add rar =0x%lu",
-				(unsigned long) rar_buffers.bus_address);
-		*input_index = i + 1;
-		str_info->decode_isize = dbufs->ibufs->buff_entry[i].size;
-		str_info->decode_ibuf_type = dbufs->ibufs->type;
-		*in_copied = str_info->decode_isize;
-	}
-	return retval;
-}
-#endif
-/*This function is used to prepare the kernel input buffers with contents
-before sending for decode*/
-static int sst_prepare_input_buffers(struct stream_info *str_info,
-			struct snd_sst_dbufs *dbufs,
-			int *input_index, int *in_copied,
-			int *input_index_valid_size, int *new_entry_flag)
-{
-	int i, cpy_size, retval = 0;
-
-	pr_debug("input_index = %d, input entries = %d\n",
-			 *input_index, dbufs->ibufs->entries);
-	for (i = *input_index; i < dbufs->ibufs->entries; i++) {
-#ifdef CONFIG_MRST_RAR_HANDLER
-		retval = sst_prepare_input_buffers_rar(str_info,
-			dbufs, input_index, in_copied,
-				input_index_valid_size, new_entry_flag);
-		if (retval) {
-			pr_err("In prepare input buffers for RAR\n");
-			return -EIO;
-		}
-#endif
-		*input_index = i;
-		if (*input_index_valid_size == 0)
-			*input_index_valid_size =
-				dbufs->ibufs->buff_entry[i].size;
-		pr_debug("inout addr = %p, size = %d\n",
-			dbufs->ibufs->buff_entry[i].buffer,
-			*input_index_valid_size);
-		pr_debug("decode_isize = %d, in_copied %d\n",
-			str_info->decode_isize, *in_copied);
-		if (*input_index_valid_size <=
-					(str_info->decode_isize - *in_copied))
-			cpy_size = *input_index_valid_size;
-		else
-			cpy_size = str_info->decode_isize - *in_copied;
-
-		pr_debug("cpy size = %d\n", cpy_size);
-		if (!dbufs->ibufs->buff_entry[i].buffer) {
-			pr_err("i/p buffer is null\n");
-			return -EINVAL;
-		}
-		pr_debug("Try copy To %p, From %p, size %d\n",
-				str_info->decode_ibuf + *in_copied,
-				dbufs->ibufs->buff_entry[i].buffer, cpy_size);
-
-		retval =
-		copy_from_user((void *)(str_info->decode_ibuf + *in_copied),
-				(void *) dbufs->ibufs->buff_entry[i].buffer,
-				cpy_size);
-		if (retval) {
-			pr_err("copy from user failed\n");
-			return -EIO;
-		}
-		*in_copied += cpy_size;
-		*input_index_valid_size -= cpy_size;
-		pr_debug("in buff size = %d, in_copied = %d\n",
-			*input_index_valid_size, *in_copied);
-		if (*input_index_valid_size != 0) {
-			pr_debug("more input buffers left\n");
-			dbufs->ibufs->buff_entry[i].buffer += cpy_size;
-			break;
-		}
-		if (*in_copied == str_info->decode_isize &&
-			*input_index_valid_size == 0 &&
-			(i+1) <= dbufs->ibufs->entries) {
-			pr_debug("all input buffers copied\n");
-			*new_entry_flag = true;
-			*input_index = i + 1;
-			break;
-		}
-	}
-	return retval;
-}
-
-/* This function is used to copy the decoded data from kernel buffers to
-the user output buffers with contents after decode*/
-static int sst_prepare_output_buffers(struct stream_info *str_info,
-			struct snd_sst_dbufs *dbufs,
-			int *output_index, int output_size,
-			int *out_copied)
-
-{
-	int i, cpy_size, retval = 0;
-	pr_debug("output_index = %d, output entries = %d\n",
-				*output_index,
-				dbufs->obufs->entries);
-	for (i = *output_index; i < dbufs->obufs->entries; i++) {
-		*output_index = i;
-		pr_debug("output addr = %p, size = %d\n",
-			dbufs->obufs->buff_entry[i].buffer,
-			dbufs->obufs->buff_entry[i].size);
-		pr_debug("output_size = %d, out_copied = %d\n",
-				output_size, *out_copied);
-		if (dbufs->obufs->buff_entry[i].size <
-				(output_size - *out_copied))
-			cpy_size = dbufs->obufs->buff_entry[i].size;
-		else
-			cpy_size = output_size - *out_copied;
-		pr_debug("cpy size = %d\n", cpy_size);
-		pr_debug("Try copy To: %p, From %p, size %d\n",
-				dbufs->obufs->buff_entry[i].buffer,
-				sst_drv_ctx->mmap_mem + *out_copied,
-				cpy_size);
-		retval = copy_to_user(dbufs->obufs->buff_entry[i].buffer,
-					sst_drv_ctx->mmap_mem + *out_copied,
-					cpy_size);
-		if (retval) {
-			pr_err("copy to user failed\n");
-			return -EIO;
-		} else
-			pr_debug("copy to user passed\n");
-		*out_copied += cpy_size;
-		dbufs->obufs->buff_entry[i].size -= cpy_size;
-		pr_debug("o/p buff size %d, out_copied %d\n",
-			dbufs->obufs->buff_entry[i].size, *out_copied);
-		if (dbufs->obufs->buff_entry[i].size != 0) {
-			*output_index = i;
-			dbufs->obufs->buff_entry[i].buffer += cpy_size;
-			break;
-		} else if (*out_copied == output_size) {
-			*output_index = i + 1;
-			break;
-		}
-	}
-	return retval;
-}
-
-/**
- * sst_decode - Send msg for decoding frames
- *
- * @str_id: ID of stream
- * @dbufs: param that holds the user input and output buffers and size
- *
- * This function is called to decode data from the firmware
- */
-int sst_decode(int str_id, struct snd_sst_dbufs *dbufs)
-{
-	int retval = 0, i;
-	unsigned long long total_input = 0 , total_output = 0;
-	unsigned int cum_input_given = 0 , cum_output_given = 0;
-	int copy_in_done = false, copy_out_done = false;
-	int input_index = 0, output_index = 0;
-	int input_index_valid_size = 0;
-	int in_copied, out_copied;
-	int new_entry_flag;
-	u64 output_size;
-	struct stream_info *str_info;
-	struct snd_sst_decode_info dec_info;
-	unsigned long long input_bytes, output_bytes;
-
-	sst_drv_ctx->scard_ops->power_down_pmic();
-	pr_debug("Powering_down_PMIC...\n");
-
-	retval = sst_validate_strid(str_id);
-	if (retval)
-		return retval;
-
-	str_info = &sst_drv_ctx->streams[str_id];
-	if (str_info->status != STREAM_INIT) {
-		pr_err("invalid stream state = %d\n",
-			       str_info->status);
-		return -EINVAL;
-	}
-
-	str_info->prev = str_info->status;
-	str_info->status = STREAM_DECODE;
-
-	for (i = 0; i < dbufs->ibufs->entries; i++)
-		cum_input_given += dbufs->ibufs->buff_entry[i].size;
-	for (i = 0; i < dbufs->obufs->entries; i++)
-		cum_output_given += dbufs->obufs->buff_entry[i].size;
-
-	/* input and output buffer allocation */
-	retval =  sst_allocate_decode_buf(str_info, dbufs,
-				cum_input_given, cum_output_given);
-	if (retval) {
-		pr_err("mem allocation failed, abort!!!\n");
-		retval = -ENOMEM;
-		goto finish;
-	}
-
-	str_info->decode_isize = str_info->idecode_alloc;
-	str_info->decode_ibuf_type = dbufs->ibufs->type;
-	str_info->decode_obuf_type = dbufs->obufs->type;
-
-	while ((copy_out_done == false) && (copy_in_done == false)) {
-		in_copied = 0;
-		new_entry_flag = false;
-		retval = sst_prepare_input_buffers(str_info,\
-			dbufs, &input_index, &in_copied,
-			&input_index_valid_size, &new_entry_flag);
-		if (retval) {
-			pr_err("prepare in buffers failed\n");
-			goto finish;
-		}
-
-		if (str_info->ops != STREAM_OPS_PLAYBACK_DRM)
-			str_info->decode_obuf = sst_drv_ctx->mmap_mem;
-
-#ifdef CONFIG_MRST_RAR_HANDLER
-		else {
-			if (dbufs->obufs->type == SST_BUF_RAR) {
-				struct RAR_buffer rar_buffers;
-				__u32 info;
-
-				pr_debug("DRM");
-				retval = copy_from_user((void *) &info,
-						dbufs->obufs->
-						buff_entry[output_index].buffer,
-						sizeof(__u32));
-
-				rar_buffers.info.size = dbufs->obufs->
-					buff_entry[output_index].size;
-				rar_buffers.info.handle =  info;
-				retval =  sst_get_RAR(&rar_buffers, 1);
-				if (retval)
-					return retval;
-
-				str_info->decode_obuf = (void *)((unsigned long)
-						rar_buffers.bus_address);
-				str_info->decode_osize = dbufs->obufs->
-					buff_entry[output_index].size;
-				str_info->decode_obuf_type = dbufs->obufs->type;
-				pr_debug("DRM handling\n");
-				pr_debug("o/p_add=0x%lu Size=0x%x\n",
-					(unsigned long) str_info->decode_obuf,
-					str_info->decode_osize);
-			} else {
-				str_info->decode_obuf = sst_drv_ctx->mmap_mem;
-				str_info->decode_osize = dbufs->obufs->
-					buff_entry[output_index].size;
-
-			}
-		}
-#endif
-		if (str_info->ops != STREAM_OPS_PLAYBACK_DRM) {
-			if (str_info->decode_isize > in_copied) {
-				str_info->decode_isize = in_copied;
-				pr_debug("i/p size = %d\n",
-						str_info->decode_isize);
-			}
-		}
-
-
-		retval = sst_send_decode_mess(str_id, str_info, &dec_info);
-		if (retval || dec_info.input_bytes_consumed == 0) {
-			pr_err("SST ERR: mess failed or no input consumed\n");
-			goto finish;
-		}
-		input_bytes = dec_info.input_bytes_consumed;
-		output_bytes = dec_info.output_bytes_produced;
-
-		pr_debug("in_copied=%d, con=%lld, prod=%lld\n",
-			in_copied, input_bytes, output_bytes);
-		if (dbufs->obufs->type == SST_BUF_RAR) {
-			output_index += 1;
-			if (output_index == dbufs->obufs->entries) {
-				copy_in_done = true;
-				pr_debug("all i/p cpy done\n");
-			}
-			total_output += output_bytes;
-		} else {
-			out_copied = 0;
-			output_size = output_bytes;
-			retval = sst_prepare_output_buffers(str_info, dbufs,
-				&output_index, output_size, &out_copied);
-			if (retval) {
-				pr_err("prep out buff fail\n");
-				goto finish;
-			}
-			if (str_info->ops != STREAM_OPS_PLAYBACK_DRM) {
-				if (in_copied != input_bytes) {
-					int bytes_left = in_copied -
-								input_bytes;
-					pr_debug("bytes %d\n",
-							bytes_left);
-					if (new_entry_flag == true)
-						input_index--;
-					while (bytes_left) {
-						struct snd_sst_buffs *ibufs;
-						struct snd_sst_buff_entry
-								*buff_entry;
-						unsigned int size_sent;
-
-						ibufs = dbufs->ibufs;
-						buff_entry =
-						&ibufs->buff_entry[input_index];
-						size_sent = buff_entry->size -\
-							input_index_valid_size;
-						if (bytes_left == size_sent) {
-								bytes_left = 0;
-						} else if (bytes_left <
-								size_sent) {
-							buff_entry->buffer +=
-							 (size_sent -
-								bytes_left);
-							buff_entry->size -=
-							 (size_sent -
-								bytes_left);
-							bytes_left = 0;
-						} else {
-							bytes_left -= size_sent;
-							input_index--;
-							input_index_valid_size =
-									0;
-						}
-					}
-
-				}
-			}
-
-			total_output += out_copied;
-			if (str_info->decode_osize != out_copied) {
-				str_info->decode_osize -= out_copied;
-				pr_debug("output size modified = %d\n",
-						str_info->decode_osize);
-			}
-		}
-		total_input += input_bytes;
-
-		if (str_info->ops == STREAM_OPS_PLAYBACK_DRM) {
-			if (total_input == cum_input_given)
-				copy_in_done = true;
-			copy_out_done = true;
-
-		} else {
-			if (total_output == cum_output_given) {
-				copy_out_done = true;
-				pr_debug("all o/p cpy done\n");
-			}
-
-			if (total_input == cum_input_given) {
-				copy_in_done = true;
-				pr_debug("all i/p cpy done\n");
-			}
-		}
-
-		pr_debug("copy_out = %d, copy_in = %d\n",
-				copy_out_done, copy_in_done);
-	}
-
-finish:
-	dbufs->input_bytes_consumed = total_input;
-	dbufs->output_bytes_produced = total_output;
-	str_info->status = str_info->prev;
-	str_info->prev = STREAM_DECODE;
-	kfree(str_info->decode_ibuf);
-	str_info->decode_ibuf = NULL;
-	return retval;
-}
diff --git a/drivers/staging/intel_sst/intelmid.c b/drivers/staging/intel_sst/intelmid.c
deleted file mode 100644
index 492b660246b4..000000000000
--- a/drivers/staging/intel_sst/intelmid.c
+++ /dev/null
@@ -1,1022 +0,0 @@
-/*
- *   intelmid.c - Intel Sound card driver for MID
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Harsha Priya <priya.harsha@intel.com>
- *		Vinod Koul <vinod.koul@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- * ALSA driver for Intel MID sound card chipset
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/interrupt.h>
-#include <linux/sched.h>
-#include <linux/firmware.h>
-#include <linux/input.h>
-#include <sound/control.h>
-#include <asm/mrst.h>
-#include <sound/pcm.h>
-#include <sound/jack.h>
-#include <sound/pcm_params.h>
-#include <sound/initval.h>
-#include <linux/gpio.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intel_sst_fw_ipc.h"
-#include "intel_sst_common.h"
-#include "intelmid_snd_control.h"
-#include "intelmid_adc_control.h"
-#include "intelmid.h"
-
-MODULE_AUTHOR("Vinod Koul <vinod.koul@intel.com>");
-MODULE_AUTHOR("Harsha Priya <priya.harsha@intel.com>");
-MODULE_AUTHOR("Dharageswari R <dharageswari.r@intel.com>");
-MODULE_AUTHOR("KP Jeeja <jeeja.kp@intel.com>");
-MODULE_DESCRIPTION("Intel MAD Sound card driver");
-MODULE_LICENSE("GPL v2");
-MODULE_SUPPORTED_DEVICE("{Intel,Intel_MAD}");
-
-
-static int card_index = SNDRV_DEFAULT_IDX1;/* Index 0-MAX */
-static char *card_id = SNDRV_DEFAULT_STR1;	/* ID for this card */
-
-module_param(card_index, int, 0444);
-MODULE_PARM_DESC(card_index, "Index value for INTELMAD soundcard.");
-module_param(card_id, charp, 0444);
-MODULE_PARM_DESC(card_id, "ID string for INTELMAD soundcard.");
-
-int	sst_card_vendor_id;
-int intelmid_audio_interrupt_enable;/*checkpatch fix*/
-struct snd_intelmad *intelmad_drv;
-
-#define INFO(_cpu_id, _irq_cache, _size) \
-	((kernel_ulong_t)&(struct snd_intelmad_probe_info) {	\
-		.cpu_id = (_cpu_id),			\
-		.irq_cache = (_irq_cache),			\
-		.size = (_size),				\
-	})
-/* Data path functionalities */
-static struct snd_pcm_hardware snd_intelmad_stream = {
-	.info =	(SNDRV_PCM_INFO_INTERLEAVED |
-			SNDRV_PCM_INFO_DOUBLE |
-			SNDRV_PCM_INFO_PAUSE |
-			SNDRV_PCM_INFO_RESUME |
-			SNDRV_PCM_INFO_MMAP|
-			SNDRV_PCM_INFO_MMAP_VALID |
-			SNDRV_PCM_INFO_BLOCK_TRANSFER |
-			SNDRV_PCM_INFO_SYNC_START),
-	.formats = (SNDRV_PCM_FMTBIT_S16 | SNDRV_PCM_FMTBIT_U16 |
-			SNDRV_PCM_FMTBIT_S24 | SNDRV_PCM_FMTBIT_U24 |
-			SNDRV_PCM_FMTBIT_S32 | SNDRV_PCM_FMTBIT_U32),
-	.rates = (SNDRV_PCM_RATE_8000|
-			SNDRV_PCM_RATE_44100 |
-			SNDRV_PCM_RATE_48000),
-	.rate_min = MIN_RATE,
-
-	.rate_max = MAX_RATE,
-	.channels_min =	MIN_CHANNEL,
-	.channels_max =	MAX_CHANNEL_AMIC,
-	.buffer_bytes_max = MAX_BUFFER,
-	.period_bytes_min = MIN_PERIOD_BYTES,
-	.period_bytes_max = MAX_PERIOD_BYTES,
-	.periods_min = MIN_PERIODS,
-	.periods_max = MAX_PERIODS,
-	.fifo_size = FIFO_SIZE,
-};
-
-
-/**
- * snd_intelmad_pcm_trigger - stream activities are handled here
- *
- * @substream:substream for which the stream function is called
- * @cmd:the stream commamd that requested from upper layer
- *
- * This function is called whenever an a stream activity is invoked
- */
-static int snd_intelmad_pcm_trigger(struct snd_pcm_substream *substream,
-					int cmd)
-{
-	int ret_val = 0, str_id;
-	struct snd_intelmad *intelmaddata;
-	struct mad_stream_pvt *stream;
-	struct intel_sst_pcm_control *sst_ops;
-
-	WARN_ON(!substream);
-
-	intelmaddata = snd_pcm_substream_chip(substream);
-	stream = substream->runtime->private_data;
-
-	WARN_ON(!intelmaddata->sstdrv_ops);
-	WARN_ON(!intelmaddata->sstdrv_ops->scard_ops);
-	sst_ops  = intelmaddata->sstdrv_ops->pcm_control;
-	str_id = stream->stream_info.str_id;
-
-	switch (cmd) {
-	case SNDRV_PCM_TRIGGER_START:
-		pr_debug("Trigger Start\n");
-		ret_val = sst_ops->device_control(SST_SND_START, &str_id);
-		if (ret_val)
-			return ret_val;
-		stream->stream_status = RUNNING;
-		stream->substream = substream;
-		break;
-	case SNDRV_PCM_TRIGGER_STOP:
-		pr_debug("in stop\n");
-		ret_val = sst_ops->device_control(SST_SND_DROP, &str_id);
-		if (ret_val)
-			return ret_val;
-		stream->stream_status = DROPPED;
-		break;
-	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
-		pr_debug("in pause\n");
-		ret_val = sst_ops->device_control(SST_SND_PAUSE, &str_id);
-		if (ret_val)
-			return ret_val;
-		stream->stream_status = PAUSED;
-		break;
-	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
-		pr_debug("in pause release\n");
-		ret_val = sst_ops->device_control(SST_SND_RESUME, &str_id);
-		if (ret_val)
-			return ret_val;
-		stream->stream_status = RUNNING;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return ret_val;
-}
-
-/**
-* snd_intelmad_pcm_prepare- internal preparation before starting a stream
-*
-* @substream:  substream for which the function is called
-*
-* This function is called when a stream is started for internal preparation.
-*/
-static int snd_intelmad_pcm_prepare(struct snd_pcm_substream *substream)
-{
-	struct mad_stream_pvt *stream;
-	int ret_val = 0;
-	struct snd_intelmad *intelmaddata;
-
-	pr_debug("pcm_prepare called\n");
-
-	WARN_ON(!substream);
-	stream = substream->runtime->private_data;
-	intelmaddata = snd_pcm_substream_chip(substream);
-	pr_debug("pb cnt = %d cap cnt = %d\n",\
-		intelmaddata->playback_cnt,
-		intelmaddata->capture_cnt);
-
-	if (stream->stream_info.str_id) {
-		pr_debug("Prepare called for already set stream\n");
-		ret_val = intelmaddata->sstdrv_ops->pcm_control->device_control(
-				SST_SND_DROP, &stream->stream_info.str_id);
-		return ret_val;
-	}
-
-	ret_val = snd_intelmad_alloc_stream(substream);
-	if (ret_val < 0)
-		return ret_val;
-	stream->dbg_cum_bytes = 0;
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
-			intelmaddata->playback_cnt++;
-	else
-		intelmaddata->capture_cnt++;
-	/* return back the stream id */
-	snprintf(substream->pcm->id, sizeof(substream->pcm->id),
-			"%d", stream->stream_info.str_id);
-	pr_debug("stream id to user = %s\n",
-			substream->pcm->id);
-
-	ret_val = snd_intelmad_init_stream(substream);
-	if (ret_val)
-		return ret_val;
-	substream->runtime->hw.info = SNDRV_PCM_INFO_BLOCK_TRANSFER;
-	return ret_val;
-}
-
-static int snd_intelmad_hw_params(struct snd_pcm_substream *substream,
-				    struct snd_pcm_hw_params *hw_params)
-{
-	int ret_val;
-
-	pr_debug("snd_intelmad_hw_params called\n");
-	ret_val = snd_pcm_lib_malloc_pages(substream,
-			params_buffer_bytes(hw_params));
-	memset(substream->runtime->dma_area, 0,
-			params_buffer_bytes(hw_params));
-
-	return ret_val;
-}
-
-static int snd_intelmad_hw_free(struct snd_pcm_substream *substream)
-{
-	pr_debug("snd_intelmad_hw_free called\n");
-	return snd_pcm_lib_free_pages(substream);
-}
-
-/**
- * snd_intelmad_pcm_pointer- to send the current buffer pointer processed by hw
- *
- * @substream:  substream for which the function is called
- *
- * This function is called by ALSA framework to get the current hw buffer ptr
- * when a period is elapsed
- */
-static snd_pcm_uframes_t snd_intelmad_pcm_pointer
-			(struct snd_pcm_substream *substream)
-{
-	/* struct snd_pcm_runtime *runtime = substream->runtime; */
-	struct mad_stream_pvt *stream;
-	struct snd_intelmad *intelmaddata;
-	int ret_val;
-
-	WARN_ON(!substream);
-
-	intelmaddata = snd_pcm_substream_chip(substream);
-	stream = substream->runtime->private_data;
-	if (stream->stream_status == INIT)
-		return 0;
-
-	ret_val = intelmaddata->sstdrv_ops->pcm_control->device_control(
-			SST_SND_BUFFER_POINTER, &stream->stream_info);
-	if (ret_val) {
-		pr_err("error code = 0x%x\n", ret_val);
-		return ret_val;
-	}
-	pr_debug("samples reported out 0x%llx\n",
-			stream->stream_info.buffer_ptr);
-	pr_debug("Frame bits:: %d period_count :: %d\n",
-			(int)substream->runtime->frame_bits,
-			(int)substream->runtime->period_size);
-
-	return stream->stream_info.buffer_ptr;
-
-}
-
-/**
- * snd_intelmad_close- to free parameteres when stream is stopped
- *
- * @substream:  substream for which the function is called
- *
- * This function is called by ALSA framework when stream is stopped
- */
-static int snd_intelmad_close(struct snd_pcm_substream *substream)
-{
-	struct snd_intelmad *intelmaddata;
-	struct mad_stream_pvt *stream;
-	int ret_val = 0, str_id;
-
-	WARN_ON(!substream);
-
-	stream = substream->runtime->private_data;
-	str_id = stream->stream_info.str_id;
-
-	pr_debug("sst: snd_intelmad_close called for %d\n", str_id);
-	intelmaddata = snd_pcm_substream_chip(substream);
-
-	pr_debug("str id = %d\n", stream->stream_info.str_id);
-	if (stream->stream_info.str_id) {
-		/* SST API to actually stop/free the stream */
-		ret_val = intelmaddata->sstdrv_ops->pcm_control->close(str_id);
-		if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
-			intelmaddata->playback_cnt--;
-		else
-			intelmaddata->capture_cnt--;
-	}
-	pr_debug("snd_intelmad_close : pb cnt = %d cap cnt = %d\n",
-		intelmaddata->playback_cnt, intelmaddata->capture_cnt);
-	kfree(substream->runtime->private_data);
-	return ret_val;
-}
-
-/**
- * snd_intelmad_open- to set runtime parameters during stream start
- *
- * @substream:  substream for which the function is called
- * @type: audio device type
- *
- * This function is called by ALSA framework when stream is started
- */
-static int snd_intelmad_open(struct snd_pcm_substream *substream,
-			enum snd_sst_audio_device_type type)
-{
-	struct snd_intelmad *intelmaddata;
-	struct snd_pcm_runtime *runtime;
-	struct mad_stream_pvt *stream;
-
-	WARN_ON(!substream);
-
-	pr_debug("snd_intelmad_open called\n");
-
-	intelmaddata = snd_pcm_substream_chip(substream);
-	runtime = substream->runtime;
-	/* set the runtime hw parameter with local snd_pcm_hardware struct */
-	runtime->hw = snd_intelmad_stream;
-	if (intelmaddata->cpu_id == CPU_CHIP_LINCROFT) {
-		/*
-		 * MRST firmware currently denies stereo recording requests.
-		 */
-		if (substream->stream == SNDRV_PCM_STREAM_CAPTURE) {
-			runtime->hw.formats = (SNDRV_PCM_FMTBIT_S16 |
-					       SNDRV_PCM_FMTBIT_U16);
-			runtime->hw.channels_max = 1;
-		}
-	}
-	if (intelmaddata->cpu_id == CPU_CHIP_PENWELL) {
-		runtime->hw = snd_intelmad_stream;
-		runtime->hw.rates = SNDRV_PCM_RATE_48000;
-		runtime->hw.rate_min = MAX_RATE;
-		runtime->hw.formats = (SNDRV_PCM_FMTBIT_S24 |
-						SNDRV_PCM_FMTBIT_U24);
-		if (intelmaddata->sstdrv_ops->scard_ops->input_dev_id == AMIC)
-			runtime->hw.channels_max = MAX_CHANNEL_AMIC;
-		else
-			runtime->hw.channels_max = MAX_CHANNEL_DMIC;
-
-	}
-	/* setup the internal datastruture stream pointers based on it being
-	playback or capture stream */
-	stream = kzalloc(sizeof(*stream), GFP_KERNEL);
-	if (!stream)
-		return -ENOMEM;
-	stream->stream_info.str_id = 0;
-	stream->device = type;
-	stream->stream_status = INIT;
-	runtime->private_data = stream;
-	return snd_pcm_hw_constraint_integer(runtime,
-			 SNDRV_PCM_HW_PARAM_PERIODS);
-}
-
-static int snd_intelmad_headset_open(struct snd_pcm_substream *substream)
-{
-	return snd_intelmad_open(substream, SND_SST_DEVICE_HEADSET);
-}
-
-static int snd_intelmad_ihf_open(struct snd_pcm_substream *substream)
-{
-	return snd_intelmad_open(substream, SND_SST_DEVICE_IHF);
-}
-
-static int snd_intelmad_vibra_open(struct snd_pcm_substream *substream)
-{
-	return snd_intelmad_open(substream, SND_SST_DEVICE_VIBRA);
-}
-
-static int snd_intelmad_haptic_open(struct snd_pcm_substream *substream)
-{
-	return snd_intelmad_open(substream, SND_SST_DEVICE_HAPTIC);
-}
-
-static struct snd_pcm_ops snd_intelmad_headset_ops = {
-	.open = snd_intelmad_headset_open,
-	.close = snd_intelmad_close,
-	.ioctl = snd_pcm_lib_ioctl,
-	.hw_params = snd_intelmad_hw_params,
-	.hw_free = snd_intelmad_hw_free,
-	.prepare = snd_intelmad_pcm_prepare,
-	.trigger = snd_intelmad_pcm_trigger,
-	.pointer = snd_intelmad_pcm_pointer,
-};
-
-static struct snd_pcm_ops snd_intelmad_ihf_ops = {
-	.open = snd_intelmad_ihf_open,
-	.close = snd_intelmad_close,
-	.ioctl = snd_pcm_lib_ioctl,
-	.hw_params = snd_intelmad_hw_params,
-	.hw_free = snd_intelmad_hw_free,
-	.prepare = snd_intelmad_pcm_prepare,
-	.trigger = snd_intelmad_pcm_trigger,
-	.pointer = snd_intelmad_pcm_pointer,
-};
-
-static struct snd_pcm_ops snd_intelmad_vibra_ops = {
-	.open = snd_intelmad_vibra_open,
-	.close = snd_intelmad_close,
-	.ioctl = snd_pcm_lib_ioctl,
-	.hw_params = snd_intelmad_hw_params,
-	.hw_free = snd_intelmad_hw_free,
-	.prepare = snd_intelmad_pcm_prepare,
-	.trigger = snd_intelmad_pcm_trigger,
-	.pointer = snd_intelmad_pcm_pointer,
-};
-
-static struct snd_pcm_ops snd_intelmad_haptic_ops = {
-	.open = snd_intelmad_haptic_open,
-	.close = snd_intelmad_close,
-	.ioctl = snd_pcm_lib_ioctl,
-	.hw_params = snd_intelmad_hw_params,
-	.hw_free = snd_intelmad_hw_free,
-	.prepare = snd_intelmad_pcm_prepare,
-	.trigger = snd_intelmad_pcm_trigger,
-	.pointer = snd_intelmad_pcm_pointer,
-};
-
-static struct snd_pcm_ops snd_intelmad_capture_ops = {
-	.open = snd_intelmad_headset_open,
-	.close = snd_intelmad_close,
-	.ioctl = snd_pcm_lib_ioctl,
-	.hw_params = snd_intelmad_hw_params,
-	.hw_free = snd_intelmad_hw_free,
-	.prepare = snd_intelmad_pcm_prepare,
-	.trigger = snd_intelmad_pcm_trigger,
-	.pointer = snd_intelmad_pcm_pointer,
-};
-
-int intelmad_get_mic_bias(void)
-{
-	struct snd_pmic_ops *pmic_ops;
-
-	if (!intelmad_drv || !intelmad_drv->sstdrv_ops)
-		return -ENODEV;
-	pmic_ops = intelmad_drv->sstdrv_ops->scard_ops;
-	if (pmic_ops && pmic_ops->pmic_get_mic_bias)
-		return pmic_ops->pmic_get_mic_bias(intelmad_drv);
-	else
-		return -ENODEV;
-}
-EXPORT_SYMBOL_GPL(intelmad_get_mic_bias);
-
-int intelmad_set_headset_state(int state)
-{
-	struct snd_pmic_ops *pmic_ops;
-
-	if (!intelmad_drv || !intelmad_drv->sstdrv_ops)
-		return -ENODEV;
-	pmic_ops = intelmad_drv->sstdrv_ops->scard_ops;
-	if (pmic_ops && pmic_ops->pmic_set_headset_state)
-		return pmic_ops->pmic_set_headset_state(state);
-	else
-		return -ENODEV;
-}
-EXPORT_SYMBOL_GPL(intelmad_set_headset_state);
-
-void sst_process_mad_jack_detection(struct work_struct *work)
-{
-	u8 interrupt_status;
-	struct mad_jack_msg_wq *mad_jack_detect =
-			container_of(work, struct mad_jack_msg_wq, wq);
-
-	struct snd_intelmad *intelmaddata =
-			mad_jack_detect->intelmaddata;
-
-	if (!intelmaddata)
-		return;
-
-	interrupt_status = mad_jack_detect->intsts;
-	if (intelmaddata->sstdrv_ops && intelmaddata->sstdrv_ops->scard_ops
-			&& intelmaddata->sstdrv_ops->scard_ops->pmic_irq_cb) {
-		intelmaddata->sstdrv_ops->scard_ops->pmic_irq_cb(
-			(void *)intelmaddata, interrupt_status);
-		intelmaddata->sstdrv_ops->scard_ops->pmic_jack_enable();
-	}
-	kfree(mad_jack_detect);
-}
-/**
- * snd_intelmad_intr_handler- interrupt handler
- *
- * @irq :  irq number of the interrupt received
- * @dev: device context
- *
- * This function is called when an interrupt is raised at the sound card
- */
-static irqreturn_t snd_intelmad_intr_handler(int irq, void *dev)
-{
-	struct snd_intelmad *intelmaddata =
-			(struct snd_intelmad *)dev;
-	u8 interrupt_status;
-	struct mad_jack_msg_wq  *mad_jack_msg;
-	memcpy_fromio(&interrupt_status,
-			((void *)(intelmaddata->int_base)),
-			sizeof(u8));
-
-	mad_jack_msg = kzalloc(sizeof(*mad_jack_msg), GFP_ATOMIC);
-	mad_jack_msg->intsts = interrupt_status;
-	mad_jack_msg->intelmaddata = intelmaddata;
-	INIT_WORK(&mad_jack_msg->wq, sst_process_mad_jack_detection);
-	queue_work(intelmaddata->mad_jack_wq, &mad_jack_msg->wq);
-
-	return IRQ_HANDLED;
-}
-
-void sst_mad_send_jack_report(struct snd_jack *jack,
-				int buttonpressevent , int status)
-{
-
-	if (!jack) {
-		pr_debug("MAD error jack empty\n");
-
-	} else {
-		snd_jack_report(jack, status);
-		/* button pressed and released */
-		if (buttonpressevent)
-			snd_jack_report(jack, 0);
-		pr_debug("MAD sending jack report Done !!!\n");
-	}
-}
-
-static int __devinit snd_intelmad_register_irq(
-		struct snd_intelmad *intelmaddata, unsigned int regbase,
-		unsigned int regsize)
-{
-	int ret_val;
-	char *drv_name;
-
-	pr_debug("irq reg regbase 0x%x, regsize 0x%x\n",
-					regbase, regsize);
-	intelmaddata->int_base = ioremap_nocache(regbase, regsize);
-	if (!intelmaddata->int_base)
-		pr_err("Mapping of cache failed\n");
-	pr_debug("irq = 0x%x\n", intelmaddata->irq);
-	if (intelmaddata->cpu_id == CPU_CHIP_PENWELL)
-		drv_name = DRIVER_NAME_MFLD;
-	else
-		drv_name = DRIVER_NAME_MRST;
-	ret_val = request_irq(intelmaddata->irq,
-				snd_intelmad_intr_handler,
-				IRQF_SHARED, drv_name,
-				intelmaddata);
-	if (ret_val)
-		pr_err("cannot register IRQ\n");
-	return ret_val;
-}
-
-static int __devinit snd_intelmad_sst_register(
-			struct snd_intelmad *intelmaddata)
-{
-	int ret_val = 0;
-	struct snd_pmic_ops *intelmad_vendor_ops[MAX_VENDORS] = {
-		&snd_pmic_ops_fs,
-		&snd_pmic_ops_mx,
-		&snd_pmic_ops_nc,
-		&snd_msic_ops
-	};
-
-	struct sc_reg_access vendor_addr = {0x00, 0x00, 0x00};
-
-	if (intelmaddata->cpu_id == CPU_CHIP_LINCROFT) {
-		ret_val = sst_sc_reg_access(&vendor_addr, PMIC_READ, 1);
-		if (ret_val)
-			return ret_val;
-		sst_card_vendor_id = (vendor_addr.value & (MASK2|MASK1|MASK0));
-		pr_debug("original n extrated vendor id = 0x%x %d\n",
-				vendor_addr.value, sst_card_vendor_id);
-		if (sst_card_vendor_id < 0 || sst_card_vendor_id > 2) {
-			pr_err("vendor card not supported!!\n");
-			return -EIO;
-		}
-	} else
-		sst_card_vendor_id = 0x3;
-
-	intelmaddata->sstdrv_ops->module_name = SST_CARD_NAMES;
-	intelmaddata->sstdrv_ops->vendor_id = sst_card_vendor_id;
-	BUG_ON(!intelmad_vendor_ops[sst_card_vendor_id]);
-	intelmaddata->sstdrv_ops->scard_ops =
-			intelmad_vendor_ops[sst_card_vendor_id];
-
-	if (intelmaddata->cpu_id == CPU_CHIP_PENWELL) {
-		intelmaddata->sstdrv_ops->scard_ops->pb_on = 0;
-		intelmaddata->sstdrv_ops->scard_ops->cap_on = 0;
-		intelmaddata->sstdrv_ops->scard_ops->input_dev_id = DMIC;
-		intelmaddata->sstdrv_ops->scard_ops->output_dev_id =
-							STEREO_HEADPHONE;
-		intelmaddata->sstdrv_ops->scard_ops->lineout_dev_id = NONE;
-	}
-
-	/* registering with SST driver to get access to SST APIs to use */
-	ret_val = register_sst_card(intelmaddata->sstdrv_ops);
-	if (ret_val) {
-		pr_err("sst card registration failed\n");
-		return ret_val;
-	}
-	sst_card_vendor_id = intelmaddata->sstdrv_ops->vendor_id;
-	intelmaddata->pmic_status = PMIC_UNINIT;
-	return ret_val;
-}
-
-static void snd_intelmad_page_free(struct snd_pcm *pcm)
-{
-	snd_pcm_lib_preallocate_free_for_all(pcm);
-}
-/* Driver Init/exit functionalities */
-/**
- * snd_intelmad_pcm_new - to setup pcm for the card
- *
- * @card:  pointer to the sound card structure
- * @intelmaddata: pointer to internal context
- * @pb: playback count for this card
- * @cap: capture count for this card
- * @index: device index
- *
- * This function is called from probe function to set up pcm params
- * and functions
- */
-static int __devinit snd_intelmad_pcm_new(struct snd_card *card,
-			struct snd_intelmad *intelmaddata,
-			unsigned int pb, unsigned int cap, unsigned int index)
-{
-	int ret_val = 0;
-	struct snd_pcm *pcm;
-	char name[32] = INTEL_MAD;
-	struct snd_pcm_ops *pb_ops = NULL, *cap_ops = NULL;
-
-	pr_debug("called for pb %d, cp %d, idx %d\n", pb, cap, index);
-	ret_val = snd_pcm_new(card, name, index, pb, cap, &pcm);
-	if (ret_val)
-		return ret_val;
-	/* setup the ops for playback and capture streams */
-	switch (index) {
-	case 0:
-		pb_ops = &snd_intelmad_headset_ops;
-		cap_ops = &snd_intelmad_capture_ops;
-		break;
-	case 1:
-		pb_ops = &snd_intelmad_ihf_ops;
-		cap_ops = &snd_intelmad_capture_ops;
-		break;
-	case 2:
-		pb_ops = &snd_intelmad_vibra_ops;
-		cap_ops = &snd_intelmad_capture_ops;
-		break;
-	case 3:
-		pb_ops = &snd_intelmad_haptic_ops;
-		cap_ops = &snd_intelmad_capture_ops;
-		break;
-	}
-	if (pb)
-		snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK, pb_ops);
-	if (cap)
-		snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE, cap_ops);
-	/* setup private data which can be retrieved when required */
-	pcm->private_data = intelmaddata;
-	pcm->private_free = snd_intelmad_page_free;
-	pcm->info_flags = 0;
-	strncpy(pcm->name, card->shortname, strlen(card->shortname));
-	/* allocate dma pages for ALSA stream operations */
-	snd_pcm_lib_preallocate_pages_for_all(pcm,
-			SNDRV_DMA_TYPE_CONTINUOUS,
-			snd_dma_continuous_data(GFP_KERNEL),
-			MIN_BUFFER, MAX_BUFFER);
-	return ret_val;
-}
-
-static int __devinit snd_intelmad_pcm(struct snd_card *card,
-				struct snd_intelmad *intelmaddata)
-{
-	int ret_val = 0;
-
-	WARN_ON(!card);
-	WARN_ON(!intelmaddata);
-	pr_debug("snd_intelmad_pcm called\n");
-	ret_val = snd_intelmad_pcm_new(card, intelmaddata, 1, 1, 0);
-	if (intelmaddata->cpu_id == CPU_CHIP_LINCROFT)
-		return ret_val;
-	ret_val = snd_intelmad_pcm_new(card, intelmaddata, 1, 0, 1);
-	if (ret_val)
-		return ret_val;
-	ret_val = snd_intelmad_pcm_new(card, intelmaddata, 1, 0, 2);
-	if (ret_val)
-		return ret_val;
-	return snd_intelmad_pcm_new(card, intelmaddata, 1, 0, 3);
-}
-
-/**
- * snd_intelmad_jack- to setup jack settings of the card
- *
- * @intelmaddata: pointer to internal context
- *
- * This function is called send jack events
- */
-static int snd_intelmad_jack(struct snd_intelmad *intelmaddata)
-{
-	struct snd_jack *jack;
-	int retval;
-
-	pr_debug("snd_intelmad_jack called\n");
-	jack = &intelmaddata->jack[0].jack;
-	snd_jack_set_key(jack, SND_JACK_BTN_0, KEY_PHONE);
-	retval = snd_jack_new(intelmaddata->card, "Intel(R) MID Audio Jack",
-		SND_JACK_HEADPHONE | SND_JACK_HEADSET |
-		SW_JACK_PHYSICAL_INSERT | SND_JACK_BTN_0
-		| SND_JACK_BTN_1, &jack);
-	pr_debug("snd_intelmad_jack called\n");
-	if (retval < 0)
-		return retval;
-	snd_jack_report(jack, 0);
-
-	jack->private_data = jack;
-	intelmaddata->jack[0].jack = *jack;
-
-	return retval;
-}
-
-/**
- * snd_intelmad_mixer- to setup mixer settings of the card
- *
- * @intelmaddata: pointer to internal context
- *
- * This function is called from probe function to set up mixer controls
- */
-static int __devinit snd_intelmad_mixer(struct snd_intelmad *intelmaddata)
-{
-	struct snd_card *card;
-	unsigned int idx;
-	int ret_val = 0, max_controls = 0;
-	char *mixername = "IntelMAD Controls";
-	struct snd_kcontrol_new *controls;
-
-	WARN_ON(!intelmaddata);
-
-	card = intelmaddata->card;
-	strncpy(card->mixername, mixername, sizeof(card->mixername)-1);
-	/* add all widget controls and expose the same */
-	if (intelmaddata->cpu_id == CPU_CHIP_PENWELL) {
-		max_controls = MAX_CTRL_MFLD;
-		controls = snd_intelmad_controls_mfld;
-	} else {
-		max_controls = MAX_CTRL_MRST;
-		controls = snd_intelmad_controls_mrst;
-	}
-	for (idx = 0; idx < max_controls; idx++) {
-		ret_val = snd_ctl_add(card,
-				snd_ctl_new1(&controls[idx],
-				intelmaddata));
-		pr_debug("mixer[idx]=%d added\n", idx);
-		if (ret_val) {
-			pr_err("in adding of control index = %d\n", idx);
-			break;
-		}
-	}
-	return ret_val;
-}
-
-static int snd_intelmad_dev_free(struct snd_device *device)
-{
-	struct snd_intelmad *intelmaddata;
-
-	WARN_ON(!device);
-
-	intelmaddata = device->device_data;
-
-	pr_debug("snd_intelmad_dev_free called\n");
-	unregister_sst_card(intelmaddata->sstdrv_ops);
-
-	/* free allocated memory for internal context */
-	destroy_workqueue(intelmaddata->mad_jack_wq);
-	device->device_data = NULL;
-	kfree(intelmaddata->sstdrv_ops);
-	kfree(intelmaddata);
-
-	return 0;
-}
-
-static int __devinit snd_intelmad_create(
-		struct snd_intelmad *intelmaddata,
-		struct snd_card *card)
-{
-	int ret_val;
-	static struct snd_device_ops ops = {
-		.dev_free =	snd_intelmad_dev_free,
-	};
-
-	WARN_ON(!intelmaddata);
-	WARN_ON(!card);
-	/* ALSA api to register for the device */
-	ret_val = snd_device_new(card, SNDRV_DEV_LOWLEVEL, intelmaddata, &ops);
-	return ret_val;
-}
-
-/**
-* snd_intelmad_probe- function registred for init
-* @pdev :  pointer to the device struture
-* This function is called when the device is initialized
-*/
-int __devinit snd_intelmad_probe(struct platform_device *pdev)
-{
-	struct snd_card *card;
-	int ret_val;
-	struct snd_intelmad *intelmaddata;
-	const struct platform_device_id *id = platform_get_device_id(pdev);
-	struct snd_intelmad_probe_info *info = (void *)id->driver_data;
-
-	pr_debug("probe for %s cpu_id %d\n", pdev->name, info->cpu_id);
-	pr_debug("rq_chache %x of size %x\n", info->irq_cache, info->size);
-	if (!strcmp(pdev->name, DRIVER_NAME_MRST))
-		pr_debug("detected MRST\n");
-	else if (!strcmp(pdev->name, DRIVER_NAME_MFLD))
-		pr_debug("detected MFLD\n");
-	else {
-		pr_err("detected unknown device abort!!\n");
-		return -EIO;
-	}
-	if ((info->cpu_id < CPU_CHIP_LINCROFT) ||
-				(info->cpu_id > CPU_CHIP_PENWELL)) {
-		pr_err("detected unknown cpu_id abort!!\n");
-		return -EIO;
-	}
-	/* allocate memory for saving internal context and working */
-	intelmaddata = kzalloc(sizeof(*intelmaddata), GFP_KERNEL);
-	if (!intelmaddata) {
-		pr_debug("mem alloctn fail\n");
-		return -ENOMEM;
-	}
-	intelmad_drv = intelmaddata;
-
-	/* allocate memory for LPE API set */
-	intelmaddata->sstdrv_ops = kzalloc(sizeof(struct intel_sst_card_ops),
-					GFP_KERNEL);
-	if (!intelmaddata->sstdrv_ops) {
-		pr_err("mem allocation for ops fail\n");
-		kfree(intelmaddata);
-		return -ENOMEM;
-	}
-
-	intelmaddata->cpu_id = info->cpu_id;
-	/* create a card instance with ALSA framework */
-	ret_val = snd_card_create(card_index, card_id, THIS_MODULE, 0, &card);
-	if (ret_val) {
-		pr_err("snd_card_create fail\n");
-		goto free_allocs;
-	}
-
-	intelmaddata->pdev = pdev;
-	intelmaddata->irq = platform_get_irq(pdev, 0);
-	platform_set_drvdata(pdev, intelmaddata);
-	intelmaddata->card = card;
-	intelmaddata->card_id = card_id;
-	intelmaddata->card_index = card_index;
-	intelmaddata->master_mute = UNMUTE;
-	intelmaddata->playback_cnt =  intelmaddata->capture_cnt = 0;
-	strncpy(card->driver, INTEL_MAD, strlen(INTEL_MAD));
-	strncpy(card->shortname, INTEL_MAD, strlen(INTEL_MAD));
-
-	intelmaddata->sstdrv_ops->module_name = SST_CARD_NAMES;
-	/* registering with LPE driver to get access to SST APIs to use */
-	ret_val = snd_intelmad_sst_register(intelmaddata);
-	if (ret_val) {
-		pr_err("snd_intelmad_sst_register failed\n");
-		goto set_null_data;
-	}
-
-	intelmaddata->pmic_status = PMIC_INIT;
-
-	ret_val = snd_intelmad_pcm(card, intelmaddata);
-	if (ret_val) {
-		pr_err("snd_intelmad_pcm failed\n");
-		goto free_sst;
-	}
-
-	ret_val = snd_intelmad_mixer(intelmaddata);
-	if (ret_val) {
-		pr_err("snd_intelmad_mixer failed\n");
-		goto free_card;
-	}
-
-	ret_val = snd_intelmad_jack(intelmaddata);
-	if (ret_val) {
-		pr_err("snd_intelmad_jack failed\n");
-		goto free_card;
-	}
-	intelmaddata->adc_address = mid_initialize_adc();
-
-	/*create work queue for jack interrupt*/
-	INIT_WORK(&intelmaddata->mad_jack_msg.wq,
-		sst_process_mad_jack_detection);
-
-	intelmaddata->mad_jack_wq = create_workqueue("sst_mad_jack_wq");
-	if (!intelmaddata->mad_jack_wq)
-		goto free_card;
-
-	ret_val = snd_intelmad_register_irq(intelmaddata,
-					info->irq_cache, info->size);
-	if (ret_val) {
-		pr_err("snd_intelmad_register_irq fail\n");
-		goto free_mad_jack_wq;
-	}
-
-	/* internal function call to register device with ALSA */
-	ret_val = snd_intelmad_create(intelmaddata, card);
-	if (ret_val) {
-		pr_err("snd_intelmad_create failed\n");
-		goto set_pvt_data;
-	}
-	card->private_data = &intelmaddata;
-	snd_card_set_dev(card, &pdev->dev);
-	ret_val = snd_card_register(card);
-	if (ret_val) {
-		pr_err("snd_card_register failed\n");
-		goto set_pvt_data;
-	}
-	if (pdev->dev.platform_data) {
-		int gpio_amp = *(int *)pdev->dev.platform_data;
-		if (gpio_request_one(gpio_amp, GPIOF_OUT_INIT_LOW, "amp power"))
-			gpio_amp = 0;
-		intelmaddata->sstdrv_ops->scard_ops->gpio_amp = gpio_amp;
-	}
-
-	pr_debug("snd_intelmad_probe complete\n");
-	return ret_val;
-
-set_pvt_data:
-	card->private_data = NULL;
-free_mad_jack_wq:
-	destroy_workqueue(intelmaddata->mad_jack_wq);
-free_card:
-	snd_card_free(intelmaddata->card);
-free_sst:
-	unregister_sst_card(intelmaddata->sstdrv_ops);
-set_null_data:
-	platform_set_drvdata(pdev, NULL);
-free_allocs:
-	pr_err("probe failed\n");
-	snd_card_free(card);
-	kfree(intelmaddata->sstdrv_ops);
-	kfree(intelmaddata);
-	return ret_val;
-}
-
-
-static int snd_intelmad_remove(struct platform_device *pdev)
-{
-	struct snd_intelmad *intelmaddata = platform_get_drvdata(pdev);
-
-	if (intelmaddata) {
-		if (intelmaddata->sstdrv_ops->scard_ops->gpio_amp)
-			gpio_free(intelmaddata->sstdrv_ops->scard_ops->gpio_amp);
-		free_irq(intelmaddata->irq, intelmaddata);
-		snd_card_free(intelmaddata->card);
-	}
-	intelmad_drv = NULL;
-	platform_set_drvdata(pdev, NULL);
-	return 0;
-}
-
-/*********************************************************************
- *		Driver initialization and exit
- *********************************************************************/
-static const struct platform_device_id snd_intelmad_ids[] = {
-	{DRIVER_NAME_MRST, INFO(CPU_CHIP_LINCROFT, AUDINT_BASE, 1)},
-	{DRIVER_NAME_MFLD, INFO(CPU_CHIP_PENWELL, 0xFFFF7FCD, 1)},
-	{"", 0},
-
-};
-
-static struct platform_driver snd_intelmad_driver = {
-	.driver = {
-		.owner = THIS_MODULE,
-		.name = "intel_mid_sound_card",
-	},
-	.id_table = snd_intelmad_ids,
-	.probe = snd_intelmad_probe,
-	.remove = __devexit_p(snd_intelmad_remove),
-};
-
-/*
- * alsa_card_intelmad_init- driver init function
- *
- * This function is called when driver module is inserted
- */
-static int __init alsa_card_intelmad_init(void)
-{
-	pr_debug("mad_init called\n");
-	return platform_driver_register(&snd_intelmad_driver);
-}
-
-/**
- * alsa_card_intelmad_exit- driver exit function
- *
- * This function is called when driver module is removed
- */
-static void __exit alsa_card_intelmad_exit(void)
-{
-	pr_debug("mad_exit called\n");
-	return platform_driver_unregister(&snd_intelmad_driver);
-}
-
-module_init(alsa_card_intelmad_init)
-module_exit(alsa_card_intelmad_exit)
-
diff --git a/drivers/staging/intel_sst/intelmid.h b/drivers/staging/intel_sst/intelmid.h
deleted file mode 100644
index 14a7ba078b7c..000000000000
--- a/drivers/staging/intel_sst/intelmid.h
+++ /dev/null
@@ -1,209 +0,0 @@
-/*
- *  intelmid.h - Intel Sound card driver for MID
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Harsha Priya <priya.harsha@intel.com>
- *		Vinod Koul <vinod.koul@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *  ALSA driver header for Intel MAD chipset
- */
-#ifndef __INTELMID_H
-#define __INTELMID_H
-
-#include <linux/time.h>
-#include <sound/jack.h>
-
-#define DRIVER_NAME_MFLD "msic_audio"
-#define DRIVER_NAME_MRST "pmic_audio"
-#define DRIVER_NAME "intelmid_audio"
-#define PMIC_SOUND_IRQ_TYPE_MASK (1 << 15)
-#define AUDINT_BASE (0xFFFFEFF8 + (6 * sizeof(u8)))
-#define REG_IRQ
-/* values #defined   */
-/* will differ for different hw - to be taken from config  */
-#define MAX_DEVICES		1
-#define MIN_RATE		8000
-#define MAX_RATE		48000
-#define MAX_BUFFER		(800*1024) /* for PCM */
-#define MIN_BUFFER		(800*1024)
-#define MAX_PERIODS		(1024*2)
-#define MIN_PERIODS		2
-#define MAX_PERIOD_BYTES MAX_BUFFER
-#define MIN_PERIOD_BYTES 32
-/*#define MIN_PERIOD_BYTES 160*/
-#define MAX_MUTE		1
-#define MIN_MUTE		0
-#define MONO_CNTL		1
-#define STEREO_CNTL		2
-#define MIN_CHANNEL		1
-#define MAX_CHANNEL_AMIC	2
-#define MAX_CHANNEL_DMIC	5
-#define FIFO_SIZE		0 /* fifo not being used */
-#define INTEL_MAD		"Intel MAD"
-#define MAX_CTRL_MRST		8
-#define MAX_CTRL_MFLD		7
-#define MAX_CTRL		8
-#define MAX_VENDORS		4
-/* TODO +6 db */
-#define MAX_VOL		64
-/* TODO -57 db */
-#define MIN_VOL		0
-#define PLAYBACK_COUNT  1
-#define CAPTURE_COUNT	1
-#define ADC_ONE_LSB_MULTIPLIER 2346
-
-#define MID_JACK_HS_LONG_PRESS SND_JACK_BTN_0
-#define MID_JACK_HS_SHORT_PRESS SND_JACK_BTN_1
-
-extern int	sst_card_vendor_id;
-
-struct mad_jack {
-	struct snd_jack jack;
-	int jack_status;
-	int jack_dev_state;
-	struct timeval buttonpressed;
-	struct timeval  buttonreleased;
-};
-
-struct mad_jack_msg_wq {
-	u8 intsts;
-	struct snd_intelmad *intelmaddata;
-	struct work_struct	wq;
-
-};
-
-struct snd_intelmad_probe_info {
-	unsigned int cpu_id;
-	unsigned int irq_cache;
-	unsigned int size;
-};
-
-/**
- * struct snd_intelmad - intelmad driver structure
- *
- * @card: ptr to the card details
- * @card_index: sound card index
- * @card_id: sound card id detected
- * @sstdrv_ops: ptr to sst driver ops
- * @pdev: ptr to platform device
- * @irq: interrupt number detected
- * @pmic_status: Device status of sound card
- * @int_base: ptr to MMIO interrupt region
- * @output_sel: device selected as o/p
- * @input_sel: device selected as i/p
- * @master_mute: master mute status
- * @jack: jack status
- * @playback_cnt: active pb streams
- * @capture_cnt: active cp streams
- * @mad_jack_msg: wq struct for jack interrupt processing
- * @mad_jack_wq: wq for jack interrupt processing
- * @jack_prev_state: Previos state of jack detected
- * @cpu_id: current cpu id loaded for
- */
-struct snd_intelmad {
-	struct snd_card	*card; /* ptr to the card details */
-	int		card_index;/*  card index  */
-	char		*card_id; /* card id */
-	struct intel_sst_card_ops *sstdrv_ops;/* ptr to sst driver ops */
-	struct platform_device *pdev;
-	int irq;
-	int pmic_status;
-	void __iomem *int_base;
-	int output_sel;
-	int input_sel;
-	int lineout_sel;
-	int master_mute;
-	struct mad_jack jack[4];
-	int playback_cnt;
-	int capture_cnt;
-	u16 adc_address;
-	struct mad_jack_msg_wq  mad_jack_msg;
-	struct workqueue_struct *mad_jack_wq;
-	u8 jack_prev_state;
-	unsigned int cpu_id;
-};
-
-struct snd_control_val {
-	int	playback_vol_max;
-	int	playback_vol_min;
-	int	capture_vol_max;
-	int	capture_vol_min;
-	int	master_vol_max;
-	int	master_vol_min;
-};
-
-struct mad_stream_pvt {
-	int			stream_status;
-	int			stream_ops;
-	struct snd_pcm_substream *substream;
-	struct pcm_stream_info stream_info;
-	ssize_t		dbg_cum_bytes;
-	enum snd_sst_device_type device;
-};
-
-enum mad_drv_status {
-    INIT = 1,
-    STARTED,
-    RUNNING,
-    PAUSED,
-    DROPPED,
-};
-
-enum mad_pmic_status {
-	PMIC_UNINIT = 1,
-	PMIC_INIT,
-};
-enum _widget_ctrl {
-	OUTPUT_SEL = 1,
-	INPUT_SEL,
-	PLAYBACK_VOL,
-	PLAYBACK_MUTE,
-	CAPTURE_VOL,
-	CAPTURE_MUTE,
-	MASTER_VOL,
-	MASTER_MUTE
-};
-enum _widget_ctrl_mfld {
-	LINEOUT_SEL_MFLD = 3,
-};
-enum hw_chs {
-	HW_CH0 = 0,
-	HW_CH1,
-	HW_CH2,
-	HW_CH3
-};
-
-void period_elapsed(void *mad_substream);
-int snd_intelmad_alloc_stream(struct snd_pcm_substream *substream);
-int snd_intelmad_init_stream(struct snd_pcm_substream *substream);
-
-int sst_sc_reg_access(struct sc_reg_access *sc_access,
-					int type, int num_val);
-#define CPU_CHIP_LINCROFT       1 /* System running lincroft */
-#define CPU_CHIP_PENWELL        2 /* System running penwell */
-
-extern struct snd_control_val intelmad_ctrl_val[];
-extern struct snd_kcontrol_new snd_intelmad_controls_mrst[];
-extern struct snd_kcontrol_new snd_intelmad_controls_mfld[];
-extern struct snd_pmic_ops *intelmad_vendor_ops[];
-void sst_mad_send_jack_report(struct snd_jack *jack,
-			int buttonpressevent , int status);
-
-#endif /* __INTELMID_H */
diff --git a/drivers/staging/intel_sst/intelmid_adc_control.h b/drivers/staging/intel_sst/intelmid_adc_control.h
deleted file mode 100644
index 65d5c3988762..000000000000
--- a/drivers/staging/intel_sst/intelmid_adc_control.h
+++ /dev/null
@@ -1,193 +0,0 @@
-#ifndef __INTELMID_ADC_CONTROL_H__
-#define __INTELMID_ADC_CONTROL_H_
-/*
- *  intelmid_adc_control.h - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corporation
- *  Authors:	R Durgadadoss <r.durgadoss@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  Common private ADC declarations for SST
- */
-
-
-#define MSIC_ADC1CNTL1		0x1C0
-#define MSIC_ADC_ENBL		0x10
-#define MSIC_ADC_START		0x08
-
-#define MSIC_ADC1CNTL3		0x1C2
-#define MSIC_ADCTHERM_ENBL	0x04
-#define MSIC_ADCRRDATA_ENBL	0x05
-
-#define MSIC_STOPBIT_MASK	16
-#define MSIC_ADCTHERM_MASK	4
-
-#define ADC_CHANLS_MAX		15 /* Number of ADC channels */
-#define ADC_LOOP_MAX		(ADC_CHANLS_MAX - 1)
-
-/* ADC channel code values */
-#define AUDIO_DETECT_CODE	0x06
-
-/* ADC base addresses */
-#define ADC_CHNL_START_ADDR	0x1C5	/* increments by 1 */
-#define ADC_DATA_START_ADDR     0x1D4   /* increments by 2 */
-
-
-/**
- * configure_adc - enables/disables the ADC for conversion
- * @val: zero: disables the ADC non-zero:enables the ADC
- *
- * Enable/Disable the ADC depending on the argument
- *
- * Can sleep
- */
-static inline int configure_adc(int val)
-{
-	int ret;
-	struct sc_reg_access sc_access = {0,};
-
-
-	sc_access.reg_addr = MSIC_ADC1CNTL1;
-	ret = sst_sc_reg_access(&sc_access, PMIC_READ, 1);
-	if (ret)
-		return ret;
-
-	if (val)
-		/* Enable and start the ADC */
-		sc_access.value |= (MSIC_ADC_ENBL | MSIC_ADC_START);
-	else
-		/* Just stop the ADC */
-		sc_access.value &= (~MSIC_ADC_START);
-	sc_access.reg_addr = MSIC_ADC1CNTL1;
-	return sst_sc_reg_access(&sc_access, PMIC_WRITE, 1);
-}
-
-/**
- * reset_stopbit - sets the stop bit to 0 on the given channel
- * @addr: address of the channel
- *
- * Can sleep
- */
-static inline int reset_stopbit(uint16_t addr)
-{
-	int ret;
-	struct sc_reg_access sc_access = {0,};
-	sc_access.reg_addr = addr;
-	ret = sst_sc_reg_access(&sc_access, PMIC_READ, 1);
-	if (ret)
-		return ret;
-	/* Set the stop bit to zero */
-	sc_access.reg_addr = addr;
-	sc_access.value = (sc_access.value) & 0xEF;
-	return sst_sc_reg_access(&sc_access, PMIC_WRITE, 1);
-}
-
-/**
- * find_free_channel - finds an empty channel for conversion
- *
- * If the ADC is not enabled then start using 0th channel
- * itself. Otherwise find an empty channel by looking for a
- * channel in which the stopbit is set to 1. returns the index
- * of the first free channel if succeeds or an error code.
- *
- * Context: can sleep
- *
- */
-static inline int find_free_channel(void)
-{
-	int ret;
-	int i;
-
-	struct sc_reg_access sc_access = {0,};
-
-	/* check whether ADC is enabled */
-	sc_access.reg_addr = MSIC_ADC1CNTL1;
-	ret = sst_sc_reg_access(&sc_access, PMIC_READ, 1);
-	if (ret)
-		return ret;
-
-	if ((sc_access.value & MSIC_ADC_ENBL) == 0)
-		return 0;
-
-	/* ADC is already enabled; Looking for an empty channel */
-	for (i = 0; i < ADC_CHANLS_MAX; i++) {
-
-		sc_access.reg_addr = ADC_CHNL_START_ADDR + i;
-		ret = sst_sc_reg_access(&sc_access, PMIC_READ, 1);
-		if (ret)
-			return ret;
-
-		if (sc_access.value & MSIC_STOPBIT_MASK) {
-			ret = i;
-			break;
-		}
-	}
-	return (ret > ADC_LOOP_MAX) ? (-EINVAL) : ret;
-}
-
-/**
- * mid_initialize_adc - initializing the ADC
- * @dev: our device structure
- *
- * Initialize the ADC for reading thermistor values. Can sleep.
- */
-static inline int mid_initialize_adc(void)
-{
-	int base_addr, chnl_addr;
-	int ret;
-	static int channel_index;
-	struct sc_reg_access sc_access = {0,};
-
-	/* Index of the first channel in which the stop bit is set */
-	channel_index = find_free_channel();
-	if (channel_index < 0) {
-		pr_err("No free ADC channels");
-		return channel_index;
-	}
-
-	base_addr = ADC_CHNL_START_ADDR + channel_index;
-
-	if (!(channel_index == 0 || channel_index == ADC_LOOP_MAX)) {
-		/* Reset stop bit for channels other than 0 and 12 */
-		ret = reset_stopbit(base_addr);
-		if (ret)
-			return ret;
-
-		/* Index of the first free channel */
-		base_addr++;
-		channel_index++;
-	}
-
-	/* Since this is the last channel, set the stop bit
-	   to 1 by ORing the DIE_SENSOR_CODE with 0x10 */
-	sc_access.reg_addr = base_addr;
-	sc_access.value = AUDIO_DETECT_CODE | 0x10;
-	ret = sst_sc_reg_access(&sc_access, PMIC_WRITE, 1);
-	if (ret) {
-		pr_err("unable to enable ADC");
-		return ret;
-	}
-
-	chnl_addr = ADC_DATA_START_ADDR + 2 * channel_index;
-	pr_debug("mid_initialize : %x", chnl_addr);
-	configure_adc(1);
-	return chnl_addr;
-}
-#endif
-
diff --git a/drivers/staging/intel_sst/intelmid_ctrl.c b/drivers/staging/intel_sst/intelmid_ctrl.c
deleted file mode 100644
index 19ec474b362a..000000000000
--- a/drivers/staging/intel_sst/intelmid_ctrl.c
+++ /dev/null
@@ -1,921 +0,0 @@
-/*
- *  intelmid_ctrl.c - Intel Sound card driver for MID
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Harsha Priya <priya.harsha@intel.com>
- *		Vinod Koul <vinod.koul@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *  ALSA driver handling mixer controls for Intel MAD chipset
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <sound/core.h>
-#include <sound/control.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intelmid_snd_control.h"
-#include "intelmid.h"
-
-#define HW_CH_BASE 4
-
-
-#define HW_CH_0	"Hw1"
-#define HW_CH_1	"Hw2"
-#define HW_CH_2	"Hw3"
-#define HW_CH_3	"Hw4"
-
-static char *router_dmics[] = {	"DMIC1",
-				"DMIC2",
-				"DMIC3",
-				"DMIC4",
-				"DMIC5",
-				"DMIC6"
-				};
-
-static char *out_names_mrst[] = {"Headphones",
-				"Internal speakers"};
-static char *in_names_mrst[] = {"AMIC",
-				"DMIC",
-				"HS_MIC"};
-static char *line_out_names_mfld[] = {"Headset",
-				"IHF    ",
-				"Vibra1 ",
-				"Vibra2 ",
-				"NONE   "};
-static char *out_names_mfld[] = {"Headset ",
-				"EarPiece  "};
-static char *in_names_mfld[] = {"AMIC",
-				"DMIC"};
-
-struct snd_control_val intelmad_ctrl_val[MAX_VENDORS] = {
-	{
-		.playback_vol_max = 63,
-		.playback_vol_min = 0,
-		.capture_vol_max = 63,
-		.capture_vol_min = 0,
-	},
-	{
-		.playback_vol_max = 0,
-		.playback_vol_min = -31,
-		.capture_vol_max = 0,
-		.capture_vol_min = -20,
-	},
-	{
-		.playback_vol_max = 0,
-		.playback_vol_min = -31,
-		.capture_vol_max = 0,
-		.capture_vol_min = -31,
-		.master_vol_max = 0,
-		.master_vol_min = -126,
-	},
-};
-
-/* control path functionalities */
-
-static inline int snd_intelmad_volume_info(struct snd_ctl_elem_info *uinfo,
-					int control_type, int max, int min)
-{
-	WARN_ON(!uinfo);
-
-	uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
-	uinfo->count = control_type;
-	uinfo->value.integer.min = min;
-	uinfo->value.integer.max = max;
-	return 0;
-}
-
-/**
-* snd_intelmad_mute_info - provides information about the mute controls
-*
-* @kcontrol:	pointer to the control
-* @uinfo:	pointer to the structure where the control's info need
-*		to be filled
-*
-* This function is called when a mixer application requests for control's info
-*/
-static int snd_intelmad_mute_info(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_info *uinfo)
-{
-	WARN_ON(!uinfo);
-	WARN_ON(!kcontrol);
-
-	/* set up the mute as a boolean mono control with min-max values */
-	uinfo->type = SNDRV_CTL_ELEM_TYPE_BOOLEAN;
-	uinfo->count = MONO_CNTL;
-	uinfo->value.integer.min = MIN_MUTE;
-	uinfo->value.integer.max = MAX_MUTE;
-	return 0;
-}
-
-/**
-* snd_intelmad_capture_volume_info - provides info about the volume control
-*
-* @kcontrol:	pointer to the control
-* @uinfo:	pointer to the structure where the control's info need
-*		to be filled
-*
-* This function is called when a mixer application requests for control's info
-*/
-static int snd_intelmad_capture_volume_info(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_info *uinfo)
-{
-	snd_intelmad_volume_info(uinfo, MONO_CNTL,
-		intelmad_ctrl_val[sst_card_vendor_id].capture_vol_max,
-		intelmad_ctrl_val[sst_card_vendor_id].capture_vol_min);
-	return 0;
-}
-
-/**
-* snd_intelmad_playback_volume_info - provides info about the volume control
-*
-* @kcontrol:	pointer to the control
-* @uinfo:	pointer to the structure where the control's info need
-*		to be filled
-*
-* This function is called when a mixer application requests for control's info
-*/
-static int snd_intelmad_playback_volume_info(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_info *uinfo)
-{
-	snd_intelmad_volume_info(uinfo, STEREO_CNTL,
-		intelmad_ctrl_val[sst_card_vendor_id].playback_vol_max,
-		intelmad_ctrl_val[sst_card_vendor_id].playback_vol_min);
-	return 0;
-}
-
-static int snd_intelmad_master_volume_info(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_info *uinfo)
-{
-	snd_intelmad_volume_info(uinfo, STEREO_CNTL,
-		intelmad_ctrl_val[sst_card_vendor_id].master_vol_max,
-		intelmad_ctrl_val[sst_card_vendor_id].master_vol_min);
-	return 0;
-}
-
-/**
-* snd_intelmad_device_info_mrst - provides information about the devices available
-*
-* @kcontrol:	pointer to the control
-* @uinfo:	pointer to the structure where the devices's info need
-*		to be filled
-*
-* This function is called when a mixer application requests for device's info
-*/
-static int snd_intelmad_device_info_mrst(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_info *uinfo)
-{
-
-	WARN_ON(!kcontrol);
-	WARN_ON(!uinfo);
-
-	/* setup device select as drop down controls with different values */
-	if (kcontrol->id.numid == OUTPUT_SEL)
-		uinfo->value.enumerated.items = ARRAY_SIZE(out_names_mrst);
-	else
-		uinfo->value.enumerated.items = ARRAY_SIZE(in_names_mrst);
-	uinfo->count = MONO_CNTL;
-	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
-
-	if (uinfo->value.enumerated.item >= uinfo->value.enumerated.items)
-		uinfo->value.enumerated.item = 1;
-	if (kcontrol->id.numid == OUTPUT_SEL)
-		strncpy(uinfo->value.enumerated.name,
-			out_names_mrst[uinfo->value.enumerated.item],
-			sizeof(uinfo->value.enumerated.name)-1);
-	else
-		strncpy(uinfo->value.enumerated.name,
-			in_names_mrst[uinfo->value.enumerated.item],
-			sizeof(uinfo->value.enumerated.name)-1);
-	return 0;
-}
-
-static int snd_intelmad_device_info_mfld(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_info *uinfo)
-{
-	struct snd_pmic_ops *scard_ops;
-	struct snd_intelmad *intelmaddata;
-
-	WARN_ON(!kcontrol);
-	WARN_ON(!uinfo);
-
-	intelmaddata = kcontrol->private_data;
-
-	WARN_ON(!intelmaddata->sstdrv_ops);
-
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-	/* setup device select as drop down controls with different values */
-	if (kcontrol->id.numid == OUTPUT_SEL)
-		uinfo->value.enumerated.items = ARRAY_SIZE(out_names_mfld);
-	else if (kcontrol->id.numid == INPUT_SEL)
-		uinfo->value.enumerated.items = ARRAY_SIZE(in_names_mfld);
-	else if (kcontrol->id.numid == LINEOUT_SEL_MFLD) {
-		uinfo->value.enumerated.items = ARRAY_SIZE(line_out_names_mfld);
-		scard_ops->line_out_names_cnt = uinfo->value.enumerated.items;
-	} else
-		return -EINVAL;
-	uinfo->count = MONO_CNTL;
-	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
-
-	if (uinfo->value.enumerated.item >= uinfo->value.enumerated.items)
-		uinfo->value.enumerated.item = 1;
-	if (kcontrol->id.numid == OUTPUT_SEL)
-		strncpy(uinfo->value.enumerated.name,
-			out_names_mfld[uinfo->value.enumerated.item],
-			sizeof(uinfo->value.enumerated.name)-1);
-	else if (kcontrol->id.numid == INPUT_SEL)
-		strncpy(uinfo->value.enumerated.name,
-			in_names_mfld[uinfo->value.enumerated.item],
-			sizeof(uinfo->value.enumerated.name)-1);
-	else if (kcontrol->id.numid == LINEOUT_SEL_MFLD)
-		strncpy(uinfo->value.enumerated.name,
-			line_out_names_mfld[uinfo->value.enumerated.item],
-			sizeof(uinfo->value.enumerated.name)-1);
-	else
-		return -EINVAL;
-	return 0;
-}
-
-/**
-* snd_intelmad_volume_get - gets the current volume for the control
-*
-* @kcontrol:	pointer to the control
-* @uval:	pointer to the structure where the control's info need
-*		to be filled
-*
-* This function is called when .get function of a control is invoked from app
-*/
-static int snd_intelmad_volume_get(struct snd_kcontrol *kcontrol,
-				struct snd_ctl_elem_value *uval)
-{
-	int ret_val = 0, cntl_list[2] = {0,};
-	int value = 0;
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-
-	pr_debug("snd_intelmad_volume_get called\n");
-
-	WARN_ON(!uval);
-	WARN_ON(!kcontrol);
-
-	intelmaddata = kcontrol->private_data;
-
-	WARN_ON(!intelmaddata->sstdrv_ops);
-
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-
-	WARN_ON(!scard_ops);
-
-	switch (kcontrol->id.numid) {
-	case PLAYBACK_VOL:
-		cntl_list[0] = PMIC_SND_RIGHT_PB_VOL;
-		cntl_list[1] = PMIC_SND_LEFT_PB_VOL;
-		break;
-
-	case CAPTURE_VOL:
-		cntl_list[0] = PMIC_SND_CAPTURE_VOL;
-		break;
-
-	case MASTER_VOL:
-		cntl_list[0] = PMIC_SND_RIGHT_MASTER_VOL;
-		cntl_list[1] = PMIC_SND_LEFT_MASTER_VOL;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	ret_val = scard_ops->get_vol(cntl_list[0], &value);
-	uval->value.integer.value[0] = value;
-
-	if (ret_val)
-		return ret_val;
-
-	if (kcontrol->id.numid == PLAYBACK_VOL ||
-		kcontrol->id.numid == MASTER_VOL) {
-		ret_val = scard_ops->get_vol(cntl_list[1], &value);
-		uval->value.integer.value[1] = value;
-	}
-	return ret_val;
-}
-
-/**
-* snd_intelmad_mute_get - gets the current mute status for the control
-*
-* @kcontrol:	pointer to the control
-* @uval:	pointer to the structure where the control's info need
-*		to be filled
-*
-* This function is called when .get function of a control is invoked from app
-*/
-static int snd_intelmad_mute_get(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_value *uval)
-{
-
-	int cntl_list = 0, ret_val = 0;
-	u8 value = 0;
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-
-	pr_debug("Mute_get called\n");
-
-	WARN_ON(!uval);
-	WARN_ON(!kcontrol);
-
-	intelmaddata = kcontrol->private_data;
-
-	WARN_ON(!intelmaddata->sstdrv_ops);
-
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-
-	WARN_ON(!scard_ops);
-
-	switch (kcontrol->id.numid) {
-	case PLAYBACK_MUTE:
-		if (intelmaddata->output_sel == STEREO_HEADPHONE)
-			cntl_list = PMIC_SND_LEFT_HP_MUTE;
-		else if ((intelmaddata->output_sel == INTERNAL_SPKR) ||
-			(intelmaddata->output_sel == MONO_EARPIECE))
-			cntl_list = PMIC_SND_LEFT_SPEAKER_MUTE;
-		break;
-
-	case CAPTURE_MUTE:
-		if (intelmaddata->input_sel == DMIC)
-			cntl_list = PMIC_SND_DMIC_MUTE;
-		else if (intelmaddata->input_sel == AMIC)
-			cntl_list = PMIC_SND_AMIC_MUTE;
-		else if (intelmaddata->input_sel == HS_MIC)
-			cntl_list = PMIC_SND_HP_MIC_MUTE;
-		break;
-	case MASTER_MUTE:
-		uval->value.integer.value[0] = intelmaddata->master_mute;
-		return 0;
-	default:
-		return -EINVAL;
-	}
-
-	ret_val = scard_ops->get_mute(cntl_list, &value);
-	uval->value.integer.value[0] = value;
-	return ret_val;
-}
-
-/**
-* snd_intelmad_volume_set - sets the volume control's info
-*
-* @kcontrol:	pointer to the control
-* @uval:	pointer to the structure where the control's info is
-*		available to be set
-*
-* This function is called when .set function of a control is invoked from app
-*/
-static int snd_intelmad_volume_set(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_value *uval)
-{
-
-	int ret_val, cntl_list[2] = {0,};
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-
-	pr_debug("volume set called:%ld %ld\n",
-			uval->value.integer.value[0],
-			uval->value.integer.value[1]);
-
-	WARN_ON(!uval);
-	WARN_ON(!kcontrol);
-
-	intelmaddata = kcontrol->private_data;
-
-	WARN_ON(!intelmaddata->sstdrv_ops);
-
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-
-	WARN_ON(!scard_ops);
-
-	switch (kcontrol->id.numid) {
-	case PLAYBACK_VOL:
-		cntl_list[0] = PMIC_SND_LEFT_PB_VOL;
-		cntl_list[1] = PMIC_SND_RIGHT_PB_VOL;
-		break;
-
-	case CAPTURE_VOL:
-		cntl_list[0] = PMIC_SND_CAPTURE_VOL;
-		break;
-
-	case MASTER_VOL:
-		cntl_list[0] = PMIC_SND_LEFT_MASTER_VOL;
-		cntl_list[1] = PMIC_SND_RIGHT_MASTER_VOL;
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
-	ret_val = scard_ops->set_vol(cntl_list[0],
-				uval->value.integer.value[0]);
-	if (ret_val)
-		return ret_val;
-
-	if (kcontrol->id.numid == PLAYBACK_VOL ||
-		kcontrol->id.numid == MASTER_VOL)
-		ret_val = scard_ops->set_vol(cntl_list[1],
-				uval->value.integer.value[1]);
-	return ret_val;
-}
-
-/**
-* snd_intelmad_mute_set - sets the mute control's info
-*
-* @kcontrol:	pointer to the control
-* @uval:	pointer to the structure where the control's info is
-*		available to be set
-*
-* This function is called when .set function of a control is invoked from app
-*/
-static int snd_intelmad_mute_set(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_value *uval)
-{
-	int cntl_list[2] = {0,}, ret_val;
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-
-	pr_debug("snd_intelmad_mute_set called\n");
-
-	WARN_ON(!uval);
-	WARN_ON(!kcontrol);
-
-	intelmaddata = kcontrol->private_data;
-
-	WARN_ON(!intelmaddata->sstdrv_ops);
-
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-
-	WARN_ON(!scard_ops);
-
-	kcontrol->private_value = uval->value.integer.value[0];
-
-	switch (kcontrol->id.numid) {
-	case PLAYBACK_MUTE:
-		if (intelmaddata->output_sel == STEREO_HEADPHONE) {
-			cntl_list[0] = PMIC_SND_LEFT_HP_MUTE;
-			cntl_list[1] = PMIC_SND_RIGHT_HP_MUTE;
-		} else if ((intelmaddata->output_sel == INTERNAL_SPKR) ||
-				(intelmaddata->output_sel == MONO_EARPIECE)) {
-			cntl_list[0] = PMIC_SND_LEFT_SPEAKER_MUTE;
-			cntl_list[1] = PMIC_SND_RIGHT_SPEAKER_MUTE;
-		}
-		break;
-
-	case CAPTURE_MUTE:/*based on sel device mute the i/p dev*/
-		if (intelmaddata->input_sel == DMIC)
-			cntl_list[0] = PMIC_SND_DMIC_MUTE;
-		else if (intelmaddata->input_sel == AMIC)
-			cntl_list[0] = PMIC_SND_AMIC_MUTE;
-		else if (intelmaddata->input_sel == HS_MIC)
-			cntl_list[0] = PMIC_SND_HP_MIC_MUTE;
-		break;
-	case MASTER_MUTE:
-		cntl_list[0] = PMIC_SND_MUTE_ALL;
-		intelmaddata->master_mute = uval->value.integer.value[0];
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	ret_val = scard_ops->set_mute(cntl_list[0],
-				uval->value.integer.value[0]);
-	if (ret_val)
-		return ret_val;
-
-	if (kcontrol->id.numid == PLAYBACK_MUTE)
-		ret_val = scard_ops->set_mute(cntl_list[1],
-					uval->value.integer.value[0]);
-	return ret_val;
-}
-
-/**
-* snd_intelmad_device_get - get the device select control's info
-*
-* @kcontrol:	pointer to the control
-* @uval:	pointer to the structure where the control's info is
-*		to be filled
-*
-* This function is called when .get function of a control is invoked from app
-*/
-static int snd_intelmad_device_get(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_value *uval)
-{
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-	pr_debug("device_get called\n");
-
-	WARN_ON(!uval);
-	WARN_ON(!kcontrol);
-
-	intelmaddata = kcontrol->private_data;
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-	if (intelmaddata->cpu_id == CPU_CHIP_PENWELL) {
-		if (kcontrol->id.numid == OUTPUT_SEL)
-			uval->value.enumerated.item[0] =
-					scard_ops->output_dev_id;
-		else if (kcontrol->id.numid == INPUT_SEL)
-			uval->value.enumerated.item[0] =
-					scard_ops->input_dev_id;
-		else if (kcontrol->id.numid == LINEOUT_SEL_MFLD)
-			uval->value.enumerated.item[0] =
-					scard_ops->lineout_dev_id;
-		else
-			return -EINVAL;
-	} else if (intelmaddata->cpu_id == CPU_CHIP_LINCROFT) {
-		if (kcontrol->id.numid == OUTPUT_SEL)
-			/* There is a mismatch here.
-			 * ALSA expects 1 for internal speaker.
-			 * But internally, we may give 2 for internal speaker.
-			 */
-			if (scard_ops->output_dev_id == MONO_EARPIECE ||
-			    scard_ops->output_dev_id == INTERNAL_SPKR)
-				uval->value.enumerated.item[0] = MONO_EARPIECE;
-			else if (scard_ops->output_dev_id == STEREO_HEADPHONE)
-				uval->value.enumerated.item[0] =
-					STEREO_HEADPHONE;
-			else
-				return -EINVAL;
-		else if (kcontrol->id.numid == INPUT_SEL)
-			uval->value.enumerated.item[0] =
-					scard_ops->input_dev_id;
-		else
-			return -EINVAL;
-	} else
-	uval->value.enumerated.item[0] = kcontrol->private_value;
-	return 0;
-}
-
-/**
-* snd_intelmad_device_set - set the device select control's info
-*
-* @kcontrol:	pointer to the control
-* @uval:	pointer to the structure where the control's info is
-*		available to be set
-*
-* This function is called when .set function of a control is invoked from app
-*/
-static int snd_intelmad_device_set(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_value *uval)
-{
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-	int ret_val = 0, vendor, status;
-	struct intel_sst_pcm_control *pcm_control;
-
-	pr_debug("snd_intelmad_device_set called\n");
-
-	WARN_ON(!uval);
-	WARN_ON(!kcontrol);
-	status = -1;
-
-	intelmaddata = kcontrol->private_data;
-
-	WARN_ON(!intelmaddata->sstdrv_ops);
-
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-
-	WARN_ON(!scard_ops);
-
-	/* store value with driver */
-	kcontrol->private_value = uval->value.enumerated.item[0];
-
-	switch (kcontrol->id.numid) {
-	case OUTPUT_SEL:
-		ret_val = scard_ops->set_output_dev(
-				uval->value.enumerated.item[0]);
-		intelmaddata->output_sel = uval->value.enumerated.item[0];
-		break;
-	case INPUT_SEL:
-		vendor = intelmaddata->sstdrv_ops->vendor_id;
-		if ((vendor == SND_MX) || (vendor == SND_FS)) {
-			pcm_control = intelmaddata->sstdrv_ops->pcm_control;
-			if (uval->value.enumerated.item[0] == HS_MIC)
-				status = 1;
-			else
-				status = 0;
-			pcm_control->device_control(
-					SST_ENABLE_RX_TIME_SLOT, &status);
-		}
-		ret_val = scard_ops->set_input_dev(
-				uval->value.enumerated.item[0]);
-		intelmaddata->input_sel = uval->value.enumerated.item[0];
-		break;
-	case LINEOUT_SEL_MFLD:
-		ret_val = scard_ops->set_lineout_dev(
-					uval->value.enumerated.item[0]);
-		intelmaddata->lineout_sel = uval->value.enumerated.item[0];
-		break;
-	default:
-		return -EINVAL;
-	}
-	kcontrol->private_value = uval->value.enumerated.item[0];
-	return ret_val;
-}
-
-static int snd_intelmad_device_dmic_get(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_value *uval)
-{
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-
-	WARN_ON(!uval);
-	WARN_ON(!kcontrol);
-
-	intelmaddata = kcontrol->private_data;
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-
-	if (scard_ops->input_dev_id != DMIC) {
-		pr_debug("input dev = 0x%x\n", scard_ops->input_dev_id);
-		return 0;
-	}
-
-	if (intelmaddata->cpu_id == CPU_CHIP_PENWELL)
-		uval->value.enumerated.item[0] = kcontrol->private_value;
-	else
-		pr_debug(" CPU id = 0x%xis invalid.\n",
-			intelmaddata->cpu_id);
-	return 0;
-}
-
-void msic_set_bit(u8 index, unsigned int *available_dmics)
-{
-	*available_dmics |= (1 << index);
-}
-
-void msic_clear_bit(u8 index, unsigned int *available_dmics)
-{
-	*available_dmics &= ~(1 << index);
-}
-
-int msic_is_set_bit(u8 index, unsigned int *available_dmics)
-{
-	int ret_val;
-
-	ret_val = (*available_dmics & (1 << index));
-	return ret_val;
-}
-
-static int snd_intelmad_device_dmic_set(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_value *uval)
-{
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-	int i, dmic_index;
-	unsigned int available_dmics;
-	int jump_count;
-	int max_dmics = ARRAY_SIZE(router_dmics);
-
-	WARN_ON(!uval);
-	WARN_ON(!kcontrol);
-
-	intelmaddata = kcontrol->private_data;
-	WARN_ON(!intelmaddata->sstdrv_ops);
-
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-	WARN_ON(!scard_ops);
-
-	if (scard_ops->input_dev_id != DMIC) {
-		pr_debug("input dev = 0x%x\n", scard_ops->input_dev_id);
-		return 0;
-	}
-
-	available_dmics = scard_ops->available_dmics;
-
-	if (kcontrol->private_value > uval->value.enumerated.item[0]) {
-		pr_debug("jump count -1.\n");
-		jump_count = -1;
-	} else {
-		pr_debug("jump count 1.\n");
-		jump_count = 1;
-	}
-
-	dmic_index =  uval->value.enumerated.item[0];
-	pr_debug("set function. dmic_index = %d, avl_dmic = 0x%x\n",
-			 dmic_index, available_dmics);
-	for (i = 0; i < max_dmics; i++) {
-		pr_debug("set function. loop index = 0x%x.  dmic_index = 0x%x\n",
-			 i, dmic_index);
-		if (!msic_is_set_bit(dmic_index, &available_dmics)) {
-			msic_clear_bit(kcontrol->private_value,
-						&available_dmics);
-			msic_set_bit(dmic_index, &available_dmics);
-			kcontrol->private_value = dmic_index;
-			scard_ops->available_dmics = available_dmics;
-			scard_ops->hw_dmic_map[kcontrol->id.numid-HW_CH_BASE] =
-				kcontrol->private_value;
-			scard_ops->set_hw_dmic_route
-				(kcontrol->id.numid-HW_CH_BASE);
-			return 0;
-		}
-
-		dmic_index += jump_count;
-
-		if (dmic_index > (max_dmics - 1) && jump_count == 1) {
-			pr_debug("Resettingthe dmic index to 0.\n");
-			dmic_index = 0;
-		} else if (dmic_index == -1 && jump_count == -1) {
-			pr_debug("Resetting the dmic index to 5.\n");
-			dmic_index = max_dmics - 1;
-		}
-	}
-
-	return -EINVAL;
-}
-
-static int snd_intelmad_device_dmic_info_mfld(struct snd_kcontrol *kcontrol,
-					struct snd_ctl_elem_info *uinfo)
-{
-	struct snd_intelmad *intelmaddata;
-	struct snd_pmic_ops *scard_ops;
-
-	uinfo->count                  = MONO_CNTL;
-	uinfo->type                   = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
-	uinfo->value.enumerated.items = ARRAY_SIZE(router_dmics);
-
-	intelmaddata = kcontrol->private_data;
-	WARN_ON(!intelmaddata->sstdrv_ops);
-
-	scard_ops = intelmaddata->sstdrv_ops->scard_ops;
-	WARN_ON(!scard_ops);
-
-	if (uinfo->value.enumerated.item >= uinfo->value.enumerated.items)
-		uinfo->value.enumerated.item =
-			uinfo->value.enumerated.items - 1;
-
-	strncpy(uinfo->value.enumerated.name,
-	router_dmics[uinfo->value.enumerated.item],
-	sizeof(uinfo->value.enumerated.name)-1);
-
-
-	msic_set_bit(kcontrol->private_value, &scard_ops->available_dmics);
-	pr_debug("info function. avl_dmic = 0x%x",
-		scard_ops->available_dmics);
-
-	scard_ops->hw_dmic_map[kcontrol->id.numid-HW_CH_BASE] =
-		kcontrol->private_value;
-
-	return 0;
-}
-
-struct snd_kcontrol_new snd_intelmad_controls_mrst[MAX_CTRL] __devinitdata = {
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"PCM Playback Source",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_info_mrst,
-	.get		=	snd_intelmad_device_get,
-	.put		=	snd_intelmad_device_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"PCM Capture Source",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_info_mrst,
-	.get		=	snd_intelmad_device_get,
-	.put		=	snd_intelmad_device_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"PCM Playback Volume",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_playback_volume_info,
-	.get		=	snd_intelmad_volume_get,
-	.put		=	snd_intelmad_volume_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"PCM Playback Switch",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_mute_info,
-	.get		=	snd_intelmad_mute_get,
-	.put		=	snd_intelmad_mute_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"PCM Capture Volume",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_capture_volume_info,
-	.get		=	snd_intelmad_volume_get,
-	.put		=	snd_intelmad_volume_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"PCM Capture Switch",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_mute_info,
-	.get		=	snd_intelmad_mute_get,
-	.put		=	snd_intelmad_mute_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"Master Playback Volume",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_master_volume_info,
-	.get		=	snd_intelmad_volume_get,
-	.put		=	snd_intelmad_volume_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"Master Playback Switch",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_mute_info,
-	.get		=	snd_intelmad_mute_get,
-	.put		=	snd_intelmad_mute_set,
-	.private_value	=	0,
-},
-};
-
-struct snd_kcontrol_new
-snd_intelmad_controls_mfld[MAX_CTRL_MFLD] __devinitdata = {
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"PCM Playback Source",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_info_mfld,
-	.get		=	snd_intelmad_device_get,
-	.put		=	snd_intelmad_device_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"PCM Capture Source",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_info_mfld,
-	.get		=	snd_intelmad_device_get,
-	.put		=	snd_intelmad_device_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	"Line out",
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_info_mfld,
-	.get		=	snd_intelmad_device_get,
-	.put		=	snd_intelmad_device_set,
-	.private_value	=	0,
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	HW_CH_0,
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_dmic_info_mfld,
-	.get		=	snd_intelmad_device_dmic_get,
-	.put		=	snd_intelmad_device_dmic_set,
-	.private_value	=	0
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	HW_CH_1,
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_dmic_info_mfld,
-	.get		=	snd_intelmad_device_dmic_get,
-	.put		=	snd_intelmad_device_dmic_set,
-	.private_value	=	1
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	HW_CH_2,
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_dmic_info_mfld,
-	.get		=	snd_intelmad_device_dmic_get,
-	.put		=	snd_intelmad_device_dmic_set,
-	.private_value	=	2
-},
-{
-	.iface		=	SNDRV_CTL_ELEM_IFACE_MIXER,
-	.name		=	HW_CH_3,
-	.access		=	SNDRV_CTL_ELEM_ACCESS_READWRITE,
-	.info		=	snd_intelmad_device_dmic_info_mfld,
-	.get		=	snd_intelmad_device_dmic_get,
-	.put		=	snd_intelmad_device_dmic_set,
-	.private_value	=	3
-}
-};
-
diff --git a/drivers/staging/intel_sst/intelmid_msic_control.c b/drivers/staging/intel_sst/intelmid_msic_control.c
deleted file mode 100644
index 70cdb1697815..000000000000
--- a/drivers/staging/intel_sst/intelmid_msic_control.c
+++ /dev/null
@@ -1,1047 +0,0 @@
-/*
- *  intelmid_vm_control.c - Intel Sound card driver for MID
- *
- *  Copyright (C) 2010 Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- * This file contains the control operations of msic vendors
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/file.h>
-#include <linux/delay.h>
-#include <sound/control.h>
-#include "intel_sst.h"
-#include <linux/input.h>
-#include "intelmid_snd_control.h"
-#include "intelmid.h"
-
-#define AUDIOMUX12  0x24c
-#define AUDIOMUX34  0x24d
-
-static int msic_init_card(void)
-{
-	struct sc_reg_access sc_access[] = {
-		/* dmic configuration */
-		{0x241, 0x85, 0},
-		{0x242, 0x02, 0},
-		/* audio paths config */
-		{0x24C, 0x10, 0},
-		{0x24D, 0x32, 0},
-		/* PCM2 interface slots */
-		/* preconfigured slots for 0-5 both tx, rx */
-		{0x272, 0x10, 0},
-		{0x273, 0x32, 0},
-		{0x274, 0xFF, 0},
-		{0x275, 0x10, 0},
-		{0x276, 0x32, 0},
-		{0x277, 0x54, 0},
-		/*Sinc5 decimator*/
-		{0x24E, 0x28, 0},
-		/*TI vibra w/a settings*/
-		{0x384, 0x80, 0},
-		{0x385, 0x80, 0},
-		{0x267, 0x00, 0},
-		{0x261, 0x00, 0},
-		/* pcm port setting */
-		{0x278, 0x00, 0},
-		{0x27B, 0x01, 0},
-		{0x27C, 0x0a, 0},
-		/* Set vol HSLRVOLCTRL, IHFVOL */
-		{0x259, 0x08, 0},
-		{0x25A, 0x08, 0},
-		{0x25B, 0x08, 0},
-		{0x25C, 0x08, 0},
-		/* HSEPRXCTRL  Enable the headset left and right FIR filters  */
-		{0x250, 0x30, 0},
-		/* HSMIXER */
-		{0x256, 0x11, 0},
-		/* amic configuration */
-		{0x249, 0x01, 0x0},
-		{0x24A, 0x01, 0x0},
-		/* unmask ocaudio/accdet interrupts */
-		{0x1d, 0x00, 0x00},
-		{0x1e, 0x00, 0x00},
-	};
-	snd_msic_ops.card_status = SND_CARD_INIT_DONE;
-	sst_sc_reg_access(sc_access, PMIC_WRITE, 28);
-	snd_msic_ops.pb_on = 0;
-	snd_msic_ops.pbhs_on = 0;
-	snd_msic_ops.cap_on = 0;
-	snd_msic_ops.input_dev_id = DMIC; /*def dev*/
-	snd_msic_ops.output_dev_id = STEREO_HEADPHONE;
-	snd_msic_ops.jack_interrupt_status = false;
-	pr_debug("msic init complete!!\n");
-	return 0;
-}
-static int msic_line_out_restore(u8 value)
-{
-	struct sc_reg_access hs_drv_en[] = {
-		{0x25d, 0x03, 0x03},
-	};
-	struct sc_reg_access ep_drv_en[] = {
-		{0x25d, 0x40, 0x40},
-	};
-	struct sc_reg_access ihf_drv_en[] = {
-		{0x25d, 0x0c, 0x0c},
-	};
-	struct sc_reg_access vib1_drv_en[] = {
-		{0x25d, 0x10, 0x10},
-	};
-	struct sc_reg_access vib2_drv_en[] = {
-		{0x25d, 0x20, 0x20},
-	};
-	struct sc_reg_access pmode_enable[] = {
-		{0x381, 0x10, 0x10},
-	};
-	int retval = 0;
-
-	pr_debug("msic_lineout_restore_lineout_dev:%d\n", value);
-
-	switch (value) {
-	case HEADSET:
-		pr_debug("Selecting Lineout-HEADSET-restore\n");
-		if (snd_msic_ops.output_dev_id == STEREO_HEADPHONE)
-			retval = sst_sc_reg_access(hs_drv_en,
-							PMIC_READ_MODIFY, 1);
-		else
-			retval = sst_sc_reg_access(ep_drv_en,
-							PMIC_READ_MODIFY, 1);
-		break;
-	case IHF:
-		pr_debug("Selecting Lineout-IHF-restore\n");
-		retval = sst_sc_reg_access(ihf_drv_en, PMIC_READ_MODIFY, 1);
-		if (retval)
-			return retval;
-		retval = sst_sc_reg_access(pmode_enable, PMIC_READ_MODIFY, 1);
-		break;
-	case VIBRA1:
-		pr_debug("Selecting Lineout-Vibra1-restore\n");
-		retval = sst_sc_reg_access(vib1_drv_en, PMIC_READ_MODIFY, 1);
-		break;
-	case VIBRA2:
-		pr_debug("Selecting Lineout-VIBRA2-restore\n");
-		retval = sst_sc_reg_access(vib2_drv_en, PMIC_READ_MODIFY, 1);
-		break;
-	case NONE:
-		pr_debug("Selecting Lineout-NONE-restore\n");
-		break;
-	default:
-		return -EINVAL;
-	}
-	return retval;
-}
-static int msic_get_lineout_prvstate(void)
-{
-	struct sc_reg_access hs_ihf_drv[2] = {
-		{0x257, 0x0, 0x0},
-		{0x25d, 0x0, 0x0},
-	};
-	struct sc_reg_access vib1drv[2] = {
-		{0x264, 0x0, 0x0},
-		{0x25D, 0x0, 0x0},
-	};
-	struct sc_reg_access vib2drv[2] = {
-		{0x26A, 0x0, 0x0},
-		{0x25D, 0x0, 0x0},
-	};
-	int retval = 0, drv_en, dac_en, dev_id, mask;
-	for (dev_id = 0; dev_id < snd_msic_ops.line_out_names_cnt; dev_id++) {
-		switch (dev_id) {
-		case HEADSET:
-			pr_debug("msic_get_lineout_prvs_state: HEADSET\n");
-			sst_sc_reg_access(hs_ihf_drv, PMIC_READ, 2);
-
-			mask = (MASK0|MASK1);
-			dac_en = (hs_ihf_drv[0].value) & mask;
-
-			mask = ((MASK0|MASK1)|MASK6);
-			drv_en = (hs_ihf_drv[1].value) & mask;
-
-			if (dac_en && (!drv_en)) {
-				snd_msic_ops.prev_lineout_dev_id = HEADSET;
-				return retval;
-			}
-			break;
-		case IHF:
-			pr_debug("msic_get_lineout_prvstate: IHF\n");
-			sst_sc_reg_access(hs_ihf_drv, PMIC_READ, 2);
-
-			mask = (MASK2 | MASK3);
-			dac_en = (hs_ihf_drv[0].value) & mask;
-
-			mask = (MASK2 | MASK3);
-			drv_en = (hs_ihf_drv[1].value) & mask;
-
-			if (dac_en && (!drv_en)) {
-				snd_msic_ops.prev_lineout_dev_id = IHF;
-				return retval;
-			}
-			break;
-		case VIBRA1:
-			pr_debug("msic_get_lineout_prvstate: vibra1\n");
-			sst_sc_reg_access(vib1drv, PMIC_READ, 2);
-
-			mask = MASK1;
-			dac_en = (vib1drv[0].value) & mask;
-
-			mask = MASK4;
-			drv_en = (vib1drv[1].value) & mask;
-
-			if (dac_en && (!drv_en)) {
-				snd_msic_ops.prev_lineout_dev_id = VIBRA1;
-				return retval;
-			}
-			break;
-		case VIBRA2:
-			pr_debug("msic_get_lineout_prvstate: vibra2\n");
-			sst_sc_reg_access(vib2drv, PMIC_READ, 2);
-
-			mask = MASK1;
-			dac_en = (vib2drv[0].value) & mask;
-
-			mask = MASK5;
-			drv_en = ((vib2drv[1].value) & mask);
-
-			if (dac_en && (!drv_en)) {
-				snd_msic_ops.prev_lineout_dev_id = VIBRA2;
-				return retval;
-			}
-			break;
-		case NONE:
-			pr_debug("msic_get_lineout_prvstate: NONE\n");
-			snd_msic_ops.prev_lineout_dev_id = NONE;
-			return retval;
-		default:
-			pr_debug("Invalid device id\n");
-			snd_msic_ops.prev_lineout_dev_id = NONE;
-			return -EINVAL;
-		}
-	}
-	return retval;
-}
-static int msic_set_selected_lineout_dev(u8 value)
-{
-	struct sc_reg_access lout_hs[] = {
-		{0x25e, 0x33, 0xFF},
-		{0x25d, 0x0, 0x43},
-	};
-	struct sc_reg_access lout_ihf[] = {
-		{0x25e, 0x55, 0xff},
-		{0x25d, 0x0, 0x0c},
-	};
-	struct sc_reg_access lout_vibra1[] = {
-
-		{0x25e, 0x61, 0xff},
-		{0x25d, 0x0, 0x10},
-	};
-	struct sc_reg_access lout_vibra2[] = {
-
-		{0x25e, 0x16, 0xff},
-		{0x25d, 0x0, 0x20},
-	};
-	struct sc_reg_access lout_def[] = {
-		{0x25e, 0x66, 0x0},
-	};
-	struct sc_reg_access pmode_disable[] = {
-		{0x381, 0x00, 0x10},
-	};
-	struct sc_reg_access pmode_enable[] = {
-		{0x381, 0x10, 0x10},
-	};
-	int retval = 0;
-
-	pr_debug("msic_set_selected_lineout_dev:%d\n", value);
-	msic_get_lineout_prvstate();
-	msic_line_out_restore(snd_msic_ops.prev_lineout_dev_id);
-	snd_msic_ops.lineout_dev_id = value;
-
-	switch (value) {
-	case HEADSET:
-		pr_debug("Selecting Lineout-HEADSET\n");
-		if (snd_msic_ops.pb_on)
-			retval = sst_sc_reg_access(lout_hs,
-					PMIC_READ_MODIFY, 2);
-			if (retval)
-				return retval;
-			retval = sst_sc_reg_access(pmode_disable,
-					PMIC_READ_MODIFY, 1);
-		break;
-	case IHF:
-		pr_debug("Selecting Lineout-IHF\n");
-		if (snd_msic_ops.pb_on)
-			retval = sst_sc_reg_access(lout_ihf,
-							PMIC_READ_MODIFY, 2);
-			if (retval)
-				return retval;
-			retval = sst_sc_reg_access(pmode_enable,
-					PMIC_READ_MODIFY, 1);
-		break;
-	case VIBRA1:
-		pr_debug("Selecting Lineout-Vibra1\n");
-		if (snd_msic_ops.pb_on)
-			retval = sst_sc_reg_access(lout_vibra1,
-							PMIC_READ_MODIFY, 2);
-			if (retval)
-				return retval;
-			retval = sst_sc_reg_access(pmode_disable,
-					PMIC_READ_MODIFY, 1);
-		break;
-	case VIBRA2:
-		pr_debug("Selecting Lineout-VIBRA2\n");
-		if (snd_msic_ops.pb_on)
-			retval = sst_sc_reg_access(lout_vibra2,
-							PMIC_READ_MODIFY, 2);
-			if (retval)
-				return retval;
-			retval = sst_sc_reg_access(pmode_disable,
-					PMIC_READ_MODIFY, 1);
-		break;
-	case NONE:
-		pr_debug("Selecting Lineout-NONE\n");
-			retval = sst_sc_reg_access(lout_def,
-							PMIC_WRITE, 1);
-			if (retval)
-				return retval;
-			retval = sst_sc_reg_access(pmode_disable,
-					PMIC_READ_MODIFY, 1);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return retval;
-}
-
-
-static int msic_power_up_pb(unsigned int device)
-{
-	struct sc_reg_access vaud[] = {
-		/* turn on the audio power supplies */
-		{0x0DB, 0x07, 0},
-	};
-	struct sc_reg_access pll[] = {
-		/* turn on PLL */
-		{0x240, 0x20, 0},
-	};
-	struct sc_reg_access vhs[] = {
-		/*  VHSP */
-		{0x0DC, 0x3D, 0},
-		/*  VHSN */
-		{0x0DD, 0x3F, 0},
-	};
-	struct sc_reg_access hsdac[] = {
-		{0x382, 0x40, 0x40},
-		/*  disable driver */
-		{0x25D, 0x0, 0x43},
-		/* DAC CONFIG ; both HP, LP on */
-		{0x257, 0x03, 0x03},
-	};
-	struct sc_reg_access hs_filter[] = {
-		/* HSEPRXCTRL  Enable the headset left and right FIR filters  */
-		{0x250, 0x30, 0},
-		/* HSMIXER */
-		{0x256, 0x11, 0},
-	};
-	struct sc_reg_access hs_enable[] = {
-		/* enable driver */
-		{0x25D, 0x3, 0x3},
-		{0x26C, 0x0, 0x2},
-		/* unmute the headset */
-		{ 0x259, 0x80, 0x80},
-		{ 0x25A, 0x80, 0x80},
-	};
-	struct sc_reg_access vihf[] = {
-		/*  VIHF ON */
-		{0x0C9, 0x27, 0x00},
-	};
-	struct sc_reg_access ihf_filter[] = {
-		/*  disable driver */
-		{0x25D, 0x00, 0x0C},
-		/*Filer DAC enable*/
-		{0x251, 0x03, 0x03},
-		{0x257, 0x0C, 0x0C},
-	};
-	struct sc_reg_access ihf_en[] = {
-		/*enable drv*/
-		{0x25D, 0x0C, 0x0c},
-	};
-	struct sc_reg_access ihf_unmute[] = {
-		/*unmute headset*/
-		{0x25B, 0x80, 0x80},
-		{0x25C, 0x80, 0x80},
-	};
-	struct sc_reg_access epdac[] = {
-		/*  disable driver */
-		{0x25D, 0x0, 0x43},
-		/* DAC CONFIG ; both HP, LP on */
-		{0x257, 0x03, 0x03},
-	};
-	struct sc_reg_access ep_enable[] = {
-		/* enable driver */
-		{0x25D, 0x40, 0x40},
-		/* unmute the headset */
-		{ 0x259, 0x80, 0x80},
-		{ 0x25A, 0x80, 0x80},
-	};
-	struct sc_reg_access vib1_en[] = {
-		/* enable driver, ADC */
-		{0x25D, 0x10, 0x10},
-		{0x264, 0x02, 0x82},
-	};
-	struct sc_reg_access vib2_en[] = {
-		/* enable driver, ADC */
-		{0x25D, 0x20, 0x20},
-		{0x26A, 0x02, 0x82},
-	};
-	struct sc_reg_access pcm2_en[] = {
-		/* enable pcm 2 */
-		{0x27C, 0x1, 0x1},
-	};
-	int retval = 0;
-
-	if (snd_msic_ops.card_status == SND_CARD_UN_INIT) {
-		retval = msic_init_card();
-		if (retval)
-			return retval;
-	}
-
-	pr_debug("powering up pb.... Device %d\n", device);
-	sst_sc_reg_access(vaud, PMIC_WRITE, 1);
-	msleep(1);
-	sst_sc_reg_access(pll, PMIC_WRITE, 1);
-	msleep(1);
-	switch (device) {
-	case SND_SST_DEVICE_HEADSET:
-		snd_msic_ops.pb_on = 1;
-		snd_msic_ops.pbhs_on = 1;
-		if (snd_msic_ops.output_dev_id == STEREO_HEADPHONE) {
-			sst_sc_reg_access(vhs, PMIC_WRITE, 2);
-			sst_sc_reg_access(hsdac, PMIC_READ_MODIFY, 3);
-			sst_sc_reg_access(hs_filter, PMIC_WRITE, 2);
-			sst_sc_reg_access(hs_enable, PMIC_READ_MODIFY, 4);
-		} else {
-			sst_sc_reg_access(epdac, PMIC_READ_MODIFY, 2);
-			sst_sc_reg_access(hs_filter, PMIC_WRITE, 2);
-			sst_sc_reg_access(ep_enable, PMIC_READ_MODIFY, 3);
-		}
-		if (snd_msic_ops.lineout_dev_id == HEADSET)
-			msic_set_selected_lineout_dev(HEADSET);
-		break;
-	case SND_SST_DEVICE_IHF:
-		snd_msic_ops.pb_on = 1;
-		sst_sc_reg_access(vihf, PMIC_WRITE, 1);
-		sst_sc_reg_access(ihf_filter, PMIC_READ_MODIFY, 3);
-		sst_sc_reg_access(ihf_en, PMIC_READ_MODIFY, 1);
-		sst_sc_reg_access(ihf_unmute, PMIC_READ_MODIFY, 2);
-		if (snd_msic_ops.lineout_dev_id == IHF)
-			msic_set_selected_lineout_dev(IHF);
-		break;
-
-	case SND_SST_DEVICE_VIBRA:
-		snd_msic_ops.pb_on = 1;
-		sst_sc_reg_access(vib1_en, PMIC_READ_MODIFY, 2);
-		if (snd_msic_ops.lineout_dev_id == VIBRA1)
-			msic_set_selected_lineout_dev(VIBRA1);
-		break;
-
-	case SND_SST_DEVICE_HAPTIC:
-		snd_msic_ops.pb_on = 1;
-		sst_sc_reg_access(vib2_en, PMIC_READ_MODIFY, 2);
-		if (snd_msic_ops.lineout_dev_id == VIBRA2)
-			msic_set_selected_lineout_dev(VIBRA2);
-		break;
-
-	default:
-		pr_warn("Wrong Device %d, selected %d\n",
-			       device, snd_msic_ops.output_dev_id);
-	}
-	return sst_sc_reg_access(pcm2_en, PMIC_READ_MODIFY, 1);
-}
-
-static int msic_power_up_cp(unsigned int device)
-{
-	struct sc_reg_access vaud[] = {
-		/* turn on the audio power supplies */
-		{0x0DB, 0x07, 0},
-	};
-	struct sc_reg_access pll[] = {
-		/* turn on PLL */
-		{0x240, 0x20, 0},
-	};
-	struct sc_reg_access dmic_bias[] = {
-		/*  Turn on AMIC supply  */
-		{0x247, 0xA0, 0xA0},
-	};
-	struct sc_reg_access dmic[] = {
-		/* mic demux enable */
-		{0x245, 0x3F, 0x3F},
-		{0x246, 0x07, 0x07},
-
-	};
-	struct sc_reg_access amic_bias[] = {
-		/*  Turn on AMIC supply  */
-		{0x247, 0xFC, 0xFC},
-	};
-	struct sc_reg_access amic[] = {
-		/*MIC EN*/
-		{0x249, 0x01, 0x01},
-		{0x24A, 0x01, 0x01},
-		/*ADC EN*/
-		{0x248, 0x05, 0x0F},
-
-	};
-	struct sc_reg_access pcm2[] = {
-		/* enable pcm 2 */
-		{0x27C, 0x1, 0x1},
-	};
-	struct sc_reg_access tx_on[] = {
-		/*wait for mic to stabalize before turning on audio channels*/
-		{0x24F, 0x3C, 0x0},
-	};
-	int retval = 0;
-
-	if (snd_msic_ops.card_status == SND_CARD_UN_INIT) {
-		retval = msic_init_card();
-		if (retval)
-			return retval;
-	}
-
-	pr_debug("powering up cp....%d\n", snd_msic_ops.input_dev_id);
-	sst_sc_reg_access(vaud, PMIC_WRITE, 1);
-	msleep(500);/*FIXME need optimzed value here*/
-	sst_sc_reg_access(pll, PMIC_WRITE, 1);
-	msleep(1);
-	snd_msic_ops.cap_on = 1;
-	if (snd_msic_ops.input_dev_id == AMIC) {
-		sst_sc_reg_access(amic_bias, PMIC_READ_MODIFY, 1);
-		msleep(1);
-		sst_sc_reg_access(amic, PMIC_READ_MODIFY, 3);
-	} else {
-		sst_sc_reg_access(dmic_bias, PMIC_READ_MODIFY, 1);
-		msleep(1);
-		sst_sc_reg_access(dmic, PMIC_READ_MODIFY, 2);
-	}
-	msleep(1);
-	sst_sc_reg_access(tx_on, PMIC_WRITE, 1);
-	return sst_sc_reg_access(pcm2, PMIC_READ_MODIFY, 1);
-}
-
-static int msic_power_down(void)
-{
-	struct sc_reg_access power_dn[] = {
-		/*  VHSP */
-		{0x0DC, 0xC4, 0},
-		/*  VHSN */
-		{0x0DD, 0x04, 0},
-		/*  VIHF */
-		{0x0C9, 0x24, 0},
-	};
-	struct sc_reg_access pll[] = {
-		/* turn off PLL*/
-		{0x240, 0x00, 0x0},
-	};
-	struct sc_reg_access vaud[] = {
-		/* turn off VAUD*/
-		{0x0DB, 0x04, 0},
-	};
-
-	pr_debug("powering dn msic\n");
-	snd_msic_ops.pbhs_on = 0;
-	snd_msic_ops.pb_on = 0;
-	snd_msic_ops.cap_on = 0;
-	sst_sc_reg_access(power_dn, PMIC_WRITE, 3);
-	msleep(1);
-	sst_sc_reg_access(pll, PMIC_WRITE, 1);
-	msleep(1);
-	sst_sc_reg_access(vaud, PMIC_WRITE, 1);
-	return 0;
-}
-
-static int msic_power_down_pb(unsigned int device)
-{
-	struct sc_reg_access drv_enable[] = {
-		{0x25D, 0x00, 0x00},
-	};
-	struct sc_reg_access hs_mute[] = {
-		{0x259, 0x80, 0x80},
-		{0x25A, 0x80, 0x80},
-		{0x26C, 0x02, 0x02},
-	};
-	struct sc_reg_access hs_off[] = {
-		{0x257, 0x00, 0x03},
-		{0x250, 0x00, 0x30},
-		{0x382, 0x00, 0x40},
-	};
-	struct sc_reg_access ihf_mute[] = {
-		{0x25B, 0x80, 0x80},
-		{0x25C, 0x80, 0x80},
-	};
-	struct sc_reg_access ihf_off[] = {
-		{0x257, 0x00, 0x0C},
-		{0x251, 0x00, 0x03},
-	};
-	struct sc_reg_access vib1_off[] = {
-		{0x264, 0x00, 0x82},
-	};
-	struct sc_reg_access vib2_off[] = {
-		{0x26A, 0x00, 0x82},
-	};
-	struct sc_reg_access lout_off[] = {
-		{0x25e, 0x66, 0x00},
-	};
-	struct sc_reg_access pmode_disable[] = {
-		{0x381, 0x00, 0x10},
-	};
-
-
-
-	pr_debug("powering dn pb for device %d\n", device);
-	switch (device) {
-	case SND_SST_DEVICE_HEADSET:
-		snd_msic_ops.pbhs_on = 0;
-		sst_sc_reg_access(hs_mute, PMIC_READ_MODIFY, 3);
-		drv_enable[0].mask = 0x43;
-		sst_sc_reg_access(drv_enable, PMIC_READ_MODIFY, 1);
-		sst_sc_reg_access(hs_off, PMIC_READ_MODIFY, 3);
-		if (snd_msic_ops.lineout_dev_id == HEADSET)
-			sst_sc_reg_access(lout_off, PMIC_WRITE, 1);
-		break;
-
-	case SND_SST_DEVICE_IHF:
-		sst_sc_reg_access(ihf_mute, PMIC_READ_MODIFY, 2);
-		drv_enable[0].mask = 0x0C;
-		sst_sc_reg_access(drv_enable, PMIC_READ_MODIFY, 1);
-		sst_sc_reg_access(ihf_off, PMIC_READ_MODIFY, 2);
-		if (snd_msic_ops.lineout_dev_id == IHF) {
-			sst_sc_reg_access(lout_off, PMIC_WRITE, 1);
-			sst_sc_reg_access(pmode_disable, PMIC_READ_MODIFY, 1);
-		}
-		break;
-
-	case SND_SST_DEVICE_VIBRA:
-		sst_sc_reg_access(vib1_off, PMIC_READ_MODIFY, 1);
-		drv_enable[0].mask = 0x10;
-		sst_sc_reg_access(drv_enable, PMIC_READ_MODIFY, 1);
-		if (snd_msic_ops.lineout_dev_id == VIBRA1)
-			sst_sc_reg_access(lout_off, PMIC_WRITE, 1);
-		break;
-
-	case SND_SST_DEVICE_HAPTIC:
-		sst_sc_reg_access(vib2_off, PMIC_READ_MODIFY, 1);
-		drv_enable[0].mask = 0x20;
-		sst_sc_reg_access(drv_enable, PMIC_READ_MODIFY, 1);
-		if (snd_msic_ops.lineout_dev_id == VIBRA2)
-			sst_sc_reg_access(lout_off, PMIC_WRITE, 1);
-		break;
-	}
-	return 0;
-}
-
-static int msic_power_down_cp(unsigned int device)
-{
-	struct sc_reg_access dmic[] = {
-		{0x247, 0x00, 0xA0},
-		{0x245, 0x00, 0x38},
-		{0x246, 0x00, 0x07},
-	};
-	struct sc_reg_access amic[] = {
-		{0x248, 0x00, 0x05},
-		{0x249, 0x00, 0x01},
-		{0x24A, 0x00, 0x01},
-		{0x247, 0x00, 0xA3},
-	};
-	struct sc_reg_access tx_off[] = {
-		{0x24F, 0x00, 0x3C},
-	};
-
-	pr_debug("powering dn cp....\n");
-	snd_msic_ops.cap_on = 0;
-	sst_sc_reg_access(tx_off, PMIC_READ_MODIFY, 1);
-	if (snd_msic_ops.input_dev_id == DMIC)
-		sst_sc_reg_access(dmic, PMIC_READ_MODIFY, 3);
-	else
-		sst_sc_reg_access(amic, PMIC_READ_MODIFY, 4);
-	return 0;
-}
-
-static int msic_set_selected_output_dev(u8 value)
-{
-	int retval = 0;
-
-	pr_debug("msic set selected output:%d\n", value);
-	snd_msic_ops.output_dev_id = value;
-	if (snd_msic_ops.pbhs_on)
-		msic_power_up_pb(SND_SST_DEVICE_HEADSET);
-	return retval;
-}
-
-static int msic_set_selected_input_dev(u8 value)
-{
-
-	struct sc_reg_access sc_access_dmic[] = {
-		{0x24C, 0x10, 0x0},
-	};
-	struct sc_reg_access sc_access_amic[] = {
-		{0x24C, 0x76, 0x0},
-
-	};
-	int retval = 0;
-
-	pr_debug("msic_set_selected_input_dev:%d\n", value);
-	snd_msic_ops.input_dev_id = value;
-	switch (value) {
-	case AMIC:
-		pr_debug("Selecting AMIC1\n");
-		retval = sst_sc_reg_access(sc_access_amic, PMIC_WRITE, 1);
-		break;
-	case DMIC:
-		pr_debug("Selecting DMIC1\n");
-		retval = sst_sc_reg_access(sc_access_dmic, PMIC_WRITE, 1);
-		break;
-	default:
-		return -EINVAL;
-
-	}
-	if (snd_msic_ops.cap_on)
-		retval = msic_power_up_cp(SND_SST_DEVICE_CAPTURE);
-	return retval;
-}
-
-static int msic_set_hw_dmic_route(u8 hw_ch_index)
-{
-	struct sc_reg_access sc_access_router;
-	int    retval = -EINVAL;
-
-	switch (hw_ch_index) {
-	case HW_CH0:
-		sc_access_router.reg_addr = AUDIOMUX12;
-		sc_access_router.value    = snd_msic_ops.hw_dmic_map[0];
-		sc_access_router.mask     = (MASK2 | MASK1 | MASK0);
-		pr_debug("hw_ch0.  value = 0x%x\n",
-				sc_access_router.value);
-		retval = sst_sc_reg_access(&sc_access_router,
-				PMIC_READ_MODIFY, 1);
-		break;
-
-	case HW_CH1:
-		sc_access_router.reg_addr = AUDIOMUX12;
-		sc_access_router.value    = (snd_msic_ops.hw_dmic_map[1]) << 4;
-		sc_access_router.mask     = (MASK6 | MASK5 | MASK4);
-		pr_debug("### hw_ch1.  value = 0x%x\n",
-				sc_access_router.value);
-		retval = sst_sc_reg_access(&sc_access_router,
-				PMIC_READ_MODIFY, 1);
-		break;
-
-	case HW_CH2:
-		sc_access_router.reg_addr = AUDIOMUX34;
-		sc_access_router.value    = snd_msic_ops.hw_dmic_map[2];
-		sc_access_router.mask     = (MASK2 | MASK1 | MASK0);
-		pr_debug("hw_ch2.  value = 0x%x\n",
-				sc_access_router.value);
-		retval = sst_sc_reg_access(&sc_access_router,
-				PMIC_READ_MODIFY, 1);
-		break;
-
-	case HW_CH3:
-		sc_access_router.reg_addr = AUDIOMUX34;
-		sc_access_router.value    = (snd_msic_ops.hw_dmic_map[3]) << 4;
-		sc_access_router.mask     = (MASK6 | MASK5 | MASK4);
-		pr_debug("hw_ch3.  value = 0x%x\n",
-				sc_access_router.value);
-		retval = sst_sc_reg_access(&sc_access_router,
-				PMIC_READ_MODIFY, 1);
-		break;
-	}
-
-	return retval;
-}
-
-
-static int msic_set_pcm_voice_params(void)
-{
-	return 0;
-}
-
-static int msic_set_pcm_audio_params(int sfreq, int word_size, int num_channel)
-{
-	return 0;
-}
-
-static int msic_set_audio_port(int status)
-{
-	return 0;
-}
-
-static int msic_set_voice_port(int status)
-{
-	return 0;
-}
-
-static int msic_set_mute(int dev_id, u8 value)
-{
-	return 0;
-}
-
-static int msic_set_vol(int dev_id, int value)
-{
-	return 0;
-}
-
-static int msic_get_mute(int dev_id, u8 *value)
-{
-	return 0;
-}
-
-static int msic_get_vol(int dev_id, int *value)
-{
-	return 0;
-}
-
-static int msic_set_headset_state(int state)
-{
-	struct sc_reg_access hs_enable[] = {
-		{0x25D, 0x03, 0x03},
-	};
-
-	if (state)
-		/*enable*/
-		sst_sc_reg_access(hs_enable, PMIC_READ_MODIFY, 1);
-	else {
-		hs_enable[0].value = 0;
-		sst_sc_reg_access(hs_enable, PMIC_READ_MODIFY, 1);
-	}
-	return 0;
-}
-
-static int msic_enable_mic_bias(void)
-{
-	struct sc_reg_access jack_interrupt_reg[] = {
-		{0x0DB, 0x07, 0x00},
-
-	};
-	struct sc_reg_access jack_bias_reg[] = {
-		{0x247, 0x0C, 0x0C},
-	};
-
-	sst_sc_reg_access(jack_interrupt_reg, PMIC_WRITE, 1);
-	sst_sc_reg_access(jack_bias_reg, PMIC_READ_MODIFY, 1);
-	return 0;
-}
-
-static int msic_disable_mic_bias(void)
-{
-	if (snd_msic_ops.jack_interrupt_status == true)
-		return 0;
-	if (!(snd_msic_ops.pb_on || snd_msic_ops.cap_on))
-		msic_power_down();
-	return 0;
-}
-
-static int msic_disable_jack_btn(void)
-{
-	struct sc_reg_access btn_disable[] = {
-		{0x26C, 0x00, 0x01}
-	};
-
-	if (!(snd_msic_ops.pb_on || snd_msic_ops.cap_on))
-		msic_power_down();
-	snd_msic_ops.jack_interrupt_status = false;
-	return sst_sc_reg_access(btn_disable, PMIC_READ_MODIFY, 1);
-}
-
-static int msic_enable_jack_btn(void)
-{
-	struct sc_reg_access btn_enable[] = {
-			{0x26b, 0x77, 0x00},
-			{0x26C, 0x01, 0x00},
-	};
-	return sst_sc_reg_access(btn_enable, PMIC_WRITE, 2);
-}
-static int msic_convert_adc_to_mvolt(unsigned int mic_bias)
-{
-	return (ADC_ONE_LSB_MULTIPLIER * mic_bias) / 1000;
-}
-int msic_get_headset_state(int mic_bias)
-{
-	struct sc_reg_access msic_hs_toggle[] = {
-		{0x070, 0x00, 0x01},
-	};
-	if (mic_bias >= 0 && mic_bias < 400) {
-
-		pr_debug("Detected Headphone!!!\n");
-		sst_sc_reg_access(msic_hs_toggle, PMIC_READ_MODIFY, 1);
-
-	} else if (mic_bias > 400 && mic_bias < 650) {
-
-		pr_debug("Detected American headset\n");
-		msic_hs_toggle[0].value = 0x01;
-		sst_sc_reg_access(msic_hs_toggle, PMIC_READ_MODIFY, 1);
-
-	} else if (mic_bias >= 650 && mic_bias < 2000) {
-
-		pr_debug("Detected Headset!!!\n");
-		sst_sc_reg_access(msic_hs_toggle, PMIC_READ_MODIFY, 1);
-		/*power on jack and btn*/
-		snd_msic_ops.jack_interrupt_status = true;
-		msic_enable_jack_btn();
-		msic_enable_mic_bias();
-		return SND_JACK_HEADSET;
-
-	} else
-		pr_debug("Detected Open Cable!!!\n");
-
-	return SND_JACK_HEADPHONE;
-}
-
-static int msic_get_mic_bias(void *arg)
-{
-	struct snd_intelmad *intelmad_drv = (struct snd_intelmad *)arg;
-	u16 adc_adr = intelmad_drv->adc_address;
-	u16 adc_val;
-	int ret;
-	struct sc_reg_access adc_ctrl3[2] = {
-			{0x1C2, 0x05, 0x0},
-	};
-
-	struct sc_reg_access audio_adc_reg1 = {0,};
-	struct sc_reg_access audio_adc_reg2 = {0,};
-
-	msic_enable_mic_bias();
-	/* Enable the msic for conversion before reading */
-	ret = sst_sc_reg_access(adc_ctrl3, PMIC_WRITE, 1);
-	if (ret)
-		return ret;
-	adc_ctrl3[0].value = 0x04;
-	/* Re-toggle the RRDATARD bit */
-	ret = sst_sc_reg_access(adc_ctrl3, PMIC_WRITE, 1);
-	if (ret)
-		return ret;
-
-	audio_adc_reg1.reg_addr = adc_adr;
-	/* Read the higher bits of data */
-	msleep(1000);
-	ret = sst_sc_reg_access(&audio_adc_reg1, PMIC_READ, 1);
-	if (ret)
-		return ret;
-	pr_debug("adc read value %x", audio_adc_reg1.value);
-
-	/* Shift bits to accomodate the lower two data bits */
-	adc_val = (audio_adc_reg1.value << 2);
-	adc_adr++;
-	audio_adc_reg2. reg_addr = adc_adr;
-	ret = sst_sc_reg_access(&audio_adc_reg2, PMIC_READ, 1);
-	if (ret)
-		return ret;
-	pr_debug("adc read value %x", audio_adc_reg2.value);
-
-	/* Adding lower two bits to the higher bits */
-	audio_adc_reg2.value &= 03;
-	adc_val += audio_adc_reg2.value;
-
-	pr_debug("ADC value 0x%x", adc_val);
-	msic_disable_mic_bias();
-	return adc_val;
-}
-
-static void msic_pmic_irq_cb(void *cb_data, u8 intsts)
-{
-	struct mad_jack *mjack = NULL;
-	unsigned int present = 0, jack_event_flag = 0, buttonpressflag = 0;
-	struct snd_intelmad *intelmaddata = cb_data;
-	int retval = 0;
-
-	pr_debug("value returned = 0x%x\n", intsts);
-
-	if (snd_msic_ops.card_status == SND_CARD_UN_INIT) {
-		retval = msic_init_card();
-		if (retval)
-			return;
-	  }
-
-	mjack = &intelmaddata->jack[0];
-	if (intsts & 0x1) {
-		pr_debug("MAD short_push detected\n");
-		present = SND_JACK_BTN_0;
-		jack_event_flag = buttonpressflag = 1;
-		mjack->jack.type = SND_JACK_BTN_0;
-		mjack->jack.key[0] = BTN_0 ;
-	}
-
-	if (intsts & 0x2) {
-		pr_debug(":MAD long_push detected\n");
-		jack_event_flag = buttonpressflag = 1;
-		mjack->jack.type = present = SND_JACK_BTN_1;
-		mjack->jack.key[1] = BTN_1;
-	}
-
-	if (intsts & 0x4) {
-		unsigned int mic_bias;
-		jack_event_flag = 1;
-		buttonpressflag = 0;
-		mic_bias = msic_get_mic_bias(intelmaddata);
-		pr_debug("mic_bias = %d\n", mic_bias);
-		mic_bias = msic_convert_adc_to_mvolt(mic_bias);
-		pr_debug("mic_bias after conversion = %d mV\n", mic_bias);
-		mjack->jack_dev_state = msic_get_headset_state(mic_bias);
-		mjack->jack.type = present = mjack->jack_dev_state;
-	}
-
-	if (intsts & 0x8) {
-		mjack->jack.type = mjack->jack_dev_state;
-		present = 0;
-		jack_event_flag = 1;
-		buttonpressflag = 0;
-		msic_disable_jack_btn();
-		msic_disable_mic_bias();
-	}
-	if (jack_event_flag)
-		sst_mad_send_jack_report(&mjack->jack,
-					buttonpressflag, present);
-}
-
-
-
-struct snd_pmic_ops snd_msic_ops = {
-	.set_input_dev	=	msic_set_selected_input_dev,
-	.set_output_dev =	msic_set_selected_output_dev,
-	.set_lineout_dev =	msic_set_selected_lineout_dev,
-	.set_hw_dmic_route =    msic_set_hw_dmic_route,
-	.set_mute	=	msic_set_mute,
-	.get_mute	=	msic_get_mute,
-	.set_vol	=	msic_set_vol,
-	.get_vol	=	msic_get_vol,
-	.init_card	=	msic_init_card,
-	.set_pcm_audio_params	= msic_set_pcm_audio_params,
-	.set_pcm_voice_params	= msic_set_pcm_voice_params,
-	.set_voice_port = msic_set_voice_port,
-	.set_audio_port = msic_set_audio_port,
-	.power_up_pmic_pb =	msic_power_up_pb,
-	.power_up_pmic_cp =	msic_power_up_cp,
-	.power_down_pmic_pb =	msic_power_down_pb,
-	.power_down_pmic_cp =	msic_power_down_cp,
-	.power_down_pmic	=	msic_power_down,
-	.pmic_irq_cb	=	msic_pmic_irq_cb,
-	.pmic_jack_enable = msic_enable_mic_bias,
-	.pmic_get_mic_bias	= msic_get_mic_bias,
-	.pmic_set_headset_state = msic_set_headset_state,
-};
diff --git a/drivers/staging/intel_sst/intelmid_pvt.c b/drivers/staging/intel_sst/intelmid_pvt.c
deleted file mode 100644
index 90e0e64c0ab0..000000000000
--- a/drivers/staging/intel_sst/intelmid_pvt.c
+++ /dev/null
@@ -1,173 +0,0 @@
-/*
- *   intelmid_pvt.h - Intel Sound card driver for MID
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Harsha Priya <priya.harsha@intel.com>
- *		Vinod Koul <vinod.koul@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- * ALSA driver for Intel MID sound card chipset - holding private functions
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/io.h>
-#include <asm/intel_scu_ipc.h>
-#include <sound/core.h>
-#include <sound/control.h>
-#include <sound/pcm.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intelmid_snd_control.h"
-#include "intelmid.h"
-
-
-void period_elapsed(void *mad_substream)
-{
-	struct snd_pcm_substream *substream = mad_substream;
-	struct mad_stream_pvt *stream;
-
-
-
-	if (!substream || !substream->runtime)
-		return;
-	stream = substream->runtime->private_data;
-	if (!stream)
-		return;
-
-	if (stream->stream_status != RUNNING)
-		return;
-	pr_debug("calling period elapsed\n");
-	snd_pcm_period_elapsed(substream);
-	return;
-}
-
-
-int snd_intelmad_alloc_stream(struct snd_pcm_substream *substream)
-{
-	struct snd_intelmad *intelmaddata = snd_pcm_substream_chip(substream);
-	struct mad_stream_pvt *stream = substream->runtime->private_data;
-	struct snd_sst_stream_params param = {{{0,},},};
-	struct snd_sst_params str_params = {0};
-	int ret_val;
-
-	/* set codec params and inform SST driver the same */
-
-	param.uc.pcm_params.codec = SST_CODEC_TYPE_PCM;
-	param.uc.pcm_params.num_chan = (u8) substream->runtime->channels;
-	param.uc.pcm_params.pcm_wd_sz = substream->runtime->sample_bits;
-	param.uc.pcm_params.reserved = 0;
-	param.uc.pcm_params.sfreq = substream->runtime->rate;
-	param.uc.pcm_params.ring_buffer_size =
-					snd_pcm_lib_buffer_bytes(substream);
-	param.uc.pcm_params.period_count = substream->runtime->period_size;
-	param.uc.pcm_params.ring_buffer_addr =
-				virt_to_phys(substream->runtime->dma_area);
-	pr_debug("period_cnt = %d\n", param.uc.pcm_params.period_count);
-	pr_debug("sfreq= %d, wd_sz = %d\n",
-		 param.uc.pcm_params.sfreq, param.uc.pcm_params.pcm_wd_sz);
-
-	str_params.sparams = param;
-	str_params.codec = SST_CODEC_TYPE_PCM;
-
-	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
-		str_params.ops = STREAM_OPS_PLAYBACK;
-		pr_debug("Playbck stream,Device %d\n", stream->device);
-	} else {
-		str_params.ops = STREAM_OPS_CAPTURE;
-		stream->device = SND_SST_DEVICE_CAPTURE;
-		pr_debug("Capture stream,Device %d\n", stream->device);
-	}
-	str_params.device_type = stream->device;
-	ret_val = intelmaddata->sstdrv_ops->pcm_control->open(&str_params);
-	pr_debug("sst: SST_SND_PLAY/CAPTURE ret_val = %x\n", ret_val);
-	if (ret_val < 0)
-		return ret_val;
-
-	stream->stream_info.str_id = ret_val;
-	stream->stream_status = INIT;
-	stream->stream_info.buffer_ptr = 0;
-	pr_debug("str id :  %d\n", stream->stream_info.str_id);
-
-	return ret_val;
-}
-
-int snd_intelmad_init_stream(struct snd_pcm_substream *substream)
-{
-	struct mad_stream_pvt *stream = substream->runtime->private_data;
-	struct snd_intelmad *intelmaddata = snd_pcm_substream_chip(substream);
-	int ret_val;
-
-	pr_debug("setting buffer ptr param\n");
-	stream->stream_info.period_elapsed = period_elapsed;
-	stream->stream_info.mad_substream = substream;
-	stream->stream_info.buffer_ptr = 0;
-	stream->stream_info.sfreq = substream->runtime->rate;
-	ret_val = intelmaddata->sstdrv_ops->pcm_control->device_control(
-			SST_SND_STREAM_INIT, &stream->stream_info);
-	if (ret_val)
-		pr_err("control_set ret error %d\n", ret_val);
-	return ret_val;
-
-}
-
-
-/**
- * sst_sc_reg_access - IPC read/write wrapper
- *
- * @sc_access:  array of data, addresses and mask
- * @type: operation type
- * @num_val: number of reg to opertae on
- *
- * Reads/writes/read-modify operations on registers accessed through SCU (sound
- * card and few SST DSP regsisters that are not accissible to IA)
- */
-int sst_sc_reg_access(struct sc_reg_access *sc_access,
-					int type, int num_val)
-{
-	int i, retval = 0;
-	if (type == PMIC_WRITE) {
-		for (i = 0; i < num_val; i++) {
-			retval = intel_scu_ipc_iowrite8(sc_access[i].reg_addr,
-							sc_access[i].value);
-			if (retval)
-				goto err;
-		}
-	} else if (type == PMIC_READ) {
-		for (i = 0; i < num_val; i++) {
-			retval = intel_scu_ipc_ioread8(sc_access[i].reg_addr,
-							&(sc_access[i].value));
-			if (retval)
-				goto err;
-		}
-	} else {
-		for (i = 0; i < num_val; i++) {
-			retval = intel_scu_ipc_update_register(
-				sc_access[i].reg_addr, sc_access[i].value,
-				sc_access[i].mask);
-			if (retval)
-				goto err;
-		}
-	}
-	return 0;
-err:
-	pr_err("IPC failed for cmd %d, %d\n", retval, type);
-	pr_err("reg:0x%2x addr:0x%2x\n",
-		sc_access[i].reg_addr, sc_access[i].value);
- 	return retval;
-}
diff --git a/drivers/staging/intel_sst/intelmid_snd_control.h b/drivers/staging/intel_sst/intelmid_snd_control.h
deleted file mode 100644
index 06ad3a10099c..000000000000
--- a/drivers/staging/intel_sst/intelmid_snd_control.h
+++ /dev/null
@@ -1,123 +0,0 @@
-#ifndef __INTELMID_SND_CTRL_H__
-#define __INTELMID_SND_CTRL_H__
-/*
- *  intelmid_snd_control.h - Intel Sound card driver for MID
- *
- *  Copyright (C) 2008-10 Intel Corporation
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This file defines all snd control functions
- */
-
-/*
-Mask bits
-*/
-#define MASK0 0x01	/* 0000 0001 */
-#define MASK1 0x02	/* 0000 0010 */
-#define MASK2 0x04	/* 0000 0100 */
-#define MASK3 0x08	/* 0000 1000 */
-#define MASK4 0x10	/* 0001 0000 */
-#define MASK5 0x20	/* 0010 0000 */
-#define MASK6 0x40	/* 0100 0000 */
-#define MASK7 0x80	/* 1000 0000 */
-/*
-value bits
-*/
-#define VALUE0	0x01	/* 0000 0001 */
-#define VALUE1	0x02	/* 0000 0010 */
-#define VALUE2	0x04	/* 0000 0100 */
-#define VALUE3	0x08	/* 0000 1000 */
-#define VALUE4	0x10	/* 0001 0000 */
-#define VALUE5	0x20	/* 0010 0000 */
-#define VALUE6	0x40	/* 0100 0000 */
-#define VALUE7	0x80	/* 1000 0000 */
-
-#define MUTE 0    /* ALSA Passes 0 for mute */
-#define UNMUTE 1  /* ALSA Passes 1 for unmute */
-
-#define MAX_VOL_PMIC_VENDOR0    0x3f /* max vol in dB for stereo & voice DAC */
-#define MIN_VOL_PMIC_VENDOR0    0 /* min vol in dB for stereo & voice DAC */
-/* Head phone volume control  */
-#define MAX_HP_VOL_PMIC_VENDOR1    6 /* max volume in dB for HP */
-#define MIN_HP_VOL_PMIC_VENDOR1    (-84) /* min volume in dB for HP */
-#define MAX_HP_VOL_INDX_PMIC_VENDOR1 40 /* Number of HP volume control values */
-
-/* Mono Earpiece Volume control */
-#define MAX_EP_VOL_PMIC_VENDOR1    0 /* max volume in dB for EP */
-#define MIN_EP_VOL_PMIC_VENDOR1    (-75) /* min volume in dB for EP */
-#define MAX_EP_VOL_INDX_PMIC_VENDOR1 32 /* Number of EP volume control values */
-
-int sst_sc_reg_access(struct sc_reg_access *sc_access,
-					int type, int num_val);
-extern struct snd_pmic_ops snd_pmic_ops_fs;
-extern struct snd_pmic_ops snd_pmic_ops_mx;
-extern struct snd_pmic_ops snd_pmic_ops_nc;
-extern struct snd_pmic_ops snd_msic_ops;
-
-/* device */
-enum SND_INPUT_DEVICE {
-	AMIC,
-	DMIC,
-	HS_MIC,
-	IN_UNDEFINED
-};
-enum SND_LINE_OUT_DEVICE {
-	HEADSET,
-	IHF,
-	VIBRA1,
-	VIBRA2,
-	NONE,
-};
-
-enum SND_OUTPUT_DEVICE {
-	STEREO_HEADPHONE,
-	MONO_EARPIECE,
-
-	INTERNAL_SPKR,
-	RECEIVER,
-	OUT_UNDEFINED
-};
-
-enum pmic_controls {
-	PMIC_SND_HP_MIC_MUTE =			0x0001,
-	PMIC_SND_AMIC_MUTE =			0x0002,
-	PMIC_SND_DMIC_MUTE =			0x0003,
-	PMIC_SND_CAPTURE_VOL =			0x0004,
-/* Output controls */
-	PMIC_SND_LEFT_PB_VOL =			0x0010,
-	PMIC_SND_RIGHT_PB_VOL =			0x0011,
-	PMIC_SND_LEFT_HP_MUTE =			0x0012,
-	PMIC_SND_RIGHT_HP_MUTE =		0x0013,
-	PMIC_SND_LEFT_SPEAKER_MUTE =		0x0014,
-	PMIC_SND_RIGHT_SPEAKER_MUTE =		0x0015,
-	PMIC_SND_RECEIVER_VOL =			0x0016,
-	PMIC_SND_RECEIVER_MUTE =		0x0017,
-	PMIC_SND_LEFT_MASTER_VOL =		0x0018,
-	PMIC_SND_RIGHT_MASTER_VOL =		0x0019,
-/* Other controls */
-	PMIC_SND_MUTE_ALL =			0x0020,
-	PMIC_MAX_CONTROLS =			0x0020,
-};
-
-#endif /* __INTELMID_SND_CTRL_H__ */
-
-
diff --git a/drivers/staging/intel_sst/intelmid_v0_control.c b/drivers/staging/intel_sst/intelmid_v0_control.c
deleted file mode 100644
index b8dfdb9bc1aa..000000000000
--- a/drivers/staging/intel_sst/intelmid_v0_control.c
+++ /dev/null
@@ -1,866 +0,0 @@
-/*
- *  intel_sst_v0_control.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corporation
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This file contains the control operations of vendor 1
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/file.h>
-#include <sound/control.h>
-#include "intel_sst.h"
-#include "intelmid_snd_control.h"
-#include "intelmid.h"
-
-enum _reg_v1 {
-	VOICEPORT1 = 0x180,
-	VOICEPORT2 = 0x181,
-	AUDIOPORT1 = 0x182,
-	AUDIOPORT2 = 0x183,
-	MISCVOICECTRL = 0x184,
-	MISCAUDCTRL = 0x185,
-	DMICCTRL1 = 0x186,
-	AUDIOBIAS = 0x187,
-	MICCTRL = 0x188,
-	MICLICTRL1 = 0x189,
-	MICLICTRL2 = 0x18A,
-	MICLICTRL3 = 0x18B,
-	VOICEDACCTRL1 = 0x18C,
-	STEREOADCCTRL = 0x18D,
-	AUD15 = 0x18E,
-	AUD16 = 0x18F,
-	AUD17 = 0x190,
-	AUD18 = 0x191,
-	RMIXOUTSEL = 0x192,
-	ANALOGLBR = 0x193,
-	ANALOGLBL = 0x194,
-	POWERCTRL1 = 0x195,
-	POWERCTRL2 = 0x196,
-	HEADSETDETECTINT = 0x197,
-	HEADSETDETECTINTMASK = 0x198,
-	TRIMENABLE = 0x199,
-};
-
-int rev_id = 0x20;
-static bool jack_det_enabled;
-
-/****
- * fs_init_card - initialize the sound card
- *
- * This initializes the audio paths to know values in case of this sound card
- */
-static int fs_init_card(void)
-{
-	struct sc_reg_access sc_access[] = {
-		{0x180, 0x00, 0x0},
-		{0x181, 0x00, 0x0},
-		{0x182, 0xF8, 0x0},
-		{0x183, 0x08, 0x0},
-		{0x184, 0x00, 0x0},
-		{0x185, 0x40, 0x0},
-		{0x186, 0x06, 0x0},
-		{0x187, 0x80, 0x0},
-		{0x188, 0x40, 0x0},
-		{0x189, 0x39, 0x0},
-		{0x18a, 0x39, 0x0},
-		{0x18b, 0x1F, 0x0},
-		{0x18c, 0x00, 0x0},
-		{0x18d, 0x00, 0x0},
-		{0x18e, 0x39, 0x0},
-		{0x18f, 0x39, 0x0},
-		{0x190, 0x39, 0x0},
-		{0x191, 0x11, 0x0},
-		{0x192, 0x0E, 0x0},
-		{0x193, 0x00, 0x0},
-		{0x194, 0x00, 0x0},
-		{0x195, 0x00, 0x0},
-		{0x196, 0x7C, 0x0},
-		{0x197, 0x00, 0x0},
-		{0x198, 0x0B, 0x0},
-		{0x199, 0x00, 0x0},
-		{0x037, 0x3F, 0x0},
-	};
-
-	snd_pmic_ops_fs.card_status = SND_CARD_INIT_DONE;
-	snd_pmic_ops_fs.master_mute = UNMUTE;
-	snd_pmic_ops_fs.mute_status = UNMUTE;
-	snd_pmic_ops_fs.num_channel = 2;
-	return sst_sc_reg_access(sc_access, PMIC_WRITE, 27);
-}
-
-static int fs_enable_audiodac(int value)
-{
-	struct sc_reg_access sc_access[3];
-	sc_access[0].reg_addr = AUD16;
-	sc_access[1].reg_addr = AUD17;
-	sc_access[2].reg_addr = AUD15;
-	sc_access[0].mask = sc_access[1].mask = sc_access[2].mask = MASK7;
-
-	if (snd_pmic_ops_fs.mute_status == MUTE)
-		return 0;
-	if (value == MUTE) {
-			sc_access[0].value = sc_access[1].value =
-				sc_access[2].value = 0x80;
-
-		} else {
-			sc_access[0].value = sc_access[1].value =
-			sc_access[2].value =  0x0;
-		}
-	if (snd_pmic_ops_fs.num_channel == 1)
-		sc_access[1].value = sc_access[2].value = 0x80;
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 3);
-
-}
-
-static int fs_power_up_pb(unsigned int port)
-{
-	struct sc_reg_access sc_access[] = {
-		{AUDIOBIAS, 0x00, MASK7},
-		{POWERCTRL1, 0xC6, 0xC6},
-		{POWERCTRL2, 0x30, 0x30},
-
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	retval = fs_enable_audiodac(MUTE);
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 3);
-
-	if (retval)
-		return retval;
-
-	pr_debug("in fs power up pb\n");
-	return fs_enable_audiodac(UNMUTE);
-}
-
-static int fs_power_down_pb(unsigned int device)
-{
-	struct sc_reg_access sc_access[] = {
-		{POWERCTRL1, 0x00, 0xC6},
-		{POWERCTRL2, 0x00, 0x30},
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	retval = fs_enable_audiodac(MUTE);
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-
-	if (retval)
-		return retval;
-
-	pr_debug("in fsl power down pb\n");
-	return fs_enable_audiodac(UNMUTE);
-}
-
-static int fs_power_up_cp(unsigned int port)
-{
-	struct sc_reg_access sc_access[] = {
-		{POWERCTRL2, 0x32, 0x32}, /*NOTE power up A ADC only as*/
-		{AUDIOBIAS, 0x00, MASK7},
-					/*as turning on V ADC causes noise*/
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-}
-
-static int fs_power_down_cp(unsigned int device)
-{
-	struct sc_reg_access sc_access[] = {
-		{POWERCTRL2, 0x00, 0x03},
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-}
-
-static int fs_power_down(void)
-{
-	int retval = 0;
-	struct sc_reg_access sc_access[] = {
-		{AUDIOBIAS, MASK7, MASK7},
-	};
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-}
-
-static int fs_set_pcm_voice_params(void)
-{
-	struct sc_reg_access sc_access[] = {
-		{0x180, 0xA0, 0},
-		{0x181, 0x04, 0},
-		{0x182, 0x0, 0},
-		{0x183, 0x0, 0},
-		{0x184, 0x18, 0},
-		{0x185, 0x40, 0},
-		{0x186, 0x06, 0},
-		{0x187, 0x0, 0},
-		{0x188, 0x10, 0},
-		{0x189, 0x39, 0},
-		{0x18a, 0x39, 0},
-		{0x18b, 0x02, 0},
-		{0x18c, 0x0, 0},
-		{0x18d, 0x0, 0},
-		{0x18e, 0x39, 0},
-		{0x18f, 0x0, 0},
-		{0x190, 0x0, 0},
-		{0x191, 0x20, 0},
-		{0x192, 0x20, 0},
-		{0x193, 0x0, 0},
-		{0x194, 0x0, 0},
-		{0x195, 0x06, 0},
-		{0x196, 0x25, 0},
-		{0x197, 0x0, 0},
-		{0x198, 0xF, 0},
-		{0x199, 0x0, 0},
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	return sst_sc_reg_access(sc_access, PMIC_WRITE, 26);
-}
-
-static int fs_set_audio_port(int status)
-{
-	struct sc_reg_access sc_access[2];
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	if (status == DEACTIVATE) {
-		/* Deactivate audio port-tristate and power */
-		sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK6|MASK7;
-		sc_access[0].reg_addr = AUDIOPORT1;
-		sc_access[1].value = 0x00;
-		sc_access[1].mask = MASK4|MASK5;
-		sc_access[1].reg_addr = POWERCTRL2;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-	} else if (status == ACTIVATE) {
-		/* activate audio port */
-		sc_access[0].value = 0xC0;
-		sc_access[0].mask = MASK6|MASK7;
-		sc_access[0].reg_addr = AUDIOPORT1;
-		sc_access[1].value = 0x30;
-		sc_access[1].mask = MASK4|MASK5;
-		sc_access[1].reg_addr = POWERCTRL2;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-	} else
-		return -EINVAL;
-}
-
-static int fs_set_voice_port(int status)
-{
-	struct sc_reg_access sc_access[2];
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	if (status == DEACTIVATE) {
-		/* Deactivate audio port-tristate and power */
-		sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK6|MASK7;
-		sc_access[0].reg_addr = VOICEPORT1;
-		sc_access[1].value = 0x00;
-		sc_access[1].mask = MASK0|MASK1;
-		sc_access[1].reg_addr = POWERCTRL2;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-	} else if (status == ACTIVATE) {
-		/* activate audio port */
-		sc_access[0].value = 0xC0;
-		sc_access[0].mask = MASK6|MASK7;
-		sc_access[0].reg_addr = VOICEPORT1;
-		sc_access[1].value = 0x03;
-		sc_access[1].mask = MASK0|MASK1;
-		sc_access[1].reg_addr = POWERCTRL2;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-	} else
-		return -EINVAL;
-}
-
-static int fs_set_pcm_audio_params(int sfreq, int word_size, int num_channel)
-{
-	u8 config1 = 0;
-	struct sc_reg_access sc_access[4];
-	int retval = 0, num_value = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-	switch (sfreq) {
-	case 8000:
-		config1 = 0x00;
-		break;
-	case 11025:
-		config1 = 0x01;
-		break;
-	case 12000:
-		config1 = 0x02;
-		break;
-	case 16000:
-		config1 = 0x03;
-		break;
-	case 22050:
-		config1 = 0x04;
-		break;
-	case 24000:
-		config1 = 0x05;
-		break;
-	case 26000:
-		config1 = 0x06;
-		break;
-	case 32000:
-		config1 = 0x07;
-		break;
-	case 44100:
-		config1 = 0x08;
-		break;
-	case 48000:
-		config1 = 0x09;
-		break;
-	}
-	snd_pmic_ops_fs.num_channel = num_channel;
-	if (snd_pmic_ops_fs.num_channel == 1)	{
-		sc_access[0].reg_addr = AUD17;
-		sc_access[1].reg_addr = AUD15;
-		sc_access[0].mask = sc_access[1].mask = MASK7;
-		sc_access[0].value = sc_access[1].value = 0x80;
-		sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-
-	} else {
-		sc_access[0].reg_addr = AUD17;
-		sc_access[1].reg_addr = AUD15;
-		sc_access[0].mask = sc_access[1].mask = MASK7;
-		sc_access[0].value = sc_access[1].value = 0x00;
-		sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-
-	}
-	pr_debug("sfreq:%d,Register value = %x\n", sfreq, config1);
-
-	if (word_size == 24) {
-		sc_access[0].reg_addr  = AUDIOPORT1;
-		sc_access[0].mask = MASK0|MASK1|MASK2|MASK3;
-		sc_access[0].value = 0xFB;
-
-
-		sc_access[1].reg_addr  = AUDIOPORT2;
-		sc_access[1].value = config1 | 0x10;
-		sc_access[1].mask = MASK0 | MASK1 | MASK2 | MASK3
-					| MASK4 | MASK5 | MASK6;
-
-		sc_access[2].reg_addr  = MISCAUDCTRL;
-		sc_access[2].value = 0x02;
-		sc_access[2].mask = 0x02;
-
-		num_value = 3 ;
-
-	} else {
-
-		sc_access[0].reg_addr  = AUDIOPORT2;
-		sc_access[0].value = config1;
-		sc_access[0].mask = MASK0|MASK1|MASK2|MASK3;
-
-		sc_access[1].reg_addr  = MISCAUDCTRL;
-		sc_access[1].value = 0x00;
-		sc_access[1].mask = 0x02;
-		num_value = 2;
-	}
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, num_value);
-
-}
-
-static int fs_set_selected_input_dev(u8 value)
-{
-	struct sc_reg_access sc_access_dmic[] = {
-		{MICCTRL, 0x81, 0xf7},
-		{MICLICTRL3, 0x00, 0xE0},
-	};
-	struct sc_reg_access sc_access_mic[] = {
-		{MICCTRL, 0x40, MASK2|MASK4|MASK5|MASK6|MASK7},
-		{MICLICTRL3, 0x00, 0xE0},
-	};
-	struct sc_reg_access sc_access_hsmic[] = {
-		{MICCTRL, 0x10, MASK2|MASK4|MASK5|MASK6|MASK7},
-		{MICLICTRL3, 0x00, 0xE0},
-	};
-
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-
-	switch (value) {
-	case AMIC:
-		pr_debug("Selecting amic not supported in mono cfg\n");
-		return sst_sc_reg_access(sc_access_mic, PMIC_READ_MODIFY, 2);
-		break;
-
-	case HS_MIC:
-		pr_debug("Selecting hsmic\n");
-		return sst_sc_reg_access(sc_access_hsmic,
-				PMIC_READ_MODIFY, 2);
-		break;
-
-	case DMIC:
-		pr_debug("Selecting dmic\n");
-		return sst_sc_reg_access(sc_access_dmic, PMIC_READ_MODIFY, 2);
-		break;
-
-	default:
-		return -EINVAL;
-
-	}
-}
-
-static int fs_set_selected_output_dev(u8 value)
-{
-	struct sc_reg_access sc_access_hp[] = {
-		{0x191, 0x11, 0x0},
-		{0x192, 0x0E, 0x0},
-	};
-	struct sc_reg_access sc_access_is[] = {
-		{0x191, 0x17, 0xFF},
-		{0x192, 0x08, 0xFF},
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-
-	switch (value) {
-	case STEREO_HEADPHONE:
-		pr_debug("SST DBG:Selecting headphone\n");
-		return sst_sc_reg_access(sc_access_hp, PMIC_WRITE, 2);
-		break;
-	case MONO_EARPIECE:
-	case INTERNAL_SPKR:
-		pr_debug("SST DBG:Selecting internal spkr\n");
-		return sst_sc_reg_access(sc_access_is, PMIC_READ_MODIFY, 2);
-		break;
-
-	default:
-		return -EINVAL;
-
-	}
-}
-
-static int fs_set_mute(int dev_id, u8 value)
-{
-	struct sc_reg_access sc_access[6] = {{0,},};
-	int reg_num = 0;
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-
-
-	pr_debug("dev_id:0x%x value:0x%x\n", dev_id, value);
-	switch (dev_id) {
-	case PMIC_SND_DMIC_MUTE:
-		sc_access[0].reg_addr = MICCTRL;
-		sc_access[1].reg_addr = MICLICTRL1;
-		sc_access[2].reg_addr = MICLICTRL2;
-		sc_access[0].mask = MASK5;
-		sc_access[1].mask = sc_access[2].mask = MASK6;
-		if (value == MUTE) {
-			sc_access[0].value = 0x20;
-			sc_access[2].value = sc_access[1].value = 0x40;
-		} else
-			sc_access[0].value = sc_access[1].value
-			= sc_access[2].value = 0x0;
-		reg_num = 3;
-		break;
-	case PMIC_SND_HP_MIC_MUTE:
-	case PMIC_SND_AMIC_MUTE:
-		sc_access[0].reg_addr = MICLICTRL1;
-		sc_access[1].reg_addr = MICLICTRL2;
-		sc_access[0].mask = sc_access[1].mask = MASK6;
-		if (value == MUTE)
-			sc_access[0].value = sc_access[1].value = 0x40;
-		else
-			sc_access[0].value = sc_access[1].value = 0x0;
-		reg_num = 2;
-		break;
-	case PMIC_SND_LEFT_SPEAKER_MUTE:
-	case PMIC_SND_LEFT_HP_MUTE:
-		sc_access[0].reg_addr = AUD16;
-		sc_access[1].reg_addr = AUD15;
-
-		sc_access[0].mask = sc_access[1].mask = MASK7;
-		if (value == MUTE)
-			sc_access[0].value = sc_access[1].value = 0x80;
-		else
-			sc_access[0].value = sc_access[1].value = 0x0;
-		reg_num = 2;
-		snd_pmic_ops_fs.mute_status = value;
-		break;
-	case PMIC_SND_RIGHT_HP_MUTE:
-	case PMIC_SND_RIGHT_SPEAKER_MUTE:
-		sc_access[0].reg_addr = AUD17;
-		sc_access[1].reg_addr = AUD15;
-		sc_access[0].mask = sc_access[1].mask = MASK7;
-		if (value == MUTE)
-			sc_access[0].value = sc_access[1].value = 0x80;
-		else
-			sc_access[0].value = sc_access[1].value = 0x0;
-		snd_pmic_ops_fs.mute_status = value;
-		if (snd_pmic_ops_fs.num_channel == 1)
-			sc_access[0].value = sc_access[1].value = 0x80;
-		reg_num = 2;
-		break;
-	case PMIC_SND_MUTE_ALL:
-		sc_access[0].reg_addr = AUD16;
-		sc_access[1].reg_addr = AUD17;
-		sc_access[2].reg_addr = AUD15;
-		sc_access[3].reg_addr = MICCTRL;
-		sc_access[4].reg_addr = MICLICTRL1;
-		sc_access[5].reg_addr = MICLICTRL2;
-		sc_access[0].mask = sc_access[1].mask =
-				sc_access[2].mask = MASK7;
-		sc_access[3].mask = MASK5;
-		sc_access[4].mask = sc_access[5].mask = MASK6;
-
-		if (value == MUTE) {
-			sc_access[0].value =
-			sc_access[1].value = sc_access[2].value = 0x80;
-			sc_access[3].value = 0x20;
-			sc_access[4].value = sc_access[5].value = 0x40;
-
-		} else {
-			sc_access[0].value = sc_access[1].value =
-			sc_access[2].value = sc_access[3].value =
-			sc_access[4].value = sc_access[5].value = 0x0;
-		}
-		if (snd_pmic_ops_fs.num_channel == 1)
-			sc_access[1].value = sc_access[2].value = 0x80;
-		reg_num = 6;
-		snd_pmic_ops_fs.mute_status = value;
-		snd_pmic_ops_fs.master_mute = value;
-		break;
-
-	}
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, reg_num);
-}
-
-static int fs_set_vol(int dev_id, int value)
-{
-	struct sc_reg_access sc_acces, sc_access[4] = {{0},};
-	int reg_num = 0;
-	int retval = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-
-	switch (dev_id) {
-	case PMIC_SND_LEFT_PB_VOL:
-		pr_debug("PMIC_SND_LEFT_PB_VOL:%d\n", value);
-		sc_access[0].value = sc_access[1].value = value;
-		sc_access[0].reg_addr = AUD16;
-		sc_access[1].reg_addr = AUD15;
-		sc_access[0].mask = sc_access[1].mask =
-			(MASK0|MASK1|MASK2|MASK3|MASK4|MASK5);
-		reg_num = 2;
-		break;
-
-	case PMIC_SND_RIGHT_PB_VOL:
-		pr_debug("PMIC_SND_RIGHT_PB_VOL:%d\n", value);
-		sc_access[0].value = sc_access[1].value = value;
-		sc_access[0].reg_addr = AUD17;
-		sc_access[1].reg_addr = AUD15;
-		sc_access[0].mask = sc_access[1].mask =
-			(MASK0|MASK1|MASK2|MASK3|MASK4|MASK5);
-		if (snd_pmic_ops_fs.num_channel == 1) {
-			sc_access[0].value = sc_access[1].value = 0x80;
-			sc_access[0].mask = sc_access[1].mask = MASK7;
-		}
-		reg_num = 2;
-		break;
-	case PMIC_SND_CAPTURE_VOL:
-		pr_debug("PMIC_SND_CAPTURE_VOL:%d\n", value);
-		sc_access[0].reg_addr = MICLICTRL1;
-		sc_access[1].reg_addr = MICLICTRL2;
-		sc_access[2].reg_addr = DMICCTRL1;
-		sc_access[2].value = value;
-		sc_access[0].value = sc_access[1].value = value;
-		sc_acces.reg_addr = MICLICTRL3;
-		sc_acces.value = value;
-		sc_acces.mask = (MASK0|MASK1|MASK2|MASK3|MASK5|MASK6|MASK7);
-		retval = sst_sc_reg_access(&sc_acces, PMIC_READ_MODIFY, 1);
-		sc_access[0].mask = sc_access[1].mask =
-		sc_access[2].mask = (MASK0|MASK1|MASK2|MASK3|MASK4|MASK5);
-		reg_num = 3;
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, reg_num);
-}
-
-static int fs_get_mute(int dev_id, u8 *value)
-{
-	struct sc_reg_access sc_access[6] = {{0,},};
-
-	int retval = 0, temp_value = 0, mask = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-
-	switch (dev_id) {
-
-	case PMIC_SND_AMIC_MUTE:
-	case PMIC_SND_HP_MIC_MUTE:
-		sc_access[0].reg_addr = MICLICTRL1;
-		mask = MASK6;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ, 1);
-		if (sc_access[0].value & mask)
-			*value = MUTE;
-		else
-			*value = UNMUTE;
-		break;
-	case PMIC_SND_DMIC_MUTE:
-		sc_access[0].reg_addr = MICCTRL;
-		mask = MASK5;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ, 1);
-		temp_value = (sc_access[0].value & mask);
-		if (temp_value == 0)
-			*value = UNMUTE;
-		else
-			*value = MUTE;
-		break;
-
-	case PMIC_SND_LEFT_HP_MUTE:
-	case PMIC_SND_LEFT_SPEAKER_MUTE:
-		sc_access[0].reg_addr = AUD16;
-		mask = MASK7;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ, 1);
-		temp_value = sc_access[0].value & mask;
-		if (temp_value == 0)
-			*value = UNMUTE;
-		else
-			*value = MUTE;
-		break;
-	case PMIC_SND_RIGHT_HP_MUTE:
-	case PMIC_SND_RIGHT_SPEAKER_MUTE:
-		sc_access[0].reg_addr = AUD17;
-		mask = MASK7;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ, 1);
-		temp_value = sc_access[0].value & mask;
-		if (temp_value == 0)
-			*value = UNMUTE;
-		else
-			*value = MUTE;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return retval;
-}
-
-static int fs_get_vol(int dev_id, int *value)
-{
-	struct sc_reg_access sc_access = {0,};
-	int retval = 0, mask = 0;
-
-	if (snd_pmic_ops_fs.card_status == SND_CARD_UN_INIT)
-		retval = fs_init_card();
-	if (retval)
-		return retval;
-
-	switch (dev_id) {
-	case PMIC_SND_CAPTURE_VOL:
-		pr_debug("PMIC_SND_CAPTURE_VOL\n");
-		sc_access.reg_addr = MICLICTRL1;
-		mask = (MASK5|MASK4|MASK3|MASK2|MASK1|MASK0);
-		break;
-	case PMIC_SND_LEFT_PB_VOL:
-		pr_debug("PMIC_SND_LEFT_PB_VOL\n");
-		sc_access.reg_addr = AUD16;
-		mask = (MASK5|MASK4|MASK3|MASK2|MASK1|MASK0);
-		break;
-	case PMIC_SND_RIGHT_PB_VOL:
-		pr_debug("PMIC_SND_RT_PB_VOL\n");
-		sc_access.reg_addr = AUD17;
-		mask = (MASK5|MASK4|MASK3|MASK2|MASK1|MASK0);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	retval = sst_sc_reg_access(&sc_access, PMIC_READ, 1);
-	pr_debug("value read = 0x%x\n", sc_access.value);
-	*value = (int) (sc_access.value & mask);
-	pr_debug("value returned = 0x%x\n", *value);
-	return retval;
-}
-
-static void fs_pmic_irq_enable(void *data)
-{
-	struct snd_intelmad *intelmaddata = data;
-	struct sc_reg_access sc_access[] = {
-				{0x187, 0x00, MASK7},
-				{0x188, 0x10, MASK4},
-				{0x18b, 0x10, MASK4},
-	};
-
-	struct sc_reg_access sc_access_write[] = {
-				{0x198, 0x00, 0x0},
-	};
-	pr_debug("Audio interrupt enable\n");
-	sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 3);
-	sst_sc_reg_access(sc_access_write, PMIC_WRITE, 1);
-
-	intelmaddata->jack[0].jack_status = 0;
-	/*intelmaddata->jack[1].jack_status = 0;*/
-
-	jack_det_enabled = true;
-	return;
-}
-
-static void fs_pmic_irq_cb(void *cb_data, u8 value)
-{
-	struct mad_jack *mjack = NULL;
-	struct snd_intelmad *intelmaddata = cb_data;
-	unsigned int present = 0, jack_event_flag = 0, buttonpressflag = 0;
-
-	mjack = &intelmaddata->jack[0];
-
-	if (value & 0x4) {
-		if (!jack_det_enabled)
-			fs_pmic_irq_enable(intelmaddata);
-
-		/* send headphone detect */
-		pr_debug(":MAD headphone %d\n", value & 0x4);
-		present = !(mjack->jack_status);
-		mjack->jack_status = present;
-		jack_event_flag = 1;
-		mjack->jack.type = SND_JACK_HEADPHONE;
-	}
-
-	if (value & 0x2) {
-		/* send short push */
-		pr_debug(":MAD short push %d\n", value & 0x2);
-		present = 1;
-		jack_event_flag = 1;
-		buttonpressflag = 1;
-		mjack->jack.type = MID_JACK_HS_SHORT_PRESS;
-	}
-
-	if (value & 0x1) {
-		/* send long push */
-		pr_debug(":MAD long push %d\n", value & 0x1);
-		present = 1;
-		jack_event_flag = 1;
-		buttonpressflag = 1;
-		mjack->jack.type = MID_JACK_HS_LONG_PRESS;
-	}
-
-	if (value & 0x8) {
-		if (!jack_det_enabled)
-			fs_pmic_irq_enable(intelmaddata);
-		/* send headset detect */
-		pr_debug(":MAD headset = %d\n", value & 0x8);
-		present = !(mjack->jack_status);
-		mjack->jack_status = present;
-		jack_event_flag = 1;
-		mjack->jack.type = SND_JACK_HEADSET;
-	}
-
-
-	if (jack_event_flag)
-		sst_mad_send_jack_report(&mjack->jack,
-						buttonpressflag, present);
-
-	return;
-}
-static int fs_jack_enable(void)
-{
-	return 0;
-}
-
-struct snd_pmic_ops snd_pmic_ops_fs = {
-	.set_input_dev = fs_set_selected_input_dev,
-	.set_output_dev = fs_set_selected_output_dev,
-	.set_mute = fs_set_mute,
-	.get_mute = fs_get_mute,
-	.set_vol = fs_set_vol,
-	.get_vol = fs_get_vol,
-	.init_card = fs_init_card,
-	.set_pcm_audio_params = fs_set_pcm_audio_params,
-	.set_pcm_voice_params = fs_set_pcm_voice_params,
-	.set_voice_port = fs_set_voice_port,
-	.set_audio_port = fs_set_audio_port,
-	.power_up_pmic_pb =	fs_power_up_pb,
-	.power_up_pmic_cp =	fs_power_up_cp,
-	.power_down_pmic_pb =	fs_power_down_pb,
-	.power_down_pmic_cp =	fs_power_down_cp,
-	.power_down_pmic	=	fs_power_down,
-	.pmic_irq_cb	=	fs_pmic_irq_cb,
-	/*
-	 * Jack detection enabling
-	 * need be delayed till first IRQ happen.
-	 */
-	.pmic_irq_enable =	NULL,
-	.pmic_jack_enable = fs_jack_enable,
-};
diff --git a/drivers/staging/intel_sst/intelmid_v1_control.c b/drivers/staging/intel_sst/intelmid_v1_control.c
deleted file mode 100644
index 9d00728d8deb..000000000000
--- a/drivers/staging/intel_sst/intelmid_v1_control.c
+++ /dev/null
@@ -1,978 +0,0 @@
-/*  intel_sst_v1_control.c - Intel SST Driver for audio engine
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *	Harsha Priya <priya.harsha@intel.com>
- *	Dharageswari R <dharageswari.r@intel.com>
- *	KP Jeeja <jeeja.kp@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This file contains the control operations of vendor 2
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/pci.h>
-#include <linux/delay.h>
-#include <linux/file.h>
-#include <asm/mrst.h>
-#include <sound/pcm.h>
-#include <sound/pcm_params.h>
-#include <sound/control.h>
-#include <sound/initval.h>
-#include "intel_sst.h"
-#include "intel_sst_ioctl.h"
-#include "intelmid.h"
-#include "intelmid_snd_control.h"
-
-#include <linux/gpio.h>
-#define KOSKI_VOICE_CODEC_ENABLE 46
-
-enum _reg_v2 {
-
-	MASTER_CLOCK_PRESCALAR  = 0x205,
-	SET_MASTER_AND_LR_CLK1	= 0x20b,
-	SET_MASTER_AND_LR_CLK2	= 0x20c,
-	MASTER_MODE_AND_DATA_DELAY = 0x20d,
-	DIGITAL_INTERFACE_TO_DAI2 = 0x20e,
-	CLK_AND_FS1 = 0x208,
-	CLK_AND_FS2 = 0x209,
-	DAI2_TO_DAC_HP = 0x210,
-	HP_OP_SINGLE_ENDED = 0x224,
-	ENABLE_OPDEV_CTRL = 0x226,
-	ENABLE_DEV_AND_USE_XTAL = 0x227,
-
-	/* Max audio subsystem (PQ49) MAX 8921 */
-	AS_IP_MODE_CTL = 0xF9,
-	AS_LEFT_SPKR_VOL_CTL = 0xFA, /* Mono Earpiece volume control */
-	AS_RIGHT_SPKR_VOL_CTL = 0xFB,
-	AS_LEFT_HP_VOL_CTL = 0xFC,
-	AS_RIGHT_HP_VOL_CTL = 0xFD,
-	AS_OP_MIX_CTL = 0xFE,
-	AS_CONFIG = 0xFF,
-
-	/* Headphone volume control & mute registers */
-	VOL_CTRL_LT = 0x21c,
-	VOL_CTRL_RT = 0x21d,
-
-};
-/**
- * mx_init_card - initialize the sound card
- *
- * This initializes the audio paths to know values in case of this sound card
- */
-static int mx_init_card(void)
-{
-	struct sc_reg_access sc_access[] = {
-		{0x200, 0x80, 0x00},
-		{0x201, 0xC0, 0x00},
-		{0x202, 0x00, 0x00},
-		{0x203, 0x00, 0x00},
-		{0x204, 0x02, 0x00},
-		{0x205, 0x10, 0x00},
-		{0x206, 0x60, 0x00},
-		{0x207, 0x00, 0x00},
-		{0x208, 0x90, 0x00},
-		{0x209, 0x51, 0x00},
-		{0x20a, 0x00, 0x00},
-		{0x20b, 0x10, 0x00},
-		{0x20c, 0x00, 0x00},
-		{0x20d, 0x00, 0x00},
-		{0x20e, 0x21, 0x00},
-		{0x20f, 0x00, 0x00},
-		{0x210, 0x84, 0x00},
-		{0x211, 0xB3, 0x00},
-		{0x212, 0x00, 0x00},
-		{0x213, 0x00, 0x00},
-		{0x214, 0x41, 0x00},
-		{0x215, 0x00, 0x00},
-		{0x216, 0x00, 0x00},
-		{0x217, 0x00, 0x00},
-		{0x218, 0x03, 0x00},
-		{0x219, 0x03, 0x00},
-		{0x21a, 0x00, 0x00},
-		{0x21b, 0x00, 0x00},
-		{0x21c, 0x00, 0x00},
-		{0x21d, 0x00, 0x00},
-		{0x21e, 0x00, 0x00},
-		{0x21f, 0x00, 0x00},
-		{0x220, 0x20, 0x00},
-		{0x221, 0x20, 0x00},
-		{0x222, 0x51, 0x00},
-		{0x223, 0x20, 0x00},
-		{0x224, 0x04, 0x00},
-		{0x225, 0x80, 0x00},
-		{0x226, 0x0F, 0x00},
-		{0x227, 0x08, 0x00},
-		{0xf9,  0x40, 0x00},
-		{0xfa,  0x1f, 0x00},
-		{0xfb,  0x1f, 0x00},
-		{0xfc,  0x1f, 0x00},
-		{0xfd,  0x1f, 0x00},
-		{0xfe,  0x00, 0x00},
-		{0xff,  0x0c, 0x00},
-	};
-	snd_pmic_ops_mx.card_status = SND_CARD_INIT_DONE;
-	snd_pmic_ops_mx.num_channel = 2;
-	snd_pmic_ops_mx.master_mute = UNMUTE;
-	snd_pmic_ops_mx.mute_status = UNMUTE;
-	return sst_sc_reg_access(sc_access, PMIC_WRITE, 47);
-}
-
-static int mx_enable_audiodac(int value)
-{
-	struct sc_reg_access sc_access[3];
-	int mute_val = 0;
-	int mute_val1 = 0;
-	int retval = 0;
-
-	sc_access[0].reg_addr = AS_LEFT_HP_VOL_CTL;
-	sc_access[1].reg_addr = AS_RIGHT_HP_VOL_CTL;
-
-	if (value == UNMUTE) {
-		mute_val = 0x1F;
-		mute_val1 = 0x00;
-	} else {
-		mute_val = 0x00;
-		mute_val1 = 0x40;
-	}
-	sc_access[0].mask = sc_access[1].mask = MASK0|MASK1|MASK2|MASK3|MASK4;
-	sc_access[0].value = sc_access[1].value = (u8)mute_val;
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-	if (retval)
-		return retval;
-	pr_debug("mute status = %d\n", snd_pmic_ops_mx.mute_status);
-	if (snd_pmic_ops_mx.mute_status == MUTE ||
-				snd_pmic_ops_mx.master_mute == MUTE)
-		return retval;
-
-	sc_access[0].reg_addr = VOL_CTRL_LT;
-	sc_access[1].reg_addr = VOL_CTRL_RT;
-	sc_access[0].mask = sc_access[1].mask = MASK6;
-	sc_access[0].value = sc_access[1].value = mute_val1;
-	if (snd_pmic_ops_mx.num_channel == 1)
-		sc_access[1].value = 0x40;
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-}
-
-static int mx_power_up_pb(unsigned int port)
-{
-
-	int retval = 0;
-	struct sc_reg_access sc_access[3];
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-	retval = mx_enable_audiodac(MUTE);
-	if (retval)
-		return retval;
-
-	msleep(10);
-
-	sc_access[0].reg_addr = AS_CONFIG;
-	sc_access[0].mask  = MASK7;
-	sc_access[0].value = 0x80;
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	if (retval)
-		return retval;
-
-	sc_access[0].reg_addr = ENABLE_OPDEV_CTRL;
-	sc_access[0].mask  = 0xff;
-	sc_access[0].value = 0x3C;
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	if (retval)
-		return retval;
-
-	sc_access[0].reg_addr = ENABLE_DEV_AND_USE_XTAL;
-	sc_access[0].mask  = 0x80;
-	sc_access[0].value = 0x80;
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	if (retval)
-		return retval;
-
-	return mx_enable_audiodac(UNMUTE);
-}
-
-static int mx_power_down_pb(unsigned int device)
-{
-	struct sc_reg_access sc_access[3];
-	int retval = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-
-	retval = mx_enable_audiodac(MUTE);
-	if (retval)
-		return retval;
-
-	sc_access[0].reg_addr = ENABLE_OPDEV_CTRL;
-	sc_access[0].mask  = MASK3|MASK2;
-	sc_access[0].value = 0x00;
-
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	if (retval)
-		return retval;
-
-	return mx_enable_audiodac(UNMUTE);
-}
-
-static int mx_power_up_cp(unsigned int port)
-{
-	int retval = 0;
-	struct sc_reg_access sc_access[] = {
-		{ENABLE_DEV_AND_USE_XTAL, 0x80, MASK7},
-		{ENABLE_OPDEV_CTRL, 0x3, 0x3},
-	};
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-}
-
-static int mx_power_down_cp(unsigned int device)
-{
-	struct sc_reg_access sc_access[] = {
-		{ENABLE_OPDEV_CTRL, 0x00, MASK1|MASK0},
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-}
-
-static int mx_power_down(void)
-{
-	int retval = 0;
-	struct sc_reg_access sc_access[3];
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-
-	retval = mx_enable_audiodac(MUTE);
-	if (retval)
-		return retval;
-
-	sc_access[0].reg_addr = AS_CONFIG;
-	sc_access[0].mask  = MASK7;
-	sc_access[0].value = 0x00;
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	if (retval)
-		return retval;
-
-	sc_access[0].reg_addr = ENABLE_DEV_AND_USE_XTAL;
-	sc_access[0].mask  = MASK7;
-	sc_access[0].value = 0x00;
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	if (retval)
-		return retval;
-
-	sc_access[0].reg_addr = ENABLE_OPDEV_CTRL;
-	sc_access[0].mask  = MASK3|MASK2;
-	sc_access[0].value = 0x00;
-	retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	if (retval)
-		return retval;
-
-	return mx_enable_audiodac(UNMUTE);
-}
-
-static int mx_set_pcm_voice_params(void)
-{
-	int retval = 0;
-	struct sc_reg_access sc_access[] = {
-		{0x200, 0x80, 0x00},
-		{0x201, 0xC0, 0x00},
-		{0x202, 0x00, 0x00},
-		{0x203, 0x00, 0x00},
-		{0x204, 0x0e, 0x00},
-		{0x205, 0x20, 0x00},
-		{0x206, 0x8f, 0x00},
-		{0x207, 0x21, 0x00},
-		{0x208, 0x18, 0x00},
-		{0x209, 0x32, 0x00},
-		{0x20a, 0x00, 0x00},
-		{0x20b, 0x5A, 0x00},
-		{0x20c, 0xBE, 0x00},/* 0x00 -> 0xBE Koski */
-		{0x20d, 0x00, 0x00}, /* DAI2 'off' */
-		{0x20e, 0x40, 0x00},
-		{0x20f, 0x00, 0x00},
-		{0x210, 0x84, 0x00},
-		{0x211, 0x33, 0x00}, /* Voice filter */
-		{0x212, 0x00, 0x00},
-		{0x213, 0x00, 0x00},
-		{0x214, 0x41, 0x00},
-		{0x215, 0x00, 0x00},
-		{0x216, 0x00, 0x00},
-		{0x217, 0x20, 0x00},
-		{0x218, 0x00, 0x00},
-		{0x219, 0x00, 0x00},
-		{0x21a, 0x40, 0x00},
-		{0x21b, 0x40, 0x00},
-		{0x21c, 0x09, 0x00},
-		{0x21d, 0x09, 0x00},
-		{0x21e, 0x00, 0x00},
-		{0x21f, 0x00, 0x00},
-		{0x220, 0x00, 0x00}, /* Microphone configurations */
-		{0x221, 0x00, 0x00}, /* Microphone configurations */
-		{0x222, 0x50, 0x00}, /* Microphone configurations */
-		{0x223, 0x21, 0x00}, /* Microphone configurations */
-		{0x224, 0x00, 0x00},
-		{0x225, 0x80, 0x00},
-		{0xf9, 0x40, 0x00},
-		{0xfa, 0x19, 0x00},
-		{0xfb, 0x19, 0x00},
-		{0xfc, 0x12, 0x00},
-		{0xfd, 0x12, 0x00},
-		{0xfe, 0x00, 0x00},
-	};
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-	pr_debug("SST DBG:mx_set_pcm_voice_params called\n");
-	return sst_sc_reg_access(sc_access, PMIC_WRITE, 44);
-}
-
-static int mx_set_pcm_audio_params(int sfreq, int word_size, int num_channel)
-{
-	int retval = 0;
-
-	int config1 = 0, config2 = 0, filter = 0xB3;
-	struct sc_reg_access sc_access[5];
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-
-	switch (sfreq) {
-	case 8000:
-		config1 = 0x10;
-		config2 = 0x00;
-		filter = 0x33;
-		break;
-	case 11025:
-		config1 = 0x16;
-		config2 = 0x0d;
-		break;
-	case 12000:
-		config1 = 0x18;
-		config2 = 0x00;
-		break;
-	case 16000:
-		config1 = 0x20;
-		config2 = 0x00;
-		break;
-	case 22050:
-		config1 = 0x2c;
-		config2 = 0x1a;
-		break;
-	case 24000:
-		config1 = 0x30;
-		config2 = 0x00;
-		break;
-	case 32000:
-		config1 = 0x40;
-		config2 = 0x00;
-		break;
-	case 44100:
-		config1 = 0x58;
-		config2 = 0x33;
-		break;
-	case 48000:
-		config1 = 0x60;
-		config2 = 0x00;
-		break;
-	}
-	snd_pmic_ops_mx.num_channel = num_channel;
-	/*mute the right channel if MONO*/
-	if (snd_pmic_ops_mx.num_channel == 1)	{
-		sc_access[0].reg_addr = VOL_CTRL_RT;
-		sc_access[0].value = 0x40;
-		sc_access[0].mask = MASK6;
-
-		sc_access[1].reg_addr = 0x224;
-		sc_access[1].value = 0x05;
-		sc_access[1].mask = MASK0|MASK1|MASK2;
-
-		retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-		if (retval)
-			return retval;
-	} else {
-		sc_access[0].reg_addr = VOL_CTRL_RT;
-		sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK6;
-
-		sc_access[1].reg_addr = 0x224;
-		sc_access[1].value = 0x04;
-		sc_access[1].mask = MASK0|MASK1|MASK2;
-
-		retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-		if (retval)
-			return retval;
-	}
-	sc_access[0].reg_addr =	0x206;
-	sc_access[0].value = config1;
-	sc_access[1].reg_addr = 0x207;
-	sc_access[1].value = config2;
-
-	if (word_size == 16) {
-		sc_access[2].value = 0x51;
-		sc_access[3].value = 0x31;
-	} else if (word_size == 24) {
-		sc_access[2].value = 0x52;
-		sc_access[3].value = 0x92;
-	}
-
-	sc_access[2].reg_addr = 0x209;
-	sc_access[3].reg_addr = 0x20e;
-
-	sc_access[4].reg_addr = 0x211;
-	sc_access[4].value = filter;
-
-	return sst_sc_reg_access(sc_access, PMIC_WRITE, 5);
-}
-
-static int mx_set_selected_output_dev(u8 dev_id)
-{
-	struct sc_reg_access sc_access[2];
-	int num_reg = 0;
-	int retval = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-
-	pr_debug("mx_set_selected_output_dev dev_id:0x%x\n", dev_id);
-	snd_pmic_ops_mx.output_dev_id = dev_id;
-	switch (dev_id) {
-	case STEREO_HEADPHONE:
-		sc_access[0].reg_addr = 0xFF;
-		sc_access[0].value = 0x8C;
-		sc_access[0].mask =
-			MASK2|MASK3|MASK5|MASK6|MASK4;
-
-		num_reg = 1;
-		break;
-	case MONO_EARPIECE:
-	case INTERNAL_SPKR:
-		sc_access[0].reg_addr = 0xFF;
-		sc_access[0].value = 0xb0;
-		sc_access[0].mask =  MASK2|MASK3|MASK5|MASK6|MASK4;
-
-		num_reg = 1;
-		break;
-	case RECEIVER:
-		pr_debug("RECEIVER Koski selected\n");
-
-		/* configuration - AS enable, receiver enable */
-		sc_access[0].reg_addr = 0xFF;
-		sc_access[0].value = 0x81;
-		sc_access[0].mask = 0xff;
-
-		num_reg = 1;
-		break;
-	default:
-		pr_err("Not a valid output dev\n");
-		return 0;
-	}
-	return sst_sc_reg_access(sc_access, PMIC_WRITE, num_reg);
-}
-
-
-static int mx_set_voice_port(int status)
-{
-	int retval = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-	if (status == ACTIVATE)
-		retval = mx_set_pcm_voice_params();
-
-	return retval;
-}
-
-static int mx_set_audio_port(int status)
-{
-	return 0;
-}
-
-static int mx_set_selected_input_dev(u8 dev_id)
-{
-	struct sc_reg_access sc_access[2];
-	int num_reg = 0;
-	int retval = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-	snd_pmic_ops_mx.input_dev_id = dev_id;
-	pr_debug("mx_set_selected_input_dev dev_id:0x%x\n", dev_id);
-
-	switch (dev_id) {
-	case AMIC:
-		sc_access[0].reg_addr = 0x223;
-		sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK7|MASK6|MASK5|MASK4|MASK0;
-		sc_access[1].reg_addr = 0x222;
-		sc_access[1].value = 0x50;
-		sc_access[1].mask = MASK7|MASK6|MASK5|MASK4;
-		num_reg = 2;
-		break;
-
-	case HS_MIC:
-		sc_access[0].reg_addr = 0x223;
-		sc_access[0].value = 0x20;
-		sc_access[0].mask = MASK7|MASK6|MASK5|MASK4|MASK0;
-		sc_access[1].reg_addr = 0x222;
-		sc_access[1].value = 0x51;
-		sc_access[1].mask = MASK7|MASK6|MASK5|MASK4;
-		num_reg = 2;
-		break;
-	case DMIC:
-		sc_access[1].reg_addr = 0x222;
-		sc_access[1].value = 0x00;
-		sc_access[1].mask = MASK7|MASK6|MASK5|MASK4|MASK0;
-		sc_access[0].reg_addr = 0x223;
-		sc_access[0].value = 0x20;
-		sc_access[0].mask = MASK7|MASK6|MASK5|MASK4|MASK0;
-		num_reg = 2;
-		break;
-	}
-	return sst_sc_reg_access(sc_access, PMIC_WRITE, num_reg);
-}
-
-static int mx_set_mute(int dev_id, u8 value)
-{
-	struct sc_reg_access sc_access[5];
-	int num_reg = 0;
-	int retval = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-
-
-	pr_debug("set_mute dev_id:0x%x , value:%d\n", dev_id, value);
-
-	switch (dev_id) {
-	case PMIC_SND_DMIC_MUTE:
-	case PMIC_SND_AMIC_MUTE:
-	case PMIC_SND_HP_MIC_MUTE:
-		sc_access[0].reg_addr =	0x220;
-		sc_access[1].reg_addr =	0x221;
-		sc_access[2].reg_addr =	0x223;
-		if (value == MUTE) {
-			sc_access[0].value = 0x00;
-			sc_access[1].value = 0x00;
-			if (snd_pmic_ops_mx.input_dev_id == DMIC)
-				sc_access[2].value = 0x00;
-			else
-				sc_access[2].value = 0x20;
-		} else {
-			sc_access[0].value = 0x20;
-			sc_access[1].value = 0x20;
-			if (snd_pmic_ops_mx.input_dev_id == DMIC)
-				sc_access[2].value = 0x20;
-			else
-				sc_access[2].value = 0x00;
-		}
-		sc_access[0].mask = MASK5|MASK6;
-		sc_access[1].mask = MASK5|MASK6;
-		sc_access[2].mask = MASK5|MASK6;
-		num_reg = 3;
-		break;
-	case PMIC_SND_LEFT_SPEAKER_MUTE:
-	case PMIC_SND_LEFT_HP_MUTE:
-		sc_access[0].reg_addr =	VOL_CTRL_LT;
-		if (value == MUTE)
-			sc_access[0].value = 0x40;
-		else
-			sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK6;
-		num_reg = 1;
-		snd_pmic_ops_mx.mute_status = value;
-		break;
-	case PMIC_SND_RIGHT_SPEAKER_MUTE:
-	case PMIC_SND_RIGHT_HP_MUTE:
-		sc_access[0].reg_addr = VOL_CTRL_RT;
-		if (snd_pmic_ops_mx.num_channel == 1)
-			value = MUTE;
-		if (value == MUTE)
-			sc_access[0].value = 0x40;
-		else
-			sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK6;
-		num_reg = 1;
-		snd_pmic_ops_mx.mute_status = value;
-		break;
-	case PMIC_SND_MUTE_ALL:
-		sc_access[0].reg_addr = VOL_CTRL_RT;
-		sc_access[1].reg_addr = VOL_CTRL_LT;
-		sc_access[2].reg_addr =	0x220;
-		sc_access[3].reg_addr =	0x221;
-		sc_access[4].reg_addr =	0x223;
-		snd_pmic_ops_mx.master_mute = value;
-		if (value == MUTE) {
-			sc_access[0].value = sc_access[1].value = 0x40;
-			sc_access[2].value = 0x00;
-			sc_access[3].value = 0x00;
-			if (snd_pmic_ops_mx.input_dev_id == DMIC)
-				sc_access[4].value = 0x00;
-			else
-				sc_access[4].value = 0x20;
-
-		} else {
-			sc_access[0].value = sc_access[1].value = 0x00;
-			sc_access[2].value = sc_access[3].value = 0x20;
-				sc_access[4].value = 0x20;
-			if (snd_pmic_ops_mx.input_dev_id == DMIC)
-				sc_access[4].value = 0x20;
-			else
-				sc_access[4].value = 0x00;
-
-
-		}
-		if (snd_pmic_ops_mx.num_channel == 1)
-			sc_access[0].value = 0x40;
-		sc_access[0].mask = sc_access[1].mask = MASK6;
-		sc_access[2].mask = MASK5|MASK6;
-		sc_access[3].mask = MASK5|MASK6|MASK2|MASK4;
-		sc_access[4].mask = MASK5|MASK6|MASK4;
-
-		num_reg = 5;
-		break;
-	case PMIC_SND_RECEIVER_MUTE:
-		sc_access[0].reg_addr =  VOL_CTRL_RT;
-		if (value == MUTE)
-			sc_access[0].value = 0x40;
-		else
-			sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK6;
-		num_reg = 1;
-		break;
-	}
-
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, num_reg);
-}
-
-static int mx_set_vol(int dev_id, int value)
-{
-	struct sc_reg_access sc_access[2] = {{0},};
-	int num_reg = 0;
-	int retval = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-	pr_debug("set_vol dev_id:0x%x ,value:%d\n", dev_id, value);
-	switch (dev_id) {
-	case PMIC_SND_RECEIVER_VOL:
-		return 0;
-		break;
-	case PMIC_SND_CAPTURE_VOL:
-		sc_access[0].reg_addr =	0x220;
-		sc_access[1].reg_addr =	0x221;
-		sc_access[0].value = sc_access[1].value = -value;
-		sc_access[0].mask = sc_access[1].mask =
-			(MASK0|MASK1|MASK2|MASK3|MASK4);
-		num_reg = 2;
-		break;
-	case PMIC_SND_LEFT_PB_VOL:
-		sc_access[0].value = -value;
-		sc_access[0].reg_addr = VOL_CTRL_LT;
-		sc_access[0].mask = (MASK0|MASK1|MASK2|MASK3|MASK4|MASK5);
-		num_reg = 1;
-		break;
-	case PMIC_SND_RIGHT_PB_VOL:
-		sc_access[0].value = -value;
-		sc_access[0].reg_addr = VOL_CTRL_RT;
-		sc_access[0].mask = (MASK0|MASK1|MASK2|MASK3|MASK4|MASK5);
-		if (snd_pmic_ops_mx.num_channel == 1) {
-			sc_access[0].value = 0x40;
-			sc_access[0].mask = MASK6;
-			sc_access[0].reg_addr = VOL_CTRL_RT;
-		}
-		num_reg = 1;
-		break;
-	}
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, num_reg);
-}
-
-static int mx_get_mute(int dev_id, u8 *value)
-{
-	struct sc_reg_access sc_access[4] = {{0},};
-	int retval = 0, num_reg = 0, mask = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-	switch (dev_id) {
-	case PMIC_SND_DMIC_MUTE:
-	case PMIC_SND_AMIC_MUTE:
-	case PMIC_SND_HP_MIC_MUTE:
-		sc_access[0].reg_addr = 0x220;
-		mask = MASK5|MASK6;
-		num_reg = 1;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ, num_reg);
-		if (retval)
-			return retval;
-		*value = sc_access[0].value & mask;
-		if (*value)
-			*value = UNMUTE;
-		else
-			*value = MUTE;
-		return retval;
-	case PMIC_SND_LEFT_HP_MUTE:
-	case PMIC_SND_LEFT_SPEAKER_MUTE:
-		sc_access[0].reg_addr = VOL_CTRL_LT;
-		num_reg = 1;
-		mask = MASK6;
-		break;
-	case PMIC_SND_RIGHT_HP_MUTE:
-	case PMIC_SND_RIGHT_SPEAKER_MUTE:
-		sc_access[0].reg_addr = VOL_CTRL_RT;
-		num_reg = 1;
-		mask = MASK6;
-		break;
-	}
-	retval = sst_sc_reg_access(sc_access, PMIC_READ, num_reg);
-	if (retval)
-		return retval;
-	*value = sc_access[0].value & mask;
-	if (*value)
-		*value = MUTE;
-	else
-		*value = UNMUTE;
-	return retval;
-}
-
-static int mx_get_vol(int dev_id, int *value)
-{
-	struct sc_reg_access sc_access = {0,};
-	int retval = 0, mask = 0, num_reg = 0;
-
-	if (snd_pmic_ops_mx.card_status == SND_CARD_UN_INIT) {
-		retval = mx_init_card();
-		if (retval)
-			return retval;
-	}
-	switch (dev_id) {
-	case PMIC_SND_CAPTURE_VOL:
-		sc_access.reg_addr = 0x220;
-		mask = MASK0|MASK1|MASK2|MASK3|MASK4;
-		num_reg = 1;
-		break;
-	case PMIC_SND_LEFT_PB_VOL:
-		sc_access.reg_addr = VOL_CTRL_LT;
-		mask = MASK0|MASK1|MASK2|MASK3|MASK4|MASK5;
-		num_reg = 1;
-		break;
-	case PMIC_SND_RIGHT_PB_VOL:
-		sc_access.reg_addr = VOL_CTRL_RT;
-		mask = MASK0|MASK1|MASK2|MASK3|MASK4|MASK5;
-		num_reg = 1;
-		break;
-	}
-	retval = sst_sc_reg_access(&sc_access, PMIC_READ, num_reg);
-	if (retval)
-		return retval;
-	*value = -(sc_access.value & mask);
-	pr_debug("get volume value extracted %d\n", *value);
-	return retval;
-}
-
-static u8 mx_get_jack_status(void)
-{
-	struct sc_reg_access sc_access_read = {0,};
-
-	sc_access_read.reg_addr = 0x201;
-	sst_sc_reg_access(&sc_access_read, PMIC_READ, 1);
-	pr_debug("value returned = 0x%x\n", sc_access_read.value);
-	return sc_access_read.value;
-}
-
-static void mx_pmic_irq_enable(void *data)
-{
-	struct snd_intelmad *intelmaddata = data;
-
-	intelmaddata->jack_prev_state = 0xc0;
-	return;
-}
-
-static void mx_pmic_irq_cb(void *cb_data, u8 intsts)
-{
-	u8 jack_cur_status, jack_prev_state = 0;
-	struct mad_jack *mjack = NULL;
-	unsigned int present = 0, jack_event_flag = 0, buttonpressflag = 0;
-	time_t  timediff;
-	struct snd_intelmad *intelmaddata = cb_data;
-
-	mjack = &intelmaddata->jack[0];
-	if (intsts & 0x2) {
-		jack_cur_status = mx_get_jack_status();
-		jack_prev_state = intelmaddata->jack_prev_state;
-		if ((jack_prev_state == 0xc0) && (jack_cur_status == 0x40)) {
-			/*headset insert detected. */
-			pr_debug("MAD headset inserted\n");
-			present = 1;
-			jack_event_flag = 1;
-			mjack->jack_status = 1;
-			mjack->jack.type = SND_JACK_HEADSET;
-		}
-
-		if ((jack_prev_state == 0xc0) && (jack_cur_status == 0x00)) {
-			/* headphone insert detected. */
-			pr_debug("MAD headphone inserted\n");
-			present = 1;
-			jack_event_flag = 1;
-			mjack->jack.type = SND_JACK_HEADPHONE;
-		}
-
-		if ((jack_prev_state == 0x40) && (jack_cur_status == 0xc0)) {
-			/* headset remove detected. */
-			pr_debug("MAD headset removed\n");
-
-			present = 0;
-			jack_event_flag = 1;
-			mjack->jack_status = 0;
-			mjack->jack.type = SND_JACK_HEADSET;
-		}
-
-		if ((jack_prev_state == 0x00) && (jack_cur_status == 0xc0)) {
-			/* headphone remove detected. */
-			pr_debug("MAD headphone removed\n");
-			present = 0;
-			jack_event_flag = 1;
-			mjack->jack.type = SND_JACK_HEADPHONE;
-		}
-
-		if ((jack_prev_state == 0x40) && (jack_cur_status == 0x00)) {
-			/* button pressed */
-			do_gettimeofday(&mjack->buttonpressed);
-			pr_debug("MAD button press detected\n");
-		}
-
-		if ((jack_prev_state == 0x00) && (jack_cur_status == 0x40)) {
-			if (mjack->jack_status) {
-				/*button pressed */
-				do_gettimeofday(
-					&mjack->buttonreleased);
-				/*button pressed */
-				pr_debug("MAD Button Released detected\n");
-				timediff = mjack->buttonreleased.tv_sec -
-					mjack->buttonpressed.tv_sec;
-				buttonpressflag = 1;
-
-				if (timediff > 1) {
-					pr_debug("MAD long press dtd\n");
-					/* send headphone detect/undetect */
-					present = 1;
-					jack_event_flag = 1;
-					mjack->jack.type =
-							MID_JACK_HS_LONG_PRESS;
-				} else {
-					pr_debug("MAD short press dtd\n");
-					/* send headphone detect/undetect */
-					present = 1;
-					jack_event_flag = 1;
-					mjack->jack.type =
-						MID_JACK_HS_SHORT_PRESS;
-				}
-			} else {
-				/***workaround for maxim
-				hw issue,0x00 t 0x40 is not
-				a valid transiton for Headset insertion */
-				/*headset insert detected. */
-				pr_debug("MAD headset inserted\n");
-				present = 1;
-				jack_event_flag = 1;
-				mjack->jack_status = 1;
-				mjack->jack.type = SND_JACK_HEADSET;
-			}
-		}
-		intelmaddata->jack_prev_state  = jack_cur_status;
-		pr_debug("mx_pmic_irq_cb prv_state= 0x%x\n",
-					intelmaddata->jack_prev_state);
-	}
-
-	if (jack_event_flag)
-		sst_mad_send_jack_report(&mjack->jack,
-						buttonpressflag, present);
-}
-static int mx_jack_enable(void)
-{
-	return 0;
-}
-
-struct snd_pmic_ops snd_pmic_ops_mx = {
-	.set_input_dev = mx_set_selected_input_dev,
-	.set_output_dev = mx_set_selected_output_dev,
-	.set_mute = mx_set_mute,
-	.get_mute = mx_get_mute,
-	.set_vol = mx_set_vol,
-	.get_vol = mx_get_vol,
-	.init_card = mx_init_card,
-	.set_pcm_audio_params = mx_set_pcm_audio_params,
-	.set_pcm_voice_params = mx_set_pcm_voice_params,
-	.set_voice_port = mx_set_voice_port,
-	.set_audio_port = mx_set_audio_port,
-	.power_up_pmic_pb =	mx_power_up_pb,
-	.power_up_pmic_cp =	mx_power_up_cp,
-	.power_down_pmic_pb =	mx_power_down_pb,
-	.power_down_pmic_cp =	mx_power_down_cp,
-	.power_down_pmic =	mx_power_down,
-	.pmic_irq_cb	 =	mx_pmic_irq_cb,
-	.pmic_irq_enable =	mx_pmic_irq_enable,
-	.pmic_jack_enable =	mx_jack_enable,
-};
-
diff --git a/drivers/staging/intel_sst/intelmid_v2_control.c b/drivers/staging/intel_sst/intelmid_v2_control.c
deleted file mode 100644
index 46ab55eb8097..000000000000
--- a/drivers/staging/intel_sst/intelmid_v2_control.c
+++ /dev/null
@@ -1,1156 +0,0 @@
-/*
- *  intelmid_v2_control.c - Intel Sound card driver for MID
- *
- *  Copyright (C) 2008-10 Intel Corp
- *  Authors:	Vinod Koul <vinod.koul@intel.com>
- *		Harsha Priya <priya.harsha@intel.com>
- *		KP Jeeja <jeeja.kp@intel.com>
- *		Dharageswari R <dharageswari.r@intel.com>
- *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; version 2 of the License.
- *
- *  This program is distributed in the hope that it will be useful, but
- *  WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *  General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License along
- *  with this program; if not, write to the Free Software Foundation, Inc.,
- *  59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- *
- * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- *  This file contains the control operations of vendor 3
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/gpio.h>
-#include <linux/pci.h>
-#include <linux/delay.h>
-#include <linux/file.h>
-#include <sound/control.h>
-#include "intel_sst.h"
-#include "intelmid_snd_control.h"
-#include "intelmid.h"
-
-enum reg_v3 {
-	VAUDIOCNT = 0x51,
-	VOICEPORT1 = 0x100,
-	VOICEPORT2 = 0x101,
-	AUDIOPORT1 = 0x102,
-	AUDIOPORT2 = 0x103,
-	ADCSAMPLERATE = 0x104,
-	DMICCTRL1 = 0x105,
-	DMICCTRL2 = 0x106,
-	MICCTRL = 0x107,
-	MICSELVOL = 0x108,
-	LILSEL = 0x109,
-	LIRSEL = 0x10a,
-	VOICEVOL = 0x10b,
-	AUDIOLVOL = 0x10c,
-	AUDIORVOL = 0x10d,
-	LMUTE = 0x10e,
-	RMUTE = 0x10f,
-	POWERCTRL1 = 0x110,
-	POWERCTRL2 = 0x111,
-	DRVPOWERCTRL = 0x112,
-	VREFPLL = 0x113,
-	PCMBUFCTRL = 0x114,
-	SOFTMUTE = 0x115,
-	DTMFPATH = 0x116,
-	DTMFVOL = 0x117,
-	DTMFFREQ = 0x118,
-	DTMFHFREQ = 0x119,
-	DTMFLFREQ = 0x11a,
-	DTMFCTRL = 0x11b,
-	DTMFASON = 0x11c,
-	DTMFASOFF = 0x11d,
-	DTMFASINUM = 0x11e,
-	CLASSDVOL = 0x11f,
-	VOICEDACAVOL = 0x120,
-	AUDDACAVOL = 0x121,
-	LOMUTEVOL = 0x122,
-	HPLVOL = 0x123,
-	HPRVOL = 0x124,
-	MONOVOL = 0x125,
-	LINEOUTMIXVOL = 0x126,
-	EPMIXVOL = 0x127,
-	LINEOUTLSEL = 0x128,
-	LINEOUTRSEL = 0x129,
-	EPMIXOUTSEL = 0x12a,
-	HPLMIXSEL = 0x12b,
-	HPRMIXSEL = 0x12c,
-	LOANTIPOP = 0x12d,
-	AUXDBNC = 0x12f,
-};
-
-static void nc_set_amp_power(int power)
-{
-	if (snd_pmic_ops_nc.gpio_amp)
-		gpio_set_value(snd_pmic_ops_nc.gpio_amp, power);
-}
-
-/****
- * nc_init_card - initialize the sound card
- *
- * This initializes the audio paths to know values in case of this sound card
- */
-static int nc_init_card(void)
-{
-	struct sc_reg_access sc_access[] = {
-		{VAUDIOCNT, 0x25, 0},
-		{VOICEPORT1, 0x00, 0},
-		{VOICEPORT2, 0x00, 0},
-		{AUDIOPORT1, 0x98, 0},
-		{AUDIOPORT2, 0x09, 0},
-		{AUDIOLVOL, 0x00, 0},
-		{AUDIORVOL, 0x00, 0},
-		{LMUTE, 0x03, 0},
-		{RMUTE, 0x03, 0},
-		{POWERCTRL1, 0x00, 0},
-		{POWERCTRL2, 0x00, 0},
-		{DRVPOWERCTRL, 0x00, 0},
-		{VREFPLL, 0x10, 0},
-		{HPLMIXSEL, 0xee, 0},
-		{HPRMIXSEL, 0xf6, 0},
-		{PCMBUFCTRL, 0x0, 0},
-		{VOICEVOL, 0x0e, 0},
-		{HPLVOL, 0x06, 0},
-		{HPRVOL, 0x06, 0},
-		{MICCTRL, 0x51, 0x00},
-		{ADCSAMPLERATE, 0x8B, 0x00},
-		{MICSELVOL, 0x5B, 0x00},
-		{LILSEL, 0x06, 0},
-		{LIRSEL, 0x46, 0},
-		{LOANTIPOP, 0x00, 0},
-		{DMICCTRL1, 0x40, 0},
-		{AUXDBNC, 0xff, 0},
-	};
-	snd_pmic_ops_nc.card_status = SND_CARD_INIT_DONE;
-	snd_pmic_ops_nc.master_mute = UNMUTE;
-	snd_pmic_ops_nc.mute_status = UNMUTE;
-	sst_sc_reg_access(sc_access, PMIC_WRITE, 27);
-	mutex_init(&snd_pmic_ops_nc.lock);
-	pr_debug("init complete!!\n");
-	return 0;
-}
-
-static int nc_enable_audiodac(int value)
-{
-	struct sc_reg_access sc_access[3];
-	int mute_val = 0;
-
-	if (snd_pmic_ops_nc.mute_status == MUTE)
-		return 0;
-
-	if (((snd_pmic_ops_nc.output_dev_id == MONO_EARPIECE) ||
-		(snd_pmic_ops_nc.output_dev_id == INTERNAL_SPKR)) &&
-		(value == UNMUTE))
-		return 0;
-	if (value == UNMUTE) {
-			/* unmute the system, set the 7th bit to zero */
-			mute_val = 0x00;
-		} else {
-			/* MUTE:Set the seventh bit */
-			mute_val = 0x04;
-
-		}
-		sc_access[0].reg_addr = LMUTE;
-		sc_access[1].reg_addr = RMUTE;
-		sc_access[0].mask = sc_access[1].mask = MASK2;
-		sc_access[0].value = sc_access[1].value = mute_val;
-
-		if (snd_pmic_ops_nc.num_channel == 1)
-			sc_access[1].value = 0x04;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-
-}
-
-static int nc_power_up_pb(unsigned int port)
-{
-	struct sc_reg_access sc_access[7];
-	int retval = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-	if (port == 0xFF)
-		return 0;
-	mutex_lock(&snd_pmic_ops_nc.lock);
-	nc_enable_audiodac(MUTE);
-	msleep(30);
-
-	pr_debug("powering up pb....\n");
-
-	sc_access[0].reg_addr = VAUDIOCNT;
-	sc_access[0].value = 0x27;
-	sc_access[0].mask = 0x27;
-	sc_access[1].reg_addr = VREFPLL;
-	if (port == 0) {
-		sc_access[1].value = 0x3A;
-		sc_access[1].mask = 0x3A;
-	} else if (port == 1) {
-		sc_access[1].value = 0x35;
-		sc_access[1].mask = 0x35;
-	}
-	retval =  sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-
-
-
-	sc_access[0].reg_addr = POWERCTRL1;
-	if (port == 0) {
-		sc_access[0].value = 0x40;
-		sc_access[0].mask = 0x40;
-	} else if (port == 1) {
-		sc_access[0].value = 0x01;
-		sc_access[0].mask = 0x01;
-	}
-	sc_access[1].reg_addr = POWERCTRL2;
-	sc_access[1].value = 0x0C;
-	sc_access[1].mask = 0x0C;
-
-	sc_access[2].reg_addr = DRVPOWERCTRL;
-	sc_access[2].value = 0x86;
-	sc_access[2].mask = 0x86;
-
-	sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 3);
-
-	msleep(30);
-
-	snd_pmic_ops_nc.pb_on = 1;
-
-	/*
-	 * There is a mismatch between Playback Sources and the enumerated
-	 * values of output sources.  This mismatch causes ALSA upper to send
-	 * Item 1 for Internal Speaker, but the expected enumeration is 2!  For
-	 * now, treat MONO_EARPIECE and INTERNAL_SPKR identically and power up
-	 * the needed resources
-	 */
-	if (snd_pmic_ops_nc.output_dev_id == MONO_EARPIECE ||
-	    snd_pmic_ops_nc.output_dev_id == INTERNAL_SPKR)
-		nc_set_amp_power(1);
-	nc_enable_audiodac(UNMUTE);
-	mutex_unlock(&snd_pmic_ops_nc.lock);
-	return 0;
-}
-
-static int nc_power_up_cp(unsigned int port)
-{
-	struct sc_reg_access sc_access[5];
-	int retval = 0;
-
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-
-	pr_debug("powering up cp....\n");
-
-	if (port == 0xFF)
-		return 0;
-	sc_access[0].reg_addr = VAUDIOCNT;
-	sc_access[0].value = 0x27;
-	sc_access[0].mask = 0x27;
-	sc_access[1].reg_addr = VREFPLL;
-	if (port == 0) {
-		sc_access[1].value = 0x3E;
-		sc_access[1].mask = 0x3E;
-	} else if (port == 1) {
-		sc_access[1].value = 0x35;
-		sc_access[1].mask = 0x35;
-	}
-
-	retval =  sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-
-
-	sc_access[0].reg_addr = POWERCTRL1;
-	if (port == 0) {
-		sc_access[0].value = 0xB4;
-		sc_access[0].mask = 0xB4;
-	} else if (port == 1) {
-		sc_access[0].value = 0xBF;
-		sc_access[0].mask = 0xBF;
-	}
-	sc_access[1].reg_addr = POWERCTRL2;
-	if (port == 0) {
-		sc_access[1].value = 0x0C;
-		sc_access[1].mask = 0x0C;
-	} else if (port == 1) {
-		sc_access[1].value = 0x02;
-		sc_access[1].mask = 0x02;
-	}
-
-	return  sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-
-}
-
-static int nc_power_down(void)
-{
-	int retval = 0;
-	struct sc_reg_access sc_access[5];
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-	nc_enable_audiodac(MUTE);
-
-
-	pr_debug("powering dn nc_power_down ....\n");
-
-	if (snd_pmic_ops_nc.output_dev_id == MONO_EARPIECE ||
-	    snd_pmic_ops_nc.output_dev_id == INTERNAL_SPKR)
-		nc_set_amp_power(0);
-
-	msleep(30);
-
-	sc_access[0].reg_addr = DRVPOWERCTRL;
-	sc_access[0].value = 0x00;
-	sc_access[0].mask = 0x00;
-
-	sst_sc_reg_access(sc_access, PMIC_WRITE, 1);
-
-	sc_access[0].reg_addr = POWERCTRL1;
-	sc_access[0].value = 0x00;
-	sc_access[0].mask = 0x00;
-
-	sc_access[1].reg_addr = POWERCTRL2;
-	sc_access[1].value = 0x00;
-	sc_access[1].mask = 0x00;
-
-
-
-	sst_sc_reg_access(sc_access, PMIC_WRITE, 2);
-
-	msleep(30);
-	sc_access[0].reg_addr = VREFPLL;
-	sc_access[0].value = 0x10;
-	sc_access[0].mask = 0x10;
-
-	sc_access[1].reg_addr = VAUDIOCNT;
-	sc_access[1].value = 0x25;
-	sc_access[1].mask = 0x25;
-
-
-	retval =  sst_sc_reg_access(sc_access, PMIC_WRITE, 2);
-
-	msleep(30);
-	return nc_enable_audiodac(UNMUTE);
-}
-
-static int nc_power_down_pb(unsigned int device)
-{
-
-	int retval = 0;
-	struct sc_reg_access sc_access[5];
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	pr_debug("powering dn pb....\n");
-	mutex_lock(&snd_pmic_ops_nc.lock);
-	nc_enable_audiodac(MUTE);
-
-
-	msleep(30);
-
-
-	sc_access[0].reg_addr = DRVPOWERCTRL;
-	sc_access[0].value = 0x00;
-	sc_access[0].mask = 0x00;
-
-	sst_sc_reg_access(sc_access, PMIC_WRITE, 1);
-
-	msleep(30);
-
-	sc_access[0].reg_addr = POWERCTRL1;
-	sc_access[0].value = 0x00;
-	sc_access[0].mask = 0x41;
-
-	sc_access[1].reg_addr = POWERCTRL2;
-	sc_access[1].value = 0x00;
-	sc_access[1].mask = 0x0C;
-
-	sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2);
-
-	msleep(30);
-
-	snd_pmic_ops_nc.pb_on = 0;
-
-	nc_enable_audiodac(UNMUTE);
-	mutex_unlock(&snd_pmic_ops_nc.lock);
-	return 0;
-}
-
-static int nc_power_down_cp(unsigned int device)
-{
-	struct sc_reg_access sc_access[] = {
-		{POWERCTRL1, 0x00, 0xBE},
-		{POWERCTRL2, 0x00, 0x02},
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	pr_debug("powering dn cp....\n");
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-}
-
-static int nc_set_pcm_voice_params(void)
-{
-	struct sc_reg_access sc_access[] = {
-			{0x100, 0xD5, 0},
-			{0x101, 0x08, 0},
-			{0x104, 0x03, 0},
-			{0x107, 0x10, 0},
-			{0x10B, 0x0E, 0},
-			{0x10E, 0x03, 0},
-			{0x10F, 0x03, 0},
-			{0x114, 0x13, 0},
-			{0x115, 0x00, 0},
-			{0x128, 0xFE, 0},
-			{0x129, 0xFE, 0},
-			{0x12A, 0xFE, 0},
-			{0x12B, 0xDE, 0},
-			{0x12C, 0xDE, 0},
-	};
-	int retval = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	sst_sc_reg_access(sc_access, PMIC_WRITE, 14);
-	pr_debug("Voice parameters set successfully!!\n");
-	return 0;
-}
-
-
-static int nc_set_pcm_audio_params(int sfreq, int word_size, int num_channel)
-{
-	int config2 = 0;
-	struct sc_reg_access sc_access;
-	int retval = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	switch (sfreq) {
-	case 8000:
-		config2 = 0x00;
-		break;
-	case 11025:
-		config2 = 0x01;
-		break;
-	case 12000:
-		config2 = 0x02;
-		break;
-	case 16000:
-		config2 = 0x03;
-		break;
-	case 22050:
-		config2 = 0x04;
-		break;
-	case 24000:
-		config2 = 0x05;
-		break;
-	case 32000:
-		config2 = 0x07;
-		break;
-	case 44100:
-		config2 = 0x08;
-		break;
-	case 48000:
-		config2 = 0x09;
-		break;
-	}
-
-	snd_pmic_ops_nc.num_channel = num_channel;
-	if (snd_pmic_ops_nc.num_channel == 1)	{
-
-		sc_access.value = 0x07;
-		sc_access.reg_addr = RMUTE;
-		pr_debug("RIGHT_HP_MUTE value%d\n", sc_access.value);
-		sc_access.mask = MASK2;
-		sst_sc_reg_access(&sc_access, PMIC_READ_MODIFY, 1);
-	} else {
-		sc_access.value = 0x00;
-		sc_access.reg_addr = RMUTE;
-		pr_debug("RIGHT_HP_MUTE value %d\n", sc_access.value);
-		sc_access.mask = MASK2;
-		sst_sc_reg_access(&sc_access, PMIC_READ_MODIFY, 1);
-
-
-	}
-
-	pr_debug("word_size = %d\n", word_size);
-
-	if (word_size == 24) {
-		sc_access.reg_addr = AUDIOPORT2;
-		sc_access.value = config2 | 0x10;
-		sc_access.mask = 0x1F;
-	} else {
-		sc_access.value = config2;
-		sc_access.mask = 0x1F;
-		sc_access.reg_addr = AUDIOPORT2;
-	}
-	sst_sc_reg_access(&sc_access, PMIC_READ_MODIFY, 1);
-
-	pr_debug("word_size = %d\n", word_size);
-	sc_access.reg_addr = AUDIOPORT1;
-	sc_access.mask = MASK5|MASK4|MASK1|MASK0;
-	if (word_size == 16)
-		sc_access.value = 0x98;
-	else if (word_size == 24)
-		sc_access.value = 0xAB;
-
-	return sst_sc_reg_access(&sc_access, PMIC_READ_MODIFY, 1);
-
-
-
-}
-
-static int nc_set_selected_output_dev(u8 value)
-{
-	struct sc_reg_access sc_access_HP[] = {
-		{LMUTE, 0x02, 0x06},
-		{RMUTE, 0x02, 0x06},
-		{DRVPOWERCTRL, 0x06, 0x06},
-	};
-	struct sc_reg_access sc_access_IS[] = {
-		{LMUTE, 0x04, 0x06},
-		{RMUTE, 0x04, 0x06},
-		{DRVPOWERCTRL, 0x00, 0x06},
-	};
-	int retval = 0;
-
-	snd_pmic_ops_nc.output_dev_id = value;
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-	pr_debug("nc set selected output:%d\n", value);
-	mutex_lock(&snd_pmic_ops_nc.lock);
-	switch (value) {
-	case STEREO_HEADPHONE:
-		if (snd_pmic_ops_nc.pb_on)
-			sst_sc_reg_access(sc_access_HP+2, PMIC_WRITE, 1);
-		retval = sst_sc_reg_access(sc_access_HP, PMIC_WRITE, 2);
-		nc_set_amp_power(0);
-		break;
-	case MONO_EARPIECE:
-	case INTERNAL_SPKR:
-		retval = sst_sc_reg_access(sc_access_IS, PMIC_WRITE, 3);
-		if (snd_pmic_ops_nc.pb_on)
-			nc_set_amp_power(1);
-		break;
-	default:
-		pr_err("rcvd illegal request: %d\n", value);
-		mutex_unlock(&snd_pmic_ops_nc.lock);
-		return -EINVAL;
-	}
-	mutex_unlock(&snd_pmic_ops_nc.lock);
-	return retval;
-}
-
-static int nc_audio_init(void)
-{
-	struct sc_reg_access sc_acces, sc_access[] = {
-			{0x100, 0x00, 0},
-			{0x101, 0x00, 0},
-			{0x104, 0x8B, 0},
-			{0x107, 0x11, 0},
-			{0x10B, 0x0E, 0},
-			{0x114, 0x00, 0},
-			{0x115, 0x00, 0},
-			{0x128, 0x00, 0},
-			{0x129, 0x00, 0},
-			{0x12A, 0x00, 0},
-			{0x12B, 0xee, 0},
-			{0x12C, 0xf6, 0},
-	};
-
-	sst_sc_reg_access(sc_access, PMIC_WRITE, 12);
-	pr_debug("Audio Init successfully!!\n");
-
-	/*set output device */
-	nc_set_selected_output_dev(snd_pmic_ops_nc.output_dev_id);
-
-	if (snd_pmic_ops_nc.num_channel == 1) {
-		sc_acces.value = 0x07;
-		sc_acces.reg_addr = RMUTE;
-		pr_debug("RIGHT_HP_MUTE value%d\n", sc_acces.value);
-		sc_acces.mask = MASK2;
-		sst_sc_reg_access(&sc_acces, PMIC_READ_MODIFY, 1);
-	} else {
-		sc_acces.value = 0x00;
-		sc_acces.reg_addr = RMUTE;
-		pr_debug("RIGHT_HP_MUTE value%d\n", sc_acces.value);
-		sc_acces.mask = MASK2;
-		sst_sc_reg_access(&sc_acces, PMIC_READ_MODIFY, 1);
-	}
-
-	return 0;
-}
-
-static int nc_set_audio_port(int status)
-{
-	struct sc_reg_access sc_access[2] = {{0,},};
-	int retval = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	if (status == DEACTIVATE) {
-		/* Deactivate audio port-tristate and power */
-		sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK4|MASK5;
-		sc_access[0].reg_addr = AUDIOPORT1;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	} else if (status == ACTIVATE) {
-		/* activate audio port */
-		nc_audio_init();
-		sc_access[0].value = 0x10;
-		sc_access[0].mask =  MASK4|MASK5 ;
-		sc_access[0].reg_addr = AUDIOPORT1;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	} else
-		return -EINVAL;
-
-}
-
-static int nc_set_voice_port(int status)
-{
-	struct sc_reg_access sc_access[2] = {{0,},};
-	int retval = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	if (status == DEACTIVATE) {
-		/* Activate Voice port */
-		sc_access[0].value = 0x00;
-		sc_access[0].mask = MASK4;
-		sc_access[0].reg_addr = VOICEPORT1;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	} else if (status == ACTIVATE) {
-		/* Deactivate voice port */
-		nc_set_pcm_voice_params();
-		sc_access[0].value = 0x10;
-		sc_access[0].mask = MASK4;
-		sc_access[0].reg_addr = VOICEPORT1;
-		return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-	} else
-		return -EINVAL;
-}
-
-static int nc_set_mute(int dev_id, u8 value)
-{
-	struct sc_reg_access sc_access[3];
-	u8 mute_val, cap_mute;
-	int retval = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	pr_debug("set device id::%d, value %d\n", dev_id, value);
-
-	switch (dev_id) {
-	case PMIC_SND_MUTE_ALL:
-		pr_debug("PMIC_SND_MUTE_ALL value %d\n", value);
-		snd_pmic_ops_nc.mute_status = value;
-		snd_pmic_ops_nc.master_mute = value;
-		if (value == UNMUTE) {
-			/* unmute the system, set the 7th bit to zero */
-			mute_val = cap_mute = 0x00;
-		} else {
-			/* MUTE:Set the seventh bit */
-			mute_val = 0x80;
-			cap_mute = 0x40;
-		}
-		sc_access[0].reg_addr = AUDIOLVOL;
-		sc_access[1].reg_addr = AUDIORVOL;
-		sc_access[0].mask = sc_access[1].mask = MASK7;
-		sc_access[0].value = sc_access[1].value = mute_val;
-		if (snd_pmic_ops_nc.num_channel == 1)
-				sc_access[1].value = 0x80;
-		if (!sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 2)) {
-			sc_access[0].reg_addr = 0x109;
-			sc_access[1].reg_addr = 0x10a;
-			sc_access[2].reg_addr = 0x105;
-			sc_access[0].mask = sc_access[1].mask =
-				sc_access[2].mask = MASK6;
-			sc_access[0].value = sc_access[1].value =
-				sc_access[2].value = cap_mute;
-
-			if ((snd_pmic_ops_nc.input_dev_id == AMIC) ||
-				(snd_pmic_ops_nc.input_dev_id == DMIC))
-					sc_access[1].value = 0x40;
-			if (snd_pmic_ops_nc.input_dev_id == HS_MIC)
-					sc_access[0].value = 0x40;
-			retval = sst_sc_reg_access(sc_access,
-					PMIC_READ_MODIFY, 3);
-		}
-		break;
-	case PMIC_SND_HP_MIC_MUTE:
-		pr_debug("PMIC_SND_HPMIC_MUTE value %d\n", value);
-		if (value == UNMUTE) {
-			/* unmute the system, set the 6th bit to one */
-			sc_access[0].value = 0x00;
-		} else {
-			/* mute the system, reset the 6th bit to zero */
-			sc_access[0].value = 0x40;
-		}
-		sc_access[0].reg_addr = LIRSEL;
-		sc_access[0].mask = MASK6;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-		break;
-	case PMIC_SND_AMIC_MUTE:
-		pr_debug("PMIC_SND_AMIC_MUTE value %d\n", value);
-		if (value == UNMUTE) {
-			/* unmute the system, set the 6th bit to one */
-			sc_access[0].value = 0x00;
-		} else {
-			/* mute the system, reset the 6th bit to zero */
-			sc_access[0].value = 0x40;
-		}
-		sc_access[0].reg_addr = LILSEL;
-		sc_access[0].mask = MASK6;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-		break;
-
-	case PMIC_SND_DMIC_MUTE:
-		pr_debug("INPUT_MUTE_DMIC value%d\n", value);
-		if (value == UNMUTE) {
-			/* unmute the system, set the 6th bit to one */
-			sc_access[1].value = 0x00;
-			sc_access[0].value = 0x00;
-		} else {
-			/* mute the system, reset the 6th bit to zero */
-			sc_access[1].value = 0x40;
-			sc_access[0].value = 0x40;
-		}
-		sc_access[0].reg_addr = DMICCTRL1;
-		sc_access[0].mask = MASK6;
-		sc_access[1].reg_addr = LILSEL;
-		sc_access[1].mask = MASK6;
-		retval = sst_sc_reg_access(sc_access,
-					PMIC_READ_MODIFY, 2);
-		break;
-
-	case PMIC_SND_LEFT_HP_MUTE:
-	case PMIC_SND_RIGHT_HP_MUTE:
-		snd_pmic_ops_nc.mute_status = value;
-		if (value == UNMUTE)
-			sc_access[0].value = 0x0;
-		else
-			sc_access[0].value = 0x04;
-
-		if (dev_id == PMIC_SND_LEFT_HP_MUTE) {
-			sc_access[0].reg_addr = LMUTE;
-			pr_debug("LEFT_HP_MUTE value %d\n",
-					sc_access[0].value);
-		} else {
-			if (snd_pmic_ops_nc.num_channel == 1)
-				sc_access[0].value = 0x04;
-			sc_access[0].reg_addr = RMUTE;
-			pr_debug("RIGHT_HP_MUTE value %d\n",
-					sc_access[0].value);
-		}
-		sc_access[0].mask = MASK2;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-		break;
-	case PMIC_SND_LEFT_SPEAKER_MUTE:
-	case PMIC_SND_RIGHT_SPEAKER_MUTE:
-		if (value == UNMUTE)
-			sc_access[0].value = 0x00;
-		else
-			sc_access[0].value = 0x03;
-		sc_access[0].reg_addr = LMUTE;
-		pr_debug("SPEAKER_MUTE %d\n", sc_access[0].value);
-		sc_access[0].mask = MASK1;
-		retval = sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, 1);
-		break;
-	default:
-		return -EINVAL;
-	}
-	return retval ;
-
-}
-
-static int nc_set_vol(int dev_id, int value)
-{
-	struct sc_reg_access sc_access[3];
-	int retval = 0, entries = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	pr_debug("set volume:%d\n", dev_id);
-	switch (dev_id) {
-	case PMIC_SND_CAPTURE_VOL:
-		pr_debug("PMIC_SND_CAPTURE_VOL:value::%d\n", value);
-		sc_access[0].value = sc_access[1].value =
-					sc_access[2].value = -value;
-		sc_access[0].mask = sc_access[1].mask = sc_access[2].mask =
-					(MASK0|MASK1|MASK2|MASK3|MASK4|MASK5);
-		sc_access[0].reg_addr = 0x10a;
-		sc_access[1].reg_addr = 0x109;
-		sc_access[2].reg_addr = 0x105;
-		entries = 3;
-		break;
-
-	case PMIC_SND_LEFT_PB_VOL:
-		pr_debug("PMIC_SND_LEFT_HP_VOL %d\n", value);
-		sc_access[0].value = -value;
-		sc_access[0].reg_addr  = HPLVOL;
-		sc_access[0].mask = (MASK0|MASK1|MASK2|MASK3|MASK4);
-		entries = 1;
-		break;
-
-	case PMIC_SND_RIGHT_PB_VOL:
-		pr_debug("PMIC_SND_RIGHT_HP_VOL value %d\n", value);
-		if (snd_pmic_ops_nc.num_channel == 1) {
-			sc_access[0].value = 0x04;
-			sc_access[0].reg_addr = RMUTE;
-			sc_access[0].mask = MASK2;
-		} else {
-			sc_access[0].value = -value;
-			sc_access[0].reg_addr  = HPRVOL;
-			sc_access[0].mask = (MASK0|MASK1|MASK2|MASK3|MASK4);
-		}
-		entries = 1;
-		break;
-
-	case PMIC_SND_LEFT_MASTER_VOL:
-		pr_debug("PMIC_SND_LEFT_MASTER_VOL value %d\n", value);
-		sc_access[0].value = -value;
-		sc_access[0].reg_addr = AUDIOLVOL;
-		sc_access[0].mask =
-			(MASK0|MASK1|MASK2|MASK3|MASK4|MASK5|MASK6);
-		entries = 1;
-		break;
-
-	case PMIC_SND_RIGHT_MASTER_VOL:
-		pr_debug("PMIC_SND_RIGHT_MASTER_VOL value %d\n", value);
-		sc_access[0].value = -value;
-		sc_access[0].reg_addr = AUDIORVOL;
-		sc_access[0].mask =
-				(MASK0|MASK1|MASK2|MASK3|MASK4|MASK5|MASK6);
-		entries = 1;
-		break;
-
-	default:
-		return -EINVAL;
-
-	}
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, entries);
-}
-
-static int nc_set_selected_input_dev(u8 value)
-{
-	struct sc_reg_access sc_access[6];
-	u8 num_val;
-	int retval = 0;
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-	snd_pmic_ops_nc.input_dev_id = value;
-
-	pr_debug("nc set selected input:%d\n", value);
-
-	switch (value) {
-	case AMIC:
-		pr_debug("Selecting AMIC\n");
-		sc_access[0].reg_addr = 0x107;
-		sc_access[0].value = 0x40;
-		sc_access[0].mask =  MASK6|MASK3|MASK1|MASK0;
-		sc_access[1].reg_addr = 0x10a;
-		sc_access[1].value = 0x40;
-		sc_access[1].mask = MASK6;
-		sc_access[2].reg_addr = 0x109;
-		sc_access[2].value = 0x00;
-		sc_access[2].mask = MASK6;
-		sc_access[3].reg_addr = 0x105;
-		sc_access[3].value = 0x40;
-		sc_access[3].mask = MASK6;
-		num_val = 4;
-		break;
-
-	case HS_MIC:
-		pr_debug("Selecting HS_MIC\n");
-		sc_access[0].reg_addr = MICCTRL;
-		sc_access[0].mask =  MASK6|MASK3|MASK1|MASK0;
-		sc_access[0].value = 0x00;
-		sc_access[1].reg_addr = 0x109;
-		sc_access[1].mask = MASK6;
-		sc_access[1].value = 0x40;
-		sc_access[2].reg_addr = 0x10a;
-		sc_access[2].mask = MASK6;
-		sc_access[2].value = 0x00;
-		sc_access[3].reg_addr = 0x105;
-		sc_access[3].value = 0x40;
-		sc_access[3].mask = MASK6;
-		sc_access[4].reg_addr = ADCSAMPLERATE;
-		sc_access[4].mask = MASK7|MASK6|MASK5|MASK4|MASK3;
-		sc_access[4].value = 0xc8;
-		num_val = 5;
-		break;
-
-	case DMIC:
-		pr_debug("DMIC\n");
-		sc_access[0].reg_addr = MICCTRL;
-		sc_access[0].mask = MASK6|MASK3|MASK1|MASK0;
-		sc_access[0].value = 0x0B;
-		sc_access[1].reg_addr = 0x105;
-		sc_access[1].value = 0x80;
-		sc_access[1].mask = MASK7|MASK6;
-		sc_access[2].reg_addr = 0x10a;
-		sc_access[2].value = 0x40;
-		sc_access[2].mask = MASK6;
-		sc_access[3].reg_addr = LILSEL;
-		sc_access[3].mask = MASK6;
-		sc_access[3].value = 0x00;
-		sc_access[4].reg_addr = ADCSAMPLERATE;
-		sc_access[4].mask =  MASK7|MASK6|MASK5|MASK4|MASK3;
-		sc_access[4].value = 0x33;
-		num_val = 5;
-		break;
-	default:
-		return -EINVAL;
-	}
-	return sst_sc_reg_access(sc_access, PMIC_READ_MODIFY, num_val);
-}
-
-static int nc_get_mute(int dev_id, u8 *value)
-{
-	int retval = 0, mask = 0;
-	struct sc_reg_access sc_access = {0,};
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	pr_debug("get mute::%d\n", dev_id);
-
-	switch (dev_id) {
-	case PMIC_SND_AMIC_MUTE:
-		pr_debug("PMIC_SND_INPUT_MUTE_MIC1\n");
-		sc_access.reg_addr = LILSEL;
-		mask = MASK6;
-		break;
-	case PMIC_SND_HP_MIC_MUTE:
-		pr_debug("PMIC_SND_INPUT_MUTE_MIC2\n");
-		sc_access.reg_addr = LIRSEL;
-		mask = MASK6;
-		break;
-	case PMIC_SND_LEFT_HP_MUTE:
-	case PMIC_SND_RIGHT_HP_MUTE:
-		mask = MASK2;
-		pr_debug("PMIC_SN_LEFT/RIGHT_HP_MUTE\n");
-		if (dev_id == PMIC_SND_RIGHT_HP_MUTE)
-			sc_access.reg_addr = RMUTE;
-		else
-			sc_access.reg_addr = LMUTE;
-		break;
-
-	case PMIC_SND_LEFT_SPEAKER_MUTE:
-		pr_debug("PMIC_MONO_EARPIECE_MUTE\n");
-		sc_access.reg_addr = RMUTE;
-		mask = MASK1;
-		break;
-	case PMIC_SND_DMIC_MUTE:
-		pr_debug("PMIC_SND_INPUT_MUTE_DMIC\n");
-		sc_access.reg_addr = 0x105;
-		mask = MASK6;
-		break;
-	default:
-		return -EINVAL;
-
-	}
-	retval = sst_sc_reg_access(&sc_access, PMIC_READ, 1);
-	pr_debug("reg value = %d\n", sc_access.value);
-	if (retval)
-		return retval;
-	*value = (sc_access.value) & mask;
-	pr_debug("masked value = %d\n", *value);
-	if (*value)
-		*value = 0;
-	else
-		*value = 1;
-	pr_debug("value returned = 0x%x\n", *value);
-	return retval;
-}
-
-static int nc_get_vol(int dev_id, int *value)
-{
-	int retval = 0, mask = 0;
-	struct sc_reg_access sc_access = {0,};
-
-	if (snd_pmic_ops_nc.card_status == SND_CARD_UN_INIT)
-		retval = nc_init_card();
-	if (retval)
-		return retval;
-
-	switch (dev_id) {
-	case PMIC_SND_CAPTURE_VOL:
-		pr_debug("PMIC_SND_INPUT_CAPTURE_VOL\n");
-		sc_access.reg_addr =  LILSEL;
-		mask = (MASK0|MASK1|MASK2|MASK3|MASK4|MASK5);
-		break;
-
-	case PMIC_SND_LEFT_MASTER_VOL:
-		pr_debug("GET_VOLUME_PMIC_LEFT_MASTER_VOL\n");
-		sc_access.reg_addr = AUDIOLVOL;
-		mask = (MASK0|MASK1|MASK2|MASK3|MASK4|MASK5|MASK6);
-		break;
-
-	case PMIC_SND_RIGHT_MASTER_VOL:
-		pr_debug("GET_VOLUME_PMIC_RIGHT_MASTER_VOL\n");
-		sc_access.reg_addr = AUDIORVOL;
-		mask = (MASK0|MASK1|MASK2|MASK3|MASK4|MASK5|MASK6);
-		break;
-
-	case PMIC_SND_RIGHT_PB_VOL:
-		pr_debug("GET_VOLUME_PMIC_RIGHT_HP_VOL\n");
-		sc_access.reg_addr = HPRVOL;
-		mask = (MASK0|MASK1|MASK2|MASK3|MASK4);
-		break;
-
-	case PMIC_SND_LEFT_PB_VOL:
-		pr_debug("GET_VOLUME_PMIC_LEFT_HP_VOL\n");
-		sc_access.reg_addr = HPLVOL;
-		mask = (MASK0|MASK1|MASK2|MASK3|MASK4);
-		break;
-
-	default:
-		return -EINVAL;
-
-	}
-	retval = sst_sc_reg_access(&sc_access, PMIC_READ, 1);
-	pr_debug("value read = 0x%x\n", sc_access.value);
-	*value = -((sc_access.value) & mask);
-	pr_debug("get vol value returned = %d\n", *value);
-	return retval;
-}
-
-static void hp_automute(enum snd_jack_types type, int present)
-{
-	u8 in = DMIC;
-	u8 out = INTERNAL_SPKR;
-	if (present) {
-		if (type == SND_JACK_HEADSET)
-			in = HS_MIC;
-		out = STEREO_HEADPHONE;
-	}
-	nc_set_selected_input_dev(in);
-	nc_set_selected_output_dev(out);
-}
-
-static void nc_pmic_irq_cb(void *cb_data, u8 intsts)
-{
-	u8 value = 0;
-	struct mad_jack *mjack = NULL;
-	unsigned int present = 0, jack_event_flag = 0, buttonpressflag = 0;
-	struct snd_intelmad *intelmaddata = cb_data;
-	struct sc_reg_access sc_access_read = {0,};
-
-	sc_access_read.reg_addr = 0x132;
-	sst_sc_reg_access(&sc_access_read, PMIC_READ, 1);
-	value = (sc_access_read.value);
-	pr_debug("value returned = 0x%x\n", value);
-
-	mjack = &intelmaddata->jack[0];
-	if (intsts & 0x1) {
-		pr_debug("SST DBG:MAD headset detected\n");
-		/* send headset detect/undetect */
-		present = (value == 0x1) ? 3 : 0;
-		jack_event_flag = 1;
-		mjack->jack.type = SND_JACK_HEADSET;
-		hp_automute(SND_JACK_HEADSET, present);
-	}
-
-	if (intsts & 0x2) {
-		pr_debug(":MAD headphone detected\n");
-		/* send headphone detect/undetect */
-		present = (value == 0x2) ? 1 : 0;
-		jack_event_flag = 1;
-		mjack->jack.type = SND_JACK_HEADPHONE;
-		hp_automute(SND_JACK_HEADPHONE, present);
-	}
-
-	if (intsts & 0x4) {
-		pr_debug("MAD short push detected\n");
-		/* send short push */
-		present = 1;
-		jack_event_flag = 1;
-		buttonpressflag = 1;
-		mjack->jack.type = MID_JACK_HS_SHORT_PRESS;
-	}
-
-	if (intsts & 0x8) {
-		pr_debug(":MAD long push detected\n");
-		/* send long push */
-		present = 1;
-		jack_event_flag = 1;
-		buttonpressflag = 1;
-		mjack->jack.type = MID_JACK_HS_LONG_PRESS;
-	}
-
-	if (jack_event_flag)
-		sst_mad_send_jack_report(&mjack->jack,
-					buttonpressflag, present);
-}
-static int nc_jack_enable(void)
-{
-	return 0;
-}
-
-struct snd_pmic_ops snd_pmic_ops_nc = {
-	.input_dev_id   =       DMIC,
-	.output_dev_id  =       INTERNAL_SPKR,
-	.set_input_dev	=	nc_set_selected_input_dev,
-	.set_output_dev =	nc_set_selected_output_dev,
-	.set_mute	=	nc_set_mute,
-	.get_mute	=	nc_get_mute,
-	.set_vol	=	nc_set_vol,
-	.get_vol	=	nc_get_vol,
-	.init_card	=	nc_init_card,
-	.set_pcm_audio_params	= nc_set_pcm_audio_params,
-	.set_pcm_voice_params	= nc_set_pcm_voice_params,
-	.set_voice_port = nc_set_voice_port,
-	.set_audio_port = nc_set_audio_port,
-	.power_up_pmic_pb =	nc_power_up_pb,
-	.power_up_pmic_cp =	nc_power_up_cp,
-	.power_down_pmic_pb =	nc_power_down_pb,
-	.power_down_pmic_cp =	nc_power_down_cp,
-	.power_down_pmic	=	nc_power_down,
-	.pmic_irq_cb	=	nc_pmic_irq_cb,
-	.pmic_jack_enable =	nc_jack_enable,
-};
diff --git a/drivers/staging/line6/Makefile b/drivers/staging/line6/Makefile
index de6bd12e9736..34a2ddacc7e9 100644
--- a/drivers/staging/line6/Makefile
+++ b/drivers/staging/line6/Makefile
@@ -12,4 +12,5 @@ line6usb-y := 		\
 		playback.o	\
 		pod.o		\
 		toneport.o	\
-		variax.o
+		variax.o	\
+		podhd.o
diff --git a/drivers/staging/line6/capture.c b/drivers/staging/line6/capture.c
index 9647154a4923..127f95247749 100644
--- a/drivers/staging/line6/capture.c
+++ b/drivers/staging/line6/capture.c
@@ -9,6 +9,7 @@
  *
  */
 
+#include <linux/slab.h>
 #include <sound/core.h>
 #include <sound/pcm.h>
 #include <sound/pcm_params.h>
@@ -192,6 +193,31 @@ void line6_capture_check_period(struct snd_line6_pcm *line6pcm, int length)
 	}
 }
 
+int line6_alloc_capture_buffer(struct snd_line6_pcm *line6pcm)
+{
+	/* We may be invoked multiple times in a row so allocate once only */
+	if (line6pcm->buffer_in)
+		return 0;
+
+	line6pcm->buffer_in =
+		kmalloc(LINE6_ISO_BUFFERS * LINE6_ISO_PACKETS *
+			line6pcm->max_packet_size, GFP_KERNEL);
+
+	if (!line6pcm->buffer_in) {
+		dev_err(line6pcm->line6->ifcdev,
+			"cannot malloc capture buffer\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void line6_free_capture_buffer(struct snd_line6_pcm *line6pcm)
+{
+	kfree(line6pcm->buffer_in);
+	line6pcm->buffer_in = NULL;
+}
+
 /*
  * Callback for completed capture URB.
  */
@@ -243,11 +269,7 @@ static void audio_in_callback(struct urb *urb)
 		length += fsize;
 
 		/* the following assumes LINE6_ISO_PACKETS == 1: */
-#if LINE6_BACKUP_MONITOR_SIGNAL
-		memcpy(line6pcm->prev_fbuf, fbuf, fsize);
-#else
 		line6pcm->prev_fbuf = fbuf;
-#endif
 		line6pcm->prev_fsize = fsize;
 
 #ifdef CONFIG_LINE6_USB_IMPULSE_RESPONSE
@@ -319,6 +341,13 @@ static int snd_line6_capture_hw_params(struct snd_pcm_substream *substream,
 	}
 	/* -- [FD] end */
 
+	if ((line6pcm->flags & MASK_CAPTURE) == 0) {
+		ret = line6_alloc_capture_buffer(line6pcm);
+
+		if (ret < 0)
+			return ret;
+	}
+
 	ret = snd_pcm_lib_malloc_pages(substream,
 				       params_buffer_bytes(hw_params));
 	if (ret < 0)
@@ -331,6 +360,13 @@ static int snd_line6_capture_hw_params(struct snd_pcm_substream *substream,
 /* hw_free capture callback */
 static int snd_line6_capture_hw_free(struct snd_pcm_substream *substream)
 {
+	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
+	if ((line6pcm->flags & MASK_CAPTURE) == 0) {
+		line6_unlink_wait_clear_audio_in_urbs(line6pcm);
+		line6_free_capture_buffer(line6pcm);
+	}
+
 	return snd_pcm_lib_free_pages(substream);
 }
 
diff --git a/drivers/staging/line6/capture.h b/drivers/staging/line6/capture.h
index a7509fbbb954..366cbaa7c88d 100644
--- a/drivers/staging/line6/capture.h
+++ b/drivers/staging/line6/capture.h
@@ -19,11 +19,13 @@
 
 extern struct snd_pcm_ops snd_line6_capture_ops;
 
+extern int line6_alloc_capture_buffer(struct snd_line6_pcm *line6pcm);
 extern void line6_capture_copy(struct snd_line6_pcm *line6pcm, char *fbuf,
 			       int fsize);
 extern void line6_capture_check_period(struct snd_line6_pcm *line6pcm,
 				       int length);
 extern int line6_create_audio_in_urbs(struct snd_line6_pcm *line6pcm);
+extern void line6_free_capture_buffer(struct snd_line6_pcm *line6pcm);
 extern int line6_submit_audio_in_all_urbs(struct snd_line6_pcm *line6pcm);
 extern void line6_unlink_audio_in_urbs(struct snd_line6_pcm *line6pcm);
 extern void line6_unlink_wait_clear_audio_in_urbs(struct snd_line6_pcm
diff --git a/drivers/staging/line6/driver.c b/drivers/staging/line6/driver.c
index 851b762319cf..6a1959e16e00 100644
--- a/drivers/staging/line6/driver.c
+++ b/drivers/staging/line6/driver.c
@@ -21,6 +21,7 @@
 #include "midi.h"
 #include "playback.h"
 #include "pod.h"
+#include "podhd.h"
 #include "revision.h"
 #include "toneport.h"
 #include "usbdefs.h"
@@ -37,6 +38,8 @@ static const struct usb_device_id line6_id_table[] = {
 	{USB_DEVICE(LINE6_VENDOR_ID, LINE6_DEVID_BASSPODXTPRO)},
 	{USB_DEVICE(LINE6_VENDOR_ID, LINE6_DEVID_GUITARPORT)},
 	{USB_DEVICE(LINE6_VENDOR_ID, LINE6_DEVID_POCKETPOD)},
+	{USB_DEVICE(LINE6_VENDOR_ID, LINE6_DEVID_PODHD300)},
+	{USB_DEVICE(LINE6_VENDOR_ID, LINE6_DEVID_PODHD500)},
 	{USB_DEVICE(LINE6_VENDOR_ID, LINE6_DEVID_PODSTUDIO_GX)},
 	{USB_DEVICE(LINE6_VENDOR_ID, LINE6_DEVID_PODSTUDIO_UX1)},
 	{USB_DEVICE(LINE6_VENDOR_ID, LINE6_DEVID_PODSTUDIO_UX2)},
@@ -56,23 +59,25 @@ MODULE_DEVICE_TABLE(usb, line6_id_table);
 
 /* *INDENT-OFF* */
 static struct line6_properties line6_properties_table[] = {
-	{ "BassPODxt",     "BassPODxt",        LINE6_BIT_BASSPODXT,     LINE6_BIT_CONTROL_PCM_HWMON },
-	{ "BassPODxtLive", "BassPODxt Live",   LINE6_BIT_BASSPODXTLIVE, LINE6_BIT_CONTROL_PCM_HWMON },
-	{ "BassPODxtPro",  "BassPODxt Pro",    LINE6_BIT_BASSPODXTPRO,  LINE6_BIT_CONTROL_PCM_HWMON },
-	{ "GuitarPort",    "GuitarPort",       LINE6_BIT_GUITARPORT,    LINE6_BIT_PCM               },
-	{ "PocketPOD",     "Pocket POD",       LINE6_BIT_POCKETPOD,     LINE6_BIT_CONTROL           },
-	{ "PODStudioGX",   "POD Studio GX",    LINE6_BIT_PODSTUDIO_GX,  LINE6_BIT_PCM               },
-	{ "PODStudioUX1",  "POD Studio UX1",   LINE6_BIT_PODSTUDIO_UX1, LINE6_BIT_PCM               },
-	{ "PODStudioUX2",  "POD Studio UX2",   LINE6_BIT_PODSTUDIO_UX2, LINE6_BIT_PCM               },
-	{ "PODX3",         "POD X3",           LINE6_BIT_PODX3,         LINE6_BIT_PCM               },
-	{ "PODX3Live",     "POD X3 Live",      LINE6_BIT_PODX3LIVE,     LINE6_BIT_PCM               },
-	{ "PODxt",         "PODxt",            LINE6_BIT_PODXT,         LINE6_BIT_CONTROL_PCM_HWMON },
-	{ "PODxtLive",     "PODxt Live",       LINE6_BIT_PODXTLIVE,     LINE6_BIT_CONTROL_PCM_HWMON },
-	{ "PODxtPro",      "PODxt Pro",        LINE6_BIT_PODXTPRO,      LINE6_BIT_CONTROL_PCM_HWMON },
-	{ "TonePortGX",    "TonePort GX",      LINE6_BIT_TONEPORT_GX,   LINE6_BIT_PCM               },
-	{ "TonePortUX1",   "TonePort UX1",     LINE6_BIT_TONEPORT_UX1,  LINE6_BIT_PCM               },
-	{ "TonePortUX2",   "TonePort UX2",     LINE6_BIT_TONEPORT_UX2,  LINE6_BIT_PCM               },
-	{ "Variax",        "Variax Workbench", LINE6_BIT_VARIAX,        LINE6_BIT_CONTROL           }
+	{ LINE6_BIT_BASSPODXT,     "BassPODxt",     "BassPODxt",        LINE6_BIT_CONTROL_PCM_HWMON },
+	{ LINE6_BIT_BASSPODXTLIVE, "BassPODxtLive", "BassPODxt Live",   LINE6_BIT_CONTROL_PCM_HWMON },
+	{ LINE6_BIT_BASSPODXTPRO,  "BassPODxtPro",  "BassPODxt Pro",    LINE6_BIT_CONTROL_PCM_HWMON },
+	{ LINE6_BIT_GUITARPORT,    "GuitarPort",    "GuitarPort",       LINE6_BIT_PCM               },
+	{ LINE6_BIT_POCKETPOD,     "PocketPOD",     "Pocket POD",       LINE6_BIT_CONTROL           },
+	{ LINE6_BIT_PODHD300,      "PODHD300",      "POD HD300",        LINE6_BIT_CONTROL_PCM_HWMON },
+	{ LINE6_BIT_PODHD500,      "PODHD500",      "POD HD500",        LINE6_BIT_CONTROL_PCM_HWMON },
+	{ LINE6_BIT_PODSTUDIO_GX,  "PODStudioGX",   "POD Studio GX",    LINE6_BIT_PCM               },
+	{ LINE6_BIT_PODSTUDIO_UX1, "PODStudioUX1",  "POD Studio UX1",   LINE6_BIT_PCM               },
+	{ LINE6_BIT_PODSTUDIO_UX2, "PODStudioUX2",  "POD Studio UX2",   LINE6_BIT_PCM               },
+	{ LINE6_BIT_PODX3,         "PODX3",         "POD X3",           LINE6_BIT_PCM               },
+	{ LINE6_BIT_PODX3LIVE,     "PODX3Live",     "POD X3 Live",      LINE6_BIT_PCM               },
+	{ LINE6_BIT_PODXT,         "PODxt",         "PODxt",            LINE6_BIT_CONTROL_PCM_HWMON },
+	{ LINE6_BIT_PODXTLIVE,     "PODxtLive",     "PODxt Live",       LINE6_BIT_CONTROL_PCM_HWMON },
+	{ LINE6_BIT_PODXTPRO,      "PODxtPro",      "PODxt Pro",        LINE6_BIT_CONTROL_PCM_HWMON },
+	{ LINE6_BIT_TONEPORT_GX,   "TonePortGX",    "TonePort GX",      LINE6_BIT_PCM               },
+	{ LINE6_BIT_TONEPORT_UX1,  "TonePortUX1",   "TonePort UX1",     LINE6_BIT_PCM               },
+	{ LINE6_BIT_TONEPORT_UX2,  "TonePortUX2",   "TonePort UX2",     LINE6_BIT_PCM               },
+	{ LINE6_BIT_VARIAX,        "Variax",        "Variax Workbench", LINE6_BIT_CONTROL           },
 };
 /* *INDENT-ON* */
 
@@ -437,6 +442,10 @@ static void line6_data_received(struct urb *urb)
 						  line6);
 			break;
 
+		case LINE6_DEVID_PODHD300:
+		case LINE6_DEVID_PODHD500:
+			break; /* let userspace handle MIDI */
+
 		case LINE6_DEVID_PODXTLIVE:
 			switch (line6->interface_number) {
 			case PODXTLIVE_INTERFACE_POD:
@@ -720,8 +729,8 @@ static int line6_probe(struct usb_interface *interface,
 		       const struct usb_device_id *id)
 {
 	int devtype;
-	struct usb_device *usbdev = NULL;
-	struct usb_line6 *line6 = NULL;
+	struct usb_device *usbdev;
+	struct usb_line6 *line6;
 	const struct line6_properties *properties;
 	int devnum;
 	int interface_number, alternate = 0;
@@ -794,6 +803,7 @@ static int line6_probe(struct usb_interface *interface,
 		}
 		break;
 
+	case LINE6_DEVID_PODHD500:
 	case LINE6_DEVID_PODX3:
 	case LINE6_DEVID_PODX3LIVE:
 		switch (interface_number) {
@@ -812,6 +822,7 @@ static int line6_probe(struct usb_interface *interface,
 	case LINE6_DEVID_BASSPODXTPRO:
 	case LINE6_DEVID_PODXT:
 	case LINE6_DEVID_PODXTPRO:
+	case LINE6_DEVID_PODHD300:
 		alternate = 5;
 		break;
 
@@ -865,6 +876,18 @@ static int line6_probe(struct usb_interface *interface,
 		ep_write = 0x03;
 		break;
 
+	case LINE6_DEVID_PODHD300:
+		size = sizeof(struct usb_line6_podhd);
+		ep_read = 0x84;
+		ep_write = 0x03;
+		break;
+
+	case LINE6_DEVID_PODHD500:
+		size = sizeof(struct usb_line6_podhd);
+		ep_read = 0x81;
+		ep_write = 0x01;
+		break;
+
 	case LINE6_DEVID_POCKETPOD:
 		size = sizeof(struct usb_line6_pod);
 		ep_read = 0x82;
@@ -923,7 +946,7 @@ static int line6_probe(struct usb_interface *interface,
 	}
 
 	if (size == 0) {
-		dev_err(line6->ifcdev,
+		dev_err(&interface->dev,
 			"driver bug: interface data size not set\n");
 		ret = -ENODEV;
 		goto err_put;
@@ -1017,6 +1040,12 @@ static int line6_probe(struct usb_interface *interface,
 		ret = line6_pod_init(interface, (struct usb_line6_pod *)line6);
 		break;
 
+	case LINE6_DEVID_PODHD300:
+	case LINE6_DEVID_PODHD500:
+		ret = line6_podhd_init(interface,
+				       (struct usb_line6_podhd *)line6);
+		break;
+
 	case LINE6_DEVID_PODXTLIVE:
 		switch (interface_number) {
 		case PODXTLIVE_INTERFACE_POD:
@@ -1139,6 +1168,11 @@ static void line6_disconnect(struct usb_interface *interface)
 			line6_pod_disconnect(interface);
 			break;
 
+		case LINE6_DEVID_PODHD300:
+		case LINE6_DEVID_PODHD500:
+			line6_podhd_disconnect(interface);
+			break;
+
 		case LINE6_DEVID_PODXTLIVE:
 			switch (interface_number) {
 			case PODXTLIVE_INTERFACE_POD:
diff --git a/drivers/staging/line6/driver.h b/drivers/staging/line6/driver.h
index 553192f49317..117bf9943568 100644
--- a/drivers/staging/line6/driver.h
+++ b/drivers/staging/line6/driver.h
@@ -88,6 +88,11 @@ static const int SYSEX_EXTRA_SIZE = sizeof(line6_midi_id) + 4;
 */
 struct line6_properties {
 	/**
+		 Bit identifying this device in the line6usb driver.
+	*/
+	int device_bit;
+
+	/**
 		 Card id string (maximum 16 characters).
 		 This can be used to address the device in ALSA programs as
 		 "default:CARD=<id>"
@@ -100,11 +105,6 @@ struct line6_properties {
 	const char *name;
 
 	/**
-		 Bit identifying this device in the line6usb driver.
-	*/
-	int device_bit;
-
-	/**
 		 Bit vector defining this device's capabilities in the
 		 line6usb driver.
 	*/
diff --git a/drivers/staging/line6/midi.c b/drivers/staging/line6/midi.c
index e554a2da643a..13d02939c3cb 100644
--- a/drivers/staging/line6/midi.c
+++ b/drivers/staging/line6/midi.c
@@ -135,7 +135,7 @@ static int send_midi_async(struct usb_line6 *line6, unsigned char *data,
 	line6_write_hexdump(line6, 'S', data, length);
 #endif
 
-	transfer_buffer = kmalloc(length, GFP_ATOMIC);
+	transfer_buffer = kmemdup(data, length, GFP_ATOMIC);
 
 	if (transfer_buffer == NULL) {
 		usb_free_urb(urb);
@@ -143,7 +143,6 @@ static int send_midi_async(struct usb_line6 *line6, unsigned char *data,
 		return -ENOMEM;
 	}
 
-	memcpy(transfer_buffer, data, length);
 	usb_fill_int_urb(urb, line6->usbdev,
 			 usb_sndbulkpipe(line6->usbdev,
 					 line6->ep_control_write),
@@ -173,6 +172,8 @@ static int send_midi_async(struct usb_line6 *line6, unsigned char *data,
 		break;
 
 	case LINE6_DEVID_VARIAX:
+	case LINE6_DEVID_PODHD300:
+	case LINE6_DEVID_PODHD500:
 		break;
 
 	default:
@@ -307,10 +308,10 @@ static ssize_t midi_set_midi_mask_transmit(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6 *line6 = usb_get_intfdata(interface);
-	unsigned long value;
+	unsigned short value;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou16(buf, 10, &value);
 	if (ret)
 		return ret;
 
@@ -339,10 +340,10 @@ static ssize_t midi_set_midi_mask_receive(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6 *line6 = usb_get_intfdata(interface);
-	unsigned long value;
+	unsigned short value;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &value);
+	ret = kstrtou16(buf, 10, &value);
 	if (ret)
 		return ret;
 
@@ -391,16 +392,32 @@ int line6_init_midi(struct usb_line6 *line6)
 		return -ENOMEM;
 
 	err = line6_midibuf_init(&line6midi->midibuf_in, MIDI_BUFFER_SIZE, 0);
-	if (err < 0)
+	if (err < 0) {
+		kfree(line6midi);
 		return err;
+	}
 
 	err = line6_midibuf_init(&line6midi->midibuf_out, MIDI_BUFFER_SIZE, 1);
-	if (err < 0)
+	if (err < 0) {
+		kfree(line6midi->midibuf_in.buf);
+		kfree(line6midi);
 		return err;
+	}
 
 	line6midi->line6 = line6;
-	line6midi->midi_mask_transmit = 1;
-	line6midi->midi_mask_receive = 4;
+
+	switch(line6->product) {
+	case LINE6_DEVID_PODHD300:
+	case LINE6_DEVID_PODHD500:
+		line6midi->midi_mask_transmit = 1;
+		line6midi->midi_mask_receive = 1;
+		break;
+
+	default:
+		line6midi->midi_mask_transmit = 1;
+		line6midi->midi_mask_receive = 4;
+	}
+
 	line6->line6midi = line6midi;
 
 	err = snd_device_new(line6->card, SNDRV_DEV_RAWMIDI, line6midi,
diff --git a/drivers/staging/line6/midi.h b/drivers/staging/line6/midi.h
index b73a025d8be9..4a9e9f947297 100644
--- a/drivers/staging/line6/midi.h
+++ b/drivers/staging/line6/midi.h
@@ -57,12 +57,12 @@ struct snd_line6_midi {
 	/**
 		 Bit mask for output MIDI channels.
 	*/
-	int midi_mask_transmit;
+	unsigned short midi_mask_transmit;
 
 	/**
 		 Bit mask for input MIDI channels.
 	*/
-	int midi_mask_receive;
+	unsigned short midi_mask_receive;
 
 	/**
 		 Buffer for incoming MIDI stream.
diff --git a/drivers/staging/line6/pcm.c b/drivers/staging/line6/pcm.c
index 9d4c8a606eea..37675e66da81 100644
--- a/drivers/staging/line6/pcm.c
+++ b/drivers/staging/line6/pcm.c
@@ -86,31 +86,22 @@ static DEVICE_ATTR(impulse_period, S_IWUSR | S_IRUGO, pcm_get_impulse_period,
 
 #endif
 
+static bool test_flags(unsigned long flags0, unsigned long flags1,
+		       unsigned long mask)
+{
+	return ((flags0 & mask) == 0) && ((flags1 & mask) != 0);
+}
+
 int line6_pcm_start(struct snd_line6_pcm *line6pcm, int channels)
 {
 	unsigned long flags_old =
 	    __sync_fetch_and_or(&line6pcm->flags, channels);
 	unsigned long flags_new = flags_old | channels;
 	int err = 0;
-
-#if LINE6_BACKUP_MONITOR_SIGNAL
-	if (!(line6pcm->line6->properties->capabilities & LINE6_BIT_HWMON)) {
-		line6pcm->prev_fbuf =
-		    kmalloc(LINE6_ISO_PACKETS * line6pcm->max_packet_size,
-			    GFP_KERNEL);
-
-		if (!line6pcm->prev_fbuf) {
-			dev_err(line6pcm->line6->ifcdev,
-				"cannot malloc monitor buffer\n");
-			return -ENOMEM;
-		}
-	}
-#else
+	
 	line6pcm->prev_fbuf = NULL;
-#endif
 
-	if (((flags_old & MASK_CAPTURE) == 0) &&
-	    ((flags_new & MASK_CAPTURE) != 0)) {
+	if (test_flags(flags_old, flags_new, MASK_CAPTURE)) {
 		/*
 		   Waiting for completion of active URBs in the stop handler is
 		   a bug, we therefore report an error if capturing is restarted
@@ -119,54 +110,47 @@ int line6_pcm_start(struct snd_line6_pcm *line6pcm, int channels)
 		if (line6pcm->active_urb_in | line6pcm->unlink_urb_in)
 			return -EBUSY;
 
-		line6pcm->buffer_in =
-		    kmalloc(LINE6_ISO_BUFFERS * LINE6_ISO_PACKETS *
-			    line6pcm->max_packet_size, GFP_KERNEL);
+		if (!(flags_new & MASK_PCM_ALSA_CAPTURE)) {
+			err = line6_alloc_capture_buffer(line6pcm);
 
-		if (!line6pcm->buffer_in) {
-			dev_err(line6pcm->line6->ifcdev,
-				"cannot malloc capture buffer\n");
-			return -ENOMEM;
+			if (err < 0)
+				goto pcm_start_error;
 		}
 
 		line6pcm->count_in = 0;
 		line6pcm->prev_fsize = 0;
 		err = line6_submit_audio_in_all_urbs(line6pcm);
 
-		if (err < 0) {
-			__sync_fetch_and_and(&line6pcm->flags, ~channels);
-			return err;
-		}
+		if (err < 0)
+			goto pcm_start_error;
 	}
 
-	if (((flags_old & MASK_PLAYBACK) == 0) &&
-	    ((flags_new & MASK_PLAYBACK) != 0)) {
+	if (test_flags(flags_old, flags_new, MASK_PLAYBACK)) {
 		/*
 		   See comment above regarding PCM restart.
 		 */
 		if (line6pcm->active_urb_out | line6pcm->unlink_urb_out)
 			return -EBUSY;
 
-		line6pcm->buffer_out =
-		    kmalloc(LINE6_ISO_BUFFERS * LINE6_ISO_PACKETS *
-			    line6pcm->max_packet_size, GFP_KERNEL);
+		if (!(flags_new & MASK_PCM_ALSA_PLAYBACK)) {
+			err = line6_alloc_playback_buffer(line6pcm);
 
-		if (!line6pcm->buffer_out) {
-			dev_err(line6pcm->line6->ifcdev,
-				"cannot malloc playback buffer\n");
-			return -ENOMEM;
+			if (err < 0)
+				goto pcm_start_error;
 		}
 
 		line6pcm->count_out = 0;
 		err = line6_submit_audio_out_all_urbs(line6pcm);
 
-		if (err < 0) {
-			__sync_fetch_and_and(&line6pcm->flags, ~channels);
-			return err;
-		}
+		if (err < 0)
+			goto pcm_start_error;
 	}
 
 	return 0;
+
+pcm_start_error:
+	__sync_fetch_and_and(&line6pcm->flags, ~channels);
+	return err;
 }
 
 int line6_pcm_stop(struct snd_line6_pcm *line6pcm, int channels)
@@ -175,22 +159,19 @@ int line6_pcm_stop(struct snd_line6_pcm *line6pcm, int channels)
 	    __sync_fetch_and_and(&line6pcm->flags, ~channels);
 	unsigned long flags_new = flags_old & ~channels;
 
-	if (((flags_old & MASK_CAPTURE) != 0) &&
-	    ((flags_new & MASK_CAPTURE) == 0)) {
+	if (test_flags(flags_new, flags_old, MASK_CAPTURE)) {
 		line6_unlink_audio_in_urbs(line6pcm);
-		kfree(line6pcm->buffer_in);
-		line6pcm->buffer_in = NULL;
+
+		if (!(flags_old & MASK_PCM_ALSA_CAPTURE))
+			line6_free_capture_buffer(line6pcm);
 	}
 
-	if (((flags_old & MASK_PLAYBACK) != 0) &&
-	    ((flags_new & MASK_PLAYBACK) == 0)) {
+	if (test_flags(flags_new, flags_old, MASK_PLAYBACK)) {
 		line6_unlink_audio_out_urbs(line6pcm);
-		kfree(line6pcm->buffer_out);
-		line6pcm->buffer_out = NULL;
+
+		if (!(flags_old & MASK_PCM_ALSA_PLAYBACK))
+			line6_free_playback_buffer(line6pcm);
 	}
-#if LINE6_BACKUP_MONITOR_SIGNAL
-	kfree(line6pcm->prev_fbuf);
-#endif
 
 	return 0;
 }
@@ -403,10 +384,12 @@ int line6_init_pcm(struct usb_line6 *line6,
 	case LINE6_DEVID_PODXT:
 	case LINE6_DEVID_PODXTLIVE:
 	case LINE6_DEVID_PODXTPRO:
+	case LINE6_DEVID_PODHD300:
 		ep_read = 0x82;
 		ep_write = 0x01;
 		break;
 
+	case LINE6_DEVID_PODHD500:
 	case LINE6_DEVID_PODX3:
 	case LINE6_DEVID_PODX3LIVE:
 		ep_read = 0x86;
@@ -451,9 +434,14 @@ int line6_init_pcm(struct usb_line6 *line6,
 	line6pcm->line6 = line6;
 	line6pcm->ep_audio_read = ep_read;
 	line6pcm->ep_audio_write = ep_write;
-	line6pcm->max_packet_size = usb_maxpacket(line6->usbdev,
-						  usb_rcvintpipe(line6->usbdev,
-								 ep_read), 0);
+
+	/* Read and write buffers are sized identically, so choose minimum */
+	line6pcm->max_packet_size = min(
+			usb_maxpacket(line6->usbdev,
+				usb_rcvisocpipe(line6->usbdev, ep_read), 0),
+			usb_maxpacket(line6->usbdev,
+				usb_sndisocpipe(line6->usbdev, ep_write), 1));
+
 	line6pcm->properties = properties;
 	line6->line6pcm = line6pcm;
 
@@ -508,6 +496,23 @@ int snd_line6_prepare(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
 
+	switch (substream->stream) {
+	case SNDRV_PCM_STREAM_PLAYBACK:
+		if ((line6pcm->flags & MASK_PLAYBACK) == 0)
+			line6_unlink_wait_clear_audio_out_urbs(line6pcm);
+
+		break;
+
+	case SNDRV_PCM_STREAM_CAPTURE:
+		if ((line6pcm->flags & MASK_CAPTURE) == 0)
+			line6_unlink_wait_clear_audio_in_urbs(line6pcm);
+
+		break;
+
+	default:
+		MISSING_CASE;
+	}
+
 	if (!test_and_set_bit(BIT_PREPARED, &line6pcm->flags)) {
 		line6pcm->count_out = 0;
 		line6pcm->pos_out = 0;
diff --git a/drivers/staging/line6/pcm.h b/drivers/staging/line6/pcm.h
index 77055b3724ad..55d8297dd3d9 100644
--- a/drivers/staging/line6/pcm.h
+++ b/drivers/staging/line6/pcm.h
@@ -39,9 +39,6 @@
 #define LINE6_IMPULSE_DEFAULT_PERIOD 100
 #endif
 
-#define LINE6_BACKUP_MONITOR_SIGNAL 0
-#define LINE6_REUSE_DMA_AREA_FOR_PLAYBACK 0
-
 /*
 	Get substream from Line6 PCM data structure
 */
@@ -149,11 +146,6 @@ struct snd_line6_pcm {
 	unsigned char *buffer_in;
 
 	/**
-		 Temporary buffer index for playback.
-	*/
-	int index_out;
-
-	/**
 		 Previously captured frame (for software monitoring).
 	*/
 	unsigned char *prev_fbuf;
diff --git a/drivers/staging/line6/playback.c b/drivers/staging/line6/playback.c
index 10c543836583..4152db2328b7 100644
--- a/drivers/staging/line6/playback.c
+++ b/drivers/staging/line6/playback.c
@@ -9,6 +9,7 @@
  *
  */
 
+#include <linux/slab.h>
 #include <sound/core.h>
 #include <sound/pcm.h>
 #include <sound/pcm_params.h>
@@ -191,13 +192,10 @@ static int submit_audio_out_urb(struct snd_line6_pcm *line6pcm)
 	urb_frames = urb_size / bytes_per_frame;
 	urb_out->transfer_buffer =
 	    line6pcm->buffer_out +
-	    line6pcm->max_packet_size * line6pcm->index_out;
+	    index * LINE6_ISO_PACKETS * line6pcm->max_packet_size;
 	urb_out->transfer_buffer_length = urb_size;
 	urb_out->context = line6pcm;
 
-	if (++line6pcm->index_out == LINE6_ISO_BUFFERS)
-		line6pcm->index_out = 0;
-
 	if (test_bit(BIT_PCM_ALSA_PLAYBACK, &line6pcm->flags) &&
 	    !test_bit(BIT_PAUSE_PLAYBACK, &line6pcm->flags)) {
 		struct snd_pcm_runtime *runtime =
@@ -222,18 +220,10 @@ static int submit_audio_out_urb(struct snd_line6_pcm *line6pcm)
 			} else
 				dev_err(line6pcm->line6->ifcdev, "driver bug: len = %d\n", len);	/* this is somewhat paranoid */
 		} else {
-#if LINE6_REUSE_DMA_AREA_FOR_PLAYBACK
-			/* set the buffer pointer */
-			urb_out->transfer_buffer =
-			    runtime->dma_area +
-			    line6pcm->pos_out * bytes_per_frame;
-#else
-			/* copy data */
 			memcpy(urb_out->transfer_buffer,
 			       runtime->dma_area +
 			       line6pcm->pos_out * bytes_per_frame,
 			       urb_out->transfer_buffer_length);
-#endif
 		}
 
 		line6pcm->pos_out += urb_frames;
@@ -361,6 +351,31 @@ void line6_unlink_wait_clear_audio_out_urbs(struct snd_line6_pcm *line6pcm)
 	wait_clear_audio_out_urbs(line6pcm);
 }
 
+int line6_alloc_playback_buffer(struct snd_line6_pcm *line6pcm)
+{
+	/* We may be invoked multiple times in a row so allocate once only */
+	if (line6pcm->buffer_out)
+		return 0;
+
+	line6pcm->buffer_out =
+		kmalloc(LINE6_ISO_BUFFERS * LINE6_ISO_PACKETS *
+			line6pcm->max_packet_size, GFP_KERNEL);
+
+	if (!line6pcm->buffer_out) {
+		dev_err(line6pcm->line6->ifcdev,
+			"cannot malloc playback buffer\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void line6_free_playback_buffer(struct snd_line6_pcm *line6pcm)
+{
+	kfree(line6pcm->buffer_out);
+	line6pcm->buffer_out = NULL;
+}
+
 /*
 	Callback for completed playback URB.
 */
@@ -469,6 +484,13 @@ static int snd_line6_playback_hw_params(struct snd_pcm_substream *substream,
 	}
 	/* -- [FD] end */
 
+	if ((line6pcm->flags & MASK_PLAYBACK) == 0) {
+		ret = line6_alloc_playback_buffer(line6pcm);
+
+		if (ret < 0)
+			return ret;
+	}
+
 	ret = snd_pcm_lib_malloc_pages(substream,
 				       params_buffer_bytes(hw_params));
 	if (ret < 0)
@@ -481,6 +503,13 @@ static int snd_line6_playback_hw_params(struct snd_pcm_substream *substream,
 /* hw_free playback callback */
 static int snd_line6_playback_hw_free(struct snd_pcm_substream *substream)
 {
+	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
+	if ((line6pcm->flags & MASK_PLAYBACK) == 0) {
+		line6_unlink_wait_clear_audio_out_urbs(line6pcm);
+		line6_free_playback_buffer(line6pcm);
+	}
+
 	return snd_pcm_lib_free_pages(substream);
 }
 
diff --git a/drivers/staging/line6/playback.h b/drivers/staging/line6/playback.h
index f2fc8c0526e3..02487ff24538 100644
--- a/drivers/staging/line6/playback.h
+++ b/drivers/staging/line6/playback.h
@@ -29,7 +29,9 @@
 
 extern struct snd_pcm_ops snd_line6_playback_ops;
 
+extern int line6_alloc_playback_buffer(struct snd_line6_pcm *line6pcm);
 extern int line6_create_audio_out_urbs(struct snd_line6_pcm *line6pcm);
+extern void line6_free_playback_buffer(struct snd_line6_pcm *line6pcm);
 extern int line6_submit_audio_out_all_urbs(struct snd_line6_pcm *line6pcm);
 extern void line6_unlink_audio_out_urbs(struct snd_line6_pcm *line6pcm);
 extern void line6_unlink_wait_clear_audio_out_urbs(struct snd_line6_pcm
diff --git a/drivers/staging/line6/pod.c b/drivers/staging/line6/pod.c
index d9b30212585c..4dadc571d961 100644
--- a/drivers/staging/line6/pod.c
+++ b/drivers/staging/line6/pod.c
@@ -1149,14 +1149,10 @@ static struct snd_kcontrol_new pod_control_monitor = {
 static void pod_destruct(struct usb_interface *interface)
 {
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
-	struct usb_line6 *line6;
 
 	if (pod == NULL)
 		return;
-	line6 = &pod->line6;
-	if (line6 == NULL)
-		return;
-	line6_cleanup_audio(line6);
+	line6_cleanup_audio(&pod->line6);
 
 	del_timer(&pod->startup_timer);
 	cancel_work_sync(&pod->startup_work);
diff --git a/drivers/staging/line6/podhd.c b/drivers/staging/line6/podhd.c
new file mode 100644
index 000000000000..7ef45437b4f2
--- /dev/null
+++ b/drivers/staging/line6/podhd.c
@@ -0,0 +1,154 @@
+/*
+ * Line6 Pod HD
+ *
+ * Copyright (C) 2011 Stefan Hajnoczi <stefanha@gmail.com>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation, version 2.
+ *
+ */
+
+#include <sound/core.h>
+#include <sound/pcm.h>
+
+#include "audio.h"
+#include "driver.h"
+#include "pcm.h"
+#include "podhd.h"
+
+#define PODHD_BYTES_PER_FRAME 6	/* 24bit audio (stereo) */
+
+static struct snd_ratden podhd_ratden = {
+	.num_min = 48000,
+	.num_max = 48000,
+	.num_step = 1,
+	.den = 1,
+};
+
+static struct line6_pcm_properties podhd_pcm_properties = {
+	.snd_line6_playback_hw = {
+				  .info = (SNDRV_PCM_INFO_MMAP |
+					   SNDRV_PCM_INFO_INTERLEAVED |
+					   SNDRV_PCM_INFO_BLOCK_TRANSFER |
+					   SNDRV_PCM_INFO_MMAP_VALID |
+					   SNDRV_PCM_INFO_PAUSE |
+#ifdef CONFIG_PM
+					   SNDRV_PCM_INFO_RESUME |
+#endif
+					   SNDRV_PCM_INFO_SYNC_START),
+				  .formats = SNDRV_PCM_FMTBIT_S24_3LE,
+				  .rates = SNDRV_PCM_RATE_48000,
+				  .rate_min = 48000,
+				  .rate_max = 48000,
+				  .channels_min = 2,
+				  .channels_max = 2,
+				  .buffer_bytes_max = 60000,
+				  .period_bytes_min = 64,
+				  .period_bytes_max = 8192,
+				  .periods_min = 1,
+				  .periods_max = 1024},
+	.snd_line6_capture_hw = {
+				 .info = (SNDRV_PCM_INFO_MMAP |
+					  SNDRV_PCM_INFO_INTERLEAVED |
+					  SNDRV_PCM_INFO_BLOCK_TRANSFER |
+					  SNDRV_PCM_INFO_MMAP_VALID |
+#ifdef CONFIG_PM
+					  SNDRV_PCM_INFO_RESUME |
+#endif
+					  SNDRV_PCM_INFO_SYNC_START),
+				 .formats = SNDRV_PCM_FMTBIT_S24_3LE,
+				 .rates = SNDRV_PCM_RATE_48000,
+				 .rate_min = 48000,
+				 .rate_max = 48000,
+				 .channels_min = 2,
+				 .channels_max = 2,
+				 .buffer_bytes_max = 60000,
+				 .period_bytes_min = 64,
+				 .period_bytes_max = 8192,
+				 .periods_min = 1,
+				 .periods_max = 1024},
+	.snd_line6_rates = {
+			    .nrats = 1,
+			    .rats = &podhd_ratden},
+	.bytes_per_frame = PODHD_BYTES_PER_FRAME
+};
+
+/*
+	POD HD destructor.
+*/
+static void podhd_destruct(struct usb_interface *interface)
+{
+	struct usb_line6_podhd *podhd = usb_get_intfdata(interface);
+
+	if (podhd == NULL)
+		return;
+	line6_cleanup_audio(&podhd->line6);
+}
+
+/*
+	Try to init POD HD device.
+*/
+static int podhd_try_init(struct usb_interface *interface,
+			  struct usb_line6_podhd *podhd)
+{
+	int err;
+	struct usb_line6 *line6 = &podhd->line6;
+
+	if ((interface == NULL) || (podhd == NULL))
+		return -ENODEV;
+
+	/* initialize audio system: */
+	err = line6_init_audio(line6);
+	if (err < 0)
+		return err;
+
+	/* initialize MIDI subsystem: */
+	err = line6_init_midi(line6);
+	if (err < 0)
+		return err;
+
+	/* initialize PCM subsystem: */
+	err = line6_init_pcm(line6, &podhd_pcm_properties);
+	if (err < 0)
+		return err;
+
+	/* register USB audio system: */
+	err = line6_register_audio(line6);
+	return err;
+}
+
+/*
+	Init POD HD device (and clean up in case of failure).
+*/
+int line6_podhd_init(struct usb_interface *interface,
+		     struct usb_line6_podhd *podhd)
+{
+	int err = podhd_try_init(interface, podhd);
+
+	if (err < 0)
+		podhd_destruct(interface);
+
+	return err;
+}
+
+/*
+	POD HD device disconnected.
+*/
+void line6_podhd_disconnect(struct usb_interface *interface)
+{
+	struct usb_line6_podhd *podhd;
+
+	if (interface == NULL)
+		return;
+	podhd = usb_get_intfdata(interface);
+
+	if (podhd != NULL) {
+		struct snd_line6_pcm *line6pcm = podhd->line6.line6pcm;
+
+		if (line6pcm != NULL)
+			line6_pcm_disconnect(line6pcm);
+	}
+
+	podhd_destruct(interface);
+}
diff --git a/drivers/staging/line6/podhd.h b/drivers/staging/line6/podhd.h
new file mode 100644
index 000000000000..652f74056bb9
--- /dev/null
+++ b/drivers/staging/line6/podhd.h
@@ -0,0 +1,30 @@
+/*
+ * Line6 Pod HD
+ *
+ * Copyright (C) 2011 Stefan Hajnoczi <stefanha@gmail.com>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation, version 2.
+ *
+ */
+
+#ifndef PODHD_H
+#define PODHD_H
+
+#include <linux/usb.h>
+
+#include "driver.h"
+
+struct usb_line6_podhd {
+	/**
+		Generic Line6 USB data.
+	*/
+	struct usb_line6 line6;
+};
+
+extern void line6_podhd_disconnect(struct usb_interface *interface);
+extern int line6_podhd_init(struct usb_interface *interface,
+			    struct usb_line6_podhd *podhd);
+
+#endif /* PODHD_H */
diff --git a/drivers/staging/line6/revision.h b/drivers/staging/line6/revision.h
index 350d0dfff8f8..b4eee2b73831 100644
--- a/drivers/staging/line6/revision.h
+++ b/drivers/staging/line6/revision.h
@@ -1,4 +1,4 @@
 #ifndef DRIVER_REVISION
 /* current subversion revision */
-#define DRIVER_REVISION " (revision 690)"
+#define DRIVER_REVISION " (904)"
 #endif
diff --git a/drivers/staging/line6/toneport.c b/drivers/staging/line6/toneport.c
index 879e6992bbc6..f31057830dbc 100644
--- a/drivers/staging/line6/toneport.c
+++ b/drivers/staging/line6/toneport.c
@@ -295,14 +295,10 @@ static struct snd_kcontrol_new toneport_control_source = {
 static void toneport_destruct(struct usb_interface *interface)
 {
 	struct usb_line6_toneport *toneport = usb_get_intfdata(interface);
-	struct usb_line6 *line6;
 
 	if (toneport == NULL)
 		return;
-	line6 = &toneport->line6;
-	if (line6 == NULL)
-		return;
-	line6_cleanup_audio(line6);
+	line6_cleanup_audio(&toneport->line6);
 }
 
 /*
diff --git a/drivers/staging/line6/usbdefs.h b/drivers/staging/line6/usbdefs.h
index c6dffe6bc1a5..aff9e5caea46 100644
--- a/drivers/staging/line6/usbdefs.h
+++ b/drivers/staging/line6/usbdefs.h
@@ -24,6 +24,8 @@
 #define LINE6_DEVID_BASSPODXTPRO  0x4252
 #define LINE6_DEVID_GUITARPORT    0x4750
 #define LINE6_DEVID_POCKETPOD     0x5051
+#define LINE6_DEVID_PODHD300      0x5057
+#define LINE6_DEVID_PODHD500      0x414D
 #define LINE6_DEVID_PODSTUDIO_GX  0x4153
 #define LINE6_DEVID_PODSTUDIO_UX1 0x4150
 #define LINE6_DEVID_PODSTUDIO_UX2 0x4151
@@ -37,48 +39,71 @@
 #define LINE6_DEVID_TONEPORT_UX2  0x4142
 #define LINE6_DEVID_VARIAX        0x534d
 
-#define LINE6_BIT_BASSPODXT       (1 <<  0)
-#define LINE6_BIT_BASSPODXTLIVE   (1 <<  1)
-#define LINE6_BIT_BASSPODXTPRO    (1 <<  2)
-#define LINE6_BIT_GUITARPORT      (1 <<  3)
-#define LINE6_BIT_POCKETPOD       (1 <<  4)
-#define LINE6_BIT_PODSTUDIO_GX    (1 <<  5)
-#define LINE6_BIT_PODSTUDIO_UX1   (1 <<  6)
-#define LINE6_BIT_PODSTUDIO_UX2   (1 <<  7)
-#define LINE6_BIT_PODX3           (1 <<  8)
-#define LINE6_BIT_PODX3LIVE       (1 <<  9)
-#define LINE6_BIT_PODXT           (1 << 10)
-#define LINE6_BIT_PODXTLIVE       (1 << 11)
-#define LINE6_BIT_PODXTPRO        (1 << 12)
-#define LINE6_BIT_TONEPORT_GX     (1 << 13)
-#define LINE6_BIT_TONEPORT_UX1    (1 << 14)
-#define LINE6_BIT_TONEPORT_UX2    (1 << 15)
-#define LINE6_BIT_VARIAX          (1 << 16)
+enum {
+	LINE6_ID_BASSPODXT,
+	LINE6_ID_BASSPODXTLIVE,
+	LINE6_ID_BASSPODXTPRO,
+	LINE6_ID_GUITARPORT,
+	LINE6_ID_POCKETPOD,
+	LINE6_ID_PODHD300,
+	LINE6_ID_PODHD500,
+	LINE6_ID_PODSTUDIO_GX,
+	LINE6_ID_PODSTUDIO_UX1,
+	LINE6_ID_PODSTUDIO_UX2,
+	LINE6_ID_PODX3,
+	LINE6_ID_PODX3LIVE,
+	LINE6_ID_PODXT,
+	LINE6_ID_PODXTLIVE,
+	LINE6_ID_PODXTPRO,
+	LINE6_ID_TONEPORT_GX,
+	LINE6_ID_TONEPORT_UX1,
+	LINE6_ID_TONEPORT_UX2,
+	LINE6_ID_VARIAX
+};
 
-#define LINE6_BITS_PRO		(LINE6_BIT_BASSPODXTPRO | \
-				 LINE6_BIT_PODXTPRO)
-#define LINE6_BITS_LIVE		(LINE6_BIT_BASSPODXTLIVE | \
-				 LINE6_BIT_PODXTLIVE | \
-				 LINE6_BIT_PODX3LIVE)
-#define LINE6_BITS_PODXTALL	(LINE6_BIT_PODXT | \
-				 LINE6_BIT_PODXTLIVE | \
-				 LINE6_BIT_PODXTPRO)
-#define LINE6_BITS_BASSPODXTALL	(LINE6_BIT_BASSPODXT | \
-				 LINE6_BIT_BASSPODXTLIVE | \
-				 LINE6_BIT_BASSPODXTPRO)
+#define LINE6_BIT(x) LINE6_BIT_ ## x = 1 << LINE6_ID_ ## x
+
+enum {
+	LINE6_BIT(BASSPODXT),
+	LINE6_BIT(BASSPODXTLIVE),
+	LINE6_BIT(BASSPODXTPRO),
+	LINE6_BIT(GUITARPORT),
+	LINE6_BIT(POCKETPOD),
+	LINE6_BIT(PODHD300),
+	LINE6_BIT(PODHD500),
+	LINE6_BIT(PODSTUDIO_GX),
+	LINE6_BIT(PODSTUDIO_UX1),
+	LINE6_BIT(PODSTUDIO_UX2),
+	LINE6_BIT(PODX3),
+	LINE6_BIT(PODX3LIVE),
+	LINE6_BIT(PODXT),
+	LINE6_BIT(PODXTLIVE),
+	LINE6_BIT(PODXTPRO),
+	LINE6_BIT(TONEPORT_GX),
+	LINE6_BIT(TONEPORT_UX1),
+	LINE6_BIT(TONEPORT_UX2),
+	LINE6_BIT(VARIAX),
+
+	LINE6_BITS_PRO = LINE6_BIT_BASSPODXTPRO | LINE6_BIT_PODXTPRO,
+	LINE6_BITS_LIVE = LINE6_BIT_BASSPODXTLIVE | LINE6_BIT_PODXTLIVE | LINE6_BIT_PODX3LIVE,
+	LINE6_BITS_PODXTALL = LINE6_BIT_PODXT | LINE6_BIT_PODXTLIVE | LINE6_BIT_PODXTPRO,
+	LINE6_BITS_PODX3ALL = LINE6_BIT_PODX3 | LINE6_BIT_PODX3LIVE,
+	LINE6_BITS_PODHDALL = LINE6_BIT_PODHD300 | LINE6_BIT_PODHD500,
+	LINE6_BITS_BASSPODXTALL	= LINE6_BIT_BASSPODXT |	LINE6_BIT_BASSPODXTLIVE | LINE6_BIT_BASSPODXTPRO
+};
 
 /* device supports settings parameter via USB */
-#define LINE6_BIT_CONTROL	(1 << 0)
+#define LINE6_BIT_CONTROL (1 << 0)
 /* device supports PCM input/output via USB */
-#define LINE6_BIT_PCM		(1 << 1)
+#define LINE6_BIT_PCM (1 << 1)
 /* device support hardware monitoring */
-#define LINE6_BIT_HWMON		(1 << 2)
+#define LINE6_BIT_HWMON (1 << 2)
 
 #define LINE6_BIT_CONTROL_PCM_HWMON	(LINE6_BIT_CONTROL |	\
 					 LINE6_BIT_PCM |	\
 					 LINE6_BIT_HWMON)
 
-#define LINE6_FALLBACK_INTERVAL		10
-#define LINE6_FALLBACK_MAXPACKETSIZE	16
+#define LINE6_FALLBACK_INTERVAL 10
+#define LINE6_FALLBACK_MAXPACKETSIZE 16
 
 #endif
diff --git a/drivers/staging/line6/variax.c b/drivers/staging/line6/variax.c
index 81241cdf1be9..d36622228b2d 100644
--- a/drivers/staging/line6/variax.c
+++ b/drivers/staging/line6/variax.c
@@ -572,14 +572,10 @@ static DEVICE_ATTR(raw2, S_IWUSR, line6_nop_read, variax_set_raw2);
 static void variax_destruct(struct usb_interface *interface)
 {
 	struct usb_line6_variax *variax = usb_get_intfdata(interface);
-	struct usb_line6 *line6;
 
 	if (variax == NULL)
 		return;
-	line6 = &variax->line6;
-	if (line6 == NULL)
-		return;
-	line6_cleanup_audio(line6);
+	line6_cleanup_audio(&variax->line6);
 
 	del_timer(&variax->startup_timer1);
 	del_timer(&variax->startup_timer2);
diff --git a/drivers/staging/mei/init.c b/drivers/staging/mei/init.c
index 8bf34794489c..4ac3696883cb 100644
--- a/drivers/staging/mei/init.c
+++ b/drivers/staging/mei/init.c
@@ -38,7 +38,6 @@ void mei_io_list_init(struct mei_io_list *list)
 {
 	/* initialize our queue list */
 	INIT_LIST_HEAD(&list->mei_cb.cb_list);
-	list->status = 0;
 }
 
 /**
@@ -49,22 +48,15 @@ void mei_io_list_init(struct mei_io_list *list)
  */
 void mei_io_list_flush(struct mei_io_list *list, struct mei_cl *cl)
 {
-	struct mei_cl_cb *cb_pos = NULL;
-	struct mei_cl_cb *cb_next = NULL;
+	struct mei_cl_cb *pos;
+	struct mei_cl_cb *next;
 
-	if (list->status != 0)
-		return;
-
-	if (list_empty(&list->mei_cb.cb_list))
-		return;
-
-	list_for_each_entry_safe(cb_pos, cb_next,
-				 &list->mei_cb.cb_list, cb_list) {
-		if (cb_pos) {
+	list_for_each_entry_safe(pos, next, &list->mei_cb.cb_list, cb_list) {
+		if (pos->file_private) {
 			struct mei_cl *cl_tmp;
-			cl_tmp = (struct mei_cl *)cb_pos->file_private;
+			cl_tmp = (struct mei_cl *)pos->file_private;
 			if (mei_cl_cmp_id(cl, cl_tmp))
-				list_del(&cb_pos->cb_list);
+				list_del(&pos->cb_list);
 		}
 	}
 }
@@ -338,16 +330,10 @@ void mei_reset(struct mei_device *dev, int interrupts_enabled)
 		}
 	}
 	/* remove all waiting requests */
-	if (dev->write_list.status == 0 &&
-		!list_empty(&dev->write_list.mei_cb.cb_list)) {
-		list_for_each_entry_safe(cb_pos, cb_next,
-				&dev->write_list.mei_cb.cb_list, cb_list) {
-			if (cb_pos) {
-				list_del(&cb_pos->cb_list);
-				mei_free_cb_private(cb_pos);
-				cb_pos = NULL;
-			}
-		}
+	list_for_each_entry_safe(cb_pos, cb_next,
+			&dev->write_list.mei_cb.cb_list, cb_list) {
+		list_del(&cb_pos->cb_list);
+		mei_free_cb_private(cb_pos);
 	}
 }
 
@@ -380,8 +366,7 @@ void mei_host_start_message(struct mei_device *dev)
 	host_start_req->host_version.major_version = HBM_MAJOR_VERSION;
 	host_start_req->host_version.minor_version = HBM_MINOR_VERSION;
 	dev->recvd_msg = false;
-	if (!mei_write_message(dev, mei_hdr,
-				       (unsigned char *) (host_start_req),
+	if (!mei_write_message(dev, mei_hdr, (unsigned char *)host_start_req,
 				       mei_hdr->length)) {
 		dev_dbg(&dev->pdev->dev, "write send version message to FW fail.\n");
 		dev->mei_state = MEI_RESETING;
@@ -414,8 +399,7 @@ void mei_host_enum_clients_message(struct mei_device *dev)
 	host_enum_req = (struct hbm_host_enum_request *) &dev->wr_msg_buf[1];
 	memset(host_enum_req, 0, sizeof(struct hbm_host_enum_request));
 	host_enum_req->cmd.cmd = HOST_ENUM_REQ_CMD;
-	if (!mei_write_message(dev, mei_hdr,
-			       (unsigned char *) (host_enum_req),
+	if (!mei_write_message(dev, mei_hdr, (unsigned char *)host_enum_req,
 				mei_hdr->length)) {
 		dev->mei_state = MEI_RESETING;
 		dev_dbg(&dev->pdev->dev, "write send enumeration request message to FW fail.\n");
@@ -605,15 +589,10 @@ void mei_host_init_iamthif(struct mei_device *dev)
 		return;
 	}
 
-	/* Do not render the system unusable when iamthif_mtu is not equal to
-	the value received from ME.
-	Assign iamthif_mtu to the value received from ME in order to solve the
-	hardware macro incompatibility. */
+	/* Assign iamthif_mtu to the value received from ME  */
 
-	dev_dbg(&dev->pdev->dev, "[DEFAULT] IAMTHIF = %d\n", dev->iamthif_mtu);
 	dev->iamthif_mtu = dev->me_clients[i].props.max_msg_length;
-	dev_dbg(&dev->pdev->dev,
-			"IAMTHIF = %d\n",
+	dev_dbg(&dev->pdev->dev, "IAMTHIF_MTU = %d\n",
 			dev->me_clients[i].props.max_msg_length);
 
 	kfree(dev->iamthif_msg_buf);
diff --git a/drivers/staging/mei/interface.c b/drivers/staging/mei/interface.c
index a65dacf00213..eb5df7fc2269 100644
--- a/drivers/staging/mei/interface.c
+++ b/drivers/staging/mei/interface.c
@@ -128,9 +128,9 @@ int mei_count_empty_write_slots(struct mei_device *dev)
  * returns 1 if success, 0 - otherwise.
  */
 int mei_write_message(struct mei_device *dev,
-			     struct mei_msg_hdr *header,
-			     unsigned char *write_buffer,
-			     unsigned long write_length)
+		      struct mei_msg_hdr *header,
+		      unsigned char *write_buffer,
+		      unsigned long write_length)
 {
 	u32 temp_msg = 0;
 	unsigned long bytes_written = 0;
@@ -216,7 +216,7 @@ int mei_count_full_read_slots(struct mei_device *dev)
  * @buffer_length: message size will be read
  */
 void mei_read_slots(struct mei_device *dev,
-		     unsigned char *buffer, unsigned long buffer_length)
+		    unsigned char *buffer, unsigned long buffer_length)
 {
 	u32 i = 0;
 	unsigned char temp_buf[sizeof(u32)];
diff --git a/drivers/staging/mei/interface.h b/drivers/staging/mei/interface.h
index 7bd38ae2c233..aeae511419c7 100644
--- a/drivers/staging/mei/interface.h
+++ b/drivers/staging/mei/interface.h
@@ -52,6 +52,17 @@ int mei_wd_send(struct mei_device *dev);
 int mei_wd_stop(struct mei_device *dev, bool preserve);
 bool mei_wd_host_init(struct mei_device *dev);
 void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout);
+/*
+ * mei_watchdog_register  - Registering watchdog interface
+ *   once we got connection to the WD Client
+ * @dev - mei device
+ */
+void mei_watchdog_register(struct mei_device *dev);
+/*
+ * mei_watchdog_unregister  - Uegistering watchdog interface
+ * @dev - mei device
+ */
+void mei_watchdog_unregister(struct mei_device *dev);
 
 int mei_flow_ctrl_reduce(struct mei_device *dev, struct mei_cl *cl);
 
diff --git a/drivers/staging/mei/interrupt.c b/drivers/staging/mei/interrupt.c
index 882d106d54e5..3544fee34e48 100644
--- a/drivers/staging/mei/interrupt.c
+++ b/drivers/staging/mei/interrupt.c
@@ -198,8 +198,7 @@ static int mei_irq_thread_read_client_message(struct mei_io_list *complete_list,
 	unsigned char *buffer = NULL;
 
 	dev_dbg(&dev->pdev->dev, "start client msg\n");
-	if (!(dev->read_list.status == 0 &&
-	      !list_empty(&dev->read_list.mei_cb.cb_list)))
+	if (list_empty(&dev->read_list.mei_cb.cb_list))
 		goto quit;
 
 	list_for_each_entry_safe(cb_pos, cb_next,
@@ -210,9 +209,6 @@ static int mei_irq_thread_read_client_message(struct mei_io_list *complete_list,
 			buffer = (unsigned char *)
 				(cb_pos->response_buffer.data +
 				cb_pos->information);
-			BUG_ON(cb_pos->response_buffer.size <
-					mei_hdr->length +
-					cb_pos->information);
 
 			if (cb_pos->response_buffer.size <
 					mei_hdr->length + cb_pos->information) {
@@ -390,24 +386,10 @@ static void mei_client_connect_response(struct mei_device *dev,
 	/* if WD or iamthif client treat specially */
 
 	if (is_treat_specially_client(&(dev->wd_cl), rs)) {
-		dev_dbg(&dev->pdev->dev, "dev->wd_timeout =%d.\n",
-				dev->wd_timeout);
-
-		dev->wd_due_counter = (dev->wd_timeout) ? 1 : 0;
-
 		dev_dbg(&dev->pdev->dev, "successfully connected to WD client.\n");
+		mei_watchdog_register(dev);
 
-		/* Registering watchdog interface device once we got connection
-		   to the WD Client
-		*/
-		if (watchdog_register_device(&amt_wd_dev)) {
-			printk(KERN_ERR "mei: unable to register watchdog device.\n");
-			dev->wd_interface_reg = false;
-		} else {
-			dev_dbg(&dev->pdev->dev, "successfully register watchdog interface.\n");
-			dev->wd_interface_reg = true;
-		}
-
+		/* next step in the state maching */
 		mei_host_init_iamthif(dev);
 		return;
 	}
@@ -416,22 +398,20 @@ static void mei_client_connect_response(struct mei_device *dev,
 		dev->iamthif_state = MEI_IAMTHIF_IDLE;
 		return;
 	}
-	if (!dev->ctrl_rd_list.status &&
-	    !list_empty(&dev->ctrl_rd_list.mei_cb.cb_list)) {
-		list_for_each_entry_safe(cb_pos, cb_next,
-			&dev->ctrl_rd_list.mei_cb.cb_list, cb_list) {
-			cl = (struct mei_cl *)cb_pos->file_private;
-			if (!cl) {
+	list_for_each_entry_safe(cb_pos, cb_next,
+				&dev->ctrl_rd_list.mei_cb.cb_list, cb_list) {
+
+		cl = (struct mei_cl *)cb_pos->file_private;
+		if (!cl) {
+			list_del(&cb_pos->cb_list);
+			return;
+		}
+		if (MEI_IOCTL == cb_pos->major_file_operations) {
+			if (is_treat_specially_client(cl, rs)) {
 				list_del(&cb_pos->cb_list);
-				return;
-			}
-			if (MEI_IOCTL == cb_pos->major_file_operations) {
-				if (is_treat_specially_client(cl, rs)) {
-					list_del(&cb_pos->cb_list);
-					cl->status = 0;
-					cl->timer_count = 0;
-					break;
-				}
+				cl->status = 0;
+				cl->timer_count = 0;
+				break;
 			}
 		}
 	}
@@ -458,29 +438,26 @@ static void mei_client_disconnect_response(struct mei_device *dev,
 			rs->host_addr,
 			rs->status);
 
-	if (!dev->ctrl_rd_list.status &&
-	    !list_empty(&dev->ctrl_rd_list.mei_cb.cb_list)) {
-		list_for_each_entry_safe(cb_pos, cb_next,
-				&dev->ctrl_rd_list.mei_cb.cb_list, cb_list) {
-			cl = (struct mei_cl *)cb_pos->file_private;
+	list_for_each_entry_safe(cb_pos, cb_next,
+			&dev->ctrl_rd_list.mei_cb.cb_list, cb_list) {
+		cl = (struct mei_cl *)cb_pos->file_private;
 
-			if (!cl) {
-				list_del(&cb_pos->cb_list);
-				return;
-			}
+		if (!cl) {
+			list_del(&cb_pos->cb_list);
+			return;
+		}
 
-			dev_dbg(&dev->pdev->dev, "list_for_each_entry_safe in ctrl_rd_list.\n");
-			if (cl->host_client_id == rs->host_addr &&
-			    cl->me_client_id == rs->me_addr) {
+		dev_dbg(&dev->pdev->dev, "list_for_each_entry_safe in ctrl_rd_list.\n");
+		if (cl->host_client_id == rs->host_addr &&
+		    cl->me_client_id == rs->me_addr) {
 
-				list_del(&cb_pos->cb_list);
-				if (!rs->status)
-					cl->state = MEI_FILE_DISCONNECTED;
+			list_del(&cb_pos->cb_list);
+			if (!rs->status)
+				cl->state = MEI_FILE_DISCONNECTED;
 
-				cl->status = 0;
-				cl->timer_count = 0;
-				break;
-			}
+			cl->status = 0;
+			cl->timer_count = 0;
+			break;
 		}
 	}
 }
@@ -718,7 +695,7 @@ static void mei_irq_thread_read_bus_message(struct mei_device *dev,
 	case CLIENT_DISCONNECT_RES_CMD:
 		disconnect_res =
 			(struct hbm_client_connect_response *) mei_msg;
-		mei_client_disconnect_response(dev,	 disconnect_res);
+		mei_client_disconnect_response(dev, disconnect_res);
 		dev_dbg(&dev->pdev->dev, "client disconnect response message received.\n");
 		wake_up(&dev->wait_recvd_msg);
 		break;
@@ -736,7 +713,7 @@ static void mei_irq_thread_read_bus_message(struct mei_device *dev,
 			mei_reset(dev, 1);
 			return;
 		}
-	       if (dev->me_clients[dev->me_client_presentation_num]
+		if (dev->me_clients[dev->me_client_presentation_num]
 					.client_id == props_res->address) {
 
 			dev->me_clients[dev->me_client_presentation_num].props
@@ -1228,7 +1205,7 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 {
 
 	struct mei_cl *cl;
-	struct mei_cl_cb *cb_pos = NULL, *cb_next = NULL;
+	struct mei_cl_cb *pos = NULL, *next = NULL;
 	struct mei_io_list *list;
 	int ret;
 
@@ -1241,36 +1218,31 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 	dev_dbg(&dev->pdev->dev, "complete all waiting for write cb.\n");
 
 	list = &dev->write_waiting_list;
-	if (!list->status && !list_empty(&list->mei_cb.cb_list)) {
-		list_for_each_entry_safe(cb_pos, cb_next,
-				&list->mei_cb.cb_list, cb_list) {
-			cl = (struct mei_cl *)cb_pos->file_private;
-			if (cl) {
-				cl->status = 0;
-				list_del(&cb_pos->cb_list);
-				if (MEI_WRITING == cl->writing_state &&
-				   (cb_pos->major_file_operations ==
-						MEI_WRITE) &&
-				   (cl != &dev->iamthif_cl)) {
-					dev_dbg(&dev->pdev->dev,
-						"MEI WRITE COMPLETE\n");
-					cl->writing_state =
-							MEI_WRITE_COMPLETE;
-					list_add_tail(&cb_pos->cb_list,
-						&cmpl_list->mei_cb.cb_list);
-				}
-				if (cl == &dev->iamthif_cl) {
-					dev_dbg(&dev->pdev->dev, "check iamthif flow control.\n");
-					if (dev->iamthif_flow_control_pending) {
-						ret =
-						_mei_irq_thread_iamthif_read(
-								dev, slots);
-						if (ret)
-							return ret;
-					}
-				}
+	list_for_each_entry_safe(pos, next,
+			&list->mei_cb.cb_list, cb_list) {
+		cl = (struct mei_cl *)pos->file_private;
+		if (cl == NULL)
+			continue;
+
+		cl->status = 0;
+		list_del(&pos->cb_list);
+		if (MEI_WRITING == cl->writing_state &&
+		   (pos->major_file_operations == MEI_WRITE) &&
+		   (cl != &dev->iamthif_cl)) {
+			dev_dbg(&dev->pdev->dev,
+				"MEI WRITE COMPLETE\n");
+			cl->writing_state = MEI_WRITE_COMPLETE;
+			list_add_tail(&pos->cb_list,
+				&cmpl_list->mei_cb.cb_list);
+		}
+		if (cl == &dev->iamthif_cl) {
+			dev_dbg(&dev->pdev->dev, "check iamthif flow control.\n");
+			if (dev->iamthif_flow_control_pending) {
+				ret = _mei_irq_thread_iamthif_read(
+						dev, slots);
+				if (ret)
+					return ret;
 			}
-
 		}
 	}
 
@@ -1317,101 +1289,88 @@ static int mei_irq_thread_write_handler(struct mei_io_list *cmpl_list,
 		return ~ENODEV;
 
 	/* complete control write list CB */
-	if (!dev->ctrl_wr_list.status) {
-		/* complete control write list CB */
-		dev_dbg(&dev->pdev->dev, "complete control write list cb.\n");
-		list_for_each_entry_safe(cb_pos, cb_next,
+	dev_dbg(&dev->pdev->dev, "complete control write list cb.\n");
+	list_for_each_entry_safe(pos, next,
 				&dev->ctrl_wr_list.mei_cb.cb_list, cb_list) {
-			cl = (struct mei_cl *)
-				cb_pos->file_private;
-			if (!cl) {
-				list_del(&cb_pos->cb_list);
-				return -ENODEV;
-			}
-			switch (cb_pos->major_file_operations) {
-			case MEI_CLOSE:
-				/* send disconnect message */
-				ret = _mei_irq_thread_close(dev, slots,
-						     cb_pos, cl, cmpl_list);
-				if (ret)
-					return ret;
-
-				break;
-			case MEI_READ:
-				/* send flow control message */
-				ret = _mei_irq_thread_read(dev, slots,
-						    cb_pos, cl, cmpl_list);
-				if (ret)
-					return ret;
+		cl = (struct mei_cl *) pos->file_private;
+		if (!cl) {
+			list_del(&pos->cb_list);
+			return -ENODEV;
+		}
+		switch (pos->major_file_operations) {
+		case MEI_CLOSE:
+			/* send disconnect message */
+			ret = _mei_irq_thread_close(dev, slots, pos, cl, cmpl_list);
+			if (ret)
+				return ret;
 
-				break;
-			case MEI_IOCTL:
-				/* connect message */
-				if (!mei_other_client_is_connecting(dev,
-						cl))
-					continue;
-				ret = _mei_irq_thread_ioctl(dev, slots,
-						     cb_pos, cl, cmpl_list);
-				if (ret)
-					return ret;
+			break;
+		case MEI_READ:
+			/* send flow control message */
+			ret = _mei_irq_thread_read(dev, slots, pos, cl, cmpl_list);
+			if (ret)
+				return ret;
 
-				break;
+			break;
+		case MEI_IOCTL:
+			/* connect message */
+			if (mei_other_client_is_connecting(dev, cl))
+				continue;
+			ret = _mei_irq_thread_ioctl(dev, slots, pos, cl, cmpl_list);
+			if (ret)
+				return ret;
 
-			default:
-				BUG();
-			}
+			break;
 
+		default:
+			BUG();
 		}
+
 	}
 	/* complete  write list CB */
-	if (!dev->write_list.status &&
-	    !list_empty(&dev->write_list.mei_cb.cb_list)) {
-		dev_dbg(&dev->pdev->dev, "complete write list cb.\n");
-		list_for_each_entry_safe(cb_pos, cb_next,
-				&dev->write_list.mei_cb.cb_list, cb_list) {
-			cl = (struct mei_cl *)cb_pos->file_private;
-
-			if (cl) {
-				if (cl != &dev->iamthif_cl) {
-					if (!mei_flow_ctrl_creds(dev,
-						cl)) {
-						dev_dbg(&dev->pdev->dev,
-							"No flow control"
-						    " credentials for client"
-						    " %d, not sending.\n",
-						    cl->host_client_id);
-						continue;
-					}
-					ret = _mei_irq_thread_cmpl(dev, slots,
-							    cb_pos,
-							    cl, cmpl_list);
-					if (ret)
-						return ret;
-
-				} else if (cl == &dev->iamthif_cl) {
-					/* IAMTHIF IOCTL */
-					dev_dbg(&dev->pdev->dev, "complete amthi write cb.\n");
-					if (!mei_flow_ctrl_creds(dev,
-							cl)) {
-						dev_dbg(&dev->pdev->dev,
-							"No flow control"
-						    " credentials for amthi"
-						    " client %d.\n",
-						    cl->host_client_id);
-						continue;
-					}
-					ret = _mei_irq_thread_cmpl_iamthif(dev,
-								slots,
-								cb_pos,
-								cl,
-								cmpl_list);
-					if (ret)
-						return ret;
-
-				}
+	dev_dbg(&dev->pdev->dev, "complete write list cb.\n");
+	list_for_each_entry_safe(pos, next,
+			&dev->write_list.mei_cb.cb_list, cb_list) {
+		cl = (struct mei_cl *)pos->file_private;
+		if (cl == NULL)
+			continue;
+
+		if (cl != &dev->iamthif_cl) {
+			if (!mei_flow_ctrl_creds(dev, cl)) {
+				dev_dbg(&dev->pdev->dev,
+					"No flow control"
+				    " credentials for client"
+				    " %d, not sending.\n",
+				    cl->host_client_id);
+				continue;
+			}
+			ret = _mei_irq_thread_cmpl(dev, slots,
+					    pos,
+					    cl, cmpl_list);
+			if (ret)
+				return ret;
+
+		} else if (cl == &dev->iamthif_cl) {
+			/* IAMTHIF IOCTL */
+			dev_dbg(&dev->pdev->dev, "complete amthi write cb.\n");
+			if (!mei_flow_ctrl_creds(dev, cl)) {
+				dev_dbg(&dev->pdev->dev,
+					"No flow control"
+				    " credentials for amthi"
+				    " client %d.\n",
+				    cl->host_client_id);
+				continue;
 			}
+			ret = _mei_irq_thread_cmpl_iamthif(dev,
+						slots,
+						pos,
+						cl,
+						cmpl_list);
+			if (ret)
+				return ret;
 
 		}
+
 	}
 	return 0;
 }
@@ -1502,18 +1461,13 @@ void mei_timer(struct work_struct *work)
 			amthi_complete_list = &dev->amthi_read_complete_list.
 					mei_cb.cb_list;
 
-			if (!list_empty(amthi_complete_list)) {
-
-				list_for_each_entry_safe(cb_pos, cb_next,
-							amthi_complete_list,
-							cb_list) {
+			list_for_each_entry_safe(cb_pos, cb_next, amthi_complete_list, cb_list) {
 
-					cl_pos = cb_pos->file_object->private_data;
+				cl_pos = cb_pos->file_object->private_data;
 
-					/* Finding the AMTHI entry. */
-					if (cl_pos ==	&dev->iamthif_cl)
-						list_del(&cb_pos->cb_list);
-				}
+				/* Finding the AMTHI entry. */
+				if (cl_pos == &dev->iamthif_cl)
+					list_del(&cb_pos->cb_list);
 			}
 			if (dev->iamthif_current_cb)
 				mei_free_cb_private(dev->iamthif_current_cb);
@@ -1527,8 +1481,8 @@ void mei_timer(struct work_struct *work)
 		}
 	}
 out:
-	 schedule_delayed_work(&dev->timer_work, 2 * HZ);
-	 mutex_unlock(&dev->device_lock);
+	schedule_delayed_work(&dev->timer_work, 2 * HZ);
+	mutex_unlock(&dev->device_lock);
 }
 
 /**
@@ -1624,7 +1578,7 @@ end:
 		wake_up_interruptible(&dev->wait_recvd_msg);
 		bus_message_received = false;
 	}
-	if (complete_list.status || list_empty(&complete_list.mei_cb.cb_list))
+	if (list_empty(&complete_list.mei_cb.cb_list))
 		return IRQ_HANDLED;
 
 
diff --git a/drivers/staging/mei/iorw.c b/drivers/staging/mei/iorw.c
index 8a61d1266515..0752ead4269a 100644
--- a/drivers/staging/mei/iorw.c
+++ b/drivers/staging/mei/iorw.c
@@ -228,18 +228,15 @@ struct mei_cl_cb *find_amthi_read_list_entry(
 		struct file *file)
 {
 	struct mei_cl *cl_temp;
-	struct mei_cl_cb *cb_pos = NULL;
-	struct mei_cl_cb *cb_next = NULL;
-
-	if (!dev->amthi_read_complete_list.status &&
-	    !list_empty(&dev->amthi_read_complete_list.mei_cb.cb_list)) {
-		list_for_each_entry_safe(cb_pos, cb_next,
-		    &dev->amthi_read_complete_list.mei_cb.cb_list, cb_list) {
-			cl_temp = (struct mei_cl *)cb_pos->file_private;
-			if (cl_temp && cl_temp == &dev->iamthif_cl &&
-				cb_pos->file_object == file)
-				return cb_pos;
-		}
+	struct mei_cl_cb *pos = NULL;
+	struct mei_cl_cb *next = NULL;
+
+	list_for_each_entry_safe(pos, next,
+	    &dev->amthi_read_complete_list.mei_cb.cb_list, cb_list) {
+		cl_temp = (struct mei_cl *)pos->file_private;
+		if (cl_temp && cl_temp == &dev->iamthif_cl &&
+			pos->file_object == file)
+			return pos;
 	}
 	return NULL;
 }
@@ -262,7 +259,7 @@ struct mei_cl_cb *find_amthi_read_list_entry(
  *  negative on failure.
  */
 int amthi_read(struct mei_device *dev, struct file *file,
-	      char __user *ubuf, size_t length, loff_t *offset)
+	       char __user *ubuf, size_t length, loff_t *offset)
 {
 	int rets;
 	int wait_ret;
@@ -334,8 +331,7 @@ int amthi_read(struct mei_device *dev, struct file *file,
 		}
 	}
 	/* if the whole message will fit remove it from the list */
-	if (cb->information >= *offset &&
-	    length >= (cb->information - *offset))
+	if (cb->information >= *offset && length >= (cb->information - *offset))
 		list_del(&cb->cb_list);
 	else if (cb->information > 0 && cb->information <= *offset) {
 		/* end of the message has been reached */
@@ -356,9 +352,7 @@ int amthi_read(struct mei_device *dev, struct file *file,
 	 * the information may be longer */
 	length = min_t(size_t, length, (cb->information - *offset));
 
-	if (copy_to_user(ubuf,
-			 cb->response_buffer.data + *offset,
-			 length))
+	if (copy_to_user(ubuf, cb->response_buffer.data + *offset, length))
 		rets = -EFAULT;
 	else {
 		rets = length;
@@ -427,7 +421,7 @@ int mei_start_read(struct mei_device *dev, struct mei_cl *cl)
 
 	cb->response_buffer.size = dev->me_clients[i].props.max_msg_length;
 	cb->response_buffer.data =
-	    kmalloc(cb->response_buffer.size, GFP_KERNEL);
+			kmalloc(cb->response_buffer.size, GFP_KERNEL);
 	if (!cb->response_buffer.data) {
 		rets = -ENOMEM;
 		goto unlock;
@@ -448,8 +442,7 @@ int mei_start_read(struct mei_device *dev, struct mei_cl *cl)
 				      &dev->read_list.mei_cb.cb_list);
 		}
 	} else {
-		list_add_tail(&cb->cb_list,
-			      &dev->ctrl_wr_list.mei_cb.cb_list);
+		list_add_tail(&cb->cb_list, &dev->ctrl_wr_list.mei_cb.cb_list);
 	}
 	return rets;
 unlock:
@@ -482,7 +475,7 @@ int amthi_write(struct mei_device *dev, struct mei_cl_cb *cb)
 	dev->iamthif_ioctl = true;
 	dev->iamthif_msg_buf_size = cb->request_buffer.size;
 	memcpy(dev->iamthif_msg_buf, cb->request_buffer.data,
-	    cb->request_buffer.size);
+	       cb->request_buffer.size);
 
 	ret = mei_flow_ctrl_creds(dev, &dev->iamthif_cl);
 	if (ret < 0)
@@ -534,8 +527,7 @@ int amthi_write(struct mei_device *dev, struct mei_cl_cb *cb)
 
 		dev_dbg(&dev->pdev->dev, "No flow control credentials, "
 				"so add iamthif cb to write list.\n");
-		list_add_tail(&cb->cb_list,
-			      &dev->write_list.mei_cb.cb_list);
+		list_add_tail(&cb->cb_list, &dev->write_list.mei_cb.cb_list);
 	}
 	return 0;
 }
@@ -550,8 +542,8 @@ int amthi_write(struct mei_device *dev, struct mei_cl_cb *cb)
 void mei_run_next_iamthif_cmd(struct mei_device *dev)
 {
 	struct mei_cl *cl_tmp;
-	struct mei_cl_cb *cb_pos = NULL;
-	struct mei_cl_cb *cb_next = NULL;
+	struct mei_cl_cb *pos = NULL;
+	struct mei_cl_cb *next = NULL;
 	int status;
 
 	if (!dev)
@@ -565,25 +557,22 @@ void mei_run_next_iamthif_cmd(struct mei_device *dev)
 	dev->iamthif_timer = 0;
 	dev->iamthif_file_object = NULL;
 
-	if (dev->amthi_cmd_list.status == 0 &&
-	    !list_empty(&dev->amthi_cmd_list.mei_cb.cb_list)) {
-		dev_dbg(&dev->pdev->dev, "complete amthi cmd_list cb.\n");
-
-		list_for_each_entry_safe(cb_pos, cb_next,
-		    &dev->amthi_cmd_list.mei_cb.cb_list, cb_list) {
-			list_del(&cb_pos->cb_list);
-			cl_tmp = (struct mei_cl *)cb_pos->file_private;
-
-			if (cl_tmp && cl_tmp == &dev->iamthif_cl) {
-				status = amthi_write(dev, cb_pos);
-				if (status) {
-					dev_dbg(&dev->pdev->dev,
-						"amthi write failed status = %d\n",
-							status);
-					return;
-				}
-				break;
+	dev_dbg(&dev->pdev->dev, "complete amthi cmd_list cb.\n");
+
+	list_for_each_entry_safe(pos, next,
+			&dev->amthi_cmd_list.mei_cb.cb_list, cb_list) {
+		list_del(&pos->cb_list);
+		cl_tmp = (struct mei_cl *)pos->file_private;
+
+		if (cl_tmp && cl_tmp == &dev->iamthif_cl) {
+			status = amthi_write(dev, pos);
+			if (status) {
+				dev_dbg(&dev->pdev->dev,
+					"amthi write failed status = %d\n",
+						status);
+				return;
 			}
+			break;
 		}
 	}
 }
diff --git a/drivers/staging/mei/main.c b/drivers/staging/mei/main.c
index eb05c36f45d4..1e1a9f996e7c 100644
--- a/drivers/staging/mei/main.c
+++ b/drivers/staging/mei/main.c
@@ -33,6 +33,7 @@
 #include <linux/compat.h>
 #include <linux/jiffies.h>
 #include <linux/interrupt.h>
+#include <linux/miscdevice.h>
 
 #include "mei_dev.h"
 #include "mei.h"
@@ -51,18 +52,10 @@ static char mei_driver_name[] = MEI_DRIVER_NAME;
 static const char mei_driver_string[] = "Intel(R) Management Engine Interface";
 static const char mei_driver_version[] = MEI_DRIVER_VERSION;
 
-/* mei char device for registration */
-static struct cdev mei_cdev;
-
-/* major number for device */
-static int mei_major;
 /* The device pointer */
 /* Currently this driver works as long as there is only a single AMT device. */
 struct pci_dev *mei_device;
 
-static struct class *mei_class;
-
-
 /* mei_pci_tbl - PCI Device ID Table */
 static DEFINE_PCI_DEVICE_TABLE(mei_pci_tbl) = {
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82946GZ)},
@@ -105,173 +98,6 @@ MODULE_DEVICE_TABLE(pci, mei_pci_tbl);
 
 static DEFINE_MUTEX(mei_mutex);
 
-/**
- * mei_probe - Device Initialization Routine
- *
- * @pdev: PCI device structure
- * @ent: entry in kcs_pci_tbl
- *
- * returns 0 on success, <0 on failure.
- */
-static int __devinit mei_probe(struct pci_dev *pdev,
-				const struct pci_device_id *ent)
-{
-	struct mei_device *dev;
-	int err;
-
-	mutex_lock(&mei_mutex);
-	if (mei_device) {
-		err = -EEXIST;
-		goto end;
-	}
-	/* enable pci dev */
-	err = pci_enable_device(pdev);
-	if (err) {
-		printk(KERN_ERR "mei: Failed to enable pci device.\n");
-		goto end;
-	}
-	/* set PCI host mastering  */
-	pci_set_master(pdev);
-	/* pci request regions for mei driver */
-	err = pci_request_regions(pdev, mei_driver_name);
-	if (err) {
-		printk(KERN_ERR "mei: Failed to get pci regions.\n");
-		goto disable_device;
-	}
-	/* allocates and initializes the mei dev structure */
-	dev = mei_device_init(pdev);
-	if (!dev) {
-		err = -ENOMEM;
-		goto release_regions;
-	}
-	/* mapping  IO device memory */
-	dev->mem_addr = pci_iomap(pdev, 0, 0);
-	if (!dev->mem_addr) {
-		printk(KERN_ERR "mei: mapping I/O device memory failure.\n");
-		err = -ENOMEM;
-		goto free_device;
-	}
-	pci_enable_msi(pdev);
-
-	 /* request and enable interrupt */
-	if (pci_dev_msi_enabled(pdev))
-		err = request_threaded_irq(pdev->irq,
-			NULL,
-			mei_interrupt_thread_handler,
-			0, mei_driver_name, dev);
-	else
-		err = request_threaded_irq(pdev->irq,
-			mei_interrupt_quick_handler,
-			mei_interrupt_thread_handler,
-			IRQF_SHARED, mei_driver_name, dev);
-
-	if (err) {
-		printk(KERN_ERR "mei: request_threaded_irq failure. irq = %d\n",
-		       pdev->irq);
-		goto unmap_memory;
-	}
-	INIT_DELAYED_WORK(&dev->timer_work, mei_timer);
-	if (mei_hw_init(dev)) {
-		printk(KERN_ERR "mei: Init hw failure.\n");
-		err = -ENODEV;
-		goto release_irq;
-	}
-	mei_device = pdev;
-	pci_set_drvdata(pdev, dev);
-	schedule_delayed_work(&dev->timer_work, HZ);
-
-	mutex_unlock(&mei_mutex);
-
-	pr_debug("mei: Driver initialization successful.\n");
-
-	return 0;
-
-release_irq:
-	/* disable interrupts */
-	dev->host_hw_state = mei_hcsr_read(dev);
-	mei_disable_interrupts(dev);
-	flush_scheduled_work();
-	free_irq(pdev->irq, dev);
-	pci_disable_msi(pdev);
-unmap_memory:
-	pci_iounmap(pdev, dev->mem_addr);
-free_device:
-	kfree(dev);
-release_regions:
-	pci_release_regions(pdev);
-disable_device:
-	pci_disable_device(pdev);
-end:
-	mutex_unlock(&mei_mutex);
-	printk(KERN_ERR "mei: Driver initialization failed.\n");
-	return err;
-}
-
-/**
- * mei_remove - Device Removal Routine
- *
- * @pdev: PCI device structure
- *
- * mei_remove is called by the PCI subsystem to alert the driver
- * that it should release a PCI device.
- */
-static void __devexit mei_remove(struct pci_dev *pdev)
-{
-	struct mei_device *dev;
-
-	if (mei_device != pdev)
-		return;
-
-	dev = pci_get_drvdata(pdev);
-	if (!dev)
-		return;
-
-	mutex_lock(&dev->device_lock);
-
-	mei_wd_stop(dev, false);
-
-	mei_device = NULL;
-
-	if (dev->iamthif_cl.state == MEI_FILE_CONNECTED) {
-		dev->iamthif_cl.state = MEI_FILE_DISCONNECTING;
-		mei_disconnect_host_client(dev, &dev->iamthif_cl);
-	}
-	if (dev->wd_cl.state == MEI_FILE_CONNECTED) {
-		dev->wd_cl.state = MEI_FILE_DISCONNECTING;
-		mei_disconnect_host_client(dev, &dev->wd_cl);
-	}
-
-	/* Unregistering watchdog device */
-	if (dev->wd_interface_reg)
-		watchdog_unregister_device(&amt_wd_dev);
-
-	/* remove entry if already in list */
-	dev_dbg(&pdev->dev, "list del iamthif and wd file list.\n");
-	mei_remove_client_from_file_list(dev, dev->wd_cl.host_client_id);
-	mei_remove_client_from_file_list(dev, dev->iamthif_cl.host_client_id);
-
-	dev->iamthif_current_cb = NULL;
-	dev->me_clients_num = 0;
-
-	mutex_unlock(&dev->device_lock);
-
-	flush_scheduled_work();
-
-	/* disable interrupts */
-	mei_disable_interrupts(dev);
-
-	free_irq(pdev->irq, dev);
-	pci_disable_msi(pdev);
-	pci_set_drvdata(pdev, NULL);
-
-	if (dev->mem_addr)
-		pci_iounmap(pdev, dev->mem_addr);
-
-	kfree(dev);
-
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-}
 
 /**
  * mei_clear_list - removes all callbacks associated with file
@@ -372,21 +198,17 @@ static struct mei_cl_cb *find_read_list_entry(
 		struct mei_device *dev,
 		struct mei_cl *cl)
 {
-	struct mei_cl_cb *cb_pos = NULL;
-	struct mei_cl_cb *cb_next = NULL;
-
-	if (!dev->read_list.status &&
-	    !list_empty(&dev->read_list.mei_cb.cb_list)) {
+	struct mei_cl_cb *pos = NULL;
+	struct mei_cl_cb *next = NULL;
 
-		dev_dbg(&dev->pdev->dev, "remove read_list CB\n");
-		list_for_each_entry_safe(cb_pos, cb_next,
-				&dev->read_list.mei_cb.cb_list, cb_list) {
-			struct mei_cl *cl_temp;
-			cl_temp = (struct mei_cl *)cb_pos->file_private;
+	dev_dbg(&dev->pdev->dev, "remove read_list CB\n");
+	list_for_each_entry_safe(pos, next,
+			&dev->read_list.mei_cb.cb_list, cb_list) {
+		struct mei_cl *cl_temp;
+		cl_temp = (struct mei_cl *)pos->file_private;
 
-			if (mei_cl_cmp_id(cl, cl_temp))
-				return cb_pos;
-		}
+		if (mei_cl_cmp_id(cl, cl_temp))
+			return pos;
 	}
 	return NULL;
 }
@@ -402,15 +224,16 @@ static struct mei_cl_cb *find_read_list_entry(
 static int mei_open(struct inode *inode, struct file *file)
 {
 	struct mei_cl *cl;
-	int if_num = iminor(inode), err;
 	struct mei_device *dev;
+	unsigned long cl_id;
+	int err;
 
 	err = -ENODEV;
 	if (!mei_device)
 		goto out;
 
 	dev = pci_get_drvdata(mei_device);
-	if (if_num != MEI_MINOR_NUMBER || !dev)
+	if (!dev)
 		goto out;
 
 	mutex_lock(&dev->device_lock);
@@ -429,14 +252,16 @@ static int mei_open(struct inode *inode, struct file *file)
 	if (dev->open_handle_count >= MEI_MAX_OPEN_HANDLE_COUNT)
 		goto out_unlock;
 
-	cl->host_client_id = find_first_zero_bit(dev->host_clients_map,
-							MEI_CLIENTS_MAX);
-	if (cl->host_client_id > MEI_CLIENTS_MAX)
+	cl_id = find_first_zero_bit(dev->host_clients_map, MEI_CLIENTS_MAX);
+	if (cl_id >= MEI_CLIENTS_MAX)
 		goto out_unlock;
 
+	cl->host_client_id  = cl_id;
+
 	dev_dbg(&dev->pdev->dev, "client_id = %d\n", cl->host_client_id);
 
 	dev->open_handle_count++;
+
 	list_add_tail(&cl->link, &dev->file_list);
 
 	set_bit(cl->host_client_id, dev->host_clients_map);
@@ -446,7 +271,7 @@ static int mei_open(struct inode *inode, struct file *file)
 	file->private_data = cl;
 	mutex_unlock(&dev->device_lock);
 
-	return 0;
+	return nonseekable_open(inode, file);
 
 out_unlock:
 	mutex_unlock(&dev->device_lock);
@@ -492,8 +317,7 @@ static int mei_release(struct inode *inode, struct file *file)
 		    cl->me_client_id);
 
 		if (dev->open_handle_count > 0) {
-			clear_bit(cl->host_client_id,
-				  dev->host_clients_map);
+			clear_bit(cl->host_client_id, dev->host_clients_map);
 			dev->open_handle_count--;
 		}
 		mei_remove_client_from_file_list(dev, cl->host_client_id);
@@ -554,7 +378,7 @@ static int mei_release(struct inode *inode, struct file *file)
  * returns >=0 data length on success , <0 on error
  */
 static ssize_t mei_read(struct file *file, char __user *ubuf,
-			 size_t length, loff_t *offset)
+			size_t length, loff_t *offset)
 {
 	struct mei_cl *cl = file->private_data;
 	struct mei_cl_cb *cb_pos = NULL;
@@ -673,9 +497,7 @@ copy_buffer:
 	/* information size may be longer */
 	length = min_t(size_t, length, (cb->information - *offset));
 
-	if (copy_to_user(ubuf,
-			 cb->response_buffer.data + *offset,
-			 length)) {
+	if (copy_to_user(ubuf, cb->response_buffer.data + *offset, length)) {
 		rets = -EFAULT;
 		goto free;
 	}
@@ -711,7 +533,7 @@ out:
  * returns >=0 data length on success , <0 on error
  */
 static ssize_t mei_write(struct file *file, const char __user *ubuf,
-			  size_t length, loff_t *offset)
+			 size_t length, loff_t *offset)
 {
 	struct mei_cl *cl = file->private_data;
 	struct mei_cl_cb *write_cb = NULL;
@@ -762,8 +584,7 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 			cl->read_cb = NULL;
 			cl->read_pending = 0;
 		}
-	} else if (cl->reading_state == MEI_IDLE &&
-		   !cl->read_pending)
+	} else if (cl->reading_state == MEI_IDLE && !cl->read_pending)
 		*offset = 0;
 
 
@@ -1034,7 +855,7 @@ out:
  */
 #ifdef CONFIG_COMPAT
 static long mei_compat_ioctl(struct file *file,
-		      unsigned int cmd, unsigned long data)
+			unsigned int cmd, unsigned long data)
 {
 	return mei_ioctl(file, cmd, (unsigned long)compat_ptr(data));
 }
@@ -1090,6 +911,206 @@ out:
 	return mask;
 }
 
+/*
+ * file operations structure will be used for mei char device.
+ */
+static const struct file_operations mei_fops = {
+	.owner = THIS_MODULE,
+	.read = mei_read,
+	.unlocked_ioctl = mei_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = mei_compat_ioctl,
+#endif
+	.open = mei_open,
+	.release = mei_release,
+	.write = mei_write,
+	.poll = mei_poll,
+	.llseek = no_llseek
+};
+
+
+/*
+ * Misc Device Struct
+ */
+static struct miscdevice  mei_misc_device = {
+		.name = MEI_DRIVER_NAME,
+		.fops = &mei_fops,
+		.minor = MISC_DYNAMIC_MINOR,
+};
+
+/**
+ * mei_probe - Device Initialization Routine
+ *
+ * @pdev: PCI device structure
+ * @ent: entry in kcs_pci_tbl
+ *
+ * returns 0 on success, <0 on failure.
+ */
+static int __devinit mei_probe(struct pci_dev *pdev,
+				const struct pci_device_id *ent)
+{
+	struct mei_device *dev;
+	int err;
+
+	mutex_lock(&mei_mutex);
+	if (mei_device) {
+		err = -EEXIST;
+		goto end;
+	}
+	/* enable pci dev */
+	err = pci_enable_device(pdev);
+	if (err) {
+		printk(KERN_ERR "mei: Failed to enable pci device.\n");
+		goto end;
+	}
+	/* set PCI host mastering  */
+	pci_set_master(pdev);
+	/* pci request regions for mei driver */
+	err = pci_request_regions(pdev, mei_driver_name);
+	if (err) {
+		printk(KERN_ERR "mei: Failed to get pci regions.\n");
+		goto disable_device;
+	}
+	/* allocates and initializes the mei dev structure */
+	dev = mei_device_init(pdev);
+	if (!dev) {
+		err = -ENOMEM;
+		goto release_regions;
+	}
+	/* mapping  IO device memory */
+	dev->mem_addr = pci_iomap(pdev, 0, 0);
+	if (!dev->mem_addr) {
+		printk(KERN_ERR "mei: mapping I/O device memory failure.\n");
+		err = -ENOMEM;
+		goto free_device;
+	}
+	pci_enable_msi(pdev);
+
+	 /* request and enable interrupt */
+	if (pci_dev_msi_enabled(pdev))
+		err = request_threaded_irq(pdev->irq,
+			NULL,
+			mei_interrupt_thread_handler,
+			0, mei_driver_name, dev);
+	else
+		err = request_threaded_irq(pdev->irq,
+			mei_interrupt_quick_handler,
+			mei_interrupt_thread_handler,
+			IRQF_SHARED, mei_driver_name, dev);
+
+	if (err) {
+		printk(KERN_ERR "mei: request_threaded_irq failure. irq = %d\n",
+		       pdev->irq);
+		goto unmap_memory;
+	}
+	INIT_DELAYED_WORK(&dev->timer_work, mei_timer);
+	if (mei_hw_init(dev)) {
+		printk(KERN_ERR "mei: Init hw failure.\n");
+		err = -ENODEV;
+		goto release_irq;
+	}
+
+	err = misc_register(&mei_misc_device);
+	if (err)
+		goto release_irq;
+
+	mei_device = pdev;
+	pci_set_drvdata(pdev, dev);
+
+
+	schedule_delayed_work(&dev->timer_work, HZ);
+
+	mutex_unlock(&mei_mutex);
+
+	pr_debug("mei: Driver initialization successful.\n");
+
+	return 0;
+
+release_irq:
+	/* disable interrupts */
+	dev->host_hw_state = mei_hcsr_read(dev);
+	mei_disable_interrupts(dev);
+	flush_scheduled_work();
+	free_irq(pdev->irq, dev);
+	pci_disable_msi(pdev);
+unmap_memory:
+	pci_iounmap(pdev, dev->mem_addr);
+free_device:
+	kfree(dev);
+release_regions:
+	pci_release_regions(pdev);
+disable_device:
+	pci_disable_device(pdev);
+end:
+	mutex_unlock(&mei_mutex);
+	printk(KERN_ERR "mei: Driver initialization failed.\n");
+	return err;
+}
+
+/**
+ * mei_remove - Device Removal Routine
+ *
+ * @pdev: PCI device structure
+ *
+ * mei_remove is called by the PCI subsystem to alert the driver
+ * that it should release a PCI device.
+ */
+static void __devexit mei_remove(struct pci_dev *pdev)
+{
+	struct mei_device *dev;
+
+	if (mei_device != pdev)
+		return;
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return;
+
+	mutex_lock(&dev->device_lock);
+
+	mei_wd_stop(dev, false);
+
+	mei_device = NULL;
+
+	if (dev->iamthif_cl.state == MEI_FILE_CONNECTED) {
+		dev->iamthif_cl.state = MEI_FILE_DISCONNECTING;
+		mei_disconnect_host_client(dev, &dev->iamthif_cl);
+	}
+	if (dev->wd_cl.state == MEI_FILE_CONNECTED) {
+		dev->wd_cl.state = MEI_FILE_DISCONNECTING;
+		mei_disconnect_host_client(dev, &dev->wd_cl);
+	}
+
+	/* Unregistering watchdog device */
+	mei_watchdog_unregister(dev);
+
+	/* remove entry if already in list */
+	dev_dbg(&pdev->dev, "list del iamthif and wd file list.\n");
+	mei_remove_client_from_file_list(dev, dev->wd_cl.host_client_id);
+	mei_remove_client_from_file_list(dev, dev->iamthif_cl.host_client_id);
+
+	dev->iamthif_current_cb = NULL;
+	dev->me_clients_num = 0;
+
+	mutex_unlock(&dev->device_lock);
+
+	flush_scheduled_work();
+
+	/* disable interrupts */
+	mei_disable_interrupts(dev);
+
+	free_irq(pdev->irq, dev);
+	pci_disable_msi(pdev);
+	pci_set_drvdata(pdev, NULL);
+
+	if (dev->mem_addr)
+		pci_iounmap(pdev, dev->mem_addr);
+
+	kfree(dev);
+
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
 #ifdef CONFIG_PM
 static int mei_pci_suspend(struct device *device)
 {
@@ -1173,131 +1194,6 @@ static struct pci_driver mei_driver = {
 	.driver.pm = MEI_PM_OPS,
 };
 
-/*
- * file operations structure will be used for mei char device.
- */
-static const struct file_operations mei_fops = {
-	.owner = THIS_MODULE,
-	.read = mei_read,
-	.unlocked_ioctl = mei_ioctl,
-#ifdef CONFIG_COMPAT
-	.compat_ioctl = mei_compat_ioctl,
-#endif
-	.open = mei_open,
-	.release = mei_release,
-	.write = mei_write,
-	.poll = mei_poll,
-};
-
-/**
- * mei_registration_cdev - sets up the cdev structure for mei device.
- *
- * @dev: char device struct
- * @hminor: minor number for registration char device
- * @fops: file operations structure
- *
- * returns 0 on success, <0 on failure.
- */
-static int mei_registration_cdev(struct cdev *dev, int hminor,
-				  const struct file_operations *fops)
-{
-	int ret, devno = MKDEV(mei_major, hminor);
-
-	cdev_init(dev, fops);
-	dev->owner = THIS_MODULE;
-	ret = cdev_add(dev, devno, 1);
-	/* Fail gracefully if need be */
-	if (ret)
-		printk(KERN_ERR "mei: Error %d registering mei device %d\n",
-		       ret, hminor);
-	return ret;
-}
-
-/**
- * mei_register_cdev - registers mei char device
- *
- * returns 0 on success, <0 on failure.
- */
-static int mei_register_cdev(void)
-{
-	int ret;
-	dev_t dev;
-
-	/* registration of char devices */
-	ret = alloc_chrdev_region(&dev, MEI_MINORS_BASE, MEI_MINORS_COUNT,
-				  MEI_DRIVER_NAME);
-	if (ret) {
-		printk(KERN_ERR "mei: Error allocating char device region.\n");
-		return ret;
-	}
-
-	mei_major = MAJOR(dev);
-
-	ret = mei_registration_cdev(&mei_cdev, MEI_MINOR_NUMBER,
-				     &mei_fops);
-	if (ret)
-		unregister_chrdev_region(MKDEV(mei_major, MEI_MINORS_BASE),
-					 MEI_MINORS_COUNT);
-
-	return ret;
-}
-
-/**
- * mei_unregister_cdev - unregisters mei char device
- */
-static void mei_unregister_cdev(void)
-{
-	cdev_del(&mei_cdev);
-	unregister_chrdev_region(MKDEV(mei_major, MEI_MINORS_BASE),
-				 MEI_MINORS_COUNT);
-}
-
-/**
- * mei_sysfs_device_create - adds device entry to sysfs
- *
- * returns 0 on success, <0 on failure.
- */
-static int mei_sysfs_device_create(void)
-{
-	struct class *class;
-	void *tmphdev;
-	int err;
-
-	class = class_create(THIS_MODULE, MEI_DRIVER_NAME);
-	if (IS_ERR(class)) {
-		err = PTR_ERR(class);
-		printk(KERN_ERR "mei: Error creating mei class.\n");
-		goto err_out;
-	}
-
-	tmphdev = device_create(class, NULL, mei_cdev.dev, NULL,
-					MEI_DEV_NAME);
-	if (IS_ERR(tmphdev)) {
-		err = PTR_ERR(tmphdev);
-		goto err_destroy;
-	}
-
-	mei_class = class;
-	return 0;
-
-err_destroy:
-	class_destroy(class);
-err_out:
-	return err;
-}
-
-/**
- * mei_sysfs_device_remove - unregisters the device entry on sysfs
- */
-static void mei_sysfs_device_remove(void)
-{
-	if (IS_ERR_OR_NULL(mei_class))
-		return;
-
-	device_destroy(mei_class, mei_cdev.dev);
-	class_destroy(mei_class);
-}
-
 /**
  * mei_init_module - Driver Registration Routine
  *
@@ -1314,26 +1210,9 @@ static int __init mei_init_module(void)
 		mei_driver_string, mei_driver_version);
 	/* init pci module */
 	ret = pci_register_driver(&mei_driver);
-	if (ret < 0) {
+	if (ret < 0)
 		printk(KERN_ERR "mei: Error registering driver.\n");
-		goto end;
-	}
 
-	ret = mei_register_cdev();
-	if (ret)
-		goto unregister_pci;
-
-	ret = mei_sysfs_device_create();
-	if (ret)
-		goto unregister_cdev;
-
-	return ret;
-
-unregister_cdev:
-	mei_unregister_cdev();
-unregister_pci:
-	pci_unregister_driver(&mei_driver);
-end:
 	return ret;
 }
 
@@ -1347,8 +1226,7 @@ module_init(mei_init_module);
  */
 static void __exit mei_exit_module(void)
 {
-	mei_sysfs_device_remove();
-	mei_unregister_cdev();
+	misc_deregister(&mei_misc_device);
 	pci_unregister_driver(&mei_driver);
 
 	pr_debug("mei: Driver unloaded successfully.\n");
diff --git a/drivers/staging/mei/mei.txt b/drivers/staging/mei/mei.txt
index 17302ad2531f..516bfe7319a6 100644
--- a/drivers/staging/mei/mei.txt
+++ b/drivers/staging/mei/mei.txt
@@ -1,78 +1,74 @@
-Intel MEI
+Intel(R) Management Engine Interface (Intel(R) MEI)
 =======================
 
 Introduction
 =======================
 
-The Intel Management Engine (Intel ME) is an isolated and
-protected computing resource (Coprocessor) residing inside
-Intel chipsets. The Intel ME provides support for computer/IT
-management features.
-The Feature set depends on the Intel chipset SKU.
+The Intel Management Engine (Intel ME) is an isolated andprotected computing
+resource (Co-processor) residing inside certain Intel chipsets. The Intel ME
+provides support for computer/IT management features. The feature set
+depends on the Intel chipset SKU.
 
-The Intel Management Engine Interface (Intel MEI, previously known
-as HECI) is the interface between the Host and Intel ME.
-This interface is exposed to the host as a PCI device.
-The Intel MEI Driver is in charge of the communication channel
-between a host application and the ME feature.
+The Intel Management Engine Interface (Intel MEI, previously known as HECI)
+is the interface between the Host and Intel ME. This interface is exposed
+to the host as a PCI device. The Intel MEI Driver is in charge of the
+communication channel between a host application and the Intel ME feature.
 
-Each Intel ME feature (Intel ME Client) is addressed by
-GUID/UUID and each feature defines its own protocol.
-The protocol is message-based with a header and payload up to
-512 bytes.
+Each Intel ME feature (Intel ME Client) is addressed by a GUID/UUID and
+each client has its own protocol. The protocol is message-based with a
+header and payload up to 512 bytes.
 
-[place holder to URL to protocol definitions]
-
-Prominent usage of the Interface is to communicate with
-Intel Active Management Technology (Intel AMT)
-implemented in firmware running on the Intel ME.
+Prominent usage of the Intel ME Interface is to communicate with Intel(R)
+Active Management Technology (Intel AMT)implemented in firmware running on
+the Intel ME.
 
 Intel AMT provides the ability to manage a host remotely out-of-band (OOB)
-even when the host processor has crashed or is in a sleep state.
+even when the operating system running on the host processor has crashed or
+is in a sleep state.
 
 Some examples of Intel AMT usage are:
    - Monitoring hardware state and platform components
-   - Remote power off/on (useful for green computing or overnight IT maintenance)
+   - Remote power off/on (useful for green computing or overnight IT
+     maintenance)
    - OS updates
    - Storage of useful platform information such as software assets
-   - built-in hardware KVM
-   - selective network isolation of Ethernet and IP protocol flows based on
-     policies set by a remote management console
+   - Built-in hardware KVM
+   - Selective network isolation of Ethernet and IP protocol flows based
+     on policies set by a remote management console
    - IDE device redirection from remote management console
 
 Intel AMT (OOB) communication is based on SOAP (deprecated
-starting with Release 6.0) over HTTP/HTTPS or WS-Management protocol
-over HTTP and HTTPS that are received from a remote
-management console application.
+starting with Release 6.0) over HTTP/S or WS-Management protocol over
+HTTP/S that are received from a remote management console application.
 
 For more information about Intel AMT:
-http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide/WordDocuments/aboutintelamt.htm
-
+http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide
 
-MEI Driver
+Intel MEI Driver
 =======================
 
-The driver exposes a character device called /dev/mei.
+The driver exposes a misc device called /dev/mei.
 
-An application maintains communication with an ME feature while
-/dev/mei is open. The binding to a specific features is performed
-by calling MEI_CONNECT_CLIENT_IOCTL, which passes the desired UUID.
-The number of instances of an ME feature that can be opened
-at the same time depends on the ME feature, but most of the
+An application maintains communication with an Intel ME feature while
+/dev/mei is open. The binding to a specific features is performed by calling
+MEI_CONNECT_CLIENT_IOCTL, which passes the desired UUID.
+The number of instances of an Intel ME feature that can be opened
+at the same time depends on the Intel ME feature, but most of the
 features allow only a single instance.
 
-
-The Intel AMT Host Interface (AMTHI) feature requires multiple
-simultaneous user applications, therefore the MEI driver handles
+The Intel AMT Host Interface (Intel AMTHI) feature supports multiple
+simultaneous user applications. Therefore, the Intel MEI driver handles
 this internally by maintaining request queues for the applications.
 
-The driver is oblivious to data that are passed between
+The driver is oblivious to data that is passed between firmware feature
+and host application.
 
-Because some of the ME features can change the system
-configuration, the driver by default allows only privileged
+Because some of the Intel ME features can change the system
+configuration, the driver by default allows only a privileged
 user to access it.
 
-A Code snippet for application communicating with AMTHI client:
+A code snippet for an application communicating with
+Intel AMTHI client:
 	struct mei_connect_client_data data;
 	fd = open(MEI_DEVICE);
 
@@ -80,7 +76,7 @@ A Code snippet for application communicating with AMTHI client:
 
 	ioctl(fd, IOCTL_MEI_CONNECT_CLIENT, &data);
 
-	printf(“Ver=%d, MaxLen=%ld\n”,
+	printf("Ver=%d, MaxLen=%ld\n",
 			data.d.in_client_uuid.protocol_version,
 			data.d.in_client_uuid.max_msg_length);
 
@@ -95,76 +91,106 @@ A Code snippet for application communicating with AMTHI client:
 	[...]
 	close(fd);
 
-ME Applications:
+IOCTL:
+======
+The Intel MEI Driver supports the following IOCTL command:
+	IOCTL_MEI_CONNECT_CLIENT	Connect to firmware Feature (client).
+
+	usage:
+		struct mei_connect_client_data clientData;
+		ioctl(fd, IOCTL_MEI_CONNECT_CLIENT, &clientData);
+
+	inputs:
+		mei_connect_client_data struct contain the following
+		input field:
+
+		in_client_uuid -	UUID of the FW Feature that needs
+					to connect to.
+	outputs:
+		out_client_properties - Client Properties: MTU and Protocol Version.
+
+	error returns:
+		EINVAL	Wrong IOCTL Number
+		ENODEV	Device or Connection is not initialized or ready.
+			(e.g. Wrong UUID)
+		ENOMEM	Unable to allocate memory to client internal data.
+		EFAULT	Fatal Error (e.g. Unable to access user input data)
+		EBUSY	Connection Already Open
+
+	Notes:
+        max_msg_length (MTU) in client properties describes the maximum
+        data that can be sent or received. (e.g. if MTU=2K, can send
+        requests up to bytes 2k and received responses upto 2k bytes).
+
+Intel ME Applications:
 ==============
 
 1) Intel Local Management Service (Intel LMS)
-	Applications running locally on the platform communicate with
-	Intel AMT Release 2.0 and later releases in the same way
-	that network applications do via SOAP over HTTP (deprecated
-	starting with Release 6.0) or with WS-Management over SOAP over
-	HTTP. which means that some Intel AMT feature can be access
-	from a local application using same Network interface as for
-	remote application.
-
-	When a local application sends a message addressed to the local
-	Intel AMT host name, the Local Manageability Service (LMS),
-	which listens for traffic directed to the host name, intercepts
-	the message and routes it to the Intel Management Engine Interface.
+
+	Applications running locally on the platform communicate with Intel AMT Release
+	2.0 and later releases in the same way that network applications do via SOAP
+	over HTTP (deprecated starting with Release 6.0) or with WS-Management over
+	SOAP over HTTP. This means that some Intel AMT features can be accessed from a
+	local application using the same network interface as a remote application
+	communicating with Intel AMT over the network.
+
+	When a local application sends a message addressed to the local Intel AMT host
+	name, the Intel LMS, which listens for traffic directed to the host name,
+	intercepts the message and routes it to the Intel MEI.
 	For more information:
-	http://software.intel.com/sites/manageability/AMT_Implementation_and_
-	Reference_Guide/WordDocuments/localaccess1.htm
-
-	The LMS opens a connection using the MEI driver to the LMS
-	FW feature using a defined UUID and then communicates with the
-	feature using a protocol
-	called Intel(R) AMT Port Forwarding Protocol (APF protocol).
-	The protocol is used to maintain multiple sessions with
-	Intel AMT from a single application.
-	See the protocol specification in
-	the Intel(R) AMT Implementation and Reference Guide
-	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide/HTMLDocuments/MPSDocuments/Intel%20AMT%20Port%20Forwarding%20Protocol%20Reference%20Manual.pdf
-
-  2) Intel AMT Remote configuration using a Local Agent:
+	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide
+	Under "About Intel AMT" => "Local Access"
+
+	For downloading Intel LMS:
+	http://software.intel.com/en-us/articles/download-the-latest-intel-amt-open-source-drivers/
+
+	The Intel LMS opens a connection using the Intel MEI driver to the Intel LMS
+	firmware feature using a defined UUID and then communicates with the feature
+	using a protocol called Intel AMT Port Forwarding Protocol(Intel APF protocol).
+	The protocol is used to maintain multiple sessions with Intel AMT from a
+	single application.
+
+	See the protocol specification in the Intel AMT Software Development Kit(SDK)
+	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide
+	Under "SDK Resources" => "Intel(R) vPro(TM) Gateway(MPS)"
+	=> "Information for Intel(R) vPro(TM) Gateway Developers"
+	=> "Description of the Intel AMT Port Forwarding (APF)Protocol"
+
+  2) Intel AMT Remote configuration using a Local Agent
 	A Local Agent enables IT personnel to configure Intel AMT out-of-the-box
-	without requiring installing additional data to enable setup.
-	The remote configuration process may involve an ISV-developed remote
-	configuration agent that runs on the host.
+	without requiring installing additional data to enable setup. The remote
+	configuration process may involve an ISV-developed remote configuration
+	agent that runs on the host.
 	For more information:
-	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide/WordDocuments/remoteconfigurationwithalocalagent.htm
+	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide
+	Under "Setup and Configuration of Intel AMT" =>
+	"SDK Tools Supporting Setup and Configuration" =>
+	"Using the Local Agent Sample"
+
+	An open source Intel AMT configuration utility,	implementing a local agent
+	that accesses the Intel MEI driver, can be found here:
+	http://software.intel.com/en-us/articles/download-the-latest-intel-amt-open-source-drivers/
 
-	How the Local Agent Works (including Command structs):
-	http://software.intel.com/sites/manageability/AMT_Implementation_and_Reference_Guide/WordDocuments/howthelocalagentsampleworks.htm
 
 Intel AMT OS Health Watchdog:
 =============================
 The Intel AMT Watchdog is an OS Health (Hang/Crash) watchdog.
 Whenever the OS hangs or crashes, Intel AMT will send an event
-to whoever subscribed to this event. This mechanism means that
-IT knows when a platform crashes even when there is a hard failure
-on the host.
-The AMT Watchdog is composed of two parts:
-	1) FW Feature - that receives the heartbeats
-	   and sends an event when the heartbeats stop.
-	2) MEI driver – connects to the watchdog (WD) feature,
-	   configures the watchdog and sends the heartbeats.
-
-The MEI driver configures the Watchdog to expire by default
-every 120sec unless set by the user using module parameters.
-The Driver then sends heartbeats every 2sec.
+to any subsciber to this event. This mechanism means that
+IT knows when a platform crashes even when there is a hard failureon the host.
 
-If WD feature does not exist (i.e. the connection failed),
-the MEI driver will disable the sending of heartbeats.
+The Intel AMT Watchdog is composed of two parts:
+	1) Firmware feature - receives the heartbeats
+	   and sends an event when the heartbeats stop.
+	2) Intel MEI driver - connects to the watchdog feature, configures the
+	   watchdog and sends the heartbeats.
 
-Module Parameters
-=================
-watchdog_timeout - the user can use this module parameter
-to change the watchdog timeout setting.
+The Intel MEI driver uses the kernel watchdog to configure the Intel AMT
+Watchdog and to send heartbeats to it. The default timeout of the
+watchdog is 120 seconds.
 
-This value sets the Intel AMT watchdog timeout interval in seconds;
-the default value is 120sec.
-in order to disable the watchdog activites set the value to 0.
-Normal values should be between 120 and 65535
+If the Intel AMT Watchdog feature does not exist (i.e. the connection failed),
+the Intel MEI driver will disable the sending of heartbeats.
 
 Supported Chipsets:
 ==================
diff --git a/drivers/staging/mei/mei_dev.h b/drivers/staging/mei/mei_dev.h
index af4b1af9eeac..82bacfc624c5 100644
--- a/drivers/staging/mei/mei_dev.h
+++ b/drivers/staging/mei/mei_dev.h
@@ -23,13 +23,6 @@
 #include "hw.h"
 
 /*
- * MEI Char Driver Minors
- */
-#define MEI_MINORS_BASE	1
-#define MEI_MINORS_COUNT	1
-#define MEI_MINOR_NUMBER	1
-
-/*
  * watch dog definition
  */
 #define MEI_WATCHDOG_DATA_SIZE         16
@@ -42,11 +35,6 @@
  */
 extern struct pci_dev *mei_device;
 
-/*
- * AMT Watchdog Device
- */
-#define INTEL_AMT_WATCHDOG_ID "INTCAMT"
-extern struct watchdog_device amt_wd_dev;
 
 /*
  * AMTHI Client UUID
@@ -175,7 +163,6 @@ struct mei_cl {
 
 struct mei_io_list {
 	struct mei_cl_cb mei_cb;
-	int status;
 };
 
 /* MEI private device struct */
diff --git a/drivers/staging/mei/wd.c b/drivers/staging/mei/wd.c
index ffca7ca32658..8094941a98f1 100644
--- a/drivers/staging/mei/wd.c
+++ b/drivers/staging/mei/wd.c
@@ -35,12 +35,16 @@ const u8 mei_wd_state_independence_msg[3][4] = {
 	{0x07, 0x02, 0x01, 0x10}
 };
 
+/*
+ * AMT Watchdog Device
+ */
+#define INTEL_AMT_WATCHDOG_ID "INTCAMT"
+
 /* UUIDs for AMT F/W clients */
 const uuid_le mei_wd_guid = UUID_LE(0x05B79A6F, 0x4628, 0x4D7F, 0x89,
 						0x9D, 0xA9, 0x15, 0x14, 0xCB,
 						0x32, 0xAB);
 
-
 void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
 {
 	dev_dbg(&dev->pdev->dev, "timeout=%d.\n", timeout);
@@ -331,14 +335,14 @@ static int mei_wd_ops_set_timeout(struct watchdog_device *wd_dev, unsigned int t
 /*
  * Watchdog Device structs
  */
-const struct watchdog_ops wd_ops = {
+static const struct watchdog_ops wd_ops = {
 		.owner = THIS_MODULE,
 		.start = mei_wd_ops_start,
 		.stop = mei_wd_ops_stop,
 		.ping = mei_wd_ops_ping,
 		.set_timeout = mei_wd_ops_set_timeout,
 };
-const struct watchdog_info wd_info = {
+static const struct watchdog_info wd_info = {
 		.identity = INTEL_AMT_WATCHDOG_ID,
 		.options = WDIOF_KEEPALIVEPING,
 };
@@ -352,3 +356,25 @@ struct watchdog_device amt_wd_dev = {
 };
 
 
+void  mei_watchdog_register(struct mei_device *dev)
+{
+	dev_dbg(&dev->pdev->dev, "dev->wd_timeout =%d.\n", dev->wd_timeout);
+
+	dev->wd_due_counter = !!dev->wd_timeout;
+
+	if (watchdog_register_device(&amt_wd_dev)) {
+		dev_err(&dev->pdev->dev, "unable to register watchdog device.\n");
+		dev->wd_interface_reg = false;
+	} else {
+		dev_dbg(&dev->pdev->dev, "successfully register watchdog interface.\n");
+		dev->wd_interface_reg = true;
+	}
+}
+
+void mei_watchdog_unregister(struct mei_device *dev)
+{
+	if (dev->wd_interface_reg)
+		watchdog_unregister_device(&amt_wd_dev);
+	dev->wd_interface_reg = false;
+}
+
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index e06b867d1e0c..fafdfa25e139 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -27,6 +27,8 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/irq.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 #include <linux/list.h>
 #include <linux/mfd/core.h>
 #include <linux/mutex.h>
@@ -727,8 +729,24 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 	}
 	platform_set_drvdata(pdev, nvec);
 	nvec->dev = &pdev->dev;
-	nvec->gpio = pdata->gpio;
-	nvec->i2c_addr = pdata->i2c_addr;
+
+	if (pdata) {
+		nvec->gpio = pdata->gpio;
+		nvec->i2c_addr = pdata->i2c_addr;
+	} else if (nvec->dev->of_node) {
+		nvec->gpio = of_get_named_gpio(nvec->dev->of_node, "request-gpios", 0);
+		if (nvec->gpio < 0) {
+			dev_err(&pdev->dev, "no gpio specified");
+			goto failed;
+		}
+		if (of_property_read_u32(nvec->dev->of_node, "slave-addr", &nvec->i2c_addr)) {
+			dev_err(&pdev->dev, "no i2c address specified");
+			goto failed;
+		}
+	} else {
+		dev_err(&pdev->dev, "no platform data\n");
+		goto failed;
+	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
@@ -893,6 +911,13 @@ static int tegra_nvec_resume(struct platform_device *pdev)
 #define tegra_nvec_resume NULL
 #endif
 
+/* Match table for of_platform binding */
+static const struct of_device_id nvidia_nvec_of_match[] __devinitconst = {
+	{ .compatible = "nvidia,nvec", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, nvidia_nvec_of_match);
+
 static struct platform_driver nvec_device_driver = {
 	.probe   = tegra_nvec_probe,
 	.remove  = __devexit_p(tegra_nvec_remove),
@@ -901,6 +926,7 @@ static struct platform_driver nvec_device_driver = {
 	.driver  = {
 		.name = "nvec",
 		.owner = THIS_MODULE,
+		.of_match_table = nvidia_nvec_of_match,
 	}
 };
 
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index af24ddfb58c9..3d9199320d86 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -34,8 +34,8 @@
 
 /* Module definitions */
 
-static int resumeline = 898;
-module_param(resumeline, int, 0444);
+static ushort resumeline = 898;
+module_param(resumeline, ushort, 0444);
 
 /* Default off since it doesn't work on DCON ASIC in B-test OLPC board */
 static int useaa = 1;
@@ -456,7 +456,7 @@ static ssize_t dcon_mono_store(struct device *dev,
 	unsigned long enable_mono;
 	int rc;
 
-	rc = strict_strtoul(buf, 10, &enable_mono);
+	rc = kstrtoul(buf, 10, &enable_mono);
 	if (rc)
 		return rc;
 
@@ -472,7 +472,7 @@ static ssize_t dcon_freeze_store(struct device *dev,
 	unsigned long output;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &output);
+	ret = kstrtoul(buf, 10, &output);
 	if (ret)
 		return ret;
 
@@ -498,10 +498,10 @@ static ssize_t dcon_freeze_store(struct device *dev,
 static ssize_t dcon_resumeline_store(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
-	unsigned long rl;
+	unsigned short rl;
 	int rc;
 
-	rc = strict_strtoul(buf, 10, &rl);
+	rc = kstrtou16(buf, 10, &rl);
 	if (rc)
 		return rc;
 
@@ -517,7 +517,7 @@ static ssize_t dcon_sleep_store(struct device *dev,
 	unsigned long output;
 	int ret;
 
-	ret = strict_strtoul(buf, 10, &output);
+	ret = kstrtoul(buf, 10, &output);
 	if (ret)
 		return ret;
 
@@ -755,9 +755,9 @@ static int dcon_resume(struct i2c_client *client)
 irqreturn_t dcon_interrupt(int irq, void *id)
 {
 	struct dcon_priv *dcon = id;
-	int status = pdata->read_status();
+	u8 status;
 
-	if (status == -1)
+	if (pdata->read_status(&status))
 		return IRQ_NONE;
 
 	switch (status & 3) {
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.h b/drivers/staging/olpc_dcon/olpc_dcon.h
index 0264c94375aa..167a41778be6 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.h
+++ b/drivers/staging/olpc_dcon/olpc_dcon.h
@@ -84,7 +84,7 @@ struct dcon_platform_data {
 	int (*init)(struct dcon_priv *);
 	void (*bus_stabilize_wiggle)(void);
 	void (*set_dconload)(int);
-	u8 (*read_status)(void);
+	int (*read_status)(u8 *);
 };
 
 #include <linux/interrupt.h>
diff --git a/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c b/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
index 2245213df607..cb6ce0cf92a0 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon_xo_1.c
@@ -183,17 +183,15 @@ static void dcon_set_dconload_1(int val)
 	gpio_set_value(OLPC_GPIO_DCON_LOAD, val);
 }
 
-static u8 dcon_read_status_xo_1(void)
+static int dcon_read_status_xo_1(u8 *status)
 {
-	u8 status;
-
-	status = gpio_get_value(OLPC_GPIO_DCON_STAT0);
-	status |= gpio_get_value(OLPC_GPIO_DCON_STAT1) << 1;
+	*status = gpio_get_value(OLPC_GPIO_DCON_STAT0);
+	*status |= gpio_get_value(OLPC_GPIO_DCON_STAT1) << 1;
 
 	/* Clear the negative edge status for GPIO7 */
 	cs5535_gpio_set(OLPC_GPIO_DCON_IRQ, GPIO_NEGATIVE_EDGE_STS);
 
-	return status;
+	return 0;
 }
 
 struct dcon_platform_data dcon_pdata_xo_1 = {
diff --git a/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c b/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
index a6a6cf2adc4d..69415eec425c 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
@@ -167,20 +167,18 @@ static void dcon_set_dconload_xo_1_5(int val)
 	gpio_set_value(VX855_GPIO(1), val);
 }
 
-static u8 dcon_read_status_xo_1_5(void)
+static int dcon_read_status_xo_1_5(u8 *status)
 {
-	u8 status;
-
 	if (!dcon_was_irq())
 		return -1;
 
 	/* i believe this is the same as "inb(0x44b) & 3" */
-	status = gpio_get_value(VX855_GPI(10));
-	status |= gpio_get_value(VX855_GPI(11)) << 1;
+	*status = gpio_get_value(VX855_GPI(10));
+	*status |= gpio_get_value(VX855_GPI(11)) << 1;
 
 	dcon_clear_irq();
 
-	return status;
+	return 0;
 }
 
 struct dcon_platform_data dcon_pdata_xo_1_5 = {
diff --git a/drivers/staging/omapdrm/Kconfig b/drivers/staging/omapdrm/Kconfig
new file mode 100644
index 000000000000..81a7cba4a0c5
--- /dev/null
+++ b/drivers/staging/omapdrm/Kconfig
@@ -0,0 +1,25 @@
+
+config DRM_OMAP
+	tristate "OMAP DRM"
+	depends on DRM && !CONFIG_FB_OMAP2
+	depends on ARCH_OMAP2PLUS
+	select DRM_KMS_HELPER
+	select OMAP2_DSS
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select FB_SYS_FOPS
+	default n
+	help
+	  DRM display driver for OMAP2/3/4 based boards.
+
+config DRM_OMAP_NUM_CRTCS
+	int "Number of CRTCs"
+	range 1 10
+	default 1  if ARCH_OMAP2 || ARCH_OMAP3
+	default 2  if ARCH_OMAP4
+	depends on DRM_OMAP
+	help
+	  Select the number of video overlays which can be used as framebuffers.
+	  The remaining overlays are reserved for video.
+
diff --git a/drivers/staging/omapdrm/Makefile b/drivers/staging/omapdrm/Makefile
new file mode 100644
index 000000000000..592cf69020cd
--- /dev/null
+++ b/drivers/staging/omapdrm/Makefile
@@ -0,0 +1,21 @@
+#
+# Makefile for the drm device driver.  This driver provides support for the
+# Direct Rendering Infrastructure (DRI)
+#
+
+ccflags-y := -Iinclude/drm -Werror
+omapdrm-y := omap_drv.o \
+	omap_debugfs.o \
+	omap_crtc.o \
+	omap_encoder.o \
+	omap_connector.o \
+	omap_fb.o \
+	omap_fbdev.o \
+	omap_gem.o \
+	omap_dmm_tiler.o \
+	tcm-sita.o
+
+# temporary:
+omapdrm-y += omap_gem_helpers.o
+
+obj-$(CONFIG_DRM_OMAP)	+= omapdrm.o
diff --git a/drivers/staging/omapdrm/TODO b/drivers/staging/omapdrm/TODO
new file mode 100644
index 000000000000..55b18377ac4f
--- /dev/null
+++ b/drivers/staging/omapdrm/TODO
@@ -0,0 +1,38 @@
+TODO
+. check error handling/cleanup paths
+. add drm_plane / overlay support
+. add video decode/encode support (via syslink3 + codec-engine)
+. still some rough edges with flipping.. event back to userspace should
+  really come after VSYNC interrupt
+. where should we do eviction (detatch_pages())?  We aren't necessarily
+  accessing the pages via a GART, so maybe we need some other threshold
+  to put a cap on the # of pages that can be pin'd.  (It is mostly only
+  of interest in case you have a swap partition/file.. which a lot of
+  these devices do not.. but it doesn't hurt for the driver to do the
+  right thing anyways.)
+  . Use mm_shrinker to trigger unpinning pages.  Need to figure out how
+    to handle next issue first (I think?)
+  . Note TTM already has some mm_shrinker stuff..  maybe an argument to
+    move to TTM?  Or maybe something that could be factored out in common?
+. GEM/shmem backed pages can have existing mappings (kernel linear map,
+  etc..), which isn't really ideal.
+. Revisit GEM sync object infrastructure.. TTM has some framework for this
+  already.  Possibly this could be refactored out and made more common?
+  There should be some way to do this with less wheel-reinvention.
+. Review DSS vs KMS mismatches.  The omap_dss_device is sort of part encoder,
+  part connector.  Which results in a bit of duct tape to fwd calls from
+  encoder to connector.  Possibly this could be done a bit better.
+. Solve PM sequencing on resume.  DMM/TILER must be reloaded before any
+  access is made from any component in the system.  Which means on suspend
+  CRTC's should be disabled, and on resume the LUT should be reprogrammed
+  before CRTC's are re-enabled, to prevent DSS from trying to DMA from a
+  buffer mapped in DMM/TILER before LUT is reloaded.
+. Add debugfs information for DMM/TILER
+
+Userspace:
+. git://github.com/robclark/xf86-video-omap.git
+
+Currently tested on
+. OMAP3530 beagleboard
+. OMAP4430 pandaboard
+. OMAP4460 pandaboard
diff --git a/drivers/staging/omapdrm/omap_connector.c b/drivers/staging/omapdrm/omap_connector.c
new file mode 100644
index 000000000000..5e2856c0e0bb
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_connector.c
@@ -0,0 +1,371 @@
+/*
+ * drivers/staging/omapdrm/omap_connector.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "omap_drv.h"
+
+#include "drm_crtc.h"
+#include "drm_crtc_helper.h"
+
+/*
+ * connector funcs
+ */
+
+#define to_omap_connector(x) container_of(x, struct omap_connector, base)
+
+struct omap_connector {
+	struct drm_connector base;
+	struct omap_dss_device *dssdev;
+};
+
+static inline void copy_timings_omap_to_drm(struct drm_display_mode *mode,
+		struct omap_video_timings *timings)
+{
+	mode->clock = timings->pixel_clock;
+
+	mode->hdisplay = timings->x_res;
+	mode->hsync_start = mode->hdisplay + timings->hfp;
+	mode->hsync_end = mode->hsync_start + timings->hsw;
+	mode->htotal = mode->hsync_end + timings->hbp;
+
+	mode->vdisplay = timings->y_res;
+	mode->vsync_start = mode->vdisplay + timings->vfp;
+	mode->vsync_end = mode->vsync_start + timings->vsw;
+	mode->vtotal = mode->vsync_end + timings->vbp;
+
+	/* note: whether or not it is interlaced, +/- h/vsync, etc,
+	 * which should be set in the mode flags, is not exposed in
+	 * the omap_video_timings struct.. but hdmi driver tracks
+	 * those separately so all we have to have to set the mode
+	 * is the way to recover these timings values, and the
+	 * omap_dss_driver would do the rest.
+	 */
+}
+
+static inline void copy_timings_drm_to_omap(struct omap_video_timings *timings,
+		struct drm_display_mode *mode)
+{
+	timings->pixel_clock = mode->clock;
+
+	timings->x_res = mode->hdisplay;
+	timings->hfp = mode->hsync_start - mode->hdisplay;
+	timings->hsw = mode->hsync_end - mode->hsync_start;
+	timings->hbp = mode->htotal - mode->hsync_end;
+
+	timings->y_res = mode->vdisplay;
+	timings->vfp = mode->vsync_start - mode->vdisplay;
+	timings->vsw = mode->vsync_end - mode->vsync_start;
+	timings->vbp = mode->vtotal - mode->vsync_end;
+}
+
+static void omap_connector_dpms(struct drm_connector *connector, int mode)
+{
+	struct omap_connector *omap_connector = to_omap_connector(connector);
+	struct omap_dss_device *dssdev = omap_connector->dssdev;
+	int old_dpms;
+
+	DBG("%s: %d", dssdev->name, mode);
+
+	old_dpms = connector->dpms;
+
+	/* from off to on, do from crtc to connector */
+	if (mode < old_dpms)
+		drm_helper_connector_dpms(connector, mode);
+
+	if (mode == DRM_MODE_DPMS_ON) {
+		/* store resume info for suspended displays */
+		switch (dssdev->state) {
+		case OMAP_DSS_DISPLAY_SUSPENDED:
+			dssdev->activate_after_resume = true;
+			break;
+		case OMAP_DSS_DISPLAY_DISABLED: {
+			int ret = dssdev->driver->enable(dssdev);
+			if (ret) {
+				DBG("%s: failed to enable: %d",
+						dssdev->name, ret);
+				dssdev->driver->disable(dssdev);
+			}
+			break;
+		}
+		default:
+			break;
+		}
+	} else {
+		/* TODO */
+	}
+
+	/* from on to off, do from connector to crtc */
+	if (mode > old_dpms)
+		drm_helper_connector_dpms(connector, mode);
+}
+
+enum drm_connector_status omap_connector_detect(
+		struct drm_connector *connector, bool force)
+{
+	struct omap_connector *omap_connector = to_omap_connector(connector);
+	struct omap_dss_device *dssdev = omap_connector->dssdev;
+	struct omap_dss_driver *dssdrv = dssdev->driver;
+	enum drm_connector_status ret;
+
+	if (dssdrv->detect) {
+		if (dssdrv->detect(dssdev)) {
+			ret = connector_status_connected;
+		} else {
+			ret = connector_status_disconnected;
+		}
+	} else {
+		ret = connector_status_unknown;
+	}
+
+	VERB("%s: %d (force=%d)", omap_connector->dssdev->name, ret, force);
+
+	return ret;
+}
+
+static void omap_connector_destroy(struct drm_connector *connector)
+{
+	struct omap_connector *omap_connector = to_omap_connector(connector);
+	struct omap_dss_device *dssdev = omap_connector->dssdev;
+
+	dssdev->driver->disable(dssdev);
+
+	DBG("%s", omap_connector->dssdev->name);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(omap_connector);
+
+	omap_dss_put_device(dssdev);
+}
+
+#define MAX_EDID  512
+
+static int omap_connector_get_modes(struct drm_connector *connector)
+{
+	struct omap_connector *omap_connector = to_omap_connector(connector);
+	struct omap_dss_device *dssdev = omap_connector->dssdev;
+	struct omap_dss_driver *dssdrv = dssdev->driver;
+	struct drm_device *dev = connector->dev;
+	int n = 0;
+
+	DBG("%s", omap_connector->dssdev->name);
+
+	/* if display exposes EDID, then we parse that in the normal way to
+	 * build table of supported modes.. otherwise (ie. fixed resolution
+	 * LCD panels) we just return a single mode corresponding to the
+	 * currently configured timings:
+	 */
+	if (dssdrv->read_edid) {
+		void *edid = kzalloc(MAX_EDID, GFP_KERNEL);
+
+		if ((dssdrv->read_edid(dssdev, edid, MAX_EDID) > 0) &&
+				drm_edid_is_valid(edid)) {
+			drm_mode_connector_update_edid_property(
+					connector, edid);
+			n = drm_add_edid_modes(connector, edid);
+			kfree(connector->display_info.raw_edid);
+			connector->display_info.raw_edid = edid;
+		} else {
+			drm_mode_connector_update_edid_property(
+					connector, NULL);
+			connector->display_info.raw_edid = NULL;
+			kfree(edid);
+		}
+	} else {
+		struct drm_display_mode *mode = drm_mode_create(dev);
+		struct omap_video_timings timings;
+
+		dssdrv->get_timings(dssdev, &timings);
+
+		copy_timings_omap_to_drm(mode, &timings);
+
+		mode->type = DRM_MODE_TYPE_DRIVER | DRM_MODE_TYPE_PREFERRED;
+		drm_mode_set_name(mode);
+		drm_mode_probed_add(connector, mode);
+
+		n = 1;
+	}
+
+	return n;
+}
+
+static int omap_connector_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	struct omap_connector *omap_connector = to_omap_connector(connector);
+	struct omap_dss_device *dssdev = omap_connector->dssdev;
+	struct omap_dss_driver *dssdrv = dssdev->driver;
+	struct omap_video_timings timings = {0};
+	struct drm_device *dev = connector->dev;
+	struct drm_display_mode *new_mode;
+	int ret = MODE_BAD;
+
+	copy_timings_drm_to_omap(&timings, mode);
+	mode->vrefresh = drm_mode_vrefresh(mode);
+
+	if (!dssdrv->check_timings(dssdev, &timings)) {
+		/* check if vrefresh is still valid */
+		new_mode = drm_mode_duplicate(dev, mode);
+		new_mode->clock = timings.pixel_clock;
+		new_mode->vrefresh = 0;
+		if (mode->vrefresh == drm_mode_vrefresh(new_mode))
+			ret = MODE_OK;
+		drm_mode_destroy(dev, new_mode);
+	}
+
+	DBG("connector: mode %s: "
+			"%d:\"%s\" %d %d %d %d %d %d %d %d %d %d 0x%x 0x%x",
+			(ret == MODE_OK) ? "valid" : "invalid",
+			mode->base.id, mode->name, mode->vrefresh, mode->clock,
+			mode->hdisplay, mode->hsync_start,
+			mode->hsync_end, mode->htotal,
+			mode->vdisplay, mode->vsync_start,
+			mode->vsync_end, mode->vtotal, mode->type, mode->flags);
+
+	return ret;
+}
+
+struct drm_encoder *omap_connector_attached_encoder(
+		struct drm_connector *connector)
+{
+	int i;
+	struct omap_connector *omap_connector = to_omap_connector(connector);
+
+	for (i = 0; i < DRM_CONNECTOR_MAX_ENCODER; i++) {
+		struct drm_mode_object *obj;
+
+		if (connector->encoder_ids[i] == 0)
+			break;
+
+		obj = drm_mode_object_find(connector->dev,
+				connector->encoder_ids[i],
+				DRM_MODE_OBJECT_ENCODER);
+
+		if (obj) {
+			struct drm_encoder *encoder = obj_to_encoder(obj);
+			struct omap_overlay_manager *mgr =
+					omap_encoder_get_manager(encoder);
+			DBG("%s: found %s", omap_connector->dssdev->name,
+					mgr->name);
+			return encoder;
+		}
+	}
+
+	DBG("%s: no encoder", omap_connector->dssdev->name);
+
+	return NULL;
+}
+
+static const struct drm_connector_funcs omap_connector_funcs = {
+	.dpms = omap_connector_dpms,
+	.detect = omap_connector_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = omap_connector_destroy,
+};
+
+static const struct drm_connector_helper_funcs omap_connector_helper_funcs = {
+	.get_modes = omap_connector_get_modes,
+	.mode_valid = omap_connector_mode_valid,
+	.best_encoder = omap_connector_attached_encoder,
+};
+
+/* called from encoder when mode is set, to propagate settings to the dssdev */
+void omap_connector_mode_set(struct drm_connector *connector,
+		struct drm_display_mode *mode)
+{
+	struct drm_device *dev = connector->dev;
+	struct omap_connector *omap_connector = to_omap_connector(connector);
+	struct omap_dss_device *dssdev = omap_connector->dssdev;
+	struct omap_dss_driver *dssdrv = dssdev->driver;
+	struct omap_video_timings timings;
+
+	copy_timings_drm_to_omap(&timings, mode);
+
+	DBG("%s: set mode: %d:\"%s\" %d %d %d %d %d %d %d %d %d %d 0x%x 0x%x",
+			omap_connector->dssdev->name,
+			mode->base.id, mode->name, mode->vrefresh, mode->clock,
+			mode->hdisplay, mode->hsync_start,
+			mode->hsync_end, mode->htotal,
+			mode->vdisplay, mode->vsync_start,
+			mode->vsync_end, mode->vtotal, mode->type, mode->flags);
+
+	if (dssdrv->check_timings(dssdev, &timings)) {
+		dev_err(dev->dev, "could not set timings\n");
+		return;
+	}
+
+	dssdrv->set_timings(dssdev, &timings);
+}
+
+/* flush an area of the framebuffer (in case of manual update display that
+ * is not automatically flushed)
+ */
+void omap_connector_flush(struct drm_connector *connector,
+		int x, int y, int w, int h)
+{
+	struct omap_connector *omap_connector = to_omap_connector(connector);
+
+	/* TODO: enable when supported in dss */
+	VERB("%s: %d,%d, %dx%d", omap_connector->dssdev->name, x, y, w, h);
+}
+
+/* initialize connector */
+struct drm_connector *omap_connector_init(struct drm_device *dev,
+		int connector_type, struct omap_dss_device *dssdev)
+{
+	struct drm_connector *connector = NULL;
+	struct omap_connector *omap_connector;
+
+	DBG("%s", dssdev->name);
+
+	omap_dss_get_device(dssdev);
+
+	omap_connector = kzalloc(sizeof(struct omap_connector), GFP_KERNEL);
+	if (!omap_connector) {
+		dev_err(dev->dev, "could not allocate connector\n");
+		goto fail;
+	}
+
+	omap_connector->dssdev = dssdev;
+	connector = &omap_connector->base;
+
+	drm_connector_init(dev, connector, &omap_connector_funcs,
+				connector_type);
+	drm_connector_helper_add(connector, &omap_connector_helper_funcs);
+
+#if 0 /* enable when dss2 supports hotplug */
+	if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_HPD)
+		connector->polled = 0;
+	else
+#endif
+		connector->polled = DRM_CONNECTOR_POLL_CONNECT |
+				DRM_CONNECTOR_POLL_DISCONNECT;
+
+	connector->interlace_allowed = 1;
+	connector->doublescan_allowed = 0;
+
+	drm_sysfs_connector_add(connector);
+
+	return connector;
+
+fail:
+	if (connector) {
+		omap_connector_destroy(connector);
+	}
+
+	return NULL;
+}
diff --git a/drivers/staging/omapdrm/omap_crtc.c b/drivers/staging/omapdrm/omap_crtc.c
new file mode 100644
index 000000000000..cffdf5e12394
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_crtc.c
@@ -0,0 +1,326 @@
+/*
+ * drivers/staging/omapdrm/omap_crtc.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "omap_drv.h"
+
+#include "drm_mode.h"
+#include "drm_crtc.h"
+#include "drm_crtc_helper.h"
+
+#define to_omap_crtc(x) container_of(x, struct omap_crtc, base)
+
+struct omap_crtc {
+	struct drm_crtc base;
+	struct omap_overlay *ovl;
+	struct omap_overlay_info info;
+	int id;
+
+	/* if there is a pending flip, this will be non-null: */
+	struct drm_pending_vblank_event *event;
+};
+
+/* push changes down to dss2 */
+static int commit(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	struct omap_overlay *ovl = omap_crtc->ovl;
+	struct omap_overlay_info *info = &omap_crtc->info;
+	int ret;
+
+	DBG("%s", omap_crtc->ovl->name);
+	DBG("%dx%d -> %dx%d (%d)", info->width, info->height, info->out_width,
+			info->out_height, info->screen_width);
+	DBG("%d,%d %08x", info->pos_x, info->pos_y, info->paddr);
+
+	/* NOTE: do we want to do this at all here, or just wait
+	 * for dpms(ON) since other CRTC's may not have their mode
+	 * set yet, so fb dimensions may still change..
+	 */
+	ret = ovl->set_overlay_info(ovl, info);
+	if (ret) {
+		dev_err(dev->dev, "could not set overlay info\n");
+		return ret;
+	}
+
+	/* our encoder doesn't necessarily get a commit() after this, in
+	 * particular in the dpms() and mode_set_base() cases, so force the
+	 * manager to update:
+	 *
+	 * could this be in the encoder somehow?
+	 */
+	if (ovl->manager) {
+		ret = ovl->manager->apply(ovl->manager);
+		if (ret) {
+			dev_err(dev->dev, "could not apply settings\n");
+			return ret;
+		}
+	}
+
+	if (info->enabled) {
+		omap_framebuffer_flush(crtc->fb, crtc->x, crtc->y,
+				crtc->fb->width, crtc->fb->height);
+	}
+
+	return 0;
+}
+
+/* update parameters that are dependent on the framebuffer dimensions and
+ * position within the fb that this crtc scans out from. This is called
+ * when framebuffer dimensions or x,y base may have changed, either due
+ * to our mode, or a change in another crtc that is scanning out of the
+ * same fb.
+ */
+static void update_scanout(struct drm_crtc *crtc)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	dma_addr_t paddr;
+	unsigned int screen_width;
+
+	omap_framebuffer_get_buffer(crtc->fb, crtc->x, crtc->y,
+			NULL, &paddr, &screen_width);
+
+	DBG("%s: %d,%d: %08x (%d)", omap_crtc->ovl->name,
+			crtc->x, crtc->y, (u32)paddr, screen_width);
+
+	omap_crtc->info.paddr = paddr;
+	omap_crtc->info.screen_width = screen_width;
+}
+
+static void omap_crtc_gamma_set(struct drm_crtc *crtc,
+		u16 *red, u16 *green, u16 *blue, uint32_t start, uint32_t size)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	DBG("%s", omap_crtc->ovl->name);
+}
+
+static void omap_crtc_destroy(struct drm_crtc *crtc)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	DBG("%s", omap_crtc->ovl->name);
+	drm_crtc_cleanup(crtc);
+	kfree(omap_crtc);
+}
+
+static void omap_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+
+	DBG("%s: %d", omap_crtc->ovl->name, mode);
+
+	if (mode == DRM_MODE_DPMS_ON) {
+		update_scanout(crtc);
+		omap_crtc->info.enabled = true;
+	} else {
+		omap_crtc->info.enabled = false;
+	}
+
+	WARN_ON(commit(crtc));
+}
+
+static bool omap_crtc_mode_fixup(struct drm_crtc *crtc,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	DBG("%s", omap_crtc->ovl->name);
+	return true;
+}
+
+static int omap_crtc_mode_set(struct drm_crtc *crtc,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode,
+			       int x, int y,
+			       struct drm_framebuffer *old_fb)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+
+	DBG("%s: %d,%d: %dx%d", omap_crtc->ovl->name, x, y,
+			mode->hdisplay, mode->vdisplay);
+
+	/* just use adjusted mode */
+	mode = adjusted_mode;
+
+	omap_crtc->info.width = mode->hdisplay;
+	omap_crtc->info.height = mode->vdisplay;
+	omap_crtc->info.out_width = mode->hdisplay;
+	omap_crtc->info.out_height = mode->vdisplay;
+	omap_crtc->info.color_mode = OMAP_DSS_COLOR_RGB24U;
+	omap_crtc->info.rotation_type = OMAP_DSS_ROT_DMA;
+	omap_crtc->info.rotation = OMAP_DSS_ROT_0;
+	omap_crtc->info.global_alpha = 0xff;
+	omap_crtc->info.mirror = 0;
+	omap_crtc->info.mirror = 0;
+	omap_crtc->info.pos_x = 0;
+	omap_crtc->info.pos_y = 0;
+#if 0 /* re-enable when these are available in DSS2 driver */
+	omap_crtc->info.zorder = 3;        /* GUI in the front, video behind */
+	omap_crtc->info.min_x_decim = 1;
+	omap_crtc->info.max_x_decim = 1;
+	omap_crtc->info.min_y_decim = 1;
+	omap_crtc->info.max_y_decim = 1;
+#endif
+
+	update_scanout(crtc);
+
+	return 0;
+}
+
+static void omap_crtc_prepare(struct drm_crtc *crtc)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	struct omap_overlay *ovl = omap_crtc->ovl;
+
+	DBG("%s", omap_crtc->ovl->name);
+
+	ovl->get_overlay_info(ovl, &omap_crtc->info);
+
+	omap_crtc_dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
+static void omap_crtc_commit(struct drm_crtc *crtc)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	DBG("%s", omap_crtc->ovl->name);
+	omap_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+static int omap_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
+		    struct drm_framebuffer *old_fb)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+
+	DBG("%s %d,%d: fb=%p", omap_crtc->ovl->name, x, y, old_fb);
+
+	update_scanout(crtc);
+
+	return commit(crtc);
+}
+
+static void omap_crtc_load_lut(struct drm_crtc *crtc)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	DBG("%s", omap_crtc->ovl->name);
+}
+
+static void page_flip_cb(void *arg)
+{
+	struct drm_crtc *crtc = arg;
+	struct drm_device *dev = crtc->dev;
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	struct drm_pending_vblank_event *event = omap_crtc->event;
+	struct timeval now;
+	unsigned long flags;
+
+	WARN_ON(!event);
+
+	omap_crtc->event = NULL;
+
+	update_scanout(crtc);
+	WARN_ON(commit(crtc));
+
+	/* wakeup userspace */
+	/* TODO: this should happen *after* flip in vsync IRQ handler */
+	if (event) {
+		spin_lock_irqsave(&dev->event_lock, flags);
+		event->event.sequence = drm_vblank_count_and_time(
+				dev, omap_crtc->id, &now);
+		event->event.tv_sec = now.tv_sec;
+		event->event.tv_usec = now.tv_usec;
+		list_add_tail(&event->base.link,
+				&event->base.file_priv->event_list);
+		wake_up_interruptible(&event->base.file_priv->event_wait);
+		spin_unlock_irqrestore(&dev->event_lock, flags);
+	}
+}
+
+static int omap_crtc_page_flip_locked(struct drm_crtc *crtc,
+		 struct drm_framebuffer *fb,
+		 struct drm_pending_vblank_event *event)
+{
+	struct drm_device *dev = crtc->dev;
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+
+	DBG("%d -> %d", crtc->fb ? crtc->fb->base.id : -1, fb->base.id);
+
+	if (omap_crtc->event) {
+		dev_err(dev->dev, "already a pending flip\n");
+		return -EINVAL;
+	}
+
+	crtc->fb = fb;
+	omap_crtc->event = event;
+
+	omap_gem_op_async(omap_framebuffer_bo(fb), OMAP_GEM_READ,
+			page_flip_cb, crtc);
+
+	return 0;
+}
+
+static const struct drm_crtc_funcs omap_crtc_funcs = {
+	.gamma_set = omap_crtc_gamma_set,
+	.set_config = drm_crtc_helper_set_config,
+	.destroy = omap_crtc_destroy,
+	.page_flip = omap_crtc_page_flip_locked,
+};
+
+static const struct drm_crtc_helper_funcs omap_crtc_helper_funcs = {
+	.dpms = omap_crtc_dpms,
+	.mode_fixup = omap_crtc_mode_fixup,
+	.mode_set = omap_crtc_mode_set,
+	.prepare = omap_crtc_prepare,
+	.commit = omap_crtc_commit,
+	.mode_set_base = omap_crtc_mode_set_base,
+	.load_lut = omap_crtc_load_lut,
+};
+
+struct omap_overlay *omap_crtc_get_overlay(struct drm_crtc *crtc)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	return omap_crtc->ovl;
+}
+
+/* initialize crtc */
+struct drm_crtc *omap_crtc_init(struct drm_device *dev,
+		struct omap_overlay *ovl, int id)
+{
+	struct drm_crtc *crtc = NULL;
+	struct omap_crtc *omap_crtc = kzalloc(sizeof(*omap_crtc), GFP_KERNEL);
+
+	DBG("%s", ovl->name);
+
+	if (!omap_crtc) {
+		dev_err(dev->dev, "could not allocate CRTC\n");
+		goto fail;
+	}
+
+	omap_crtc->ovl = ovl;
+	omap_crtc->id = id;
+	crtc = &omap_crtc->base;
+	drm_crtc_init(dev, crtc, &omap_crtc_funcs);
+	drm_crtc_helper_add(crtc, &omap_crtc_helper_funcs);
+
+	return crtc;
+
+fail:
+	if (crtc) {
+		omap_crtc_destroy(crtc);
+	}
+	return NULL;
+}
diff --git a/drivers/staging/omapdrm/omap_debugfs.c b/drivers/staging/omapdrm/omap_debugfs.c
new file mode 100644
index 000000000000..da920dfdc59c
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_debugfs.c
@@ -0,0 +1,42 @@
+/*
+ * drivers/staging/omapdrm/omap_debugfs.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob.clark@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "omap_drv.h"
+#include "omap_dmm_tiler.h"
+
+#ifdef CONFIG_DEBUG_FS
+
+static struct drm_info_list omap_debugfs_list[] = {
+	{"tiler_map", tiler_map_show, 0},
+};
+
+int omap_debugfs_init(struct drm_minor *minor)
+{
+	return drm_debugfs_create_files(omap_debugfs_list,
+			ARRAY_SIZE(omap_debugfs_list),
+			minor->debugfs_root, minor);
+}
+
+void omap_debugfs_cleanup(struct drm_minor *minor)
+{
+	drm_debugfs_remove_files(omap_debugfs_list,
+			ARRAY_SIZE(omap_debugfs_list), minor);
+}
+
+#endif
diff --git a/drivers/staging/omapdrm/omap_dmm_priv.h b/drivers/staging/omapdrm/omap_dmm_priv.h
new file mode 100644
index 000000000000..2f529ab4b7c7
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_dmm_priv.h
@@ -0,0 +1,187 @@
+/*
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Author: Rob Clark <rob@ti.com>
+ *         Andy Gross <andy.gross@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef OMAP_DMM_PRIV_H
+#define OMAP_DMM_PRIV_H
+
+#define DMM_REVISION          0x000
+#define DMM_HWINFO            0x004
+#define DMM_LISA_HWINFO       0x008
+#define DMM_DMM_SYSCONFIG     0x010
+#define DMM_LISA_LOCK         0x01C
+#define DMM_LISA_MAP__0       0x040
+#define DMM_LISA_MAP__1       0x044
+#define DMM_TILER_HWINFO      0x208
+#define DMM_TILER_OR__0       0x220
+#define DMM_TILER_OR__1       0x224
+#define DMM_PAT_HWINFO        0x408
+#define DMM_PAT_GEOMETRY      0x40C
+#define DMM_PAT_CONFIG        0x410
+#define DMM_PAT_VIEW__0       0x420
+#define DMM_PAT_VIEW__1       0x424
+#define DMM_PAT_VIEW_MAP__0   0x440
+#define DMM_PAT_VIEW_MAP_BASE 0x460
+#define DMM_PAT_IRQ_EOI       0x478
+#define DMM_PAT_IRQSTATUS_RAW 0x480
+#define DMM_PAT_IRQSTATUS     0x490
+#define DMM_PAT_IRQENABLE_SET 0x4A0
+#define DMM_PAT_IRQENABLE_CLR 0x4B0
+#define DMM_PAT_STATUS__0     0x4C0
+#define DMM_PAT_STATUS__1     0x4C4
+#define DMM_PAT_STATUS__2     0x4C8
+#define DMM_PAT_STATUS__3     0x4CC
+#define DMM_PAT_DESCR__0      0x500
+#define DMM_PAT_DESCR__1      0x510
+#define DMM_PAT_DESCR__2      0x520
+#define DMM_PAT_DESCR__3      0x530
+#define DMM_PEG_HWINFO        0x608
+#define DMM_PEG_PRIO          0x620
+#define DMM_PEG_PRIO_PAT      0x640
+
+#define DMM_IRQSTAT_DST			(1<<0)
+#define DMM_IRQSTAT_LST			(1<<1)
+#define DMM_IRQSTAT_ERR_INV_DSC		(1<<2)
+#define DMM_IRQSTAT_ERR_INV_DATA	(1<<3)
+#define DMM_IRQSTAT_ERR_UPD_AREA	(1<<4)
+#define DMM_IRQSTAT_ERR_UPD_CTRL	(1<<5)
+#define DMM_IRQSTAT_ERR_UPD_DATA	(1<<6)
+#define DMM_IRQSTAT_ERR_LUT_MISS	(1<<7)
+
+#define DMM_IRQSTAT_ERR_MASK	(DMM_IRQ_STAT_ERR_INV_DSC | \
+				DMM_IRQ_STAT_ERR_INV_DATA | \
+				DMM_IRQ_STAT_ERR_UPD_AREA | \
+				DMM_IRQ_STAT_ERR_UPD_CTRL | \
+				DMM_IRQ_STAT_ERR_UPD_DATA | \
+				DMM_IRQ_STAT_ERR_LUT_MISS)
+
+#define DMM_PATSTATUS_READY		(1<<0)
+#define DMM_PATSTATUS_VALID		(1<<1)
+#define DMM_PATSTATUS_RUN		(1<<2)
+#define DMM_PATSTATUS_DONE		(1<<3)
+#define DMM_PATSTATUS_LINKED		(1<<4)
+#define DMM_PATSTATUS_BYPASSED		(1<<7)
+#define DMM_PATSTATUS_ERR_INV_DESCR	(1<<10)
+#define DMM_PATSTATUS_ERR_INV_DATA	(1<<11)
+#define DMM_PATSTATUS_ERR_UPD_AREA	(1<<12)
+#define DMM_PATSTATUS_ERR_UPD_CTRL	(1<<13)
+#define DMM_PATSTATUS_ERR_UPD_DATA	(1<<14)
+#define DMM_PATSTATUS_ERR_ACCESS	(1<<15)
+
+/* note: don't treat DMM_PATSTATUS_ERR_ACCESS as an error */
+#define DMM_PATSTATUS_ERR	(DMM_PATSTATUS_ERR_INV_DESCR | \
+				DMM_PATSTATUS_ERR_INV_DATA | \
+				DMM_PATSTATUS_ERR_UPD_AREA | \
+				DMM_PATSTATUS_ERR_UPD_CTRL | \
+				DMM_PATSTATUS_ERR_UPD_DATA)
+
+
+
+enum {
+	PAT_STATUS,
+	PAT_DESCR
+};
+
+struct pat_ctrl {
+	u32 start:4;
+	u32 dir:4;
+	u32 lut_id:8;
+	u32 sync:12;
+	u32 ini:4;
+};
+
+struct pat {
+	uint32_t next_pa;
+	struct pat_area area;
+	struct pat_ctrl ctrl;
+	uint32_t data_pa;
+};
+
+#define DMM_FIXED_RETRY_COUNT 1000
+
+/* create refill buffer big enough to refill all slots, plus 3 descriptors..
+ * 3 descriptors is probably the worst-case for # of 2d-slices in a 1d area,
+ * but I guess you don't hit that worst case at the same time as full area
+ * refill
+ */
+#define DESCR_SIZE 128
+#define REFILL_BUFFER_SIZE ((4 * 128 * 256) + (3 * DESCR_SIZE))
+
+struct dmm;
+
+struct dmm_txn {
+	void *engine_handle;
+	struct tcm *tcm;
+
+	uint8_t *current_va;
+	dma_addr_t current_pa;
+
+	struct pat *last_pat;
+};
+
+struct refill_engine {
+	int id;
+	struct dmm *dmm;
+	struct tcm *tcm;
+
+	uint8_t *refill_va;
+	dma_addr_t refill_pa;
+
+	/* only one trans per engine for now */
+	struct dmm_txn txn;
+
+	/* offset to lut associated with container */
+	u32 *lut_offset;
+
+	wait_queue_head_t wait_for_refill;
+
+	struct list_head idle_node;
+};
+
+struct dmm {
+	struct device *dev;
+	void __iomem *base;
+	int irq;
+
+	struct page *dummy_page;
+	dma_addr_t dummy_pa;
+
+	void *refill_va;
+	dma_addr_t refill_pa;
+
+	/* refill engines */
+	struct semaphore engine_sem;
+	struct list_head idle_head;
+	struct refill_engine *engines;
+	int num_engines;
+
+	/* container information */
+	int container_width;
+	int container_height;
+	int lut_width;
+	int lut_height;
+	int num_lut;
+
+	/* array of LUT - TCM containers */
+	struct tcm **tcm;
+
+	/* LUT table storage */
+	u32 *lut;
+
+	/* allocation list and lock */
+	struct list_head alloc_head;
+	spinlock_t list_lock;
+};
+
+#endif
diff --git a/drivers/staging/omapdrm/omap_dmm_tiler.c b/drivers/staging/omapdrm/omap_dmm_tiler.c
new file mode 100644
index 000000000000..852d9440f725
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_dmm_tiler.c
@@ -0,0 +1,830 @@
+/*
+ * DMM IOMMU driver support functions for TI OMAP processors.
+ *
+ * Author: Rob Clark <rob@ti.com>
+ *         Andy Gross <andy.gross@ti.com>
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h> /* platform_device() */
+#include <linux/errno.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/time.h>
+#include <linux/list.h>
+#include <linux/semaphore.h>
+
+#include "omap_dmm_tiler.h"
+#include "omap_dmm_priv.h"
+
+/* mappings for associating views to luts */
+static struct tcm *containers[TILFMT_NFORMATS];
+static struct dmm *omap_dmm;
+
+/* Geometry table */
+#define GEOM(xshift, yshift, bytes_per_pixel) { \
+		.x_shft = (xshift), \
+		.y_shft = (yshift), \
+		.cpp    = (bytes_per_pixel), \
+		.slot_w = 1 << (SLOT_WIDTH_BITS - (xshift)), \
+		.slot_h = 1 << (SLOT_HEIGHT_BITS - (yshift)), \
+	}
+
+static const struct {
+	uint32_t x_shft;	/* unused X-bits (as part of bpp) */
+	uint32_t y_shft;	/* unused Y-bits (as part of bpp) */
+	uint32_t cpp;		/* bytes/chars per pixel */
+	uint32_t slot_w;	/* width of each slot (in pixels) */
+	uint32_t slot_h;	/* height of each slot (in pixels) */
+} geom[TILFMT_NFORMATS] = {
+		[TILFMT_8BIT]  = GEOM(0, 0, 1),
+		[TILFMT_16BIT] = GEOM(0, 1, 2),
+		[TILFMT_32BIT] = GEOM(1, 1, 4),
+		[TILFMT_PAGE]  = GEOM(SLOT_WIDTH_BITS, SLOT_HEIGHT_BITS, 1),
+};
+
+
+/* lookup table for registers w/ per-engine instances */
+static const uint32_t reg[][4] = {
+		[PAT_STATUS] = {DMM_PAT_STATUS__0, DMM_PAT_STATUS__1,
+				DMM_PAT_STATUS__2, DMM_PAT_STATUS__3},
+		[PAT_DESCR]  = {DMM_PAT_DESCR__0, DMM_PAT_DESCR__1,
+				DMM_PAT_DESCR__2, DMM_PAT_DESCR__3},
+};
+
+/* simple allocator to grab next 16 byte aligned memory from txn */
+static void *alloc_dma(struct dmm_txn *txn, size_t sz, dma_addr_t *pa)
+{
+	void *ptr;
+	struct refill_engine *engine = txn->engine_handle;
+
+	/* dmm programming requires 16 byte aligned addresses */
+	txn->current_pa = round_up(txn->current_pa, 16);
+	txn->current_va = (void *)round_up((long)txn->current_va, 16);
+
+	ptr = txn->current_va;
+	*pa = txn->current_pa;
+
+	txn->current_pa += sz;
+	txn->current_va += sz;
+
+	BUG_ON((txn->current_va - engine->refill_va) > REFILL_BUFFER_SIZE);
+
+	return ptr;
+}
+
+/* check status and spin until wait_mask comes true */
+static int wait_status(struct refill_engine *engine, uint32_t wait_mask)
+{
+	struct dmm *dmm = engine->dmm;
+	uint32_t r = 0, err, i;
+
+	i = DMM_FIXED_RETRY_COUNT;
+	while (true) {
+		r = readl(dmm->base + reg[PAT_STATUS][engine->id]);
+		err = r & DMM_PATSTATUS_ERR;
+		if (err)
+			return -EFAULT;
+
+		if ((r & wait_mask) == wait_mask)
+			break;
+
+		if (--i == 0)
+			return -ETIMEDOUT;
+
+		udelay(1);
+	}
+
+	return 0;
+}
+
+irqreturn_t omap_dmm_irq_handler(int irq, void *arg)
+{
+	struct dmm *dmm = arg;
+	uint32_t status = readl(dmm->base + DMM_PAT_IRQSTATUS);
+	int i;
+
+	/* ack IRQ */
+	writel(status, dmm->base + DMM_PAT_IRQSTATUS);
+
+	for (i = 0; i < dmm->num_engines; i++) {
+		if (status & DMM_IRQSTAT_LST)
+			wake_up_interruptible(&dmm->engines[i].wait_for_refill);
+
+		status >>= 8;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * Get a handle for a DMM transaction
+ */
+static struct dmm_txn *dmm_txn_init(struct dmm *dmm, struct tcm *tcm)
+{
+	struct dmm_txn *txn = NULL;
+	struct refill_engine *engine = NULL;
+
+	down(&dmm->engine_sem);
+
+	/* grab an idle engine */
+	spin_lock(&dmm->list_lock);
+	if (!list_empty(&dmm->idle_head)) {
+		engine = list_entry(dmm->idle_head.next, struct refill_engine,
+					idle_node);
+		list_del(&engine->idle_node);
+	}
+	spin_unlock(&dmm->list_lock);
+
+	BUG_ON(!engine);
+
+	txn = &engine->txn;
+	engine->tcm = tcm;
+	txn->engine_handle = engine;
+	txn->last_pat = NULL;
+	txn->current_va = engine->refill_va;
+	txn->current_pa = engine->refill_pa;
+
+	return txn;
+}
+
+/**
+ * Add region to DMM transaction.  If pages or pages[i] is NULL, then the
+ * corresponding slot is cleared (ie. dummy_pa is programmed)
+ */
+static int dmm_txn_append(struct dmm_txn *txn, struct pat_area *area,
+		struct page **pages, uint32_t npages, uint32_t roll)
+{
+	dma_addr_t pat_pa = 0;
+	uint32_t *data;
+	struct pat *pat;
+	struct refill_engine *engine = txn->engine_handle;
+	int columns = (1 + area->x1 - area->x0);
+	int rows = (1 + area->y1 - area->y0);
+	int i = columns*rows;
+	u32 *lut = omap_dmm->lut + (engine->tcm->lut_id * omap_dmm->lut_width *
+			omap_dmm->lut_height) +
+			(area->y0 * omap_dmm->lut_width) + area->x0;
+
+	pat = alloc_dma(txn, sizeof(struct pat), &pat_pa);
+
+	if (txn->last_pat)
+		txn->last_pat->next_pa = (uint32_t)pat_pa;
+
+	pat->area = *area;
+	pat->ctrl = (struct pat_ctrl){
+			.start = 1,
+			.lut_id = engine->tcm->lut_id,
+		};
+
+	data = alloc_dma(txn, 4*i, &pat->data_pa);
+
+	while (i--) {
+		int n = i + roll;
+		if (n >= npages)
+			n -= npages;
+		data[i] = (pages && pages[n]) ?
+			page_to_phys(pages[n]) : engine->dmm->dummy_pa;
+	}
+
+	/* fill in lut with new addresses */
+	for (i = 0; i < rows; i++, lut += omap_dmm->lut_width)
+		memcpy(lut, &data[i*columns], columns * sizeof(u32));
+
+	txn->last_pat = pat;
+
+	return 0;
+}
+
+/**
+ * Commit the DMM transaction.
+ */
+static int dmm_txn_commit(struct dmm_txn *txn, bool wait)
+{
+	int ret = 0;
+	struct refill_engine *engine = txn->engine_handle;
+	struct dmm *dmm = engine->dmm;
+
+	if (!txn->last_pat) {
+		dev_err(engine->dmm->dev, "need at least one txn\n");
+		ret = -EINVAL;
+		goto cleanup;
+	}
+
+	txn->last_pat->next_pa = 0;
+
+	/* write to PAT_DESCR to clear out any pending transaction */
+	writel(0x0, dmm->base + reg[PAT_DESCR][engine->id]);
+
+	/* wait for engine ready: */
+	ret = wait_status(engine, DMM_PATSTATUS_READY);
+	if (ret) {
+		ret = -EFAULT;
+		goto cleanup;
+	}
+
+	/* kick reload */
+	writel(engine->refill_pa,
+		dmm->base + reg[PAT_DESCR][engine->id]);
+
+	if (wait) {
+		if (wait_event_interruptible_timeout(engine->wait_for_refill,
+				wait_status(engine, DMM_PATSTATUS_READY) == 0,
+				msecs_to_jiffies(1)) <= 0) {
+			dev_err(dmm->dev, "timed out waiting for done\n");
+			ret = -ETIMEDOUT;
+		}
+	}
+
+cleanup:
+	spin_lock(&dmm->list_lock);
+	list_add(&engine->idle_node, &dmm->idle_head);
+	spin_unlock(&dmm->list_lock);
+
+	up(&omap_dmm->engine_sem);
+	return ret;
+}
+
+/*
+ * DMM programming
+ */
+static int fill(struct tcm_area *area, struct page **pages,
+		uint32_t npages, uint32_t roll, bool wait)
+{
+	int ret = 0;
+	struct tcm_area slice, area_s;
+	struct dmm_txn *txn;
+
+	txn = dmm_txn_init(omap_dmm, area->tcm);
+	if (IS_ERR_OR_NULL(txn))
+		return PTR_ERR(txn);
+
+	tcm_for_each_slice(slice, *area, area_s) {
+		struct pat_area p_area = {
+				.x0 = slice.p0.x,  .y0 = slice.p0.y,
+				.x1 = slice.p1.x,  .y1 = slice.p1.y,
+		};
+
+		ret = dmm_txn_append(txn, &p_area, pages, npages, roll);
+		if (ret)
+			goto fail;
+
+		roll += tcm_sizeof(slice);
+	}
+
+	ret = dmm_txn_commit(txn, wait);
+
+fail:
+	return ret;
+}
+
+/*
+ * Pin/unpin
+ */
+
+/* note: slots for which pages[i] == NULL are filled w/ dummy page
+ */
+int tiler_pin(struct tiler_block *block, struct page **pages,
+		uint32_t npages, uint32_t roll, bool wait)
+{
+	int ret;
+
+	ret = fill(&block->area, pages, npages, roll, wait);
+
+	if (ret)
+		tiler_unpin(block);
+
+	return ret;
+}
+
+int tiler_unpin(struct tiler_block *block)
+{
+	return fill(&block->area, NULL, 0, 0, false);
+}
+
+/*
+ * Reserve/release
+ */
+struct tiler_block *tiler_reserve_2d(enum tiler_fmt fmt, uint16_t w,
+		uint16_t h, uint16_t align)
+{
+	struct tiler_block *block = kzalloc(sizeof(*block), GFP_KERNEL);
+	u32 min_align = 128;
+	int ret;
+
+	BUG_ON(!validfmt(fmt));
+
+	/* convert width/height to slots */
+	w = DIV_ROUND_UP(w, geom[fmt].slot_w);
+	h = DIV_ROUND_UP(h, geom[fmt].slot_h);
+
+	/* convert alignment to slots */
+	min_align = max(min_align, (geom[fmt].slot_w * geom[fmt].cpp));
+	align = ALIGN(align, min_align);
+	align /= geom[fmt].slot_w * geom[fmt].cpp;
+
+	block->fmt = fmt;
+
+	ret = tcm_reserve_2d(containers[fmt], w, h, align, &block->area);
+	if (ret) {
+		kfree(block);
+		return 0;
+	}
+
+	/* add to allocation list */
+	spin_lock(&omap_dmm->list_lock);
+	list_add(&block->alloc_node, &omap_dmm->alloc_head);
+	spin_unlock(&omap_dmm->list_lock);
+
+	return block;
+}
+
+struct tiler_block *tiler_reserve_1d(size_t size)
+{
+	struct tiler_block *block = kzalloc(sizeof(*block), GFP_KERNEL);
+	int num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
+
+	if (!block)
+		return 0;
+
+	block->fmt = TILFMT_PAGE;
+
+	if (tcm_reserve_1d(containers[TILFMT_PAGE], num_pages,
+				&block->area)) {
+		kfree(block);
+		return 0;
+	}
+
+	spin_lock(&omap_dmm->list_lock);
+	list_add(&block->alloc_node, &omap_dmm->alloc_head);
+	spin_unlock(&omap_dmm->list_lock);
+
+	return block;
+}
+
+/* note: if you have pin'd pages, you should have already unpin'd first! */
+int tiler_release(struct tiler_block *block)
+{
+	int ret = tcm_free(&block->area);
+
+	if (block->area.tcm)
+		dev_err(omap_dmm->dev, "failed to release block\n");
+
+	spin_lock(&omap_dmm->list_lock);
+	list_del(&block->alloc_node);
+	spin_unlock(&omap_dmm->list_lock);
+
+	kfree(block);
+	return ret;
+}
+
+/*
+ * Utils
+ */
+
+/* calculate the tiler space address of a pixel in a view orientation */
+static u32 tiler_get_address(u32 orient, enum tiler_fmt fmt, u32 x, u32 y)
+{
+	u32 x_bits, y_bits, tmp, x_mask, y_mask, alignment;
+
+	x_bits = CONT_WIDTH_BITS - geom[fmt].x_shft;
+	y_bits = CONT_HEIGHT_BITS - geom[fmt].y_shft;
+	alignment = geom[fmt].x_shft + geom[fmt].y_shft;
+
+	/* validate coordinate */
+	x_mask = MASK(x_bits);
+	y_mask = MASK(y_bits);
+
+	if (x < 0 || x > x_mask || y < 0 || y > y_mask)
+		return 0;
+
+	/* account for mirroring */
+	if (orient & MASK_X_INVERT)
+		x ^= x_mask;
+	if (orient & MASK_Y_INVERT)
+		y ^= y_mask;
+
+	/* get coordinate address */
+	if (orient & MASK_XY_FLIP)
+		tmp = ((x << y_bits) + y);
+	else
+		tmp = ((y << x_bits) + x);
+
+	return TIL_ADDR((tmp << alignment), orient, fmt);
+}
+
+dma_addr_t tiler_ssptr(struct tiler_block *block)
+{
+	BUG_ON(!validfmt(block->fmt));
+
+	return TILVIEW_8BIT + tiler_get_address(0, block->fmt,
+			block->area.p0.x * geom[block->fmt].slot_w,
+			block->area.p0.y * geom[block->fmt].slot_h);
+}
+
+void tiler_align(enum tiler_fmt fmt, uint16_t *w, uint16_t *h)
+{
+	BUG_ON(!validfmt(fmt));
+	*w = round_up(*w, geom[fmt].slot_w);
+	*h = round_up(*h, geom[fmt].slot_h);
+}
+
+uint32_t tiler_stride(enum tiler_fmt fmt)
+{
+	BUG_ON(!validfmt(fmt));
+
+	return 1 << (CONT_WIDTH_BITS + geom[fmt].y_shft);
+}
+
+size_t tiler_size(enum tiler_fmt fmt, uint16_t w, uint16_t h)
+{
+	tiler_align(fmt, &w, &h);
+	return geom[fmt].cpp * w * h;
+}
+
+size_t tiler_vsize(enum tiler_fmt fmt, uint16_t w, uint16_t h)
+{
+	BUG_ON(!validfmt(fmt));
+	return round_up(geom[fmt].cpp * w, PAGE_SIZE) * h;
+}
+
+int omap_dmm_remove(void)
+{
+	struct tiler_block *block, *_block;
+	int i;
+
+	if (omap_dmm) {
+		/* free all area regions */
+		spin_lock(&omap_dmm->list_lock);
+		list_for_each_entry_safe(block, _block, &omap_dmm->alloc_head,
+					alloc_node) {
+			list_del(&block->alloc_node);
+			kfree(block);
+		}
+		spin_unlock(&omap_dmm->list_lock);
+
+		for (i = 0; i < omap_dmm->num_lut; i++)
+			if (omap_dmm->tcm && omap_dmm->tcm[i])
+				omap_dmm->tcm[i]->deinit(omap_dmm->tcm[i]);
+		kfree(omap_dmm->tcm);
+
+		kfree(omap_dmm->engines);
+		if (omap_dmm->refill_va)
+			dma_free_coherent(omap_dmm->dev,
+				REFILL_BUFFER_SIZE * omap_dmm->num_engines,
+				omap_dmm->refill_va,
+				omap_dmm->refill_pa);
+		if (omap_dmm->dummy_page)
+			__free_page(omap_dmm->dummy_page);
+
+		vfree(omap_dmm->lut);
+
+		if (omap_dmm->irq != -1)
+			free_irq(omap_dmm->irq, omap_dmm);
+
+		kfree(omap_dmm);
+	}
+
+	return 0;
+}
+
+int omap_dmm_init(struct drm_device *dev)
+{
+	int ret = -EFAULT, i;
+	struct tcm_area area = {0};
+	u32 hwinfo, pat_geom, lut_table_size;
+	struct omap_drm_platform_data *pdata = dev->dev->platform_data;
+
+	if (!pdata || !pdata->dmm_pdata) {
+		dev_err(dev->dev, "dmm platform data not present, skipping\n");
+		return ret;
+	}
+
+	omap_dmm = kzalloc(sizeof(*omap_dmm), GFP_KERNEL);
+	if (!omap_dmm) {
+		dev_err(dev->dev, "failed to allocate driver data section\n");
+		goto fail;
+	}
+
+	/* lookup hwmod data - base address and irq */
+	omap_dmm->base = pdata->dmm_pdata->base;
+	omap_dmm->irq = pdata->dmm_pdata->irq;
+	omap_dmm->dev = dev->dev;
+
+	if (!omap_dmm->base) {
+		dev_err(dev->dev, "failed to get dmm base address\n");
+		goto fail;
+	}
+
+	hwinfo = readl(omap_dmm->base + DMM_PAT_HWINFO);
+	omap_dmm->num_engines = (hwinfo >> 24) & 0x1F;
+	omap_dmm->num_lut = (hwinfo >> 16) & 0x1F;
+	omap_dmm->container_width = 256;
+	omap_dmm->container_height = 128;
+
+	/* read out actual LUT width and height */
+	pat_geom = readl(omap_dmm->base + DMM_PAT_GEOMETRY);
+	omap_dmm->lut_width = ((pat_geom >> 16) & 0xF) << 5;
+	omap_dmm->lut_height = ((pat_geom >> 24) & 0xF) << 5;
+
+	/* initialize DMM registers */
+	writel(0x88888888, omap_dmm->base + DMM_PAT_VIEW__0);
+	writel(0x88888888, omap_dmm->base + DMM_PAT_VIEW__1);
+	writel(0x80808080, omap_dmm->base + DMM_PAT_VIEW_MAP__0);
+	writel(0x80000000, omap_dmm->base + DMM_PAT_VIEW_MAP_BASE);
+	writel(0x88888888, omap_dmm->base + DMM_TILER_OR__0);
+	writel(0x88888888, omap_dmm->base + DMM_TILER_OR__1);
+
+	ret = request_irq(omap_dmm->irq, omap_dmm_irq_handler, IRQF_SHARED,
+				"omap_dmm_irq_handler", omap_dmm);
+
+	if (ret) {
+		dev_err(dev->dev, "couldn't register IRQ %d, error %d\n",
+			omap_dmm->irq, ret);
+		omap_dmm->irq = -1;
+		goto fail;
+	}
+
+	/* Enable all interrupts for each refill engine except
+	 * ERR_LUT_MISS<n> (which is just advisory, and we don't care
+	 * about because we want to be able to refill live scanout
+	 * buffers for accelerated pan/scroll) and FILL_DSC<n> which
+	 * we just generally don't care about.
+	 */
+	writel(0x7e7e7e7e, omap_dmm->base + DMM_PAT_IRQENABLE_SET);
+
+	lut_table_size = omap_dmm->lut_width * omap_dmm->lut_height *
+			omap_dmm->num_lut;
+
+	omap_dmm->lut = vmalloc(lut_table_size * sizeof(*omap_dmm->lut));
+	if (!omap_dmm->lut) {
+		dev_err(dev->dev, "could not allocate lut table\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	omap_dmm->dummy_page = alloc_page(GFP_KERNEL | __GFP_DMA32);
+	if (!omap_dmm->dummy_page) {
+		dev_err(dev->dev, "could not allocate dummy page\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+	omap_dmm->dummy_pa = page_to_phys(omap_dmm->dummy_page);
+
+	/* alloc refill memory */
+	omap_dmm->refill_va = dma_alloc_coherent(dev->dev,
+				REFILL_BUFFER_SIZE * omap_dmm->num_engines,
+				&omap_dmm->refill_pa, GFP_KERNEL);
+	if (!omap_dmm->refill_va) {
+		dev_err(dev->dev, "could not allocate refill memory\n");
+		goto fail;
+	}
+
+	/* alloc engines */
+	omap_dmm->engines = kzalloc(
+			omap_dmm->num_engines * sizeof(struct refill_engine),
+			GFP_KERNEL);
+	if (!omap_dmm->engines) {
+		dev_err(dev->dev, "could not allocate engines\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	sema_init(&omap_dmm->engine_sem, omap_dmm->num_engines);
+	INIT_LIST_HEAD(&omap_dmm->idle_head);
+	for (i = 0; i < omap_dmm->num_engines; i++) {
+		omap_dmm->engines[i].id = i;
+		omap_dmm->engines[i].dmm = omap_dmm;
+		omap_dmm->engines[i].refill_va = omap_dmm->refill_va +
+						(REFILL_BUFFER_SIZE * i);
+		omap_dmm->engines[i].refill_pa = omap_dmm->refill_pa +
+						(REFILL_BUFFER_SIZE * i);
+		init_waitqueue_head(&omap_dmm->engines[i].wait_for_refill);
+
+		list_add(&omap_dmm->engines[i].idle_node, &omap_dmm->idle_head);
+	}
+
+	omap_dmm->tcm = kzalloc(omap_dmm->num_lut * sizeof(*omap_dmm->tcm),
+				GFP_KERNEL);
+	if (!omap_dmm->tcm) {
+		dev_err(dev->dev, "failed to allocate lut ptrs\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	/* init containers */
+	for (i = 0; i < omap_dmm->num_lut; i++) {
+		omap_dmm->tcm[i] = sita_init(omap_dmm->container_width,
+						omap_dmm->container_height,
+						NULL);
+
+		if (!omap_dmm->tcm[i]) {
+			dev_err(dev->dev, "failed to allocate container\n");
+			ret = -ENOMEM;
+			goto fail;
+		}
+
+		omap_dmm->tcm[i]->lut_id = i;
+	}
+
+	/* assign access mode containers to applicable tcm container */
+	/* OMAP 4 has 1 container for all 4 views */
+	containers[TILFMT_8BIT] = omap_dmm->tcm[0];
+	containers[TILFMT_16BIT] = omap_dmm->tcm[0];
+	containers[TILFMT_32BIT] = omap_dmm->tcm[0];
+	containers[TILFMT_PAGE] = omap_dmm->tcm[0];
+
+	INIT_LIST_HEAD(&omap_dmm->alloc_head);
+	spin_lock_init(&omap_dmm->list_lock);
+
+	area = (struct tcm_area) {
+		.is2d = true,
+		.tcm = NULL,
+		.p1.x = omap_dmm->container_width - 1,
+		.p1.y = omap_dmm->container_height - 1,
+	};
+
+	for (i = 0; i < lut_table_size; i++)
+		omap_dmm->lut[i] = omap_dmm->dummy_pa;
+
+	/* initialize all LUTs to dummy page entries */
+	for (i = 0; i < omap_dmm->num_lut; i++) {
+		area.tcm = omap_dmm->tcm[i];
+		if (fill(&area, NULL, 0, 0, true))
+			dev_err(omap_dmm->dev, "refill failed");
+	}
+
+	dev_info(omap_dmm->dev, "initialized all PAT entries\n");
+
+	return 0;
+
+fail:
+	omap_dmm_remove();
+	return ret;
+}
+
+/*
+ * debugfs support
+ */
+
+#ifdef CONFIG_DEBUG_FS
+
+static const char *alphabet = "abcdefghijklmnopqrstuvwxyz"
+				"ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789";
+static const char *special = ".,:;'\"`~!^-+";
+
+static void fill_map(char **map, int xdiv, int ydiv, struct tcm_area *a,
+							char c, bool ovw)
+{
+	int x, y;
+	for (y = a->p0.y / ydiv; y <= a->p1.y / ydiv; y++)
+		for (x = a->p0.x / xdiv; x <= a->p1.x / xdiv; x++)
+			if (map[y][x] == ' ' || ovw)
+				map[y][x] = c;
+}
+
+static void fill_map_pt(char **map, int xdiv, int ydiv, struct tcm_pt *p,
+									char c)
+{
+	map[p->y / ydiv][p->x / xdiv] = c;
+}
+
+static char read_map_pt(char **map, int xdiv, int ydiv, struct tcm_pt *p)
+{
+	return map[p->y / ydiv][p->x / xdiv];
+}
+
+static int map_width(int xdiv, int x0, int x1)
+{
+	return (x1 / xdiv) - (x0 / xdiv) + 1;
+}
+
+static void text_map(char **map, int xdiv, char *nice, int yd, int x0, int x1)
+{
+	char *p = map[yd] + (x0 / xdiv);
+	int w = (map_width(xdiv, x0, x1) - strlen(nice)) / 2;
+	if (w >= 0) {
+		p += w;
+		while (*nice)
+			*p++ = *nice++;
+	}
+}
+
+static void map_1d_info(char **map, int xdiv, int ydiv, char *nice,
+							struct tcm_area *a)
+{
+	sprintf(nice, "%dK", tcm_sizeof(*a) * 4);
+	if (a->p0.y + 1 < a->p1.y) {
+		text_map(map, xdiv, nice, (a->p0.y + a->p1.y) / 2 / ydiv, 0,
+							256 - 1);
+	} else if (a->p0.y < a->p1.y) {
+		if (strlen(nice) < map_width(xdiv, a->p0.x, 256 - 1))
+			text_map(map, xdiv, nice, a->p0.y / ydiv,
+					a->p0.x + xdiv,	256 - 1);
+		else if (strlen(nice) < map_width(xdiv, 0, a->p1.x))
+			text_map(map, xdiv, nice, a->p1.y / ydiv,
+					0, a->p1.y - xdiv);
+	} else if (strlen(nice) + 1 < map_width(xdiv, a->p0.x, a->p1.x)) {
+		text_map(map, xdiv, nice, a->p0.y / ydiv, a->p0.x, a->p1.x);
+	}
+}
+
+static void map_2d_info(char **map, int xdiv, int ydiv, char *nice,
+							struct tcm_area *a)
+{
+	sprintf(nice, "(%d*%d)", tcm_awidth(*a), tcm_aheight(*a));
+	if (strlen(nice) + 1 < map_width(xdiv, a->p0.x, a->p1.x))
+		text_map(map, xdiv, nice, (a->p0.y + a->p1.y) / 2 / ydiv,
+							a->p0.x, a->p1.x);
+}
+
+int tiler_map_show(struct seq_file *s, void *arg)
+{
+	int xdiv = 2, ydiv = 1;
+	char **map = NULL, *global_map;
+	struct tiler_block *block;
+	struct tcm_area a, p;
+	int i;
+	const char *m2d = alphabet;
+	const char *a2d = special;
+	const char *m2dp = m2d, *a2dp = a2d;
+	char nice[128];
+	int h_adj = omap_dmm->lut_height / ydiv;
+	int w_adj = omap_dmm->lut_width / xdiv;
+	unsigned long flags;
+
+	map = kzalloc(h_adj * sizeof(*map), GFP_KERNEL);
+	global_map = kzalloc((w_adj + 1) * h_adj, GFP_KERNEL);
+
+	if (!map || !global_map)
+		goto error;
+
+	memset(global_map, ' ', (w_adj + 1) * h_adj);
+	for (i = 0; i < omap_dmm->lut_height; i++) {
+		map[i] = global_map + i * (w_adj + 1);
+		map[i][w_adj] = 0;
+	}
+	spin_lock_irqsave(&omap_dmm->list_lock, flags);
+
+	list_for_each_entry(block, &omap_dmm->alloc_head, alloc_node) {
+		if (block->fmt != TILFMT_PAGE) {
+			fill_map(map, xdiv, ydiv, &block->area, *m2dp, true);
+			if (!*++a2dp)
+				a2dp = a2d;
+			if (!*++m2dp)
+				m2dp = m2d;
+			map_2d_info(map, xdiv, ydiv, nice, &block->area);
+		} else {
+			bool start = read_map_pt(map, xdiv, ydiv,
+							&block->area.p0)
+									== ' ';
+			bool end = read_map_pt(map, xdiv, ydiv, &block->area.p1)
+									== ' ';
+			tcm_for_each_slice(a, block->area, p)
+				fill_map(map, xdiv, ydiv, &a, '=', true);
+			fill_map_pt(map, xdiv, ydiv, &block->area.p0,
+							start ? '<' : 'X');
+			fill_map_pt(map, xdiv, ydiv, &block->area.p1,
+							end ? '>' : 'X');
+			map_1d_info(map, xdiv, ydiv, nice, &block->area);
+		}
+	}
+
+	spin_unlock_irqrestore(&omap_dmm->list_lock, flags);
+
+	if (s) {
+		seq_printf(s, "BEGIN DMM TILER MAP\n");
+		for (i = 0; i < 128; i++)
+			seq_printf(s, "%03d:%s\n", i, map[i]);
+		seq_printf(s, "END TILER MAP\n");
+	} else {
+		dev_dbg(omap_dmm->dev, "BEGIN DMM TILER MAP\n");
+		for (i = 0; i < 128; i++)
+			dev_dbg(omap_dmm->dev, "%03d:%s\n", i, map[i]);
+		dev_dbg(omap_dmm->dev, "END TILER MAP\n");
+	}
+
+error:
+	kfree(map);
+	kfree(global_map);
+
+	return 0;
+}
+#endif
diff --git a/drivers/staging/omapdrm/omap_dmm_tiler.h b/drivers/staging/omapdrm/omap_dmm_tiler.h
new file mode 100644
index 000000000000..f87cb657d683
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_dmm_tiler.h
@@ -0,0 +1,135 @@
+/*
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Author: Rob Clark <rob@ti.com>
+ *         Andy Gross <andy.gross@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#ifndef OMAP_DMM_TILER_H
+#define OMAP_DMM_TILER_H
+
+#include "omap_drv.h"
+#include "tcm.h"
+
+enum tiler_fmt {
+	TILFMT_8BIT = 0,
+	TILFMT_16BIT,
+	TILFMT_32BIT,
+	TILFMT_PAGE,
+	TILFMT_NFORMATS
+};
+
+struct pat_area {
+	u32 x0:8;
+	u32 y0:8;
+	u32 x1:8;
+	u32 y1:8;
+};
+
+struct tiler_block {
+	struct list_head alloc_node;	/* node for global block list */
+	struct tcm_area area;		/* area */
+	enum tiler_fmt fmt;		/* format */
+};
+
+/* bits representing the same slot in DMM-TILER hw-block */
+#define SLOT_WIDTH_BITS         6
+#define SLOT_HEIGHT_BITS        6
+
+/* bits reserved to describe coordinates in DMM-TILER hw-block */
+#define CONT_WIDTH_BITS         14
+#define CONT_HEIGHT_BITS        13
+
+/* calculated constants */
+#define TILER_PAGE              (1 << (SLOT_WIDTH_BITS + SLOT_HEIGHT_BITS))
+#define TILER_WIDTH             (1 << (CONT_WIDTH_BITS - SLOT_WIDTH_BITS))
+#define TILER_HEIGHT            (1 << (CONT_HEIGHT_BITS - SLOT_HEIGHT_BITS))
+
+/* tiler space addressing bitfields */
+#define MASK_XY_FLIP		(1 << 31)
+#define MASK_Y_INVERT		(1 << 30)
+#define MASK_X_INVERT		(1 << 29)
+#define SHIFT_ACC_MODE		27
+#define MASK_ACC_MODE		3
+
+#define MASK(bits) ((1 << (bits)) - 1)
+
+#define TILVIEW_8BIT    0x60000000u
+#define TILVIEW_16BIT   (TILVIEW_8BIT  + VIEW_SIZE)
+#define TILVIEW_32BIT   (TILVIEW_16BIT + VIEW_SIZE)
+#define TILVIEW_PAGE    (TILVIEW_32BIT + VIEW_SIZE)
+#define TILVIEW_END     (TILVIEW_PAGE  + VIEW_SIZE)
+
+/* create tsptr by adding view orientation and access mode */
+#define TIL_ADDR(x, orient, a)\
+	((u32) (x) | (orient) | ((a) << SHIFT_ACC_MODE))
+
+/* externally accessible functions */
+int omap_dmm_init(struct drm_device *dev);
+int omap_dmm_remove(void);
+
+#ifdef CONFIG_DEBUG_FS
+int tiler_map_show(struct seq_file *s, void *arg);
+#endif
+
+/* pin/unpin */
+int tiler_pin(struct tiler_block *block, struct page **pages,
+		uint32_t npages, uint32_t roll, bool wait);
+int tiler_unpin(struct tiler_block *block);
+
+/* reserve/release */
+struct tiler_block *tiler_reserve_2d(enum tiler_fmt fmt, uint16_t w, uint16_t h,
+				uint16_t align);
+struct tiler_block *tiler_reserve_1d(size_t size);
+int tiler_release(struct tiler_block *block);
+
+/* utilities */
+dma_addr_t tiler_ssptr(struct tiler_block *block);
+uint32_t tiler_stride(enum tiler_fmt fmt);
+size_t tiler_size(enum tiler_fmt fmt, uint16_t w, uint16_t h);
+size_t tiler_vsize(enum tiler_fmt fmt, uint16_t w, uint16_t h);
+void tiler_align(enum tiler_fmt fmt, uint16_t *w, uint16_t *h);
+
+
+/* GEM bo flags -> tiler fmt */
+static inline enum tiler_fmt gem2fmt(uint32_t flags)
+{
+	switch (flags & OMAP_BO_TILED) {
+	case OMAP_BO_TILED_8:
+		return TILFMT_8BIT;
+	case OMAP_BO_TILED_16:
+		return TILFMT_16BIT;
+	case OMAP_BO_TILED_32:
+		return TILFMT_32BIT;
+	default:
+		return TILFMT_PAGE;
+	}
+}
+
+static inline bool validfmt(enum tiler_fmt fmt)
+{
+	switch (fmt) {
+	case TILFMT_8BIT:
+	case TILFMT_16BIT:
+	case TILFMT_32BIT:
+	case TILFMT_PAGE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+struct omap_dmm_platform_data {
+	void __iomem *base;
+	int irq;
+};
+
+#endif
diff --git a/drivers/staging/omapdrm/omap_drm.h b/drivers/staging/omapdrm/omap_drm.h
new file mode 100644
index 000000000000..f0ac34a8973e
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_drm.h
@@ -0,0 +1,123 @@
+/*
+ * include/drm/omap_drm.h
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __OMAP_DRM_H__
+#define __OMAP_DRM_H__
+
+#include <drm/drm.h>
+
+/* Please note that modifications to all structs defined here are
+ * subject to backwards-compatibility constraints.
+ */
+
+#define OMAP_PARAM_CHIPSET_ID	1	/* ie. 0x3430, 0x4430, etc */
+
+struct drm_omap_param {
+	uint64_t param;			/* in */
+	uint64_t value;			/* in (set_param), out (get_param) */
+};
+
+#define OMAP_BO_SCANOUT		0x00000001	/* scanout capable (phys contiguous) */
+#define OMAP_BO_CACHE_MASK	0x00000006	/* cache type mask, see cache modes */
+#define OMAP_BO_TILED_MASK	0x00000f00	/* tiled mapping mask, see tiled modes */
+
+/* cache modes */
+#define OMAP_BO_CACHED		0x00000000	/* default */
+#define OMAP_BO_WC		0x00000002	/* write-combine */
+#define OMAP_BO_UNCACHED	0x00000004	/* strongly-ordered (uncached) */
+
+/* tiled modes */
+#define OMAP_BO_TILED_8		0x00000100
+#define OMAP_BO_TILED_16	0x00000200
+#define OMAP_BO_TILED_32	0x00000300
+#define OMAP_BO_TILED		(OMAP_BO_TILED_8 | OMAP_BO_TILED_16 | OMAP_BO_TILED_32)
+
+union omap_gem_size {
+	uint32_t bytes;		/* (for non-tiled formats) */
+	struct {
+		uint16_t width;
+		uint16_t height;
+	} tiled;		/* (for tiled formats) */
+};
+
+struct drm_omap_gem_new {
+	union omap_gem_size size;	/* in */
+	uint32_t flags;			/* in */
+	uint32_t handle;		/* out */
+	uint32_t __pad;
+};
+
+/* mask of operations: */
+enum omap_gem_op {
+	OMAP_GEM_READ = 0x01,
+	OMAP_GEM_WRITE = 0x02,
+};
+
+struct drm_omap_gem_cpu_prep {
+	uint32_t handle;		/* buffer handle (in) */
+	uint32_t op;			/* mask of omap_gem_op (in) */
+};
+
+struct drm_omap_gem_cpu_fini {
+	uint32_t handle;		/* buffer handle (in) */
+	uint32_t op;			/* mask of omap_gem_op (in) */
+	/* TODO maybe here we pass down info about what regions are touched
+	 * by sw so we can be clever about cache ops?  For now a placeholder,
+	 * set to zero and we just do full buffer flush..
+	 */
+	uint32_t nregions;
+	uint32_t __pad;
+};
+
+struct drm_omap_gem_info {
+	uint32_t handle;		/* buffer handle (in) */
+	uint32_t pad;
+	uint64_t offset;		/* mmap offset (out) */
+	/* note: in case of tiled buffers, the user virtual size can be
+	 * different from the physical size (ie. how many pages are needed
+	 * to back the object) which is returned in DRM_IOCTL_GEM_OPEN..
+	 * This size here is the one that should be used if you want to
+	 * mmap() the buffer:
+	 */
+	uint32_t size;			/* virtual size for mmap'ing (out) */
+	uint32_t __pad;
+};
+
+#define DRM_OMAP_GET_PARAM		0x00
+#define DRM_OMAP_SET_PARAM		0x01
+/* placeholder for plugin-api
+#define DRM_OMAP_GET_BASE		0x02
+*/
+#define DRM_OMAP_GEM_NEW		0x03
+#define DRM_OMAP_GEM_CPU_PREP		0x04
+#define DRM_OMAP_GEM_CPU_FINI		0x05
+#define DRM_OMAP_GEM_INFO		0x06
+#define DRM_OMAP_NUM_IOCTLS		0x07
+
+#define DRM_IOCTL_OMAP_GET_PARAM	DRM_IOWR(DRM_COMMAND_BASE + DRM_OMAP_GET_PARAM, struct drm_omap_param)
+#define DRM_IOCTL_OMAP_SET_PARAM	DRM_IOW (DRM_COMMAND_BASE + DRM_OMAP_SET_PARAM, struct drm_omap_param)
+/* placeholder for plugin-api
+#define DRM_IOCTL_OMAP_GET_BASE		DRM_IOWR(DRM_COMMAND_BASE + DRM_OMAP_GET_BASE, struct drm_omap_get_base)
+*/
+#define DRM_IOCTL_OMAP_GEM_NEW		DRM_IOWR(DRM_COMMAND_BASE + DRM_OMAP_GEM_NEW, struct drm_omap_gem_new)
+#define DRM_IOCTL_OMAP_GEM_CPU_PREP	DRM_IOW (DRM_COMMAND_BASE + DRM_OMAP_GEM_CPU_PREP, struct drm_omap_gem_cpu_prep)
+#define DRM_IOCTL_OMAP_GEM_CPU_FINI	DRM_IOW (DRM_COMMAND_BASE + DRM_OMAP_GEM_CPU_FINI, struct drm_omap_gem_cpu_fini)
+#define DRM_IOCTL_OMAP_GEM_INFO		DRM_IOWR(DRM_COMMAND_BASE + DRM_OMAP_GEM_INFO, struct drm_omap_gem_info)
+
+#endif /* __OMAP_DRM_H__ */
diff --git a/drivers/staging/omapdrm/omap_drv.c b/drivers/staging/omapdrm/omap_drv.c
new file mode 100644
index 000000000000..602aa2dd49c8
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_drv.c
@@ -0,0 +1,821 @@
+/*
+ * drivers/staging/omapdrm/omap_drv.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "omap_drv.h"
+
+#include "drm_crtc_helper.h"
+#include "drm_fb_helper.h"
+
+#define DRIVER_NAME		MODULE_NAME
+#define DRIVER_DESC		"OMAP DRM"
+#define DRIVER_DATE		"20110917"
+#define DRIVER_MAJOR		1
+#define DRIVER_MINOR		0
+#define DRIVER_PATCHLEVEL	0
+
+struct drm_device *drm_device;
+
+static int num_crtc = CONFIG_DRM_OMAP_NUM_CRTCS;
+
+MODULE_PARM_DESC(num_crtc, "Number of overlays to use as CRTCs");
+module_param(num_crtc, int, 0600);
+
+/*
+ * mode config funcs
+ */
+
+/* Notes about mapping DSS and DRM entities:
+ *    CRTC:        overlay
+ *    encoder:     manager.. with some extension to allow one primary CRTC
+ *                 and zero or more video CRTC's to be mapped to one encoder?
+ *    connector:   dssdev.. manager can be attached/detached from different
+ *                 devices
+ */
+
+static void omap_fb_output_poll_changed(struct drm_device *dev)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	DBG("dev=%p", dev);
+	if (priv->fbdev) {
+		drm_fb_helper_hotplug_event(priv->fbdev);
+	}
+}
+
+static struct drm_mode_config_funcs omap_mode_config_funcs = {
+	.fb_create = omap_framebuffer_create,
+	.output_poll_changed = omap_fb_output_poll_changed,
+};
+
+static int get_connector_type(struct omap_dss_device *dssdev)
+{
+	switch (dssdev->type) {
+	case OMAP_DISPLAY_TYPE_HDMI:
+		return DRM_MODE_CONNECTOR_HDMIA;
+	case OMAP_DISPLAY_TYPE_DPI:
+		if (!strcmp(dssdev->name, "dvi"))
+			return DRM_MODE_CONNECTOR_DVID;
+		/* fallthrough */
+	default:
+		return DRM_MODE_CONNECTOR_Unknown;
+	}
+}
+
+#if 0 /* enable when dss2 supports hotplug */
+static int omap_drm_notifier(struct notifier_block *nb,
+		unsigned long evt, void *arg)
+{
+	switch (evt) {
+	case OMAP_DSS_SIZE_CHANGE:
+	case OMAP_DSS_HOTPLUG_CONNECT:
+	case OMAP_DSS_HOTPLUG_DISCONNECT: {
+		struct drm_device *dev = drm_device;
+		DBG("hotplug event: evt=%d, dev=%p", evt, dev);
+		if (dev) {
+			drm_sysfs_hotplug_event(dev);
+		}
+		return NOTIFY_OK;
+	}
+	default:  /* don't care about other events for now */
+		return NOTIFY_DONE;
+	}
+}
+#endif
+
+static void dump_video_chains(void)
+{
+	int i;
+
+	DBG("dumping video chains: ");
+	for (i = 0; i < omap_dss_get_num_overlays(); i++) {
+		struct omap_overlay *ovl = omap_dss_get_overlay(i);
+		struct omap_overlay_manager *mgr = ovl->manager;
+		struct omap_dss_device *dssdev = mgr ? mgr->device : NULL;
+		if (dssdev) {
+			DBG("%d: %s -> %s -> %s", i, ovl->name, mgr->name,
+						dssdev->name);
+		} else if (mgr) {
+			DBG("%d: %s -> %s", i, ovl->name, mgr->name);
+		} else {
+			DBG("%d: %s", i, ovl->name);
+		}
+	}
+}
+
+/* create encoders for each manager */
+static int create_encoder(struct drm_device *dev,
+		struct omap_overlay_manager *mgr)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	struct drm_encoder *encoder = omap_encoder_init(dev, mgr);
+
+	if (!encoder) {
+		dev_err(dev->dev, "could not create encoder: %s\n",
+				mgr->name);
+		return -ENOMEM;
+	}
+
+	BUG_ON(priv->num_encoders >= ARRAY_SIZE(priv->encoders));
+
+	priv->encoders[priv->num_encoders++] = encoder;
+
+	return 0;
+}
+
+/* create connectors for each display device */
+static int create_connector(struct drm_device *dev,
+		struct omap_dss_device *dssdev)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	static struct notifier_block *notifier;
+	struct drm_connector *connector;
+	int j;
+
+	if (!dssdev->driver) {
+		dev_warn(dev->dev, "%s has no driver.. skipping it\n",
+				dssdev->name);
+		return 0;
+	}
+
+	if (!(dssdev->driver->get_timings ||
+				dssdev->driver->read_edid)) {
+		dev_warn(dev->dev, "%s driver does not support "
+			"get_timings or read_edid.. skipping it!\n",
+			dssdev->name);
+		return 0;
+	}
+
+	connector = omap_connector_init(dev,
+			get_connector_type(dssdev), dssdev);
+
+	if (!connector) {
+		dev_err(dev->dev, "could not create connector: %s\n",
+				dssdev->name);
+		return -ENOMEM;
+	}
+
+	BUG_ON(priv->num_connectors >= ARRAY_SIZE(priv->connectors));
+
+	priv->connectors[priv->num_connectors++] = connector;
+
+#if 0 /* enable when dss2 supports hotplug */
+	notifier = kzalloc(sizeof(struct notifier_block), GFP_KERNEL);
+	notifier->notifier_call = omap_drm_notifier;
+	omap_dss_add_notify(dssdev, notifier);
+#else
+	notifier = NULL;
+#endif
+
+	for (j = 0; j < priv->num_encoders; j++) {
+		struct omap_overlay_manager *mgr =
+			omap_encoder_get_manager(priv->encoders[j]);
+		if (mgr->device == dssdev) {
+			drm_mode_connector_attach_encoder(connector,
+					priv->encoders[j]);
+		}
+	}
+
+	return 0;
+}
+
+/* create up to max_overlays CRTCs mapping to overlays.. by default,
+ * connect the overlays to different managers/encoders, giving priority
+ * to encoders connected to connectors with a detected connection
+ */
+static int create_crtc(struct drm_device *dev, struct omap_overlay *ovl,
+		int *j, unsigned int connected_connectors)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	struct omap_overlay_manager *mgr = NULL;
+	struct drm_crtc *crtc;
+
+	if (ovl->manager) {
+		DBG("disconnecting %s from %s", ovl->name,
+					ovl->manager->name);
+		ovl->unset_manager(ovl);
+	}
+
+	/* find next best connector, ones with detected connection first
+	 */
+	while (*j < priv->num_connectors && !mgr) {
+		if (connected_connectors & (1 << *j)) {
+			struct drm_encoder *encoder =
+				omap_connector_attached_encoder(
+						priv->connectors[*j]);
+			if (encoder) {
+				mgr = omap_encoder_get_manager(encoder);
+			}
+		}
+		(*j)++;
+	}
+
+	/* if we couldn't find another connected connector, lets start
+	 * looking at the unconnected connectors:
+	 *
+	 * note: it might not be immediately apparent, but thanks to
+	 * the !mgr check in both this loop and the one above, the only
+	 * way to enter this loop is with *j == priv->num_connectors,
+	 * so idx can never go negative.
+	 */
+	while (*j < 2 * priv->num_connectors && !mgr) {
+		int idx = *j - priv->num_connectors;
+		if (!(connected_connectors & (1 << idx))) {
+			struct drm_encoder *encoder =
+				omap_connector_attached_encoder(
+						priv->connectors[idx]);
+			if (encoder) {
+				mgr = omap_encoder_get_manager(encoder);
+			}
+		}
+		(*j)++;
+	}
+
+	if (mgr) {
+		DBG("connecting %s to %s", ovl->name, mgr->name);
+		ovl->set_manager(ovl, mgr);
+	}
+
+	crtc = omap_crtc_init(dev, ovl, priv->num_crtcs);
+
+	if (!crtc) {
+		dev_err(dev->dev, "could not create CRTC: %s\n",
+				ovl->name);
+		return -ENOMEM;
+	}
+
+	BUG_ON(priv->num_crtcs >= ARRAY_SIZE(priv->crtcs));
+
+	priv->crtcs[priv->num_crtcs++] = crtc;
+
+	return 0;
+}
+
+static int match_dev_name(struct omap_dss_device *dssdev, void *data)
+{
+	return !strcmp(dssdev->name, data);
+}
+
+static unsigned int detect_connectors(struct drm_device *dev)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	unsigned int connected_connectors = 0;
+	int i;
+
+	for (i = 0; i < priv->num_connectors; i++) {
+		struct drm_connector *connector = priv->connectors[i];
+		if (omap_connector_detect(connector, true) ==
+				connector_status_connected) {
+			connected_connectors |= (1 << i);
+		}
+	}
+
+	return connected_connectors;
+}
+
+static int omap_modeset_init(struct drm_device *dev)
+{
+	const struct omap_drm_platform_data *pdata = dev->dev->platform_data;
+	struct omap_kms_platform_data *kms_pdata = NULL;
+	struct omap_drm_private *priv = dev->dev_private;
+	struct omap_dss_device *dssdev = NULL;
+	int i, j;
+	unsigned int connected_connectors = 0;
+
+	drm_mode_config_init(dev);
+
+	if (pdata && pdata->kms_pdata) {
+		kms_pdata = pdata->kms_pdata;
+
+		/* if platform data is provided by the board file, use it to
+		 * control which overlays, managers, and devices we own.
+		 */
+		for (i = 0; i < kms_pdata->mgr_cnt; i++) {
+			struct omap_overlay_manager *mgr =
+				omap_dss_get_overlay_manager(
+						kms_pdata->mgr_ids[i]);
+			create_encoder(dev, mgr);
+		}
+
+		for (i = 0; i < kms_pdata->dev_cnt; i++) {
+			struct omap_dss_device *dssdev =
+				omap_dss_find_device(
+					(void *)kms_pdata->dev_names[i],
+					match_dev_name);
+			if (!dssdev) {
+				dev_warn(dev->dev, "no such dssdev: %s\n",
+						kms_pdata->dev_names[i]);
+				continue;
+			}
+			create_connector(dev, dssdev);
+		}
+
+		connected_connectors = detect_connectors(dev);
+
+		j = 0;
+		for (i = 0; i < kms_pdata->ovl_cnt; i++) {
+			struct omap_overlay *ovl =
+				omap_dss_get_overlay(kms_pdata->ovl_ids[i]);
+			create_crtc(dev, ovl, &j, connected_connectors);
+		}
+	} else {
+		/* otherwise just grab up to CONFIG_DRM_OMAP_NUM_CRTCS and try
+		 * to make educated guesses about everything else
+		 */
+		int max_overlays = min(omap_dss_get_num_overlays(), num_crtc);
+
+		for (i = 0; i < omap_dss_get_num_overlay_managers(); i++) {
+			create_encoder(dev, omap_dss_get_overlay_manager(i));
+		}
+
+		for_each_dss_dev(dssdev) {
+			create_connector(dev, dssdev);
+		}
+
+		connected_connectors = detect_connectors(dev);
+
+		j = 0;
+		for (i = 0; i < max_overlays; i++) {
+			create_crtc(dev, omap_dss_get_overlay(i),
+					&j, connected_connectors);
+		}
+	}
+
+	/* for now keep the mapping of CRTCs and encoders static.. */
+	for (i = 0; i < priv->num_encoders; i++) {
+		struct drm_encoder *encoder = priv->encoders[i];
+		struct omap_overlay_manager *mgr =
+				omap_encoder_get_manager(encoder);
+
+		encoder->possible_crtcs = 0;
+
+		for (j = 0; j < priv->num_crtcs; j++) {
+			struct omap_overlay *ovl =
+					omap_crtc_get_overlay(priv->crtcs[j]);
+			if (ovl->manager == mgr) {
+				encoder->possible_crtcs |= (1 << j);
+			}
+		}
+
+		DBG("%s: possible_crtcs=%08x", mgr->name,
+					encoder->possible_crtcs);
+	}
+
+	dump_video_chains();
+
+	dev->mode_config.min_width = 256;
+	dev->mode_config.min_height = 256;
+
+	/* note: eventually will need some cpu_is_omapXYZ() type stuff here
+	 * to fill in these limits properly on different OMAP generations..
+	 */
+	dev->mode_config.max_width = 2048;
+	dev->mode_config.max_height = 2048;
+
+	dev->mode_config.funcs = &omap_mode_config_funcs;
+
+	return 0;
+}
+
+static void omap_modeset_free(struct drm_device *dev)
+{
+	drm_mode_config_cleanup(dev);
+}
+
+/*
+ * drm ioctl funcs
+ */
+
+
+static int ioctl_get_param(struct drm_device *dev, void *data,
+		struct drm_file *file_priv)
+{
+	struct drm_omap_param *args = data;
+
+	DBG("%p: param=%llu", dev, args->param);
+
+	switch (args->param) {
+	case OMAP_PARAM_CHIPSET_ID:
+		args->value = GET_OMAP_TYPE;
+		break;
+	default:
+		DBG("unknown parameter %lld", args->param);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ioctl_set_param(struct drm_device *dev, void *data,
+		struct drm_file *file_priv)
+{
+	struct drm_omap_param *args = data;
+
+	switch (args->param) {
+	default:
+		DBG("unknown parameter %lld", args->param);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ioctl_gem_new(struct drm_device *dev, void *data,
+		struct drm_file *file_priv)
+{
+	struct drm_omap_gem_new *args = data;
+	DBG("%p:%p: size=0x%08x, flags=%08x", dev, file_priv,
+			args->size.bytes, args->flags);
+	return omap_gem_new_handle(dev, file_priv, args->size,
+			args->flags, &args->handle);
+}
+
+static int ioctl_gem_cpu_prep(struct drm_device *dev, void *data,
+		struct drm_file *file_priv)
+{
+	struct drm_omap_gem_cpu_prep *args = data;
+	struct drm_gem_object *obj;
+	int ret;
+
+	VERB("%p:%p: handle=%d, op=%x", dev, file_priv, args->handle, args->op);
+
+	obj = drm_gem_object_lookup(dev, file_priv, args->handle);
+	if (!obj) {
+		return -ENOENT;
+	}
+
+	ret = omap_gem_op_sync(obj, args->op);
+
+	if (!ret) {
+		ret = omap_gem_op_start(obj, args->op);
+	}
+
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+static int ioctl_gem_cpu_fini(struct drm_device *dev, void *data,
+		struct drm_file *file_priv)
+{
+	struct drm_omap_gem_cpu_fini *args = data;
+	struct drm_gem_object *obj;
+	int ret;
+
+	VERB("%p:%p: handle=%d", dev, file_priv, args->handle);
+
+	obj = drm_gem_object_lookup(dev, file_priv, args->handle);
+	if (!obj) {
+		return -ENOENT;
+	}
+
+	/* XXX flushy, flushy */
+	ret = 0;
+
+	if (!ret) {
+		ret = omap_gem_op_finish(obj, args->op);
+	}
+
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+static int ioctl_gem_info(struct drm_device *dev, void *data,
+		struct drm_file *file_priv)
+{
+	struct drm_omap_gem_info *args = data;
+	struct drm_gem_object *obj;
+	int ret = 0;
+
+	DBG("%p:%p: handle=%d", dev, file_priv, args->handle);
+
+	obj = drm_gem_object_lookup(dev, file_priv, args->handle);
+	if (!obj) {
+		return -ENOENT;
+	}
+
+	args->size = omap_gem_mmap_size(obj);
+	args->offset = omap_gem_mmap_offset(obj);
+
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+struct drm_ioctl_desc ioctls[DRM_COMMAND_END - DRM_COMMAND_BASE] = {
+	DRM_IOCTL_DEF_DRV(OMAP_GET_PARAM, ioctl_get_param, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(OMAP_SET_PARAM, ioctl_set_param, DRM_UNLOCKED|DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF_DRV(OMAP_GEM_NEW, ioctl_gem_new, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(OMAP_GEM_CPU_PREP, ioctl_gem_cpu_prep, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(OMAP_GEM_CPU_FINI, ioctl_gem_cpu_fini, DRM_UNLOCKED|DRM_AUTH),
+	DRM_IOCTL_DEF_DRV(OMAP_GEM_INFO, ioctl_gem_info, DRM_UNLOCKED|DRM_AUTH),
+};
+
+/*
+ * drm driver funcs
+ */
+
+/**
+ * load - setup chip and create an initial config
+ * @dev: DRM device
+ * @flags: startup flags
+ *
+ * The driver load routine has to do several things:
+ *   - initialize the memory manager
+ *   - allocate initial config memory
+ *   - setup the DRM framebuffer with the allocated memory
+ */
+static int dev_load(struct drm_device *dev, unsigned long flags)
+{
+	struct omap_drm_private *priv;
+	int ret;
+
+	DBG("load: dev=%p", dev);
+
+	drm_device = dev;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(dev->dev, "could not allocate priv\n");
+		return -ENOMEM;
+	}
+
+	dev->dev_private = priv;
+
+	omap_gem_init(dev);
+
+	ret = omap_modeset_init(dev);
+	if (ret) {
+		dev_err(dev->dev, "omap_modeset_init failed: ret=%d\n", ret);
+		dev->dev_private = NULL;
+		kfree(priv);
+		return ret;
+	}
+
+	priv->fbdev = omap_fbdev_init(dev);
+	if (!priv->fbdev) {
+		dev_warn(dev->dev, "omap_fbdev_init failed\n");
+		/* well, limp along without an fbdev.. maybe X11 will work? */
+	}
+
+	drm_kms_helper_poll_init(dev);
+
+	ret = drm_vblank_init(dev, priv->num_crtcs);
+	if (ret) {
+		dev_warn(dev->dev, "could not init vblank\n");
+	}
+
+	return 0;
+}
+
+static int dev_unload(struct drm_device *dev)
+{
+	DBG("unload: dev=%p", dev);
+
+	drm_vblank_cleanup(dev);
+	drm_kms_helper_poll_fini(dev);
+
+	omap_fbdev_free(dev);
+	omap_modeset_free(dev);
+	omap_gem_deinit(dev);
+
+	kfree(dev->dev_private);
+	dev->dev_private = NULL;
+
+	return 0;
+}
+
+static int dev_open(struct drm_device *dev, struct drm_file *file)
+{
+	file->driver_priv = NULL;
+
+	DBG("open: dev=%p, file=%p", dev, file);
+
+	return 0;
+}
+
+static int dev_firstopen(struct drm_device *dev)
+{
+	DBG("firstopen: dev=%p", dev);
+	return 0;
+}
+
+/**
+ * lastclose - clean up after all DRM clients have exited
+ * @dev: DRM device
+ *
+ * Take care of cleaning up after all DRM clients have exited.  In the
+ * mode setting case, we want to restore the kernel's initial mode (just
+ * in case the last client left us in a bad state).
+ */
+static void dev_lastclose(struct drm_device *dev)
+{
+	/* we don't support vga-switcheroo.. so just make sure the fbdev
+	 * mode is active
+	 */
+	struct omap_drm_private *priv = dev->dev_private;
+	int ret;
+
+	DBG("lastclose: dev=%p", dev);
+
+	ret = drm_fb_helper_restore_fbdev_mode(priv->fbdev);
+	if (ret)
+		DBG("failed to restore crtc mode");
+}
+
+static void dev_preclose(struct drm_device *dev, struct drm_file *file)
+{
+	DBG("preclose: dev=%p", dev);
+}
+
+static void dev_postclose(struct drm_device *dev, struct drm_file *file)
+{
+	DBG("postclose: dev=%p, file=%p", dev, file);
+}
+
+/**
+ * enable_vblank - enable vblank interrupt events
+ * @dev: DRM device
+ * @crtc: which irq to enable
+ *
+ * Enable vblank interrupts for @crtc.  If the device doesn't have
+ * a hardware vblank counter, this routine should be a no-op, since
+ * interrupts will have to stay on to keep the count accurate.
+ *
+ * RETURNS
+ * Zero on success, appropriate errno if the given @crtc's vblank
+ * interrupt cannot be enabled.
+ */
+static int dev_enable_vblank(struct drm_device *dev, int crtc)
+{
+	DBG("enable_vblank: dev=%p, crtc=%d", dev, crtc);
+	return 0;
+}
+
+/**
+ * disable_vblank - disable vblank interrupt events
+ * @dev: DRM device
+ * @crtc: which irq to enable
+ *
+ * Disable vblank interrupts for @crtc.  If the device doesn't have
+ * a hardware vblank counter, this routine should be a no-op, since
+ * interrupts will have to stay on to keep the count accurate.
+ */
+static void dev_disable_vblank(struct drm_device *dev, int crtc)
+{
+	DBG("disable_vblank: dev=%p, crtc=%d", dev, crtc);
+}
+
+static irqreturn_t dev_irq_handler(DRM_IRQ_ARGS)
+{
+	return IRQ_HANDLED;
+}
+
+static void dev_irq_preinstall(struct drm_device *dev)
+{
+	DBG("irq_preinstall: dev=%p", dev);
+}
+
+static int dev_irq_postinstall(struct drm_device *dev)
+{
+	DBG("irq_postinstall: dev=%p", dev);
+	return 0;
+}
+
+static void dev_irq_uninstall(struct drm_device *dev)
+{
+	DBG("irq_uninstall: dev=%p", dev);
+}
+
+static struct vm_operations_struct omap_gem_vm_ops = {
+	.fault = omap_gem_fault,
+	.open = drm_gem_vm_open,
+	.close = drm_gem_vm_close,
+};
+
+static struct drm_driver omap_drm_driver = {
+		.driver_features =
+				DRIVER_HAVE_IRQ | DRIVER_MODESET | DRIVER_GEM,
+		.load = dev_load,
+		.unload = dev_unload,
+		.open = dev_open,
+		.firstopen = dev_firstopen,
+		.lastclose = dev_lastclose,
+		.preclose = dev_preclose,
+		.postclose = dev_postclose,
+		.get_vblank_counter = drm_vblank_count,
+		.enable_vblank = dev_enable_vblank,
+		.disable_vblank = dev_disable_vblank,
+		.irq_preinstall = dev_irq_preinstall,
+		.irq_postinstall = dev_irq_postinstall,
+		.irq_uninstall = dev_irq_uninstall,
+		.irq_handler = dev_irq_handler,
+		.reclaim_buffers = drm_core_reclaim_buffers,
+#ifdef CONFIG_DEBUG_FS
+		.debugfs_init = omap_debugfs_init,
+		.debugfs_cleanup = omap_debugfs_cleanup,
+#endif
+		.gem_init_object = omap_gem_init_object,
+		.gem_free_object = omap_gem_free_object,
+		.gem_vm_ops = &omap_gem_vm_ops,
+		.dumb_create = omap_gem_dumb_create,
+		.dumb_map_offset = omap_gem_dumb_map_offset,
+		.dumb_destroy = omap_gem_dumb_destroy,
+		.ioctls = ioctls,
+		.num_ioctls = DRM_OMAP_NUM_IOCTLS,
+		.fops = {
+				.owner = THIS_MODULE,
+				.open = drm_open,
+				.unlocked_ioctl = drm_ioctl,
+				.release = drm_release,
+				.mmap = omap_gem_mmap,
+				.poll = drm_poll,
+				.fasync = drm_fasync,
+				.read = drm_read,
+				.llseek = noop_llseek,
+		},
+		.name = DRIVER_NAME,
+		.desc = DRIVER_DESC,
+		.date = DRIVER_DATE,
+		.major = DRIVER_MAJOR,
+		.minor = DRIVER_MINOR,
+		.patchlevel = DRIVER_PATCHLEVEL,
+};
+
+static int pdev_suspend(struct platform_device *pDevice, pm_message_t state)
+{
+	DBG("");
+	return 0;
+}
+
+static int pdev_resume(struct platform_device *device)
+{
+	DBG("");
+	return 0;
+}
+
+static void pdev_shutdown(struct platform_device *device)
+{
+	DBG("");
+}
+
+static int pdev_probe(struct platform_device *device)
+{
+	DBG("%s", device->name);
+	return drm_platform_init(&omap_drm_driver, device);
+}
+
+static int pdev_remove(struct platform_device *device)
+{
+	DBG("");
+	drm_platform_exit(&omap_drm_driver, device);
+	return 0;
+}
+
+struct platform_driver pdev = {
+		.driver = {
+			.name = DRIVER_NAME,
+			.owner = THIS_MODULE,
+		},
+		.probe = pdev_probe,
+		.remove = pdev_remove,
+		.suspend = pdev_suspend,
+		.resume = pdev_resume,
+		.shutdown = pdev_shutdown,
+};
+
+static int __init omap_drm_init(void)
+{
+	DBG("init");
+	return platform_driver_register(&pdev);
+}
+
+static void __exit omap_drm_fini(void)
+{
+	DBG("fini");
+	platform_driver_unregister(&pdev);
+}
+
+/* need late_initcall() so we load after dss_driver's are loaded */
+late_initcall(omap_drm_init);
+module_exit(omap_drm_fini);
+
+MODULE_AUTHOR("Rob Clark <rob@ti.com>");
+MODULE_DESCRIPTION("OMAP DRM Display Driver");
+MODULE_ALIAS("platform:" DRIVER_NAME);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/omapdrm/omap_drv.h b/drivers/staging/omapdrm/omap_drv.h
new file mode 100644
index 000000000000..76c42515ecc5
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_drv.h
@@ -0,0 +1,135 @@
+/*
+ * drivers/staging/omapdrm/omap_drv.h
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __OMAP_DRV_H__
+#define __OMAP_DRV_H__
+
+#include <video/omapdss.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <drm/drmP.h>
+#include "omap_drm.h"
+#include "omap_priv.h"
+
+#define DBG(fmt, ...) DRM_DEBUG(fmt"\n", ##__VA_ARGS__)
+#define VERB(fmt, ...) if (0) DRM_DEBUG(fmt, ##__VA_ARGS__) /* verbose debug */
+
+#define MODULE_NAME     "omapdrm"
+
+/* max # of mapper-id's that can be assigned.. todo, come up with a better
+ * (but still inexpensive) way to store/access per-buffer mapper private
+ * data..
+ */
+#define MAX_MAPPERS 2
+
+struct omap_drm_private {
+	unsigned int num_crtcs;
+	struct drm_crtc *crtcs[8];
+	unsigned int num_encoders;
+	struct drm_encoder *encoders[8];
+	unsigned int num_connectors;
+	struct drm_connector *connectors[8];
+
+	struct drm_fb_helper *fbdev;
+
+	bool has_dmm;
+};
+
+#ifdef CONFIG_DEBUG_FS
+int omap_debugfs_init(struct drm_minor *minor);
+void omap_debugfs_cleanup(struct drm_minor *minor);
+#endif
+
+struct drm_fb_helper *omap_fbdev_init(struct drm_device *dev);
+void omap_fbdev_free(struct drm_device *dev);
+
+struct drm_crtc *omap_crtc_init(struct drm_device *dev,
+		struct omap_overlay *ovl, int id);
+struct omap_overlay *omap_crtc_get_overlay(struct drm_crtc *crtc);
+
+struct drm_encoder *omap_encoder_init(struct drm_device *dev,
+		struct omap_overlay_manager *mgr);
+struct omap_overlay_manager *omap_encoder_get_manager(
+		struct drm_encoder *encoder);
+struct drm_encoder *omap_connector_attached_encoder(
+		struct drm_connector *connector);
+enum drm_connector_status omap_connector_detect(
+		struct drm_connector *connector, bool force);
+
+struct drm_connector *omap_connector_init(struct drm_device *dev,
+		int connector_type, struct omap_dss_device *dssdev);
+void omap_connector_mode_set(struct drm_connector *connector,
+		struct drm_display_mode *mode);
+void omap_connector_flush(struct drm_connector *connector,
+		int x, int y, int w, int h);
+
+struct drm_framebuffer *omap_framebuffer_create(struct drm_device *dev,
+		struct drm_file *file, struct drm_mode_fb_cmd *mode_cmd);
+struct drm_framebuffer *omap_framebuffer_init(struct drm_device *dev,
+		struct drm_mode_fb_cmd *mode_cmd, struct drm_gem_object *bo);
+struct drm_gem_object *omap_framebuffer_bo(struct drm_framebuffer *fb);
+int omap_framebuffer_get_buffer(struct drm_framebuffer *fb, int x, int y,
+		void **vaddr, dma_addr_t *paddr, unsigned int *screen_width);
+struct drm_connector *omap_framebuffer_get_next_connector(
+		struct drm_framebuffer *fb, struct drm_connector *from);
+void omap_framebuffer_flush(struct drm_framebuffer *fb,
+		int x, int y, int w, int h);
+
+void omap_gem_init(struct drm_device *dev);
+void omap_gem_deinit(struct drm_device *dev);
+
+struct drm_gem_object *omap_gem_new(struct drm_device *dev,
+		union omap_gem_size gsize, uint32_t flags);
+int omap_gem_new_handle(struct drm_device *dev, struct drm_file *file,
+		union omap_gem_size gsize, uint32_t flags, uint32_t *handle);
+void omap_gem_free_object(struct drm_gem_object *obj);
+int omap_gem_init_object(struct drm_gem_object *obj);
+void *omap_gem_vaddr(struct drm_gem_object *obj);
+int omap_gem_dumb_map_offset(struct drm_file *file, struct drm_device *dev,
+		uint32_t handle, uint64_t *offset);
+int omap_gem_dumb_destroy(struct drm_file *file, struct drm_device *dev,
+		uint32_t handle);
+int omap_gem_dumb_create(struct drm_file *file, struct drm_device *dev,
+		struct drm_mode_create_dumb *args);
+int omap_gem_mmap(struct file *filp, struct vm_area_struct *vma);
+int omap_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
+int omap_gem_op_start(struct drm_gem_object *obj, enum omap_gem_op op);
+int omap_gem_op_finish(struct drm_gem_object *obj, enum omap_gem_op op);
+int omap_gem_op_sync(struct drm_gem_object *obj, enum omap_gem_op op);
+int omap_gem_op_async(struct drm_gem_object *obj, enum omap_gem_op op,
+		void (*fxn)(void *arg), void *arg);
+int omap_gem_roll(struct drm_gem_object *obj, uint32_t roll);
+int omap_gem_get_paddr(struct drm_gem_object *obj,
+		dma_addr_t *paddr, bool remap);
+int omap_gem_put_paddr(struct drm_gem_object *obj);
+uint64_t omap_gem_mmap_offset(struct drm_gem_object *obj);
+size_t omap_gem_mmap_size(struct drm_gem_object *obj);
+
+static inline int align_pitch(int pitch, int width, int bpp)
+{
+	int bytespp = (bpp + 7) / 8;
+	/* in case someone tries to feed us a completely bogus stride: */
+	pitch = max(pitch, width * bytespp);
+	/* PVR needs alignment to 8 pixels.. right now that is the most
+	 * restrictive stride requirement..
+	 */
+	return ALIGN(pitch, 8 * bytespp);
+}
+
+#endif /* __OMAP_DRV_H__ */
diff --git a/drivers/staging/omapdrm/omap_encoder.c b/drivers/staging/omapdrm/omap_encoder.c
new file mode 100644
index 000000000000..06c52cb62d2f
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_encoder.c
@@ -0,0 +1,171 @@
+/*
+ * drivers/staging/omapdrm/omap_encoder.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "omap_drv.h"
+
+#include "drm_crtc.h"
+#include "drm_crtc_helper.h"
+
+/*
+ * encoder funcs
+ */
+
+#define to_omap_encoder(x) container_of(x, struct omap_encoder, base)
+
+struct omap_encoder {
+	struct drm_encoder base;
+	struct omap_overlay_manager *mgr;
+};
+
+static void omap_encoder_destroy(struct drm_encoder *encoder)
+{
+	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
+	DBG("%s", omap_encoder->mgr->name);
+	drm_encoder_cleanup(encoder);
+	kfree(omap_encoder);
+}
+
+static void omap_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
+	DBG("%s: %d", omap_encoder->mgr->name, mode);
+}
+
+static bool omap_encoder_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
+	DBG("%s", omap_encoder->mgr->name);
+	return true;
+}
+
+static void omap_encoder_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
+	struct drm_device *dev = encoder->dev;
+	struct omap_drm_private *priv = dev->dev_private;
+	int i;
+
+	mode = adjusted_mode;
+
+	DBG("%s: set mode: %dx%d", omap_encoder->mgr->name,
+			mode->hdisplay, mode->vdisplay);
+
+	for (i = 0; i < priv->num_connectors; i++) {
+		struct drm_connector *connector = priv->connectors[i];
+		if (connector->encoder == encoder) {
+			omap_connector_mode_set(connector, mode);
+		}
+	}
+}
+
+static void omap_encoder_prepare(struct drm_encoder *encoder)
+{
+	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
+	struct drm_encoder_helper_funcs *encoder_funcs =
+				encoder->helper_private;
+	DBG("%s", omap_encoder->mgr->name);
+	encoder_funcs->dpms(encoder, DRM_MODE_DPMS_OFF);
+}
+
+static void omap_encoder_commit(struct drm_encoder *encoder)
+{
+	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
+	struct drm_encoder_helper_funcs *encoder_funcs =
+				encoder->helper_private;
+	DBG("%s", omap_encoder->mgr->name);
+	omap_encoder->mgr->apply(omap_encoder->mgr);
+	encoder_funcs->dpms(encoder, DRM_MODE_DPMS_ON);
+}
+
+static const struct drm_encoder_funcs omap_encoder_funcs = {
+	.destroy = omap_encoder_destroy,
+};
+
+static const struct drm_encoder_helper_funcs omap_encoder_helper_funcs = {
+	.dpms = omap_encoder_dpms,
+	.mode_fixup = omap_encoder_mode_fixup,
+	.mode_set = omap_encoder_mode_set,
+	.prepare = omap_encoder_prepare,
+	.commit = omap_encoder_commit,
+};
+
+struct omap_overlay_manager *omap_encoder_get_manager(
+		struct drm_encoder *encoder)
+{
+	struct omap_encoder *omap_encoder = to_omap_encoder(encoder);
+	return omap_encoder->mgr;
+}
+
+/* initialize encoder */
+struct drm_encoder *omap_encoder_init(struct drm_device *dev,
+		struct omap_overlay_manager *mgr)
+{
+	struct drm_encoder *encoder = NULL;
+	struct omap_encoder *omap_encoder;
+	struct omap_overlay_manager_info info;
+	int ret;
+
+	DBG("%s", mgr->name);
+
+	omap_encoder = kzalloc(sizeof(*omap_encoder), GFP_KERNEL);
+	if (!omap_encoder) {
+		dev_err(dev->dev, "could not allocate encoder\n");
+		goto fail;
+	}
+
+	omap_encoder->mgr = mgr;
+	encoder = &omap_encoder->base;
+
+	drm_encoder_init(dev, encoder, &omap_encoder_funcs,
+			 DRM_MODE_ENCODER_TMDS);
+	drm_encoder_helper_add(encoder, &omap_encoder_helper_funcs);
+
+	mgr->get_manager_info(mgr, &info);
+
+	/* TODO: fix hard-coded setup.. */
+	info.default_color = 0x00000000;
+	info.trans_key = 0x00000000;
+	info.trans_key_type = OMAP_DSS_COLOR_KEY_GFX_DST;
+	info.trans_enabled = false;
+
+	ret = mgr->set_manager_info(mgr, &info);
+	if (ret) {
+		dev_err(dev->dev, "could not set manager info\n");
+		goto fail;
+	}
+
+	ret = mgr->apply(mgr);
+	if (ret) {
+		dev_err(dev->dev, "could not apply\n");
+		goto fail;
+	}
+
+	return encoder;
+
+fail:
+	if (encoder) {
+		omap_encoder_destroy(encoder);
+	}
+
+	return NULL;
+}
diff --git a/drivers/staging/omapdrm/omap_fb.c b/drivers/staging/omapdrm/omap_fb.c
new file mode 100644
index 000000000000..0b50c5b3b564
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_fb.c
@@ -0,0 +1,243 @@
+/*
+ * drivers/staging/omapdrm/omap_fb.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "omap_drv.h"
+
+#include "drm_crtc.h"
+#include "drm_crtc_helper.h"
+
+
+/*
+ * framebuffer funcs
+ */
+
+#define to_omap_framebuffer(x) container_of(x, struct omap_framebuffer, base)
+
+struct omap_framebuffer {
+	struct drm_framebuffer base;
+	struct drm_gem_object *bo;
+	int size;
+	dma_addr_t paddr;
+};
+
+static int omap_framebuffer_create_handle(struct drm_framebuffer *fb,
+		struct drm_file *file_priv,
+		unsigned int *handle)
+{
+	struct omap_framebuffer *omap_fb = to_omap_framebuffer(fb);
+    return drm_gem_handle_create(file_priv, omap_fb->bo, handle);
+}
+
+static void omap_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+	struct drm_device *dev = fb->dev;
+	struct omap_framebuffer *omap_fb = to_omap_framebuffer(fb);
+
+	DBG("destroy: FB ID: %d (%p)", fb->base.id, fb);
+
+	drm_framebuffer_cleanup(fb);
+
+	if (omap_fb->bo) {
+		if (omap_fb->paddr && omap_gem_put_paddr(omap_fb->bo))
+			dev_err(dev->dev, "could not unmap!\n");
+		drm_gem_object_unreference_unlocked(omap_fb->bo);
+	}
+
+	kfree(omap_fb);
+}
+
+static int omap_framebuffer_dirty(struct drm_framebuffer *fb,
+		struct drm_file *file_priv, unsigned flags, unsigned color,
+		struct drm_clip_rect *clips, unsigned num_clips)
+{
+	int i;
+
+	for (i = 0; i < num_clips; i++) {
+		omap_framebuffer_flush(fb, clips[i].x1, clips[i].y1,
+					clips[i].x2 - clips[i].x1,
+					clips[i].y2 - clips[i].y1);
+	}
+
+	return 0;
+}
+
+static const struct drm_framebuffer_funcs omap_framebuffer_funcs = {
+	.create_handle = omap_framebuffer_create_handle,
+	.destroy = omap_framebuffer_destroy,
+	.dirty = omap_framebuffer_dirty,
+};
+
+/* returns the buffer size */
+int omap_framebuffer_get_buffer(struct drm_framebuffer *fb, int x, int y,
+		void **vaddr, dma_addr_t *paddr, unsigned int *screen_width)
+{
+	struct omap_framebuffer *omap_fb = to_omap_framebuffer(fb);
+	int bpp = fb->bits_per_pixel / 8;
+	unsigned long offset;
+
+	offset = (x * bpp) + (y * fb->pitch);
+
+	if (vaddr) {
+		void *bo_vaddr = omap_gem_vaddr(omap_fb->bo);
+		/* note: we can only count on having a vaddr for buffers that
+		 * are allocated physically contiguously to begin with (ie.
+		 * dma_alloc_coherent()).  But this should be ok because it
+		 * is only used by legacy fbdev
+		 */
+		BUG_ON(IS_ERR_OR_NULL(bo_vaddr));
+		*vaddr = bo_vaddr + offset;
+	}
+
+	*paddr = omap_fb->paddr + offset;
+	*screen_width = fb->pitch / bpp;
+
+	return omap_fb->size - offset;
+}
+
+struct drm_gem_object *omap_framebuffer_bo(struct drm_framebuffer *fb)
+{
+	struct omap_framebuffer *omap_fb = to_omap_framebuffer(fb);
+	return omap_fb->bo;
+}
+
+/* iterate thru all the connectors, returning ones that are attached
+ * to the same fb..
+ */
+struct drm_connector *omap_framebuffer_get_next_connector(
+		struct drm_framebuffer *fb, struct drm_connector *from)
+{
+	struct drm_device *dev = fb->dev;
+	struct list_head *connector_list = &dev->mode_config.connector_list;
+	struct drm_connector *connector = from;
+
+	if (!from) {
+		return list_first_entry(connector_list, typeof(*from), head);
+	}
+
+	list_for_each_entry_from(connector, connector_list, head) {
+		if (connector != from) {
+			struct drm_encoder *encoder = connector->encoder;
+			struct drm_crtc *crtc = encoder ? encoder->crtc : NULL;
+			if (crtc && crtc->fb == fb) {
+				return connector;
+			}
+		}
+	}
+
+	return NULL;
+}
+
+/* flush an area of the framebuffer (in case of manual update display that
+ * is not automatically flushed)
+ */
+void omap_framebuffer_flush(struct drm_framebuffer *fb,
+		int x, int y, int w, int h)
+{
+	struct drm_connector *connector = NULL;
+
+	VERB("flush: %d,%d %dx%d, fb=%p", x, y, w, h, fb);
+
+	while ((connector = omap_framebuffer_get_next_connector(fb, connector))) {
+		/* only consider connectors that are part of a chain */
+		if (connector->encoder && connector->encoder->crtc) {
+			/* TODO: maybe this should propagate thru the crtc who
+			 * could do the coordinate translation..
+			 */
+			struct drm_crtc *crtc = connector->encoder->crtc;
+			int cx = max(0, x - crtc->x);
+			int cy = max(0, y - crtc->y);
+			int cw = w + (x - crtc->x) - cx;
+			int ch = h + (y - crtc->y) - cy;
+
+			omap_connector_flush(connector, cx, cy, cw, ch);
+		}
+	}
+}
+
+struct drm_framebuffer *omap_framebuffer_create(struct drm_device *dev,
+		struct drm_file *file, struct drm_mode_fb_cmd *mode_cmd)
+{
+	struct drm_gem_object *bo;
+	struct drm_framebuffer *fb;
+	bo = drm_gem_object_lookup(dev, file, mode_cmd->handle);
+	if (!bo) {
+		return ERR_PTR(-ENOENT);
+	}
+	fb = omap_framebuffer_init(dev, mode_cmd, bo);
+	if (!fb) {
+		return ERR_PTR(-ENOMEM);
+	}
+	return fb;
+}
+
+struct drm_framebuffer *omap_framebuffer_init(struct drm_device *dev,
+		struct drm_mode_fb_cmd *mode_cmd, struct drm_gem_object *bo)
+{
+	struct omap_framebuffer *omap_fb;
+	struct drm_framebuffer *fb = NULL;
+	int size, ret;
+
+	DBG("create framebuffer: dev=%p, mode_cmd=%p (%dx%d@%d)",
+			dev, mode_cmd, mode_cmd->width, mode_cmd->height,
+			mode_cmd->bpp);
+
+	/* in case someone tries to feed us a completely bogus stride: */
+	mode_cmd->pitch = align_pitch(mode_cmd->pitch,
+			mode_cmd->width, mode_cmd->bpp);
+
+	omap_fb = kzalloc(sizeof(*omap_fb), GFP_KERNEL);
+	if (!omap_fb) {
+		dev_err(dev->dev, "could not allocate fb\n");
+		goto fail;
+	}
+
+	fb = &omap_fb->base;
+	ret = drm_framebuffer_init(dev, fb, &omap_framebuffer_funcs);
+	if (ret) {
+		dev_err(dev->dev, "framebuffer init failed: %d\n", ret);
+		goto fail;
+	}
+
+	DBG("create: FB ID: %d (%p)", fb->base.id, fb);
+
+	size = PAGE_ALIGN(mode_cmd->pitch * mode_cmd->height);
+
+	if (size > bo->size) {
+		dev_err(dev->dev, "provided buffer object is too small!\n");
+		goto fail;
+	}
+
+	omap_fb->bo = bo;
+	omap_fb->size = size;
+
+	if (omap_gem_get_paddr(bo, &omap_fb->paddr, true)) {
+		dev_err(dev->dev, "could not map (paddr)!\n");
+		goto fail;
+	}
+
+	drm_helper_mode_fill_fb_struct(fb, mode_cmd);
+
+	return fb;
+
+fail:
+	if (fb) {
+		omap_framebuffer_destroy(fb);
+	}
+	return NULL;
+}
diff --git a/drivers/staging/omapdrm/omap_fbdev.c b/drivers/staging/omapdrm/omap_fbdev.c
new file mode 100644
index 000000000000..093ae2f87b20
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_fbdev.c
@@ -0,0 +1,372 @@
+/*
+ * drivers/staging/omapdrm/omap_fbdev.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "omap_drv.h"
+
+#include "drm_crtc.h"
+#include "drm_fb_helper.h"
+
+MODULE_PARM_DESC(ywrap, "Enable ywrap scrolling (omap44xx and later, default 'y')");
+static bool ywrap_enabled = true;
+module_param_named(ywrap, ywrap_enabled, bool, 0644);
+
+/*
+ * fbdev funcs, to implement legacy fbdev interface on top of drm driver
+ */
+
+#define to_omap_fbdev(x) container_of(x, struct omap_fbdev, base)
+
+struct omap_fbdev {
+	struct drm_fb_helper base;
+	struct drm_framebuffer *fb;
+	struct drm_gem_object *bo;
+	bool ywrap_enabled;
+};
+
+static void omap_fbdev_flush(struct fb_info *fbi, int x, int y, int w, int h);
+static struct drm_fb_helper *get_fb(struct fb_info *fbi);
+
+static ssize_t omap_fbdev_write(struct fb_info *fbi, const char __user *buf,
+		size_t count, loff_t *ppos)
+{
+	ssize_t res;
+
+	res = fb_sys_write(fbi, buf, count, ppos);
+	omap_fbdev_flush(fbi, 0, 0, fbi->var.xres, fbi->var.yres);
+
+	return res;
+}
+
+static void omap_fbdev_fillrect(struct fb_info *fbi,
+		const struct fb_fillrect *rect)
+{
+	sys_fillrect(fbi, rect);
+	omap_fbdev_flush(fbi, rect->dx, rect->dy, rect->width, rect->height);
+}
+
+static void omap_fbdev_copyarea(struct fb_info *fbi,
+		const struct fb_copyarea *area)
+{
+	sys_copyarea(fbi, area);
+	omap_fbdev_flush(fbi, area->dx, area->dy, area->width, area->height);
+}
+
+static void omap_fbdev_imageblit(struct fb_info *fbi,
+		const struct fb_image *image)
+{
+	sys_imageblit(fbi, image);
+	omap_fbdev_flush(fbi, image->dx, image->dy,
+				image->width, image->height);
+}
+
+static int omap_fbdev_pan_display(struct fb_var_screeninfo *var,
+		struct fb_info *fbi)
+{
+	struct drm_fb_helper *helper = get_fb(fbi);
+	struct omap_fbdev *fbdev = to_omap_fbdev(helper);
+	int npages;
+
+	if (!helper)
+		goto fallback;
+
+	if (!fbdev->ywrap_enabled)
+		goto fallback;
+
+	/* DMM roll shifts in 4K pages: */
+	npages = fbi->fix.line_length >> PAGE_SHIFT;
+	omap_gem_roll(fbdev->bo, var->yoffset * npages);
+
+	return 0;
+
+fallback:
+	return drm_fb_helper_pan_display(var, fbi);
+}
+
+static struct fb_ops omap_fb_ops = {
+	.owner = THIS_MODULE,
+
+	/* Note: to properly handle manual update displays, we wrap the
+	 * basic fbdev ops which write to the framebuffer
+	 */
+	.fb_read = fb_sys_read,
+	.fb_write = omap_fbdev_write,
+	.fb_fillrect = omap_fbdev_fillrect,
+	.fb_copyarea = omap_fbdev_copyarea,
+	.fb_imageblit = omap_fbdev_imageblit,
+
+	.fb_check_var = drm_fb_helper_check_var,
+	.fb_set_par = drm_fb_helper_set_par,
+	.fb_pan_display = omap_fbdev_pan_display,
+	.fb_blank = drm_fb_helper_blank,
+	.fb_setcmap = drm_fb_helper_setcmap,
+
+	.fb_debug_enter = drm_fb_helper_debug_enter,
+	.fb_debug_leave = drm_fb_helper_debug_leave,
+};
+
+static int omap_fbdev_create(struct drm_fb_helper *helper,
+		struct drm_fb_helper_surface_size *sizes)
+{
+	struct omap_fbdev *fbdev = to_omap_fbdev(helper);
+	struct drm_device *dev = helper->dev;
+	struct omap_drm_private *priv = dev->dev_private;
+	struct drm_framebuffer *fb = NULL;
+	union omap_gem_size gsize;
+	struct fb_info *fbi = NULL;
+	struct drm_mode_fb_cmd mode_cmd = {0};
+	dma_addr_t paddr;
+	void __iomem *vaddr;
+	int size, screen_width;
+	int ret;
+
+	/* only doing ARGB32 since this is what is needed to alpha-blend
+	 * with video overlays:
+	 */
+	sizes->surface_bpp = 32;
+	sizes->surface_depth = 32;
+
+	DBG("create fbdev: %dx%d@%d (%dx%d)", sizes->surface_width,
+			sizes->surface_height, sizes->surface_bpp,
+			sizes->fb_width, sizes->fb_height);
+
+	mode_cmd.width = sizes->surface_width;
+	mode_cmd.height = sizes->surface_height;
+
+	mode_cmd.bpp = sizes->surface_bpp;
+	mode_cmd.depth = sizes->surface_depth;
+
+	mode_cmd.pitch = align_pitch(
+			mode_cmd.width * ((mode_cmd.bpp + 7) / 8),
+			mode_cmd.width, mode_cmd.bpp);
+
+	fbdev->ywrap_enabled = priv->has_dmm && ywrap_enabled;
+	if (fbdev->ywrap_enabled) {
+		/* need to align pitch to page size if using DMM scrolling */
+		mode_cmd.pitch = ALIGN(mode_cmd.pitch, PAGE_SIZE);
+	}
+
+	/* allocate backing bo */
+	gsize = (union omap_gem_size){
+		.bytes = PAGE_ALIGN(mode_cmd.pitch * mode_cmd.height),
+	};
+	DBG("allocating %d bytes for fb %d", gsize.bytes, dev->primary->index);
+	fbdev->bo = omap_gem_new(dev, gsize, OMAP_BO_SCANOUT | OMAP_BO_WC);
+	if (!fbdev->bo) {
+		dev_err(dev->dev, "failed to allocate buffer object\n");
+		goto fail;
+	}
+
+	fb = omap_framebuffer_init(dev, &mode_cmd, fbdev->bo);
+	if (!fb) {
+		dev_err(dev->dev, "failed to allocate fb\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	mutex_lock(&dev->struct_mutex);
+
+	fbi = framebuffer_alloc(0, dev->dev);
+	if (!fbi) {
+		dev_err(dev->dev, "failed to allocate fb info\n");
+		ret = -ENOMEM;
+		goto fail_unlock;
+	}
+
+	DBG("fbi=%p, dev=%p", fbi, dev);
+
+	fbdev->fb = fb;
+	helper->fb = fb;
+	helper->fbdev = fbi;
+
+	fbi->par = helper;
+	fbi->flags = FBINFO_DEFAULT;
+	fbi->fbops = &omap_fb_ops;
+
+	strcpy(fbi->fix.id, MODULE_NAME);
+
+	ret = fb_alloc_cmap(&fbi->cmap, 256, 0);
+	if (ret) {
+		ret = -ENOMEM;
+		goto fail_unlock;
+	}
+
+	drm_fb_helper_fill_fix(fbi, fb->pitch, fb->depth);
+	drm_fb_helper_fill_var(fbi, helper, sizes->fb_width, sizes->fb_height);
+
+	size = omap_framebuffer_get_buffer(fb, 0, 0,
+			&vaddr, &paddr, &screen_width);
+
+	dev->mode_config.fb_base = paddr;
+
+	fbi->screen_base = vaddr;
+	fbi->screen_size = size;
+	fbi->fix.smem_start = paddr;
+	fbi->fix.smem_len = size;
+
+	/* if we have DMM, then we can use it for scrolling by just
+	 * shuffling pages around in DMM rather than doing sw blit.
+	 */
+	if (fbdev->ywrap_enabled) {
+		DRM_INFO("Enabling DMM ywrap scrolling\n");
+		fbi->flags |= FBINFO_HWACCEL_YWRAP | FBINFO_READS_FAST;
+		fbi->fix.ywrapstep = 1;
+	}
+
+
+	DBG("par=%p, %dx%d", fbi->par, fbi->var.xres, fbi->var.yres);
+	DBG("allocated %dx%d fb", fbdev->fb->width, fbdev->fb->height);
+
+	mutex_unlock(&dev->struct_mutex);
+
+	return 0;
+
+fail_unlock:
+	mutex_unlock(&dev->struct_mutex);
+fail:
+
+	if (ret) {
+		if (fbi)
+			framebuffer_release(fbi);
+		if (fb)
+			fb->funcs->destroy(fb);
+	}
+
+	return ret;
+}
+
+static void omap_crtc_fb_gamma_set(struct drm_crtc *crtc,
+		u16 red, u16 green, u16 blue, int regno)
+{
+	DBG("fbdev: set gamma");
+}
+
+static void omap_crtc_fb_gamma_get(struct drm_crtc *crtc,
+		u16 *red, u16 *green, u16 *blue, int regno)
+{
+	DBG("fbdev: get gamma");
+}
+
+static int omap_fbdev_probe(struct drm_fb_helper *helper,
+		struct drm_fb_helper_surface_size *sizes)
+{
+	int new_fb = 0;
+	int ret;
+
+	if (!helper->fb) {
+		ret = omap_fbdev_create(helper, sizes);
+		if (ret)
+			return ret;
+		new_fb = 1;
+	}
+	return new_fb;
+}
+
+static struct drm_fb_helper_funcs omap_fb_helper_funcs = {
+	.gamma_set = omap_crtc_fb_gamma_set,
+	.gamma_get = omap_crtc_fb_gamma_get,
+	.fb_probe = omap_fbdev_probe,
+};
+
+static struct drm_fb_helper *get_fb(struct fb_info *fbi)
+{
+	if (!fbi || strcmp(fbi->fix.id, MODULE_NAME)) {
+		/* these are not the fb's you're looking for */
+		return NULL;
+	}
+	return fbi->par;
+}
+
+/* flush an area of the framebuffer (in case of manual update display that
+ * is not automatically flushed)
+ */
+static void omap_fbdev_flush(struct fb_info *fbi, int x, int y, int w, int h)
+{
+	struct drm_fb_helper *helper = get_fb(fbi);
+
+	if (!helper)
+		return;
+
+	VERB("flush fbdev: %d,%d %dx%d, fbi=%p", x, y, w, h, fbi);
+
+	omap_framebuffer_flush(helper->fb, x, y, w, h);
+}
+
+/* initialize fbdev helper */
+struct drm_fb_helper *omap_fbdev_init(struct drm_device *dev)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	struct omap_fbdev *fbdev = NULL;
+	struct drm_fb_helper *helper;
+	int ret = 0;
+
+	fbdev = kzalloc(sizeof(*fbdev), GFP_KERNEL);
+	if (!fbdev) {
+		dev_err(dev->dev, "could not allocate fbdev\n");
+		goto fail;
+	}
+
+	helper = &fbdev->base;
+
+	helper->funcs = &omap_fb_helper_funcs;
+
+	ret = drm_fb_helper_init(dev, helper,
+			priv->num_crtcs, priv->num_connectors);
+	if (ret) {
+		dev_err(dev->dev, "could not init fbdev: ret=%d\n", ret);
+		goto fail;
+	}
+
+	drm_fb_helper_single_add_all_connectors(helper);
+	drm_fb_helper_initial_config(helper, 32);
+
+	priv->fbdev = helper;
+
+	return helper;
+
+fail:
+	kfree(fbdev);
+	return NULL;
+}
+
+void omap_fbdev_free(struct drm_device *dev)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	struct drm_fb_helper *helper = priv->fbdev;
+	struct omap_fbdev *fbdev;
+	struct fb_info *fbi;
+
+	DBG();
+
+	fbi = helper->fbdev;
+
+	unregister_framebuffer(fbi);
+	framebuffer_release(fbi);
+
+	drm_fb_helper_fini(helper);
+
+	fbdev = to_omap_fbdev(priv->fbdev);
+
+	kfree(fbdev);
+
+	/* this will free the backing object */
+	if (fbdev->fb)
+		fbdev->fb->funcs->destroy(fbdev->fb);
+
+	priv->fbdev = NULL;
+}
diff --git a/drivers/staging/omapdrm/omap_gem.c b/drivers/staging/omapdrm/omap_gem.c
new file mode 100644
index 000000000000..e0ebd1d139f6
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_gem.c
@@ -0,0 +1,1231 @@
+/*
+ * drivers/staging/omapdrm/omap_gem.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob.clark@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <linux/spinlock.h>
+#include <linux/shmem_fs.h>
+
+#include "omap_drv.h"
+#include "omap_dmm_tiler.h"
+
+/* remove these once drm core helpers are merged */
+struct page ** _drm_gem_get_pages(struct drm_gem_object *obj, gfp_t gfpmask);
+void _drm_gem_put_pages(struct drm_gem_object *obj, struct page **pages,
+		bool dirty, bool accessed);
+int _drm_gem_create_mmap_offset_size(struct drm_gem_object *obj, size_t size);
+
+/*
+ * GEM buffer object implementation.
+ */
+
+#define to_omap_bo(x) container_of(x, struct omap_gem_object, base)
+
+/* note: we use upper 8 bits of flags for driver-internal flags: */
+#define OMAP_BO_DMA			0x01000000	/* actually is physically contiguous */
+#define OMAP_BO_EXT_SYNC	0x02000000	/* externally allocated sync object */
+#define OMAP_BO_EXT_MEM		0x04000000	/* externally allocated memory */
+
+
+struct omap_gem_object {
+	struct drm_gem_object base;
+
+	uint32_t flags;
+
+	/** width/height for tiled formats (rounded up to slot boundaries) */
+	uint16_t width, height;
+
+	/** roll applied when mapping to DMM */
+	uint32_t roll;
+
+	/**
+	 * If buffer is allocated physically contiguous, the OMAP_BO_DMA flag
+	 * is set and the paddr is valid.  Also if the buffer is remapped in
+	 * TILER and paddr_cnt > 0, then paddr is valid.  But if you are using
+	 * the physical address and OMAP_BO_DMA is not set, then you should
+	 * be going thru omap_gem_{get,put}_paddr() to ensure the mapping is
+	 * not removed from under your feet.
+	 *
+	 * Note that OMAP_BO_SCANOUT is a hint from userspace that DMA capable
+	 * buffer is requested, but doesn't mean that it is.  Use the
+	 * OMAP_BO_DMA flag to determine if the buffer has a DMA capable
+	 * physical address.
+	 */
+	dma_addr_t paddr;
+
+	/**
+	 * # of users of paddr
+	 */
+	uint32_t paddr_cnt;
+
+	/**
+	 * tiler block used when buffer is remapped in DMM/TILER.
+	 */
+	struct tiler_block *block;
+
+	/**
+	 * Array of backing pages, if allocated.  Note that pages are never
+	 * allocated for buffers originally allocated from contiguous memory
+	 */
+	struct page **pages;
+
+	/** addresses corresponding to pages in above array */
+	dma_addr_t *addrs;
+
+	/**
+	 * Virtual address, if mapped.
+	 */
+	void *vaddr;
+
+	/**
+	 * sync-object allocated on demand (if needed)
+	 *
+	 * Per-buffer sync-object for tracking pending and completed hw/dma
+	 * read and write operations.  The layout in memory is dictated by
+	 * the SGX firmware, which uses this information to stall the command
+	 * stream if a surface is not ready yet.
+	 *
+	 * Note that when buffer is used by SGX, the sync-object needs to be
+	 * allocated from a special heap of sync-objects.  This way many sync
+	 * objects can be packed in a page, and not waste GPU virtual address
+	 * space.  Because of this we have to have a omap_gem_set_sync_object()
+	 * API to allow replacement of the syncobj after it has (potentially)
+	 * already been allocated.  A bit ugly but I haven't thought of a
+	 * better alternative.
+	 */
+	struct {
+		uint32_t write_pending;
+		uint32_t write_complete;
+		uint32_t read_pending;
+		uint32_t read_complete;
+	} *sync;
+};
+
+/* To deal with userspace mmap'ings of 2d tiled buffers, which (a) are
+ * not necessarily pinned in TILER all the time, and (b) when they are
+ * they are not necessarily page aligned, we reserve one or more small
+ * regions in each of the 2d containers to use as a user-GART where we
+ * can create a second page-aligned mapping of parts of the buffer
+ * being accessed from userspace.
+ *
+ * Note that we could optimize slightly when we know that multiple
+ * tiler containers are backed by the same PAT.. but I'll leave that
+ * for later..
+ */
+#define NUM_USERGART_ENTRIES 2
+struct usergart_entry {
+	struct tiler_block *block;	/* the reserved tiler block */
+	dma_addr_t paddr;
+	struct drm_gem_object *obj;	/* the current pinned obj */
+	pgoff_t obj_pgoff;		/* page offset of obj currently
+					   mapped in */
+};
+static struct {
+	struct usergart_entry entry[NUM_USERGART_ENTRIES];
+	int height;				/* height in rows */
+	int height_shift;		/* ilog2(height in rows) */
+	int slot_shift;			/* ilog2(width per slot) */
+	int stride_pfn;			/* stride in pages */
+	int last;				/* index of last used entry */
+} *usergart;
+
+static void evict_entry(struct drm_gem_object *obj,
+		enum tiler_fmt fmt, struct usergart_entry *entry)
+{
+	if (obj->dev->dev_mapping) {
+		size_t size = PAGE_SIZE * usergart[fmt].height;
+		loff_t off = omap_gem_mmap_offset(obj) +
+				(entry->obj_pgoff << PAGE_SHIFT);
+		unmap_mapping_range(obj->dev->dev_mapping, off, size, 1);
+	}
+
+	entry->obj = NULL;
+}
+
+/* Evict a buffer from usergart, if it is mapped there */
+static void evict(struct drm_gem_object *obj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+
+	if (omap_obj->flags & OMAP_BO_TILED) {
+		enum tiler_fmt fmt = gem2fmt(omap_obj->flags);
+		int i;
+
+		if (!usergart)
+			return;
+
+		for (i = 0; i < NUM_USERGART_ENTRIES; i++) {
+			struct usergart_entry *entry = &usergart[fmt].entry[i];
+			if (entry->obj == obj)
+				evict_entry(obj, fmt, entry);
+		}
+	}
+}
+
+/* GEM objects can either be allocated from contiguous memory (in which
+ * case obj->filp==NULL), or w/ shmem backing (obj->filp!=NULL).  But non
+ * contiguous buffers can be remapped in TILER/DMM if they need to be
+ * contiguous... but we don't do this all the time to reduce pressure
+ * on TILER/DMM space when we know at allocation time that the buffer
+ * will need to be scanned out.
+ */
+static inline bool is_shmem(struct drm_gem_object *obj)
+{
+	return obj->filp != NULL;
+}
+
+static int get_pages(struct drm_gem_object *obj, struct page ***pages);
+
+static DEFINE_SPINLOCK(sync_lock);
+
+/** ensure backing pages are allocated */
+static int omap_gem_attach_pages(struct drm_gem_object *obj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	struct page **pages;
+
+	WARN_ON(omap_obj->pages);
+
+	/* TODO: __GFP_DMA32 .. but somehow GFP_HIGHMEM is coming from the
+	 * mapping_gfp_mask(mapping) which conflicts w/ GFP_DMA32.. probably
+	 * we actually want CMA memory for it all anyways..
+	 */
+	pages = _drm_gem_get_pages(obj, GFP_KERNEL);
+	if (IS_ERR(pages)) {
+		dev_err(obj->dev->dev, "could not get pages: %ld\n", PTR_ERR(pages));
+		return PTR_ERR(pages);
+	}
+
+	/* for non-cached buffers, ensure the new pages are clean because
+	 * DSS, GPU, etc. are not cache coherent:
+	 */
+	if (omap_obj->flags & (OMAP_BO_WC|OMAP_BO_UNCACHED)) {
+		int i, npages = obj->size >> PAGE_SHIFT;
+		dma_addr_t *addrs = kmalloc(npages * sizeof(addrs), GFP_KERNEL);
+		for (i = 0; i < npages; i++) {
+			addrs[i] = dma_map_page(obj->dev->dev, pages[i],
+					0, PAGE_SIZE, DMA_BIDIRECTIONAL);
+		}
+		omap_obj->addrs = addrs;
+	}
+
+	omap_obj->pages = pages;
+	return 0;
+}
+
+/** release backing pages */
+static void omap_gem_detach_pages(struct drm_gem_object *obj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+
+	/* for non-cached buffers, ensure the new pages are clean because
+	 * DSS, GPU, etc. are not cache coherent:
+	 */
+	if (omap_obj->flags & (OMAP_BO_WC|OMAP_BO_UNCACHED)) {
+		int i, npages = obj->size >> PAGE_SHIFT;
+		for (i = 0; i < npages; i++) {
+			dma_unmap_page(obj->dev->dev, omap_obj->addrs[i],
+					PAGE_SIZE, DMA_BIDIRECTIONAL);
+		}
+		kfree(omap_obj->addrs);
+		omap_obj->addrs = NULL;
+	}
+
+	_drm_gem_put_pages(obj, omap_obj->pages, true, false);
+	omap_obj->pages = NULL;
+}
+
+/** get mmap offset */
+uint64_t omap_gem_mmap_offset(struct drm_gem_object *obj)
+{
+	if (!obj->map_list.map) {
+		/* Make it mmapable */
+		size_t size = omap_gem_mmap_size(obj);
+		int ret = _drm_gem_create_mmap_offset_size(obj, size);
+
+		if (ret) {
+			dev_err(obj->dev->dev, "could not allocate mmap offset");
+			return 0;
+		}
+	}
+
+	return (uint64_t)obj->map_list.hash.key << PAGE_SHIFT;
+}
+
+/** get mmap size */
+size_t omap_gem_mmap_size(struct drm_gem_object *obj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	size_t size = obj->size;
+
+	if (omap_obj->flags & OMAP_BO_TILED) {
+		/* for tiled buffers, the virtual size has stride rounded up
+		 * to 4kb.. (to hide the fact that row n+1 might start 16kb or
+		 * 32kb later!).  But we don't back the entire buffer with
+		 * pages, only the valid picture part.. so need to adjust for
+		 * this in the size used to mmap and generate mmap offset
+		 */
+		size = tiler_vsize(gem2fmt(omap_obj->flags),
+				omap_obj->width, omap_obj->height);
+	}
+
+	return size;
+}
+
+
+/* Normal handling for the case of faulting in non-tiled buffers */
+static int fault_1d(struct drm_gem_object *obj,
+		struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	unsigned long pfn;
+	pgoff_t pgoff;
+
+	/* We don't use vmf->pgoff since that has the fake offset: */
+	pgoff = ((unsigned long)vmf->virtual_address -
+			vma->vm_start) >> PAGE_SHIFT;
+
+	if (omap_obj->pages) {
+		pfn = page_to_pfn(omap_obj->pages[pgoff]);
+	} else {
+		BUG_ON(!(omap_obj->flags & OMAP_BO_DMA));
+		pfn = (omap_obj->paddr >> PAGE_SHIFT) + pgoff;
+	}
+
+	VERB("Inserting %p pfn %lx, pa %lx", vmf->virtual_address,
+			pfn, pfn << PAGE_SHIFT);
+
+	return vm_insert_mixed(vma, (unsigned long)vmf->virtual_address, pfn);
+}
+
+/* Special handling for the case of faulting in 2d tiled buffers */
+static int fault_2d(struct drm_gem_object *obj,
+		struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	struct usergart_entry *entry;
+	enum tiler_fmt fmt = gem2fmt(omap_obj->flags);
+	struct page *pages[64];  /* XXX is this too much to have on stack? */
+	unsigned long pfn;
+	pgoff_t pgoff, base_pgoff;
+	void __user *vaddr;
+	int i, ret, slots;
+
+	if (!usergart)
+		return -EFAULT;
+
+	/* TODO: this fxn might need a bit tweaking to deal w/ tiled buffers
+	 * that are wider than 4kb
+	 */
+
+	/* We don't use vmf->pgoff since that has the fake offset: */
+	pgoff = ((unsigned long)vmf->virtual_address -
+			vma->vm_start) >> PAGE_SHIFT;
+
+	/* actual address we start mapping at is rounded down to previous slot
+	 * boundary in the y direction:
+	 */
+	base_pgoff = round_down(pgoff, usergart[fmt].height);
+	vaddr = vmf->virtual_address - ((pgoff - base_pgoff) << PAGE_SHIFT);
+	entry = &usergart[fmt].entry[usergart[fmt].last];
+
+	slots = omap_obj->width >> usergart[fmt].slot_shift;
+
+	/* evict previous buffer using this usergart entry, if any: */
+	if (entry->obj)
+		evict_entry(entry->obj, fmt, entry);
+
+	entry->obj = obj;
+	entry->obj_pgoff = base_pgoff;
+
+	/* now convert base_pgoff to phys offset from virt offset:
+	 */
+	base_pgoff = (base_pgoff >> usergart[fmt].height_shift) * slots;
+
+	/* map in pages.  Note the height of the slot is also equal to the
+	 * number of pages that need to be mapped in to fill 4kb wide CPU page.
+	 * If the height is 64, then 64 pages fill a 4kb wide by 64 row region.
+	 * Beyond the valid pixel part of the buffer, we set pages[i] to NULL to
+	 * get a dummy page mapped in.. if someone reads/writes it they will get
+	 * random/undefined content, but at least it won't be corrupting
+	 * whatever other random page used to be mapped in, or other undefined
+	 * behavior.
+	 */
+	memcpy(pages, &omap_obj->pages[base_pgoff],
+			sizeof(struct page *) * slots);
+	memset(pages + slots, 0,
+			sizeof(struct page *) * (usergart[fmt].height - slots));
+
+	ret = tiler_pin(entry->block, pages, ARRAY_SIZE(pages), 0, true);
+	if (ret) {
+		dev_err(obj->dev->dev, "failed to pin: %d\n", ret);
+		return ret;
+	}
+
+	i = usergart[fmt].height;
+	pfn = entry->paddr >> PAGE_SHIFT;
+
+	VERB("Inserting %p pfn %lx, pa %lx", vmf->virtual_address,
+			pfn, pfn << PAGE_SHIFT);
+
+	while (i--) {
+		vm_insert_mixed(vma, (unsigned long)vaddr, pfn);
+		pfn += usergart[fmt].stride_pfn;
+		vaddr += PAGE_SIZE;
+	}
+
+	/* simple round-robin: */
+	usergart[fmt].last = (usergart[fmt].last + 1) % NUM_USERGART_ENTRIES;
+
+	return 0;
+}
+
+/**
+ * omap_gem_fault		-	pagefault handler for GEM objects
+ * @vma: the VMA of the GEM object
+ * @vmf: fault detail
+ *
+ * Invoked when a fault occurs on an mmap of a GEM managed area. GEM
+ * does most of the work for us including the actual map/unmap calls
+ * but we need to do the actual page work.
+ *
+ * The VMA was set up by GEM. In doing so it also ensured that the
+ * vma->vm_private_data points to the GEM object that is backing this
+ * mapping.
+ */
+int omap_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct drm_gem_object *obj = vma->vm_private_data;
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	struct drm_device *dev = obj->dev;
+	struct page **pages;
+	int ret;
+
+	/* Make sure we don't parallel update on a fault, nor move or remove
+	 * something from beneath our feet
+	 */
+	mutex_lock(&dev->struct_mutex);
+
+	/* if a shmem backed object, make sure we have pages attached now */
+	ret = get_pages(obj, &pages);
+	if (ret) {
+		goto fail;
+	}
+
+	/* where should we do corresponding put_pages().. we are mapping
+	 * the original page, rather than thru a GART, so we can't rely
+	 * on eviction to trigger this.  But munmap() or all mappings should
+	 * probably trigger put_pages()?
+	 */
+
+	if (omap_obj->flags & OMAP_BO_TILED)
+		ret = fault_2d(obj, vma, vmf);
+	else
+		ret = fault_1d(obj, vma, vmf);
+
+
+fail:
+	mutex_unlock(&dev->struct_mutex);
+	switch (ret) {
+	case 0:
+	case -ERESTARTSYS:
+	case -EINTR:
+		return VM_FAULT_NOPAGE;
+	case -ENOMEM:
+		return VM_FAULT_OOM;
+	default:
+		return VM_FAULT_SIGBUS;
+	}
+}
+
+/** We override mainly to fix up some of the vm mapping flags.. */
+int omap_gem_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct omap_gem_object *omap_obj;
+	int ret;
+
+	ret = drm_gem_mmap(filp, vma);
+	if (ret) {
+		DBG("mmap failed: %d", ret);
+		return ret;
+	}
+
+	/* after drm_gem_mmap(), it is safe to access the obj */
+	omap_obj = to_omap_bo(vma->vm_private_data);
+
+	vma->vm_flags &= ~VM_PFNMAP;
+	vma->vm_flags |= VM_MIXEDMAP;
+
+	if (omap_obj->flags & OMAP_BO_WC) {
+		vma->vm_page_prot = pgprot_writecombine(vm_get_page_prot(vma->vm_flags));
+	} else if (omap_obj->flags & OMAP_BO_UNCACHED) {
+		vma->vm_page_prot = pgprot_noncached(vm_get_page_prot(vma->vm_flags));
+	} else {
+		vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
+	}
+
+	return ret;
+}
+
+/**
+ * omap_gem_dumb_create	-	create a dumb buffer
+ * @drm_file: our client file
+ * @dev: our device
+ * @args: the requested arguments copied from userspace
+ *
+ * Allocate a buffer suitable for use for a frame buffer of the
+ * form described by user space. Give userspace a handle by which
+ * to reference it.
+ */
+int omap_gem_dumb_create(struct drm_file *file, struct drm_device *dev,
+		struct drm_mode_create_dumb *args)
+{
+	union omap_gem_size gsize;
+
+	/* in case someone tries to feed us a completely bogus stride: */
+	args->pitch = align_pitch(args->pitch, args->width, args->bpp);
+	args->size = PAGE_ALIGN(args->pitch * args->height);
+
+	gsize = (union omap_gem_size){
+		.bytes = args->size,
+	};
+
+	return omap_gem_new_handle(dev, file, gsize,
+			OMAP_BO_SCANOUT | OMAP_BO_WC, &args->handle);
+}
+
+/**
+ * omap_gem_dumb_destroy	-	destroy a dumb buffer
+ * @file: client file
+ * @dev: our DRM device
+ * @handle: the object handle
+ *
+ * Destroy a handle that was created via omap_gem_dumb_create.
+ */
+int omap_gem_dumb_destroy(struct drm_file *file, struct drm_device *dev,
+		uint32_t handle)
+{
+	/* No special work needed, drop the reference and see what falls out */
+	return drm_gem_handle_delete(file, handle);
+}
+
+/**
+ * omap_gem_dumb_map	-	buffer mapping for dumb interface
+ * @file: our drm client file
+ * @dev: drm device
+ * @handle: GEM handle to the object (from dumb_create)
+ *
+ * Do the necessary setup to allow the mapping of the frame buffer
+ * into user memory. We don't have to do much here at the moment.
+ */
+int omap_gem_dumb_map_offset(struct drm_file *file, struct drm_device *dev,
+		uint32_t handle, uint64_t *offset)
+{
+	struct drm_gem_object *obj;
+	int ret = 0;
+
+	/* GEM does all our handle to object mapping */
+	obj = drm_gem_object_lookup(dev, file, handle);
+	if (obj == NULL) {
+		ret = -ENOENT;
+		goto fail;
+	}
+
+	*offset = omap_gem_mmap_offset(obj);
+
+	drm_gem_object_unreference_unlocked(obj);
+
+fail:
+	return ret;
+}
+
+/* Set scrolling position.  This allows us to implement fast scrolling
+ * for console.
+ */
+int omap_gem_roll(struct drm_gem_object *obj, uint32_t roll)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	uint32_t npages = obj->size >> PAGE_SHIFT;
+	int ret = 0;
+
+	if (roll > npages) {
+		dev_err(obj->dev->dev, "invalid roll: %d\n", roll);
+		return -EINVAL;
+	}
+
+	omap_obj->roll = roll;
+
+	if (in_atomic() || mutex_is_locked(&obj->dev->struct_mutex)) {
+		/* this can get called from fbcon in atomic context.. so
+		 * just ignore it and wait for next time called from
+		 * interruptible context to update the PAT.. the result
+		 * may be that user sees wrap-around instead of scrolling
+		 * momentarily on the screen.  If we wanted to be fancier
+		 * we could perhaps schedule some workqueue work at this
+		 * point.
+		 */
+		return 0;
+	}
+
+	mutex_lock(&obj->dev->struct_mutex);
+
+	/* if we aren't mapped yet, we don't need to do anything */
+	if (omap_obj->block) {
+		struct page **pages;
+		ret = get_pages(obj, &pages);
+		if (ret)
+			goto fail;
+		ret = tiler_pin(omap_obj->block, pages, npages, roll, true);
+		if (ret)
+			dev_err(obj->dev->dev, "could not repin: %d\n", ret);
+	}
+
+fail:
+	mutex_unlock(&obj->dev->struct_mutex);
+
+	return ret;
+}
+
+/* Get physical address for DMA.. if 'remap' is true, and the buffer is not
+ * already contiguous, remap it to pin in physically contiguous memory.. (ie.
+ * map in TILER)
+ */
+int omap_gem_get_paddr(struct drm_gem_object *obj,
+		dma_addr_t *paddr, bool remap)
+{
+	struct omap_drm_private *priv = obj->dev->dev_private;
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = 0;
+
+	mutex_lock(&obj->dev->struct_mutex);
+
+	if (remap && is_shmem(obj) && priv->has_dmm) {
+		if (omap_obj->paddr_cnt == 0) {
+			struct page **pages;
+			uint32_t npages = obj->size >> PAGE_SHIFT;
+			enum tiler_fmt fmt = gem2fmt(omap_obj->flags);
+			struct tiler_block *block;
+
+			BUG_ON(omap_obj->block);
+
+			ret = get_pages(obj, &pages);
+			if (ret)
+				goto fail;
+
+			if (omap_obj->flags & OMAP_BO_TILED) {
+				block = tiler_reserve_2d(fmt,
+						omap_obj->width,
+						omap_obj->height, 0);
+			} else {
+				block = tiler_reserve_1d(obj->size);
+			}
+
+			if (IS_ERR(block)) {
+				ret = PTR_ERR(block);
+				dev_err(obj->dev->dev,
+					"could not remap: %d (%d)\n", ret, fmt);
+				goto fail;
+			}
+
+			/* TODO: enable async refill.. */
+			ret = tiler_pin(block, pages, npages,
+					omap_obj->roll, true);
+			if (ret) {
+				tiler_release(block);
+				dev_err(obj->dev->dev,
+						"could not pin: %d\n", ret);
+				goto fail;
+			}
+
+			omap_obj->paddr = tiler_ssptr(block);
+			omap_obj->block = block;
+
+			DBG("got paddr: %08x", omap_obj->paddr);
+		}
+
+		omap_obj->paddr_cnt++;
+
+		*paddr = omap_obj->paddr;
+	} else if (omap_obj->flags & OMAP_BO_DMA) {
+		*paddr = omap_obj->paddr;
+	} else {
+		ret = -EINVAL;
+	}
+
+fail:
+	mutex_unlock(&obj->dev->struct_mutex);
+
+	return ret;
+}
+
+/* Release physical address, when DMA is no longer being performed.. this
+ * could potentially unpin and unmap buffers from TILER
+ */
+int omap_gem_put_paddr(struct drm_gem_object *obj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = 0;
+
+	mutex_lock(&obj->dev->struct_mutex);
+	if (omap_obj->paddr_cnt > 0) {
+		omap_obj->paddr_cnt--;
+		if (omap_obj->paddr_cnt == 0) {
+			ret = tiler_unpin(omap_obj->block);
+			if (ret) {
+				dev_err(obj->dev->dev,
+					"could not unpin pages: %d\n", ret);
+				goto fail;
+			}
+			ret = tiler_release(omap_obj->block);
+			if (ret) {
+				dev_err(obj->dev->dev,
+					"could not release unmap: %d\n", ret);
+			}
+			omap_obj->block = NULL;
+		}
+	}
+fail:
+	mutex_unlock(&obj->dev->struct_mutex);
+	return ret;
+}
+
+/* acquire pages when needed (for example, for DMA where physically
+ * contiguous buffer is not required
+ */
+static int get_pages(struct drm_gem_object *obj, struct page ***pages)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = 0;
+
+	if (is_shmem(obj) && !omap_obj->pages) {
+		ret = omap_gem_attach_pages(obj);
+		if (ret) {
+			dev_err(obj->dev->dev, "could not attach pages\n");
+			return ret;
+		}
+	}
+
+	/* TODO: even phys-contig.. we should have a list of pages? */
+	*pages = omap_obj->pages;
+
+	return 0;
+}
+
+int omap_gem_get_pages(struct drm_gem_object *obj, struct page ***pages)
+{
+	int ret;
+	mutex_lock(&obj->dev->struct_mutex);
+	ret = get_pages(obj, pages);
+	mutex_unlock(&obj->dev->struct_mutex);
+	return ret;
+}
+
+/* release pages when DMA no longer being performed */
+int omap_gem_put_pages(struct drm_gem_object *obj)
+{
+	/* do something here if we dynamically attach/detach pages.. at
+	 * least they would no longer need to be pinned if everyone has
+	 * released the pages..
+	 */
+	return 0;
+}
+
+/* Get kernel virtual address for CPU access.. this more or less only
+ * exists for omap_fbdev.  This should be called with struct_mutex
+ * held.
+ */
+void *omap_gem_vaddr(struct drm_gem_object *obj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	WARN_ON(! mutex_is_locked(&obj->dev->struct_mutex));
+	if (!omap_obj->vaddr) {
+		struct page **pages;
+		int ret = get_pages(obj, &pages);
+		if (ret)
+			return ERR_PTR(ret);
+		omap_obj->vaddr = vmap(pages, obj->size >> PAGE_SHIFT,
+				VM_MAP, pgprot_writecombine(PAGE_KERNEL));
+	}
+	return omap_obj->vaddr;
+}
+
+/* Buffer Synchronization:
+ */
+
+struct omap_gem_sync_waiter {
+	struct list_head list;
+	struct omap_gem_object *omap_obj;
+	enum omap_gem_op op;
+	uint32_t read_target, write_target;
+	/* notify called w/ sync_lock held */
+	void (*notify)(void *arg);
+	void *arg;
+};
+
+/* list of omap_gem_sync_waiter.. the notify fxn gets called back when
+ * the read and/or write target count is achieved which can call a user
+ * callback (ex. to kick 3d and/or 2d), wakeup blocked task (prep for
+ * cpu access), etc.
+ */
+static LIST_HEAD(waiters);
+
+static inline bool is_waiting(struct omap_gem_sync_waiter *waiter)
+{
+	struct omap_gem_object *omap_obj = waiter->omap_obj;
+	if ((waiter->op & OMAP_GEM_READ) &&
+			(omap_obj->sync->read_complete < waiter->read_target))
+		return true;
+	if ((waiter->op & OMAP_GEM_WRITE) &&
+			(omap_obj->sync->write_complete < waiter->write_target))
+		return true;
+	return false;
+}
+
+/* macro for sync debug.. */
+#define SYNCDBG 0
+#define SYNC(fmt, ...) do { if (SYNCDBG) \
+		printk(KERN_ERR "%s:%d: "fmt"\n", \
+				__func__, __LINE__, ##__VA_ARGS__); \
+	} while (0)
+
+
+static void sync_op_update(void)
+{
+	struct omap_gem_sync_waiter *waiter, *n;
+	list_for_each_entry_safe(waiter, n, &waiters, list) {
+		if (!is_waiting(waiter)) {
+			list_del(&waiter->list);
+			SYNC("notify: %p", waiter);
+			waiter->notify(waiter->arg);
+			kfree(waiter);
+		}
+	}
+}
+
+static inline int sync_op(struct drm_gem_object *obj,
+		enum omap_gem_op op, bool start)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = 0;
+
+	spin_lock(&sync_lock);
+
+	if (!omap_obj->sync) {
+		omap_obj->sync = kzalloc(sizeof(*omap_obj->sync), GFP_ATOMIC);
+		if (!omap_obj->sync) {
+			ret = -ENOMEM;
+			goto unlock;
+		}
+	}
+
+	if (start) {
+		if (op & OMAP_GEM_READ)
+			omap_obj->sync->read_pending++;
+		if (op & OMAP_GEM_WRITE)
+			omap_obj->sync->write_pending++;
+	} else {
+		if (op & OMAP_GEM_READ)
+			omap_obj->sync->read_complete++;
+		if (op & OMAP_GEM_WRITE)
+			omap_obj->sync->write_complete++;
+		sync_op_update();
+	}
+
+unlock:
+	spin_unlock(&sync_lock);
+
+	return ret;
+}
+
+/* it is a bit lame to handle updates in this sort of polling way, but
+ * in case of PVR, the GPU can directly update read/write complete
+ * values, and not really tell us which ones it updated.. this also
+ * means that sync_lock is not quite sufficient.  So we'll need to
+ * do something a bit better when it comes time to add support for
+ * separate 2d hw..
+ */
+void omap_gem_op_update(void)
+{
+	spin_lock(&sync_lock);
+	sync_op_update();
+	spin_unlock(&sync_lock);
+}
+
+/* mark the start of read and/or write operation */
+int omap_gem_op_start(struct drm_gem_object *obj, enum omap_gem_op op)
+{
+	return sync_op(obj, op, true);
+}
+
+int omap_gem_op_finish(struct drm_gem_object *obj, enum omap_gem_op op)
+{
+	return sync_op(obj, op, false);
+}
+
+static DECLARE_WAIT_QUEUE_HEAD(sync_event);
+
+static void sync_notify(void *arg)
+{
+	struct task_struct **waiter_task = arg;
+	*waiter_task = NULL;
+	wake_up_all(&sync_event);
+}
+
+int omap_gem_op_sync(struct drm_gem_object *obj, enum omap_gem_op op)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = 0;
+	if (omap_obj->sync) {
+		struct task_struct *waiter_task = current;
+		struct omap_gem_sync_waiter *waiter =
+				kzalloc(sizeof(*waiter), GFP_KERNEL);
+
+		if (!waiter) {
+			return -ENOMEM;
+		}
+
+		waiter->omap_obj = omap_obj;
+		waiter->op = op;
+		waiter->read_target = omap_obj->sync->read_pending;
+		waiter->write_target = omap_obj->sync->write_pending;
+		waiter->notify = sync_notify;
+		waiter->arg = &waiter_task;
+
+		spin_lock(&sync_lock);
+		if (is_waiting(waiter)) {
+			SYNC("waited: %p", waiter);
+			list_add_tail(&waiter->list, &waiters);
+			spin_unlock(&sync_lock);
+			ret = wait_event_interruptible(sync_event,
+					(waiter_task == NULL));
+			spin_lock(&sync_lock);
+			if (waiter_task) {
+				SYNC("interrupted: %p", waiter);
+				/* we were interrupted */
+				list_del(&waiter->list);
+				waiter_task = NULL;
+			} else {
+				/* freed in sync_op_update() */
+				waiter = NULL;
+			}
+		}
+		spin_unlock(&sync_lock);
+
+		if (waiter) {
+			kfree(waiter);
+		}
+	}
+	return ret;
+}
+
+/* call fxn(arg), either synchronously or asynchronously if the op
+ * is currently blocked..  fxn() can be called from any context
+ *
+ * (TODO for now fxn is called back from whichever context calls
+ * omap_gem_op_update().. but this could be better defined later
+ * if needed)
+ *
+ * TODO more code in common w/ _sync()..
+ */
+int omap_gem_op_async(struct drm_gem_object *obj, enum omap_gem_op op,
+		void (*fxn)(void *arg), void *arg)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	if (omap_obj->sync) {
+		struct omap_gem_sync_waiter *waiter =
+				kzalloc(sizeof(*waiter), GFP_ATOMIC);
+
+		if (!waiter) {
+			return -ENOMEM;
+		}
+
+		waiter->omap_obj = omap_obj;
+		waiter->op = op;
+		waiter->read_target = omap_obj->sync->read_pending;
+		waiter->write_target = omap_obj->sync->write_pending;
+		waiter->notify = fxn;
+		waiter->arg = arg;
+
+		spin_lock(&sync_lock);
+		if (is_waiting(waiter)) {
+			SYNC("waited: %p", waiter);
+			list_add_tail(&waiter->list, &waiters);
+			spin_unlock(&sync_lock);
+			return 0;
+		}
+
+		spin_unlock(&sync_lock);
+	}
+
+	/* no waiting.. */
+	fxn(arg);
+
+	return 0;
+}
+
+/* special API so PVR can update the buffer to use a sync-object allocated
+ * from it's sync-obj heap.  Only used for a newly allocated (from PVR's
+ * perspective) sync-object, so we overwrite the new syncobj w/ values
+ * from the already allocated syncobj (if there is one)
+ */
+int omap_gem_set_sync_object(struct drm_gem_object *obj, void *syncobj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = 0;
+
+	spin_lock(&sync_lock);
+
+	if ((omap_obj->flags & OMAP_BO_EXT_SYNC) && !syncobj) {
+		/* clearing a previously set syncobj */
+		syncobj = kzalloc(sizeof(*omap_obj->sync), GFP_ATOMIC);
+		if (!syncobj) {
+			ret = -ENOMEM;
+			goto unlock;
+		}
+		memcpy(syncobj, omap_obj->sync, sizeof(*omap_obj->sync));
+		omap_obj->flags &= ~OMAP_BO_EXT_SYNC;
+		omap_obj->sync = syncobj;
+	} else if (syncobj && !(omap_obj->flags & OMAP_BO_EXT_SYNC)) {
+		/* replacing an existing syncobj */
+		if (omap_obj->sync) {
+			memcpy(syncobj, omap_obj->sync, sizeof(*omap_obj->sync));
+			kfree(omap_obj->sync);
+		}
+		omap_obj->flags |= OMAP_BO_EXT_SYNC;
+		omap_obj->sync = syncobj;
+	}
+
+unlock:
+	spin_unlock(&sync_lock);
+	return ret;
+}
+
+int omap_gem_init_object(struct drm_gem_object *obj)
+{
+	return -EINVAL;          /* unused */
+}
+
+/* don't call directly.. called from GEM core when it is time to actually
+ * free the object..
+ */
+void omap_gem_free_object(struct drm_gem_object *obj)
+{
+	struct drm_device *dev = obj->dev;
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+
+	evict(obj);
+
+	if (obj->map_list.map) {
+		drm_gem_free_mmap_offset(obj);
+	}
+
+	/* don't free externally allocated backing memory */
+	if (!(omap_obj->flags & OMAP_BO_EXT_MEM)) {
+		if (omap_obj->pages) {
+			omap_gem_detach_pages(obj);
+		}
+		if (!is_shmem(obj)) {
+			dma_free_writecombine(dev->dev, obj->size,
+					omap_obj->vaddr, omap_obj->paddr);
+		} else if (omap_obj->vaddr) {
+			vunmap(omap_obj->vaddr);
+		}
+	}
+
+	/* don't free externally allocated syncobj */
+	if (!(omap_obj->flags & OMAP_BO_EXT_SYNC)) {
+		kfree(omap_obj->sync);
+	}
+
+	drm_gem_object_release(obj);
+
+	kfree(obj);
+}
+
+/* convenience method to construct a GEM buffer object, and userspace handle */
+int omap_gem_new_handle(struct drm_device *dev, struct drm_file *file,
+		union omap_gem_size gsize, uint32_t flags, uint32_t *handle)
+{
+	struct drm_gem_object *obj;
+	int ret;
+
+	obj = omap_gem_new(dev, gsize, flags);
+	if (!obj)
+		return -ENOMEM;
+
+	ret = drm_gem_handle_create(file, obj, handle);
+	if (ret) {
+		drm_gem_object_release(obj);
+		kfree(obj); /* TODO isn't there a dtor to call? just copying i915 */
+		return ret;
+	}
+
+	/* drop reference from allocate - handle holds it now */
+	drm_gem_object_unreference_unlocked(obj);
+
+	return 0;
+}
+
+/* GEM buffer object constructor */
+struct drm_gem_object *omap_gem_new(struct drm_device *dev,
+		union omap_gem_size gsize, uint32_t flags)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	struct omap_gem_object *omap_obj;
+	struct drm_gem_object *obj = NULL;
+	size_t size;
+	int ret;
+
+	if (flags & OMAP_BO_TILED) {
+		if (!usergart) {
+			dev_err(dev->dev, "Tiled buffers require DMM\n");
+			goto fail;
+		}
+
+		/* tiled buffers are always shmem paged backed.. when they are
+		 * scanned out, they are remapped into DMM/TILER
+		 */
+		flags &= ~OMAP_BO_SCANOUT;
+
+		/* currently don't allow cached buffers.. there is some caching
+		 * stuff that needs to be handled better
+		 */
+		flags &= ~(OMAP_BO_CACHED|OMAP_BO_UNCACHED);
+		flags |= OMAP_BO_WC;
+
+		/* align dimensions to slot boundaries... */
+		tiler_align(gem2fmt(flags),
+				&gsize.tiled.width, &gsize.tiled.height);
+
+		/* ...and calculate size based on aligned dimensions */
+		size = tiler_size(gem2fmt(flags),
+				gsize.tiled.width, gsize.tiled.height);
+	} else {
+		size = PAGE_ALIGN(gsize.bytes);
+	}
+
+	omap_obj = kzalloc(sizeof(*omap_obj), GFP_KERNEL);
+	if (!omap_obj) {
+		dev_err(dev->dev, "could not allocate GEM object\n");
+		goto fail;
+	}
+
+	obj = &omap_obj->base;
+
+	if ((flags & OMAP_BO_SCANOUT) && !priv->has_dmm) {
+		/* attempt to allocate contiguous memory if we don't
+		 * have DMM for remappign discontiguous buffers
+		 */
+		omap_obj->vaddr =  dma_alloc_writecombine(dev->dev, size,
+				&omap_obj->paddr, GFP_KERNEL);
+		if (omap_obj->vaddr) {
+			flags |= OMAP_BO_DMA;
+		}
+	}
+
+	omap_obj->flags = flags;
+
+	if (flags & OMAP_BO_TILED) {
+		omap_obj->width = gsize.tiled.width;
+		omap_obj->height = gsize.tiled.height;
+	}
+
+	if (flags & (OMAP_BO_DMA|OMAP_BO_EXT_MEM)) {
+		ret = drm_gem_private_object_init(dev, obj, size);
+	} else {
+		ret = drm_gem_object_init(dev, obj, size);
+	}
+
+	if (ret) {
+		goto fail;
+	}
+
+	return obj;
+
+fail:
+	if (obj) {
+		omap_gem_free_object(obj);
+	}
+	return NULL;
+}
+
+/* init/cleanup.. if DMM is used, we need to set some stuff up.. */
+void omap_gem_init(struct drm_device *dev)
+{
+	struct omap_drm_private *priv = dev->dev_private;
+	const enum tiler_fmt fmts[] = {
+			TILFMT_8BIT, TILFMT_16BIT, TILFMT_32BIT
+	};
+	int i, j, ret;
+
+	ret = omap_dmm_init(dev);
+	if (ret) {
+		/* DMM only supported on OMAP4 and later, so this isn't fatal */
+		dev_warn(dev->dev, "omap_dmm_init failed, disabling DMM\n");
+		return;
+	}
+
+	usergart = kzalloc(3 * sizeof(*usergart), GFP_KERNEL);
+	if (!usergart) {
+		dev_warn(dev->dev, "could not allocate usergart\n");
+		return;
+	}
+
+	/* reserve 4k aligned/wide regions for userspace mappings: */
+	for (i = 0; i < ARRAY_SIZE(fmts); i++) {
+		uint16_t h = 1, w = PAGE_SIZE >> i;
+		tiler_align(fmts[i], &w, &h);
+		/* note: since each region is 1 4kb page wide, and minimum
+		 * number of rows, the height ends up being the same as the
+		 * # of pages in the region
+		 */
+		usergart[i].height = h;
+		usergart[i].height_shift = ilog2(h);
+		usergart[i].stride_pfn = tiler_stride(fmts[i]) >> PAGE_SHIFT;
+		usergart[i].slot_shift = ilog2((PAGE_SIZE / h) >> i);
+		for (j = 0; j < NUM_USERGART_ENTRIES; j++) {
+			struct usergart_entry *entry = &usergart[i].entry[j];
+			struct tiler_block *block =
+					tiler_reserve_2d(fmts[i], w, h,
+							PAGE_SIZE);
+			if (IS_ERR(block)) {
+				dev_err(dev->dev,
+						"reserve failed: %d, %d, %ld\n",
+						i, j, PTR_ERR(block));
+				return;
+			}
+			entry->paddr = tiler_ssptr(block);
+			entry->block = block;
+
+			DBG("%d:%d: %dx%d: paddr=%08x stride=%d", i, j, w, h,
+					entry->paddr,
+					usergart[i].stride_pfn << PAGE_SHIFT);
+		}
+	}
+
+	priv->has_dmm = true;
+}
+
+void omap_gem_deinit(struct drm_device *dev)
+{
+	/* I believe we can rely on there being no more outstanding GEM
+	 * objects which could depend on usergart/dmm at this point.
+	 */
+	omap_dmm_remove();
+	kfree(usergart);
+}
diff --git a/drivers/staging/omapdrm/omap_gem_helpers.c b/drivers/staging/omapdrm/omap_gem_helpers.c
new file mode 100644
index 000000000000..29275c7209e9
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_gem_helpers.c
@@ -0,0 +1,169 @@
+/*
+ * drivers/staging/omapdrm/omap_gem_helpers.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob.clark@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* temporary copy of drm_gem_{get,put}_pages() until the
+ * "drm/gem: add functions to get/put pages" patch is merged..
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/shmem_fs.h>
+
+#include <drm/drmP.h>
+
+/**
+ * drm_gem_get_pages - helper to allocate backing pages for a GEM object
+ * @obj: obj in question
+ * @gfpmask: gfp mask of requested pages
+ */
+struct page ** _drm_gem_get_pages(struct drm_gem_object *obj, gfp_t gfpmask)
+{
+	struct inode *inode;
+	struct address_space *mapping;
+	struct page *p, **pages;
+	int i, npages;
+
+	/* This is the shared memory object that backs the GEM resource */
+	inode = obj->filp->f_path.dentry->d_inode;
+	mapping = inode->i_mapping;
+
+	npages = obj->size >> PAGE_SHIFT;
+
+	pages = drm_malloc_ab(npages, sizeof(struct page *));
+	if (pages == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	gfpmask |= mapping_gfp_mask(mapping);
+
+	for (i = 0; i < npages; i++) {
+		p = shmem_read_mapping_page_gfp(mapping, i, gfpmask);
+		if (IS_ERR(p))
+			goto fail;
+		pages[i] = p;
+
+		/* There is a hypothetical issue w/ drivers that require
+		 * buffer memory in the low 4GB.. if the pages are un-
+		 * pinned, and swapped out, they can end up swapped back
+		 * in above 4GB.  If pages are already in memory, then
+		 * shmem_read_mapping_page_gfp will ignore the gfpmask,
+		 * even if the already in-memory page disobeys the mask.
+		 *
+		 * It is only a theoretical issue today, because none of
+		 * the devices with this limitation can be populated with
+		 * enough memory to trigger the issue.  But this BUG_ON()
+		 * is here as a reminder in case the problem with
+		 * shmem_read_mapping_page_gfp() isn't solved by the time
+		 * it does become a real issue.
+		 *
+		 * See this thread: http://lkml.org/lkml/2011/7/11/238
+		 */
+		BUG_ON((gfpmask & __GFP_DMA32) &&
+				(page_to_pfn(p) >= 0x00100000UL));
+	}
+
+	return pages;
+
+fail:
+	while (i--) {
+		page_cache_release(pages[i]);
+	}
+	drm_free_large(pages);
+	return ERR_PTR(PTR_ERR(p));
+}
+
+/**
+ * drm_gem_put_pages - helper to free backing pages for a GEM object
+ * @obj: obj in question
+ * @pages: pages to free
+ */
+void _drm_gem_put_pages(struct drm_gem_object *obj, struct page **pages,
+		bool dirty, bool accessed)
+{
+	int i, npages;
+
+	npages = obj->size >> PAGE_SHIFT;
+
+	for (i = 0; i < npages; i++) {
+		if (dirty)
+			set_page_dirty(pages[i]);
+
+		if (accessed)
+			mark_page_accessed(pages[i]);
+
+		/* Undo the reference we took when populating the table */
+		page_cache_release(pages[i]);
+	}
+
+	drm_free_large(pages);
+}
+
+int
+_drm_gem_create_mmap_offset_size(struct drm_gem_object *obj, size_t size)
+{
+	struct drm_device *dev = obj->dev;
+	struct drm_gem_mm *mm = dev->mm_private;
+	struct drm_map_list *list;
+	struct drm_local_map *map;
+	int ret = 0;
+
+	/* Set the object up for mmap'ing */
+	list = &obj->map_list;
+	list->map = kzalloc(sizeof(struct drm_map_list), GFP_KERNEL);
+	if (!list->map)
+		return -ENOMEM;
+
+	map = list->map;
+	map->type = _DRM_GEM;
+	map->size = size;
+	map->handle = obj;
+
+	/* Get a DRM GEM mmap offset allocated... */
+	list->file_offset_node = drm_mm_search_free(&mm->offset_manager,
+			size / PAGE_SIZE, 0, 0);
+
+	if (!list->file_offset_node) {
+		DRM_ERROR("failed to allocate offset for bo %d\n", obj->name);
+		ret = -ENOSPC;
+		goto out_free_list;
+	}
+
+	list->file_offset_node = drm_mm_get_block(list->file_offset_node,
+			size / PAGE_SIZE, 0);
+	if (!list->file_offset_node) {
+		ret = -ENOMEM;
+		goto out_free_list;
+	}
+
+	list->hash.key = list->file_offset_node->start;
+	ret = drm_ht_insert_item(&mm->offset_hash, &list->hash);
+	if (ret) {
+		DRM_ERROR("failed to add to map hash\n");
+		goto out_free_mm;
+	}
+
+	return 0;
+
+out_free_mm:
+	drm_mm_put_block(list->file_offset_node);
+out_free_list:
+	kfree(list->map);
+	list->map = NULL;
+
+	return ret;
+}
diff --git a/drivers/staging/omapdrm/omap_priv.h b/drivers/staging/omapdrm/omap_priv.h
new file mode 100644
index 000000000000..c324709aa9a1
--- /dev/null
+++ b/drivers/staging/omapdrm/omap_priv.h
@@ -0,0 +1,47 @@
+/*
+ * include/drm/omap_priv.h
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Rob Clark <rob@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __OMAP_PRIV_H__
+#define __OMAP_PRIV_H__
+
+/* Non-userspace facing APIs
+ */
+
+/* optional platform data to configure the default configuration of which
+ * pipes/overlays/CRTCs are used.. if this is not provided, then instead the
+ * first CONFIG_DRM_OMAP_NUM_CRTCS are used, and they are each connected to
+ * one manager, with priority given to managers that are connected to
+ * detected devices.  This should be a good default behavior for most cases,
+ * but yet there still might be times when you wish to do something different.
+ */
+struct omap_kms_platform_data {
+	int ovl_cnt;
+	const int *ovl_ids;
+	int mgr_cnt;
+	const int *mgr_ids;
+	int dev_cnt;
+	const char **dev_names;
+};
+
+struct omap_drm_platform_data {
+	struct omap_kms_platform_data *kms_pdata;
+	struct omap_dmm_platform_data *dmm_pdata;
+};
+
+#endif /* __OMAP_DRM_H__ */
diff --git a/drivers/staging/omapdrm/tcm-sita.c b/drivers/staging/omapdrm/tcm-sita.c
new file mode 100644
index 000000000000..10d5ac3dae4b
--- /dev/null
+++ b/drivers/staging/omapdrm/tcm-sita.c
@@ -0,0 +1,703 @@
+/*
+ * tcm-sita.c
+ *
+ * SImple Tiler Allocator (SiTA): 2D and 1D allocation(reservation) algorithm
+ *
+ * Authors: Ravi Ramachandra <r.ramachandra@ti.com>,
+ *          Lajos Molnar <molnar@ti.com>
+ *
+ * Copyright (C) 2009-2010 Texas Instruments, Inc.
+ *
+ * This package is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
+ * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ */
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "tcm-sita.h"
+
+#define ALIGN_DOWN(value, align) ((value) & ~((align) - 1))
+
+/* Individual selection criteria for different scan areas */
+static s32 CR_L2R_T2B = CR_BIAS_HORIZONTAL;
+static s32 CR_R2L_T2B = CR_DIAGONAL_BALANCE;
+
+/*********************************************
+ *	TCM API - Sita Implementation
+ *********************************************/
+static s32 sita_reserve_2d(struct tcm *tcm, u16 h, u16 w, u8 align,
+			   struct tcm_area *area);
+static s32 sita_reserve_1d(struct tcm *tcm, u32 slots, struct tcm_area *area);
+static s32 sita_free(struct tcm *tcm, struct tcm_area *area);
+static void sita_deinit(struct tcm *tcm);
+
+/*********************************************
+ *	Main Scanner functions
+ *********************************************/
+static s32 scan_areas_and_find_fit(struct tcm *tcm, u16 w, u16 h, u16 align,
+				   struct tcm_area *area);
+
+static s32 scan_l2r_t2b(struct tcm *tcm, u16 w, u16 h, u16 align,
+			struct tcm_area *field, struct tcm_area *area);
+
+static s32 scan_r2l_t2b(struct tcm *tcm, u16 w, u16 h, u16 align,
+			struct tcm_area *field, struct tcm_area *area);
+
+static s32 scan_r2l_b2t_one_dim(struct tcm *tcm, u32 num_slots,
+			struct tcm_area *field, struct tcm_area *area);
+
+/*********************************************
+ *	Support Infrastructure Methods
+ *********************************************/
+static s32 is_area_free(struct tcm_area ***map, u16 x0, u16 y0, u16 w, u16 h);
+
+static s32 update_candidate(struct tcm *tcm, u16 x0, u16 y0, u16 w, u16 h,
+			    struct tcm_area *field, s32 criteria,
+			    struct score *best);
+
+static void get_nearness_factor(struct tcm_area *field,
+				struct tcm_area *candidate,
+				struct nearness_factor *nf);
+
+static void get_neighbor_stats(struct tcm *tcm, struct tcm_area *area,
+			       struct neighbor_stats *stat);
+
+static void fill_area(struct tcm *tcm,
+				struct tcm_area *area, struct tcm_area *parent);
+
+
+/*********************************************/
+
+/*********************************************
+ *	Utility Methods
+ *********************************************/
+struct tcm *sita_init(u16 width, u16 height, struct tcm_pt *attr)
+{
+	struct tcm *tcm;
+	struct sita_pvt *pvt;
+	struct tcm_area area = {0};
+	s32 i;
+
+	if (width == 0 || height == 0)
+		return NULL;
+
+	tcm = kmalloc(sizeof(*tcm), GFP_KERNEL);
+	pvt = kmalloc(sizeof(*pvt), GFP_KERNEL);
+	if (!tcm || !pvt)
+		goto error;
+
+	memset(tcm, 0, sizeof(*tcm));
+	memset(pvt, 0, sizeof(*pvt));
+
+	/* Updating the pointers to SiTA implementation APIs */
+	tcm->height = height;
+	tcm->width = width;
+	tcm->reserve_2d = sita_reserve_2d;
+	tcm->reserve_1d = sita_reserve_1d;
+	tcm->free = sita_free;
+	tcm->deinit = sita_deinit;
+	tcm->pvt = (void *)pvt;
+
+	spin_lock_init(&(pvt->lock));
+
+	/* Creating tam map */
+	pvt->map = kmalloc(sizeof(*pvt->map) * tcm->width, GFP_KERNEL);
+	if (!pvt->map)
+		goto error;
+
+	for (i = 0; i < tcm->width; i++) {
+		pvt->map[i] =
+			kmalloc(sizeof(**pvt->map) * tcm->height,
+								GFP_KERNEL);
+		if (pvt->map[i] == NULL) {
+			while (i--)
+				kfree(pvt->map[i]);
+			kfree(pvt->map);
+			goto error;
+		}
+	}
+
+	if (attr && attr->x <= tcm->width && attr->y <= tcm->height) {
+		pvt->div_pt.x = attr->x;
+		pvt->div_pt.y = attr->y;
+
+	} else {
+		/* Defaulting to 3:1 ratio on width for 2D area split */
+		/* Defaulting to 3:1 ratio on height for 2D and 1D split */
+		pvt->div_pt.x = (tcm->width * 3) / 4;
+		pvt->div_pt.y = (tcm->height * 3) / 4;
+	}
+
+	spin_lock(&(pvt->lock));
+	assign(&area, 0, 0, width - 1, height - 1);
+	fill_area(tcm, &area, NULL);
+	spin_unlock(&(pvt->lock));
+	return tcm;
+
+error:
+	kfree(tcm);
+	kfree(pvt);
+	return NULL;
+}
+
+static void sita_deinit(struct tcm *tcm)
+{
+	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+	struct tcm_area area = {0};
+	s32 i;
+
+	area.p1.x = tcm->width - 1;
+	area.p1.y = tcm->height - 1;
+
+	spin_lock(&(pvt->lock));
+	fill_area(tcm, &area, NULL);
+	spin_unlock(&(pvt->lock));
+
+	for (i = 0; i < tcm->height; i++)
+		kfree(pvt->map[i]);
+	kfree(pvt->map);
+	kfree(pvt);
+}
+
+/**
+ * Reserve a 1D area in the container
+ *
+ * @param num_slots	size of 1D area
+ * @param area		pointer to the area that will be populated with the
+ *			reserved area
+ *
+ * @return 0 on success, non-0 error value on failure.
+ */
+static s32 sita_reserve_1d(struct tcm *tcm, u32 num_slots,
+			   struct tcm_area *area)
+{
+	s32 ret;
+	struct tcm_area field = {0};
+	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+
+	spin_lock(&(pvt->lock));
+
+	/* Scanning entire container */
+	assign(&field, tcm->width - 1, tcm->height - 1, 0, 0);
+
+	ret = scan_r2l_b2t_one_dim(tcm, num_slots, &field, area);
+	if (!ret)
+		/* update map */
+		fill_area(tcm, area, area);
+
+	spin_unlock(&(pvt->lock));
+	return ret;
+}
+
+/**
+ * Reserve a 2D area in the container
+ *
+ * @param w	width
+ * @param h	height
+ * @param area	pointer to the area that will be populated with the reesrved
+ *		area
+ *
+ * @return 0 on success, non-0 error value on failure.
+ */
+static s32 sita_reserve_2d(struct tcm *tcm, u16 h, u16 w, u8 align,
+			   struct tcm_area *area)
+{
+	s32 ret;
+	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+
+	/* not supporting more than 64 as alignment */
+	if (align > 64)
+		return -EINVAL;
+
+	/* we prefer 1, 32 and 64 as alignment */
+	align = align <= 1 ? 1 : align <= 32 ? 32 : 64;
+
+	spin_lock(&(pvt->lock));
+	ret = scan_areas_and_find_fit(tcm, w, h, align, area);
+	if (!ret)
+		/* update map */
+		fill_area(tcm, area, area);
+
+	spin_unlock(&(pvt->lock));
+	return ret;
+}
+
+/**
+ * Unreserve a previously allocated 2D or 1D area
+ * @param area	area to be freed
+ * @return 0 - success
+ */
+static s32 sita_free(struct tcm *tcm, struct tcm_area *area)
+{
+	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+
+	spin_lock(&(pvt->lock));
+
+	/* check that this is in fact an existing area */
+	WARN_ON(pvt->map[area->p0.x][area->p0.y] != area ||
+		pvt->map[area->p1.x][area->p1.y] != area);
+
+	/* Clear the contents of the associated tiles in the map */
+	fill_area(tcm, area, NULL);
+
+	spin_unlock(&(pvt->lock));
+
+	return 0;
+}
+
+/**
+ * Note: In general the cordinates in the scan field area relevant to the can
+ * sweep directions. The scan origin (e.g. top-left corner) will always be
+ * the p0 member of the field.  Therfore, for a scan from top-left p0.x <= p1.x
+ * and p0.y <= p1.y; whereas, for a scan from bottom-right p1.x <= p0.x and p1.y
+ * <= p0.y
+ */
+
+/**
+ * Raster scan horizontally right to left from top to bottom to find a place for
+ * a 2D area of given size inside a scan field.
+ *
+ * @param w	width of desired area
+ * @param h	height of desired area
+ * @param align	desired area alignment
+ * @param area	pointer to the area that will be set to the best position
+ * @param field	area to scan (inclusive)
+ *
+ * @return 0 on success, non-0 error value on failure.
+ */
+static s32 scan_r2l_t2b(struct tcm *tcm, u16 w, u16 h, u16 align,
+			struct tcm_area *field, struct tcm_area *area)
+{
+	s32 x, y;
+	s16 start_x, end_x, start_y, end_y, found_x = -1;
+	struct tcm_area ***map = ((struct sita_pvt *)tcm->pvt)->map;
+	struct score best = {{0}, {0}, {0}, 0};
+
+	start_x = field->p0.x;
+	end_x = field->p1.x;
+	start_y = field->p0.y;
+	end_y = field->p1.y;
+
+	/* check scan area co-ordinates */
+	if (field->p0.x < field->p1.x ||
+	    field->p1.y < field->p0.y)
+		return -EINVAL;
+
+	/* check if allocation would fit in scan area */
+	if (w > LEN(start_x, end_x) || h > LEN(end_y, start_y))
+		return -ENOSPC;
+
+	/* adjust start_x and end_y, as allocation would not fit beyond */
+	start_x = ALIGN_DOWN(start_x - w + 1, align); /* - 1 to be inclusive */
+	end_y = end_y - h + 1;
+
+	/* check if allocation would still fit in scan area */
+	if (start_x < end_x)
+		return -ENOSPC;
+
+	/* scan field top-to-bottom, right-to-left */
+	for (y = start_y; y <= end_y; y++) {
+		for (x = start_x; x >= end_x; x -= align) {
+			if (is_area_free(map, x, y, w, h)) {
+				found_x = x;
+
+				/* update best candidate */
+				if (update_candidate(tcm, x, y, w, h, field,
+							CR_R2L_T2B, &best))
+					goto done;
+
+				/* change upper x bound */
+				end_x = x + 1;
+				break;
+			} else if (map[x][y] && map[x][y]->is2d) {
+				/* step over 2D areas */
+				x = ALIGN(map[x][y]->p0.x - w + 1, align);
+			}
+		}
+
+		/* break if you find a free area shouldering the scan field */
+		if (found_x == start_x)
+			break;
+	}
+
+	if (!best.a.tcm)
+		return -ENOSPC;
+done:
+	assign(area, best.a.p0.x, best.a.p0.y, best.a.p1.x, best.a.p1.y);
+	return 0;
+}
+
+/**
+ * Raster scan horizontally left to right from top to bottom to find a place for
+ * a 2D area of given size inside a scan field.
+ *
+ * @param w	width of desired area
+ * @param h	height of desired area
+ * @param align	desired area alignment
+ * @param area	pointer to the area that will be set to the best position
+ * @param field	area to scan (inclusive)
+ *
+ * @return 0 on success, non-0 error value on failure.
+ */
+static s32 scan_l2r_t2b(struct tcm *tcm, u16 w, u16 h, u16 align,
+			struct tcm_area *field, struct tcm_area *area)
+{
+	s32 x, y;
+	s16 start_x, end_x, start_y, end_y, found_x = -1;
+	struct tcm_area ***map = ((struct sita_pvt *)tcm->pvt)->map;
+	struct score best = {{0}, {0}, {0}, 0};
+
+	start_x = field->p0.x;
+	end_x = field->p1.x;
+	start_y = field->p0.y;
+	end_y = field->p1.y;
+
+	/* check scan area co-ordinates */
+	if (field->p1.x < field->p0.x ||
+	    field->p1.y < field->p0.y)
+		return -EINVAL;
+
+	/* check if allocation would fit in scan area */
+	if (w > LEN(end_x, start_x) || h > LEN(end_y, start_y))
+		return -ENOSPC;
+
+	start_x = ALIGN(start_x, align);
+
+	/* check if allocation would still fit in scan area */
+	if (w > LEN(end_x, start_x))
+		return -ENOSPC;
+
+	/* adjust end_x and end_y, as allocation would not fit beyond */
+	end_x = end_x - w + 1; /* + 1 to be inclusive */
+	end_y = end_y - h + 1;
+
+	/* scan field top-to-bottom, left-to-right */
+	for (y = start_y; y <= end_y; y++) {
+		for (x = start_x; x <= end_x; x += align) {
+			if (is_area_free(map, x, y, w, h)) {
+				found_x = x;
+
+				/* update best candidate */
+				if (update_candidate(tcm, x, y, w, h, field,
+							CR_L2R_T2B, &best))
+					goto done;
+				/* change upper x bound */
+				end_x = x - 1;
+
+				break;
+			} else if (map[x][y] && map[x][y]->is2d) {
+				/* step over 2D areas */
+				x = ALIGN_DOWN(map[x][y]->p1.x, align);
+			}
+		}
+
+		/* break if you find a free area shouldering the scan field */
+		if (found_x == start_x)
+			break;
+	}
+
+	if (!best.a.tcm)
+		return -ENOSPC;
+done:
+	assign(area, best.a.p0.x, best.a.p0.y, best.a.p1.x, best.a.p1.y);
+	return 0;
+}
+
+/**
+ * Raster scan horizontally right to left from bottom to top to find a place
+ * for a 1D area of given size inside a scan field.
+ *
+ * @param num_slots	size of desired area
+ * @param align		desired area alignment
+ * @param area		pointer to the area that will be set to the best
+ *			position
+ * @param field		area to scan (inclusive)
+ *
+ * @return 0 on success, non-0 error value on failure.
+ */
+static s32 scan_r2l_b2t_one_dim(struct tcm *tcm, u32 num_slots,
+				struct tcm_area *field, struct tcm_area *area)
+{
+	s32 found = 0;
+	s16 x, y;
+	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+	struct tcm_area *p;
+
+	/* check scan area co-ordinates */
+	if (field->p0.y < field->p1.y)
+		return -EINVAL;
+
+	/**
+	 * Currently we only support full width 1D scan field, which makes sense
+	 * since 1D slot-ordering spans the full container width.
+	 */
+	if (tcm->width != field->p0.x - field->p1.x + 1)
+		return -EINVAL;
+
+	/* check if allocation would fit in scan area */
+	if (num_slots > tcm->width * LEN(field->p0.y, field->p1.y))
+		return -ENOSPC;
+
+	x = field->p0.x;
+	y = field->p0.y;
+
+	/* find num_slots consecutive free slots to the left */
+	while (found < num_slots) {
+		if (y < 0)
+			return -ENOSPC;
+
+		/* remember bottom-right corner */
+		if (found == 0) {
+			area->p1.x = x;
+			area->p1.y = y;
+		}
+
+		/* skip busy regions */
+		p = pvt->map[x][y];
+		if (p) {
+			/* move to left of 2D areas, top left of 1D */
+			x = p->p0.x;
+			if (!p->is2d)
+				y = p->p0.y;
+
+			/* start over */
+			found = 0;
+		} else {
+			/* count consecutive free slots */
+			found++;
+			if (found == num_slots)
+				break;
+		}
+
+		/* move to the left */
+		if (x == 0)
+			y--;
+		x = (x ? : tcm->width) - 1;
+
+	}
+
+	/* set top-left corner */
+	area->p0.x = x;
+	area->p0.y = y;
+	return 0;
+}
+
+/**
+ * Find a place for a 2D area of given size inside a scan field based on its
+ * alignment needs.
+ *
+ * @param w	width of desired area
+ * @param h	height of desired area
+ * @param align	desired area alignment
+ * @param area	pointer to the area that will be set to the best position
+ *
+ * @return 0 on success, non-0 error value on failure.
+ */
+static s32 scan_areas_and_find_fit(struct tcm *tcm, u16 w, u16 h, u16 align,
+				   struct tcm_area *area)
+{
+	s32 ret = 0;
+	struct tcm_area field = {0};
+	u16 boundary_x, boundary_y;
+	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+
+	if (align > 1) {
+		/* prefer top-left corner */
+		boundary_x = pvt->div_pt.x - 1;
+		boundary_y = pvt->div_pt.y - 1;
+
+		/* expand width and height if needed */
+		if (w > pvt->div_pt.x)
+			boundary_x = tcm->width - 1;
+		if (h > pvt->div_pt.y)
+			boundary_y = tcm->height - 1;
+
+		assign(&field, 0, 0, boundary_x, boundary_y);
+		ret = scan_l2r_t2b(tcm, w, h, align, &field, area);
+
+		/* scan whole container if failed, but do not scan 2x */
+		if (ret != 0 && (boundary_x != tcm->width - 1 ||
+				 boundary_y != tcm->height - 1)) {
+			/* scan the entire container if nothing found */
+			assign(&field, 0, 0, tcm->width - 1, tcm->height - 1);
+			ret = scan_l2r_t2b(tcm, w, h, align, &field, area);
+		}
+	} else if (align == 1) {
+		/* prefer top-right corner */
+		boundary_x = pvt->div_pt.x;
+		boundary_y = pvt->div_pt.y - 1;
+
+		/* expand width and height if needed */
+		if (w > (tcm->width - pvt->div_pt.x))
+			boundary_x = 0;
+		if (h > pvt->div_pt.y)
+			boundary_y = tcm->height - 1;
+
+		assign(&field, tcm->width - 1, 0, boundary_x, boundary_y);
+		ret = scan_r2l_t2b(tcm, w, h, align, &field, area);
+
+		/* scan whole container if failed, but do not scan 2x */
+		if (ret != 0 && (boundary_x != 0 ||
+				 boundary_y != tcm->height - 1)) {
+			/* scan the entire container if nothing found */
+			assign(&field, tcm->width - 1, 0, 0, tcm->height - 1);
+			ret = scan_r2l_t2b(tcm, w, h, align, &field,
+					   area);
+		}
+	}
+
+	return ret;
+}
+
+/* check if an entire area is free */
+static s32 is_area_free(struct tcm_area ***map, u16 x0, u16 y0, u16 w, u16 h)
+{
+	u16 x = 0, y = 0;
+	for (y = y0; y < y0 + h; y++) {
+		for (x = x0; x < x0 + w; x++) {
+			if (map[x][y])
+				return false;
+		}
+	}
+	return true;
+}
+
+/* fills an area with a parent tcm_area */
+static void fill_area(struct tcm *tcm, struct tcm_area *area,
+			struct tcm_area *parent)
+{
+	s32 x, y;
+	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+	struct tcm_area a, a_;
+
+	/* set area's tcm; otherwise, enumerator considers it invalid */
+	area->tcm = tcm;
+
+	tcm_for_each_slice(a, *area, a_) {
+		for (x = a.p0.x; x <= a.p1.x; ++x)
+			for (y = a.p0.y; y <= a.p1.y; ++y)
+				pvt->map[x][y] = parent;
+
+	}
+}
+
+/**
+ * Compares a candidate area to the current best area, and if it is a better
+ * fit, it updates the best to this one.
+ *
+ * @param x0, y0, w, h		top, left, width, height of candidate area
+ * @param field			scan field
+ * @param criteria		scan criteria
+ * @param best			best candidate and its scores
+ *
+ * @return 1 (true) if the candidate area is known to be the final best, so no
+ * more searching should be performed
+ */
+static s32 update_candidate(struct tcm *tcm, u16 x0, u16 y0, u16 w, u16 h,
+			    struct tcm_area *field, s32 criteria,
+			    struct score *best)
+{
+	struct score me;	/* score for area */
+
+	/*
+	 * NOTE: For horizontal bias we always give the first found, because our
+	 * scan is horizontal-raster-based and the first candidate will always
+	 * have the horizontal bias.
+	 */
+	bool first = criteria & CR_BIAS_HORIZONTAL;
+
+	assign(&me.a, x0, y0, x0 + w - 1, y0 + h - 1);
+
+	/* calculate score for current candidate */
+	if (!first) {
+		get_neighbor_stats(tcm, &me.a, &me.n);
+		me.neighs = me.n.edge + me.n.busy;
+		get_nearness_factor(field, &me.a, &me.f);
+	}
+
+	/* the 1st candidate is always the best */
+	if (!best->a.tcm)
+		goto better;
+
+	BUG_ON(first);
+
+	/* diagonal balance check */
+	if ((criteria & CR_DIAGONAL_BALANCE) &&
+		best->neighs <= me.neighs &&
+		(best->neighs < me.neighs ||
+		 /* this implies that neighs and occupied match */
+		 best->n.busy < me.n.busy ||
+		 (best->n.busy == me.n.busy &&
+		  /* check the nearness factor */
+		  best->f.x + best->f.y > me.f.x + me.f.y)))
+		goto better;
+
+	/* not better, keep going */
+	return 0;
+
+better:
+	/* save current area as best */
+	memcpy(best, &me, sizeof(me));
+	best->a.tcm = tcm;
+	return first;
+}
+
+/**
+ * Calculate the nearness factor of an area in a search field.  The nearness
+ * factor is smaller if the area is closer to the search origin.
+ */
+static void get_nearness_factor(struct tcm_area *field, struct tcm_area *area,
+				struct nearness_factor *nf)
+{
+	/**
+	 * Using signed math as field coordinates may be reversed if
+	 * search direction is right-to-left or bottom-to-top.
+	 */
+	nf->x = (s32)(area->p0.x - field->p0.x) * 1000 /
+		(field->p1.x - field->p0.x);
+	nf->y = (s32)(area->p0.y - field->p0.y) * 1000 /
+		(field->p1.y - field->p0.y);
+}
+
+/* get neighbor statistics */
+static void get_neighbor_stats(struct tcm *tcm, struct tcm_area *area,
+			 struct neighbor_stats *stat)
+{
+	s16 x = 0, y = 0;
+	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+
+	/* Clearing any exisiting values */
+	memset(stat, 0, sizeof(*stat));
+
+	/* process top & bottom edges */
+	for (x = area->p0.x; x <= area->p1.x; x++) {
+		if (area->p0.y == 0)
+			stat->edge++;
+		else if (pvt->map[x][area->p0.y - 1])
+			stat->busy++;
+
+		if (area->p1.y == tcm->height - 1)
+			stat->edge++;
+		else if (pvt->map[x][area->p1.y + 1])
+			stat->busy++;
+	}
+
+	/* process left & right edges */
+	for (y = area->p0.y; y <= area->p1.y; ++y) {
+		if (area->p0.x == 0)
+			stat->edge++;
+		else if (pvt->map[area->p0.x - 1][y])
+			stat->busy++;
+
+		if (area->p1.x == tcm->width - 1)
+			stat->edge++;
+		else if (pvt->map[area->p1.x + 1][y])
+			stat->busy++;
+	}
+}
diff --git a/drivers/staging/omapdrm/tcm-sita.h b/drivers/staging/omapdrm/tcm-sita.h
new file mode 100644
index 000000000000..0444f868671c
--- /dev/null
+++ b/drivers/staging/omapdrm/tcm-sita.h
@@ -0,0 +1,95 @@
+/*
+ * tcm_sita.h
+ *
+ * SImple Tiler Allocator (SiTA) private structures.
+ *
+ * Author: Ravi Ramachandra <r.ramachandra@ti.com>
+ *
+ * Copyright (C) 2009-2011 Texas Instruments, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * * Neither the name of Texas Instruments Incorporated nor the names of
+ *   its contributors may be used to endorse or promote products derived
+ *   from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _TCM_SITA_H
+#define _TCM_SITA_H
+
+#include "tcm.h"
+
+/* length between two coordinates */
+#define LEN(a, b) ((a) > (b) ? (a) - (b) + 1 : (b) - (a) + 1)
+
+enum criteria {
+	CR_MAX_NEIGHS		= 0x01,
+	CR_FIRST_FOUND		= 0x10,
+	CR_BIAS_HORIZONTAL	= 0x20,
+	CR_BIAS_VERTICAL	= 0x40,
+	CR_DIAGONAL_BALANCE	= 0x80
+};
+
+/* nearness to the beginning of the search field from 0 to 1000 */
+struct nearness_factor {
+	s32 x;
+	s32 y;
+};
+
+/*
+ * Statistics on immediately neighboring slots.  Edge is the number of
+ * border segments that are also border segments of the scan field.  Busy
+ * refers to the number of neighbors that are occupied.
+ */
+struct neighbor_stats {
+	u16 edge;
+	u16 busy;
+};
+
+/* structure to keep the score of a potential allocation */
+struct score {
+	struct nearness_factor	f;
+	struct neighbor_stats	n;
+	struct tcm_area		a;
+	u16    neighs;		/* number of busy neighbors */
+};
+
+struct sita_pvt {
+	spinlock_t lock;	/* spinlock to protect access */
+	struct tcm_pt div_pt;	/* divider point splitting container */
+	struct tcm_area ***map;	/* pointers to the parent area for each slot */
+};
+
+/* assign coordinates to area */
+static inline
+void assign(struct tcm_area *a, u16 x0, u16 y0, u16 x1, u16 y1)
+{
+	a->p0.x = x0;
+	a->p0.y = y0;
+	a->p1.x = x1;
+	a->p1.y = y1;
+}
+
+#endif
diff --git a/drivers/staging/omapdrm/tcm.h b/drivers/staging/omapdrm/tcm.h
new file mode 100644
index 000000000000..d273e3ee0b4c
--- /dev/null
+++ b/drivers/staging/omapdrm/tcm.h
@@ -0,0 +1,326 @@
+/*
+ * tcm.h
+ *
+ * TILER container manager specification and support functions for TI
+ * TILER driver.
+ *
+ * Author: Lajos Molnar <molnar@ti.com>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ *
+ * * Neither the name of Texas Instruments Incorporated nor the names of
+ *   its contributors may be used to endorse or promote products derived
+ *   from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TCM_H
+#define TCM_H
+
+struct tcm;
+
+/* point */
+struct tcm_pt {
+	u16 x;
+	u16 y;
+};
+
+/* 1d or 2d area */
+struct tcm_area {
+	bool is2d;		/* whether area is 1d or 2d */
+	struct tcm    *tcm;	/* parent */
+	struct tcm_pt  p0;
+	struct tcm_pt  p1;
+};
+
+struct tcm {
+	u16 width, height;	/* container dimensions */
+	int lut_id;		/* Lookup table identifier */
+
+	/* 'pvt' structure shall contain any tcm details (attr) along with
+	linked list of allocated areas and mutex for mutually exclusive access
+	to the list.  It may also contain copies of width and height to notice
+	any changes to the publicly available width and height fields. */
+	void *pvt;
+
+	/* function table */
+	s32 (*reserve_2d)(struct tcm *tcm, u16 height, u16 width, u8 align,
+			  struct tcm_area *area);
+	s32 (*reserve_1d)(struct tcm *tcm, u32 slots, struct tcm_area *area);
+	s32 (*free)      (struct tcm *tcm, struct tcm_area *area);
+	void (*deinit)   (struct tcm *tcm);
+};
+
+/*=============================================================================
+    BASIC TILER CONTAINER MANAGER INTERFACE
+=============================================================================*/
+
+/*
+ * NOTE:
+ *
+ * Since some basic parameter checking is done outside the TCM algorithms,
+ * TCM implementation do NOT have to check the following:
+ *
+ *   area pointer is NULL
+ *   width and height fits within container
+ *   number of pages is more than the size of the container
+ *
+ */
+
+struct tcm *sita_init(u16 width, u16 height, struct tcm_pt *attr);
+
+
+/**
+ * Deinitialize tiler container manager.
+ *
+ * @param tcm	Pointer to container manager.
+ *
+ * @return 0 on success, non-0 error value on error.  The call
+ *	   should free as much memory as possible and meaningful
+ *	   even on failure.  Some error codes: -ENODEV: invalid
+ *	   manager.
+ */
+static inline void tcm_deinit(struct tcm *tcm)
+{
+	if (tcm)
+		tcm->deinit(tcm);
+}
+
+/**
+ * Reserves a 2D area in the container.
+ *
+ * @param tcm		Pointer to container manager.
+ * @param height	Height(in pages) of area to be reserved.
+ * @param width		Width(in pages) of area to be reserved.
+ * @param align		Alignment requirement for top-left corner of area. Not
+ *			all values may be supported by the container manager,
+ *			but it must support 0 (1), 32 and 64.
+ *			0 value is equivalent to 1.
+ * @param area		Pointer to where the reserved area should be stored.
+ *
+ * @return 0 on success.  Non-0 error code on failure.  Also,
+ *	   the tcm field of the area will be set to NULL on
+ *	   failure.  Some error codes: -ENODEV: invalid manager,
+ *	   -EINVAL: invalid area, -ENOMEM: not enough space for
+ *	    allocation.
+ */
+static inline s32 tcm_reserve_2d(struct tcm *tcm, u16 width, u16 height,
+				 u16 align, struct tcm_area *area)
+{
+	/* perform rudimentary error checking */
+	s32 res = tcm  == NULL ? -ENODEV :
+		(area == NULL || width == 0 || height == 0 ||
+		 /* align must be a 2 power */
+		 (align & (align - 1))) ? -EINVAL :
+		(height > tcm->height || width > tcm->width) ? -ENOMEM : 0;
+
+	if (!res) {
+		area->is2d = true;
+		res = tcm->reserve_2d(tcm, height, width, align, area);
+		area->tcm = res ? NULL : tcm;
+	}
+
+	return res;
+}
+
+/**
+ * Reserves a 1D area in the container.
+ *
+ * @param tcm		Pointer to container manager.
+ * @param slots		Number of (contiguous) slots to reserve.
+ * @param area		Pointer to where the reserved area should be stored.
+ *
+ * @return 0 on success.  Non-0 error code on failure.  Also,
+ *	   the tcm field of the area will be set to NULL on
+ *	   failure.  Some error codes: -ENODEV: invalid manager,
+ *	   -EINVAL: invalid area, -ENOMEM: not enough space for
+ *	    allocation.
+ */
+static inline s32 tcm_reserve_1d(struct tcm *tcm, u32 slots,
+				 struct tcm_area *area)
+{
+	/* perform rudimentary error checking */
+	s32 res = tcm  == NULL ? -ENODEV :
+		(area == NULL || slots == 0) ? -EINVAL :
+		slots > (tcm->width * (u32) tcm->height) ? -ENOMEM : 0;
+
+	if (!res) {
+		area->is2d = false;
+		res = tcm->reserve_1d(tcm, slots, area);
+		area->tcm = res ? NULL : tcm;
+	}
+
+	return res;
+}
+
+/**
+ * Free a previously reserved area from the container.
+ *
+ * @param area	Pointer to area reserved by a prior call to
+ *		tcm_reserve_1d or tcm_reserve_2d call, whether
+ *		it was successful or not. (Note: all fields of
+ *		the structure must match.)
+ *
+ * @return 0 on success.  Non-0 error code on failure.  Also, the tcm
+ *	   field of the area is set to NULL on success to avoid subsequent
+ *	   freeing.  This call will succeed even if supplying
+ *	   the area from a failed reserved call.
+ */
+static inline s32 tcm_free(struct tcm_area *area)
+{
+	s32 res = 0; /* free succeeds by default */
+
+	if (area && area->tcm) {
+		res = area->tcm->free(area->tcm, area);
+		if (res == 0)
+			area->tcm = NULL;
+	}
+
+	return res;
+}
+
+/*=============================================================================
+    HELPER FUNCTION FOR ANY TILER CONTAINER MANAGER
+=============================================================================*/
+
+/**
+ * This method slices off the topmost 2D slice from the parent area, and stores
+ * it in the 'slice' parameter.  The 'parent' parameter will get modified to
+ * contain the remaining portion of the area.  If the whole parent area can
+ * fit in a 2D slice, its tcm pointer is set to NULL to mark that it is no
+ * longer a valid area.
+ *
+ * @param parent	Pointer to a VALID parent area that will get modified
+ * @param slice		Pointer to the slice area that will get modified
+ */
+static inline void tcm_slice(struct tcm_area *parent, struct tcm_area *slice)
+{
+	*slice = *parent;
+
+	/* check if we need to slice */
+	if (slice->tcm && !slice->is2d &&
+		slice->p0.y != slice->p1.y &&
+		(slice->p0.x || (slice->p1.x != slice->tcm->width - 1))) {
+		/* set end point of slice (start always remains) */
+		slice->p1.x = slice->tcm->width - 1;
+		slice->p1.y = (slice->p0.x) ? slice->p0.y : slice->p1.y - 1;
+		/* adjust remaining area */
+		parent->p0.x = 0;
+		parent->p0.y = slice->p1.y + 1;
+	} else {
+		/* mark this as the last slice */
+		parent->tcm = NULL;
+	}
+}
+
+/* Verify if a tcm area is logically valid */
+static inline bool tcm_area_is_valid(struct tcm_area *area)
+{
+	return area && area->tcm &&
+		/* coordinate bounds */
+		area->p1.x < area->tcm->width &&
+		area->p1.y < area->tcm->height &&
+		area->p0.y <= area->p1.y &&
+		/* 1D coordinate relationship + p0.x check */
+		((!area->is2d &&
+		  area->p0.x < area->tcm->width &&
+		  area->p0.x + area->p0.y * area->tcm->width <=
+		  area->p1.x + area->p1.y * area->tcm->width) ||
+		 /* 2D coordinate relationship */
+		 (area->is2d &&
+		  area->p0.x <= area->p1.x));
+}
+
+/* see if a coordinate is within an area */
+static inline bool __tcm_is_in(struct tcm_pt *p, struct tcm_area *a)
+{
+	u16 i;
+
+	if (a->is2d) {
+		return p->x >= a->p0.x && p->x <= a->p1.x &&
+		       p->y >= a->p0.y && p->y <= a->p1.y;
+	} else {
+		i = p->x + p->y * a->tcm->width;
+		return i >= a->p0.x + a->p0.y * a->tcm->width &&
+		       i <= a->p1.x + a->p1.y * a->tcm->width;
+	}
+}
+
+/* calculate area width */
+static inline u16 __tcm_area_width(struct tcm_area *area)
+{
+	return area->p1.x - area->p0.x + 1;
+}
+
+/* calculate area height */
+static inline u16 __tcm_area_height(struct tcm_area *area)
+{
+	return area->p1.y - area->p0.y + 1;
+}
+
+/* calculate number of slots in an area */
+static inline u16 __tcm_sizeof(struct tcm_area *area)
+{
+	return area->is2d ?
+		__tcm_area_width(area) * __tcm_area_height(area) :
+		(area->p1.x - area->p0.x + 1) + (area->p1.y - area->p0.y) *
+							area->tcm->width;
+}
+#define tcm_sizeof(area) __tcm_sizeof(&(area))
+#define tcm_awidth(area) __tcm_area_width(&(area))
+#define tcm_aheight(area) __tcm_area_height(&(area))
+#define tcm_is_in(pt, area) __tcm_is_in(&(pt), &(area))
+
+/* limit a 1D area to the first N pages */
+static inline s32 tcm_1d_limit(struct tcm_area *a, u32 num_pg)
+{
+	if (__tcm_sizeof(a) < num_pg)
+		return -ENOMEM;
+	if (!num_pg)
+		return -EINVAL;
+
+	a->p1.x = (a->p0.x + num_pg - 1) % a->tcm->width;
+	a->p1.y = a->p0.y + ((a->p0.x + num_pg - 1) / a->tcm->width);
+	return 0;
+}
+
+/**
+ * Iterate through 2D slices of a valid area. Behaves
+ * syntactically as a for(;;) statement.
+ *
+ * @param var		Name of a local variable of type 'struct
+ *			tcm_area *' that will get modified to
+ *			contain each slice.
+ * @param area		Pointer to the VALID parent area. This
+ *			structure will not get modified
+ *			throughout the loop.
+ *
+ */
+#define tcm_for_each_slice(var, area, safe) \
+	for (safe = area, \
+	     tcm_slice(&safe, &var); \
+	     var.tcm; tcm_slice(&safe, &var))
+
+#endif
diff --git a/drivers/staging/phison/phison.c b/drivers/staging/phison/phison.c
index 683657cb21f5..d77b21f1eb2d 100644
--- a/drivers/staging/phison/phison.c
+++ b/drivers/staging/phison/phison.c
@@ -70,7 +70,7 @@ static int phison_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 }
 
 static DEFINE_PCI_DEVICE_TABLE(phison_pci_tbl) = {
-	{ PCI_VENDOR_ID_PHISON, PCI_DEVICE_ID_PS5000, PCI_ANY_ID, PCI_ANY_ID,
+	{ PCI_DEVICE(PCI_VENDOR_ID_PHISON, PCI_DEVICE_ID_PS5000),
 	  PCI_CLASS_STORAGE_IDE << 8, 0xffff00, 0 },
 	{ 0, },
 };
diff --git a/drivers/staging/rtl8192e/Kconfig b/drivers/staging/rtl8192e/Kconfig
index 750c347bfbe1..f87e21101857 100644
--- a/drivers/staging/rtl8192e/Kconfig
+++ b/drivers/staging/rtl8192e/Kconfig
@@ -1,9 +1,43 @@
-config RTL8192E
-	tristate "RealTek RTL8192E Wireless LAN NIC driver"
-	depends on PCI && WLAN
-	depends on m
-	select WIRELESS_EXT
-	select WEXT_PRIV
-	select CRYPTO
-	default N
+config RTLLIB
+	tristate "Support for rtllib wireless devices"
+	depends on WLAN && m
+	default n
+	select LIB80211
 	---help---
+	  If you have a wireless card that uses rtllib, say
+	  Y. Currently the only card is the rtl8192e.
+
+	  If unsure, say N.
+
+if RTLLIB
+
+config RTLLIB_CRYPTO_CCMP
+	tristate "Support for rtllib CCMP crypto"
+	depends on RTLLIB
+	default y
+	---help---
+	  CCMP crypto driver for rtllib.
+
+	  If you enabled RTLLIB, you want this.
+
+config RTLLIB_CRYPTO_TKIP
+	tristate "Support for rtllib TKIP crypto"
+	depends on RTLLIB
+	default y
+	---help---
+	  TKIP crypto driver for rtllib.
+
+	  If you enabled RTLLIB, you want this.
+
+config RTLLIB_CRYPTO_WEP
+	tristate "Support for rtllib WEP crypto"
+	depends on RTLLIB
+	default y
+	---help---
+	  TKIP crypto driver for rtllib.
+
+	  If you enabled RTLLIB, you want this.
+
+source "drivers/staging/rtl8192e/rtl8192e/Kconfig"
+
+endif
diff --git a/drivers/staging/rtl8192e/Makefile b/drivers/staging/rtl8192e/Makefile
index a66a9ad33686..cb18db74d78c 100644
--- a/drivers/staging/rtl8192e/Makefile
+++ b/drivers/staging/rtl8192e/Makefile
@@ -1,41 +1,21 @@
-ccflags-y += -DUSE_FW_SOURCE_IMG_FILE
-ccflags-y += -DCONFIG_PM_RTL
-ccflags-y += -DCONFIG_PM
-ccflags-y += -DHAVE_NET_DEVICE_OPS
-ccflags-y += -DENABLE_DOT11D
-
-r8192e_pci-objs :=		\
-	rtl_core.o		\
-	rtl_eeprom.o		\
-	rtl_ps.o		\
-	rtl_wx.o		\
-	rtl_cam.o		\
-	rtl_dm.o		\
-	rtl_pm.o		\
-	rtl_pci.o		\
-	rtl_debug.o		\
-	rtl_ethtool.o		\
-	r8192E_dev.o		\
-	r8192E_phy.o		\
-	r8192E_firmware.o	\
-	r8192E_cmdpkt.o		\
-	r8192E_hwimg.o		\
-	r8190P_rtl8256.o	\
+rtllib-objs :=			\
+	dot11d.o		\
+	rtllib_module.o		\
 	rtllib_rx.o		\
-	rtllib_softmac.o	\
 	rtllib_tx.o		\
 	rtllib_wx.o		\
-	rtllib_module.o		\
+	rtllib_softmac.o	\
 	rtllib_softmac_wx.o	\
-	rtl819x_HTProc.o	\
-	rtl819x_TSProc.o	\
 	rtl819x_BAProc.o	\
-	dot11d.o		\
-	rtllib_crypt.o		\
-	rtllib_crypt_tkip.o	\
-	rtllib_crypt_ccmp.o	\
-	rtllib_crypt_wep.o
+	rtl819x_HTProc.o	\
+	rtl819x_TSProc.o
+
+obj-$(CONFIG_RTLLIB) += rtllib.o
+
+obj-$(CONFIG_RTLLIB_CRYPTO_CCMP) += rtllib_crypt_ccmp.o
+obj-$(CONFIG_RTLLIB_CRYPTO_TKIP) += rtllib_crypt_tkip.o
+obj-$(CONFIG_RTLLIB_CRYPTO_WEP) += rtllib_crypt_wep.o
 
-obj-$(CONFIG_RTL8192E) += r8192e_pci.o
+obj-$(CONFIG_RTL8192E) += rtl8192e/
 
 ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/staging/rtl8192e/dot11d.c b/drivers/staging/rtl8192e/dot11d.c
index ee0381e0a81f..f7b14f8b7b83 100644
--- a/drivers/staging/rtl8192e/dot11d.c
+++ b/drivers/staging/rtl8192e/dot11d.c
@@ -46,7 +46,7 @@ static struct channel_list ChannelPlan[] = {
 	  56, 60, 64}, 21}
 };
 
-void Dot11d_Init(struct rtllib_device *ieee)
+void dot11d_init(struct rtllib_device *ieee)
 {
 	struct rt_dot11d_info *pDot11dInfo = GET_DOT11D_INFO(ieee);
 	pDot11dInfo->bEnabled = false;
@@ -58,6 +58,7 @@ void Dot11d_Init(struct rtllib_device *ieee)
 	RESET_CIE_WATCHDOG(ieee);
 
 }
+EXPORT_SYMBOL(dot11d_init);
 
 void Dot11d_Channelmap(u8 channel_plan, struct rtllib_device *ieee)
 {
@@ -99,6 +100,7 @@ void Dot11d_Channelmap(u8 channel_plan, struct rtllib_device *ieee)
 		break;
 	}
 }
+EXPORT_SYMBOL(Dot11d_Channelmap);
 
 
 void Dot11d_Reset(struct rtllib_device *ieee)
diff --git a/drivers/staging/rtl8192e/dot11d.h b/drivers/staging/rtl8192e/dot11d.h
index 032f7004a7f0..71f4549a378f 100644
--- a/drivers/staging/rtl8192e/dot11d.h
+++ b/drivers/staging/rtl8192e/dot11d.h
@@ -93,7 +93,7 @@ static inline void cpMacAddr(unsigned char *des, unsigned char *src)
 #define IS_DOT11D_STATE_DONE(__pIeeeDev)			\
 	(GET_DOT11D_INFO(__pIeeeDev)->State == DOT11D_STATE_DONE)
 
-void Dot11d_Init(struct rtllib_device *dev);
+void dot11d_init(struct rtllib_device *dev);
 void Dot11d_Channelmap(u8 channel_plan, struct rtllib_device *ieee);
 void Dot11d_Reset(struct rtllib_device *dev);
 void Dot11d_UpdateCountryIe(struct rtllib_device *dev, u8 *pTaddr,
diff --git a/drivers/staging/rtl8192e/rtl8192e/Kconfig b/drivers/staging/rtl8192e/rtl8192e/Kconfig
new file mode 100644
index 000000000000..50e0d91a409a
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/Kconfig
@@ -0,0 +1,9 @@
+config RTL8192E
+	tristate "RealTek RTL8192E Wireless LAN NIC driver"
+	depends on PCI && WLAN && RTLLIB
+	depends on m
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select CRYPTO
+	default N
+	---help---
diff --git a/drivers/staging/rtl8192e/rtl8192e/Makefile b/drivers/staging/rtl8192e/rtl8192e/Makefile
new file mode 100644
index 000000000000..313a92ec6833
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtl8192e/Makefile
@@ -0,0 +1,21 @@
+r8192e_pci-objs :=		\
+	r8192E_dev.o		\
+	r8192E_phy.o		\
+	r8192E_firmware.o	\
+	r8192E_cmdpkt.o		\
+	r8192E_hwimg.o		\
+	r8190P_rtl8256.o	\
+	rtl_cam.o		\
+	rtl_core.o		\
+	rtl_debug.o		\
+	rtl_dm.o		\
+	rtl_eeprom.o		\
+	rtl_ethtool.o		\
+	rtl_pci.o		\
+	rtl_pm.o		\
+	rtl_ps.o		\
+	rtl_wx.o		\
+
+obj-$(CONFIG_RTL8192E) += r8192e_pci.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/staging/rtl8192e/r8190P_def.h b/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
index b7bb71fa9ecd..b7bb71fa9ecd 100644
--- a/drivers/staging/rtl8192e/r8190P_def.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
diff --git a/drivers/staging/rtl8192e/r8190P_rtl8256.c b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
index 0da56c80f088..0da56c80f088 100644
--- a/drivers/staging/rtl8192e/r8190P_rtl8256.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
diff --git a/drivers/staging/rtl8192e/r8190P_rtl8256.h b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h
index 64e831d2f4e5..64e831d2f4e5 100644
--- a/drivers/staging/rtl8192e/r8190P_rtl8256.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h
diff --git a/drivers/staging/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
index 58d044ea5524..58d044ea5524 100644
--- a/drivers/staging/rtl8192e/r8192E_cmdpkt.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
diff --git a/drivers/staging/rtl8192e/r8192E_cmdpkt.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
index 23219e17e5a1..23219e17e5a1 100644
--- a/drivers/staging/rtl8192e/r8192E_cmdpkt.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
diff --git a/drivers/staging/rtl8192e/r8192E_dev.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
index 808aab6fa5ef..808aab6fa5ef 100644
--- a/drivers/staging/rtl8192e/r8192E_dev.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
diff --git a/drivers/staging/rtl8192e/r8192E_dev.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h
index b9b3b52f9120..b9b3b52f9120 100644
--- a/drivers/staging/rtl8192e/r8192E_dev.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h
diff --git a/drivers/staging/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index 37719859bdae..37719859bdae 100644
--- a/drivers/staging/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
diff --git a/drivers/staging/rtl8192e/r8192E_firmware.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
index caa878833106..caa878833106 100644
--- a/drivers/staging/rtl8192e/r8192E_firmware.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
diff --git a/drivers/staging/rtl8192e/r8192E_hw.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h
index 43c3fb859d10..43c3fb859d10 100644
--- a/drivers/staging/rtl8192e/r8192E_hw.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h
diff --git a/drivers/staging/rtl8192e/r8192E_hwimg.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
index 08e7dbb6694b..08e7dbb6694b 100644
--- a/drivers/staging/rtl8192e/r8192E_hwimg.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
diff --git a/drivers/staging/rtl8192e/r8192E_hwimg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
index 019836bb36c2..019836bb36c2 100644
--- a/drivers/staging/rtl8192e/r8192E_hwimg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
diff --git a/drivers/staging/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index 7fe69a3348d7..3e705efaaf22 100644
--- a/drivers/staging/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -23,7 +23,6 @@
 #include "r8190P_rtl8256.h"
 #include "r8192E_phy.h"
 #include "rtl_dm.h"
-#include "dot11d.h"
 
 #include "r8192E_hwimg.h"
 
@@ -859,7 +858,7 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 	RT_TRACE(COMP_TRACE, "====>%s()====stage:%d, step:%d, channel:%d\n",
 		  __func__, *stage, *step, channel);
 
-	if (!IsLegalChannel(priv->rtllib, channel)) {
+	if (!rtllib_legal_channel(priv->rtllib, channel)) {
 		RT_TRACE(COMP_ERR, "=============>set to illegal channel:%d\n",
 			 channel);
 		return true;
diff --git a/drivers/staging/rtl8192e/r8192E_phy.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
index 7318f8857af2..7318f8857af2 100644
--- a/drivers/staging/rtl8192e/r8192E_phy.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
diff --git a/drivers/staging/rtl8192e/r8192E_phyreg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
index 7899dd538dcd..7899dd538dcd 100644
--- a/drivers/staging/rtl8192e/r8192E_phyreg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
diff --git a/drivers/staging/rtl8192e/r819xE_phyreg.h b/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
index d5de279f6644..d5de279f6644 100644
--- a/drivers/staging/rtl8192e/r819xE_phyreg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
diff --git a/drivers/staging/rtl8192e/rtl_cam.c b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
index baf3b6342e44..baf3b6342e44 100644
--- a/drivers/staging/rtl8192e/rtl_cam.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
diff --git a/drivers/staging/rtl8192e/rtl_cam.h b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
index fa607f98b172..fa607f98b172 100644
--- a/drivers/staging/rtl8192e/rtl_cam.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
diff --git a/drivers/staging/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index 5ad96649f407..71adb6b3344d 100644
--- a/drivers/staging/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -53,9 +53,7 @@
 #include "rtl_wx.h"
 #include "rtl_dm.h"
 
-#ifdef CONFIG_PM_RTL
 #include "rtl_pm.h"
-#endif
 
 int hwwep = 1;
 static int channels = 0x3fff;
@@ -581,7 +579,7 @@ static void rtl8192_update_beacon(void *data)
 	struct rtllib_network *net = &ieee->current_network;
 
 	if (ieee->pHTInfo->bCurrentHTSupport)
-		HTUpdateSelfAndPeerSetting(ieee, net);
+		HT_update_self_and_peer_setting(ieee, net);
 	ieee->pHTInfo->bCurrentRT2RTLongSlotTime =
 		 net->bssht.bdRT2RTLongSlotTime;
 	ieee->pHTInfo->RT2RT_HT_Mode = net->bssht.RT2RT_HT_Mode;
@@ -1289,7 +1287,7 @@ static short rtl8192_get_channel_map(struct net_device *dev)
 		priv->ChannelPlan = COUNTRY_CODE_FCC;
 	}
 	RT_TRACE(COMP_INIT, "Channel plan is %d\n", priv->ChannelPlan);
-	Dot11d_Init(priv->rtllib);
+	dot11d_init(priv->rtllib);
 	Dot11d_Channelmap(priv->ChannelPlan, priv->rtllib);
 	for (i = 1; i <= 11; i++)
 		(priv->rtllib->active_channel_map)[i] = 1;
@@ -1305,7 +1303,6 @@ static short rtl8192_init(struct net_device *dev)
 
 	memset(&(priv->stats), 0, sizeof(struct rt_stats));
 
-	rtl8192_dbgp_flag_init(dev);
 	rtl8192_init_priv_handler(dev);
 	rtl8192_init_priv_constant(dev);
 	rtl8192_init_priv_variable(dev);
@@ -2839,7 +2836,6 @@ done:
 /****************************************************************************
 	---------------------------- PCI_STUFF---------------------------
 *****************************************************************************/
-#ifdef HAVE_NET_DEVICE_OPS
 static const struct net_device_ops rtl8192_netdev_ops = {
 	.ndo_open = rtl8192_open,
 	.ndo_stop = rtl8192_close,
@@ -2851,7 +2847,6 @@ static const struct net_device_ops rtl8192_netdev_ops = {
 	.ndo_change_mtu = eth_change_mtu,
 	.ndo_start_xmit = rtllib_xmit,
 };
-#endif
 
 static int __devinit rtl8192_pci_probe(struct pci_dev *pdev,
 			const struct pci_device_id *id)
@@ -2938,17 +2933,7 @@ static int __devinit rtl8192_pci_probe(struct pci_dev *pdev,
 	dev->irq = pdev->irq;
 	priv->irq = 0;
 
-#ifdef HAVE_NET_DEVICE_OPS
 	dev->netdev_ops = &rtl8192_netdev_ops;
-#else
-	dev->open = rtl8192_open;
-	dev->stop = rtl8192_close;
-	dev->tx_timeout = rtl8192_tx_timeout;
-	dev->do_ioctl = rtl8192_ioctl;
-	dev->set_multicast_list = r8192_set_multicast;
-	dev->set_mac_address = r8192_set_mac_adr;
-	dev->hard_start_xmit = rtllib_xmit;
-#endif
 
 	dev->wireless_handlers = (struct iw_handler_def *)
 				 &r8192_wx_handlers_def;
@@ -2974,10 +2959,7 @@ static int __devinit rtl8192_pci_probe(struct pci_dev *pdev,
 
 	register_netdev(dev);
 	RT_TRACE(COMP_INIT, "dev name: %s\n", dev->name);
-	err = rtl_debug_module_init(priv, dev->name);
-	if (err)
-		RT_TRACE(COMP_DBG, "failed to create debugfs files. Ignoring "
-			 "error: %d\n", err);
+
 	rtl8192_proc_init_one(dev);
 
 	if (priv->polling_timer_on == 0)
@@ -3015,7 +2997,6 @@ static void __devexit rtl8192_pci_disconnect(struct pci_dev *pdev)
 		del_timer_sync(&priv->gpio_polling_timer);
 		cancel_delayed_work(&priv->gpio_change_rf_wq);
 		priv->polling_timer_on = 0;
-		rtl_debug_module_remove(priv);
 		rtl8192_proc_remove_one(dev);
 		rtl8192_down(dev, true);
 		deinit_hal_dm(dev);
@@ -3103,43 +3084,9 @@ bool NicIFDisableNIC(struct net_device *dev)
 
 static int __init rtl8192_pci_module_init(void)
 {
-	int ret;
-	int error;
-
-	ret = rtllib_init();
-	if (ret) {
-		printk(KERN_ERR "rtllib_init() failed %d\n", ret);
-		return ret;
-	}
-	ret = rtllib_crypto_init();
-	if (ret) {
-		printk(KERN_ERR "rtllib_crypto_init() failed %d\n", ret);
-		return ret;
-	}
-	ret = rtllib_crypto_tkip_init();
-	if (ret) {
-		printk(KERN_ERR "rtllib_crypto_tkip_init() failed %d\n", ret);
-		return ret;
-	}
-	ret = rtllib_crypto_ccmp_init();
-	if (ret) {
-		printk(KERN_ERR "rtllib_crypto_ccmp_init() failed %d\n", ret);
-		return ret;
-	}
-	ret = rtllib_crypto_wep_init();
-	if (ret) {
-		printk(KERN_ERR "rtllib_crypto_wep_init() failed %d\n", ret);
-		return ret;
-	}
 	printk(KERN_INFO "\nLinux kernel driver for RTL8192E WLAN cards\n");
 	printk(KERN_INFO "Copyright (c) 2007-2008, Realsil Wlan Driver\n");
 
-	error = rtl_create_debugfs_root();
-	if (error) {
-		RT_TRACE(COMP_DBG, "Create debugfs root fail: %d\n", error);
-		goto err_out;
-	}
-
 	rtl8192_proc_module_init();
 	if (0 != pci_register_driver(&rtl8192_pci_driver)) {
 		DMESG("No device found");
@@ -3147,9 +3094,6 @@ static int __init rtl8192_pci_module_init(void)
 		return -ENODEV;
 	}
 	return 0;
-err_out:
-	return error;
-
 }
 
 static void __exit rtl8192_pci_module_exit(void)
@@ -3158,12 +3102,6 @@ static void __exit rtl8192_pci_module_exit(void)
 
 	RT_TRACE(COMP_DOWN, "Exiting");
 	rtl8192_proc_module_remove();
-	rtl_remove_debugfs_root();
-	rtllib_crypto_tkip_exit();
-	rtllib_crypto_ccmp_exit();
-	rtllib_crypto_wep_exit();
-	rtllib_crypto_deinit();
-	rtllib_exit();
 }
 
 void check_rfctrl_gpio_timer(unsigned long data)
diff --git a/drivers/staging/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index f9af5153d9cf..2a2519cc284d 100644
--- a/drivers/staging/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -44,11 +44,14 @@
 #include <linux/proc_fs.h>
 #include <linux/if_arp.h>
 #include <linux/random.h>
-#include <linux/version.h>
 #include <linux/io.h>
-#include "rtllib.h"
 
-#include "dot11d.h"
+/* Need this defined before including local include files */
+#define DRV_NAME "rtl819xE"
+
+#include "../rtllib.h"
+
+#include "../dot11d.h"
 
 #include "r8192E_firmware.h"
 #include "r8192E_hw.h"
@@ -56,7 +59,6 @@
 #include "r8190P_def.h"
 #include "r8192E_dev.h"
 
-#include "rtl_debug.h"
 #include "rtl_eeprom.h"
 #include "rtl_ps.h"
 #include "rtl_pci.h"
@@ -67,8 +69,6 @@
 #define DRV_AUTHOR  "<wlanfae@realtek.com>"
 #define DRV_VERSION  "0014.0401.2010"
 
-#define DRV_NAME "rtl819xE"
-
 #define IS_HARDWARE_TYPE_819xP(_priv)		\
 	((((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8190P) || \
 	(((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8192E))
@@ -215,41 +215,6 @@ enum RTL819x_PHY_PARAM {
 	RTL819X_EFUSE_MAP			= 19,
 };
 
-enum RTL_DEBUG {
-	COMP_TRACE		= BIT0,
-	COMP_DBG		= BIT1,
-	COMP_INIT		= BIT2,
-	COMP_RECV		= BIT3,
-	COMP_SEND		= BIT4,
-	COMP_CMD		= BIT5,
-	COMP_POWER		= BIT6,
-	COMP_EPROM		= BIT7,
-	COMP_SWBW		= BIT8,
-	COMP_SEC		= BIT9,
-	COMP_LPS		= BIT10,
-	COMP_QOS		= BIT11,
-	COMP_RATE		= BIT12,
-	COMP_RXDESC		= BIT13,
-	COMP_PHY		= BIT14,
-	COMP_DIG		= BIT15,
-	COMP_TXAGC		= BIT16,
-	COMP_HALDM		= BIT17,
-	COMP_POWER_TRACKING	= BIT18,
-	COMP_CH			= BIT19,
-	COMP_RF			= BIT20,
-	COMP_FIRMWARE		= BIT21,
-	COMP_HT			= BIT22,
-	COMP_RESET		= BIT23,
-	COMP_CMDPKT		= BIT24,
-	COMP_SCAN		= BIT25,
-	COMP_PS			= BIT26,
-	COMP_DOWN		= BIT27,
-	COMP_INTR		= BIT28,
-	COMP_LED		= BIT29,
-	COMP_MLME		= BIT30,
-	COMP_ERR		= BIT31
-};
-
 enum nic_t {
 	NIC_UNKNOWN     = 0,
 	NIC_8192E       = 1,
@@ -1121,4 +1086,10 @@ void ActUpdateChannelAccessSetting(struct net_device *dev,
 			   enum wireless_mode WirelessMode,
 			   struct channel_access_setting *ChnlAccessSetting);
 
+/* proc stuff from rtl_debug.c */
+void rtl8192_proc_init_one(struct net_device *dev);
+void rtl8192_proc_remove_one(struct net_device *dev);
+void rtl8192_proc_module_init(void);
+void rtl8192_proc_module_remove(void);
+
 #endif
diff --git a/drivers/staging/rtl8192e/rtl_crypto.h b/drivers/staging/rtl8192e/rtl8192e/rtl_crypto.h
index ee57c0f4fa69..ee57c0f4fa69 100644
--- a/drivers/staging/rtl8192e/rtl_crypto.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_crypto.h
diff --git a/drivers/staging/rtl8192e/rtl_debug.c b/drivers/staging/rtl8192e/rtl8192e/rtl_debug.c
index 22bc2dd6e438..c19b14cd6f77 100644
--- a/drivers/staging/rtl8192e/rtl_debug.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_debug.c
@@ -22,91 +22,12 @@
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
 ******************************************************************************/
-#include "rtl_debug.h"
 #include "rtl_core.h"
 #include "r8192E_phy.h"
 #include "r8192E_phyreg.h"
 #include "r8190P_rtl8256.h" /* RTL8225 Radio frontend */
 #include "r8192E_cmdpkt.h"
 
-u32 rt_global_debug_component = \
-				COMP_ERR ;
-
-/*------------------Declare variable-----------------------*/
-u32	DBGP_Type[DBGP_TYPE_MAX];
-
-/*-----------------------------------------------------------------------------
- * Function:    DBGP_Flag_Init
- *
- * Overview:    Refresh all debug print control flag content to zero.
- *
- * Input:       NONE
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Revised History:
- *  When		Who		Remark
- *  10/20/2006	MHC		Create Version 0.
- *
- *---------------------------------------------------------------------------*/
-void	rtl8192_dbgp_flag_init(struct net_device *dev)
-{
-	u8 i;
-
-	for (i = 0; i < DBGP_TYPE_MAX; i++)
-		DBGP_Type[i] = 0;
-
-
-}	/* DBGP_Flag_Init */
-
-/* this is only for debugging */
-void print_buffer(u32 *buffer, int len)
-{
-	int i;
-	u8 *buf = (u8 *)buffer;
-
-	printk(KERN_INFO "ASCII BUFFER DUMP (len: %x):\n", len);
-
-	for (i = 0; i < len; i++)
-		printk(KERN_INFO "%c", buf[i]);
-
-	printk(KERN_INFO "\nBINARY BUFFER DUMP (len: %x):\n", len);
-
-	for (i = 0; i < len; i++)
-		printk(KERN_INFO "%x", buf[i]);
-
-	printk(KERN_INFO "\n");
-}
-
-/* this is only for debug */
-void dump_eprom(struct net_device *dev)
-{
-	int i;
-
-	for (i = 0; i < 0xff; i++)
-		RT_TRACE(COMP_INIT, "EEPROM addr %x : %x", i,
-			 eprom_read(dev, i));
-}
-
-/* this is only for debug */
-void rtl8192_dump_reg(struct net_device *dev)
-{
-	int i;
-	int n;
-	int max = 0x5ff;
-
-	RT_TRACE(COMP_INIT, "Dumping NIC register map");
-
-	for (n = 0; n <= max; ) {
-		printk(KERN_INFO "\nD: %2x> ", n);
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			printk(KERN_INFO "%2x ", read_nic_byte(dev, n));
-	}
-	printk(KERN_INFO "\n");
-}
-
 /****************************************************************************
    -----------------------------PROCFS STUFF-------------------------
 *****************************************************************************/
diff --git a/drivers/staging/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index a7fa9aad6f2d..a7fa9aad6f2d 100644
--- a/drivers/staging/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
diff --git a/drivers/staging/rtl8192e/rtl_dm.h b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
index ab44a9a6927c..ab44a9a6927c 100644
--- a/drivers/staging/rtl8192e/rtl_dm.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
diff --git a/drivers/staging/rtl8192e/rtl_eeprom.c b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c
index c1ccff4a8321..c1ccff4a8321 100644
--- a/drivers/staging/rtl8192e/rtl_eeprom.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c
diff --git a/drivers/staging/rtl8192e/rtl_eeprom.h b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h
index 9452e1683a72..9452e1683a72 100644
--- a/drivers/staging/rtl8192e/rtl_eeprom.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h
diff --git a/drivers/staging/rtl8192e/rtl_ethtool.c b/drivers/staging/rtl8192e/rtl8192e/rtl_ethtool.c
index 36452fb7cef8..36452fb7cef8 100644
--- a/drivers/staging/rtl8192e/rtl_ethtool.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ethtool.c
diff --git a/drivers/staging/rtl8192e/rtl_pci.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
index ddadcc3e4e7c..ddadcc3e4e7c 100644
--- a/drivers/staging/rtl8192e/rtl_pci.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
diff --git a/drivers/staging/rtl8192e/rtl_pci.h b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h
index 7ea5a47dfd2b..28c7da677a80 100644
--- a/drivers/staging/rtl8192e/rtl_pci.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h
@@ -27,7 +27,6 @@
 
 #include <linux/types.h>
 #include <linux/pci.h>
-#include "rtllib.h"
 
 static inline void NdisRawWritePortUlong(u32 port,  u32 val)
 {
diff --git a/drivers/staging/rtl8192e/rtl_pm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
index 92e2fde7f5f4..8e1a5d55dce8 100644
--- a/drivers/staging/rtl8192e/rtl_pm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
@@ -17,7 +17,6 @@
  * wlanfae <wlanfae@realtek.com>
 ******************************************************************************/
 
-#ifdef CONFIG_PM_RTL
 #include "rtl_core.h"
 #include "r8192E_hw.h"
 #include "r8190P_rtl8256.h"
@@ -133,4 +132,3 @@ int rtl8192E_enable_wake(struct pci_dev *dev, pm_message_t state, int enable)
 	return -EAGAIN;
 }
 
-#endif
diff --git a/drivers/staging/rtl8192e/rtl_pm.h b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h
index 4d7f4067cc78..e5299fc3b34a 100644
--- a/drivers/staging/rtl8192e/rtl_pm.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h
@@ -17,8 +17,6 @@
  * wlanfae <wlanfae@realtek.com>
 ******************************************************************************/
 
-#ifdef CONFIG_PM_RTL
-
 #ifndef R8192E_PM_H
 #define R8192E_PM_H
 
@@ -31,5 +29,3 @@ int rtl8192E_resume(struct pci_dev *dev);
 int rtl8192E_enable_wake(struct pci_dev *dev, pm_message_t state, int enable);
 
 #endif
-
-#endif
diff --git a/drivers/staging/rtl8192e/rtl_ps.c b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
index c9a7c563b682..c9a7c563b682 100644
--- a/drivers/staging/rtl8192e/rtl_ps.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
diff --git a/drivers/staging/rtl8192e/rtl_ps.h b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.h
index a9c2d799c08f..df79d6c4ca03 100644
--- a/drivers/staging/rtl8192e/rtl_ps.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.h
@@ -26,7 +26,7 @@
 #define _RTL_PS_H
 
 #include <linux/types.h>
-#include "rtllib.h"
+
 struct net_device;
 
 #define RT_CHECK_FOR_HANG_PERIOD 2
diff --git a/drivers/staging/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index 93b1edbe6bae..4e93669210af 100644
--- a/drivers/staging/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -19,7 +19,6 @@
 
 #include <linux/string.h>
 #include "rtl_core.h"
-#include "dot11d.h"
 
 #define RATE_COUNT 12
 static u32 rtl8192_rates[] = {
@@ -803,7 +802,7 @@ static int r8192_wx_set_enc(struct net_device *dev,
 
 		switch (wrqu->encoding.flags & IW_ENCODE_INDEX) {
 		case 0:
-			key_idx = ieee->tx_keyidx;
+			key_idx = ieee->crypt_info.tx_keyidx;
 			break;
 		case 1:
 			key_idx = 0;
diff --git a/drivers/staging/rtl8192e/rtl_wx.h b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
index 6a51a25ec87d..6a51a25ec87d 100644
--- a/drivers/staging/rtl8192e/rtl_wx.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index 8b9d85c48be6..32fbbc9d0d92 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -18,7 +18,6 @@
 ******************************************************************************/
 #include "rtllib.h"
 #include "rtl819x_BA.h"
-#include "rtl_core.h"
 
 static void ActivateBAEntry(struct rtllib_device *ieee, struct ba_record *pBA,
 			    u16 Time)
diff --git a/drivers/staging/rtl8192e/rtl819x_HTProc.c b/drivers/staging/rtl8192e/rtl819x_HTProc.c
index b1c0c566882f..8b7412980ebb 100644
--- a/drivers/staging/rtl8192e/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_HTProc.c
@@ -943,8 +943,8 @@ void HTResetSelfAndSavePeerSetting(struct rtllib_device *ieee,
 	}
 }
 
-void HTUpdateSelfAndPeerSetting(struct rtllib_device *ieee,
-				struct rtllib_network *pNetwork)
+void HT_update_self_and_peer_setting(struct rtllib_device *ieee,
+				     struct rtllib_network *pNetwork)
 {
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
 	struct ht_info_ele *pPeerHTInfo =
@@ -955,6 +955,7 @@ void HTUpdateSelfAndPeerSetting(struct rtllib_device *ieee,
 			pHTInfo->CurrentOpMode = pPeerHTInfo->OptMode;
 	}
 }
+EXPORT_SYMBOL(HT_update_self_and_peer_setting);
 
 void HTUseDefaultSetting(struct rtllib_device *ieee)
 {
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
index 09a602f74329..711a096be7a7 100644
--- a/drivers/staging/rtl8192e/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -497,6 +497,7 @@ void RemovePeerTS(struct rtllib_device *ieee, u8 *Addr)
 		}
 	}
 }
+EXPORT_SYMBOL(RemovePeerTS);
 
 void RemoveAllTS(struct rtllib_device *ieee)
 {
diff --git a/drivers/staging/rtl8192e/rtl_debug.h b/drivers/staging/rtl8192e/rtl_debug.h
deleted file mode 100644
index 50fb9a9b828a..000000000000
--- a/drivers/staging/rtl8192e/rtl_debug.h
+++ /dev/null
@@ -1,299 +0,0 @@
-/******************************************************************************
- * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
- *
- * Based on the r8180 driver, which is:
- * Copyright 2004-2005 Andrea Merello <andreamrl@tiscali.it>, et al.
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
-#ifndef _RTL_DEBUG_H
-#define _RTL_DEBUG_H
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/version.h>
-#include <linux/debugfs.h>
-
-struct r8192_priv;
-struct _tx_desc_8192se;
-struct _TX_DESC_8192CE;
-struct net_device;
-
-#define	DBG_LOUD	4
-
-#define RT_ASSERT(_Exp, Fmt)				\
-		if (!(_Exp)) {				\
-			printk("Rtl819x: ");		\
-			printk Fmt;			\
-		}
-
-enum dbgp_flag {
-	FQoS				= 0,
-	FTX				= 1,
-	FRX				= 2,
-	FSEC				= 3,
-	FMGNT				= 4,
-	FMLME				= 5,
-	FRESOURCE			= 6,
-	FBEACON				= 7,
-	FISR				= 8,
-	FPHY				= 9,
-	FMP				= 10,
-	FEEPROM				= 11,
-	FPWR				= 12,
-	FDM				= 13,
-	FDBGCtrl			= 14,
-	FC2H				= 15,
-	FBT				= 16,
-	FINIT				= 17,
-	FIOCTL				= 18,
-	DBGP_TYPE_MAX
-};
-
-#define		QoS_INIT				BIT0
-#define		QoS_VISTA				BIT1
-
-#define		TX_DESC					BIT0
-#define		TX_DESC_TID				BIT1
-
-#define		RX_DATA					BIT0
-#define		RX_PHY_STS				BIT1
-#define		RX_PHY_SS				BIT2
-#define		RX_PHY_SQ				BIT3
-#define		RX_PHY_ASTS				BIT4
-#define		RX_ERR_LEN				BIT5
-#define		RX_DEFRAG				BIT6
-#define		RX_ERR_RATE				BIT7
-
-
-
-#define		MEDIA_STS				BIT0
-#define		LINK_STS				BIT1
-
-#define		OS_CHK					BIT0
-
-#define		BCN_SHOW				BIT0
-#define		BCN_PEER				BIT1
-
-#define		ISR_CHK					BIT0
-
-#define		PHY_BBR					BIT0
-#define		PHY_BBW					BIT1
-#define		PHY_RFR					BIT2
-#define		PHY_RFW					BIT3
-#define		PHY_MACR				BIT4
-#define		PHY_MACW				BIT5
-#define		PHY_ALLR				BIT6
-#define		PHY_ALLW				BIT7
-#define		PHY_TXPWR				BIT8
-#define		PHY_PWRDIFF				BIT9
-
-#define		MP_RX					BIT0
-#define		MP_SWICH_CH				BIT1
-
-#define		EEPROM_W				BIT0
-#define		EFUSE_PG				BIT1
-#define		EFUSE_READ_ALL				BIT2
-
-#define		LPS					BIT0
-#define		IPS					BIT1
-#define		PWRSW					BIT2
-#define		PWRHW					BIT3
-#define		PWRHAL					BIT4
-
-#define		WA_IOT					BIT0
-#define		DM_PWDB					BIT1
-#define		DM_Monitor				BIT2
-#define		DM_DIG					BIT3
-#define		DM_EDCA_Turbo				BIT4
-
-#define		DbgCtrl_Trace				BIT0
-#define		DbgCtrl_InbandNoise			BIT1
-
-#define		BT_TRACE				BIT0
-#define		BT_RFPoll				BIT1
-
-#define		C2H_Summary				BIT0
-#define		C2H_PacketData				BIT1
-#define		C2H_ContentData				BIT2
-#define		BT_TRACE				BIT0
-#define		BT_RFPoll				BIT1
-
-#define		INIT_EEPROM				BIT0
-#define		INIT_TxPower				BIT1
-#define		INIT_IQK				BIT2
-#define		INIT_RF					BIT3
-
-#define		IOCTL_TRACE				BIT0
-#define		IOCTL_BT_EVENT				BIT1
-#define		IOCTL_BT_EVENT_DETAIL			BIT2
-#define		IOCTL_BT_TX_ACLDATA			BIT3
-#define		IOCTL_BT_TX_ACLDATA_DETAIL		BIT4
-#define		IOCTL_BT_RX_ACLDATA			BIT5
-#define		IOCTL_BT_RX_ACLDATA_DETAIL		BIT6
-#define		IOCTL_BT_HCICMD				BIT7
-#define		IOCTL_BT_HCICMD_DETAIL			BIT8
-#define		IOCTL_IRP				BIT9
-#define		IOCTL_IRP_DETAIL			BIT10
-#define		IOCTL_CALLBACK_FUN			BIT11
-#define		IOCTL_STATE				BIT12
-#define		IOCTL_BT_TP				BIT13
-#define		IOCTL_BT_LOGO				BIT14
-
-/* 2007/07/13 MH  ------For DeBuG Print modeue------*/
-/*------------------------------Define structure----------------------------*/
-
-
-/*------------------------Export Marco Definition---------------------------*/
-#define		DEBUG_PRINT		1
-
-#if (DEBUG_PRINT == 1)
-#define RTPRINT(dbgtype, dbgflag, printstr)			\
-{								\
-	if (DBGP_Type[dbgtype] & dbgflag) {			\
-		printk printstr;				\
-	}							\
-}
-
-#define	RTPRINT_ADDR(dbgtype, dbgflag, printstr, _Ptr)		\
-{								\
-	if (DBGP_Type[dbgtype] & dbgflag) {			\
-		int __i;					\
-		u8 *ptr = (u8 *)_Ptr;				\
-		printk printstr;				\
-		printk(" ");					\
-		for (__i = 0; __i < 6; __i++)			\
-			printk("%02X%s", ptr[__i],		\
-			       (__i == 5) ? "" : "-");		\
-			printk("\n");				\
-	}							\
-}
-
-#define RTPRINT_DATA(dbgtype, dbgflag, _TitleString, _HexData, _HexDataLen)\
-{								\
-	if (DBGP_Type[dbgtype] & dbgflag) {			\
-		int __i;					\
-		u8 *ptr = (u8 *)_HexData;			\
-		printk(_TitleString);				\
-		for (__i = 0; __i < (int)_HexDataLen; __i++) {	\
-			printk("%02X%s", ptr[__i], (((__i + 1)	\
-			       % 4) == 0) ? "  " : " ");	\
-			if (((__i + 1) % 16) == 0)		\
-				printk("\n");			\
-		}						\
-		printk("\n");					\
-	}							\
-}
-#else
-#define	RTPRINT(dbgtype, dbgflag, printstr)
-#define	RTPRINT_ADDR(dbgtype, dbgflag, printstr, _Ptr)
-#define RTPRINT_DATA(dbgtype, dbgflag, _TitleString, _HexData, _HexDataLen)
-#endif
-
-extern u32	DBGP_Type[DBGP_TYPE_MAX];
-
-#define RT_PRINT_DATA(_Comp, _Level, _TitleString, _HexData, _HexDataLen) \
-do {\
-	if (((_Comp) & rt_global_debug_component) &&			\
-	     (_Level <= rt_global_debug_component)) {			\
-		int __i;						\
-		u8*	ptr = (u8 *)_HexData;				\
-		printk(KERN_INFO "Rtl819x: ");				\
-		printk(_TitleString);					\
-		for (__i = 0; __i < (int)_HexDataLen; __i++) {		\
-			printk("%02X%s", ptr[__i], (((__i + 1) %	\
-			       4) == 0) ? "  " : " ");			\
-			if (((__i + 1) % 16) == 0)			\
-				printk("\n");				\
-		}							\
-		printk("\n");						\
-	} \
-} while (0);
-
-#define DMESG(x, a...)
-#define DMESGW(x, a...)
-#define DMESGE(x, a...)
-extern u32 rt_global_debug_component;
-#define RT_TRACE(component, x, args...) \
-do {			\
-	if (rt_global_debug_component & component) \
-		printk(KERN_DEBUG DRV_NAME ":" x "\n" , \
-		       ##args);\
-} while (0);
-
-#define assert(expr) \
-	if (!(expr)) {				  \
-		printk(KERN_INFO "Assertion failed! %s,%s,%s,line=%d\n", \
-		#expr, __FILE__, __func__, __LINE__);	  \
-	}
-#define RT_DEBUG_DATA(level, data, datalen)      \
-	do {								\
-		if ((rt_global_debug_component & (level)) == (level)) {\
-			int _i;				  \
-			u8 *_pdata = (u8 *)data;		 \
-			printk(KERN_DEBUG DRV_NAME ": %s()\n", __func__);   \
-			for (_i = 0; _i < (int)(datalen); _i++) {	\
-				printk(KERN_INFO "%2x ", _pdata[_i]);	\
-				if ((_i+1) % 16 == 0)			\
-					printk("\n");			\
-			}			       \
-			printk(KERN_INFO "\n");	  \
-		}				       \
-	} while (0)
-
-struct rtl_fs_debug {
-	const char *name;
-	struct dentry *dir_drv;
-	struct dentry *debug_register;
-	u32 hw_type;
-	u32 hw_offset;
-	bool hw_holding;
-};
-
-void print_buffer(u32 *buffer, int len);
-void dump_eprom(struct net_device *dev);
-void rtl8192_dump_reg(struct net_device *dev);
-
-/* debugfs stuff */
-static inline int rtl_debug_module_init(struct r8192_priv *priv,
-					const char *name)
-{
-	return 0;
-}
-
-static inline void rtl_debug_module_remove(struct r8192_priv *priv)
-{
-}
-
-static inline int rtl_create_debugfs_root(void)
-{
-	return 0;
-}
-
-static inline void rtl_remove_debugfs_root(void)
-{
-}
-
-/* proc stuff */
-void rtl8192_proc_init_one(struct net_device *dev);
-void rtl8192_proc_remove_one(struct net_device *dev);
-void rtl8192_proc_module_init(void);
-void rtl8192_proc_module_remove(void);
-void rtl8192_dbgp_flag_init(struct net_device *dev);
-
-#endif
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index de25975ccee4..e26aec86a5c8 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -25,7 +25,6 @@
 #define RTLLIB_H
 #include <linux/if_ether.h> /* ETH_ALEN */
 #include <linux/kernel.h>   /* ARRAY_SIZE */
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/jiffies.h>
@@ -36,12 +35,14 @@
 #include <linux/delay.h>
 #include <linux/wireless.h>
 
+#include "rtllib_debug.h"
 #include "rtl819x_HT.h"
 #include "rtl819x_BA.h"
 #include "rtl819x_TS.h"
 
 #include <linux/netdevice.h>
 #include <linux/if_arp.h> /* ARPHRD_ETHER */
+#include <net/lib80211.h>
 
 #define MAX_PRECMD_CNT 16
 #define MAX_RFDEPENDCMD_CNT 16
@@ -870,69 +871,6 @@ enum _REG_PREAMBLE_MODE {
 #define WLAN_ERP_USE_PROTECTION (1<<1)
 #define WLAN_ERP_BARKER_PREAMBLE (1<<2)
 
-/* Status codes */
-enum rtllib_statuscode {
-	WLAN_STATUS_SUCCESS = 0,
-	WLAN_STATUS_UNSPECIFIED_FAILURE = 1,
-	WLAN_STATUS_CAPS_UNSUPPORTED = 10,
-	WLAN_STATUS_REASSOC_NO_ASSOC = 11,
-	WLAN_STATUS_ASSOC_DENIED_UNSPEC = 12,
-	WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG = 13,
-	WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION = 14,
-	WLAN_STATUS_CHALLENGE_FAIL = 15,
-	WLAN_STATUS_AUTH_TIMEOUT = 16,
-	WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA = 17,
-	WLAN_STATUS_ASSOC_DENIED_RATES = 18,
-	/* 802.11b */
-	WLAN_STATUS_ASSOC_DENIED_NOSHORTPREAMBLE = 19,
-	WLAN_STATUS_ASSOC_DENIED_NOPBCC = 20,
-	WLAN_STATUS_ASSOC_DENIED_NOAGILITY = 21,
-	/* 802.11h */
-	WLAN_STATUS_ASSOC_DENIED_NOSPECTRUM = 22,
-	WLAN_STATUS_ASSOC_REJECTED_BAD_POWER = 23,
-	WLAN_STATUS_ASSOC_REJECTED_BAD_SUPP_CHAN = 24,
-	/* 802.11g */
-	WLAN_STATUS_ASSOC_DENIED_NOSHORTTIME = 25,
-	WLAN_STATUS_ASSOC_DENIED_NODSSSOFDM = 26,
-	/* 802.11i */
-	WLAN_STATUS_INVALID_IE = 40,
-	WLAN_STATUS_INVALID_GROUP_CIPHER = 41,
-	WLAN_STATUS_INVALID_PAIRWISE_CIPHER = 42,
-	WLAN_STATUS_INVALID_AKMP = 43,
-	WLAN_STATUS_UNSUPP_RSN_VERSION = 44,
-	WLAN_STATUS_INVALID_RSN_IE_CAP = 45,
-	WLAN_STATUS_CIPHER_SUITE_REJECTED = 46,
-};
-
-/* Reason codes */
-enum rtllib_reasoncode {
-	WLAN_REASON_UNSPECIFIED = 1,
-	WLAN_REASON_PREV_AUTH_NOT_VALID = 2,
-	WLAN_REASON_DEAUTH_LEAVING = 3,
-	WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY = 4,
-	WLAN_REASON_DISASSOC_AP_BUSY = 5,
-	WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA = 6,
-	WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA = 7,
-	WLAN_REASON_DISASSOC_STA_HAS_LEFT = 8,
-	WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH = 9,
-	/* 802.11h */
-	WLAN_REASON_DISASSOC_BAD_POWER = 10,
-	WLAN_REASON_DISASSOC_BAD_SUPP_CHAN = 11,
-	/* 802.11i */
-	WLAN_REASON_INVALID_IE = 13,
-	WLAN_REASON_MIC_FAILURE = 14,
-	WLAN_REASON_4WAY_HANDSHAKE_TIMEOUT = 15,
-	WLAN_REASON_GROUP_KEY_HANDSHAKE_TIMEOUT = 16,
-	WLAN_REASON_IE_DIFFERENT = 17,
-	WLAN_REASON_INVALID_GROUP_CIPHER = 18,
-	WLAN_REASON_INVALID_PAIRWISE_CIPHER = 19,
-	WLAN_REASON_INVALID_AKMP = 20,
-	WLAN_REASON_UNSUPP_RSN_VERSION = 21,
-	WLAN_REASON_INVALID_RSN_IE_CAP = 22,
-	WLAN_REASON_IEEE8021X_FAILED = 23,
-	WLAN_REASON_CIPHER_SUITE_REJECTED = 24,
-};
-
 #define RTLLIB_STATMASK_SIGNAL (1<<0)
 #define RTLLIB_STATMASK_RSSI (1<<1)
 #define RTLLIB_STATMASK_NOISE (1<<2)
@@ -1122,8 +1060,6 @@ struct rtllib_stats {
 
 struct rtllib_device;
 
-#include "rtllib_crypt.h"
-
 #define SEC_KEY_1	 (1<<0)
 #define SEC_KEY_2	 (1<<1)
 #define SEC_KEY_3	 (1<<2)
@@ -1146,7 +1082,6 @@ struct rtllib_device;
 #define SEC_ALG_TKIP		2
 #define SEC_ALG_CCMP		4
 
-#define WEP_KEYS		4
 #define WEP_KEY_LEN		13
 #define SCM_KEY_LEN		32
 #define SCM_TEMPORAL_KEY_LENGTH 16
@@ -1158,8 +1093,8 @@ struct rtllib_security {
 	    auth_algo:4,
 	    unicast_uses_group:1,
 	    encrypt:1;
-	u8 key_sizes[WEP_KEYS];
-	u8 keys[WEP_KEYS][SCM_KEY_LEN];
+	u8 key_sizes[NUM_WEP_KEYS];
+	u8 keys[NUM_WEP_KEYS][SCM_KEY_LEN];
 	u8 level;
 	u16 flags;
 } __packed;
@@ -2251,14 +2186,10 @@ struct rtllib_device {
 	u8 ap_mac_addr[6];
 	u16 pairwise_key_type;
 	u16 group_key_type;
-	struct list_head crypt_deinit_list;
-	struct rtllib_crypt_data *crypt[WEP_KEYS];
 
-	int tx_keyidx; /* default TX key index (crypt[tx_keyidx]) */
-	struct sw_cam_table swcamtable[TOTAL_CAM_ENTRY];
-	struct timer_list crypt_deinit_timer;
-	int crypt_quiesced;
+	struct lib80211_crypt_info crypt_info;
 
+	struct sw_cam_table swcamtable[TOTAL_CAM_ENTRY];
 	int bcrx_sta_key; /* use individual keys to override default keys even
 			   * with RX of broad/multicast frames */
 
@@ -2774,7 +2705,7 @@ extern void rtllib_rx_mgt(struct rtllib_device *ieee,
 			     struct rtllib_rx_stats *stats);
 extern void rtllib_rx_probe_rq(struct rtllib_device *ieee,
 			   struct sk_buff *skb);
-extern int IsLegalChannel(struct rtllib_device *rtllib, u8 channel);
+extern int rtllib_legal_channel(struct rtllib_device *rtllib, u8 channel);
 
 /* rtllib_wx.c */
 extern int rtllib_wx_get_scan(struct rtllib_device *ieee,
@@ -2804,7 +2735,7 @@ extern int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len);
 
 /* rtllib_softmac.c */
 extern short rtllib_is_54g(struct rtllib_network *net);
-extern short rtllib_is_shortslot(struct rtllib_network net);
+extern short rtllib_is_shortslot(const struct rtllib_network *net);
 extern int rtllib_rx_frame_softmac(struct rtllib_device *ieee,
 				   struct sk_buff *skb,
 				   struct rtllib_rx_stats *rx_stats, u16 type,
@@ -2971,8 +2902,8 @@ extern void HTInitializeHTInfo(struct rtllib_device *ieee);
 extern void HTInitializeBssDesc(struct bss_ht *pBssHT);
 extern void HTResetSelfAndSavePeerSetting(struct rtllib_device *ieee,
 					  struct rtllib_network *pNetwork);
-extern void HTUpdateSelfAndPeerSetting(struct rtllib_device *ieee,
-				       struct rtllib_network *pNetwork);
+extern void HT_update_self_and_peer_setting(struct rtllib_device *ieee,
+					    struct rtllib_network *pNetwork);
 extern u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
 			      u8 *pMCSFilter);
 extern u8 MCS_FILTER_ALL[];
@@ -3052,21 +2983,6 @@ static inline const char *escape_essid(const char *essid, u8 essid_len)
 	(HTMcsToDataRate(_ieee, (u8)_MGN_RATE)))
 
 /* fun with the built-in rtllib stack... */
-int rtllib_init(void);
-void rtllib_exit(void);
-int rtllib_crypto_init(void);
-void rtllib_crypto_deinit(void);
-int rtllib_crypto_tkip_init(void);
-void rtllib_crypto_tkip_exit(void);
-int rtllib_crypto_ccmp_init(void);
-void rtllib_crypto_ccmp_exit(void);
-int rtllib_crypto_wep_init(void);
-void rtllib_crypto_wep_exit(void);
-
-void rtllib_MgntDisconnectIBSS(struct rtllib_device *rtllib);
-void rtllib_MlmeDisassociateRequest(struct rtllib_device *rtllib, u8 *asSta,
-				    u8 asRsn);
-void rtllib_MgntDisconnectAP(struct rtllib_device *rtllib, u8 asRsn);
 bool rtllib_MgntDisconnect(struct rtllib_device *rtllib, u8 asRsn);
 
 
@@ -3133,12 +3049,5 @@ extern void rtllib_TURBO_Info(struct rtllib_device *ieee, u8 **tag_p);
 #define MUTEX_LOCK_PRIV(pmutex) mutex_lock(pmutex)
 #define MUTEX_UNLOCK_PRIV(pmutex) mutex_unlock(pmutex)
 #endif
-static inline void dump_buf(u8 *buf, u32 len)
-{
-	u32 i;
-	printk(KERN_INFO "-----------------Len %d----------------\n", len);
-	for (i = 0; i < len; i++)
-		printk("%2.2x-", *(buf+i));
-	printk("\n");
-}
+
 #endif /* RTLLIB_H */
diff --git a/drivers/staging/rtl8192e/rtllib_crypt.c b/drivers/staging/rtl8192e/rtllib_crypt.c
index acda37b81848..86152d0e6b5d 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt.c
@@ -11,7 +11,6 @@
  *
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -22,7 +21,7 @@
 
 struct rtllib_crypto_alg {
 	struct list_head list;
-	struct rtllib_crypto_ops *ops;
+	struct lib80211_crypto_ops *ops;
 };
 
 
@@ -33,15 +32,15 @@ struct rtllib_crypto {
 
 static struct rtllib_crypto *hcrypt;
 
-void rtllib_crypt_deinit_entries(struct rtllib_device *ieee,
+void rtllib_crypt_deinit_entries(struct lib80211_crypt_info *info,
 					   int force)
 {
 	struct list_head *ptr, *n;
-	struct rtllib_crypt_data *entry;
+	struct lib80211_crypt_data *entry;
 
-	for (ptr = ieee->crypt_deinit_list.next, n = ptr->next;
-	     ptr != &ieee->crypt_deinit_list; ptr = n, n = ptr->next) {
-		entry = list_entry(ptr, struct rtllib_crypt_data, list);
+	for (ptr = info->crypt_deinit_list.next, n = ptr->next;
+	     ptr != &info->crypt_deinit_list; ptr = n, n = ptr->next) {
+		entry = list_entry(ptr, struct lib80211_crypt_data, list);
 
 		if (atomic_read(&entry->refcnt) != 0 && !force)
 			continue;
@@ -53,28 +52,30 @@ void rtllib_crypt_deinit_entries(struct rtllib_device *ieee,
 		kfree(entry);
 	}
 }
+EXPORT_SYMBOL(rtllib_crypt_deinit_entries);
 
 void rtllib_crypt_deinit_handler(unsigned long data)
 {
-	struct rtllib_device *ieee = (struct rtllib_device *)data;
+	struct lib80211_crypt_info *info = (struct lib80211_crypt_info *)data;
 	unsigned long flags;
 
-	spin_lock_irqsave(&ieee->lock, flags);
-	rtllib_crypt_deinit_entries(ieee, 0);
-	if (!list_empty(&ieee->crypt_deinit_list)) {
+	spin_lock_irqsave(info->lock, flags);
+	rtllib_crypt_deinit_entries(info, 0);
+	if (!list_empty(&info->crypt_deinit_list)) {
 		printk(KERN_DEBUG "%s: entries remaining in delayed crypt "
-		       "deletion list\n", ieee->dev->name);
-		ieee->crypt_deinit_timer.expires = jiffies + HZ;
-		add_timer(&ieee->crypt_deinit_timer);
+		       "deletion list\n", info->name);
+		info->crypt_deinit_timer.expires = jiffies + HZ;
+		add_timer(&info->crypt_deinit_timer);
 	}
-	spin_unlock_irqrestore(&ieee->lock, flags);
+	spin_unlock_irqrestore(info->lock, flags);
 
 }
+EXPORT_SYMBOL(rtllib_crypt_deinit_handler);
 
-void rtllib_crypt_delayed_deinit(struct rtllib_device *ieee,
-				    struct rtllib_crypt_data **crypt)
+void rtllib_crypt_delayed_deinit(struct lib80211_crypt_info *info,
+				 struct lib80211_crypt_data **crypt)
 {
-	struct rtllib_crypt_data *tmp;
+	struct lib80211_crypt_data *tmp;
 	unsigned long flags;
 
 	if (*crypt == NULL)
@@ -87,16 +88,17 @@ void rtllib_crypt_delayed_deinit(struct rtllib_device *ieee,
 	 * decrypt operations. Use a list of delayed deinits to avoid needing
 	 * locking. */
 
-	spin_lock_irqsave(&ieee->lock, flags);
-	list_add(&tmp->list, &ieee->crypt_deinit_list);
-	if (!timer_pending(&ieee->crypt_deinit_timer)) {
-		ieee->crypt_deinit_timer.expires = jiffies + HZ;
-		add_timer(&ieee->crypt_deinit_timer);
+	spin_lock_irqsave(info->lock, flags);
+	list_add(&tmp->list, &info->crypt_deinit_list);
+	if (!timer_pending(&info->crypt_deinit_timer)) {
+		info->crypt_deinit_timer.expires = jiffies + HZ;
+		add_timer(&info->crypt_deinit_timer);
 	}
-	spin_unlock_irqrestore(&ieee->lock, flags);
+	spin_unlock_irqrestore(info->lock, flags);
 }
+EXPORT_SYMBOL(rtllib_crypt_delayed_deinit);
 
-int rtllib_register_crypto_ops(struct rtllib_crypto_ops *ops)
+int rtllib_register_crypto_ops(struct lib80211_crypto_ops *ops)
 {
 	unsigned long flags;
 	struct rtllib_crypto_alg *alg;
@@ -104,11 +106,10 @@ int rtllib_register_crypto_ops(struct rtllib_crypto_ops *ops)
 	if (hcrypt == NULL)
 		return -1;
 
-	alg = kmalloc(sizeof(*alg), GFP_KERNEL);
+	alg = kzalloc(sizeof(*alg), GFP_KERNEL);
 	if (alg == NULL)
 		return -ENOMEM;
 
-	memset(alg, 0, sizeof(*alg));
 	alg->ops = ops;
 
 	spin_lock_irqsave(&hcrypt->lock, flags);
@@ -120,8 +121,9 @@ int rtllib_register_crypto_ops(struct rtllib_crypto_ops *ops)
 
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_register_crypto_ops);
 
-int rtllib_unregister_crypto_ops(struct rtllib_crypto_ops *ops)
+int rtllib_unregister_crypto_ops(struct lib80211_crypto_ops *ops)
 {
 	unsigned long flags;
 	struct list_head *ptr;
@@ -150,9 +152,10 @@ int rtllib_unregister_crypto_ops(struct rtllib_crypto_ops *ops)
 
 	return del_alg ? 0 : -1;
 }
+EXPORT_SYMBOL(rtllib_unregister_crypto_ops);
 
 
-struct rtllib_crypto_ops *rtllib_get_crypto_ops(const char *name)
+struct lib80211_crypto_ops *rtllib_get_crypto_ops(const char *name)
 {
 	unsigned long flags;
 	struct list_head *ptr;
@@ -177,12 +180,13 @@ struct rtllib_crypto_ops *rtllib_get_crypto_ops(const char *name)
 	else
 		return NULL;
 }
+EXPORT_SYMBOL(rtllib_get_crypto_ops);
 
 
 static void * rtllib_crypt_null_init(int keyidx) { return (void *) 1; }
 static void rtllib_crypt_null_deinit(void *priv) {}
 
-static struct rtllib_crypto_ops rtllib_crypt_null = {
+static struct lib80211_crypto_ops rtllib_crypt_null = {
 	.name			= "NULL",
 	.init			= rtllib_crypt_null_init,
 	.deinit			= rtllib_crypt_null_deinit,
@@ -192,8 +196,10 @@ static struct rtllib_crypto_ops rtllib_crypt_null = {
 	.decrypt_msdu		= NULL,
 	.set_key		= NULL,
 	.get_key		= NULL,
-	.extra_prefix_len	= 0,
-	.extra_postfix_len	= 0,
+	.extra_mpdu_prefix_len	= 0,
+	.extra_mpdu_postfix_len	= 0,
+	.extra_msdu_prefix_len	= 0,
+	.extra_msdu_postfix_len	= 0,
 	.owner			= THIS_MODULE,
 };
 
@@ -202,15 +208,14 @@ int __init rtllib_crypto_init(void)
 {
 	int ret = -ENOMEM;
 
-	hcrypt = kmalloc(sizeof(*hcrypt), GFP_KERNEL);
+	hcrypt = kzalloc(sizeof(*hcrypt), GFP_KERNEL);
 	if (!hcrypt)
 		goto out;
 
-	memset(hcrypt, 0, sizeof(*hcrypt));
 	INIT_LIST_HEAD(&hcrypt->algs);
 	spin_lock_init(&hcrypt->lock);
 
-	ret = rtllib_register_crypto_ops(&rtllib_crypt_null);
+	ret = lib80211_register_crypto_ops(&rtllib_crypt_null);
 	if (ret < 0) {
 		kfree(hcrypt);
 		hcrypt = NULL;
@@ -239,3 +244,8 @@ void __exit rtllib_crypto_deinit(void)
 
 	kfree(hcrypt);
 }
+
+module_init(rtllib_crypto_init);
+module_exit(rtllib_crypto_deinit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_crypt.h b/drivers/staging/rtl8192e/rtllib_crypt.h
index 49b90b73ed9c..e177c9287b44 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt.h
+++ b/drivers/staging/rtl8192e/rtllib_crypt.h
@@ -25,61 +25,11 @@
 
 #include <linux/skbuff.h>
 
-struct rtllib_crypto_ops {
-	const char *name;
-
-	/* init new crypto context (e.g., allocate private data space,
-	 * select IV, etc.); returns NULL on failure or pointer to allocated
-	 * private data on success */
-	void * (*init)(int keyidx);
-
-	/* deinitialize crypto context and free allocated private data */
-	void (*deinit)(void *priv);
-
-	/* encrypt/decrypt return < 0 on error or >= 0 on success. The return
-	 * value from decrypt_mpdu is passed as the keyidx value for
-	 * decrypt_msdu. skb must have enough head and tail room for the
-	 * encryption; if not, error will be returned; these functions are
-	 * called for all MPDUs (i.e., fragments).
-	 */
-	int (*encrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
-	int (*decrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
-
-	/* These functions are called for full MSDUs, i.e. full frames.
-	 * These can be NULL if full MSDU operations are not needed. */
-	int (*encrypt_msdu)(struct sk_buff *skb, int hdr_len, void *priv);
-	int (*decrypt_msdu)(struct sk_buff *skb, int keyidx, int hdr_len,
-			    void *priv, struct rtllib_device* ieee);
-
-	int (*set_key)(void *key, int len, u8 *seq, void *priv);
-	int (*get_key)(void *key, int len, u8 *seq, void *priv);
-
-	/* procfs handler for printing out key information and possible
-	 * statistics */
-	char * (*print_stats)(char *p, void *priv);
-
-	/* maximum number of bytes added by encryption; encrypt buf is
-	 * allocated with extra_prefix_len bytes, copy of in_buf, and
-	 * extra_postfix_len; encrypt need not use all this space, but
-	 * the result must start at the beginning of the struct buffer and
-	 * correct length must be returned */
-	int extra_prefix_len, extra_postfix_len;
-
-	struct module *owner;
-};
-
-struct rtllib_crypt_data {
-	struct list_head list; /* delayed deletion list */
-	struct rtllib_crypto_ops *ops;
-	void *priv;
-	atomic_t refcnt;
-};
-
-int rtllib_register_crypto_ops(struct rtllib_crypto_ops *ops);
-int rtllib_unregister_crypto_ops(struct rtllib_crypto_ops *ops);
-struct rtllib_crypto_ops *rtllib_get_crypto_ops(const char *name);
-void rtllib_crypt_deinit_entries(struct rtllib_device *, int);
-void rtllib_crypt_deinit_handler(unsigned long);
-void rtllib_crypt_delayed_deinit(struct rtllib_device *ieee,
-				 struct rtllib_crypt_data **crypt);
+int rtllib_register_crypto_ops(struct lib80211_crypto_ops *ops);
+int rtllib_unregister_crypto_ops(struct lib80211_crypto_ops *ops);
+struct lib80211_crypto_ops *rtllib_get_crypto_ops(const char *name);
+void rtllib_crypt_deinit_entries(struct lib80211_crypt_info *info, int force);
+void rtllib_crypt_deinit_handler(unsigned long data);
+void rtllib_crypt_delayed_deinit(struct lib80211_crypt_info *info,
+				 struct lib80211_crypt_data **crypt);
 #endif
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
index 6196b9aa3a09..4217b88e6fc3 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
@@ -9,7 +9,6 @@
  * more details.
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -63,10 +62,9 @@ static void *rtllib_ccmp_init(int key_idx)
 {
 	struct rtllib_ccmp_data *priv;
 
-	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
 	if (priv == NULL)
 		goto fail;
-	memset(priv, 0, sizeof(*priv));
 	priv->key_idx = key_idx;
 
 	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
@@ -429,13 +427,8 @@ static char *rtllib_ccmp_print_stats(char *p, void *priv)
 	return p;
 }
 
-void rtllib_ccmp_null(void)
-{
-	return;
-}
-
-static struct rtllib_crypto_ops rtllib_crypt_ccmp = {
-	.name			= "CCMP",
+static struct lib80211_crypto_ops rtllib_crypt_ccmp = {
+	.name			= "R-CCMP",
 	.init			= rtllib_ccmp_init,
 	.deinit			= rtllib_ccmp_deinit,
 	.encrypt_mpdu		= rtllib_ccmp_encrypt,
@@ -445,19 +438,24 @@ static struct rtllib_crypto_ops rtllib_crypt_ccmp = {
 	.set_key		= rtllib_ccmp_set_key,
 	.get_key		= rtllib_ccmp_get_key,
 	.print_stats		= rtllib_ccmp_print_stats,
-	.extra_prefix_len	= CCMP_HDR_LEN,
-	.extra_postfix_len	= CCMP_MIC_LEN,
+	.extra_mpdu_prefix_len = CCMP_HDR_LEN,
+	.extra_mpdu_postfix_len = CCMP_MIC_LEN,
 	.owner			= THIS_MODULE,
 };
 
 
 int __init rtllib_crypto_ccmp_init(void)
 {
-	return rtllib_register_crypto_ops(&rtllib_crypt_ccmp);
+	return lib80211_register_crypto_ops(&rtllib_crypt_ccmp);
 }
 
 
 void __exit rtllib_crypto_ccmp_exit(void)
 {
-	rtllib_unregister_crypto_ops(&rtllib_crypt_ccmp);
+	lib80211_unregister_crypto_ops(&rtllib_crypt_ccmp);
 }
+
+module_init(rtllib_crypto_ccmp_init);
+module_exit(rtllib_crypto_ccmp_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
index 6a0c87886422..800925053fb0 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
@@ -9,7 +9,6 @@
  * more details.
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -60,10 +59,9 @@ static void *rtllib_tkip_init(int key_idx)
 {
 	struct rtllib_tkip_data *priv;
 
-	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
 	if (priv == NULL)
 		goto fail;
-	memset(priv, 0, sizeof(*priv));
 	priv->key_idx = key_idx;
 	priv->tx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
 			CRYPTO_ALG_ASYNC);
@@ -598,8 +596,7 @@ static void rtllib_michael_mic_failure(struct net_device *dev,
 }
 
 static int rtllib_michael_mic_verify(struct sk_buff *skb, int keyidx,
-				     int hdr_len, void *priv,
-				     struct rtllib_device *ieee)
+				     int hdr_len, void *priv)
 {
 	struct rtllib_tkip_data *tkey = priv;
 	u8 mic[8];
@@ -618,23 +615,20 @@ static int rtllib_michael_mic_verify(struct sk_buff *skb, int keyidx,
 			skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 		return -1;
 
-	if ((memcmp(mic, skb->data + skb->len - 8, 8) != 0) ||
-	   (ieee->force_mic_error)) {
+	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 		struct rtllib_hdr_4addr *hdr;
 		hdr = (struct rtllib_hdr_4addr *) skb->data;
 		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 		       "MSDU from %pM keyidx=%d\n",
 		       skb->dev ? skb->dev->name : "N/A", hdr->addr2,
 		       keyidx);
-		printk(KERN_DEBUG "%d, force_mic_error = %d\n",
-		       (memcmp(mic, skb->data + skb->len - 8, 8) != 0),\
-			ieee->force_mic_error);
+		printk(KERN_DEBUG "%d\n",
+		       memcmp(mic, skb->data + skb->len - 8, 8) != 0);
 		if (skb->dev) {
 			printk(KERN_INFO "skb->dev != NULL\n");
 			rtllib_michael_mic_failure(skb->dev, hdr, keyidx);
 		}
 		tkey->dot11RSNAStatsTKIPLocalMICFailures++;
-		ieee->force_mic_error = false;
 		return -1;
 	}
 
@@ -740,9 +734,8 @@ static char *rtllib_tkip_print_stats(char *p, void *priv)
 	return p;
 }
 
-
-static struct rtllib_crypto_ops rtllib_crypt_tkip = {
-	.name			= "TKIP",
+static struct lib80211_crypto_ops rtllib_crypt_tkip = {
+	.name			= "R-TKIP",
 	.init			= rtllib_tkip_init,
 	.deinit			= rtllib_tkip_deinit,
 	.encrypt_mpdu		= rtllib_tkip_encrypt,
@@ -752,24 +745,25 @@ static struct rtllib_crypto_ops rtllib_crypt_tkip = {
 	.set_key		= rtllib_tkip_set_key,
 	.get_key		= rtllib_tkip_get_key,
 	.print_stats		= rtllib_tkip_print_stats,
-	.extra_prefix_len	= 4 + 4, /* IV + ExtIV */
-	.extra_postfix_len	= 8 + 4, /* MIC + ICV */
+	.extra_mpdu_prefix_len = 4 + 4,	/* IV + ExtIV */
+	.extra_mpdu_postfix_len = 4,	/* ICV */
+	.extra_msdu_postfix_len = 8,	/* MIC */
 	.owner			= THIS_MODULE,
 };
 
 
 int __init rtllib_crypto_tkip_init(void)
 {
-	return rtllib_register_crypto_ops(&rtllib_crypt_tkip);
+	return lib80211_register_crypto_ops(&rtllib_crypt_tkip);
 }
 
 
 void __exit rtllib_crypto_tkip_exit(void)
 {
-	rtllib_unregister_crypto_ops(&rtllib_crypt_tkip);
+	lib80211_unregister_crypto_ops(&rtllib_crypt_tkip);
 }
 
-void rtllib_tkip_null(void)
-{
-	return;
-}
+module_init(rtllib_crypto_tkip_init);
+module_exit(rtllib_crypto_tkip_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_wep.c b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
index c59bf10fe780..8cdf38913a33 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_wep.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
@@ -9,7 +9,6 @@
  * more details.
  */
 
-#include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -38,10 +37,9 @@ static void *prism2_wep_init(int keyidx)
 {
 	struct prism2_wep_data *priv;
 
-	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
 	if (priv == NULL)
 		goto fail;
-	memset(priv, 0, sizeof(*priv));
 	priv->key_idx = keyidx;
 
 	priv->tx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
@@ -257,9 +255,8 @@ static char *prism2_wep_print_stats(char *p, void *priv)
 	return p;
 }
 
-
-static struct rtllib_crypto_ops rtllib_crypt_wep = {
-	.name			= "WEP",
+static struct lib80211_crypto_ops rtllib_crypt_wep = {
+	.name			= "R-WEP",
 	.init			= prism2_wep_init,
 	.deinit			= prism2_wep_deinit,
 	.encrypt_mpdu		= prism2_wep_encrypt,
@@ -269,24 +266,24 @@ static struct rtllib_crypto_ops rtllib_crypt_wep = {
 	.set_key		= prism2_wep_set_key,
 	.get_key		= prism2_wep_get_key,
 	.print_stats		= prism2_wep_print_stats,
-	.extra_prefix_len	= 4, /* IV */
-	.extra_postfix_len	= 4, /* ICV */
+	.extra_mpdu_prefix_len  = 4,	/* IV */
+	.extra_mpdu_postfix_len = 4,	/* ICV */
 	.owner			= THIS_MODULE,
 };
 
 
 int __init rtllib_crypto_wep_init(void)
 {
-	return rtllib_register_crypto_ops(&rtllib_crypt_wep);
+	return lib80211_register_crypto_ops(&rtllib_crypt_wep);
 }
 
 
 void __exit rtllib_crypto_wep_exit(void)
 {
-	rtllib_unregister_crypto_ops(&rtllib_crypt_wep);
+	lib80211_unregister_crypto_ops(&rtllib_crypt_wep);
 }
 
-void rtllib_wep_null(void)
-{
-	return;
-}
+module_init(rtllib_crypto_wep_init);
+module_exit(rtllib_crypto_wep_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_debug.h b/drivers/staging/rtl8192e/rtllib_debug.h
new file mode 100644
index 000000000000..2bfc1155f505
--- /dev/null
+++ b/drivers/staging/rtl8192e/rtllib_debug.h
@@ -0,0 +1,86 @@
+/******************************************************************************
+ * Copyright(c) 2008 - 2010 Realtek Corporation. All rights reserved.
+ *
+ * Based on the r8180 driver, which is:
+ * Copyright 2004-2005 Andrea Merello <andreamrl@tiscali.it>, et al.
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+******************************************************************************/
+#ifndef _RTL_DEBUG_H
+#define _RTL_DEBUG_H
+
+/* Allow files to override DRV_NAME */
+#ifndef DRV_NAME
+#define DRV_NAME "rtllib_92e"
+#endif
+
+#define DMESG(x, a...)
+
+extern u32 rt_global_debug_component;
+
+/* These are the defines for rt_global_debug_component */
+enum RTL_DEBUG {
+	COMP_TRACE		= (1 << 0),
+	COMP_DBG		= (1 << 1),
+	COMP_INIT		= (1 << 2),
+	COMP_RECV		= (1 << 3),
+	COMP_SEND		= (1 << 4),
+	COMP_CMD		= (1 << 5),
+	COMP_POWER		= (1 << 6),
+	COMP_EPROM		= (1 << 7),
+	COMP_SWBW		= (1 << 8),
+	COMP_SEC		= (1 << 9),
+	COMP_LPS		= (1 << 10),
+	COMP_QOS		= (1 << 11),
+	COMP_RATE		= (1 << 12),
+	COMP_RXDESC		= (1 << 13),
+	COMP_PHY		= (1 << 14),
+	COMP_DIG		= (1 << 15),
+	COMP_TXAGC		= (1 << 16),
+	COMP_HALDM		= (1 << 17),
+	COMP_POWER_TRACKING	= (1 << 18),
+	COMP_CH			= (1 << 19),
+	COMP_RF			= (1 << 20),
+	COMP_FIRMWARE		= (1 << 21),
+	COMP_HT			= (1 << 22),
+	COMP_RESET		= (1 << 23),
+	COMP_CMDPKT		= (1 << 24),
+	COMP_SCAN		= (1 << 25),
+	COMP_PS			= (1 << 26),
+	COMP_DOWN		= (1 << 27),
+	COMP_INTR		= (1 << 28),
+	COMP_LED		= (1 << 29),
+	COMP_MLME		= (1 << 30),
+	COMP_ERR		= (1 << 31)
+};
+
+#define RT_TRACE(component, x, args...)		\
+do {			\
+	if (rt_global_debug_component & component) \
+		printk(KERN_DEBUG DRV_NAME ":" x "\n" , \
+		       ##args);\
+} while (0);
+
+#define assert(expr) \
+	if (!(expr)) {				  \
+		printk(KERN_INFO "Assertion failed! %s,%s,%s,line=%d\n", \
+		#expr, __FILE__, __func__, __LINE__);	  \
+	}
+
+#endif
diff --git a/drivers/staging/rtl8192e/rtllib_module.c b/drivers/staging/rtl8192e/rtllib_module.c
index c36a140a4568..f9dae958a5d4 100644
--- a/drivers/staging/rtl8192e/rtllib_module.c
+++ b/drivers/staging/rtl8192e/rtllib_module.c
@@ -45,7 +45,6 @@
 #include <linux/slab.h>
 #include <linux/tcp.h>
 #include <linux/types.h>
-#include <linux/version.h>
 #include <linux/wireless.h>
 #include <linux/etherdevice.h>
 #include <linux/uaccess.h>
@@ -54,7 +53,9 @@
 #include "rtllib.h"
 
 
-#define DRV_NAME "rtllib_92e"
+u32 rt_global_debug_component = COMP_ERR;
+EXPORT_SYMBOL(rt_global_debug_component);
+
 
 void _setup_timer(struct timer_list *ptimer, void *fun, unsigned long data)
 {
@@ -135,10 +136,6 @@ struct net_device *alloc_rtllib(int sizeof_priv)
 	ieee->host_decrypt = 1;
 	ieee->ieee802_1x = 1; /* Default to supporting 802.1x */
 
-	INIT_LIST_HEAD(&ieee->crypt_deinit_list);
-	_setup_timer(&ieee->crypt_deinit_timer,
-		    rtllib_crypt_deinit_handler,
-		    (unsigned long) ieee);
 	ieee->rtllib_ap_sec_type = rtllib_ap_sec_type;
 
 	spin_lock_init(&ieee->lock);
@@ -148,6 +145,9 @@ struct net_device *alloc_rtllib(int sizeof_priv)
 	atomic_set(&(ieee->atm_chnlop), 0);
 	atomic_set(&(ieee->atm_swbw), 0);
 
+	/* SAM FIXME */
+	lib80211_crypt_info_init(&ieee->crypt_info, "RTLLIB", &ieee->lock);
+
 	ieee->bHalfNMode = false;
 	ieee->wpa_enabled = 0;
 	ieee->tkip_countermeasures = 0;
@@ -177,10 +177,6 @@ struct net_device *alloc_rtllib(int sizeof_priv)
 		ieee->last_packet_time[i] = 0;
 	}
 
-	rtllib_tkip_null();
-	rtllib_wep_null();
-	rtllib_ccmp_null();
-
 	return dev;
 
  failed:
@@ -188,32 +184,23 @@ struct net_device *alloc_rtllib(int sizeof_priv)
 		free_netdev(dev);
 	return NULL;
 }
+EXPORT_SYMBOL(alloc_rtllib);
 
 void free_rtllib(struct net_device *dev)
 {
 	struct rtllib_device *ieee = (struct rtllib_device *)
 				      netdev_priv_rsl(dev);
-	int i;
 
 	kfree(ieee->pHTInfo);
 	ieee->pHTInfo = NULL;
 	rtllib_softmac_free(ieee);
-	del_timer_sync(&ieee->crypt_deinit_timer);
-	rtllib_crypt_deinit_entries(ieee, 1);
-
-	for (i = 0; i < WEP_KEYS; i++) {
-		struct rtllib_crypt_data *crypt = ieee->crypt[i];
-		if (crypt) {
-			if (crypt->ops)
-				crypt->ops->deinit(crypt->priv);
-			kfree(crypt);
-			ieee->crypt[i] = NULL;
-		}
-	}
+
+	lib80211_crypt_info_free(&ieee->crypt_info);
 
 	rtllib_networks_free(ieee);
 	free_netdev(dev);
 }
+EXPORT_SYMBOL(free_rtllib);
 
 u32 rtllib_debug_level;
 static int debug = \
@@ -287,3 +274,8 @@ void __exit rtllib_exit(void)
 		rtllib_proc = NULL;
 	}
 }
+
+module_init(rtllib_init);
+module_exit(rtllib_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index 8d0af5ed8ecf..6c5061f12bad 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -36,7 +36,6 @@
 #include <linux/slab.h>
 #include <linux/tcp.h>
 #include <linux/types.h>
-#include <linux/version.h>
 #include <linux/wireless.h>
 #include <linux/etherdevice.h>
 #include <linux/uaccess.h>
@@ -281,7 +280,7 @@ static int rtllib_is_eapol_frame(struct rtllib_device *ieee,
 /* Called only as a tasklet (software IRQ), by rtllib_rx */
 static inline int
 rtllib_rx_frame_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
-			struct rtllib_crypt_data *crypt)
+			struct lib80211_crypt_data *crypt)
 {
 	struct rtllib_hdr_4addr *hdr;
 	int res, hdrlen;
@@ -322,7 +321,7 @@ rtllib_rx_frame_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 /* Called only as a tasklet (software IRQ), by rtllib_rx */
 static inline int
 rtllib_rx_frame_decrypt_msdu(struct rtllib_device *ieee, struct sk_buff *skb,
-			     int keyidx, struct rtllib_crypt_data *crypt)
+			     int keyidx, struct lib80211_crypt_data *crypt)
 {
 	struct rtllib_hdr_4addr *hdr;
 	int res, hdrlen;
@@ -341,7 +340,7 @@ rtllib_rx_frame_decrypt_msdu(struct rtllib_device *ieee, struct sk_buff *skb,
 	hdrlen = rtllib_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
 
 	atomic_inc(&crypt->refcnt);
-	res = crypt->ops->decrypt_msdu(skb, keyidx, hdrlen, crypt->priv, ieee);
+	res = crypt->ops->decrypt_msdu(skb, keyidx, hdrlen, crypt->priv);
 	atomic_dec(&crypt->refcnt);
 	if (res < 0) {
 		printk(KERN_DEBUG "%s: MSDU decryption/MIC verification failed"
@@ -1010,7 +1009,7 @@ static int rtllib_rx_data_filter(struct rtllib_device *ieee, u16 fc,
 }
 
 static int rtllib_rx_get_crypt(struct rtllib_device *ieee, struct sk_buff *skb,
-			struct rtllib_crypt_data **crypt, size_t hdrlen)
+			struct lib80211_crypt_data **crypt, size_t hdrlen)
 {
 	struct rtllib_hdr_4addr *hdr = (struct rtllib_hdr_4addr *)skb->data;
 	u16 fc = le16_to_cpu(hdr->frame_ctl);
@@ -1020,7 +1019,7 @@ static int rtllib_rx_get_crypt(struct rtllib_device *ieee, struct sk_buff *skb,
 		if (skb->len >= hdrlen + 3)
 			idx = skb->data[hdrlen + 3] >> 6;
 
-		*crypt = ieee->crypt[idx];
+		*crypt = ieee->crypt_info.crypt[idx];
 		/* allow NULL decrypt to indicate an station specific override
 		 * for default encryption */
 		if (*crypt && ((*crypt)->ops == NULL ||
@@ -1045,7 +1044,7 @@ static int rtllib_rx_get_crypt(struct rtllib_device *ieee, struct sk_buff *skb,
 
 static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 		      struct rtllib_rx_stats *rx_stats,
-		      struct rtllib_crypt_data *crypt, size_t hdrlen)
+		      struct lib80211_crypt_data *crypt, size_t hdrlen)
 {
 	struct rtllib_hdr_4addr *hdr;
 	int keyidx = 0;
@@ -1253,7 +1252,7 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 {
 	struct net_device *dev = ieee->dev;
 	struct rtllib_hdr_4addr *hdr = (struct rtllib_hdr_4addr *)skb->data;
-	struct rtllib_crypt_data *crypt = NULL;
+	struct lib80211_crypt_data *crypt = NULL;
 	struct rtllib_rxb *rxb = NULL;
 	struct rx_ts_record *pTS = NULL;
 	u16 fc, sc, SeqNum = 0;
@@ -1497,6 +1496,7 @@ int rtllib_rx(struct rtllib_device *ieee, struct sk_buff *skb,
 	ieee->stats.rx_dropped++;
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_rx);
 
 static u8 qos_oui[QOS_OUI_LEN] = { 0x00, 0x50, 0xF2 };
 
@@ -2492,7 +2492,7 @@ static int IsPassiveChannel(struct rtllib_device *rtllib, u8 channel)
 	return 0;
 }
 
-int IsLegalChannel(struct rtllib_device *rtllib, u8 channel)
+int rtllib_legal_channel(struct rtllib_device *rtllib, u8 channel)
 {
 	if (MAX_CHANNEL_NUMBER < channel) {
 		printk(KERN_INFO "%s(): Invalid Channel\n", __func__);
@@ -2503,6 +2503,7 @@ int IsLegalChannel(struct rtllib_device *rtllib, u8 channel)
 
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_legal_channel);
 
 static inline void rtllib_process_probe_response(
 	struct rtllib_device *ieee,
@@ -2553,7 +2554,7 @@ static inline void rtllib_process_probe_response(
 	}
 
 
-	if (!IsLegalChannel(ieee, network->channel))
+	if (!rtllib_legal_channel(ieee, network->channel))
 		goto free_network;
 
 	if (WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index b5086850f0de..1637f1110991 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -15,11 +15,9 @@
 
 
 #include "rtllib.h"
-#include "rtl_core.h"
 
 #include <linux/random.h>
 #include <linux/delay.h>
-#include <linux/version.h>
 #include <linux/uaccess.h>
 #include "dot11d.h"
 
@@ -28,9 +26,9 @@ short rtllib_is_54g(struct rtllib_network *net)
 	return (net->rates_ex_len > 0) || (net->rates_len > 4);
 }
 
-short rtllib_is_shortslot(struct rtllib_network net)
+short rtllib_is_shortslot(const struct rtllib_network *net)
 {
-	return net.capability & WLAN_CAPABILITY_SHORT_SLOT_TIME;
+	return net->capability & WLAN_CAPABILITY_SHORT_SLOT_TIME;
 }
 
 /* returns the total length needed for pleacing the RATE MFIE
@@ -468,6 +466,7 @@ void rtllib_EnableIntelPromiscuousMode(struct net_device *dev,
 
 	ieee->bNetPromiscuousMode = true;
 }
+EXPORT_SYMBOL(rtllib_EnableIntelPromiscuousMode);
 
 
 /*
@@ -490,6 +489,7 @@ void rtllib_DisableIntelPromiscuousMode(struct net_device *dev,
 
 	ieee->bNetPromiscuousMode = false;
 }
+EXPORT_SYMBOL(rtllib_DisableIntelPromiscuousMode);
 
 static void rtllib_send_probe(struct rtllib_device *ieee, u8 is_mesh)
 {
@@ -685,6 +685,7 @@ void rtllib_stop_send_beacons(struct rtllib_device *ieee)
 	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
 		rtllib_beacons_stop(ieee);
 }
+EXPORT_SYMBOL(rtllib_stop_send_beacons);
 
 
 void rtllib_start_send_beacons(struct rtllib_device *ieee)
@@ -694,6 +695,7 @@ void rtllib_start_send_beacons(struct rtllib_device *ieee)
 	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
 		rtllib_beacons_start(ieee);
 }
+EXPORT_SYMBOL(rtllib_start_send_beacons);
 
 
 static void rtllib_softmac_stop_scan(struct rtllib_device *ieee)
@@ -719,6 +721,7 @@ void rtllib_stop_scan(struct rtllib_device *ieee)
 			ieee->rtllib_stop_hw_scan(ieee->dev);
 	}
 }
+EXPORT_SYMBOL(rtllib_stop_scan);
 
 void rtllib_stop_scan_syncro(struct rtllib_device *ieee)
 {
@@ -729,6 +732,7 @@ void rtllib_stop_scan_syncro(struct rtllib_device *ieee)
 			ieee->rtllib_stop_hw_scan(ieee->dev);
 	}
 }
+EXPORT_SYMBOL(rtllib_stop_scan_syncro);
 
 bool rtllib_act_scanning(struct rtllib_device *ieee, bool sync_scan)
 {
@@ -741,6 +745,7 @@ bool rtllib_act_scanning(struct rtllib_device *ieee, bool sync_scan)
 		return test_bit(STATUS_SCANNING, &ieee->status);
 	}
 }
+EXPORT_SYMBOL(rtllib_act_scanning);
 
 /* called with ieee->lock held */
 static void rtllib_start_scan(struct rtllib_device *ieee)
@@ -781,6 +786,7 @@ void rtllib_start_scan_syncro(struct rtllib_device *ieee, u8 is_mesh)
 			ieee->rtllib_start_hw_scan(ieee->dev);
 	}
 }
+EXPORT_SYMBOL(rtllib_start_scan_syncro);
 
 inline struct sk_buff *rtllib_authentication_req(struct rtllib_network *beacon,
 	struct rtllib_device *ieee, int challengelen, u8 *daddr)
@@ -830,7 +836,7 @@ static struct sk_buff *rtllib_probe_resp(struct rtllib_device *ieee, u8 *dest)
 	struct sk_buff *skb = NULL;
 	int encrypt;
 	int atim_len, erp_len;
-	struct rtllib_crypt_data *crypt;
+	struct lib80211_crypt_data *crypt;
 
 	char *ssid = ieee->current_network.ssid;
 	int ssid_len = ieee->current_network.ssid_len;
@@ -865,9 +871,9 @@ static struct sk_buff *rtllib_probe_resp(struct rtllib_device *ieee, u8 *dest)
 	} else
 		erp_len = 0;
 
-	crypt = ieee->crypt[ieee->tx_keyidx];
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 	encrypt = ieee->host_encrypt && crypt && crypt->ops &&
-		((0 == strcmp(crypt->ops->name, "WEP") || wpa_ie_len));
+		((0 == strcmp(crypt->ops->name, "R-WEP") || wpa_ie_len));
 	if (ieee->pHTInfo->bCurrentHTSupport) {
 		tmp_ht_cap_buf = (u8 *) &(ieee->pHTInfo->SelfHTCap);
 		tmp_ht_cap_len = sizeof(ieee->pHTInfo->SelfHTCap);
@@ -917,7 +923,7 @@ static struct sk_buff *rtllib_probe_resp(struct rtllib_device *ieee, u8 *dest)
 		cpu_to_le16((beacon_buf->capability |=
 				 WLAN_CAPABILITY_SHORT_SLOT_TIME));
 
-	crypt = ieee->crypt[ieee->tx_keyidx];
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 	if (encrypt)
 		beacon_buf->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
 
@@ -976,7 +982,7 @@ static struct sk_buff *rtllib_assoc_resp(struct rtllib_device *ieee, u8 *dest)
 	struct sk_buff *skb;
 	u8 *tag;
 
-	struct rtllib_crypt_data *crypt;
+	struct lib80211_crypt_data *crypt;
 	struct rtllib_assoc_response_frame *assoc;
 	short encrypt;
 
@@ -1007,7 +1013,7 @@ static struct sk_buff *rtllib_assoc_resp(struct rtllib_device *ieee, u8 *dest)
 				 cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT_TIME);
 
 	if (ieee->host_encrypt)
-		crypt = ieee->crypt[ieee->tx_keyidx];
+		crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 	else
 		crypt = NULL;
 
@@ -1172,7 +1178,7 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 	unsigned int ckip_ie_len = 0;
 	unsigned int ccxrm_ie_len = 0;
 	unsigned int cxvernum_ie_len = 0;
-	struct rtllib_crypt_data *crypt;
+	struct lib80211_crypt_data *crypt;
 	int encrypt;
 	int	PMKCacheIdx;
 
@@ -1185,10 +1191,10 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 	unsigned int turbo_info_len = beacon->Turbo_Enable ? 9 : 0;
 
 	int len = 0;
-	crypt = ieee->crypt[ieee->tx_keyidx];
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 	if (crypt != NULL)
 		encrypt = ieee->host_encrypt && crypt && crypt->ops &&
-			  ((0 == strcmp(crypt->ops->name, "WEP") ||
+			  ((0 == strcmp(crypt->ops->name, "R-WEP") ||
 			  wpa_ie_len));
 	else
 		encrypt = 0;
@@ -1956,6 +1962,7 @@ void rtllib_sta_ps_send_null_frame(struct rtllib_device *ieee, short pwr)
 	if (buf)
 		softmac_ps_mgmt_xmit(buf, ieee);
 }
+EXPORT_SYMBOL(rtllib_sta_ps_send_null_frame);
 
 void rtllib_sta_ps_send_pspoll_frame(struct rtllib_device *ieee)
 {
@@ -2168,6 +2175,7 @@ void rtllib_ps_tx_ack(struct rtllib_device *ieee, short success)
 	}
 	spin_unlock_irqrestore(&ieee->lock, flags);
 }
+EXPORT_SYMBOL(rtllib_ps_tx_ack);
 
 static void rtllib_process_action(struct rtllib_device *ieee, struct sk_buff *skb)
 {
@@ -2540,6 +2548,7 @@ void rtllib_reset_queue(struct rtllib_device *ieee)
 	spin_unlock_irqrestore(&ieee->lock, flags);
 
 }
+EXPORT_SYMBOL(rtllib_reset_queue);
 
 void rtllib_wake_queue(struct rtllib_device *ieee)
 {
@@ -2928,6 +2937,7 @@ struct sk_buff *rtllib_get_beacon(struct rtllib_device *ieee)
 
 	return skb;
 }
+EXPORT_SYMBOL(rtllib_get_beacon);
 
 void rtllib_softmac_stop_protocol(struct rtllib_device *ieee, u8 mesh_flag,
 				  u8 shutdown)
@@ -2937,6 +2947,7 @@ void rtllib_softmac_stop_protocol(struct rtllib_device *ieee, u8 mesh_flag,
 	rtllib_stop_protocol(ieee, shutdown);
 	up(&ieee->wx_sem);
 }
+EXPORT_SYMBOL(rtllib_softmac_stop_protocol);
 
 
 void rtllib_stop_protocol(struct rtllib_device *ieee, u8 shutdown)
@@ -2985,6 +2996,7 @@ void rtllib_softmac_start_protocol(struct rtllib_device *ieee, u8 mesh_flag)
 	rtllib_start_protocol(ieee);
 	up(&ieee->wx_sem);
 }
+EXPORT_SYMBOL(rtllib_softmac_start_protocol);
 
 void rtllib_start_protocol(struct rtllib_device *ieee)
 {
@@ -3048,10 +3060,9 @@ void rtllib_softmac_init(struct rtllib_device *ieee)
 	ieee->state = RTLLIB_NOLINK;
 	for (i = 0; i < 5; i++)
 		ieee->seq_ctrl[i] = 0;
-	ieee->pDot11dInfo = kmalloc(sizeof(struct rt_dot11d_info), GFP_ATOMIC);
+	ieee->pDot11dInfo = kzalloc(sizeof(struct rt_dot11d_info), GFP_ATOMIC);
 	if (!ieee->pDot11dInfo)
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't alloc memory for DOT11D\n");
-	memset(ieee->pDot11dInfo, 0, sizeof(struct rt_dot11d_info));
 	ieee->LinkDetectInfo.SlotIndex = 0;
 	ieee->LinkDetectInfo.SlotNum = 2;
 	ieee->LinkDetectInfo.NumRecvBcnInPeriod = 0;
@@ -3207,11 +3218,11 @@ static int rtllib_wpa_set_wpa_ie(struct rtllib_device *ieee,
 		return -EINVAL;
 
 	if (param->u.wpa_ie.len) {
-		buf = kmalloc(param->u.wpa_ie.len, GFP_KERNEL);
+		buf = kmemdup(param->u.wpa_ie.data, param->u.wpa_ie.len,
+			      GFP_KERNEL);
 		if (buf == NULL)
 			return -ENOMEM;
 
-		memcpy(buf, param->u.wpa_ie.data, param->u.wpa_ie.len);
 		kfree(ieee->wpa_ie);
 		ieee->wpa_ie = buf;
 		ieee->wpa_ie_len = param->u.wpa_ie.len;
@@ -3334,8 +3345,8 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 				  u8 is_mesh)
 {
 	int ret = 0;
-	struct rtllib_crypto_ops *ops;
-	struct rtllib_crypt_data **crypt;
+	struct lib80211_crypto_ops *ops;
+	struct lib80211_crypt_data **crypt;
 
 	struct rtllib_security sec = {
 		.flags = 0,
@@ -3354,9 +3365,9 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
 	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
 	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
-		if (param->u.crypt.idx >= WEP_KEYS)
+		if (param->u.crypt.idx >= NUM_WEP_KEYS)
 			return -EINVAL;
-		crypt = &ieee->crypt[param->u.crypt.idx];
+		crypt = &ieee->crypt_info.crypt[param->u.crypt.idx];
 	} else {
 		return -EINVAL;
 	}
@@ -3366,7 +3377,7 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 			sec.enabled = 0;
 			sec.level = SEC_LEVEL_0;
 			sec.flags |= SEC_ENABLED | SEC_LEVEL;
-			rtllib_crypt_delayed_deinit(ieee, crypt);
+			lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 		}
 		goto done;
 	}
@@ -3375,19 +3386,19 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 
 	/* IPW HW cannot build TKIP MIC, host decryption still needed. */
 	if (!(ieee->host_encrypt || ieee->host_decrypt) &&
-	    strcmp(param->u.crypt.alg, "TKIP"))
+	    strcmp(param->u.crypt.alg, "R-TKIP"))
 		goto skip_host_crypt;
 
-	ops = rtllib_get_crypto_ops(param->u.crypt.alg);
-	if (ops == NULL && strcmp(param->u.crypt.alg, "WEP") == 0) {
+	ops = lib80211_get_crypto_ops(param->u.crypt.alg);
+	if (ops == NULL && strcmp(param->u.crypt.alg, "R-WEP") == 0) {
 		request_module("rtllib_crypt_wep");
-		ops = rtllib_get_crypto_ops(param->u.crypt.alg);
-	} else if (ops == NULL && strcmp(param->u.crypt.alg, "TKIP") == 0) {
+		ops = lib80211_get_crypto_ops(param->u.crypt.alg);
+	} else if (ops == NULL && strcmp(param->u.crypt.alg, "R-TKIP") == 0) {
 		request_module("rtllib_crypt_tkip");
-		ops = rtllib_get_crypto_ops(param->u.crypt.alg);
-	} else if (ops == NULL && strcmp(param->u.crypt.alg, "CCMP") == 0) {
+		ops = lib80211_get_crypto_ops(param->u.crypt.alg);
+	} else if (ops == NULL && strcmp(param->u.crypt.alg, "R-CCMP") == 0) {
 		request_module("rtllib_crypt_ccmp");
-		ops = rtllib_get_crypto_ops(param->u.crypt.alg);
+		ops = lib80211_get_crypto_ops(param->u.crypt.alg);
 	}
 	if (ops == NULL) {
 		printk(KERN_INFO "unknown crypto alg '%s'\n",
@@ -3397,17 +3408,17 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 		goto done;
 	}
 	if (*crypt == NULL || (*crypt)->ops != ops) {
-		struct rtllib_crypt_data *new_crypt;
+		struct lib80211_crypt_data *new_crypt;
 
-		rtllib_crypt_delayed_deinit(ieee, crypt);
+		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 
-		new_crypt = (struct rtllib_crypt_data *)
+		new_crypt = (struct lib80211_crypt_data *)
 			kmalloc(sizeof(*new_crypt), GFP_KERNEL);
 		if (new_crypt == NULL) {
 			ret = -ENOMEM;
 			goto done;
 		}
-		memset(new_crypt, 0, sizeof(struct rtllib_crypt_data));
+		memset(new_crypt, 0, sizeof(struct lib80211_crypt_data));
 		new_crypt->ops = ops;
 		if (new_crypt->ops)
 			new_crypt->priv =
@@ -3435,7 +3446,7 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 
  skip_host_crypt:
 	if (param->u.crypt.set_tx) {
-		ieee->tx_keyidx = param->u.crypt.idx;
+		ieee->crypt_info.tx_keyidx = param->u.crypt.idx;
 		sec.active_key = param->u.crypt.idx;
 		sec.flags |= SEC_ACTIVE_KEY;
 	} else
@@ -3448,13 +3459,13 @@ static int rtllib_wpa_set_encryption(struct rtllib_device *ieee,
 		sec.key_sizes[param->u.crypt.idx] = param->u.crypt.key_len;
 		sec.flags |= (1 << param->u.crypt.idx);
 
-		if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+		if (strcmp(param->u.crypt.alg, "R-WEP") == 0) {
 			sec.flags |= SEC_LEVEL;
 			sec.level = SEC_LEVEL_1;
-		} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+		} else if (strcmp(param->u.crypt.alg, "R-TKIP") == 0) {
 			sec.flags |= SEC_LEVEL;
 			sec.level = SEC_LEVEL_2;
-		} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+		} else if (strcmp(param->u.crypt.alg, "R-CCMP") == 0) {
 			sec.flags |= SEC_LEVEL;
 			sec.level = SEC_LEVEL_3;
 		}
@@ -3551,13 +3562,13 @@ u8 rtllib_ap_sec_type(struct rtllib_device *ieee)
 	static u8 ccmp_ie[4] = {0x00, 0x50, 0xf2, 0x04};
 	static u8 ccmp_rsn_ie[4] = {0x00, 0x0f, 0xac, 0x04};
 	int wpa_ie_len = ieee->wpa_ie_len;
-	struct rtllib_crypt_data *crypt;
+	struct lib80211_crypt_data *crypt;
 	int encrypt;
 
-	crypt = ieee->crypt[ieee->tx_keyidx];
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 	encrypt = (ieee->current_network.capability & WLAN_CAPABILITY_PRIVACY)
 		  || (ieee->host_encrypt && crypt && crypt->ops &&
-		  (0 == strcmp(crypt->ops->name, "WEP")));
+		  (0 == strcmp(crypt->ops->name, "R-WEP")));
 
 	/* simply judge  */
 	if (encrypt && (wpa_ie_len == 0)) {
@@ -3634,6 +3645,7 @@ out:
 
 	return ret;
 }
+EXPORT_SYMBOL(rtllib_wpa_supplicant_ioctl);
 
 void rtllib_MgntDisconnectIBSS(struct rtllib_device *rtllib)
 {
@@ -3719,6 +3731,7 @@ bool rtllib_MgntDisconnect(struct rtllib_device *rtllib, u8 asRsn)
 
 	return true;
 }
+EXPORT_SYMBOL(rtllib_MgntDisconnect);
 
 void notify_wx_assoc_event(struct rtllib_device *ieee)
 {
@@ -3739,3 +3752,4 @@ void notify_wx_assoc_event(struct rtllib_device *ieee)
 	}
 	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
 }
+EXPORT_SYMBOL(notify_wx_assoc_event);
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
index 22988fbd444b..1523bc7a2105 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -15,7 +15,6 @@
 
 
 #include "rtllib.h"
-#include "rtl_core.h"
 #include "dot11d.h"
 /* FIXME: add A freqs */
 
@@ -25,6 +24,7 @@ const long rtllib_wlan_frequencies[] = {
 	2452, 2457, 2462, 2467,
 	2472, 2484
 };
+EXPORT_SYMBOL(rtllib_wlan_frequencies);
 
 
 int rtllib_wx_set_freq(struct rtllib_device *ieee, struct iw_request_info *a,
@@ -82,6 +82,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(rtllib_wx_set_freq);
 
 
 int rtllib_wx_get_freq(struct rtllib_device *ieee,
@@ -97,6 +98,7 @@ int rtllib_wx_get_freq(struct rtllib_device *ieee,
 	fwrq->e = 1;
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_get_freq);
 
 int rtllib_wx_get_wap(struct rtllib_device *ieee,
 			    struct iw_request_info *info,
@@ -125,6 +127,7 @@ int rtllib_wx_get_wap(struct rtllib_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_get_wap);
 
 
 int rtllib_wx_set_wap(struct rtllib_device *ieee,
@@ -184,6 +187,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(rtllib_wx_set_wap);
 
 int rtllib_wx_get_essid(struct rtllib_device *ieee, struct iw_request_info *a,
 			 union iwreq_data *wrqu, char *b)
@@ -220,6 +224,7 @@ out:
 	return ret;
 
 }
+EXPORT_SYMBOL(rtllib_wx_get_essid);
 
 int rtllib_wx_set_rate(struct rtllib_device *ieee,
 			     struct iw_request_info *info,
@@ -231,6 +236,7 @@ int rtllib_wx_set_rate(struct rtllib_device *ieee,
 	ieee->rate = target_rate/100000;
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_set_rate);
 
 int rtllib_wx_get_rate(struct rtllib_device *ieee,
 			     struct iw_request_info *info,
@@ -243,6 +249,7 @@ int rtllib_wx_get_rate(struct rtllib_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_get_rate);
 
 
 int rtllib_wx_set_rts(struct rtllib_device *ieee,
@@ -259,6 +266,7 @@ int rtllib_wx_set_rts(struct rtllib_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_set_rts);
 
 int rtllib_wx_get_rts(struct rtllib_device *ieee,
 			     struct iw_request_info *info,
@@ -269,6 +277,7 @@ int rtllib_wx_get_rts(struct rtllib_device *ieee,
 	wrqu->rts.disabled = (wrqu->rts.value == DEFAULT_RTS_THRESHOLD);
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_get_rts);
 
 int rtllib_wx_set_mode(struct rtllib_device *ieee, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
@@ -314,6 +323,7 @@ out:
 	up(&ieee->wx_sem);
 	return set_mode_status;
 }
+EXPORT_SYMBOL(rtllib_wx_set_mode);
 
 void rtllib_wx_sync_scan_wq(void *data)
 {
@@ -428,6 +438,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(rtllib_wx_set_scan);
 
 int rtllib_wx_set_essid(struct rtllib_device *ieee,
 			struct iw_request_info *a,
@@ -490,6 +501,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(rtllib_wx_set_essid);
 
 int rtllib_wx_get_mode(struct rtllib_device *ieee, struct iw_request_info *a,
 		       union iwreq_data *wrqu, char *b)
@@ -497,6 +509,7 @@ int rtllib_wx_get_mode(struct rtllib_device *ieee, struct iw_request_info *a,
 	wrqu->mode = ieee->iw_mode;
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_get_mode);
 
 int rtllib_wx_set_rawtx(struct rtllib_device *ieee,
 			struct iw_request_info *info,
@@ -533,6 +546,7 @@ int rtllib_wx_set_rawtx(struct rtllib_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_set_rawtx);
 
 int rtllib_wx_get_name(struct rtllib_device *ieee,
 			     struct iw_request_info *info,
@@ -548,6 +562,7 @@ int rtllib_wx_get_name(struct rtllib_device *ieee,
 		strcat(wrqu->name, "n");
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_get_name);
 
 
 /* this is mostly stolen from hostap */
@@ -605,6 +620,7 @@ exit:
 	return ret;
 
 }
+EXPORT_SYMBOL(rtllib_wx_set_power);
 
 /* this is stolen from hostap */
 int rtllib_wx_get_power(struct rtllib_device *ieee,
@@ -643,3 +659,4 @@ exit:
 	return ret;
 
 }
+EXPORT_SYMBOL(rtllib_wx_get_power);
diff --git a/drivers/staging/rtl8192e/rtllib_tx.c b/drivers/staging/rtl8192e/rtllib_tx.c
index 44e8006bc1af..f451bfc27a86 100644
--- a/drivers/staging/rtl8192e/rtllib_tx.c
+++ b/drivers/staging/rtl8192e/rtllib_tx.c
@@ -46,7 +46,6 @@
 #include <linux/slab.h>
 #include <linux/tcp.h>
 #include <linux/types.h>
-#include <linux/version.h>
 #include <linux/wireless.h>
 #include <linux/etherdevice.h>
 #include <linux/uaccess.h>
@@ -180,10 +179,10 @@ inline int rtllib_put_snap(u8 *data, u16 h_proto)
 int rtllib_encrypt_fragment(struct rtllib_device *ieee, struct sk_buff *frag,
 			    int hdr_len)
 {
-	struct rtllib_crypt_data *crypt = NULL;
+	struct lib80211_crypt_data *crypt = NULL;
 	int res;
 
-	crypt = ieee->crypt[ieee->tx_keyidx];
+	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 
 	if (!(crypt && crypt->ops)) {
 		printk(KERN_INFO "=========>%s(), crypt is null\n", __func__);
@@ -569,7 +568,7 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 	};
 	u8 dest[ETH_ALEN], src[ETH_ALEN];
 	int qos_actived = ieee->current_network.qos_data.active;
-	struct rtllib_crypt_data *crypt = NULL;
+	struct lib80211_crypt_data *crypt = NULL;
 	struct cb_desc *tcb_desc;
 	u8 bIsMulticast = false;
 	u8 IsAmsdu = false;
@@ -646,7 +645,7 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		}
 
 		skb->priority = rtllib_classify(skb, IsAmsdu);
-		crypt = ieee->crypt[ieee->tx_keyidx];
+		crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 		encrypt = !(ether_type == ETH_P_PAE && ieee->ieee802_1x) &&
 			ieee->host_encrypt && crypt && crypt->ops;
 		if (!encrypt && ieee->ieee802_1x &&
@@ -742,8 +741,10 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		/* Each fragment may need to have room for encryptiong
 		 * pre/postfix */
 		if (encrypt) {
-			bytes_per_frag -= crypt->ops->extra_prefix_len +
-				crypt->ops->extra_postfix_len;
+			bytes_per_frag -= crypt->ops->extra_mpdu_prefix_len +
+				crypt->ops->extra_mpdu_postfix_len +
+				crypt->ops->extra_msdu_prefix_len +
+				crypt->ops->extra_msdu_postfix_len;
 		}
 		/* Number of fragments is the total bytes_per_frag /
 		* payload_per_fragment */
@@ -791,7 +792,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 				else
 					tcb_desc->bHwSec = 0;
 				skb_reserve(skb_frag,
-					    crypt->ops->extra_prefix_len);
+					    crypt->ops->extra_mpdu_prefix_len +
+					    crypt->ops->extra_msdu_prefix_len);
 			} else {
 				tcb_desc->bHwSec = 0;
 			}
@@ -965,3 +967,4 @@ int rtllib_xmit(struct sk_buff *skb, struct net_device *dev)
 	memset(skb->cb, 0, sizeof(skb->cb));
 	return rtllib_xmit_inter(skb, dev);
 }
+EXPORT_SYMBOL(rtllib_xmit);
diff --git a/drivers/staging/rtl8192e/rtllib_wx.c b/drivers/staging/rtl8192e/rtllib_wx.c
index 8cea4a60e1b3..c27ff7edbaf2 100644
--- a/drivers/staging/rtl8192e/rtllib_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_wx.c
@@ -30,7 +30,6 @@
 
 ******************************************************************************/
 #include <linux/wireless.h>
-#include <linux/version.h>
 #include <linux/kmod.h>
 #include <linux/module.h>
 
@@ -295,6 +294,7 @@ int rtllib_wx_get_scan(struct rtllib_device *ieee,
 
 	return err;
 }
+EXPORT_SYMBOL(rtllib_wx_get_scan);
 
 int rtllib_wx_set_encode(struct rtllib_device *ieee,
 			    struct iw_request_info *info,
@@ -306,44 +306,44 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 		.flags = 0
 	};
 	int i, key, key_provided, len;
-	struct rtllib_crypt_data **crypt;
+	struct lib80211_crypt_data **crypt;
 
 	RTLLIB_DEBUG_WX("SET_ENCODE\n");
 
 	key = erq->flags & IW_ENCODE_INDEX;
 	if (key) {
-		if (key > WEP_KEYS)
+		if (key > NUM_WEP_KEYS)
 			return -EINVAL;
 		key--;
 		key_provided = 1;
 	} else {
 		key_provided = 0;
-		key = ieee->tx_keyidx;
+		key = ieee->crypt_info.tx_keyidx;
 	}
 
 	RTLLIB_DEBUG_WX("Key: %d [%s]\n", key, key_provided ?
 			   "provided" : "default");
-	crypt = &ieee->crypt[key];
+	crypt = &ieee->crypt_info.crypt[key];
 	if (erq->flags & IW_ENCODE_DISABLED) {
 		if (key_provided && *crypt) {
 			RTLLIB_DEBUG_WX("Disabling encryption on key %d.\n",
 					   key);
-			rtllib_crypt_delayed_deinit(ieee, crypt);
+			lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 		} else
 			RTLLIB_DEBUG_WX("Disabling encryption.\n");
 
 		/* Check all the keys to see if any are still configured,
 		 * and if no key index was provided, de-init them all */
-		for (i = 0; i < WEP_KEYS; i++) {
-			if (ieee->crypt[i] != NULL) {
+		for (i = 0; i < NUM_WEP_KEYS; i++) {
+			if (ieee->crypt_info.crypt[i] != NULL) {
 				if (key_provided)
 					break;
-				rtllib_crypt_delayed_deinit(ieee,
-							    &ieee->crypt[i]);
+				lib80211_crypt_delayed_deinit(&ieee->crypt_info,
+							    &ieee->crypt_info.crypt[i]);
 			}
 		}
 
-		if (i == WEP_KEYS) {
+		if (i == NUM_WEP_KEYS) {
 			sec.enabled = 0;
 			sec.level = SEC_LEVEL_0;
 			sec.flags |= SEC_ENABLED | SEC_LEVEL;
@@ -358,25 +358,24 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 	sec.flags |= SEC_ENABLED;
 
 	if (*crypt != NULL && (*crypt)->ops != NULL &&
-	    strcmp((*crypt)->ops->name, "WEP") != 0) {
+	    strcmp((*crypt)->ops->name, "R-WEP") != 0) {
 		/* changing to use WEP; deinit previously used algorithm
 		 * on this key */
-		rtllib_crypt_delayed_deinit(ieee, crypt);
+		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 	}
 
 	if (*crypt == NULL) {
-		struct rtllib_crypt_data *new_crypt;
+		struct lib80211_crypt_data *new_crypt;
 
 		/* take WEP into use */
-		new_crypt = kmalloc(sizeof(struct rtllib_crypt_data),
+		new_crypt = kzalloc(sizeof(struct lib80211_crypt_data),
 				    GFP_KERNEL);
 		if (new_crypt == NULL)
 			return -ENOMEM;
-		memset(new_crypt, 0, sizeof(struct rtllib_crypt_data));
-		new_crypt->ops = rtllib_get_crypto_ops("WEP");
+		new_crypt->ops = lib80211_get_crypto_ops("R-WEP");
 		if (!new_crypt->ops) {
 			request_module("rtllib_crypt_wep");
-			new_crypt->ops = rtllib_get_crypto_ops("WEP");
+			new_crypt->ops = lib80211_get_crypto_ops("R-WEP");
 		}
 
 		if (new_crypt->ops)
@@ -412,7 +411,7 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 		 * explicitely set */
 		if (key == sec.active_key)
 			sec.flags |= SEC_ACTIVE_KEY;
-		ieee->tx_keyidx = key;
+		ieee->crypt_info.tx_keyidx = key;
 
 	} else {
 		len = (*crypt)->ops->get_key(sec.keys[key], WEP_KEY_LEN,
@@ -435,7 +434,7 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 		if (key_provided) {
 			RTLLIB_DEBUG_WX(
 				"Setting key %d to default Tx key.\n", key);
-			ieee->tx_keyidx = key;
+			ieee->crypt_info.tx_keyidx = key;
 			sec.active_key = key;
 			sec.flags |= SEC_ACTIVE_KEY;
 		}
@@ -470,6 +469,7 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_set_encode);
 
 int rtllib_wx_get_encode(struct rtllib_device *ieee,
 			    struct iw_request_info *info,
@@ -477,7 +477,7 @@ int rtllib_wx_get_encode(struct rtllib_device *ieee,
 {
 	struct iw_point *erq = &(wrqu->encoding);
 	int len, key;
-	struct rtllib_crypt_data *crypt;
+	struct lib80211_crypt_data *crypt;
 
 	RTLLIB_DEBUG_WX("GET_ENCODE\n");
 
@@ -486,13 +486,13 @@ int rtllib_wx_get_encode(struct rtllib_device *ieee,
 
 	key = erq->flags & IW_ENCODE_INDEX;
 	if (key) {
-		if (key > WEP_KEYS)
+		if (key > NUM_WEP_KEYS)
 			return -EINVAL;
 		key--;
 	} else {
-		key = ieee->tx_keyidx;
+		key = ieee->crypt_info.tx_keyidx;
 	}
-	crypt = ieee->crypt[key];
+	crypt = ieee->crypt_info.crypt[key];
 
 	erq->flags = key + 1;
 
@@ -513,6 +513,7 @@ int rtllib_wx_get_encode(struct rtllib_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_get_encode);
 
 int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 			       struct iw_request_info *info,
@@ -525,29 +526,29 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 	int i, idx;
 	int group_key = 0;
 	const char *alg, *module;
-	struct rtllib_crypto_ops *ops;
-	struct rtllib_crypt_data **crypt;
+	struct lib80211_crypto_ops *ops;
+	struct lib80211_crypt_data **crypt;
 
 	struct rtllib_security sec = {
 		.flags = 0,
 	};
 	idx = encoding->flags & IW_ENCODE_INDEX;
 	if (idx) {
-		if (idx < 1 || idx > WEP_KEYS)
+		if (idx < 1 || idx > NUM_WEP_KEYS)
 			return -EINVAL;
 		idx--;
 	} else{
-			idx = ieee->tx_keyidx;
+			idx = ieee->crypt_info.tx_keyidx;
 	}
 	if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) {
-		crypt = &ieee->crypt[idx];
+		crypt = &ieee->crypt_info.crypt[idx];
 		group_key = 1;
 	} else {
 		/* some Cisco APs use idx>0 for unicast in dynamic WEP */
 		if (idx != 0 && ext->alg != IW_ENCODE_ALG_WEP)
 			return -EINVAL;
 		if (ieee->iw_mode == IW_MODE_INFRA)
-			crypt = &ieee->crypt[idx];
+			crypt = &ieee->crypt_info.crypt[idx];
 		else
 			return -EINVAL;
 	}
@@ -556,13 +557,13 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 	if ((encoding->flags & IW_ENCODE_DISABLED) ||
 	    ext->alg == IW_ENCODE_ALG_NONE) {
 		if (*crypt)
-			rtllib_crypt_delayed_deinit(ieee, crypt);
+			lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 
-		for (i = 0; i < WEP_KEYS; i++) {
-			if (ieee->crypt[i] != NULL)
+		for (i = 0; i < NUM_WEP_KEYS; i++) {
+			if (ieee->crypt_info.crypt[i] != NULL)
 				break;
 		}
-		if (i == WEP_KEYS) {
+		if (i == NUM_WEP_KEYS) {
 			sec.enabled = 0;
 			sec.level = SEC_LEVEL_0;
 			sec.flags |= SEC_LEVEL;
@@ -573,15 +574,15 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 	sec.enabled = 1;
 	switch (ext->alg) {
 	case IW_ENCODE_ALG_WEP:
-		alg = "WEP";
+		alg = "R-WEP";
 		module = "rtllib_crypt_wep";
 		break;
 	case IW_ENCODE_ALG_TKIP:
-		alg = "TKIP";
+		alg = "R-TKIP";
 		module = "rtllib_crypt_tkip";
 		break;
 	case IW_ENCODE_ALG_CCMP:
-		alg = "CCMP";
+		alg = "R-CCMP";
 		module = "rtllib_crypt_ccmp";
 		break;
 	default:
@@ -592,14 +593,14 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 	}
 	printk(KERN_INFO "alg name:%s\n", alg);
 
-	 ops = rtllib_get_crypto_ops(alg);
+	ops = lib80211_get_crypto_ops(alg);
 	if (ops == NULL) {
 		char tempbuf[100];
 
 		memset(tempbuf, 0x00, 100);
 		sprintf(tempbuf, "%s", module);
 		request_module("%s", tempbuf);
-		ops = rtllib_get_crypto_ops(alg);
+		ops = lib80211_get_crypto_ops(alg);
 	}
 	if (ops == NULL) {
 		RTLLIB_DEBUG_WX("%s: unknown crypto alg %d\n",
@@ -610,9 +611,9 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 	}
 
 	if (*crypt == NULL || (*crypt)->ops != ops) {
-		struct rtllib_crypt_data *new_crypt;
+		struct lib80211_crypt_data *new_crypt;
 
-		rtllib_crypt_delayed_deinit(ieee, crypt);
+		lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 
 		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
 		if (new_crypt == NULL) {
@@ -641,7 +642,7 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 		goto done;
 	}
 	if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
-		ieee->tx_keyidx = idx;
+		ieee->crypt_info.tx_keyidx = idx;
 		sec.active_key = idx;
 		sec.flags |= SEC_ACTIVE_KEY;
 	}
@@ -674,6 +675,7 @@ done:
 	}
 	return ret;
 }
+EXPORT_SYMBOL(rtllib_wx_set_encode_ext);
 
 int rtllib_wx_get_encode_ext(struct rtllib_device *ieee,
 			       struct iw_request_info *info,
@@ -681,7 +683,7 @@ int rtllib_wx_get_encode_ext(struct rtllib_device *ieee,
 {
 	struct iw_point *encoding = &wrqu->encoding;
 	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
-	struct rtllib_crypt_data *crypt;
+	struct lib80211_crypt_data *crypt;
 	int idx, max_key_len;
 
 	max_key_len = encoding->length - sizeof(*ext);
@@ -690,18 +692,18 @@ int rtllib_wx_get_encode_ext(struct rtllib_device *ieee,
 
 	idx = encoding->flags & IW_ENCODE_INDEX;
 	if (idx) {
-		if (idx < 1 || idx > WEP_KEYS)
+		if (idx < 1 || idx > NUM_WEP_KEYS)
 			return -EINVAL;
 		idx--;
 	} else {
-		idx = ieee->tx_keyidx;
+		idx = ieee->crypt_info.tx_keyidx;
 	}
 	if (!(ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) &&
 	    (ext->alg != IW_ENCODE_ALG_WEP))
 		if (idx != 0 || (ieee->iw_mode != IW_MODE_INFRA))
 			return -EINVAL;
 
-	crypt = ieee->crypt[idx];
+	crypt = ieee->crypt_info.crypt[idx];
 
 	encoding->flags = idx + 1;
 	memset(ext, 0, sizeof(*ext));
@@ -711,11 +713,11 @@ int rtllib_wx_get_encode_ext(struct rtllib_device *ieee,
 		ext->key_len = 0;
 		encoding->flags |= IW_ENCODE_DISABLED;
 	} else {
-		if (strcmp(crypt->ops->name, "WEP") == 0)
+		if (strcmp(crypt->ops->name, "R-WEP") == 0)
 			ext->alg = IW_ENCODE_ALG_WEP;
-		else if (strcmp(crypt->ops->name, "TKIP"))
+		else if (strcmp(crypt->ops->name, "R-TKIP"))
 			ext->alg = IW_ENCODE_ALG_TKIP;
-		else if (strcmp(crypt->ops->name, "CCMP"))
+		else if (strcmp(crypt->ops->name, "R-CCMP"))
 			ext->alg = IW_ENCODE_ALG_CCMP;
 		else
 			return -EINVAL;
@@ -778,6 +780,7 @@ int rtllib_wx_set_mlme(struct rtllib_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_set_mlme);
 
 int rtllib_wx_set_auth(struct rtllib_device *ieee,
 			       struct iw_request_info *info,
@@ -830,6 +833,7 @@ int rtllib_wx_set_auth(struct rtllib_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_set_auth);
 
 int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len)
 {
@@ -846,10 +850,9 @@ int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len)
 
 			ieee->wps_ie_len = (len < MAX_WZC_IE_LEN) ? (len) :
 					   (MAX_WZC_IE_LEN);
-			buf = kmalloc(ieee->wps_ie_len, GFP_KERNEL);
+			buf = kmemdup(ie, ieee->wps_ie_len, GFP_KERNEL);
 			if (buf == NULL)
 				return -ENOMEM;
-			memcpy(buf, ie, ieee->wps_ie_len);
 			ieee->wps_ie = buf;
 			return 0;
 		}
@@ -860,10 +863,9 @@ int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len)
 	if (len) {
 		if (len != ie[1]+2)
 			return -EINVAL;
-		buf = kmalloc(len, GFP_KERNEL);
+		buf = kmemdup(ie, len, GFP_KERNEL);
 		if (buf == NULL)
 			return -ENOMEM;
-		memcpy(buf, ie, len);
 		kfree(ieee->wpa_ie);
 		ieee->wpa_ie = buf;
 		ieee->wpa_ie_len = len;
@@ -874,3 +876,4 @@ int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len)
 	}
 	return 0;
 }
+EXPORT_SYMBOL(rtllib_wx_set_gen_ie);
diff --git a/drivers/staging/rtl8192u/ieee80211/api.c b/drivers/staging/rtl8192u/ieee80211/api.c
deleted file mode 100644
index 5f46e50e586e..000000000000
--- a/drivers/staging/rtl8192u/ieee80211/api.c
+++ /dev/null
@@ -1,244 +0,0 @@
-/*
- * Scatterlist Cryptographic API.
- *
- * Copyright (c) 2002 James Morris <jmorris@intercode.com.au>
- * Copyright (c) 2002 David S. Miller (davem@redhat.com)
- *
- * Portions derived from Cryptoapi, by Alexander Kjeldaas <astor@fast.no>
- * and Nettle, by Niels M鰈ler.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the Free
- * Software Foundation; either version 2 of the License, or (at your option)
- * any later version.
- *
- */
-#include "kmap_types.h"
-
-#include <linux/init.h>
-#include <linux/module.h>
-//#include <linux/crypto.h>
-#include "rtl_crypto.h"
-#include <linux/errno.h>
-#include <linux/rwsem.h>
-#include <linux/slab.h>
-#include "internal.h"
-
-LIST_HEAD(crypto_alg_list);
-DECLARE_RWSEM(crypto_alg_sem);
-
-static inline int crypto_alg_get(struct crypto_alg *alg)
-{
-	return try_inc_mod_count(alg->cra_module);
-}
-
-static inline void crypto_alg_put(struct crypto_alg *alg)
-{
-	if (alg->cra_module)
-		__MOD_DEC_USE_COUNT(alg->cra_module);
-}
-
-struct crypto_alg *crypto_alg_lookup(const char *name)
-{
-	struct crypto_alg *q, *alg = NULL;
-
-	if (!name)
-		return NULL;
-
-	down_read(&crypto_alg_sem);
-
-	list_for_each_entry(q, &crypto_alg_list, cra_list) {
-		if (!(strcmp(q->cra_name, name))) {
-			if (crypto_alg_get(q))
-				alg = q;
-			break;
-		}
-	}
-
-	up_read(&crypto_alg_sem);
-	return alg;
-}
-
-static int crypto_init_flags(struct crypto_tfm *tfm, u32 flags)
-{
-	tfm->crt_flags = 0;
-
-	switch (crypto_tfm_alg_type(tfm)) {
-	case CRYPTO_ALG_TYPE_CIPHER:
-		return crypto_init_cipher_flags(tfm, flags);
-
-	case CRYPTO_ALG_TYPE_DIGEST:
-		return crypto_init_digest_flags(tfm, flags);
-
-	case CRYPTO_ALG_TYPE_COMPRESS:
-		return crypto_init_compress_flags(tfm, flags);
-
-	default:
-		break;
-	}
-
-	BUG();
-	return -EINVAL;
-}
-
-static int crypto_init_ops(struct crypto_tfm *tfm)
-{
-	switch (crypto_tfm_alg_type(tfm)) {
-	case CRYPTO_ALG_TYPE_CIPHER:
-		return crypto_init_cipher_ops(tfm);
-
-	case CRYPTO_ALG_TYPE_DIGEST:
-		return crypto_init_digest_ops(tfm);
-
-	case CRYPTO_ALG_TYPE_COMPRESS:
-		return crypto_init_compress_ops(tfm);
-
-	default:
-		break;
-	}
-
-	BUG();
-	return -EINVAL;
-}
-
-static void crypto_exit_ops(struct crypto_tfm *tfm)
-{
-	switch (crypto_tfm_alg_type(tfm)) {
-	case CRYPTO_ALG_TYPE_CIPHER:
-		crypto_exit_cipher_ops(tfm);
-		break;
-
-	case CRYPTO_ALG_TYPE_DIGEST:
-		crypto_exit_digest_ops(tfm);
-		break;
-
-	case CRYPTO_ALG_TYPE_COMPRESS:
-		crypto_exit_compress_ops(tfm);
-		break;
-
-	default:
-		BUG();
-
-	}
-}
-
-struct crypto_tfm *crypto_alloc_tfm(const char *name, u32 flags)
-{
-	struct crypto_tfm *tfm = NULL;
-	struct crypto_alg *alg;
-
-	alg = crypto_alg_mod_lookup(name);
-	if (alg == NULL)
-		goto out;
-
-	tfm = kzalloc(sizeof(*tfm) + alg->cra_ctxsize, GFP_KERNEL);
-	if (tfm == NULL)
-		goto out_put;
-
-	tfm->__crt_alg = alg;
-
-	if (crypto_init_flags(tfm, flags))
-		goto out_free_tfm;
-
-	if (crypto_init_ops(tfm)) {
-		crypto_exit_ops(tfm);
-		goto out_free_tfm;
-	}
-
-	goto out;
-
-out_free_tfm:
-	kfree(tfm);
-	tfm = NULL;
-out_put:
-	crypto_alg_put(alg);
-out:
-	return tfm;
-}
-
-void crypto_free_tfm(struct crypto_tfm *tfm)
-{
-	struct crypto_alg *alg = tfm->__crt_alg;
-	int size = sizeof(*tfm) + alg->cra_ctxsize;
-
-	crypto_exit_ops(tfm);
-	crypto_alg_put(alg);
-	memset(tfm, 0, size);
-	kfree(tfm);
-}
-
-int crypto_register_alg(struct crypto_alg *alg)
-{
-	int ret = 0;
-	struct crypto_alg *q;
-
-	down_write(&crypto_alg_sem);
-
-	list_for_each_entry(q, &crypto_alg_list, cra_list) {
-		if (!(strcmp(q->cra_name, alg->cra_name))) {
-			ret = -EEXIST;
-			goto out;
-		}
-	}
-
-	list_add_tail(&alg->cra_list, &crypto_alg_list);
-out:
-	up_write(&crypto_alg_sem);
-	return ret;
-}
-
-int crypto_unregister_alg(struct crypto_alg *alg)
-{
-	int ret = -ENOENT;
-	struct crypto_alg *q;
-
-	BUG_ON(!alg->cra_module);
-
-	down_write(&crypto_alg_sem);
-	list_for_each_entry(q, &crypto_alg_list, cra_list) {
-		if (alg == q) {
-			list_del(&alg->cra_list);
-			ret = 0;
-			goto out;
-		}
-	}
-out:
-	up_write(&crypto_alg_sem);
-	return ret;
-}
-
-int crypto_alg_available(const char *name, u32 flags)
-{
-	int ret = 0;
-	struct crypto_alg *alg = crypto_alg_mod_lookup(name);
-
-	if (alg) {
-		crypto_alg_put(alg);
-		ret = 1;
-	}
-
-	return ret;
-}
-
-static int __init init_crypto(void)
-{
-	printk(KERN_INFO "Initializing Cryptographic API\n");
-	crypto_init_proc();
-	return 0;
-}
-
-__initcall(init_crypto);
-
-/*
-EXPORT_SYMBOL_GPL(crypto_register_alg);
-EXPORT_SYMBOL_GPL(crypto_unregister_alg);
-EXPORT_SYMBOL_GPL(crypto_alloc_tfm);
-EXPORT_SYMBOL_GPL(crypto_free_tfm);
-EXPORT_SYMBOL_GPL(crypto_alg_available);
-*/
-
-EXPORT_SYMBOL_NOVERS(crypto_register_alg);
-EXPORT_SYMBOL_NOVERS(crypto_unregister_alg);
-EXPORT_SYMBOL_NOVERS(crypto_alloc_tfm);
-EXPORT_SYMBOL_NOVERS(crypto_free_tfm);
-EXPORT_SYMBOL_NOVERS(crypto_alg_available);
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index ef8eb6c7ee41..4277d0304b7a 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -551,7 +551,7 @@ void r8712_survey_event_callback(struct _adapter *adapter, u8 *pbuf)
 			ibss_wlan = r8712_find_network(
 						&pmlmepriv->scanned_queue,
 						pnetwork->MacAddress);
-			if (!ibss_wlan) {
+			if (ibss_wlan) {
 				memcpy(ibss_wlan->network.IEs,
 					pnetwork->IEs, 8);
 				goto exit;
diff --git a/drivers/staging/rts5139/rts51x.h b/drivers/staging/rts5139/rts51x.h
index 9415d5c05502..b2c58390bfc5 100644
--- a/drivers/staging/rts5139/rts51x.h
+++ b/drivers/staging/rts5139/rts51x.h
@@ -34,7 +34,6 @@
 #include <linux/mutex.h>
 #include <linux/cdrom.h>
 #include <linux/kernel.h>
-#include <linux/version.h>
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
diff --git a/drivers/staging/rts5139/rts51x_transport.h b/drivers/staging/rts5139/rts51x_transport.h
index f7aa87f7f1a9..8464c4836d5b 100644
--- a/drivers/staging/rts5139/rts51x_transport.h
+++ b/drivers/staging/rts5139/rts51x_transport.h
@@ -28,7 +28,6 @@
 #define __RTS51X_TRANSPORT_H
 
 #include <linux/kernel.h>
-#include <linux/version.h>
 
 #include "rts51x.h"
 #include "rts51x_chip.h"
diff --git a/drivers/staging/sep/sep_driver.c b/drivers/staging/sep/sep_driver.c
index f47571ea745d..6b3d156d4140 100644
--- a/drivers/staging/sep/sep_driver.c
+++ b/drivers/staging/sep/sep_driver.c
@@ -2120,6 +2120,8 @@ static int sep_prepare_input_output_dma_table_in_dcb(struct sep_device *sep,
 			}
 		}
 		if (tail_size) {
+			if (tail_size > sizeof(dcb_table_ptr->tail_data))
+				return -EINVAL;
 			if (is_kva == true) {
 				memcpy(dcb_table_ptr->tail_data,
 					(void *)(app_in_address + data_in_size -
diff --git a/drivers/staging/serial/68360serial.c b/drivers/staging/serial/68360serial.c
index 0a3e8787ed50..daf0b1d0dc28 100644
--- a/drivers/staging/serial/68360serial.c
+++ b/drivers/staging/serial/68360serial.c
@@ -2771,8 +2771,8 @@ static int __init rs_360_init(void)
 			*/
 			/* cpm_install_handler(IRQ_MACHSPEC | state->irq, rs_360_interrupt, info);  */
 			/*request_irq(IRQ_MACHSPEC | state->irq, rs_360_interrupt, */
-			request_irq(state->irq, rs_360_interrupt,
-						IRQ_FLG_LOCK, "ttyS", (void *)info);
+			request_irq(state->irq, rs_360_interrupt, 0, "ttyS",
+				    (void *)info);
 
 			/* Set up the baud rate generator.
 			*/
diff --git a/drivers/staging/sm7xx/smtcfb.c b/drivers/staging/sm7xx/smtcfb.c
index 39dbf339a4fc..ae0035f327e7 100644
--- a/drivers/staging/sm7xx/smtcfb.c
+++ b/drivers/staging/sm7xx/smtcfb.c
@@ -1024,9 +1024,9 @@ failed_free:
 
 /* Jason (08/11/2009) PCI_DRV wrapper essential structs */
 static DEFINE_PCI_DEVICE_TABLE(smtcfb_pci_table) = {
-	{0x126f, 0x710, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x126f, 0x712, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{0x126f, 0x720, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{ PCI_DEVICE(0x126f, 0x710), },
+	{ PCI_DEVICE(0x126f, 0x712), },
+	{ PCI_DEVICE(0x126f, 0x720), },
 	{0,}
 };
 
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index 07a7f5432597..2093896c546b 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -265,12 +265,11 @@ static ssize_t keymap_store(struct kobject *kobj, struct kobj_attribute *attr,
 	unsigned long flags;
 
 	spk_lock(flags);
-	in_buff = kmalloc(count + 1, GFP_ATOMIC);
+	in_buff = kmemdup(buf, count + 1, GFP_ATOMIC);
 	if (!in_buff) {
 		spk_unlock(flags);
 		return -ENOMEM;
 	}
-	memcpy(in_buff, buf, count + 1);
 	if (strchr("dDrR", *in_buff)) {
 		set_key_info(key_defaults, key_buf);
 		pr_info("keymap set to default values\n");
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index 8be560458977..c7b03f0ef2dd 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -2268,8 +2268,6 @@ static int __init speakup_init(void)
 		set_mask_bits(0, i, 2);
 
 	set_key_info(key_defaults, key_buf);
-	if (quiet_boot)
-		spk_shut_up |= 0x01;
 
 	/* From here on out, initializations can fail. */
 	err = speakup_add_virtual_keyboard();
@@ -2292,6 +2290,9 @@ static int __init speakup_init(void)
 				goto error_kobjects;
 		}
 
+	if (quiet_boot)
+		spk_shut_up |= 0x01;
+
 	err = speakup_kobj_init();
 	if (err)
 		goto error_kobjects;
diff --git a/drivers/staging/spectra/Kconfig b/drivers/staging/spectra/Kconfig
deleted file mode 100644
index 4fc206484830..000000000000
--- a/drivers/staging/spectra/Kconfig
+++ /dev/null
@@ -1,41 +0,0 @@
-
-menuconfig SPECTRA
-	tristate "Denali Spectra Flash Translation Layer"
-	depends on BLOCK
-	depends on X86_MRST
-	default n
-	---help---
-	  Enable the FTL pseudo-filesystem used with the NAND Flash
-	  controller on Intel Moorestown Platform to pretend to be a disk.
-
-choice
-	prompt "Compile for"
-	depends on SPECTRA
-	default SPECTRA_MRST_HW
-
-config SPECTRA_MRST_HW
-	bool "Moorestown hardware mode"
-	help
-	  Driver communicates with the Moorestown hardware's register interface.
-	  in DMA mode.
-
-config SPECTRA_MTD
-	bool "Linux MTD mode"
-	depends on MTD
-	help
-	  Driver communicates with the kernel MTD subsystem instead of its own
-	  built-in hardware driver.
-
-config SPECTRA_EMU
-	bool "RAM emulator testing"
-	help
-	  Driver emulates Flash on a RAM buffer and / or disk file.  Useful to test the behavior of FTL layer.
-
-endchoice
-
-config SPECTRA_MRST_HW_DMA
-       bool
-       default n
-       depends on SPECTRA_MRST_HW
-       help
-         Use DMA for native hardware interface.
diff --git a/drivers/staging/spectra/Makefile b/drivers/staging/spectra/Makefile
deleted file mode 100644
index f777dfba05a5..000000000000
--- a/drivers/staging/spectra/Makefile
+++ /dev/null
@@ -1,11 +0,0 @@
-#
-# Makefile of Intel Moorestown NAND controller driver
-#
-
-obj-$(CONFIG_SPECTRA) += spectra.o
-spectra-y := ffsport.o flash.o lld.o
-spectra-$(CONFIG_SPECTRA_MRST_HW) += lld_nand.o 
-spectra-$(CONFIG_SPECTRA_MRST_HW_DMA) += lld_cdma.o
-spectra-$(CONFIG_SPECTRA_EMU) += lld_emu.o
-spectra-$(CONFIG_SPECTRA_MTD) += lld_mtd.o
-
diff --git a/drivers/staging/spectra/README b/drivers/staging/spectra/README
deleted file mode 100644
index ecba559b899c..000000000000
--- a/drivers/staging/spectra/README
+++ /dev/null
@@ -1,29 +0,0 @@
-This is a driver for NAND controller of Intel Moorestown platform.
-
-This driver is a standalone linux block device driver, it acts as if it's a normal hard disk.
-It includes three layer:
-	block layer interface - file ffsport.c
-	Flash Translation Layer (FTL) - file flash.c (implement the NAND flash Translation Layer, includs address mapping, garbage collection, wear-leveling and so on)
-	Low level layer - file lld_nand.c/lld_cdma.c/lld_emu.c (which implements actual controller hardware registers access)
-
-This driver can be build as modules or build-in.
-
-Dependency:
-This driver has dependency on IA Firmware of Intel Moorestown platform.
-It need the IA Firmware to create the block table for the first time.
-And to validate this driver code without IA Firmware, you can change the
-macro AUTO_FORMAT_FLASH from 0 to 1 in file spectraswconfig.h. Thus the
-driver will erase the whole nand flash and create a new block table.
-
-TODO:
-	- Enable Command DMA feature support
-	- lower the memory footprint
-	- Remove most of the unnecessary global variables
-	- Change all the upcase variable / functions name to lowercase
-	- Some other misc bugs
-
-Please send patches to:
-	Greg Kroah-Hartman <gregkh@suse.de>
-
-And Cc to: Gao Yunpeng <yunpeng.gao@intel.com>
-
diff --git a/drivers/staging/spectra/ffsdefs.h b/drivers/staging/spectra/ffsdefs.h
deleted file mode 100644
index a9e9cd233d2a..000000000000
--- a/drivers/staging/spectra/ffsdefs.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#ifndef _FFSDEFS_
-#define _FFSDEFS_
-
-#define CLEAR 0			/*use this to clear a field instead of "fail"*/
-#define SET   1			/*use this to set a field instead of "pass"*/
-#define FAIL 1			/*failed flag*/
-#define PASS 0			/*success flag*/
-#define ERR -1			/*error flag*/
-
-#define   ERASE_CMD             10
-#define   WRITE_MAIN_CMD        11
-#define   READ_MAIN_CMD         12
-#define   WRITE_SPARE_CMD       13
-#define   READ_SPARE_CMD        14
-#define   WRITE_MAIN_SPARE_CMD  15
-#define   READ_MAIN_SPARE_CMD   16
-#define   MEMCOPY_CMD           17
-#define   DUMMY_CMD             99
-
-#define     EVENT_PASS                                  0x00
-#define     EVENT_CORRECTABLE_DATA_ERROR_FIXED         0x01
-#define     EVENT_UNCORRECTABLE_DATA_ERROR              0x02
-#define     EVENT_TIME_OUT                              0x03
-#define     EVENT_PROGRAM_FAILURE                       0x04
-#define     EVENT_ERASE_FAILURE                         0x05
-#define     EVENT_MEMCOPY_FAILURE                       0x06
-#define     EVENT_FAIL                                  0x07
-
-#define     EVENT_NONE                                  0x22
-#define     EVENT_DMA_CMD_COMP                          0x77
-#define     EVENT_ECC_TRANSACTION_DONE                  0x88
-#define     EVENT_DMA_CMD_FAIL                          0x99
-
-#define CMD_PASS        0
-#define CMD_FAIL        1
-#define CMD_ABORT       2
-#define CMD_NOT_DONE    3
-
-#endif /* _FFSDEFS_ */
diff --git a/drivers/staging/spectra/ffsport.c b/drivers/staging/spectra/ffsport.c
deleted file mode 100644
index 86d556d6cf98..000000000000
--- a/drivers/staging/spectra/ffsport.c
+++ /dev/null
@@ -1,834 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#include "ffsport.h"
-#include "flash.h"
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/blkdev.h>
-#include <linux/wait.h>
-#include <linux/mutex.h>
-#include <linux/kthread.h>
-#include <linux/log2.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/async.h>
-
-/**** Helper functions used for Div, Remainder operation on u64 ****/
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_Calc_Used_Bits
-* Inputs:       Power of 2 number
-* Outputs:      Number of Used Bits
-*               0, if the argument is 0
-* Description:  Calculate the number of bits used by a given power of 2 number
-*               Number can be up to 32 bit
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_Calc_Used_Bits(u32 n)
-{
-	int tot_bits = 0;
-
-	if (n >= 1 << 16) {
-		n >>= 16;
-		tot_bits += 16;
-	}
-
-	if (n >= 1 << 8) {
-		n >>=  8;
-		tot_bits +=  8;
-	}
-
-	if (n >= 1 << 4) {
-		n >>=  4;
-		tot_bits +=  4;
-	}
-
-	if (n >= 1 << 2) {
-		n >>=  2;
-		tot_bits +=  2;
-	}
-
-	if (n >= 1 << 1)
-		tot_bits +=  1;
-
-	return ((n == 0) ? (0) : tot_bits);
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_u64_Div
-* Inputs:       Number of u64
-*               A power of 2 number as Division
-* Outputs:      Quotient of the Divisor operation
-* Description:  It divides the address by divisor by using bit shift operation
-*               (essentially without explicitely using "/").
-*               Divisor is a power of 2 number and Divided is of u64
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u64 GLOB_u64_Div(u64 addr, u32 divisor)
-{
-	return  (u64)(addr >> GLOB_Calc_Used_Bits(divisor));
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_u64_Remainder
-* Inputs:       Number of u64
-*               Divisor Type (1 -PageAddress, 2- BlockAddress)
-* Outputs:      Remainder of the Division operation
-* Description:  It calculates the remainder of a number (of u64) by
-*               divisor(power of 2 number ) by using bit shifting and multiply
-*               operation(essentially without explicitely using "/").
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u64 GLOB_u64_Remainder(u64 addr, u32 divisor_type)
-{
-	u64 result = 0;
-
-	if (divisor_type == 1) { /* Remainder -- Page */
-		result = (addr >> DeviceInfo.nBitsInPageDataSize);
-		result = result * DeviceInfo.wPageDataSize;
-	} else if (divisor_type == 2) { /* Remainder -- Block */
-		result = (addr >> DeviceInfo.nBitsInBlockDataSize);
-		result = result * DeviceInfo.wBlockDataSize;
-	}
-
-	result = addr - result;
-
-	return result;
-}
-
-#define NUM_DEVICES             1
-#define PARTITIONS              8
-
-#define GLOB_SBD_NAME          "nd"
-#define GLOB_SBD_IRQ_NUM       (29)
-
-#define GLOB_SBD_IOCTL_GC                        (0x7701)
-#define GLOB_SBD_IOCTL_WL                        (0x7702)
-#define GLOB_SBD_IOCTL_FORMAT                    (0x7703)
-#define GLOB_SBD_IOCTL_ERASE_FLASH               (0x7704)
-#define GLOB_SBD_IOCTL_FLUSH_CACHE               (0x7705)
-#define GLOB_SBD_IOCTL_COPY_BLK_TABLE            (0x7706)
-#define GLOB_SBD_IOCTL_COPY_WEAR_LEVELING_TABLE  (0x7707)
-#define GLOB_SBD_IOCTL_GET_NAND_INFO             (0x7708)
-#define GLOB_SBD_IOCTL_WRITE_DATA                (0x7709)
-#define GLOB_SBD_IOCTL_READ_DATA                 (0x770A)
-
-static int reserved_mb = 0;
-module_param(reserved_mb, int, 0);
-MODULE_PARM_DESC(reserved_mb, "Reserved space for OS image, in MiB (default 25 MiB)");
-
-int nand_debug_level;
-module_param(nand_debug_level, int, 0644);
-MODULE_PARM_DESC(nand_debug_level, "debug level value: 1-3");
-
-MODULE_LICENSE("GPL");
-
-struct spectra_nand_dev {
-	struct pci_dev *dev;
-	u64 size;
-	u16 users;
-	spinlock_t qlock;
-	void __iomem *ioaddr;  /* Mapped address */
-	struct request_queue *queue;
-	struct task_struct *thread;
-	struct gendisk *gd;
-	u8 *tmp_buf;
-};
-
-
-static int GLOB_SBD_majornum;
-
-static char *GLOB_version = GLOB_VERSION;
-
-static struct spectra_nand_dev nand_device[NUM_DEVICES];
-
-static struct mutex spectra_lock;
-
-static int res_blks_os = 1;
-
-struct spectra_indentfy_dev_tag IdentifyDeviceData;
-
-static int force_flush_cache(void)
-{
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	if (ERR == GLOB_FTL_Flush_Cache()) {
-		printk(KERN_ERR "Fail to Flush FTL Cache!\n");
-		return -EFAULT;
-	}
-#if CMD_DMA
-		if (glob_ftl_execute_cmds())
-			return -EIO;
-		else
-			return 0;
-#endif
-	return 0;
-}
-
-struct ioctl_rw_page_info {
-	u8 *data;
-	unsigned int page;
-};
-
-static int ioctl_read_page_data(unsigned long arg)
-{
-	u8 *buf;
-	struct ioctl_rw_page_info info;
-	int result = PASS;
-
-	if (copy_from_user(&info, (void __user *)arg, sizeof(info)))
-		return -EFAULT;
-
-	buf = kmalloc(IdentifyDeviceData.PageDataSize, GFP_ATOMIC);
-	if (!buf) {
-		printk(KERN_ERR "ioctl_read_page_data: "
-		       "failed to allocate memory\n");
-		return -ENOMEM;
-	}
-
-	mutex_lock(&spectra_lock);
-	result = GLOB_FTL_Page_Read(buf,
-		(u64)info.page * IdentifyDeviceData.PageDataSize);
-	mutex_unlock(&spectra_lock);
-
-	if (copy_to_user((void __user *)info.data, buf,
-			   IdentifyDeviceData.PageDataSize)) {
-		printk(KERN_ERR "ioctl_read_page_data: "
-		       "failed to copy user data\n");
-		kfree(buf);
-		return -EFAULT;
-	}
-
-	kfree(buf);
-	return result;
-}
-
-static int ioctl_write_page_data(unsigned long arg)
-{
-	u8 *buf;
-	struct ioctl_rw_page_info info;
-	int result = PASS;
-
-	if (copy_from_user(&info, (void __user *)arg, sizeof(info)))
-		return -EFAULT;
-
-	buf = memdup_user((void __user *)info.data,
-			  IdentifyDeviceData.PageDataSize);
-	if (IS_ERR(buf)) {
-		printk(KERN_ERR "ioctl_write_page_data: "
-		       "failed to copy user data\n");
-		return PTR_ERR(buf);
-	}
-
-	mutex_lock(&spectra_lock);
-	result = GLOB_FTL_Page_Write(buf,
-		(u64)info.page * IdentifyDeviceData.PageDataSize);
-	mutex_unlock(&spectra_lock);
-
-	kfree(buf);
-	return result;
-}
-
-/* Return how many blocks should be reserved for bad block replacement */
-static int get_res_blk_num_bad_blk(void)
-{
-	return IdentifyDeviceData.wDataBlockNum / 10;
-}
-
-/* Return how many blocks should be reserved for OS image */
-static int get_res_blk_num_os(void)
-{
-	u32 res_blks, blk_size;
-
-	blk_size = IdentifyDeviceData.PageDataSize *
-		IdentifyDeviceData.PagesPerBlock;
-
-	res_blks = (reserved_mb * 1024 * 1024) / blk_size;
-
-	if ((res_blks < 1) || (res_blks >= IdentifyDeviceData.wDataBlockNum))
-		res_blks = 1; /* Reserved 1 block for block table */
-
-	return res_blks;
-}
-
-/* Transfer a full request. */
-static int do_transfer(struct spectra_nand_dev *tr, struct request *req)
-{
-	u64 start_addr, addr;
-	u32 logical_start_sect, hd_start_sect;
-	u32 nsect, hd_sects;
-	u32 rsect, tsect = 0;
-	char *buf;
-	u32 ratio = IdentifyDeviceData.PageDataSize >> 9;
-
-	start_addr = (u64)(blk_rq_pos(req)) << 9;
-	/* Add a big enough offset to prevent the OS Image from
-	*  being accessed or damaged by file system */
-	start_addr += IdentifyDeviceData.PageDataSize *
-			IdentifyDeviceData.PagesPerBlock *
-			res_blks_os;
-
-	if (req->cmd_type & REQ_FLUSH) {
-		if (force_flush_cache()) /* Fail to flush cache */
-			return -EIO;
-		else
-			return 0;
-	}
-
-	if (req->cmd_type != REQ_TYPE_FS)
-		return -EIO;
-
-	if (blk_rq_pos(req) + blk_rq_cur_sectors(req) > get_capacity(tr->gd)) {
-		printk(KERN_ERR "Spectra error: request over the NAND "
-			"capacity!sector %d, current_nr_sectors %d, "
-			"while capacity is %d\n",
-			(int)blk_rq_pos(req),
-			blk_rq_cur_sectors(req),
-			(int)get_capacity(tr->gd));
-		return -EIO;
-	}
-
-	logical_start_sect = start_addr >> 9;
-	hd_start_sect = logical_start_sect / ratio;
-	rsect = logical_start_sect - hd_start_sect * ratio;
-
-	addr = (u64)hd_start_sect * ratio * 512;
-	buf = req->buffer;
-	nsect = blk_rq_cur_sectors(req);
-
-	if (rsect)
-		tsect =  (ratio - rsect) < nsect ? (ratio - rsect) : nsect;
-
-	switch (rq_data_dir(req)) {
-	case READ:
-		/* Read the first NAND page */
-		if (rsect) {
-			if (GLOB_FTL_Page_Read(tr->tmp_buf, addr)) {
-				printk(KERN_ERR "Error in %s, Line %d\n",
-					__FILE__, __LINE__);
-				return -EIO;
-			}
-			memcpy(buf, tr->tmp_buf + (rsect << 9), tsect << 9);
-			addr += IdentifyDeviceData.PageDataSize;
-			buf += tsect << 9;
-			nsect -= tsect;
-		}
-
-		/* Read the other NAND pages */
-		for (hd_sects = nsect / ratio; hd_sects > 0; hd_sects--) {
-			if (GLOB_FTL_Page_Read(buf, addr)) {
-				printk(KERN_ERR "Error in %s, Line %d\n",
-					__FILE__, __LINE__);
-				return -EIO;
-			}
-			addr += IdentifyDeviceData.PageDataSize;
-			buf += IdentifyDeviceData.PageDataSize;
-		}
-
-		/* Read the last NAND pages */
-		if (nsect % ratio) {
-			if (GLOB_FTL_Page_Read(tr->tmp_buf, addr)) {
-				printk(KERN_ERR "Error in %s, Line %d\n",
-					__FILE__, __LINE__);
-				return -EIO;
-			}
-			memcpy(buf, tr->tmp_buf, (nsect % ratio) << 9);
-		}
-#if CMD_DMA
-		if (glob_ftl_execute_cmds())
-			return -EIO;
-		else
-			return 0;
-#endif
-		return 0;
-
-	case WRITE:
-		/* Write the first NAND page */
-		if (rsect) {
-			if (GLOB_FTL_Page_Read(tr->tmp_buf, addr)) {
-				printk(KERN_ERR "Error in %s, Line %d\n",
-					__FILE__, __LINE__);
-				return -EIO;
-			}
-			memcpy(tr->tmp_buf + (rsect << 9), buf, tsect << 9);
-			if (GLOB_FTL_Page_Write(tr->tmp_buf, addr)) {
-				printk(KERN_ERR "Error in %s, Line %d\n",
-					__FILE__, __LINE__);
-				return -EIO;
-			}
-			addr += IdentifyDeviceData.PageDataSize;
-			buf += tsect << 9;
-			nsect -= tsect;
-		}
-
-		/* Write the other NAND pages */
-		for (hd_sects = nsect / ratio; hd_sects > 0; hd_sects--) {
-			if (GLOB_FTL_Page_Write(buf, addr)) {
-				printk(KERN_ERR "Error in %s, Line %d\n",
-					__FILE__, __LINE__);
-				return -EIO;
-			}
-			addr += IdentifyDeviceData.PageDataSize;
-			buf += IdentifyDeviceData.PageDataSize;
-		}
-
-		/* Write the last NAND pages */
-		if (nsect % ratio) {
-			if (GLOB_FTL_Page_Read(tr->tmp_buf, addr)) {
-				printk(KERN_ERR "Error in %s, Line %d\n",
-					__FILE__, __LINE__);
-				return -EIO;
-			}
-			memcpy(tr->tmp_buf, buf, (nsect % ratio) << 9);
-			if (GLOB_FTL_Page_Write(tr->tmp_buf, addr)) {
-				printk(KERN_ERR "Error in %s, Line %d\n",
-					__FILE__, __LINE__);
-				return -EIO;
-			}
-		}
-#if CMD_DMA
-		if (glob_ftl_execute_cmds())
-			return -EIO;
-		else
-			return 0;
-#endif
-		return 0;
-
-	default:
-		printk(KERN_NOTICE "Unknown request %u\n", rq_data_dir(req));
-		return -EIO;
-	}
-}
-
-/* This function is copied from drivers/mtd/mtd_blkdevs.c */
-static int spectra_trans_thread(void *arg)
-{
-	struct spectra_nand_dev *tr = arg;
-	struct request_queue *rq = tr->queue;
-	struct request *req = NULL;
-
-	/* we might get involved when memory gets low, so use PF_MEMALLOC */
-	current->flags |= PF_MEMALLOC;
-
-	spin_lock_irq(rq->queue_lock);
-	while (!kthread_should_stop()) {
-		int res;
-
-		if (!req) {
-			req = blk_fetch_request(rq);
-			if (!req) {
-				set_current_state(TASK_INTERRUPTIBLE);
-				spin_unlock_irq(rq->queue_lock);
-				schedule();
-				spin_lock_irq(rq->queue_lock);
-				continue;
-			}
-		}
-
-		spin_unlock_irq(rq->queue_lock);
-
-		mutex_lock(&spectra_lock);
-		res = do_transfer(tr, req);
-		mutex_unlock(&spectra_lock);
-
-		spin_lock_irq(rq->queue_lock);
-
-		if (!__blk_end_request_cur(req, res))
-			req = NULL;
-	}
-
-	if (req)
-		__blk_end_request_all(req, -EIO);
-
-	spin_unlock_irq(rq->queue_lock);
-
-	return 0;
-}
-
-
-/* Request function that "handles clustering". */
-static void GLOB_SBD_request(struct request_queue *rq)
-{
-	struct spectra_nand_dev *pdev = rq->queuedata;
-	wake_up_process(pdev->thread);
-}
-
-static int GLOB_SBD_open(struct block_device *bdev, fmode_t mode)
-
-{
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-	return 0;
-}
-
-static int GLOB_SBD_release(struct gendisk *disk, fmode_t mode)
-{
-	int ret;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	mutex_lock(&spectra_lock);
-	ret = force_flush_cache();
-	mutex_unlock(&spectra_lock);
-
-	return 0;
-}
-
-static int GLOB_SBD_getgeo(struct block_device *bdev, struct hd_geometry *geo)
-{
-	geo->heads = 4;
-	geo->sectors = 16;
-	geo->cylinders = get_capacity(bdev->bd_disk) / (4 * 16);
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		"heads: %d, sectors: %d, cylinders: %d\n",
-		geo->heads, geo->sectors, geo->cylinders);
-
-	return 0;
-}
-
-int GLOB_SBD_ioctl(struct block_device *bdev, fmode_t mode,
-		unsigned int cmd, unsigned long arg)
-{
-	int ret;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	switch (cmd) {
-	case GLOB_SBD_IOCTL_GC:
-		nand_dbg_print(NAND_DBG_DEBUG,
-			       "Spectra IOCTL: Garbage Collection "
-			       "being performed\n");
-		if (PASS != GLOB_FTL_Garbage_Collection())
-			return -EFAULT;
-		return 0;
-
-	case GLOB_SBD_IOCTL_WL:
-		nand_dbg_print(NAND_DBG_DEBUG,
-			       "Spectra IOCTL: Static Wear Leveling "
-			       "being performed\n");
-		if (PASS != GLOB_FTL_Wear_Leveling())
-			return -EFAULT;
-		return 0;
-
-	case GLOB_SBD_IOCTL_FORMAT:
-		nand_dbg_print(NAND_DBG_DEBUG, "Spectra IOCTL: Flash format "
-			       "being performed\n");
-		if (PASS != GLOB_FTL_Flash_Format())
-			return -EFAULT;
-		return 0;
-
-	case GLOB_SBD_IOCTL_FLUSH_CACHE:
-		nand_dbg_print(NAND_DBG_DEBUG, "Spectra IOCTL: Cache flush "
-			       "being performed\n");
-		mutex_lock(&spectra_lock);
-		ret = force_flush_cache();
-		mutex_unlock(&spectra_lock);
-		return ret;
-
-	case GLOB_SBD_IOCTL_COPY_BLK_TABLE:
-		nand_dbg_print(NAND_DBG_DEBUG, "Spectra IOCTL: "
-			       "Copy block table\n");
-		if (copy_to_user((void __user *)arg,
-			get_blk_table_start_addr(),
-			get_blk_table_len()))
-			return -EFAULT;
-		return 0;
-
-	case GLOB_SBD_IOCTL_COPY_WEAR_LEVELING_TABLE:
-		nand_dbg_print(NAND_DBG_DEBUG, "Spectra IOCTL: "
-			       "Copy wear leveling table\n");
-		if (copy_to_user((void __user *)arg,
-			get_wear_leveling_table_start_addr(),
-			get_wear_leveling_table_len()))
-			return -EFAULT;
-		return 0;
-
-	case GLOB_SBD_IOCTL_GET_NAND_INFO:
-		nand_dbg_print(NAND_DBG_DEBUG, "Spectra IOCTL: "
-			       "Get NAND info\n");
-		if (copy_to_user((void __user *)arg, &IdentifyDeviceData,
-			sizeof(IdentifyDeviceData)))
-			return -EFAULT;
-		return 0;
-
-	case GLOB_SBD_IOCTL_WRITE_DATA:
-		nand_dbg_print(NAND_DBG_DEBUG, "Spectra IOCTL: "
-			       "Write one page data\n");
-		return ioctl_write_page_data(arg);
-
-	case GLOB_SBD_IOCTL_READ_DATA:
-		nand_dbg_print(NAND_DBG_DEBUG, "Spectra IOCTL: "
-			       "Read one page data\n");
-		return ioctl_read_page_data(arg);
-	}
-
-	return -ENOTTY;
-}
-
-static DEFINE_MUTEX(ffsport_mutex);
-
-int GLOB_SBD_unlocked_ioctl(struct block_device *bdev, fmode_t mode,
-		unsigned int cmd, unsigned long arg)
-{
-	int ret;
-
-	mutex_lock(&ffsport_mutex);
-	ret = GLOB_SBD_ioctl(bdev, mode, cmd, arg);
-	mutex_unlock(&ffsport_mutex);
-
-	return ret;
-}
-
-static struct block_device_operations GLOB_SBD_ops = {
-	.owner = THIS_MODULE,
-	.open = GLOB_SBD_open,
-	.release = GLOB_SBD_release,
-	.ioctl = GLOB_SBD_unlocked_ioctl,
-	.getgeo = GLOB_SBD_getgeo,
-};
-
-static int SBD_setup_device(struct spectra_nand_dev *dev, int which)
-{
-	int res_blks;
-	u32 sects;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	memset(dev, 0, sizeof(struct spectra_nand_dev));
-
-	nand_dbg_print(NAND_DBG_WARN, "Reserved %d blocks "
-		"for OS image, %d blocks for bad block replacement.\n",
-		get_res_blk_num_os(),
-		get_res_blk_num_bad_blk());
-
-	res_blks = get_res_blk_num_bad_blk() + get_res_blk_num_os();
-
-	dev->size = (u64)IdentifyDeviceData.PageDataSize *
-		IdentifyDeviceData.PagesPerBlock *
-		(IdentifyDeviceData.wDataBlockNum - res_blks);
-
-	res_blks_os = get_res_blk_num_os();
-
-	spin_lock_init(&dev->qlock);
-
-	dev->tmp_buf = kmalloc(IdentifyDeviceData.PageDataSize, GFP_ATOMIC);
-	if (!dev->tmp_buf) {
-		printk(KERN_ERR "Failed to kmalloc memory in %s Line %d, exit.\n",
-			__FILE__, __LINE__);
-		goto out_vfree;
-	}
-
-	dev->queue = blk_init_queue(GLOB_SBD_request, &dev->qlock);
-	if (dev->queue == NULL) {
-		printk(KERN_ERR
-		       "Spectra: Request queue could not be initialized."
-			" Aborting\n ");
-		goto out_vfree;
-	}
-	dev->queue->queuedata = dev;
-
-	/* As Linux block layer doesn't support >4KB hardware sector,  */
-	/* Here we force report 512 byte hardware sector size to Kernel */
-	blk_queue_logical_block_size(dev->queue, 512);
-
-	blk_queue_flush(dev->queue, REQ_FLUSH);
-
-	dev->thread = kthread_run(spectra_trans_thread, dev, "nand_thd");
-	if (IS_ERR(dev->thread)) {
-		blk_cleanup_queue(dev->queue);
-		unregister_blkdev(GLOB_SBD_majornum, GLOB_SBD_NAME);
-		return PTR_ERR(dev->thread);
-	}
-
-	dev->gd = alloc_disk(PARTITIONS);
-	if (!dev->gd) {
-		printk(KERN_ERR
-		       "Spectra: Could not allocate disk. Aborting \n ");
-		goto out_vfree;
-	}
-	dev->gd->major = GLOB_SBD_majornum;
-	dev->gd->first_minor = which * PARTITIONS;
-	dev->gd->fops = &GLOB_SBD_ops;
-	dev->gd->queue = dev->queue;
-	dev->gd->private_data = dev;
-	snprintf(dev->gd->disk_name, 32, "%s%c", GLOB_SBD_NAME, which + 'a');
-
-	sects = dev->size >> 9;
-	nand_dbg_print(NAND_DBG_WARN, "Capacity sects: %d\n", sects);
-	set_capacity(dev->gd, sects);
-
-	add_disk(dev->gd);
-
-	return 0;
-out_vfree:
-	return -ENOMEM;
-}
-
-/*
-static ssize_t show_nand_block_num(struct device *dev,
-	struct device_attribute *attr, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%d\n",
-		(int)IdentifyDeviceData.wDataBlockNum);
-}
-
-static ssize_t show_nand_pages_per_block(struct device *dev,
-	struct device_attribute *attr, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%d\n",
-		(int)IdentifyDeviceData.PagesPerBlock);
-}
-
-static ssize_t show_nand_page_size(struct device *dev,
-	struct device_attribute *attr, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%d\n",
-		(int)IdentifyDeviceData.PageDataSize);
-}
-
-static DEVICE_ATTR(nand_block_num, 0444, show_nand_block_num, NULL);
-static DEVICE_ATTR(nand_pages_per_block, 0444, show_nand_pages_per_block, NULL);
-static DEVICE_ATTR(nand_page_size, 0444, show_nand_page_size, NULL);
-
-static void create_sysfs_entry(struct device *dev)
-{
-	if (device_create_file(dev, &dev_attr_nand_block_num))
-		printk(KERN_ERR "Spectra: "
-			"failed to create sysfs entry nand_block_num.\n");
-	if (device_create_file(dev, &dev_attr_nand_pages_per_block))
-		printk(KERN_ERR "Spectra: "
-		"failed to create sysfs entry nand_pages_per_block.\n");
-	if (device_create_file(dev, &dev_attr_nand_page_size))
-		printk(KERN_ERR "Spectra: "
-		"failed to create sysfs entry nand_page_size.\n");
-}
-*/
-
-static void register_spectra_ftl_async(void *unused, async_cookie_t cookie)
-{
-	int i;
-
-	/* create_sysfs_entry(&dev->dev); */
-
-	if (PASS != GLOB_FTL_IdentifyDevice(&IdentifyDeviceData)) {
-		printk(KERN_ERR "Spectra: Unable to Read Flash Device. "
-		       "Aborting\n");
-		return;
-	} else {
-		nand_dbg_print(NAND_DBG_WARN, "In GLOB_SBD_init: "
-			       "Num blocks=%d, pagesperblock=%d, "
-			       "pagedatasize=%d, ECCBytesPerSector=%d\n",
-		       (int)IdentifyDeviceData.NumBlocks,
-		       (int)IdentifyDeviceData.PagesPerBlock,
-		       (int)IdentifyDeviceData.PageDataSize,
-		       (int)IdentifyDeviceData.wECCBytesPerSector);
-	}
-
-	printk(KERN_ALERT "Spectra: searching block table, please wait ...\n");
-	if (GLOB_FTL_Init() != PASS) {
-		printk(KERN_ERR "Spectra: Unable to Initialize FTL Layer. "
-		       "Aborting\n");
-		goto out_ftl_flash_register;
-	}
-	printk(KERN_ALERT "Spectra: block table has been found.\n");
-
-	GLOB_SBD_majornum = register_blkdev(0, GLOB_SBD_NAME);
-	if (GLOB_SBD_majornum <= 0) {
-		printk(KERN_ERR "Unable to get the major %d for Spectra",
-		       GLOB_SBD_majornum);
-		goto out_ftl_flash_register;
-	}
-
-	for (i = 0; i < NUM_DEVICES; i++)
-		if (SBD_setup_device(&nand_device[i], i) == -ENOMEM)
-			goto out_blk_register;
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		       "Spectra: module loaded with major number %d\n",
-		       GLOB_SBD_majornum);
-
-	return;
-
-out_blk_register:
-	unregister_blkdev(GLOB_SBD_majornum, GLOB_SBD_NAME);
-out_ftl_flash_register:
-	GLOB_FTL_Cache_Release();
-	printk(KERN_ERR "Spectra: Module load failed.\n");
-}
-
-int register_spectra_ftl()
-{
-	async_schedule(register_spectra_ftl_async, NULL);
-	return 0;
-}
-EXPORT_SYMBOL_GPL(register_spectra_ftl);
-
-static int GLOB_SBD_init(void)
-{
-	/* Set debug output level (0~3) here. 3 is most verbose */
-	printk(KERN_ALERT "Spectra: %s\n", GLOB_version);
-
-	mutex_init(&spectra_lock);
-
-	if (PASS != GLOB_FTL_Flash_Init()) {
-		printk(KERN_ERR "Spectra: Unable to Initialize Flash Device. "
-		       "Aborting\n");
-		return -ENODEV;
-	}
-	return 0;
-}
-
-static void __exit GLOB_SBD_exit(void)
-{
-	int i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	for (i = 0; i < NUM_DEVICES; i++) {
-		struct spectra_nand_dev *dev = &nand_device[i];
-		if (dev->gd) {
-			del_gendisk(dev->gd);
-			put_disk(dev->gd);
-		}
-		if (dev->queue)
-			blk_cleanup_queue(dev->queue);
-		kfree(dev->tmp_buf);
-	}
-
-	unregister_blkdev(GLOB_SBD_majornum, GLOB_SBD_NAME);
-
-	mutex_lock(&spectra_lock);
-	force_flush_cache();
-	mutex_unlock(&spectra_lock);
-
-	GLOB_FTL_Cache_Release();
-
-	GLOB_FTL_Flash_Release();
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		       "Spectra FTL module (major number %d) unloaded.\n",
-		       GLOB_SBD_majornum);
-}
-
-module_init(GLOB_SBD_init);
-module_exit(GLOB_SBD_exit);
diff --git a/drivers/staging/spectra/ffsport.h b/drivers/staging/spectra/ffsport.h
deleted file mode 100644
index 85c0750612f6..000000000000
--- a/drivers/staging/spectra/ffsport.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#ifndef _FFSPORT_
-#define _FFSPORT_
-
-#include "ffsdefs.h"
-
-#if defined __GNUC__
-#define PACKED
-#define PACKED_GNU __attribute__ ((packed))
-#define UNALIGNED
-#endif
-
-#include <linux/semaphore.h>
-#include <linux/string.h>	/* for strcpy(), stricmp(), etc */
-#include <linux/mm.h>		/* for kmalloc(), kfree() */
-#include <linux/vmalloc.h>
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/init.h>
-
-#include <linux/kernel.h>	/* printk() */
-#include <linux/fs.h>		/* everything... */
-#include <linux/errno.h>	/* error codes */
-#include <linux/types.h>	/* size_t */
-#include <linux/genhd.h>
-#include <linux/blkdev.h>
-#include <linux/hdreg.h>
-#include <linux/pci.h>
-#include "flash.h"
-
-#define VERBOSE    1
-
-#define NAND_DBG_WARN  1
-#define NAND_DBG_DEBUG 2
-#define NAND_DBG_TRACE 3
-
-extern int nand_debug_level;
-
-#ifdef VERBOSE
-#define nand_dbg_print(level, args...)			\
-	do {						\
-		if (level <= nand_debug_level)		\
-			printk(KERN_ALERT args);	\
-	} while (0)
-#else
-#define nand_dbg_print(level, args...)
-#endif
-
-#ifdef SUPPORT_BIG_ENDIAN
-#define INVERTUINT16(w)   ((u16)(((u16)(w)) << 8) | \
-			   (u16)((u16)(w) >> 8))
-
-#define INVERTUINT32(dw)  (((u32)(dw) << 24) | \
-			   (((u32)(dw) << 8) & 0x00ff0000) | \
-			   (((u32)(dw) >> 8) & 0x0000ff00) | \
-			   ((u32)(dw) >> 24))
-#else
-#define INVERTUINT16(w)   w
-#define INVERTUINT32(dw)  dw
-#endif
-
-extern int GLOB_Calc_Used_Bits(u32 n);
-extern u64 GLOB_u64_Div(u64 addr, u32 divisor);
-extern u64 GLOB_u64_Remainder(u64 addr, u32 divisor_type);
-extern int register_spectra_ftl(void);
-
-#endif /* _FFSPORT_ */
diff --git a/drivers/staging/spectra/flash.c b/drivers/staging/spectra/flash.c
deleted file mode 100644
index aead358e5c2a..000000000000
--- a/drivers/staging/spectra/flash.c
+++ /dev/null
@@ -1,4305 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#include <linux/fs.h>
-#include <linux/slab.h>
-
-#include "flash.h"
-#include "ffsdefs.h"
-#include "lld.h"
-#include "lld_nand.h"
-#if CMD_DMA
-#include "lld_cdma.h"
-#endif
-
-#define BLK_FROM_ADDR(addr)  ((u32)(addr >> DeviceInfo.nBitsInBlockDataSize))
-#define PAGE_FROM_ADDR(addr, Block)  ((u16)((addr - (u64)Block * \
-	DeviceInfo.wBlockDataSize) >> DeviceInfo.nBitsInPageDataSize))
-
-#define IS_SPARE_BLOCK(blk)     (BAD_BLOCK != (pbt[blk] &\
-	BAD_BLOCK) && SPARE_BLOCK == (pbt[blk] & SPARE_BLOCK))
-
-#define IS_DATA_BLOCK(blk)      (0 == (pbt[blk] & BAD_BLOCK))
-
-#define IS_DISCARDED_BLOCK(blk) (BAD_BLOCK != (pbt[blk] &\
-	BAD_BLOCK) && DISCARD_BLOCK == (pbt[blk] & DISCARD_BLOCK))
-
-#define IS_BAD_BLOCK(blk)       (BAD_BLOCK == (pbt[blk] & BAD_BLOCK))
-
-#if DEBUG_BNDRY
-void debug_boundary_lineno_error(int chnl, int limit, int no,
-				int lineno, char *filename)
-{
-	if (chnl >= limit)
-		printk(KERN_ERR "Boundary Check Fail value %d >= limit %d, "
-		"at  %s:%d. Other info:%d. Aborting...\n",
-		chnl, limit, filename, lineno, no);
-}
-/* static int globalmemsize; */
-#endif
-
-static u16 FTL_Cache_If_Hit(u64 dwPageAddr);
-static int FTL_Cache_Read(u64 dwPageAddr);
-static void FTL_Cache_Read_Page(u8 *pData, u64 dwPageAddr,
-				u16 cache_blk);
-static void FTL_Cache_Write_Page(u8 *pData, u64 dwPageAddr,
-				 u8 cache_blk, u16 flag);
-static int FTL_Cache_Write(void);
-static void FTL_Calculate_LRU(void);
-static u32 FTL_Get_Block_Index(u32 wBlockNum);
-
-static int FTL_Search_Block_Table_IN_Block(u32 BT_Block,
-					   u8 BT_Tag, u16 *Page);
-static int FTL_Read_Block_Table(void);
-static int FTL_Write_Block_Table(int wForce);
-static int FTL_Write_Block_Table_Data(void);
-static int FTL_Check_Block_Table(int wOldTable);
-static int FTL_Static_Wear_Leveling(void);
-static u32 FTL_Replace_Block_Table(void);
-static int FTL_Write_IN_Progress_Block_Table_Page(void);
-
-static u32 FTL_Get_Page_Num(u64 length);
-static u64 FTL_Get_Physical_Block_Addr(u64 blk_addr);
-
-static u32 FTL_Replace_OneBlock(u32 wBlockNum,
-				      u32 wReplaceNum);
-static u32 FTL_Replace_LWBlock(u32 wBlockNum,
-				     int *pGarbageCollect);
-static u32 FTL_Replace_MWBlock(void);
-static int FTL_Replace_Block(u64 blk_addr);
-static int FTL_Adjust_Relative_Erase_Count(u32 Index_of_MAX);
-
-struct device_info_tag DeviceInfo;
-struct flash_cache_tag Cache;
-static struct spectra_l2_cache_info cache_l2;
-
-static u8 *cache_l2_page_buf;
-static u8 *cache_l2_blk_buf;
-
-u8 *g_pBlockTable;
-u8 *g_pWearCounter;
-u16 *g_pReadCounter;
-u32 *g_pBTBlocks;
-static u16 g_wBlockTableOffset;
-static u32 g_wBlockTableIndex;
-static u8 g_cBlockTableStatus;
-
-static u8 *g_pTempBuf;
-static u8 *flag_check_blk_table;
-static u8 *tmp_buf_search_bt_in_block;
-static u8 *spare_buf_search_bt_in_block;
-static u8 *spare_buf_bt_search_bt_in_block;
-static u8 *tmp_buf1_read_blk_table;
-static u8 *tmp_buf2_read_blk_table;
-static u8 *flags_static_wear_leveling;
-static u8 *tmp_buf_write_blk_table_data;
-static u8 *tmp_buf_read_disturbance;
-
-u8 *buf_read_page_main_spare;
-u8 *buf_write_page_main_spare;
-u8 *buf_read_page_spare;
-u8 *buf_get_bad_block;
-
-#if (RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE && CMD_DMA)
-struct flash_cache_delta_list_tag int_cache[MAX_CHANS + MAX_DESCS];
-struct flash_cache_tag cache_start_copy;
-#endif
-
-int g_wNumFreeBlocks;
-u8 g_SBDCmdIndex;
-
-static u8 *g_pIPF;
-static u8 bt_flag = FIRST_BT_ID;
-static u8 bt_block_changed;
-
-static u16 cache_block_to_write;
-static u8 last_erased = FIRST_BT_ID;
-
-static u8 GC_Called;
-static u8 BT_GC_Called;
-
-#if CMD_DMA
-#define COPY_BACK_BUF_NUM 10
-
-static u8 ftl_cmd_cnt;  /* Init value is 0 */
-u8 *g_pBTDelta;
-u8 *g_pBTDelta_Free;
-u8 *g_pBTStartingCopy;
-u8 *g_pWearCounterCopy;
-u16 *g_pReadCounterCopy;
-u8 *g_pBlockTableCopies;
-u8 *g_pNextBlockTable;
-static u8 *cp_back_buf_copies[COPY_BACK_BUF_NUM];
-static int cp_back_buf_idx;
-
-static u8 *g_temp_buf;
-
-#pragma pack(push, 1)
-#pragma pack(1)
-struct BTableChangesDelta {
-	u8 ftl_cmd_cnt;
-	u8 ValidFields;
-	u16 g_wBlockTableOffset;
-	u32 g_wBlockTableIndex;
-	u32 BT_Index;
-	u32 BT_Entry_Value;
-	u32 WC_Index;
-	u8 WC_Entry_Value;
-	u32 RC_Index;
-	u16 RC_Entry_Value;
-};
-
-#pragma pack(pop)
-
-struct BTableChangesDelta *p_BTableChangesDelta;
-#endif
-
-
-#define MARK_BLOCK_AS_BAD(blocknode)      (blocknode |= BAD_BLOCK)
-#define MARK_BLK_AS_DISCARD(blk)  (blk = (blk & ~SPARE_BLOCK) | DISCARD_BLOCK)
-
-#define FTL_Get_LBAPBA_Table_Mem_Size_Bytes() (DeviceInfo.wDataBlockNum *\
-						sizeof(u32))
-#define FTL_Get_WearCounter_Table_Mem_Size_Bytes() (DeviceInfo.wDataBlockNum *\
-						sizeof(u8))
-#define FTL_Get_ReadCounter_Table_Mem_Size_Bytes() (DeviceInfo.wDataBlockNum *\
-						sizeof(u16))
-#if SUPPORT_LARGE_BLOCKNUM
-#define FTL_Get_LBAPBA_Table_Flash_Size_Bytes() (DeviceInfo.wDataBlockNum *\
-						sizeof(u8) * 3)
-#else
-#define FTL_Get_LBAPBA_Table_Flash_Size_Bytes() (DeviceInfo.wDataBlockNum *\
-						sizeof(u16))
-#endif
-#define FTL_Get_WearCounter_Table_Flash_Size_Bytes \
-	FTL_Get_WearCounter_Table_Mem_Size_Bytes
-#define FTL_Get_ReadCounter_Table_Flash_Size_Bytes \
-	FTL_Get_ReadCounter_Table_Mem_Size_Bytes
-
-static u32 FTL_Get_Block_Table_Flash_Size_Bytes(void)
-{
-	u32 byte_num;
-
-	if (DeviceInfo.MLCDevice) {
-		byte_num = FTL_Get_LBAPBA_Table_Flash_Size_Bytes() +
-			DeviceInfo.wDataBlockNum * sizeof(u8) +
-			DeviceInfo.wDataBlockNum * sizeof(u16);
-	} else {
-		byte_num = FTL_Get_LBAPBA_Table_Flash_Size_Bytes() +
-			DeviceInfo.wDataBlockNum * sizeof(u8);
-	}
-
-	byte_num += 4 * sizeof(u8);
-
-	return byte_num;
-}
-
-static u16  FTL_Get_Block_Table_Flash_Size_Pages(void)
-{
-	return (u16)FTL_Get_Page_Num(FTL_Get_Block_Table_Flash_Size_Bytes());
-}
-
-static int FTL_Copy_Block_Table_To_Flash(u8 *flashBuf, u32 sizeToTx,
-					u32 sizeTxed)
-{
-	u32 wBytesCopied, blk_tbl_size, wBytes;
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	blk_tbl_size = FTL_Get_LBAPBA_Table_Flash_Size_Bytes();
-	for (wBytes = 0;
-	(wBytes < sizeToTx) && ((wBytes + sizeTxed) < blk_tbl_size);
-	wBytes++) {
-#if SUPPORT_LARGE_BLOCKNUM
-		flashBuf[wBytes] = (u8)(pbt[(wBytes + sizeTxed) / 3]
-		>> (((wBytes + sizeTxed) % 3) ?
-		((((wBytes + sizeTxed) % 3) == 2) ? 0 : 8) : 16)) & 0xFF;
-#else
-		flashBuf[wBytes] = (u8)(pbt[(wBytes + sizeTxed) / 2]
-		>> (((wBytes + sizeTxed) % 2) ? 0 : 8)) & 0xFF;
-#endif
-	}
-
-	sizeTxed = (sizeTxed > blk_tbl_size) ? (sizeTxed - blk_tbl_size) : 0;
-	blk_tbl_size = FTL_Get_WearCounter_Table_Flash_Size_Bytes();
-	wBytesCopied = wBytes;
-	wBytes = ((blk_tbl_size - sizeTxed) > (sizeToTx - wBytesCopied)) ?
-		(sizeToTx - wBytesCopied) : (blk_tbl_size - sizeTxed);
-	memcpy(flashBuf + wBytesCopied, g_pWearCounter + sizeTxed, wBytes);
-
-	sizeTxed = (sizeTxed > blk_tbl_size) ? (sizeTxed - blk_tbl_size) : 0;
-
-	if (DeviceInfo.MLCDevice) {
-		blk_tbl_size = FTL_Get_ReadCounter_Table_Flash_Size_Bytes();
-		wBytesCopied += wBytes;
-		for (wBytes = 0; ((wBytes + wBytesCopied) < sizeToTx) &&
-			((wBytes + sizeTxed) < blk_tbl_size); wBytes++)
-			flashBuf[wBytes + wBytesCopied] =
-			(g_pReadCounter[(wBytes + sizeTxed) / 2] >>
-			(((wBytes + sizeTxed) % 2) ? 0 : 8)) & 0xFF;
-	}
-
-	return wBytesCopied + wBytes;
-}
-
-static int FTL_Copy_Block_Table_From_Flash(u8 *flashBuf,
-				u32 sizeToTx, u32 sizeTxed)
-{
-	u32 wBytesCopied, blk_tbl_size, wBytes;
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	blk_tbl_size = FTL_Get_LBAPBA_Table_Flash_Size_Bytes();
-	for (wBytes = 0; (wBytes < sizeToTx) &&
-		((wBytes + sizeTxed) < blk_tbl_size); wBytes++) {
-#if SUPPORT_LARGE_BLOCKNUM
-		if (!((wBytes + sizeTxed) % 3))
-			pbt[(wBytes + sizeTxed) / 3] = 0;
-		pbt[(wBytes + sizeTxed) / 3] |=
-			(flashBuf[wBytes] << (((wBytes + sizeTxed) % 3) ?
-			((((wBytes + sizeTxed) % 3) == 2) ? 0 : 8) : 16));
-#else
-		if (!((wBytes + sizeTxed) % 2))
-			pbt[(wBytes + sizeTxed) / 2] = 0;
-		pbt[(wBytes + sizeTxed) / 2] |=
-			(flashBuf[wBytes] << (((wBytes + sizeTxed) % 2) ?
-			0 : 8));
-#endif
-	}
-
-	sizeTxed = (sizeTxed > blk_tbl_size) ? (sizeTxed - blk_tbl_size) : 0;
-	blk_tbl_size = FTL_Get_WearCounter_Table_Flash_Size_Bytes();
-	wBytesCopied = wBytes;
-	wBytes = ((blk_tbl_size - sizeTxed) > (sizeToTx - wBytesCopied)) ?
-		(sizeToTx - wBytesCopied) : (blk_tbl_size - sizeTxed);
-	memcpy(g_pWearCounter + sizeTxed, flashBuf + wBytesCopied, wBytes);
-	sizeTxed = (sizeTxed > blk_tbl_size) ? (sizeTxed - blk_tbl_size) : 0;
-
-	if (DeviceInfo.MLCDevice) {
-		wBytesCopied += wBytes;
-		blk_tbl_size = FTL_Get_ReadCounter_Table_Flash_Size_Bytes();
-		for (wBytes = 0; ((wBytes + wBytesCopied) < sizeToTx) &&
-			((wBytes + sizeTxed) < blk_tbl_size); wBytes++) {
-			if (((wBytes + sizeTxed) % 2))
-				g_pReadCounter[(wBytes + sizeTxed) / 2] = 0;
-			g_pReadCounter[(wBytes + sizeTxed) / 2] |=
-				(flashBuf[wBytes] <<
-				(((wBytes + sizeTxed) % 2) ? 0 : 8));
-		}
-	}
-
-	return wBytesCopied+wBytes;
-}
-
-static int FTL_Insert_Block_Table_Signature(u8 *buf, u8 tag)
-{
-	int i;
-
-	for (i = 0; i < BTSIG_BYTES; i++)
-		buf[BTSIG_OFFSET + i] =
-		((tag + (i * BTSIG_DELTA) - FIRST_BT_ID) %
-		(1 + LAST_BT_ID-FIRST_BT_ID)) + FIRST_BT_ID;
-
-	return PASS;
-}
-
-static int FTL_Extract_Block_Table_Tag(u8 *buf, u8 **tagarray)
-{
-	static u8 tag[BTSIG_BYTES >> 1];
-	int i, j, k, tagi, tagtemp, status;
-
-	*tagarray = (u8 *)tag;
-	tagi = 0;
-
-	for (i = 0; i < (BTSIG_BYTES - 1); i++) {
-		for (j = i + 1; (j < BTSIG_BYTES) &&
-			(tagi < (BTSIG_BYTES >> 1)); j++) {
-			tagtemp = buf[BTSIG_OFFSET + j] -
-				buf[BTSIG_OFFSET + i];
-			if (tagtemp && !(tagtemp % BTSIG_DELTA)) {
-				tagtemp = (buf[BTSIG_OFFSET + i] +
-					(1 + LAST_BT_ID - FIRST_BT_ID) -
-					(i * BTSIG_DELTA)) %
-					(1 + LAST_BT_ID - FIRST_BT_ID);
-				status = FAIL;
-				for (k = 0; k < tagi; k++) {
-					if (tagtemp == tag[k])
-						status = PASS;
-				}
-
-				if (status == FAIL) {
-					tag[tagi++] = tagtemp;
-					i = (j == (i + 1)) ? i + 1 : i;
-					j = (j == (i + 1)) ? i + 1 : i;
-				}
-			}
-		}
-	}
-
-	return tagi;
-}
-
-
-static int FTL_Execute_SPL_Recovery(void)
-{
-	u32 j, block, blks;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	int ret;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-				__FILE__, __LINE__, __func__);
-
-	blks = DeviceInfo.wSpectraEndBlock - DeviceInfo.wSpectraStartBlock;
-	for (j = 0; j <= blks; j++) {
-		block = (pbt[j]);
-		if (((block & BAD_BLOCK) != BAD_BLOCK) &&
-			((block & SPARE_BLOCK) == SPARE_BLOCK)) {
-			ret =  GLOB_LLD_Erase_Block(block & ~BAD_BLOCK);
-			if (FAIL == ret) {
-				nand_dbg_print(NAND_DBG_WARN,
-					"NAND Program fail in %s, Line %d, "
-					"Function: %s, new Bad Block %d "
-					"generated!\n",
-					__FILE__, __LINE__, __func__,
-					(int)(block & ~BAD_BLOCK));
-				MARK_BLOCK_AS_BAD(pbt[j]);
-			}
-		}
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_IdentifyDevice
-* Inputs:       pointer to identify data structure
-* Outputs:      PASS / FAIL
-* Description:  the identify data structure is filled in with
-*                   information for the block driver.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_IdentifyDevice(struct spectra_indentfy_dev_tag *dev_data)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-				__FILE__, __LINE__, __func__);
-
-	dev_data->NumBlocks = DeviceInfo.wTotalBlocks;
-	dev_data->PagesPerBlock = DeviceInfo.wPagesPerBlock;
-	dev_data->PageDataSize = DeviceInfo.wPageDataSize;
-	dev_data->wECCBytesPerSector = DeviceInfo.wECCBytesPerSector;
-	dev_data->wDataBlockNum = DeviceInfo.wDataBlockNum;
-
-	return PASS;
-}
-
-/* ..... */
-static int allocate_memory(void)
-{
-	u32 block_table_size, page_size, block_size, mem_size;
-	u32 total_bytes = 0;
-	int i;
-#if CMD_DMA
-	int j;
-#endif
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	page_size = DeviceInfo.wPageSize;
-	block_size = DeviceInfo.wPagesPerBlock * DeviceInfo.wPageDataSize;
-
-	block_table_size = DeviceInfo.wDataBlockNum *
-		(sizeof(u32) + sizeof(u8) + sizeof(u16));
-	block_table_size += (DeviceInfo.wPageDataSize -
-		(block_table_size % DeviceInfo.wPageDataSize)) %
-		DeviceInfo.wPageDataSize;
-
-	/* Malloc memory for block tables */
-	g_pBlockTable = kzalloc(block_table_size, GFP_ATOMIC);
-	if (!g_pBlockTable)
-		goto block_table_fail;
-	total_bytes += block_table_size;
-
-	g_pWearCounter = (u8 *)(g_pBlockTable +
-		DeviceInfo.wDataBlockNum * sizeof(u32));
-
-	if (DeviceInfo.MLCDevice)
-		g_pReadCounter = (u16 *)(g_pBlockTable +
-			DeviceInfo.wDataBlockNum *
-			(sizeof(u32) + sizeof(u8)));
-
-	/* Malloc memory and init for cache items */
-	for (i = 0; i < CACHE_ITEM_NUM; i++) {
-		Cache.array[i].address = NAND_CACHE_INIT_ADDR;
-		Cache.array[i].use_cnt = 0;
-		Cache.array[i].changed = CLEAR;
-		Cache.array[i].buf = kzalloc(Cache.cache_item_size,
-					     GFP_ATOMIC);
-		if (!Cache.array[i].buf)
-			goto cache_item_fail;
-		total_bytes += Cache.cache_item_size;
-	}
-
-	/* Malloc memory for IPF */
-	g_pIPF = kzalloc(page_size, GFP_ATOMIC);
-	if (!g_pIPF)
-		goto ipf_fail;
-	total_bytes += page_size;
-
-	/* Malloc memory for data merging during Level2 Cache flush */
-	cache_l2_page_buf = kmalloc(page_size, GFP_ATOMIC);
-	if (!cache_l2_page_buf)
-		goto cache_l2_page_buf_fail;
-	memset(cache_l2_page_buf, 0xff, page_size);
-	total_bytes += page_size;
-
-	cache_l2_blk_buf = kmalloc(block_size, GFP_ATOMIC);
-	if (!cache_l2_blk_buf)
-		goto cache_l2_blk_buf_fail;
-	memset(cache_l2_blk_buf, 0xff, block_size);
-	total_bytes += block_size;
-
-	/* Malloc memory for temp buffer */
-	g_pTempBuf = kzalloc(Cache.cache_item_size, GFP_ATOMIC);
-	if (!g_pTempBuf)
-		goto Temp_buf_fail;
-	total_bytes += Cache.cache_item_size;
-
-	/* Malloc memory for block table blocks */
-	mem_size = (1 + LAST_BT_ID - FIRST_BT_ID) * sizeof(u32);
-	g_pBTBlocks = kmalloc(mem_size, GFP_ATOMIC);
-	if (!g_pBTBlocks)
-		goto bt_blocks_fail;
-	memset(g_pBTBlocks, 0xff, mem_size);
-	total_bytes += mem_size;
-
-	/* Malloc memory for function FTL_Check_Block_Table */
-	flag_check_blk_table = kmalloc(DeviceInfo.wDataBlockNum, GFP_ATOMIC);
-	if (!flag_check_blk_table)
-		goto flag_check_blk_table_fail;
-	total_bytes += DeviceInfo.wDataBlockNum;
-
-	/* Malloc memory for function FTL_Search_Block_Table_IN_Block */
-	tmp_buf_search_bt_in_block = kmalloc(page_size, GFP_ATOMIC);
-	if (!tmp_buf_search_bt_in_block)
-		goto tmp_buf_search_bt_in_block_fail;
-	memset(tmp_buf_search_bt_in_block, 0xff, page_size);
-	total_bytes += page_size;
-
-	mem_size = DeviceInfo.wPageSize - DeviceInfo.wPageDataSize;
-	spare_buf_search_bt_in_block = kmalloc(mem_size, GFP_ATOMIC);
-	if (!spare_buf_search_bt_in_block)
-		goto spare_buf_search_bt_in_block_fail;
-	memset(spare_buf_search_bt_in_block, 0xff, mem_size);
-	total_bytes += mem_size;
-
-	spare_buf_bt_search_bt_in_block = kmalloc(mem_size, GFP_ATOMIC);
-	if (!spare_buf_bt_search_bt_in_block)
-		goto spare_buf_bt_search_bt_in_block_fail;
-	memset(spare_buf_bt_search_bt_in_block, 0xff, mem_size);
-	total_bytes += mem_size;
-
-	/* Malloc memory for function FTL_Read_Block_Table */
-	tmp_buf1_read_blk_table = kmalloc(page_size, GFP_ATOMIC);
-	if (!tmp_buf1_read_blk_table)
-		goto tmp_buf1_read_blk_table_fail;
-	memset(tmp_buf1_read_blk_table, 0xff, page_size);
-	total_bytes += page_size;
-
-	tmp_buf2_read_blk_table = kmalloc(page_size, GFP_ATOMIC);
-	if (!tmp_buf2_read_blk_table)
-		goto tmp_buf2_read_blk_table_fail;
-	memset(tmp_buf2_read_blk_table, 0xff, page_size);
-	total_bytes += page_size;
-
-	/* Malloc memory for function FTL_Static_Wear_Leveling */
-	flags_static_wear_leveling = kmalloc(DeviceInfo.wDataBlockNum,
-					GFP_ATOMIC);
-	if (!flags_static_wear_leveling)
-		goto flags_static_wear_leveling_fail;
-	total_bytes += DeviceInfo.wDataBlockNum;
-
-	/* Malloc memory for function FTL_Write_Block_Table_Data */
-	if (FTL_Get_Block_Table_Flash_Size_Pages() > 3)
-		mem_size = FTL_Get_Block_Table_Flash_Size_Bytes() -
-				2 * DeviceInfo.wPageSize;
-	else
-		mem_size = DeviceInfo.wPageSize;
-	tmp_buf_write_blk_table_data = kmalloc(mem_size, GFP_ATOMIC);
-	if (!tmp_buf_write_blk_table_data)
-		goto tmp_buf_write_blk_table_data_fail;
-	memset(tmp_buf_write_blk_table_data, 0xff, mem_size);
-	total_bytes += mem_size;
-
-	/* Malloc memory for function FTL_Read_Disturbance */
-	tmp_buf_read_disturbance = kmalloc(block_size, GFP_ATOMIC);
-	if (!tmp_buf_read_disturbance)
-		goto tmp_buf_read_disturbance_fail;
-	memset(tmp_buf_read_disturbance, 0xff, block_size);
-	total_bytes += block_size;
-
-	/* Alloc mem for function NAND_Read_Page_Main_Spare of lld_nand.c */
-	buf_read_page_main_spare = kmalloc(DeviceInfo.wPageSize, GFP_ATOMIC);
-	if (!buf_read_page_main_spare)
-		goto buf_read_page_main_spare_fail;
-	total_bytes += DeviceInfo.wPageSize;
-
-	/* Alloc mem for function NAND_Write_Page_Main_Spare of lld_nand.c */
-	buf_write_page_main_spare = kmalloc(DeviceInfo.wPageSize, GFP_ATOMIC);
-	if (!buf_write_page_main_spare)
-		goto buf_write_page_main_spare_fail;
-	total_bytes += DeviceInfo.wPageSize;
-
-	/* Alloc mem for function NAND_Read_Page_Spare of lld_nand.c */
-	buf_read_page_spare = kmalloc(DeviceInfo.wPageSpareSize, GFP_ATOMIC);
-	if (!buf_read_page_spare)
-		goto buf_read_page_spare_fail;
-	memset(buf_read_page_spare, 0xff, DeviceInfo.wPageSpareSize);
-	total_bytes += DeviceInfo.wPageSpareSize;
-
-	/* Alloc mem for function NAND_Get_Bad_Block of lld_nand.c */
-	buf_get_bad_block = kmalloc(DeviceInfo.wPageSpareSize, GFP_ATOMIC);
-	if (!buf_get_bad_block)
-		goto buf_get_bad_block_fail;
-	memset(buf_get_bad_block, 0xff, DeviceInfo.wPageSpareSize);
-	total_bytes += DeviceInfo.wPageSpareSize;
-
-#if CMD_DMA
-	g_temp_buf = kmalloc(block_size, GFP_ATOMIC);
-	if (!g_temp_buf)
-		goto temp_buf_fail;
-	memset(g_temp_buf, 0xff, block_size);
-	total_bytes += block_size;
-
-	/* Malloc memory for copy of block table used in CDMA mode */
-	g_pBTStartingCopy = kzalloc(block_table_size, GFP_ATOMIC);
-	if (!g_pBTStartingCopy)
-		goto bt_starting_copy;
-	total_bytes += block_table_size;
-
-	g_pWearCounterCopy = (u8 *)(g_pBTStartingCopy +
-		DeviceInfo.wDataBlockNum * sizeof(u32));
-
-	if (DeviceInfo.MLCDevice)
-		g_pReadCounterCopy = (u16 *)(g_pBTStartingCopy +
-			DeviceInfo.wDataBlockNum *
-			(sizeof(u32) + sizeof(u8)));
-
-	/* Malloc memory for block table copies */
-	mem_size = 5 * DeviceInfo.wDataBlockNum * sizeof(u32) +
-			5 * DeviceInfo.wDataBlockNum * sizeof(u8);
-	if (DeviceInfo.MLCDevice)
-		mem_size += 5 * DeviceInfo.wDataBlockNum * sizeof(u16);
-	g_pBlockTableCopies = kzalloc(mem_size, GFP_ATOMIC);
-	if (!g_pBlockTableCopies)
-		goto blk_table_copies_fail;
-	total_bytes += mem_size;
-	g_pNextBlockTable = g_pBlockTableCopies;
-
-	/* Malloc memory for Block Table Delta */
-	mem_size = MAX_DESCS * sizeof(struct BTableChangesDelta);
-	g_pBTDelta = kzalloc(mem_size, GFP_ATOMIC);
-	if (!g_pBTDelta)
-		goto bt_delta_fail;
-	total_bytes += mem_size;
-	g_pBTDelta_Free = g_pBTDelta;
-
-	/* Malloc memory for Copy Back Buffers */
-	for (j = 0; j < COPY_BACK_BUF_NUM; j++) {
-		cp_back_buf_copies[j] = kzalloc(block_size, GFP_ATOMIC);
-		if (!cp_back_buf_copies[j])
-			goto cp_back_buf_copies_fail;
-		total_bytes += block_size;
-	}
-	cp_back_buf_idx = 0;
-
-	/* Malloc memory for pending commands list */
-	mem_size = sizeof(struct pending_cmd) * MAX_DESCS;
-	info.pcmds = kzalloc(mem_size, GFP_KERNEL);
-	if (!info.pcmds)
-		goto pending_cmds_buf_fail;
-	total_bytes += mem_size;
-
-	/* Malloc memory for CDMA descripter table */
-	mem_size = sizeof(struct cdma_descriptor) * MAX_DESCS;
-	info.cdma_desc_buf = kzalloc(mem_size, GFP_KERNEL);
-	if (!info.cdma_desc_buf)
-		goto cdma_desc_buf_fail;
-	total_bytes += mem_size;
-
-	/* Malloc memory for Memcpy descripter table */
-	mem_size = sizeof(struct memcpy_descriptor) * MAX_DESCS;
-	info.memcp_desc_buf = kzalloc(mem_size, GFP_KERNEL);
-	if (!info.memcp_desc_buf)
-		goto memcp_desc_buf_fail;
-	total_bytes += mem_size;
-#endif
-
-	nand_dbg_print(NAND_DBG_WARN,
-		"Total memory allocated in FTL layer: %d\n", total_bytes);
-
-	return PASS;
-
-#if CMD_DMA
-memcp_desc_buf_fail:
-	kfree(info.cdma_desc_buf);
-cdma_desc_buf_fail:
-	kfree(info.pcmds);
-pending_cmds_buf_fail:
-cp_back_buf_copies_fail:
-	j--;
-	for (; j >= 0; j--)
-		kfree(cp_back_buf_copies[j]);
-	kfree(g_pBTDelta);
-bt_delta_fail:
-	kfree(g_pBlockTableCopies);
-blk_table_copies_fail:
-	kfree(g_pBTStartingCopy);
-bt_starting_copy:
-	kfree(g_temp_buf);
-temp_buf_fail:
-	kfree(buf_get_bad_block);
-#endif
-
-buf_get_bad_block_fail:
-	kfree(buf_read_page_spare);
-buf_read_page_spare_fail:
-	kfree(buf_write_page_main_spare);
-buf_write_page_main_spare_fail:
-	kfree(buf_read_page_main_spare);
-buf_read_page_main_spare_fail:
-	kfree(tmp_buf_read_disturbance);
-tmp_buf_read_disturbance_fail:
-	kfree(tmp_buf_write_blk_table_data);
-tmp_buf_write_blk_table_data_fail:
-	kfree(flags_static_wear_leveling);
-flags_static_wear_leveling_fail:
-	kfree(tmp_buf2_read_blk_table);
-tmp_buf2_read_blk_table_fail:
-	kfree(tmp_buf1_read_blk_table);
-tmp_buf1_read_blk_table_fail:
-	kfree(spare_buf_bt_search_bt_in_block);
-spare_buf_bt_search_bt_in_block_fail:
-	kfree(spare_buf_search_bt_in_block);
-spare_buf_search_bt_in_block_fail:
-	kfree(tmp_buf_search_bt_in_block);
-tmp_buf_search_bt_in_block_fail:
-	kfree(flag_check_blk_table);
-flag_check_blk_table_fail:
-	kfree(g_pBTBlocks);
-bt_blocks_fail:
-	kfree(g_pTempBuf);
-Temp_buf_fail:
-	kfree(cache_l2_blk_buf);
-cache_l2_blk_buf_fail:
-	kfree(cache_l2_page_buf);
-cache_l2_page_buf_fail:
-	kfree(g_pIPF);
-ipf_fail:
-cache_item_fail:
-	i--;
-	for (; i >= 0; i--)
-		kfree(Cache.array[i].buf);
-	kfree(g_pBlockTable);
-block_table_fail:
-	printk(KERN_ERR "Failed to kmalloc memory in %s Line %d.\n",
-		__FILE__, __LINE__);
-
-	return -ENOMEM;
-}
-
-/* .... */
-static int free_memory(void)
-{
-	int i;
-
-#if CMD_DMA
-	kfree(info.memcp_desc_buf);
-	kfree(info.cdma_desc_buf);
-	kfree(info.pcmds);
-	for (i = COPY_BACK_BUF_NUM - 1; i >= 0; i--)
-		kfree(cp_back_buf_copies[i]);
-	kfree(g_pBTDelta);
-	kfree(g_pBlockTableCopies);
-	kfree(g_pBTStartingCopy);
-	kfree(g_temp_buf);
-	kfree(buf_get_bad_block);
-#endif
-	kfree(buf_read_page_spare);
-	kfree(buf_write_page_main_spare);
-	kfree(buf_read_page_main_spare);
-	kfree(tmp_buf_read_disturbance);
-	kfree(tmp_buf_write_blk_table_data);
-	kfree(flags_static_wear_leveling);
-	kfree(tmp_buf2_read_blk_table);
-	kfree(tmp_buf1_read_blk_table);
-	kfree(spare_buf_bt_search_bt_in_block);
-	kfree(spare_buf_search_bt_in_block);
-	kfree(tmp_buf_search_bt_in_block);
-	kfree(flag_check_blk_table);
-	kfree(g_pBTBlocks);
-	kfree(g_pTempBuf);
-	kfree(g_pIPF);
-	for (i = CACHE_ITEM_NUM - 1; i >= 0; i--)
-		kfree(Cache.array[i].buf);
-	kfree(g_pBlockTable);
-
-	return 0;
-}
-
-static void dump_cache_l2_table(void)
-{
-	struct list_head *p;
-	struct spectra_l2_cache_list *pnd;
-	int n;
-
-	n = 0;
-	list_for_each(p, &cache_l2.table.list) {
-		pnd = list_entry(p, struct spectra_l2_cache_list, list);
-		nand_dbg_print(NAND_DBG_WARN, "dump_cache_l2_table node: %d, logical_blk_num: %d\n", n, pnd->logical_blk_num);
-/*
-		for (i = 0; i < DeviceInfo.wPagesPerBlock; i++) {
-			if (pnd->pages_array[i] != MAX_U32_VALUE)
-				nand_dbg_print(NAND_DBG_WARN, "    pages_array[%d]: 0x%x\n", i, pnd->pages_array[i]);
-		}
-*/
-		n++;
-	}
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Init
-* Inputs:       none
-* Outputs:      PASS=0 / FAIL=1
-* Description:  allocates the memory for cache array,
-*               important data structures
-*               clears the cache array
-*               reads the block table from flash into array
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Init(void)
-{
-	int i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	Cache.pages_per_item = 1;
-	Cache.cache_item_size = 1 * DeviceInfo.wPageDataSize;
-
-	if (allocate_memory() != PASS)
-		return FAIL;
-
-#if CMD_DMA
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	memcpy((void *)&cache_start_copy, (void *)&Cache,
-		sizeof(struct flash_cache_tag));
-	memset((void *)&int_cache, -1,
-		sizeof(struct flash_cache_delta_list_tag) *
-		(MAX_CHANS + MAX_DESCS));
-#endif
-	ftl_cmd_cnt = 0;
-#endif
-
-	if (FTL_Read_Block_Table() != PASS)
-		return FAIL;
-
-	/* Init the Level2 Cache data structure */
-	for (i = 0; i < BLK_NUM_FOR_L2_CACHE; i++)
-		cache_l2.blk_array[i] = MAX_U32_VALUE;
-	cache_l2.cur_blk_idx = 0;
-	cache_l2.cur_page_num = 0;
-	INIT_LIST_HEAD(&cache_l2.table.list);
-	cache_l2.table.logical_blk_num = MAX_U32_VALUE;
-
-	dump_cache_l2_table();
-
-	return 0;
-}
-
-
-#if CMD_DMA
-#if 0
-static void save_blk_table_changes(u16 idx)
-{
-	u8 ftl_cmd;
-	u32 *pbt = (u32 *)g_pBTStartingCopy;
-
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	u16 id;
-	u8 cache_blks;
-
-	id = idx - MAX_CHANS;
-	if (int_cache[id].item != -1) {
-		cache_blks = int_cache[id].item;
-		cache_start_copy.array[cache_blks].address =
-			int_cache[id].cache.address;
-		cache_start_copy.array[cache_blks].changed =
-			int_cache[id].cache.changed;
-	}
-#endif
-
-	ftl_cmd = p_BTableChangesDelta->ftl_cmd_cnt;
-
-	while (ftl_cmd <= PendingCMD[idx].Tag) {
-		if (p_BTableChangesDelta->ValidFields == 0x01) {
-			g_wBlockTableOffset =
-				p_BTableChangesDelta->g_wBlockTableOffset;
-		} else if (p_BTableChangesDelta->ValidFields == 0x0C) {
-			pbt[p_BTableChangesDelta->BT_Index] =
-				p_BTableChangesDelta->BT_Entry_Value;
-			debug_boundary_error(((
-				p_BTableChangesDelta->BT_Index)),
-				DeviceInfo.wDataBlockNum, 0);
-		} else if (p_BTableChangesDelta->ValidFields == 0x03) {
-			g_wBlockTableOffset =
-				p_BTableChangesDelta->g_wBlockTableOffset;
-			g_wBlockTableIndex =
-				p_BTableChangesDelta->g_wBlockTableIndex;
-		} else if (p_BTableChangesDelta->ValidFields == 0x30) {
-			g_pWearCounterCopy[p_BTableChangesDelta->WC_Index] =
-				p_BTableChangesDelta->WC_Entry_Value;
-		} else if ((DeviceInfo.MLCDevice) &&
-			(p_BTableChangesDelta->ValidFields == 0xC0)) {
-			g_pReadCounterCopy[p_BTableChangesDelta->RC_Index] =
-				p_BTableChangesDelta->RC_Entry_Value;
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"In event status setting read counter "
-				"GLOB_ftl_cmd_cnt %u Count %u Index %u\n",
-				ftl_cmd,
-				p_BTableChangesDelta->RC_Entry_Value,
-				(unsigned int)p_BTableChangesDelta->RC_Index);
-		} else {
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"This should never occur \n");
-		}
-		p_BTableChangesDelta += 1;
-		ftl_cmd = p_BTableChangesDelta->ftl_cmd_cnt;
-	}
-}
-
-static void discard_cmds(u16 n)
-{
-	u32 *pbt = (u32 *)g_pBTStartingCopy;
-	u8 ftl_cmd;
-	unsigned long k;
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	u8 cache_blks;
-	u16 id;
-#endif
-
-	if ((PendingCMD[n].CMD == WRITE_MAIN_CMD) ||
-		(PendingCMD[n].CMD == WRITE_MAIN_SPARE_CMD)) {
-		for (k = 0; k < DeviceInfo.wDataBlockNum; k++) {
-			if (PendingCMD[n].Block == (pbt[k] & (~BAD_BLOCK)))
-				MARK_BLK_AS_DISCARD(pbt[k]);
-		}
-	}
-
-	ftl_cmd = p_BTableChangesDelta->ftl_cmd_cnt;
-	while (ftl_cmd <= PendingCMD[n].Tag) {
-		p_BTableChangesDelta += 1;
-		ftl_cmd = p_BTableChangesDelta->ftl_cmd_cnt;
-	}
-
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	id = n - MAX_CHANS;
-
-	if (int_cache[id].item != -1) {
-		cache_blks = int_cache[id].item;
-		if (PendingCMD[n].CMD == MEMCOPY_CMD) {
-			if ((cache_start_copy.array[cache_blks].buf <=
-				PendingCMD[n].DataDestAddr) &&
-				((cache_start_copy.array[cache_blks].buf +
-				Cache.cache_item_size) >
-				PendingCMD[n].DataDestAddr)) {
-				cache_start_copy.array[cache_blks].address =
-						NAND_CACHE_INIT_ADDR;
-				cache_start_copy.array[cache_blks].use_cnt =
-								0;
-				cache_start_copy.array[cache_blks].changed =
-								CLEAR;
-			}
-		} else {
-			cache_start_copy.array[cache_blks].address =
-					int_cache[id].cache.address;
-			cache_start_copy.array[cache_blks].changed =
-					int_cache[id].cache.changed;
-		}
-	}
-#endif
-}
-
-static void process_cmd_pass(int *first_failed_cmd, u16 idx)
-{
-	if (0 == *first_failed_cmd)
-		save_blk_table_changes(idx);
-	else
-		discard_cmds(idx);
-}
-
-static void process_cmd_fail_abort(int *first_failed_cmd,
-				u16 idx, int event)
-{
-	u32 *pbt = (u32 *)g_pBTStartingCopy;
-	u8 ftl_cmd;
-	unsigned long i;
-	int erase_fail, program_fail;
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	u8 cache_blks;
-	u16 id;
-#endif
-
-	if (0 == *first_failed_cmd)
-		*first_failed_cmd = PendingCMD[idx].SBDCmdIndex;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Uncorrectable error has occurred "
-		"while executing %u Command %u accesing Block %u\n",
-		(unsigned int)p_BTableChangesDelta->ftl_cmd_cnt,
-		PendingCMD[idx].CMD,
-		(unsigned int)PendingCMD[idx].Block);
-
-	ftl_cmd = p_BTableChangesDelta->ftl_cmd_cnt;
-	while (ftl_cmd <= PendingCMD[idx].Tag) {
-		p_BTableChangesDelta += 1;
-		ftl_cmd = p_BTableChangesDelta->ftl_cmd_cnt;
-	}
-
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	id = idx - MAX_CHANS;
-
-	if (int_cache[id].item != -1) {
-		cache_blks = int_cache[id].item;
-		if ((PendingCMD[idx].CMD == WRITE_MAIN_CMD)) {
-			cache_start_copy.array[cache_blks].address =
-					int_cache[id].cache.address;
-			cache_start_copy.array[cache_blks].changed = SET;
-		} else if ((PendingCMD[idx].CMD == READ_MAIN_CMD)) {
-			cache_start_copy.array[cache_blks].address =
-				NAND_CACHE_INIT_ADDR;
-			cache_start_copy.array[cache_blks].use_cnt = 0;
-			cache_start_copy.array[cache_blks].changed =
-							CLEAR;
-		} else if (PendingCMD[idx].CMD == ERASE_CMD) {
-			/* ? */
-		} else if (PendingCMD[idx].CMD == MEMCOPY_CMD) {
-			/* ? */
-		}
-	}
-#endif
-
-	erase_fail = (event == EVENT_ERASE_FAILURE) &&
-			(PendingCMD[idx].CMD == ERASE_CMD);
-
-	program_fail = (event == EVENT_PROGRAM_FAILURE) &&
-			((PendingCMD[idx].CMD == WRITE_MAIN_CMD) ||
-			(PendingCMD[idx].CMD == WRITE_MAIN_SPARE_CMD));
-
-	if (erase_fail || program_fail) {
-		for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-			if (PendingCMD[idx].Block ==
-				(pbt[i] & (~BAD_BLOCK)))
-				MARK_BLOCK_AS_BAD(pbt[i]);
-		}
-	}
-}
-
-static void process_cmd(int *first_failed_cmd, u16 idx, int event)
-{
-	u8 ftl_cmd;
-	int cmd_match = 0;
-
-	if (p_BTableChangesDelta->ftl_cmd_cnt == PendingCMD[idx].Tag)
-		cmd_match = 1;
-
-	if (PendingCMD[idx].Status == CMD_PASS) {
-		process_cmd_pass(first_failed_cmd, idx);
-	} else if ((PendingCMD[idx].Status == CMD_FAIL) ||
-			(PendingCMD[idx].Status == CMD_ABORT)) {
-		process_cmd_fail_abort(first_failed_cmd, idx, event);
-	} else if ((PendingCMD[idx].Status == CMD_NOT_DONE) &&
-					PendingCMD[idx].Tag) {
-		nand_dbg_print(NAND_DBG_DEBUG,
-			" Command no. %hu is not executed\n",
-			(unsigned int)PendingCMD[idx].Tag);
-		ftl_cmd = p_BTableChangesDelta->ftl_cmd_cnt;
-		while (ftl_cmd <= PendingCMD[idx].Tag) {
-			p_BTableChangesDelta += 1;
-			ftl_cmd = p_BTableChangesDelta->ftl_cmd_cnt;
-		}
-	}
-}
-#endif
-
-static void process_cmd(int *first_failed_cmd, u16 idx, int event)
-{
-	printk(KERN_ERR "temporary workaround function. "
-		"Should not be called! \n");
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:    	GLOB_FTL_Event_Status
-* Inputs:       none
-* Outputs:      Event Code
-* Description:	It is called by SBD after hardware interrupt signalling
-*               completion of commands chain
-*               It does following things
-*               get event status from LLD
-*               analyze command chain status
-*               determine last command executed
-*               analyze results
-*               rebuild the block table in case of uncorrectable error
-*               return event code
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Event_Status(int *first_failed_cmd)
-{
-	int event_code = PASS;
-	u16 i_P;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	*first_failed_cmd = 0;
-
-	event_code = GLOB_LLD_Event_Status();
-
-	switch (event_code) {
-	case EVENT_PASS:
-		nand_dbg_print(NAND_DBG_DEBUG, "Handling EVENT_PASS\n");
-		break;
-	case EVENT_UNCORRECTABLE_DATA_ERROR:
-		nand_dbg_print(NAND_DBG_DEBUG, "Handling Uncorrectable ECC!\n");
-		break;
-	case EVENT_PROGRAM_FAILURE:
-	case EVENT_ERASE_FAILURE:
-		nand_dbg_print(NAND_DBG_WARN, "Handling Ugly case. "
-			"Event code: 0x%x\n", event_code);
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta;
-		for (i_P = MAX_CHANS; i_P < (ftl_cmd_cnt + MAX_CHANS);
-				i_P++)
-			process_cmd(first_failed_cmd, i_P, event_code);
-		memcpy(g_pBlockTable, g_pBTStartingCopy,
-			DeviceInfo.wDataBlockNum * sizeof(u32));
-		memcpy(g_pWearCounter, g_pWearCounterCopy,
-			DeviceInfo.wDataBlockNum * sizeof(u8));
-		if (DeviceInfo.MLCDevice)
-			memcpy(g_pReadCounter, g_pReadCounterCopy,
-				DeviceInfo.wDataBlockNum * sizeof(u16));
-
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-		memcpy((void *)&Cache, (void *)&cache_start_copy,
-			sizeof(struct flash_cache_tag));
-		memset((void *)&int_cache, -1,
-			sizeof(struct flash_cache_delta_list_tag) *
-			(MAX_DESCS + MAX_CHANS));
-#endif
-		break;
-	default:
-		nand_dbg_print(NAND_DBG_WARN,
-			"Handling unexpected event code - 0x%x\n",
-			event_code);
-		event_code = ERR;
-		break;
-	}
-
-	memcpy(g_pBTStartingCopy, g_pBlockTable,
-		DeviceInfo.wDataBlockNum * sizeof(u32));
-	memcpy(g_pWearCounterCopy, g_pWearCounter,
-		DeviceInfo.wDataBlockNum * sizeof(u8));
-	if (DeviceInfo.MLCDevice)
-		memcpy(g_pReadCounterCopy, g_pReadCounter,
-			DeviceInfo.wDataBlockNum * sizeof(u16));
-
-	g_pBTDelta_Free = g_pBTDelta;
-	ftl_cmd_cnt = 0;
-	g_pNextBlockTable = g_pBlockTableCopies;
-	cp_back_buf_idx = 0;
-
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	memcpy((void *)&cache_start_copy, (void *)&Cache,
-		sizeof(struct flash_cache_tag));
-	memset((void *)&int_cache, -1,
-		sizeof(struct flash_cache_delta_list_tag) *
-		(MAX_DESCS + MAX_CHANS));
-#endif
-
-	return event_code;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     glob_ftl_execute_cmds
-* Inputs:       none
-* Outputs:      none
-* Description:  pass thru to LLD
-***************************************************************/
-u16 glob_ftl_execute_cmds(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE,
-		"glob_ftl_execute_cmds: ftl_cmd_cnt %u\n",
-		(unsigned int)ftl_cmd_cnt);
-	g_SBDCmdIndex = 0;
-	return glob_lld_execute_cmds();
-}
-
-#endif
-
-#if !CMD_DMA
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Read Immediate
-* Inputs:         pointer to data
-*                     address of data
-* Outputs:      PASS / FAIL
-* Description:  Reads one page of data into RAM directly from flash without
-*       using or disturbing cache.It is assumed this function is called
-*       with CMD-DMA disabled.
-*****************************************************************/
-int GLOB_FTL_Read_Immediate(u8 *read_data, u64 addr)
-{
-	int wResult = FAIL;
-	u32 Block;
-	u16 Page;
-	u32 phy_blk;
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	Block = BLK_FROM_ADDR(addr);
-	Page = PAGE_FROM_ADDR(addr, Block);
-
-	if (!IS_SPARE_BLOCK(Block))
-		return FAIL;
-
-	phy_blk = pbt[Block];
-	wResult = GLOB_LLD_Read_Page_Main(read_data, phy_blk, Page, 1);
-
-	if (DeviceInfo.MLCDevice) {
-		g_pReadCounter[phy_blk - DeviceInfo.wSpectraStartBlock]++;
-		if (g_pReadCounter[phy_blk - DeviceInfo.wSpectraStartBlock]
-			>= MAX_READ_COUNTER)
-			FTL_Read_Disturbance(phy_blk);
-		if (g_cBlockTableStatus != IN_PROGRESS_BLOCK_TABLE) {
-			g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-			FTL_Write_IN_Progress_Block_Table_Page();
-		}
-	}
-
-	return wResult;
-}
-#endif
-
-#ifdef SUPPORT_BIG_ENDIAN
-/*********************************************************************
-* Function:     FTL_Invert_Block_Table
-* Inputs:       none
-* Outputs:      none
-* Description:  Re-format the block table in ram based on BIG_ENDIAN and
-*                     LARGE_BLOCKNUM if necessary
-**********************************************************************/
-static void FTL_Invert_Block_Table(void)
-{
-	u32 i;
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-#ifdef SUPPORT_LARGE_BLOCKNUM
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-		pbt[i] = INVERTUINT32(pbt[i]);
-		g_pWearCounter[i] = INVERTUINT32(g_pWearCounter[i]);
-	}
-#else
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-		pbt[i] = INVERTUINT16(pbt[i]);
-		g_pWearCounter[i] = INVERTUINT16(g_pWearCounter[i]);
-	}
-#endif
-}
-#endif
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Flash_Init
-* Inputs:       none
-* Outputs:      PASS=0 / FAIL=0x01 (based on read ID)
-* Description:  The flash controller is initialized
-*               The flash device is reset
-*               Perform a flash READ ID command to confirm that a
-*                   valid device is attached and active.
-*                   The DeviceInfo structure gets filled in
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Flash_Init(void)
-{
-	int status = FAIL;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	g_SBDCmdIndex = 0;
-
-	status = GLOB_LLD_Flash_Init();
-
-	return status;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Inputs:       none
-* Outputs:      PASS=0 / FAIL=0x01 (based on read ID)
-* Description:  The flash controller is released
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Flash_Release(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	return GLOB_LLD_Flash_Release();
-}
-
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Cache_Release
-* Inputs:       none
-* Outputs:      none
-* Description:  release all allocated memory in GLOB_FTL_Init
-*               (allocated in GLOB_FTL_Init)
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-void GLOB_FTL_Cache_Release(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	free_memory();
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Cache_If_Hit
-* Inputs:       Page Address
-* Outputs:      Block number/UNHIT BLOCK
-* Description:  Determines if the addressed page is in cache
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static u16 FTL_Cache_If_Hit(u64 page_addr)
-{
-	u16 item;
-	u64 addr;
-	int i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	item = UNHIT_CACHE_ITEM;
-	for (i = 0; i < CACHE_ITEM_NUM; i++) {
-		addr = Cache.array[i].address;
-		if ((page_addr >= addr) &&
-			(page_addr < (addr + Cache.cache_item_size))) {
-			item = i;
-			break;
-		}
-	}
-
-	return item;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Calculate_LRU
-* Inputs:       None
-* Outputs:      None
-* Description:  Calculate the least recently block in a cache and record its
-*               index in LRU field.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static void FTL_Calculate_LRU(void)
-{
-	u16 i, bCurrentLRU, bTempCount;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	bCurrentLRU = 0;
-	bTempCount = MAX_WORD_VALUE;
-
-	for (i = 0; i < CACHE_ITEM_NUM; i++) {
-		if (Cache.array[i].use_cnt < bTempCount) {
-			bCurrentLRU = i;
-			bTempCount = Cache.array[i].use_cnt;
-		}
-	}
-
-	Cache.LRU = bCurrentLRU;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Cache_Read_Page
-* Inputs:       pointer to read buffer, logical address and cache item number
-* Outputs:      None
-* Description:  Read the page from the cached block addressed by blocknumber
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static void FTL_Cache_Read_Page(u8 *data_buf, u64 logic_addr, u16 cache_item)
-{
-	u8 *start_addr;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	start_addr = Cache.array[cache_item].buf;
-	start_addr += (u32)(((logic_addr - Cache.array[cache_item].address) >>
-		DeviceInfo.nBitsInPageDataSize) * DeviceInfo.wPageDataSize);
-
-#if CMD_DMA
-	GLOB_LLD_MemCopy_CMD(data_buf, start_addr,
-			DeviceInfo.wPageDataSize, 0);
-	ftl_cmd_cnt++;
-#else
-	memcpy(data_buf, start_addr, DeviceInfo.wPageDataSize);
-#endif
-
-	if (Cache.array[cache_item].use_cnt < MAX_WORD_VALUE)
-		Cache.array[cache_item].use_cnt++;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Cache_Read_All
-* Inputs:       pointer to read buffer,block address
-* Outputs:      PASS=0 / FAIL =1
-* Description:  It reads pages in cache
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Cache_Read_All(u8 *pData, u64 phy_addr)
-{
-	int wResult = PASS;
-	u32 Block;
-	u32 lba;
-	u16 Page;
-	u16 PageCount;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 i;
-
-	Block = BLK_FROM_ADDR(phy_addr);
-	Page = PAGE_FROM_ADDR(phy_addr, Block);
-	PageCount = Cache.pages_per_item;
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-			"%s, Line %d, Function: %s, Block: 0x%x\n",
-			__FILE__, __LINE__, __func__, Block);
-
-	lba = 0xffffffff;
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-		if ((pbt[i] & (~BAD_BLOCK)) == Block) {
-			lba = i;
-			if (IS_SPARE_BLOCK(i) || IS_BAD_BLOCK(i) ||
-				IS_DISCARDED_BLOCK(i)) {
-				/* Add by yunpeng -2008.12.3 */
-#if CMD_DMA
-				GLOB_LLD_MemCopy_CMD(pData, g_temp_buf,
-				PageCount * DeviceInfo.wPageDataSize, 0);
-				ftl_cmd_cnt++;
-#else
-				memset(pData, 0xFF,
-					PageCount * DeviceInfo.wPageDataSize);
-#endif
-				return wResult;
-			} else {
-				continue; /* break ?? */
-			}
-		}
-	}
-
-	if (0xffffffff == lba)
-		printk(KERN_ERR "FTL_Cache_Read_All: Block is not found in BT\n");
-
-#if CMD_DMA
-	wResult = GLOB_LLD_Read_Page_Main_cdma(pData, Block, Page,
-			PageCount, LLD_CMD_FLAG_MODE_CDMA);
-	if (DeviceInfo.MLCDevice) {
-		g_pReadCounter[Block - DeviceInfo.wSpectraStartBlock]++;
-		nand_dbg_print(NAND_DBG_DEBUG,
-			       "Read Counter modified in ftl_cmd_cnt %u"
-				" Block %u Counter%u\n",
-			       ftl_cmd_cnt, (unsigned int)Block,
-			       g_pReadCounter[Block -
-			       DeviceInfo.wSpectraStartBlock]);
-
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta_Free;
-		g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-		p_BTableChangesDelta->ftl_cmd_cnt = ftl_cmd_cnt;
-		p_BTableChangesDelta->RC_Index =
-			Block - DeviceInfo.wSpectraStartBlock;
-		p_BTableChangesDelta->RC_Entry_Value =
-			g_pReadCounter[Block - DeviceInfo.wSpectraStartBlock];
-		p_BTableChangesDelta->ValidFields = 0xC0;
-
-		ftl_cmd_cnt++;
-
-		if (g_pReadCounter[Block - DeviceInfo.wSpectraStartBlock] >=
-		    MAX_READ_COUNTER)
-			FTL_Read_Disturbance(Block);
-		if (g_cBlockTableStatus != IN_PROGRESS_BLOCK_TABLE) {
-			g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-			FTL_Write_IN_Progress_Block_Table_Page();
-		}
-	} else {
-		ftl_cmd_cnt++;
-	}
-#else
-	wResult = GLOB_LLD_Read_Page_Main(pData, Block, Page, PageCount);
-	if (wResult == FAIL)
-		return wResult;
-
-	if (DeviceInfo.MLCDevice) {
-		g_pReadCounter[Block - DeviceInfo.wSpectraStartBlock]++;
-		if (g_pReadCounter[Block - DeviceInfo.wSpectraStartBlock] >=
-						MAX_READ_COUNTER)
-			FTL_Read_Disturbance(Block);
-		if (g_cBlockTableStatus != IN_PROGRESS_BLOCK_TABLE) {
-			g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-			FTL_Write_IN_Progress_Block_Table_Page();
-		}
-	}
-#endif
-	return wResult;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Cache_Write_All
-* Inputs:       pointer to cache in sys memory
-*               address of free block in flash
-* Outputs:      PASS=0 / FAIL=1
-* Description:  writes all the pages of the block in cache to flash
-*
-*               NOTE:need to make sure this works ok when cache is limited
-*               to a partial block. This is where copy-back would be
-*               activated.  This would require knowing which pages in the
-*               cached block are clean/dirty.Right now we only know if
-*               the whole block is clean/dirty.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Cache_Write_All(u8 *pData, u64 blk_addr)
-{
-	u16 wResult = PASS;
-	u32 Block;
-	u16 Page;
-	u16 PageCount;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	nand_dbg_print(NAND_DBG_DEBUG, "This block %d going to be written "
-		"on %d\n", cache_block_to_write,
-		(u32)(blk_addr >> DeviceInfo.nBitsInBlockDataSize));
-
-	Block = BLK_FROM_ADDR(blk_addr);
-	Page = PAGE_FROM_ADDR(blk_addr, Block);
-	PageCount = Cache.pages_per_item;
-
-#if CMD_DMA
-	if (FAIL == GLOB_LLD_Write_Page_Main_cdma(pData,
-					Block, Page, PageCount)) {
-		nand_dbg_print(NAND_DBG_WARN,
-			"NAND Program fail in %s, Line %d, "
-			"Function: %s, new Bad Block %d generated! "
-			"Need Bad Block replacing.\n",
-			__FILE__, __LINE__, __func__, Block);
-		wResult = FAIL;
-	}
-	ftl_cmd_cnt++;
-#else
-	if (FAIL == GLOB_LLD_Write_Page_Main(pData, Block, Page, PageCount)) {
-		nand_dbg_print(NAND_DBG_WARN, "NAND Program fail in %s,"
-			" Line %d, Function %s, new Bad Block %d generated!"
-			"Need Bad Block replacing.\n",
-			__FILE__, __LINE__, __func__, Block);
-		wResult = FAIL;
-	}
-#endif
-	return wResult;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Copy_Block
-* Inputs:       source block address
-*               Destination block address
-* Outputs:      PASS=0 / FAIL=1
-* Description:  used only for static wear leveling to move the block
-*               containing static data to new blocks(more worn)
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int FTL_Copy_Block(u64 old_blk_addr, u64 blk_addr)
-{
-	int i, r1, r2, wResult = PASS;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	for (i = 0; i < DeviceInfo.wPagesPerBlock; i += Cache.pages_per_item) {
-		r1 = FTL_Cache_Read_All(g_pTempBuf, old_blk_addr +
-					i * DeviceInfo.wPageDataSize);
-		r2 = FTL_Cache_Write_All(g_pTempBuf, blk_addr +
-					i * DeviceInfo.wPageDataSize);
-		if ((ERR == r1) || (FAIL == r2)) {
-			wResult = FAIL;
-			break;
-		}
-	}
-
-	return wResult;
-}
-
-/* Search the block table to find out the least wear block and then return it */
-static u32 find_least_worn_blk_for_l2_cache(void)
-{
-	int i;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u8 least_wear_cnt = MAX_BYTE_VALUE;
-	u32 least_wear_blk_idx = MAX_U32_VALUE;
-	u32 phy_idx;
-
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-		if (IS_SPARE_BLOCK(i)) {
-			phy_idx = (u32)((~BAD_BLOCK) & pbt[i]);
-			if (phy_idx > DeviceInfo.wSpectraEndBlock)
-				printk(KERN_ERR "find_least_worn_blk_for_l2_cache: "
-					"Too big phy block num (%d)\n", phy_idx);
-			if (g_pWearCounter[phy_idx -DeviceInfo.wSpectraStartBlock] < least_wear_cnt) {
-				least_wear_cnt = g_pWearCounter[phy_idx - DeviceInfo.wSpectraStartBlock];
-				least_wear_blk_idx = i;
-			}
-		}
-	}
-
-	nand_dbg_print(NAND_DBG_WARN,
-		"find_least_worn_blk_for_l2_cache: "
-		"find block %d with least worn counter (%d)\n",
-		least_wear_blk_idx, least_wear_cnt);
-
-	return least_wear_blk_idx;
-}
-
-
-
-/* Get blocks for Level2 Cache */
-static int get_l2_cache_blks(void)
-{
-	int n;
-	u32 blk;
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	for (n = 0; n < BLK_NUM_FOR_L2_CACHE; n++) {
-		blk = find_least_worn_blk_for_l2_cache();
-		if (blk >= DeviceInfo.wDataBlockNum) {
-			nand_dbg_print(NAND_DBG_WARN,
-				"find_least_worn_blk_for_l2_cache: "
-				"No enough free NAND blocks (n: %d) for L2 Cache!\n", n);
-			return FAIL;
-		}
-		/* Tag the free block as discard in block table */
-		pbt[blk] = (pbt[blk] & (~BAD_BLOCK)) | DISCARD_BLOCK;
-		/* Add the free block to the L2 Cache block array */
-		cache_l2.blk_array[n] = pbt[blk] & (~BAD_BLOCK);
-	}
-
-	return PASS;
-}
-
-static int erase_l2_cache_blocks(void)
-{
-	int i, ret = PASS;
-	u32 pblk, lblk = BAD_BLOCK;
-	u64 addr;
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	for (i = 0; i < BLK_NUM_FOR_L2_CACHE; i++) {
-		pblk = cache_l2.blk_array[i];
-
-		/* If the L2 cache block is invalid, then just skip it */
-		if (MAX_U32_VALUE == pblk)
-			continue;
-
-		BUG_ON(pblk > DeviceInfo.wSpectraEndBlock);
-
-		addr = (u64)pblk << DeviceInfo.nBitsInBlockDataSize;
-		if (PASS == GLOB_FTL_Block_Erase(addr)) {
-			/* Get logical block number of the erased block */
-			lblk = FTL_Get_Block_Index(pblk);
-			BUG_ON(BAD_BLOCK == lblk);
-			/* Tag it as free in the block table */
-			pbt[lblk] &= (u32)(~DISCARD_BLOCK);
-			pbt[lblk] |= (u32)(SPARE_BLOCK);
-		} else {
-			MARK_BLOCK_AS_BAD(pbt[lblk]);
-			ret = ERR;
-		}
-	}
-
-	return ret;
-}
-
-/*
- * Merge the valid data page in the L2 cache blocks into NAND.
-*/
-static int flush_l2_cache(void)
-{
-	struct list_head *p;
-	struct spectra_l2_cache_list *pnd, *tmp_pnd;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 phy_blk, l2_blk;
-	u64 addr;
-	u16 l2_page;
-	int i, ret = PASS;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	if (list_empty(&cache_l2.table.list)) /* No data to flush */
-		return ret;
-
-	//dump_cache_l2_table();
-
-	if (IN_PROGRESS_BLOCK_TABLE != g_cBlockTableStatus) {
-		g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-		FTL_Write_IN_Progress_Block_Table_Page();
-	}
-
-	list_for_each(p, &cache_l2.table.list) {
-		pnd = list_entry(p, struct spectra_l2_cache_list, list);
-		if (IS_SPARE_BLOCK(pnd->logical_blk_num) ||
-			IS_BAD_BLOCK(pnd->logical_blk_num) ||
-			IS_DISCARDED_BLOCK(pnd->logical_blk_num)) {
-			nand_dbg_print(NAND_DBG_WARN, "%s, Line %d\n", __FILE__, __LINE__);
-			memset(cache_l2_blk_buf, 0xff, DeviceInfo.wPagesPerBlock * DeviceInfo.wPageDataSize);			
-		} else {
-			nand_dbg_print(NAND_DBG_WARN, "%s, Line %d\n", __FILE__, __LINE__);
-			phy_blk = pbt[pnd->logical_blk_num] & (~BAD_BLOCK);
-			ret = GLOB_LLD_Read_Page_Main(cache_l2_blk_buf,
-				phy_blk, 0, DeviceInfo.wPagesPerBlock);
-			if (ret == FAIL) {
-				printk(KERN_ERR "Read NAND page fail in %s, Line %d\n", __FILE__, __LINE__);
-			}
-		}
-
-		for (i = 0; i < DeviceInfo.wPagesPerBlock; i++) {
-			if (pnd->pages_array[i] != MAX_U32_VALUE) {
-				l2_blk = cache_l2.blk_array[(pnd->pages_array[i] >> 16) & 0xffff];
-				l2_page = pnd->pages_array[i] & 0xffff;
-				ret = GLOB_LLD_Read_Page_Main(cache_l2_page_buf, l2_blk, l2_page, 1);
-				if (ret == FAIL) {
-					printk(KERN_ERR "Read NAND page fail in %s, Line %d\n", __FILE__, __LINE__);
-				}
-				memcpy(cache_l2_blk_buf + i * DeviceInfo.wPageDataSize, cache_l2_page_buf, DeviceInfo.wPageDataSize);
-			}
-		}
-
-		/* Find a free block and tag the original block as discarded */
-		addr = (u64)pnd->logical_blk_num << DeviceInfo.nBitsInBlockDataSize;
-		ret = FTL_Replace_Block(addr);
-		if (ret == FAIL) {
-			printk(KERN_ERR "FTL_Replace_Block fail in %s, Line %d\n", __FILE__, __LINE__);
-		}
-
-		/* Write back the updated data into NAND */
-		phy_blk = pbt[pnd->logical_blk_num] & (~BAD_BLOCK);
-		if (FAIL == GLOB_LLD_Write_Page_Main(cache_l2_blk_buf, phy_blk, 0, DeviceInfo.wPagesPerBlock)) {
-			nand_dbg_print(NAND_DBG_WARN,
-				"Program NAND block %d fail in %s, Line %d\n",
-				phy_blk, __FILE__, __LINE__);
-			/* This may not be really a bad block. So just tag it as discarded. */
-			/* Then it has a chance to be erased when garbage collection. */
-			/* If it is really bad, then the erase will fail and it will be marked */
-			/* as bad then. Otherwise it will be marked as free and can be used again */
-			MARK_BLK_AS_DISCARD(pbt[pnd->logical_blk_num]);
-			/* Find another free block and write it again */
-			FTL_Replace_Block(addr);
-			phy_blk = pbt[pnd->logical_blk_num] & (~BAD_BLOCK);
-			if (FAIL == GLOB_LLD_Write_Page_Main(cache_l2_blk_buf, phy_blk, 0, DeviceInfo.wPagesPerBlock)) {
-				printk(KERN_ERR "Failed to write back block %d when flush L2 cache."
-					"Some data will be lost!\n", phy_blk);
-				MARK_BLOCK_AS_BAD(pbt[pnd->logical_blk_num]);
-			}
-		} else {
-			/* tag the new free block as used block */
-			pbt[pnd->logical_blk_num] &= (~SPARE_BLOCK);
-		}
-	}
-
-	/* Destroy the L2 Cache table and free the memory of all nodes */
-	list_for_each_entry_safe(pnd, tmp_pnd, &cache_l2.table.list, list) {
-		list_del(&pnd->list);
-		kfree(pnd);
-	}
-
-	/* Erase discard L2 cache blocks */
-	if (erase_l2_cache_blocks() != PASS)
-		nand_dbg_print(NAND_DBG_WARN,
-			" Erase L2 cache blocks error in %s, Line %d\n",
-			__FILE__, __LINE__);
-
-	/* Init the Level2 Cache data structure */
-	for (i = 0; i < BLK_NUM_FOR_L2_CACHE; i++)
-		cache_l2.blk_array[i] = MAX_U32_VALUE;
-	cache_l2.cur_blk_idx = 0;
-	cache_l2.cur_page_num = 0;
-	INIT_LIST_HEAD(&cache_l2.table.list);
-	cache_l2.table.logical_blk_num = MAX_U32_VALUE;
-
-	return ret;
-}
-
-/*
- * Write back a changed victim cache item to the Level2 Cache
- * and update the L2 Cache table to map the change.
- * If the L2 Cache is full, then start to do the L2 Cache flush.
-*/
-static int write_back_to_l2_cache(u8 *buf, u64 logical_addr)
-{
-	u32 logical_blk_num;
-	u16 logical_page_num;
-	struct list_head *p;
-	struct spectra_l2_cache_list *pnd, *pnd_new;
-	u32 node_size;
-	int i, found;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	/*
-	 * If Level2 Cache table is empty, then it means either:
-	 * 1. This is the first time that the function called after FTL_init
-	 * or
-	 * 2. The Level2 Cache has just been flushed
-	 *
-	 * So, 'steal' some free blocks from NAND for L2 Cache using
-	 * by just mask them as discard in the block table
-	*/
-	if (list_empty(&cache_l2.table.list)) {
-		BUG_ON(cache_l2.cur_blk_idx != 0);
-		BUG_ON(cache_l2.cur_page_num!= 0);
-		BUG_ON(cache_l2.table.logical_blk_num != MAX_U32_VALUE);
-		if (FAIL == get_l2_cache_blks()) {
-			GLOB_FTL_Garbage_Collection();
-			if (FAIL == get_l2_cache_blks()) {
-				printk(KERN_ALERT "Fail to get L2 cache blks!\n");
-				return FAIL;
-			}
-		}
-	}
-
-	logical_blk_num = BLK_FROM_ADDR(logical_addr);
-	logical_page_num = PAGE_FROM_ADDR(logical_addr, logical_blk_num);
-	BUG_ON(logical_blk_num == MAX_U32_VALUE);
-
-	/* Write the cache item data into the current position of L2 Cache */
-#if CMD_DMA
-	/*
-	 * TODO
-	 */
-#else
-	if (FAIL == GLOB_LLD_Write_Page_Main(buf,
-		cache_l2.blk_array[cache_l2.cur_blk_idx],
-		cache_l2.cur_page_num, 1)) {
-		nand_dbg_print(NAND_DBG_WARN, "NAND Program fail in "
-			"%s, Line %d, new Bad Block %d generated!\n",
-			__FILE__, __LINE__,
-			cache_l2.blk_array[cache_l2.cur_blk_idx]);
-
-		/* TODO: tag the current block as bad and try again */
-
-		return FAIL;
-	}
-#endif
-
-	/* 
-	 * Update the L2 Cache table.
-	 *
-	 * First seaching in the table to see whether the logical block
-	 * has been mapped. If not, then kmalloc a new node for the
-	 * logical block, fill data, and then insert it to the list.
-	 * Otherwise, just update the mapped node directly.
-	 */
-	found = 0;
-	list_for_each(p, &cache_l2.table.list) {
-		pnd = list_entry(p, struct spectra_l2_cache_list, list);
-		if (pnd->logical_blk_num == logical_blk_num) {
-			pnd->pages_array[logical_page_num] =
-				(cache_l2.cur_blk_idx << 16) |
-				cache_l2.cur_page_num;
-			found = 1;
-			break;
-		}
-	}
-	if (!found) { /* Create new node for the logical block here */
-
-		/* The logical pages to physical pages map array is
-		 * located at the end of struct spectra_l2_cache_list.
-		 */ 
-		node_size = sizeof(struct spectra_l2_cache_list) +
-			sizeof(u32) * DeviceInfo.wPagesPerBlock;
-		pnd_new = kmalloc(node_size, GFP_ATOMIC);
-		if (!pnd_new) {
-			printk(KERN_ERR "Failed to kmalloc in %s Line %d\n",
-				__FILE__, __LINE__);
-			/* 
-			 * TODO: Need to flush all the L2 cache into NAND ASAP
-			 * since no memory available here
-			 */
-		}
-		pnd_new->logical_blk_num = logical_blk_num;
-		for (i = 0; i < DeviceInfo.wPagesPerBlock; i++)
-			pnd_new->pages_array[i] = MAX_U32_VALUE;
-		pnd_new->pages_array[logical_page_num] =
-			(cache_l2.cur_blk_idx << 16) | cache_l2.cur_page_num;
-		list_add(&pnd_new->list, &cache_l2.table.list);
-	}
-
-	/* Increasing the current position pointer of the L2 Cache */
-	cache_l2.cur_page_num++;
-	if (cache_l2.cur_page_num >= DeviceInfo.wPagesPerBlock) {
-		cache_l2.cur_blk_idx++;
-		if (cache_l2.cur_blk_idx >= BLK_NUM_FOR_L2_CACHE) {
-			/* The L2 Cache is full. Need to flush it now */
-			nand_dbg_print(NAND_DBG_WARN,
-				"L2 Cache is full, will start to flush it\n");
-			flush_l2_cache();
-		} else {
-			cache_l2.cur_page_num = 0;
-		}
-	}
-
-	return PASS;
-}
-
-/*
- * Search in the Level2 Cache table to find the cache item.
- * If find, read the data from the NAND page of L2 Cache,
- * Otherwise, return FAIL.
- */
-static int search_l2_cache(u8 *buf, u64 logical_addr)
-{
-	u32 logical_blk_num;
-	u16 logical_page_num;
-	struct list_head *p;
-	struct spectra_l2_cache_list *pnd;
-	u32 tmp = MAX_U32_VALUE;
-	u32 phy_blk;
-	u16 phy_page;
-	int ret = FAIL;
-
-	logical_blk_num = BLK_FROM_ADDR(logical_addr);
-	logical_page_num = PAGE_FROM_ADDR(logical_addr, logical_blk_num);
-
-	list_for_each(p, &cache_l2.table.list) {
-		pnd = list_entry(p, struct spectra_l2_cache_list, list);
-		if (pnd->logical_blk_num == logical_blk_num) {
-			tmp = pnd->pages_array[logical_page_num];
-			break;
-		}
-	}
-
-	if (tmp != MAX_U32_VALUE) { /* Found valid map */
-		phy_blk = cache_l2.blk_array[(tmp >> 16) & 0xFFFF];
-		phy_page = tmp & 0xFFFF;
-#if CMD_DMA
-		/* TODO */
-#else
-		ret = GLOB_LLD_Read_Page_Main(buf, phy_blk, phy_page, 1);
-#endif
-	}
-
-	return ret;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Cache_Write_Page
-* Inputs:       Pointer to buffer, page address, cache block number
-* Outputs:      PASS=0 / FAIL=1
-* Description:  It writes the data in Cache Block
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static void FTL_Cache_Write_Page(u8 *pData, u64 page_addr,
-				u8 cache_blk, u16 flag)
-{
-	u8 *pDest;
-	u64 addr;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	addr = Cache.array[cache_blk].address;
-	pDest = Cache.array[cache_blk].buf;
-
-	pDest += (unsigned long)(page_addr - addr);
-	Cache.array[cache_blk].changed = SET;
-#if CMD_DMA
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	int_cache[ftl_cmd_cnt].item = cache_blk;
-	int_cache[ftl_cmd_cnt].cache.address =
-			Cache.array[cache_blk].address;
-	int_cache[ftl_cmd_cnt].cache.changed =
-			Cache.array[cache_blk].changed;
-#endif
-	GLOB_LLD_MemCopy_CMD(pDest, pData, DeviceInfo.wPageDataSize, flag);
-	ftl_cmd_cnt++;
-#else
-	memcpy(pDest, pData, DeviceInfo.wPageDataSize);
-#endif
-	if (Cache.array[cache_blk].use_cnt < MAX_WORD_VALUE)
-		Cache.array[cache_blk].use_cnt++;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Cache_Write
-* Inputs:       none
-* Outputs:      PASS=0 / FAIL=1
-* Description:  It writes least frequently used Cache block to flash if it
-*               has been changed
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Cache_Write(void)
-{
-	int i, bResult = PASS;
-	u16 bNO, least_count = 0xFFFF;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	FTL_Calculate_LRU();
-
-	bNO = Cache.LRU;
-	nand_dbg_print(NAND_DBG_DEBUG, "FTL_Cache_Write: "
-		"Least used cache block is %d\n", bNO);
-
-	if (Cache.array[bNO].changed != SET)
-		return bResult;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "FTL_Cache_Write: Cache"
-		" Block %d containing logical block %d is dirty\n",
-		bNO,
-		(u32)(Cache.array[bNO].address >>
-		DeviceInfo.nBitsInBlockDataSize));
-#if CMD_DMA
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	int_cache[ftl_cmd_cnt].item = bNO;
-	int_cache[ftl_cmd_cnt].cache.address =
-				Cache.array[bNO].address;
-	int_cache[ftl_cmd_cnt].cache.changed = CLEAR;
-#endif
-#endif
-	bResult = write_back_to_l2_cache(Cache.array[bNO].buf,
-			Cache.array[bNO].address);
-	if (bResult != ERR)
-		Cache.array[bNO].changed = CLEAR;
-
-	least_count = Cache.array[bNO].use_cnt;
-
-	for (i = 0; i < CACHE_ITEM_NUM; i++) {
-		if (i == bNO)
-			continue;
-		if (Cache.array[i].use_cnt > 0)
-			Cache.array[i].use_cnt -= least_count;
-	}
-
-	return bResult;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Cache_Read
-* Inputs:       Page address
-* Outputs:      PASS=0 / FAIL=1
-* Description:  It reads the block from device in Cache Block
-*               Set the LRU count to 1
-*               Mark the Cache Block as clean
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Cache_Read(u64 logical_addr)
-{
-	u64 item_addr, phy_addr;
-	u16 num;
-	int ret;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	num = Cache.LRU; /* The LRU cache item will be overwritten */
-
-	item_addr = (u64)GLOB_u64_Div(logical_addr, Cache.cache_item_size) *
-		Cache.cache_item_size;
-	Cache.array[num].address = item_addr;
-	Cache.array[num].use_cnt = 1;
-	Cache.array[num].changed = CLEAR;
-
-#if CMD_DMA
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-	int_cache[ftl_cmd_cnt].item = num;
-	int_cache[ftl_cmd_cnt].cache.address =
-			Cache.array[num].address;
-	int_cache[ftl_cmd_cnt].cache.changed =
-			Cache.array[num].changed;
-#endif
-#endif
-	/*
-	 * Search in L2 Cache. If hit, fill data into L1 Cache item buffer,
-	 * Otherwise, read it from NAND
-	 */
-	ret = search_l2_cache(Cache.array[num].buf, logical_addr);
-	if (PASS == ret) /* Hit in L2 Cache */
-		return ret;
-
-	/* Compute the physical start address of NAND device according to */
-	/* the logical start address of the cache item (LRU cache item) */
-	phy_addr = FTL_Get_Physical_Block_Addr(item_addr) +
-		GLOB_u64_Remainder(item_addr, 2);
-
-	return FTL_Cache_Read_All(Cache.array[num].buf, phy_addr);
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Check_Block_Table
-* Inputs:       ?
-* Outputs:      PASS=0 / FAIL=1
-* Description:  It checks the correctness of each block table entry
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Check_Block_Table(int wOldTable)
-{
-	u32 i;
-	int wResult = PASS;
-	u32 blk_idx;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u8 *pFlag = flag_check_blk_table;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (NULL != pFlag) {
-		memset(pFlag, FAIL, DeviceInfo.wDataBlockNum);
-		for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-			blk_idx = (u32)(pbt[i] & (~BAD_BLOCK));
-
-			/*
-			 * 20081006/KBV - Changed to pFlag[i] reference
-			 * to avoid buffer overflow
-			 */
-
-			/*
-			 * 2008-10-20 Yunpeng Note: This change avoid
-			 * buffer overflow, but changed function of
-			 * the code, so it should be re-write later
-			 */
-			if ((blk_idx > DeviceInfo.wSpectraEndBlock) ||
-				PASS == pFlag[i]) {
-				wResult = FAIL;
-				break;
-			} else {
-				pFlag[i] = PASS;
-			}
-		}
-	}
-
-	return wResult;
-}
-
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Write_Block_Table
-* Inputs:       flasg
-* Outputs:      0=Block Table was updated. No write done. 1=Block write needs to
-* happen. -1 Error
-* Description:  It writes the block table
-*               Block table always mapped to LBA 0 which inturn mapped
-*               to any physical block
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Write_Block_Table(int wForce)
-{
-	u32 *pbt = (u32 *)g_pBlockTable;
-	int wSuccess = PASS;
-	u32 wTempBlockTableIndex;
-	u16 bt_pages, new_bt_offset;
-	u8 blockchangeoccured = 0;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	bt_pages = FTL_Get_Block_Table_Flash_Size_Pages();
-
-	if (IN_PROGRESS_BLOCK_TABLE != g_cBlockTableStatus)
-		return 0;
-
-	if (PASS == wForce) {
-		g_wBlockTableOffset =
-			(u16)(DeviceInfo.wPagesPerBlock - bt_pages);
-#if CMD_DMA
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta_Free;
-		g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-
-		p_BTableChangesDelta->ftl_cmd_cnt = ftl_cmd_cnt;
-		p_BTableChangesDelta->g_wBlockTableOffset =
-			g_wBlockTableOffset;
-		p_BTableChangesDelta->ValidFields = 0x01;
-#endif
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		"Inside FTL_Write_Block_Table: block %d Page:%d\n",
-		g_wBlockTableIndex, g_wBlockTableOffset);
-
-	do {
-		new_bt_offset = g_wBlockTableOffset + bt_pages + 1;
-		if ((0 == (new_bt_offset % DeviceInfo.wPagesPerBlock)) ||
-			(new_bt_offset > DeviceInfo.wPagesPerBlock) ||
-			(FAIL == wSuccess)) {
-			wTempBlockTableIndex = FTL_Replace_Block_Table();
-			if (BAD_BLOCK == wTempBlockTableIndex)
-				return ERR;
-			if (!blockchangeoccured) {
-				bt_block_changed = 1;
-				blockchangeoccured = 1;
-			}
-
-			g_wBlockTableIndex = wTempBlockTableIndex;
-			g_wBlockTableOffset = 0;
-			pbt[BLOCK_TABLE_INDEX] = g_wBlockTableIndex;
-#if CMD_DMA
-			p_BTableChangesDelta =
-				(struct BTableChangesDelta *)g_pBTDelta_Free;
-			g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-
-			p_BTableChangesDelta->ftl_cmd_cnt =
-				    ftl_cmd_cnt;
-			p_BTableChangesDelta->g_wBlockTableOffset =
-				    g_wBlockTableOffset;
-			p_BTableChangesDelta->g_wBlockTableIndex =
-				    g_wBlockTableIndex;
-			p_BTableChangesDelta->ValidFields = 0x03;
-
-			p_BTableChangesDelta =
-				(struct BTableChangesDelta *)g_pBTDelta_Free;
-			g_pBTDelta_Free +=
-				sizeof(struct BTableChangesDelta);
-
-			p_BTableChangesDelta->ftl_cmd_cnt =
-				    ftl_cmd_cnt;
-			p_BTableChangesDelta->BT_Index =
-				    BLOCK_TABLE_INDEX;
-			p_BTableChangesDelta->BT_Entry_Value =
-				    pbt[BLOCK_TABLE_INDEX];
-			p_BTableChangesDelta->ValidFields = 0x0C;
-#endif
-		}
-
-		wSuccess = FTL_Write_Block_Table_Data();
-		if (FAIL == wSuccess)
-			MARK_BLOCK_AS_BAD(pbt[BLOCK_TABLE_INDEX]);
-	} while (FAIL == wSuccess);
-
-	g_cBlockTableStatus = CURRENT_BLOCK_TABLE;
-
-	return 1;
-}
-
-static int  force_format_nand(void)
-{
-	u32 i;
-
-	/* Force erase the whole unprotected physical partiton of NAND */
-	printk(KERN_ALERT "Start to force erase whole NAND device ...\n");
-	printk(KERN_ALERT "From phyical block %d to %d\n",
-		DeviceInfo.wSpectraStartBlock, DeviceInfo.wSpectraEndBlock);
-	for (i = DeviceInfo.wSpectraStartBlock; i <= DeviceInfo.wSpectraEndBlock; i++) {
-		if (GLOB_LLD_Erase_Block(i))
-			printk(KERN_ERR "Failed to force erase NAND block %d\n", i);
-	}
-	printk(KERN_ALERT "Force Erase ends. Please reboot the system ...\n");
-	while(1);
-
-	return PASS;
-}
-
-int GLOB_FTL_Flash_Format(void)
-{
-	//return FTL_Format_Flash(1);
-	return force_format_nand();
-
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Search_Block_Table_IN_Block
-* Inputs:       Block Number
-*               Pointer to page
-* Outputs:      PASS / FAIL
-*               Page contatining the block table
-* Description:  It searches the block table in the block
-*               passed as an argument.
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Search_Block_Table_IN_Block(u32 BT_Block,
-						u8 BT_Tag, u16 *Page)
-{
-	u16 i, j, k;
-	u16 Result = PASS;
-	u16 Last_IPF = 0;
-	u8  BT_Found = 0;
-	u8 *tagarray;
-	u8 *tempbuf = tmp_buf_search_bt_in_block;
-	u8 *pSpareBuf = spare_buf_search_bt_in_block;
-	u8 *pSpareBufBTLastPage = spare_buf_bt_search_bt_in_block;
-	u8 bt_flag_last_page = 0xFF;
-	u8 search_in_previous_pages = 0;
-	u16 bt_pages;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		       "Searching block table in %u block\n",
-		       (unsigned int)BT_Block);
-
-	bt_pages = FTL_Get_Block_Table_Flash_Size_Pages();
-
-	for (i = bt_pages; i < DeviceInfo.wPagesPerBlock;
-				i += (bt_pages + 1)) {
-		nand_dbg_print(NAND_DBG_DEBUG,
-			       "Searching last IPF: %d\n", i);
-		Result = GLOB_LLD_Read_Page_Main_Polling(tempbuf,
-							BT_Block, i, 1);
-
-		if (0 == memcmp(tempbuf, g_pIPF, DeviceInfo.wPageDataSize)) {
-			if ((i + bt_pages + 1) < DeviceInfo.wPagesPerBlock) {
-				continue;
-			} else {
-				search_in_previous_pages = 1;
-				Last_IPF = i;
-			}
-		}
-
-		if (!search_in_previous_pages) {
-			if (i != bt_pages) {
-				i -= (bt_pages + 1);
-				Last_IPF = i;
-			}
-		}
-
-		if (0 == Last_IPF)
-			break;
-
-		if (!search_in_previous_pages) {
-			i = i + 1;
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Reading the spare area of Block %u Page %u",
-				(unsigned int)BT_Block, i);
-			Result = GLOB_LLD_Read_Page_Spare(pSpareBuf,
-							BT_Block, i, 1);
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Reading the spare area of Block %u Page %u",
-				(unsigned int)BT_Block, i + bt_pages - 1);
-			Result = GLOB_LLD_Read_Page_Spare(pSpareBufBTLastPage,
-				BT_Block, i + bt_pages - 1, 1);
-
-			k = 0;
-			j = FTL_Extract_Block_Table_Tag(pSpareBuf, &tagarray);
-			if (j) {
-				for (; k < j; k++) {
-					if (tagarray[k] == BT_Tag)
-						break;
-				}
-			}
-
-			if (k < j)
-				bt_flag = tagarray[k];
-			else
-				Result = FAIL;
-
-			if (Result == PASS) {
-				k = 0;
-				j = FTL_Extract_Block_Table_Tag(
-					pSpareBufBTLastPage, &tagarray);
-				if (j) {
-					for (; k < j; k++) {
-						if (tagarray[k] == BT_Tag)
-							break;
-					}
-				}
-
-				if (k < j)
-					bt_flag_last_page = tagarray[k];
-				else
-					Result = FAIL;
-
-				if (Result == PASS) {
-					if (bt_flag == bt_flag_last_page) {
-						nand_dbg_print(NAND_DBG_DEBUG,
-							"Block table is found"
-							" in page after IPF "
-							"at block %d "
-							"page %d\n",
-							(int)BT_Block, i);
-						BT_Found = 1;
-						*Page  = i;
-						g_cBlockTableStatus =
-							CURRENT_BLOCK_TABLE;
-						break;
-					} else {
-						Result = FAIL;
-					}
-				}
-			}
-		}
-
-		if (search_in_previous_pages)
-			i = i - bt_pages;
-		else
-			i = i - (bt_pages + 1);
-
-		Result = PASS;
-
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Reading the spare area of Block %d Page %d",
-			(int)BT_Block, i);
-
-		Result = GLOB_LLD_Read_Page_Spare(pSpareBuf, BT_Block, i, 1);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Reading the spare area of Block %u Page %u",
-			(unsigned int)BT_Block, i + bt_pages - 1);
-
-		Result = GLOB_LLD_Read_Page_Spare(pSpareBufBTLastPage,
-					BT_Block, i + bt_pages - 1, 1);
-
-		k = 0;
-		j = FTL_Extract_Block_Table_Tag(pSpareBuf, &tagarray);
-		if (j) {
-			for (; k < j; k++) {
-				if (tagarray[k] == BT_Tag)
-					break;
-			}
-		}
-
-		if (k < j)
-			bt_flag = tagarray[k];
-		else
-			Result = FAIL;
-
-		if (Result == PASS) {
-			k = 0;
-			j = FTL_Extract_Block_Table_Tag(pSpareBufBTLastPage,
-						&tagarray);
-			if (j) {
-				for (; k < j; k++) {
-					if (tagarray[k] == BT_Tag)
-						break;
-				}
-			}
-
-			if (k < j) {
-				bt_flag_last_page = tagarray[k];
-			} else {
-				Result = FAIL;
-				break;
-			}
-
-			if (Result == PASS) {
-				if (bt_flag == bt_flag_last_page) {
-					nand_dbg_print(NAND_DBG_DEBUG,
-						"Block table is found "
-						"in page prior to IPF "
-						"at block %u page %d\n",
-						(unsigned int)BT_Block, i);
-					BT_Found = 1;
-					*Page  = i;
-					g_cBlockTableStatus =
-						IN_PROGRESS_BLOCK_TABLE;
-					break;
-				} else {
-					Result = FAIL;
-					break;
-				}
-			}
-		}
-	}
-
-	if (Result == FAIL) {
-		if ((Last_IPF > bt_pages) && (i < Last_IPF) && (!BT_Found)) {
-			BT_Found = 1;
-			*Page = i - (bt_pages + 1);
-		}
-		if ((Last_IPF == bt_pages) && (i < Last_IPF) && (!BT_Found))
-			goto func_return;
-	}
-
-	if (Last_IPF == 0) {
-		i = 0;
-		Result = PASS;
-		nand_dbg_print(NAND_DBG_DEBUG, "Reading the spare area of "
-			"Block %u Page %u", (unsigned int)BT_Block, i);
-
-		Result = GLOB_LLD_Read_Page_Spare(pSpareBuf, BT_Block, i, 1);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Reading the spare area of Block %u Page %u",
-			(unsigned int)BT_Block, i + bt_pages - 1);
-		Result = GLOB_LLD_Read_Page_Spare(pSpareBufBTLastPage,
-					BT_Block, i + bt_pages - 1, 1);
-
-		k = 0;
-		j = FTL_Extract_Block_Table_Tag(pSpareBuf, &tagarray);
-		if (j) {
-			for (; k < j; k++) {
-				if (tagarray[k] == BT_Tag)
-					break;
-			}
-		}
-
-		if (k < j)
-			bt_flag = tagarray[k];
-		else
-			Result = FAIL;
-
-		if (Result == PASS) {
-			k = 0;
-			j = FTL_Extract_Block_Table_Tag(pSpareBufBTLastPage,
-							&tagarray);
-			if (j) {
-				for (; k < j; k++) {
-					if (tagarray[k] == BT_Tag)
-						break;
-				}
-			}
-
-			if (k < j)
-				bt_flag_last_page = tagarray[k];
-			else
-				Result = FAIL;
-
-			if (Result == PASS) {
-				if (bt_flag == bt_flag_last_page) {
-					nand_dbg_print(NAND_DBG_DEBUG,
-						"Block table is found "
-						"in page after IPF at "
-						"block %u page %u\n",
-						(unsigned int)BT_Block,
-						(unsigned int)i);
-					BT_Found = 1;
-					*Page  = i;
-					g_cBlockTableStatus =
-						CURRENT_BLOCK_TABLE;
-					goto func_return;
-				} else {
-					Result = FAIL;
-				}
-			}
-		}
-
-		if (Result == FAIL)
-			goto func_return;
-	}
-func_return:
-	return Result;
-}
-
-u8 *get_blk_table_start_addr(void)
-{
-	return g_pBlockTable;
-}
-
-unsigned long get_blk_table_len(void)
-{
-	return DeviceInfo.wDataBlockNum * sizeof(u32);
-}
-
-u8 *get_wear_leveling_table_start_addr(void)
-{
-	return g_pWearCounter;
-}
-
-unsigned long get_wear_leveling_table_len(void)
-{
-	return DeviceInfo.wDataBlockNum * sizeof(u8);
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Read_Block_Table
-* Inputs:       none
-* Outputs:      PASS / FAIL
-* Description:  read the flash spare area and find a block containing the
-*               most recent block table(having largest block_table_counter).
-*               Find the last written Block table in this block.
-*               Check the correctness of Block Table
-*               If CDMA is enabled, this function is called in
-*               polling mode.
-*               We don't need to store changes in Block table in this
-*               function as it is called only at initialization
-*
-*               Note: Currently this function is called at initialization
-*               before any read/erase/write command issued to flash so,
-*               there is no need to wait for CDMA list to complete as of now
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Read_Block_Table(void)
-{
-	u16 i = 0;
-	int k, j;
-	u8 *tempBuf, *tagarray;
-	int wResult = FAIL;
-	int status = FAIL;
-	u8 block_table_found = 0;
-	int search_result;
-	u32 Block;
-	u16 Page = 0;
-	u16 PageCount;
-	u16 bt_pages;
-	int wBytesCopied = 0, tempvar;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	tempBuf = tmp_buf1_read_blk_table;
-	bt_pages = FTL_Get_Block_Table_Flash_Size_Pages();
-
-	for (j = DeviceInfo.wSpectraStartBlock;
-		j <= (int)DeviceInfo.wSpectraEndBlock;
-			j++) {
-		status = GLOB_LLD_Read_Page_Spare(tempBuf, j, 0, 1);
-		k = 0;
-		i = FTL_Extract_Block_Table_Tag(tempBuf, &tagarray);
-		if (i) {
-			status  = GLOB_LLD_Read_Page_Main_Polling(tempBuf,
-								j, 0, 1);
-			for (; k < i; k++) {
-				if (tagarray[k] == tempBuf[3])
-					break;
-			}
-		}
-
-		if (k < i)
-			k = tagarray[k];
-		else
-			continue;
-
-		nand_dbg_print(NAND_DBG_DEBUG,
-				"Block table is contained in Block %d %d\n",
-				       (unsigned int)j, (unsigned int)k);
-
-		if (g_pBTBlocks[k-FIRST_BT_ID] == BTBLOCK_INVAL) {
-			g_pBTBlocks[k-FIRST_BT_ID] = j;
-			block_table_found = 1;
-		} else {
-			printk(KERN_ERR "FTL_Read_Block_Table -"
-				"This should never happens. "
-				"Two block table have same counter %u!\n", k);
-		}
-	}
-
-	if (block_table_found) {
-		if (g_pBTBlocks[FIRST_BT_ID - FIRST_BT_ID] != BTBLOCK_INVAL &&
-		g_pBTBlocks[LAST_BT_ID - FIRST_BT_ID] != BTBLOCK_INVAL) {
-			j = LAST_BT_ID;
-			while ((j > FIRST_BT_ID) &&
-			(g_pBTBlocks[j - FIRST_BT_ID] != BTBLOCK_INVAL))
-				j--;
-			if (j == FIRST_BT_ID) {
-				j = LAST_BT_ID;
-				last_erased = LAST_BT_ID;
-			} else {
-				last_erased = (u8)j + 1;
-				while ((j > FIRST_BT_ID) && (BTBLOCK_INVAL ==
-					g_pBTBlocks[j - FIRST_BT_ID]))
-					j--;
-			}
-		} else {
-			j = FIRST_BT_ID;
-			while (g_pBTBlocks[j - FIRST_BT_ID] == BTBLOCK_INVAL)
-				j++;
-			last_erased = (u8)j;
-			while ((j < LAST_BT_ID) && (BTBLOCK_INVAL !=
-				g_pBTBlocks[j - FIRST_BT_ID]))
-				j++;
-			if (g_pBTBlocks[j-FIRST_BT_ID] == BTBLOCK_INVAL)
-				j--;
-		}
-
-		if (last_erased > j)
-			j += (1 + LAST_BT_ID - FIRST_BT_ID);
-
-		for (; (j >= last_erased) && (FAIL == wResult); j--) {
-			i = (j - FIRST_BT_ID) %
-				(1 + LAST_BT_ID - FIRST_BT_ID);
-			search_result =
-			FTL_Search_Block_Table_IN_Block(g_pBTBlocks[i],
-						i + FIRST_BT_ID, &Page);
-			if (g_cBlockTableStatus == IN_PROGRESS_BLOCK_TABLE)
-				block_table_found = 0;
-
-			while ((search_result == PASS) && (FAIL == wResult)) {
-				nand_dbg_print(NAND_DBG_DEBUG,
-					"FTL_Read_Block_Table:"
-					"Block: %u Page: %u "
-					"contains block table\n",
-					(unsigned int)g_pBTBlocks[i],
-					(unsigned int)Page);
-
-				tempBuf = tmp_buf2_read_blk_table;
-
-				for (k = 0; k < bt_pages; k++) {
-					Block = g_pBTBlocks[i];
-					PageCount = 1;
-
-					status  =
-					GLOB_LLD_Read_Page_Main_Polling(
-					tempBuf, Block, Page, PageCount);
-
-					tempvar = k ? 0 : 4;
-
-					wBytesCopied +=
-					FTL_Copy_Block_Table_From_Flash(
-					tempBuf + tempvar,
-					DeviceInfo.wPageDataSize - tempvar,
-					wBytesCopied);
-
-					Page++;
-				}
-
-				wResult = FTL_Check_Block_Table(FAIL);
-				if (FAIL == wResult) {
-					block_table_found = 0;
-					if (Page > bt_pages)
-						Page -= ((bt_pages<<1) + 1);
-					else
-						search_result = FAIL;
-				}
-			}
-		}
-	}
-
-	if (PASS == wResult) {
-		if (!block_table_found)
-			FTL_Execute_SPL_Recovery();
-
-		if (g_cBlockTableStatus == IN_PROGRESS_BLOCK_TABLE)
-			g_wBlockTableOffset = (u16)Page + 1;
-		else
-			g_wBlockTableOffset = (u16)Page - bt_pages;
-
-		g_wBlockTableIndex = (u32)g_pBTBlocks[i];
-
-#if CMD_DMA
-		if (DeviceInfo.MLCDevice)
-			memcpy(g_pBTStartingCopy, g_pBlockTable,
-				DeviceInfo.wDataBlockNum * sizeof(u32)
-				+ DeviceInfo.wDataBlockNum * sizeof(u8)
-				+ DeviceInfo.wDataBlockNum * sizeof(u16));
-		else
-			memcpy(g_pBTStartingCopy, g_pBlockTable,
-				DeviceInfo.wDataBlockNum * sizeof(u32)
-				+ DeviceInfo.wDataBlockNum * sizeof(u8));
-#endif
-	}
-
-	if (FAIL == wResult)
-		printk(KERN_ERR "Yunpeng - "
-		"Can not find valid spectra block table!\n");
-
-#if AUTO_FORMAT_FLASH
-	if (FAIL == wResult) {
-		nand_dbg_print(NAND_DBG_DEBUG, "doing auto-format\n");
-		wResult = FTL_Format_Flash(0);
-	}
-#endif
-
-	return wResult;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Get_Page_Num
-* Inputs:       Size in bytes
-* Outputs:      Size in pages
-* Description:  It calculates the pages required for the length passed
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static u32 FTL_Get_Page_Num(u64 length)
-{
-	return (u32)((length >> DeviceInfo.nBitsInPageDataSize) +
-		(GLOB_u64_Remainder(length , 1) > 0 ? 1 : 0));
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Get_Physical_Block_Addr
-* Inputs:       Block Address (byte format)
-* Outputs:      Physical address of the block.
-* Description:  It translates LBA to PBA by returning address stored
-*               at the LBA location in the block table
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static u64 FTL_Get_Physical_Block_Addr(u64 logical_addr)
-{
-	u32 *pbt;
-	u64 physical_addr;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	pbt = (u32 *)g_pBlockTable;
-	physical_addr = (u64) DeviceInfo.wBlockDataSize *
-		(pbt[BLK_FROM_ADDR(logical_addr)] & (~BAD_BLOCK));
-
-	return physical_addr;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Get_Block_Index
-* Inputs:       Physical Block no.
-* Outputs:      Logical block no. /BAD_BLOCK
-* Description:  It returns the logical block no. for the PBA passed
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static u32 FTL_Get_Block_Index(u32 wBlockNum)
-{
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++)
-		if (wBlockNum == (pbt[i] & (~BAD_BLOCK)))
-			return i;
-
-	return BAD_BLOCK;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Wear_Leveling
-* Inputs:       none
-* Outputs:      PASS=0
-* Description:  This is static wear leveling (done by explicit call)
-*               do complete static wear leveling
-*               do complete garbage collection
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Wear_Leveling(void)
-{
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	FTL_Static_Wear_Leveling();
-	GLOB_FTL_Garbage_Collection();
-
-	return PASS;
-}
-
-static void find_least_most_worn(u8 *chg,
-	u32 *least_idx, u8 *least_cnt,
-	u32 *most_idx, u8 *most_cnt)
-{
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 idx;
-	u8 cnt;
-	int i;
-
-	for (i = BLOCK_TABLE_INDEX + 1; i < DeviceInfo.wDataBlockNum; i++) {
-		if (IS_BAD_BLOCK(i) || PASS == chg[i])
-			continue;
-
-		idx = (u32) ((~BAD_BLOCK) & pbt[i]);
-		cnt = g_pWearCounter[idx - DeviceInfo.wSpectraStartBlock];
-
-		if (IS_SPARE_BLOCK(i)) {
-			if (cnt > *most_cnt) {
-				*most_cnt = cnt;
-				*most_idx = idx;
-			}
-		}
-
-		if (IS_DATA_BLOCK(i)) {
-			if (cnt < *least_cnt) {
-				*least_cnt = cnt;
-				*least_idx = idx;
-			}
-		}
-
-		if (PASS == chg[*most_idx] || PASS == chg[*least_idx]) {
-			debug_boundary_error(*most_idx,
-				DeviceInfo.wDataBlockNum, 0);
-			debug_boundary_error(*least_idx,
-				DeviceInfo.wDataBlockNum, 0);
-			continue;
-		}
-	}
-}
-
-static int move_blks_for_wear_leveling(u8 *chg,
-	u32 *least_idx, u32 *rep_blk_num, int *result)
-{
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 rep_blk;
-	int j, ret_cp_blk, ret_erase;
-	int ret = PASS;
-
-	chg[*least_idx] = PASS;
-	debug_boundary_error(*least_idx, DeviceInfo.wDataBlockNum, 0);
-
-	rep_blk = FTL_Replace_MWBlock();
-	if (rep_blk != BAD_BLOCK) {
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"More than two spare blocks exist so do it\n");
-		nand_dbg_print(NAND_DBG_DEBUG, "Block Replaced is %d\n",
-				rep_blk);
-
-		chg[rep_blk] = PASS;
-
-		if (IN_PROGRESS_BLOCK_TABLE != g_cBlockTableStatus) {
-			g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-			FTL_Write_IN_Progress_Block_Table_Page();
-		}
-
-		for (j = 0; j < RETRY_TIMES; j++) {
-			ret_cp_blk = FTL_Copy_Block((u64)(*least_idx) *
-				DeviceInfo.wBlockDataSize,
-				(u64)rep_blk * DeviceInfo.wBlockDataSize);
-			if (FAIL == ret_cp_blk) {
-				ret_erase = GLOB_FTL_Block_Erase((u64)rep_blk
-					* DeviceInfo.wBlockDataSize);
-				if (FAIL == ret_erase)
-					MARK_BLOCK_AS_BAD(pbt[rep_blk]);
-			} else {
-				nand_dbg_print(NAND_DBG_DEBUG,
-					"FTL_Copy_Block == OK\n");
-				break;
-			}
-		}
-
-		if (j < RETRY_TIMES) {
-			u32 tmp;
-			u32 old_idx = FTL_Get_Block_Index(*least_idx);
-			u32 rep_idx = FTL_Get_Block_Index(rep_blk);
-			tmp = (u32)(DISCARD_BLOCK | pbt[old_idx]);
-			pbt[old_idx] = (u32)((~SPARE_BLOCK) &
-							pbt[rep_idx]);
-			pbt[rep_idx] = tmp;
-#if CMD_DMA
-			p_BTableChangesDelta = (struct BTableChangesDelta *)
-						g_pBTDelta_Free;
-			g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-			p_BTableChangesDelta->ftl_cmd_cnt =
-						ftl_cmd_cnt;
-			p_BTableChangesDelta->BT_Index = old_idx;
-			p_BTableChangesDelta->BT_Entry_Value = pbt[old_idx];
-			p_BTableChangesDelta->ValidFields = 0x0C;
-
-			p_BTableChangesDelta = (struct BTableChangesDelta *)
-						g_pBTDelta_Free;
-			g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-
-			p_BTableChangesDelta->ftl_cmd_cnt =
-						ftl_cmd_cnt;
-			p_BTableChangesDelta->BT_Index = rep_idx;
-			p_BTableChangesDelta->BT_Entry_Value = pbt[rep_idx];
-			p_BTableChangesDelta->ValidFields = 0x0C;
-#endif
-		} else {
-			pbt[FTL_Get_Block_Index(rep_blk)] |= BAD_BLOCK;
-#if CMD_DMA
-			p_BTableChangesDelta = (struct BTableChangesDelta *)
-						g_pBTDelta_Free;
-			g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-
-			p_BTableChangesDelta->ftl_cmd_cnt =
-						ftl_cmd_cnt;
-			p_BTableChangesDelta->BT_Index =
-					FTL_Get_Block_Index(rep_blk);
-			p_BTableChangesDelta->BT_Entry_Value =
-					pbt[FTL_Get_Block_Index(rep_blk)];
-			p_BTableChangesDelta->ValidFields = 0x0C;
-#endif
-			*result = FAIL;
-			ret = FAIL;
-		}
-
-		if (((*rep_blk_num)++) > WEAR_LEVELING_BLOCK_NUM)
-			ret = FAIL;
-	} else {
-		printk(KERN_ERR "Less than 3 spare blocks exist so quit\n");
-		ret = FAIL;
-	}
-
-	return ret;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Static_Wear_Leveling
-* Inputs:       none
-* Outputs:      PASS=0 / FAIL=1
-* Description:  This is static wear leveling (done by explicit call)
-*               search for most&least used
-*               if difference < GATE:
-*                   update the block table with exhange
-*                   mark block table in flash as IN_PROGRESS
-*                   copy flash block
-*               the caller should handle GC clean up after calling this function
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int FTL_Static_Wear_Leveling(void)
-{
-	u8 most_worn_cnt;
-	u8 least_worn_cnt;
-	u32 most_worn_idx;
-	u32 least_worn_idx;
-	int result = PASS;
-	int go_on = PASS;
-	u32 replaced_blks = 0;
-	u8 *chang_flag = flags_static_wear_leveling;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (!chang_flag)
-		return FAIL;
-
-	memset(chang_flag, FAIL, DeviceInfo.wDataBlockNum);
-	while (go_on == PASS) {
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"starting static wear leveling\n");
-		most_worn_cnt = 0;
-		least_worn_cnt = 0xFF;
-		least_worn_idx = BLOCK_TABLE_INDEX;
-		most_worn_idx = BLOCK_TABLE_INDEX;
-
-		find_least_most_worn(chang_flag, &least_worn_idx,
-			&least_worn_cnt, &most_worn_idx, &most_worn_cnt);
-
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Used and least worn is block %u, whos count is %u\n",
-			(unsigned int)least_worn_idx,
-			(unsigned int)least_worn_cnt);
-
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Free and  most worn is block %u, whos count is %u\n",
-			(unsigned int)most_worn_idx,
-			(unsigned int)most_worn_cnt);
-
-		if ((most_worn_cnt > least_worn_cnt) &&
-			(most_worn_cnt - least_worn_cnt > WEAR_LEVELING_GATE))
-			go_on = move_blks_for_wear_leveling(chang_flag,
-				&least_worn_idx, &replaced_blks, &result);
-		else
-			go_on = FAIL;
-	}
-
-	return result;
-}
-
-#if CMD_DMA
-static int do_garbage_collection(u32 discard_cnt)
-{
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 pba;
-	u8 bt_block_erased = 0;
-	int i, cnt, ret = FAIL;
-	u64 addr;
-
-	i = 0;
-	while ((i < DeviceInfo.wDataBlockNum) && (discard_cnt > 0) &&
-			((ftl_cmd_cnt + 28) < 256)) {
-		if (((pbt[i] & BAD_BLOCK) != BAD_BLOCK) &&
-				(pbt[i] & DISCARD_BLOCK)) {
-			if (IN_PROGRESS_BLOCK_TABLE != g_cBlockTableStatus) {
-				g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-				FTL_Write_IN_Progress_Block_Table_Page();
-			}
-
-			addr = FTL_Get_Physical_Block_Addr((u64)i *
-						DeviceInfo.wBlockDataSize);
-			pba = BLK_FROM_ADDR(addr);
-
-			for (cnt = FIRST_BT_ID; cnt <= LAST_BT_ID; cnt++) {
-				if (pba == g_pBTBlocks[cnt - FIRST_BT_ID]) {
-					nand_dbg_print(NAND_DBG_DEBUG,
-						"GC will erase BT block %u\n",
-						(unsigned int)pba);
-					discard_cnt--;
-					i++;
-					bt_block_erased = 1;
-					break;
-				}
-			}
-
-			if (bt_block_erased) {
-				bt_block_erased = 0;
-				continue;
-			}
-
-			addr = FTL_Get_Physical_Block_Addr((u64)i *
-						DeviceInfo.wBlockDataSize);
-
-			if (PASS == GLOB_FTL_Block_Erase(addr)) {
-				pbt[i] &= (u32)(~DISCARD_BLOCK);
-				pbt[i] |= (u32)(SPARE_BLOCK);
-				p_BTableChangesDelta =
-					(struct BTableChangesDelta *)
-					g_pBTDelta_Free;
-				g_pBTDelta_Free +=
-					sizeof(struct BTableChangesDelta);
-				p_BTableChangesDelta->ftl_cmd_cnt =
-					ftl_cmd_cnt - 1;
-				p_BTableChangesDelta->BT_Index = i;
-				p_BTableChangesDelta->BT_Entry_Value = pbt[i];
-				p_BTableChangesDelta->ValidFields = 0x0C;
-				discard_cnt--;
-				ret = PASS;
-			} else {
-				MARK_BLOCK_AS_BAD(pbt[i]);
-			}
-		}
-
-		i++;
-	}
-
-	return ret;
-}
-
-#else
-static int do_garbage_collection(u32 discard_cnt)
-{
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 pba;
-	u8 bt_block_erased = 0;
-	int i, cnt, ret = FAIL;
-	u64 addr;
-
-	i = 0;
-	while ((i < DeviceInfo.wDataBlockNum) && (discard_cnt > 0)) {
-		if (((pbt[i] & BAD_BLOCK) != BAD_BLOCK) &&
-				(pbt[i] & DISCARD_BLOCK)) {
-			if (IN_PROGRESS_BLOCK_TABLE != g_cBlockTableStatus) {
-				g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-				FTL_Write_IN_Progress_Block_Table_Page();
-			}
-
-			addr = FTL_Get_Physical_Block_Addr((u64)i *
-						DeviceInfo.wBlockDataSize);
-			pba = BLK_FROM_ADDR(addr);
-
-			for (cnt = FIRST_BT_ID; cnt <= LAST_BT_ID; cnt++) {
-				if (pba == g_pBTBlocks[cnt - FIRST_BT_ID]) {
-					nand_dbg_print(NAND_DBG_DEBUG,
-						"GC will erase BT block %d\n",
-						pba);
-					discard_cnt--;
-					i++;
-					bt_block_erased = 1;
-					break;
-				}
-			}
-
-			if (bt_block_erased) {
-				bt_block_erased = 0;
-				continue;
-			}
-
-			/* If the discard block is L2 cache block, then just skip it */
-			for (cnt = 0; cnt < BLK_NUM_FOR_L2_CACHE; cnt++) {
-				if (cache_l2.blk_array[cnt] == pba) {
-					nand_dbg_print(NAND_DBG_DEBUG,
-						"GC will erase L2 cache blk %d\n",
-						pba);
-					break;
-				}
-			}
-			if (cnt < BLK_NUM_FOR_L2_CACHE) { /* Skip it */
-				discard_cnt--;
-				i++;
-				continue;
-			}
-
-			addr = FTL_Get_Physical_Block_Addr((u64)i *
-						DeviceInfo.wBlockDataSize);
-
-			if (PASS == GLOB_FTL_Block_Erase(addr)) {
-				pbt[i] &= (u32)(~DISCARD_BLOCK);
-				pbt[i] |= (u32)(SPARE_BLOCK);
-				discard_cnt--;
-				ret = PASS;
-			} else {
-				MARK_BLOCK_AS_BAD(pbt[i]);
-			}
-		}
-
-		i++;
-	}
-
-	return ret;
-}
-#endif
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Garbage_Collection
-* Inputs:       none
-* Outputs:      PASS / FAIL (returns the number of un-erased blocks
-* Description:  search the block table for all discarded blocks to erase
-*               for each discarded block:
-*                   set the flash block to IN_PROGRESS
-*                   erase the block
-*                   update the block table
-*                   write the block table to flash
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Garbage_Collection(void)
-{
-	u32 i;
-	u32 wDiscard = 0;
-	int wResult = FAIL;
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	if (GC_Called) {
-		printk(KERN_ALERT "GLOB_FTL_Garbage_Collection() "
-			"has been re-entered! Exit.\n");
-		return PASS;
-	}
-
-	GC_Called = 1;
-
-	GLOB_FTL_BT_Garbage_Collection();
-
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-		if (IS_DISCARDED_BLOCK(i))
-			wDiscard++;
-	}
-
-	if (wDiscard <= 0) {
-		GC_Called = 0;
-		return wResult;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		"Found %d discarded blocks\n", wDiscard);
-
-	FTL_Write_Block_Table(FAIL);
-
-	wResult = do_garbage_collection(wDiscard);
-
-	FTL_Write_Block_Table(FAIL);
-
-	GC_Called = 0;
-
-	return wResult;
-}
-
-
-#if CMD_DMA
-static int do_bt_garbage_collection(void)
-{
-	u32 pba, lba;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 *pBTBlocksNode = (u32 *)g_pBTBlocks;
-	u64 addr;
-	int i, ret = FAIL;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	if (BT_GC_Called)
-		return PASS;
-
-	BT_GC_Called = 1;
-
-	for (i = last_erased; (i <= LAST_BT_ID) &&
-		(g_pBTBlocks[((i + 2) % (1 + LAST_BT_ID - FIRST_BT_ID)) +
-		FIRST_BT_ID - FIRST_BT_ID] != BTBLOCK_INVAL) &&
-		((ftl_cmd_cnt + 28)) < 256; i++) {
-		pba = pBTBlocksNode[i - FIRST_BT_ID];
-		lba = FTL_Get_Block_Index(pba);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"do_bt_garbage_collection: pba %d, lba %d\n",
-			pba, lba);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Block Table Entry: %d", pbt[lba]);
-
-		if (((pbt[lba] & BAD_BLOCK) != BAD_BLOCK) &&
-			(pbt[lba] & DISCARD_BLOCK)) {
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"do_bt_garbage_collection_cdma: "
-				"Erasing Block tables present in block %d\n",
-				pba);
-			addr = FTL_Get_Physical_Block_Addr((u64)lba *
-						DeviceInfo.wBlockDataSize);
-			if (PASS == GLOB_FTL_Block_Erase(addr)) {
-				pbt[lba] &= (u32)(~DISCARD_BLOCK);
-				pbt[lba] |= (u32)(SPARE_BLOCK);
-
-				p_BTableChangesDelta =
-					(struct BTableChangesDelta *)
-					g_pBTDelta_Free;
-				g_pBTDelta_Free +=
-					sizeof(struct BTableChangesDelta);
-
-				p_BTableChangesDelta->ftl_cmd_cnt =
-					ftl_cmd_cnt - 1;
-				p_BTableChangesDelta->BT_Index = lba;
-				p_BTableChangesDelta->BT_Entry_Value =
-								pbt[lba];
-
-				p_BTableChangesDelta->ValidFields = 0x0C;
-
-				ret = PASS;
-				pBTBlocksNode[last_erased - FIRST_BT_ID] =
-							BTBLOCK_INVAL;
-				nand_dbg_print(NAND_DBG_DEBUG,
-					"resetting bt entry at index %d "
-					"value %d\n", i,
-					pBTBlocksNode[i - FIRST_BT_ID]);
-				if (last_erased == LAST_BT_ID)
-					last_erased = FIRST_BT_ID;
-				else
-					last_erased++;
-			} else {
-				MARK_BLOCK_AS_BAD(pbt[lba]);
-			}
-		}
-	}
-
-	BT_GC_Called = 0;
-
-	return ret;
-}
-
-#else
-static int do_bt_garbage_collection(void)
-{
-	u32 pba, lba;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 *pBTBlocksNode = (u32 *)g_pBTBlocks;
-	u64 addr;
-	int i, ret = FAIL;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	if (BT_GC_Called)
-		return PASS;
-
-	BT_GC_Called = 1;
-
-	for (i = last_erased; (i <= LAST_BT_ID) &&
-		(g_pBTBlocks[((i + 2) % (1 + LAST_BT_ID - FIRST_BT_ID)) +
-		FIRST_BT_ID - FIRST_BT_ID] != BTBLOCK_INVAL); i++) {
-		pba = pBTBlocksNode[i - FIRST_BT_ID];
-		lba = FTL_Get_Block_Index(pba);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"do_bt_garbage_collection_cdma: pba %d, lba %d\n",
-			pba, lba);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Block Table Entry: %d", pbt[lba]);
-
-		if (((pbt[lba] & BAD_BLOCK) != BAD_BLOCK) &&
-			(pbt[lba] & DISCARD_BLOCK)) {
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"do_bt_garbage_collection: "
-				"Erasing Block tables present in block %d\n",
-				pba);
-			addr = FTL_Get_Physical_Block_Addr((u64)lba *
-						DeviceInfo.wBlockDataSize);
-			if (PASS == GLOB_FTL_Block_Erase(addr)) {
-				pbt[lba] &= (u32)(~DISCARD_BLOCK);
-				pbt[lba] |= (u32)(SPARE_BLOCK);
-				ret = PASS;
-				pBTBlocksNode[last_erased - FIRST_BT_ID] =
-							BTBLOCK_INVAL;
-				nand_dbg_print(NAND_DBG_DEBUG,
-					"resetting bt entry at index %d "
-					"value %d\n", i,
-					pBTBlocksNode[i - FIRST_BT_ID]);
-				if (last_erased == LAST_BT_ID)
-					last_erased = FIRST_BT_ID;
-				else
-					last_erased++;
-			} else {
-				MARK_BLOCK_AS_BAD(pbt[lba]);
-			}
-		}
-	}
-
-	BT_GC_Called = 0;
-
-	return ret;
-}
-
-#endif
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_BT_Garbage_Collection
-* Inputs:       none
-* Outputs:      PASS / FAIL (returns the number of un-erased blocks
-* Description:  Erases discarded blocks containing Block table
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_BT_Garbage_Collection(void)
-{
-	return do_bt_garbage_collection();
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Replace_OneBlock
-* Inputs:       Block number 1
-*               Block number 2
-* Outputs:      Replaced Block Number
-* Description:  Interchange block table entries at wBlockNum and wReplaceNum
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static u32 FTL_Replace_OneBlock(u32 blk, u32 rep_blk)
-{
-	u32 tmp_blk;
-	u32 replace_node = BAD_BLOCK;
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	if (rep_blk != BAD_BLOCK) {
-		if (IS_BAD_BLOCK(blk))
-			tmp_blk = pbt[blk];
-		else
-			tmp_blk = DISCARD_BLOCK | (~SPARE_BLOCK & pbt[blk]);
-
-		replace_node = (u32) ((~SPARE_BLOCK) & pbt[rep_blk]);
-		pbt[blk] = replace_node;
-		pbt[rep_blk] = tmp_blk;
-
-#if CMD_DMA
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta_Free;
-		g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-
-		p_BTableChangesDelta->ftl_cmd_cnt = ftl_cmd_cnt;
-		p_BTableChangesDelta->BT_Index = blk;
-		p_BTableChangesDelta->BT_Entry_Value = pbt[blk];
-
-		p_BTableChangesDelta->ValidFields = 0x0C;
-
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta_Free;
-		g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-
-		p_BTableChangesDelta->ftl_cmd_cnt = ftl_cmd_cnt;
-		p_BTableChangesDelta->BT_Index = rep_blk;
-		p_BTableChangesDelta->BT_Entry_Value = pbt[rep_blk];
-		p_BTableChangesDelta->ValidFields = 0x0C;
-#endif
-	}
-
-	return replace_node;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Write_Block_Table_Data
-* Inputs:       Block table size in pages
-* Outputs:      PASS=0 / FAIL=1
-* Description:  Write block table data in flash
-*               If first page and last page
-*                  Write data+BT flag
-*               else
-*                  Write data
-*               BT flag is a counter. Its value is incremented for block table
-*               write in a new Block
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Write_Block_Table_Data(void)
-{
-	u64 dwBlockTableAddr, pTempAddr;
-	u32 Block;
-	u16 Page, PageCount;
-	u8 *tempBuf = tmp_buf_write_blk_table_data;
-	int wBytesCopied;
-	u16 bt_pages;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	dwBlockTableAddr =
-		(u64)((u64)g_wBlockTableIndex * DeviceInfo.wBlockDataSize +
-		(u64)g_wBlockTableOffset * DeviceInfo.wPageDataSize);
-	pTempAddr = dwBlockTableAddr;
-
-	bt_pages = FTL_Get_Block_Table_Flash_Size_Pages();
-
-	nand_dbg_print(NAND_DBG_DEBUG, "FTL_Write_Block_Table_Data: "
-			       "page= %d BlockTableIndex= %d "
-			       "BlockTableOffset=%d\n", bt_pages,
-			       g_wBlockTableIndex, g_wBlockTableOffset);
-
-	Block = BLK_FROM_ADDR(pTempAddr);
-	Page = PAGE_FROM_ADDR(pTempAddr, Block);
-	PageCount = 1;
-
-	if (bt_block_changed) {
-		if (bt_flag == LAST_BT_ID) {
-			bt_flag = FIRST_BT_ID;
-			g_pBTBlocks[bt_flag - FIRST_BT_ID] = Block;
-		} else if (bt_flag < LAST_BT_ID) {
-			bt_flag++;
-			g_pBTBlocks[bt_flag - FIRST_BT_ID] = Block;
-		}
-
-		if ((bt_flag > (LAST_BT_ID-4)) &&
-			g_pBTBlocks[FIRST_BT_ID - FIRST_BT_ID] !=
-						BTBLOCK_INVAL) {
-			bt_block_changed = 0;
-			GLOB_FTL_BT_Garbage_Collection();
-		}
-
-		bt_block_changed = 0;
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Block Table Counter is %u Block %u\n",
-			bt_flag, (unsigned int)Block);
-	}
-
-	memset(tempBuf, 0, 3);
-	tempBuf[3] = bt_flag;
-	wBytesCopied = FTL_Copy_Block_Table_To_Flash(tempBuf + 4,
-			DeviceInfo.wPageDataSize - 4, 0);
-	memset(&tempBuf[wBytesCopied + 4], 0xff,
-		DeviceInfo.wPageSize - (wBytesCopied + 4));
-	FTL_Insert_Block_Table_Signature(&tempBuf[DeviceInfo.wPageDataSize],
-					bt_flag);
-
-#if CMD_DMA
-	memcpy(g_pNextBlockTable, tempBuf,
-		DeviceInfo.wPageSize * sizeof(u8));
-	nand_dbg_print(NAND_DBG_DEBUG, "Writing First Page of Block Table "
-		"Block %u Page %u\n", (unsigned int)Block, Page);
-	if (FAIL == GLOB_LLD_Write_Page_Main_Spare_cdma(g_pNextBlockTable,
-		Block, Page, 1,
-		LLD_CMD_FLAG_MODE_CDMA | LLD_CMD_FLAG_ORDER_BEFORE_REST)) {
-		nand_dbg_print(NAND_DBG_WARN, "NAND Program fail in "
-			"%s, Line %d, Function: %s, "
-			"new Bad Block %d generated!\n",
-			__FILE__, __LINE__, __func__, Block);
-		goto func_return;
-	}
-
-	ftl_cmd_cnt++;
-	g_pNextBlockTable += ((DeviceInfo.wPageSize * sizeof(u8)));
-#else
-	if (FAIL == GLOB_LLD_Write_Page_Main_Spare(tempBuf, Block, Page, 1)) {
-		nand_dbg_print(NAND_DBG_WARN,
-			"NAND Program fail in %s, Line %d, Function: %s, "
-			"new Bad Block %d generated!\n",
-			__FILE__, __LINE__, __func__, Block);
-		goto func_return;
-	}
-#endif
-
-	if (bt_pages > 1) {
-		PageCount = bt_pages - 1;
-		if (PageCount > 1) {
-			wBytesCopied += FTL_Copy_Block_Table_To_Flash(tempBuf,
-				DeviceInfo.wPageDataSize * (PageCount - 1),
-				wBytesCopied);
-
-#if CMD_DMA
-			memcpy(g_pNextBlockTable, tempBuf,
-				(PageCount - 1) * DeviceInfo.wPageDataSize);
-			if (FAIL == GLOB_LLD_Write_Page_Main_cdma(
-				g_pNextBlockTable, Block, Page + 1,
-				PageCount - 1)) {
-				nand_dbg_print(NAND_DBG_WARN,
-					"NAND Program fail in %s, Line %d, "
-					"Function: %s, "
-					"new Bad Block %d generated!\n",
-					__FILE__, __LINE__, __func__,
-					(int)Block);
-				goto func_return;
-			}
-
-			ftl_cmd_cnt++;
-			g_pNextBlockTable += (PageCount - 1) *
-				DeviceInfo.wPageDataSize * sizeof(u8);
-#else
-			if (FAIL == GLOB_LLD_Write_Page_Main(tempBuf,
-					Block, Page + 1, PageCount - 1)) {
-				nand_dbg_print(NAND_DBG_WARN,
-					"NAND Program fail in %s, Line %d, "
-					"Function: %s, "
-					"new Bad Block %d generated!\n",
-					__FILE__, __LINE__, __func__,
-					(int)Block);
-				goto func_return;
-			}
-#endif
-		}
-
-		wBytesCopied = FTL_Copy_Block_Table_To_Flash(tempBuf,
-				DeviceInfo.wPageDataSize, wBytesCopied);
-		memset(&tempBuf[wBytesCopied], 0xff,
-			DeviceInfo.wPageSize-wBytesCopied);
-		FTL_Insert_Block_Table_Signature(
-			&tempBuf[DeviceInfo.wPageDataSize], bt_flag);
-#if CMD_DMA
-		memcpy(g_pNextBlockTable, tempBuf,
-				DeviceInfo.wPageSize * sizeof(u8));
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Writing the last Page of Block Table "
-			"Block %u Page %u\n",
-			(unsigned int)Block, Page + bt_pages - 1);
-		if (FAIL == GLOB_LLD_Write_Page_Main_Spare_cdma(
-			g_pNextBlockTable, Block, Page + bt_pages - 1, 1,
-			LLD_CMD_FLAG_MODE_CDMA |
-			LLD_CMD_FLAG_ORDER_BEFORE_REST)) {
-			nand_dbg_print(NAND_DBG_WARN,
-				"NAND Program fail in %s, Line %d, "
-				"Function: %s, new Bad Block %d generated!\n",
-				__FILE__, __LINE__, __func__, Block);
-			goto func_return;
-		}
-		ftl_cmd_cnt++;
-#else
-		if (FAIL == GLOB_LLD_Write_Page_Main_Spare(tempBuf,
-					Block, Page+bt_pages - 1, 1)) {
-			nand_dbg_print(NAND_DBG_WARN,
-				"NAND Program fail in %s, Line %d, "
-				"Function: %s, "
-				"new Bad Block %d generated!\n",
-				__FILE__, __LINE__, __func__, Block);
-			goto func_return;
-		}
-#endif
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "FTL_Write_Block_Table_Data: done\n");
-
-func_return:
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Replace_Block_Table
-* Inputs:       None
-* Outputs:      PASS=0 / FAIL=1
-* Description:  Get a new block to write block table
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static u32 FTL_Replace_Block_Table(void)
-{
-	u32 blk;
-	int gc;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	blk = FTL_Replace_LWBlock(BLOCK_TABLE_INDEX, &gc);
-
-	if ((BAD_BLOCK == blk) && (PASS == gc)) {
-		GLOB_FTL_Garbage_Collection();
-		blk = FTL_Replace_LWBlock(BLOCK_TABLE_INDEX, &gc);
-	}
-	if (BAD_BLOCK == blk)
-		printk(KERN_ERR "%s, %s: There is no spare block. "
-			"It should never happen\n",
-			__FILE__, __func__);
-
-	nand_dbg_print(NAND_DBG_DEBUG, "New Block table Block is %d\n", blk);
-
-	return blk;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Replace_LWBlock
-* Inputs:       Block number
-*               Pointer to Garbage Collect flag
-* Outputs:
-* Description:  Determine the least weared block by traversing
-*               block table
-*               Set Garbage collection to be called if number of spare
-*               block is less than Free Block Gate count
-*               Change Block table entry to map least worn block for current
-*               operation
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static u32 FTL_Replace_LWBlock(u32 wBlockNum, int *pGarbageCollect)
-{
-	u32 i;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u8 wLeastWornCounter = 0xFF;
-	u32 wLeastWornIndex = BAD_BLOCK;
-	u32 wSpareBlockNum = 0;
-	u32 wDiscardBlockNum = 0;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	if (IS_SPARE_BLOCK(wBlockNum)) {
-		*pGarbageCollect = FAIL;
-		pbt[wBlockNum] = (u32)(pbt[wBlockNum] & (~SPARE_BLOCK));
-#if CMD_DMA
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta_Free;
-		g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-		p_BTableChangesDelta->ftl_cmd_cnt =
-						ftl_cmd_cnt;
-		p_BTableChangesDelta->BT_Index = (u32)(wBlockNum);
-		p_BTableChangesDelta->BT_Entry_Value = pbt[wBlockNum];
-		p_BTableChangesDelta->ValidFields = 0x0C;
-#endif
-		return pbt[wBlockNum];
-	}
-
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-		if (IS_DISCARDED_BLOCK(i))
-			wDiscardBlockNum++;
-
-		if (IS_SPARE_BLOCK(i)) {
-			u32 wPhysicalIndex = (u32)((~BAD_BLOCK) & pbt[i]);
-			if (wPhysicalIndex > DeviceInfo.wSpectraEndBlock)
-				printk(KERN_ERR "FTL_Replace_LWBlock: "
-					"This should never occur!\n");
-			if (g_pWearCounter[wPhysicalIndex -
-				DeviceInfo.wSpectraStartBlock] <
-				wLeastWornCounter) {
-				wLeastWornCounter =
-					g_pWearCounter[wPhysicalIndex -
-					DeviceInfo.wSpectraStartBlock];
-				wLeastWornIndex = i;
-			}
-			wSpareBlockNum++;
-		}
-	}
-
-	nand_dbg_print(NAND_DBG_WARN,
-		"FTL_Replace_LWBlock: Least Worn Counter %d\n",
-		(int)wLeastWornCounter);
-
-	if ((wDiscardBlockNum >= NUM_FREE_BLOCKS_GATE) ||
-		(wSpareBlockNum <= NUM_FREE_BLOCKS_GATE))
-		*pGarbageCollect = PASS;
-	else
-		*pGarbageCollect = FAIL;
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		"FTL_Replace_LWBlock: Discarded Blocks %u Spare"
-		" Blocks %u\n",
-		(unsigned int)wDiscardBlockNum,
-		(unsigned int)wSpareBlockNum);
-
-	return FTL_Replace_OneBlock(wBlockNum, wLeastWornIndex);
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Replace_MWBlock
-* Inputs:       None
-* Outputs:      most worn spare block no./BAD_BLOCK
-* Description:  It finds most worn spare block.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static u32 FTL_Replace_MWBlock(void)
-{
-	u32 i;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u8 wMostWornCounter = 0;
-	u32 wMostWornIndex = BAD_BLOCK;
-	u32 wSpareBlockNum = 0;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-		if (IS_SPARE_BLOCK(i)) {
-			u32 wPhysicalIndex = (u32)((~SPARE_BLOCK) & pbt[i]);
-			if (g_pWearCounter[wPhysicalIndex -
-			    DeviceInfo.wSpectraStartBlock] >
-			    wMostWornCounter) {
-				wMostWornCounter =
-				    g_pWearCounter[wPhysicalIndex -
-				    DeviceInfo.wSpectraStartBlock];
-				wMostWornIndex = wPhysicalIndex;
-			}
-			wSpareBlockNum++;
-		}
-	}
-
-	if (wSpareBlockNum <= 2)
-		return BAD_BLOCK;
-
-	return wMostWornIndex;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Replace_Block
-* Inputs:       Block Address
-* Outputs:      PASS=0 / FAIL=1
-* Description:  If block specified by blk_addr parameter is not free,
-*               replace it with the least worn block.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Replace_Block(u64 blk_addr)
-{
-	u32 current_blk = BLK_FROM_ADDR(blk_addr);
-	u32 *pbt = (u32 *)g_pBlockTable;
-	int wResult = PASS;
-	int GarbageCollect = FAIL;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	if (IS_SPARE_BLOCK(current_blk)) {
-		pbt[current_blk] = (~SPARE_BLOCK) & pbt[current_blk];
-#if CMD_DMA
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta_Free;
-		g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-		p_BTableChangesDelta->ftl_cmd_cnt =
-			ftl_cmd_cnt;
-		p_BTableChangesDelta->BT_Index = current_blk;
-		p_BTableChangesDelta->BT_Entry_Value = pbt[current_blk];
-		p_BTableChangesDelta->ValidFields = 0x0C ;
-#endif
-		return wResult;
-	}
-
-	FTL_Replace_LWBlock(current_blk, &GarbageCollect);
-
-	if (PASS == GarbageCollect)
-		wResult = GLOB_FTL_Garbage_Collection();
-
-	return wResult;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Is_BadBlock
-* Inputs:       block number to test
-* Outputs:      PASS (block is BAD) / FAIL (block is not bad)
-* Description:  test if this block number is flagged as bad
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Is_BadBlock(u32 wBlockNum)
-{
-	u32 *pbt = (u32 *)g_pBlockTable;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	if (wBlockNum >= DeviceInfo.wSpectraStartBlock
-		&& BAD_BLOCK == (pbt[wBlockNum] & BAD_BLOCK))
-		return PASS;
-	else
-		return FAIL;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Flush_Cache
-* Inputs:       none
-* Outputs:      PASS=0 / FAIL=1
-* Description:  flush all the cache blocks to flash
-*               if a cache block is not dirty, don't do anything with it
-*               else, write the block and update the block table
-* Note:         This function should be called at shutdown/power down.
-*               to write important data into device
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Flush_Cache(void)
-{
-	int i, ret;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	for (i = 0; i < CACHE_ITEM_NUM; i++) {
-		if (SET == Cache.array[i].changed) {
-#if CMD_DMA
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-			int_cache[ftl_cmd_cnt].item = i;
-			int_cache[ftl_cmd_cnt].cache.address =
-					Cache.array[i].address;
-			int_cache[ftl_cmd_cnt].cache.changed = CLEAR;
-#endif
-#endif
-			ret = write_back_to_l2_cache(Cache.array[i].buf, Cache.array[i].address);
-			if (PASS == ret) {
-				Cache.array[i].changed = CLEAR;
-			} else {
-				printk(KERN_ALERT "Failed when write back to L2 cache!\n");
-				/* TODO - How to handle this? */
-			}
-		}
-	}
-
-	flush_l2_cache();
-
-	return FTL_Write_Block_Table(FAIL);
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Page_Read
-* Inputs:       pointer to data
-*                   logical address of data (u64 is LBA * Bytes/Page)
-* Outputs:      PASS=0 / FAIL=1
-* Description:  reads a page of data into RAM from the cache
-*               if the data is not already in cache, read from flash to cache
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Page_Read(u8 *data, u64 logical_addr)
-{
-	u16 cache_item;
-	int res = PASS;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "GLOB_FTL_Page_Read - "
-		"page_addr: %llu\n", logical_addr);
-
-	cache_item = FTL_Cache_If_Hit(logical_addr);
-
-	if (UNHIT_CACHE_ITEM == cache_item) {
-		nand_dbg_print(NAND_DBG_DEBUG,
-			       "GLOB_FTL_Page_Read: Cache not hit\n");
-		res = FTL_Cache_Write();
-		if (ERR == FTL_Cache_Read(logical_addr))
-			res = ERR;
-		cache_item = Cache.LRU;
-	}
-
-	FTL_Cache_Read_Page(data, logical_addr, cache_item);
-
-	return res;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Page_Write
-* Inputs:       pointer to data
-*               address of data (ADDRESSTYPE is LBA * Bytes/Page)
-* Outputs:      PASS=0 / FAIL=1
-* Description:  writes a page of data from RAM to the cache
-*               if the data is not already in cache, write back the
-*               least recently used block and read the addressed block
-*               from flash to cache
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Page_Write(u8 *pData, u64 dwPageAddr)
-{
-	u16 cache_blk;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	int wResult = PASS;
-
-	nand_dbg_print(NAND_DBG_TRACE, "GLOB_FTL_Page_Write - "
-		"dwPageAddr: %llu\n", dwPageAddr);
-
-	cache_blk = FTL_Cache_If_Hit(dwPageAddr);
-
-	if (UNHIT_CACHE_ITEM == cache_blk) {
-		wResult = FTL_Cache_Write();
-		if (IS_BAD_BLOCK(BLK_FROM_ADDR(dwPageAddr))) {
-			wResult = FTL_Replace_Block(dwPageAddr);
-			pbt[BLK_FROM_ADDR(dwPageAddr)] |= SPARE_BLOCK;
-			if (wResult == FAIL)
-				return FAIL;
-		}
-		if (ERR == FTL_Cache_Read(dwPageAddr))
-			wResult = ERR;
-		cache_blk = Cache.LRU;
-		FTL_Cache_Write_Page(pData, dwPageAddr, cache_blk, 0);
-	} else {
-#if CMD_DMA
-		FTL_Cache_Write_Page(pData, dwPageAddr, cache_blk,
-				LLD_CMD_FLAG_ORDER_BEFORE_REST);
-#else
-		FTL_Cache_Write_Page(pData, dwPageAddr, cache_blk, 0);
-#endif
-	}
-
-	return wResult;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     GLOB_FTL_Block_Erase
-* Inputs:       address of block to erase (now in byte format, should change to
-* block format)
-* Outputs:      PASS=0 / FAIL=1
-* Description:  erases the specified block
-*               increments the erase count
-*               If erase count reaches its upper limit,call function to
-*               do the adjustment as per the relative erase count values
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int GLOB_FTL_Block_Erase(u64 blk_addr)
-{
-	int status;
-	u32 BlkIdx;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	BlkIdx = (u32)(blk_addr >> DeviceInfo.nBitsInBlockDataSize);
-
-	if (BlkIdx < DeviceInfo.wSpectraStartBlock) {
-		printk(KERN_ERR "GLOB_FTL_Block_Erase: "
-			"This should never occur\n");
-		return FAIL;
-	}
-
-#if CMD_DMA
-	status = GLOB_LLD_Erase_Block_cdma(BlkIdx, LLD_CMD_FLAG_MODE_CDMA);
-	if (status == FAIL)
-		nand_dbg_print(NAND_DBG_WARN,
-			       "NAND Program fail in %s, Line %d, "
-			       "Function: %s, new Bad Block %d generated!\n",
-			       __FILE__, __LINE__, __func__, BlkIdx);
-#else
-	status = GLOB_LLD_Erase_Block(BlkIdx);
-	if (status == FAIL) {
-		nand_dbg_print(NAND_DBG_WARN,
-			       "NAND Program fail in %s, Line %d, "
-			       "Function: %s, new Bad Block %d generated!\n",
-			       __FILE__, __LINE__, __func__, BlkIdx);
-		return status;
-	}
-#endif
-
-	if (DeviceInfo.MLCDevice) {
-		g_pReadCounter[BlkIdx - DeviceInfo.wSpectraStartBlock] = 0;
-		if (g_cBlockTableStatus != IN_PROGRESS_BLOCK_TABLE) {
-			g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-			FTL_Write_IN_Progress_Block_Table_Page();
-		}
-	}
-
-	g_pWearCounter[BlkIdx - DeviceInfo.wSpectraStartBlock]++;
-
-#if CMD_DMA
-	p_BTableChangesDelta =
-		(struct BTableChangesDelta *)g_pBTDelta_Free;
-	g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-	p_BTableChangesDelta->ftl_cmd_cnt = ftl_cmd_cnt;
-	p_BTableChangesDelta->WC_Index =
-		BlkIdx - DeviceInfo.wSpectraStartBlock;
-	p_BTableChangesDelta->WC_Entry_Value =
-		g_pWearCounter[BlkIdx - DeviceInfo.wSpectraStartBlock];
-	p_BTableChangesDelta->ValidFields = 0x30;
-
-	if (DeviceInfo.MLCDevice) {
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta_Free;
-		g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-		p_BTableChangesDelta->ftl_cmd_cnt =
-			ftl_cmd_cnt;
-		p_BTableChangesDelta->RC_Index =
-			BlkIdx - DeviceInfo.wSpectraStartBlock;
-		p_BTableChangesDelta->RC_Entry_Value =
-			g_pReadCounter[BlkIdx -
-				DeviceInfo.wSpectraStartBlock];
-		p_BTableChangesDelta->ValidFields = 0xC0;
-	}
-
-	ftl_cmd_cnt++;
-#endif
-
-	if (g_pWearCounter[BlkIdx - DeviceInfo.wSpectraStartBlock] == 0xFE)
-		FTL_Adjust_Relative_Erase_Count(BlkIdx);
-
-	return status;
-}
-
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Adjust_Relative_Erase_Count
-* Inputs:       index to block that was just incremented and is at the max
-* Outputs:      PASS=0 / FAIL=1
-* Description:  If any erase counts at MAX, adjusts erase count of every
-*               block by subtracting least worn
-*               counter from counter value of every entry in wear table
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-static int FTL_Adjust_Relative_Erase_Count(u32 Index_of_MAX)
-{
-	u8 wLeastWornCounter = MAX_BYTE_VALUE;
-	u8 wWearCounter;
-	u32 i, wWearIndex;
-	u32 *pbt = (u32 *)g_pBlockTable;
-	int wResult = PASS;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		__FILE__, __LINE__, __func__);
-
-	for (i = 0; i < DeviceInfo.wDataBlockNum; i++) {
-		if (IS_BAD_BLOCK(i))
-			continue;
-		wWearIndex = (u32)(pbt[i] & (~BAD_BLOCK));
-
-		if ((wWearIndex - DeviceInfo.wSpectraStartBlock) < 0)
-			printk(KERN_ERR "FTL_Adjust_Relative_Erase_Count:"
-					"This should never occur\n");
-		wWearCounter = g_pWearCounter[wWearIndex -
-			DeviceInfo.wSpectraStartBlock];
-		if (wWearCounter < wLeastWornCounter)
-			wLeastWornCounter = wWearCounter;
-	}
-
-	if (wLeastWornCounter == 0) {
-		nand_dbg_print(NAND_DBG_WARN,
-			"Adjusting Wear Levelling Counters: Special Case\n");
-		g_pWearCounter[Index_of_MAX -
-			DeviceInfo.wSpectraStartBlock]--;
-#if CMD_DMA
-		p_BTableChangesDelta =
-			(struct BTableChangesDelta *)g_pBTDelta_Free;
-		g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-		p_BTableChangesDelta->ftl_cmd_cnt = ftl_cmd_cnt;
-		p_BTableChangesDelta->WC_Index =
-			Index_of_MAX - DeviceInfo.wSpectraStartBlock;
-		p_BTableChangesDelta->WC_Entry_Value =
-			g_pWearCounter[Index_of_MAX -
-				DeviceInfo.wSpectraStartBlock];
-		p_BTableChangesDelta->ValidFields = 0x30;
-#endif
-		FTL_Static_Wear_Leveling();
-	} else {
-		for (i = 0; i < DeviceInfo.wDataBlockNum; i++)
-			if (!IS_BAD_BLOCK(i)) {
-				wWearIndex = (u32)(pbt[i] & (~BAD_BLOCK));
-				g_pWearCounter[wWearIndex -
-					DeviceInfo.wSpectraStartBlock] =
-					(u8)(g_pWearCounter
-					[wWearIndex -
-					DeviceInfo.wSpectraStartBlock] -
-					wLeastWornCounter);
-#if CMD_DMA
-				p_BTableChangesDelta =
-				(struct BTableChangesDelta *)g_pBTDelta_Free;
-				g_pBTDelta_Free +=
-					sizeof(struct BTableChangesDelta);
-
-				p_BTableChangesDelta->ftl_cmd_cnt =
-					ftl_cmd_cnt;
-				p_BTableChangesDelta->WC_Index = wWearIndex -
-					DeviceInfo.wSpectraStartBlock;
-				p_BTableChangesDelta->WC_Entry_Value =
-					g_pWearCounter[wWearIndex -
-					DeviceInfo.wSpectraStartBlock];
-				p_BTableChangesDelta->ValidFields = 0x30;
-#endif
-			}
-	}
-
-	return wResult;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Write_IN_Progress_Block_Table_Page
-* Inputs:       None
-* Outputs:      None
-* Description:  It writes in-progress flag page to the page next to
-*               block table
-***********************************************************************/
-static int FTL_Write_IN_Progress_Block_Table_Page(void)
-{
-	int wResult = PASS;
-	u16 bt_pages;
-	u16 dwIPFPageAddr;
-#if CMD_DMA
-#else
-	u32 *pbt = (u32 *)g_pBlockTable;
-	u32 wTempBlockTableIndex;
-#endif
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-	bt_pages = FTL_Get_Block_Table_Flash_Size_Pages();
-
-	dwIPFPageAddr = g_wBlockTableOffset + bt_pages;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Writing IPF at "
-			       "Block %d Page %d\n",
-			       g_wBlockTableIndex, dwIPFPageAddr);
-
-#if CMD_DMA
-	wResult = GLOB_LLD_Write_Page_Main_Spare_cdma(g_pIPF,
-		g_wBlockTableIndex, dwIPFPageAddr, 1,
-		LLD_CMD_FLAG_MODE_CDMA | LLD_CMD_FLAG_ORDER_BEFORE_REST);
-	if (wResult == FAIL) {
-		nand_dbg_print(NAND_DBG_WARN,
-			       "NAND Program fail in %s, Line %d, "
-			       "Function: %s, new Bad Block %d generated!\n",
-			       __FILE__, __LINE__, __func__,
-			       g_wBlockTableIndex);
-	}
-	g_wBlockTableOffset = dwIPFPageAddr + 1;
-	p_BTableChangesDelta = (struct BTableChangesDelta *)g_pBTDelta_Free;
-	g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-	p_BTableChangesDelta->ftl_cmd_cnt = ftl_cmd_cnt;
-	p_BTableChangesDelta->g_wBlockTableOffset = g_wBlockTableOffset;
-	p_BTableChangesDelta->ValidFields = 0x01;
-	ftl_cmd_cnt++;
-#else
-	wResult = GLOB_LLD_Write_Page_Main_Spare(g_pIPF,
-		g_wBlockTableIndex, dwIPFPageAddr, 1);
-	if (wResult == FAIL) {
-		nand_dbg_print(NAND_DBG_WARN,
-			       "NAND Program fail in %s, Line %d, "
-			       "Function: %s, new Bad Block %d generated!\n",
-			       __FILE__, __LINE__, __func__,
-			       (int)g_wBlockTableIndex);
-		MARK_BLOCK_AS_BAD(pbt[BLOCK_TABLE_INDEX]);
-		wTempBlockTableIndex = FTL_Replace_Block_Table();
-		bt_block_changed = 1;
-		if (BAD_BLOCK == wTempBlockTableIndex)
-			return ERR;
-		g_wBlockTableIndex = wTempBlockTableIndex;
-		g_wBlockTableOffset = 0;
-		/* Block table tag is '00'. Means it's used one */
-		pbt[BLOCK_TABLE_INDEX] = g_wBlockTableIndex;
-		return FAIL;
-	}
-	g_wBlockTableOffset = dwIPFPageAddr + 1;
-#endif
-	return wResult;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     FTL_Read_Disturbance
-* Inputs:       block address
-* Outputs:      PASS=0 / FAIL=1
-* Description:  used to handle read disturbance. Data in block that
-*               reaches its read limit is moved to new block
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int FTL_Read_Disturbance(u32 blk_addr)
-{
-	int wResult = FAIL;
-	u32 *pbt = (u32 *) g_pBlockTable;
-	u32 dwOldBlockAddr = blk_addr;
-	u32 wBlockNum;
-	u32 i;
-	u32 wLeastReadCounter = 0xFFFF;
-	u32 wLeastReadIndex = BAD_BLOCK;
-	u32 wSpareBlockNum = 0;
-	u32 wTempNode;
-	u32 wReplacedNode;
-	u8 *g_pTempBuf;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-			       __FILE__, __LINE__, __func__);
-
-#if CMD_DMA
-	g_pTempBuf = cp_back_buf_copies[cp_back_buf_idx];
-	cp_back_buf_idx++;
-	if (cp_back_buf_idx > COPY_BACK_BUF_NUM) {
-		printk(KERN_ERR "cp_back_buf_copies overflow! Exit."
-		"Maybe too many pending commands in your CDMA chain.\n");
-		return FAIL;
-	}
-#else
-	g_pTempBuf = tmp_buf_read_disturbance;
-#endif
-
-	wBlockNum = FTL_Get_Block_Index(blk_addr);
-
-	do {
-		/* This is a bug.Here 'i' should be logical block number
-		 * and start from 1 (0 is reserved for block table).
-		 * Have fixed it.        - Yunpeng 2008. 12. 19
-		 */
-		for (i = 1; i < DeviceInfo.wDataBlockNum; i++) {
-			if (IS_SPARE_BLOCK(i)) {
-				u32 wPhysicalIndex =
-					(u32)((~SPARE_BLOCK) & pbt[i]);
-				if (g_pReadCounter[wPhysicalIndex -
-					DeviceInfo.wSpectraStartBlock] <
-					wLeastReadCounter) {
-					wLeastReadCounter =
-						g_pReadCounter[wPhysicalIndex -
-						DeviceInfo.wSpectraStartBlock];
-					wLeastReadIndex = i;
-				}
-				wSpareBlockNum++;
-			}
-		}
-
-		if (wSpareBlockNum <= NUM_FREE_BLOCKS_GATE) {
-			wResult = GLOB_FTL_Garbage_Collection();
-			if (PASS == wResult)
-				continue;
-			else
-				break;
-		} else {
-			wTempNode = (u32)(DISCARD_BLOCK | pbt[wBlockNum]);
-			wReplacedNode = (u32)((~SPARE_BLOCK) &
-					pbt[wLeastReadIndex]);
-#if CMD_DMA
-			pbt[wBlockNum] = wReplacedNode;
-			pbt[wLeastReadIndex] = wTempNode;
-			p_BTableChangesDelta =
-				(struct BTableChangesDelta *)g_pBTDelta_Free;
-			g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-
-			p_BTableChangesDelta->ftl_cmd_cnt =
-					ftl_cmd_cnt;
-			p_BTableChangesDelta->BT_Index = wBlockNum;
-			p_BTableChangesDelta->BT_Entry_Value = pbt[wBlockNum];
-			p_BTableChangesDelta->ValidFields = 0x0C;
-
-			p_BTableChangesDelta =
-				(struct BTableChangesDelta *)g_pBTDelta_Free;
-			g_pBTDelta_Free += sizeof(struct BTableChangesDelta);
-
-			p_BTableChangesDelta->ftl_cmd_cnt =
-					ftl_cmd_cnt;
-			p_BTableChangesDelta->BT_Index = wLeastReadIndex;
-			p_BTableChangesDelta->BT_Entry_Value =
-					pbt[wLeastReadIndex];
-			p_BTableChangesDelta->ValidFields = 0x0C;
-
-			wResult = GLOB_LLD_Read_Page_Main_cdma(g_pTempBuf,
-				dwOldBlockAddr, 0, DeviceInfo.wPagesPerBlock,
-				LLD_CMD_FLAG_MODE_CDMA);
-			if (wResult == FAIL)
-				return wResult;
-
-			ftl_cmd_cnt++;
-
-			if (wResult != FAIL) {
-				if (FAIL == GLOB_LLD_Write_Page_Main_cdma(
-					g_pTempBuf, pbt[wBlockNum], 0,
-					DeviceInfo.wPagesPerBlock)) {
-					nand_dbg_print(NAND_DBG_WARN,
-						"NAND Program fail in "
-						"%s, Line %d, Function: %s, "
-						"new Bad Block %d "
-						"generated!\n",
-						__FILE__, __LINE__, __func__,
-						(int)pbt[wBlockNum]);
-					wResult = FAIL;
-					MARK_BLOCK_AS_BAD(pbt[wBlockNum]);
-				}
-				ftl_cmd_cnt++;
-			}
-#else
-			wResult = GLOB_LLD_Read_Page_Main(g_pTempBuf,
-				dwOldBlockAddr, 0, DeviceInfo.wPagesPerBlock);
-			if (wResult == FAIL)
-				return wResult;
-
-			if (wResult != FAIL) {
-				/* This is a bug. At this time, pbt[wBlockNum]
-				is still the physical address of
-				discard block, and should not be write.
-				Have fixed it as below.
-					-- Yunpeng 2008.12.19
-				*/
-				wResult = GLOB_LLD_Write_Page_Main(g_pTempBuf,
-					wReplacedNode, 0,
-					DeviceInfo.wPagesPerBlock);
-				if (wResult == FAIL) {
-					nand_dbg_print(NAND_DBG_WARN,
-						"NAND Program fail in "
-						"%s, Line %d, Function: %s, "
-						"new Bad Block %d "
-						"generated!\n",
-						__FILE__, __LINE__, __func__,
-						(int)wReplacedNode);
-					MARK_BLOCK_AS_BAD(wReplacedNode);
-				} else {
-					pbt[wBlockNum] = wReplacedNode;
-					pbt[wLeastReadIndex] = wTempNode;
-				}
-			}
-
-			if ((wResult == PASS) && (g_cBlockTableStatus !=
-				IN_PROGRESS_BLOCK_TABLE)) {
-				g_cBlockTableStatus = IN_PROGRESS_BLOCK_TABLE;
-				FTL_Write_IN_Progress_Block_Table_Page();
-			}
-#endif
-		}
-	} while (wResult != PASS)
-	;
-
-#if CMD_DMA
-	/* ... */
-#endif
-
-	return wResult;
-}
-
diff --git a/drivers/staging/spectra/flash.h b/drivers/staging/spectra/flash.h
deleted file mode 100644
index e59cf4ede551..000000000000
--- a/drivers/staging/spectra/flash.h
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#ifndef _FLASH_INTERFACE_
-#define _FLASH_INTERFACE_
-
-#include "ffsport.h"
-#include "spectraswconfig.h"
-
-#define MAX_BYTE_VALUE        0xFF
-#define MAX_WORD_VALUE        0xFFFF
-#define MAX_U32_VALUE        0xFFFFFFFF
-
-#define MAX_BLOCKNODE_VALUE     0xFFFFFF
-#define DISCARD_BLOCK           0x800000
-#define SPARE_BLOCK             0x400000
-#define BAD_BLOCK               0xC00000
-
-#define UNHIT_CACHE_ITEM         0xFFFF
-
-#define NAND_CACHE_INIT_ADDR    0xffffffffffffffffULL
-
-#define IN_PROGRESS_BLOCK_TABLE   0x00
-#define CURRENT_BLOCK_TABLE       0x01
-
-#define BTSIG_OFFSET   (0)
-#define BTSIG_BYTES    (5)
-#define BTSIG_DELTA    (3)
-
-#define MAX_READ_COUNTER  0x2710
-
-#define FIRST_BT_ID		(1)
-#define LAST_BT_ID    (254)
-#define BTBLOCK_INVAL  (u32)(0xFFFFFFFF)
-
-struct device_info_tag {
-	u16 wDeviceMaker;
-	u16 wDeviceID;
-	u32 wDeviceType;
-	u32 wSpectraStartBlock;
-	u32 wSpectraEndBlock;
-	u32 wTotalBlocks;
-	u16 wPagesPerBlock;
-	u16 wPageSize;
-	u16 wPageDataSize;
-	u16 wPageSpareSize;
-	u16 wNumPageSpareFlag;
-	u16 wECCBytesPerSector;
-	u32 wBlockSize;
-	u32 wBlockDataSize;
-	u32 wDataBlockNum;
-	u8 bPlaneNum;
-	u16 wDeviceMainAreaSize;
-	u16 wDeviceSpareAreaSize;
-	u16 wDevicesConnected;
-	u16 wDeviceWidth;
-	u16 wHWRevision;
-	u16 wHWFeatures;
-
-	u16 wONFIDevFeatures;
-	u16 wONFIOptCommands;
-	u16 wONFITimingMode;
-	u16 wONFIPgmCacheTimingMode;
-
-	u16 MLCDevice;
-	u16 wSpareSkipBytes;
-
-	u8 nBitsInPageNumber;
-	u8 nBitsInPageDataSize;
-	u8 nBitsInBlockDataSize;
-};
-
-extern struct device_info_tag DeviceInfo;
-
-/* Cache item format */
-struct flash_cache_item_tag {
-	u64 address;
-	u16 use_cnt;
-	u16 changed;
-	u8 *buf;
-};
-
-struct flash_cache_tag {
-	u32 cache_item_size; /* Size in bytes of each cache item */
-	u16 pages_per_item; /* How many NAND pages in each cache item */
-	u16 LRU; /* No. of the least recently used cache item */
-	struct flash_cache_item_tag array[CACHE_ITEM_NUM];
-};
-
-/*
- *Data structure for each list node of the management table
- * used for the Level 2 Cache. Each node maps one logical NAND block.
- */
-struct spectra_l2_cache_list {
-	struct list_head list;
-	u32 logical_blk_num; /* Logical block number */
-	u32 pages_array[]; /* Page map array of this logical block.
-			   * Array index is the logical block number,
-			   * and for every item of this arry:
-			   * high 16 bit is index of the L2 cache block num,
-			   * low 16 bit is the phy page num
-			   * of the above L2 cache block.
-			   * This array will be kmalloc during run time.
-			   */
-};
-
-struct spectra_l2_cache_info {
-	u32 blk_array[BLK_NUM_FOR_L2_CACHE];
-	u16 cur_blk_idx; /* idx to the phy block number of current using */
-	u16 cur_page_num; /* pages number of current using */
-	struct spectra_l2_cache_list table; /* First node of the table */
-};
-
-#define RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE    1
-
-#if RESTORE_CACHE_ON_CDMA_CHAIN_FAILURE
-struct flash_cache_mod_item_tag {
-	u64 address;
-	u8 changed;
-};
-
-struct flash_cache_delta_list_tag {
-	u8 item;  /* used cache item */
-	struct flash_cache_mod_item_tag cache;
-};
-#endif
-
-extern struct flash_cache_tag Cache;
-
-extern u8 *buf_read_page_main_spare;
-extern u8 *buf_write_page_main_spare;
-extern u8 *buf_read_page_spare;
-extern u8 *buf_get_bad_block;
-extern u8 *cdma_desc_buf;
-extern u8 *memcp_desc_buf;
-
-/* struture used for IndentfyDevice function */
-struct spectra_indentfy_dev_tag {
-	u32 NumBlocks;
-	u16 PagesPerBlock;
-	u16 PageDataSize;
-	u16 wECCBytesPerSector;
-	u32 wDataBlockNum;
-};
-
-int GLOB_FTL_Flash_Init(void);
-int GLOB_FTL_Flash_Release(void);
-/*void GLOB_FTL_Erase_Flash(void);*/
-int GLOB_FTL_Block_Erase(u64 block_addr);
-int GLOB_FTL_Is_BadBlock(u32 block_num);
-int GLOB_FTL_IdentifyDevice(struct spectra_indentfy_dev_tag *dev_data);
-int GLOB_FTL_Event_Status(int *);
-u16 glob_ftl_execute_cmds(void);
-
-/*int FTL_Read_Disturbance(ADDRESSTYPE dwBlockAddr);*/
-int FTL_Read_Disturbance(u32 dwBlockAddr);
-
-/*Flash r/w based on cache*/
-int GLOB_FTL_Page_Read(u8 *read_data, u64 page_addr);
-int GLOB_FTL_Page_Write(u8 *write_data, u64 page_addr);
-int GLOB_FTL_Wear_Leveling(void);
-int GLOB_FTL_Flash_Format(void);
-int GLOB_FTL_Init(void);
-int GLOB_FTL_Flush_Cache(void);
-int GLOB_FTL_Garbage_Collection(void);
-int GLOB_FTL_BT_Garbage_Collection(void);
-void GLOB_FTL_Cache_Release(void);
-u8 *get_blk_table_start_addr(void);
-u8 *get_wear_leveling_table_start_addr(void);
-unsigned long get_blk_table_len(void);
-unsigned long get_wear_leveling_table_len(void);
-
-#if DEBUG_BNDRY
-void debug_boundary_lineno_error(int chnl, int limit, int no, int lineno,
-				char *filename);
-#define debug_boundary_error(chnl, limit, no) debug_boundary_lineno_error(chnl,\
-						limit, no, __LINE__, __FILE__)
-#else
-#define debug_boundary_error(chnl, limit, no) ;
-#endif
-
-#endif /*_FLASH_INTERFACE_*/
diff --git a/drivers/staging/spectra/lld.c b/drivers/staging/spectra/lld.c
deleted file mode 100644
index 5c3b9762dc3e..000000000000
--- a/drivers/staging/spectra/lld.c
+++ /dev/null
@@ -1,339 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#include "spectraswconfig.h"
-#include "ffsport.h"
-#include "ffsdefs.h"
-#include "lld.h"
-#include "lld_nand.h"
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-#if FLASH_EMU		/* vector all the LLD calls to the LLD_EMU code */
-#include "lld_emu.h"
-#include "lld_cdma.h"
-
-/* common functions: */
-u16 GLOB_LLD_Flash_Reset(void)
-{
-	return emu_Flash_Reset();
-}
-
-u16 GLOB_LLD_Read_Device_ID(void)
-{
-	return emu_Read_Device_ID();
-}
-
-int GLOB_LLD_Flash_Release(void)
-{
-	return emu_Flash_Release();
-}
-
-u16 GLOB_LLD_Flash_Init(void)
-{
-	return emu_Flash_Init();
-}
-
-u16 GLOB_LLD_Erase_Block(u32 block_add)
-{
-	return emu_Erase_Block(block_add);
-}
-
-u16 GLOB_LLD_Write_Page_Main(u8 *write_data, u32 block, u16 Page,
-				u16 PageCount)
-{
-	return emu_Write_Page_Main(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Main(u8 *read_data, u32 block, u16 Page,
-			       u16 PageCount)
-{
-	return emu_Read_Page_Main(read_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Main_Polling(u8 *read_data,
-			u32 block, u16 page, u16 page_count)
-{
-	return emu_Read_Page_Main(read_data, block, page, page_count);
-}
-
-u16 GLOB_LLD_Write_Page_Main_Spare(u8 *write_data, u32 block,
-				      u16 Page, u16 PageCount)
-{
-	return emu_Write_Page_Main_Spare(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Main_Spare(u8 *read_data, u32 block,
-				     u16 Page, u16 PageCount)
-{
-	return emu_Read_Page_Main_Spare(read_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Write_Page_Spare(u8 *write_data, u32 block, u16 Page,
-				 u16 PageCount)
-{
-	return emu_Write_Page_Spare(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Spare(u8 *read_data, u32 block, u16 Page,
-				u16 PageCount)
-{
-	return emu_Read_Page_Spare(read_data, block, Page, PageCount);
-}
-
-u16  GLOB_LLD_Get_Bad_Block(u32 block)
-{
-    return  emu_Get_Bad_Block(block);
-}
-
-#endif /* FLASH_EMU */
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-#if FLASH_MTD		/* vector all the LLD calls to the LLD_MTD code */
-#include "lld_mtd.h"
-#include "lld_cdma.h"
-
-/* common functions: */
-u16 GLOB_LLD_Flash_Reset(void)
-{
-	return mtd_Flash_Reset();
-}
-
-u16 GLOB_LLD_Read_Device_ID(void)
-{
-	return mtd_Read_Device_ID();
-}
-
-int GLOB_LLD_Flash_Release(void)
-{
-	return mtd_Flash_Release();
-}
-
-u16 GLOB_LLD_Flash_Init(void)
-{
-	return mtd_Flash_Init();
-}
-
-u16 GLOB_LLD_Erase_Block(u32 block_add)
-{
-	return mtd_Erase_Block(block_add);
-}
-
-u16 GLOB_LLD_Write_Page_Main(u8 *write_data, u32 block, u16 Page,
-				u16 PageCount)
-{
-	return mtd_Write_Page_Main(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Main(u8 *read_data, u32 block, u16 Page,
-			       u16 PageCount)
-{
-	return mtd_Read_Page_Main(read_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Main_Polling(u8 *read_data,
-			u32 block, u16 page, u16 page_count)
-{
-	return mtd_Read_Page_Main(read_data, block, page, page_count);
-}
-
-u16 GLOB_LLD_Write_Page_Main_Spare(u8 *write_data, u32 block,
-				      u16 Page, u16 PageCount)
-{
-	return mtd_Write_Page_Main_Spare(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Main_Spare(u8 *read_data, u32 block,
-				     u16 Page, u16 PageCount)
-{
-	return mtd_Read_Page_Main_Spare(read_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Write_Page_Spare(u8 *write_data, u32 block, u16 Page,
-				 u16 PageCount)
-{
-	return mtd_Write_Page_Spare(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Spare(u8 *read_data, u32 block, u16 Page,
-				u16 PageCount)
-{
-	return mtd_Read_Page_Spare(read_data, block, Page, PageCount);
-}
-
-u16  GLOB_LLD_Get_Bad_Block(u32 block)
-{
-    return  mtd_Get_Bad_Block(block);
-}
-
-#endif /* FLASH_MTD */
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-#if FLASH_NAND	/* vector all the LLD calls to the NAND controller code */
-#include "lld_nand.h"
-#include "lld_cdma.h"
-#include "flash.h"
-
-/* common functions for LLD_NAND */
-void GLOB_LLD_ECC_Control(int enable)
-{
-	NAND_ECC_Ctrl(enable);
-}
-
-/* common functions for LLD_NAND */
-u16 GLOB_LLD_Flash_Reset(void)
-{
-	return NAND_Flash_Reset();
-}
-
-u16 GLOB_LLD_Read_Device_ID(void)
-{
-	return NAND_Read_Device_ID();
-}
-
-u16 GLOB_LLD_UnlockArrayAll(void)
-{
-	return NAND_UnlockArrayAll();
-}
-
-u16 GLOB_LLD_Flash_Init(void)
-{
-	return NAND_Flash_Init();
-}
-
-int GLOB_LLD_Flash_Release(void)
-{
-	return nand_release_spectra();
-}
-
-u16 GLOB_LLD_Erase_Block(u32 block_add)
-{
-	return NAND_Erase_Block(block_add);
-}
-
-
-u16 GLOB_LLD_Write_Page_Main(u8 *write_data, u32 block, u16 Page,
-				u16 PageCount)
-{
-	return NAND_Write_Page_Main(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Main(u8 *read_data, u32 block, u16 page,
-			       u16 page_count)
-{
-	if (page_count == 1) /* Using polling to improve read speed */
-		return NAND_Read_Page_Main_Polling(read_data, block, page, 1);
-	else
-		return NAND_Read_Page_Main(read_data, block, page, page_count);
-}
-
-u16 GLOB_LLD_Read_Page_Main_Polling(u8 *read_data,
-			u32 block, u16 page, u16 page_count)
-{
-	return NAND_Read_Page_Main_Polling(read_data,
-			block, page, page_count);
-}
-
-u16 GLOB_LLD_Write_Page_Main_Spare(u8 *write_data, u32 block,
-				      u16 Page, u16 PageCount)
-{
-	return NAND_Write_Page_Main_Spare(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Write_Page_Spare(u8 *write_data, u32 block, u16 Page,
-				 u16 PageCount)
-{
-	return NAND_Write_Page_Spare(write_data, block, Page, PageCount);
-}
-
-u16 GLOB_LLD_Read_Page_Main_Spare(u8 *read_data, u32 block,
-				     u16 page, u16 page_count)
-{
-	return NAND_Read_Page_Main_Spare(read_data, block, page, page_count);
-}
-
-u16 GLOB_LLD_Read_Page_Spare(u8 *read_data, u32 block, u16 Page,
-				u16 PageCount)
-{
-	return NAND_Read_Page_Spare(read_data, block, Page, PageCount);
-}
-
-u16  GLOB_LLD_Get_Bad_Block(u32 block)
-{
-	return  NAND_Get_Bad_Block(block);
-}
-
-#if CMD_DMA
-u16 GLOB_LLD_Event_Status(void)
-{
-	return CDMA_Event_Status();
-}
-
-u16 glob_lld_execute_cmds(void)
-{
-	return CDMA_Execute_CMDs();
-}
-
-u16 GLOB_LLD_MemCopy_CMD(u8 *dest, u8 *src,
-			u32 ByteCount, u16 flag)
-{
-	/* Replace the hardware memcopy with software memcpy function */
-	if (CDMA_Execute_CMDs())
-		return FAIL;
-	memcpy(dest, src, ByteCount);
-	return PASS;
-
-	/* return CDMA_MemCopy_CMD(dest, src, ByteCount, flag); */
-}
-
-u16 GLOB_LLD_Erase_Block_cdma(u32 block, u16 flags)
-{
-	return CDMA_Data_CMD(ERASE_CMD, 0, block, 0, 0, flags);
-}
-
-u16 GLOB_LLD_Write_Page_Main_cdma(u8 *data, u32 block, u16 page, u16 count)
-{
-	return CDMA_Data_CMD(WRITE_MAIN_CMD, data, block, page, count, 0);
-}
-
-u16 GLOB_LLD_Read_Page_Main_cdma(u8 *data, u32 block, u16 page,
-				u16 count, u16 flags)
-{
-	return CDMA_Data_CMD(READ_MAIN_CMD, data, block, page, count, flags);
-}
-
-u16 GLOB_LLD_Write_Page_Main_Spare_cdma(u8 *data, u32 block, u16 page,
-					u16 count, u16 flags)
-{
-	return CDMA_Data_CMD(WRITE_MAIN_SPARE_CMD,
-			data, block, page, count, flags);
-}
-
-u16 GLOB_LLD_Read_Page_Main_Spare_cdma(u8 *data,
-				u32 block, u16 page, u16 count)
-{
-	return CDMA_Data_CMD(READ_MAIN_SPARE_CMD, data, block, page, count,
-			LLD_CMD_FLAG_MODE_CDMA);
-}
-
-#endif /* CMD_DMA */
-#endif /* FLASH_NAND */
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-
-/* end of LLD.c */
diff --git a/drivers/staging/spectra/lld.h b/drivers/staging/spectra/lld.h
deleted file mode 100644
index d3738e0e1fea..000000000000
--- a/drivers/staging/spectra/lld.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-
-
-#ifndef _LLD_
-#define _LLD_
-
-#include "ffsport.h"
-#include "spectraswconfig.h"
-#include "flash.h"
-
-#define GOOD_BLOCK 0
-#define DEFECTIVE_BLOCK 1
-#define READ_ERROR 2
-
-#define CLK_X  5
-#define CLK_MULTI 4
-
-/* Typedefs */
-
-/*  prototypes: API for LLD */
-/* Currently, Write_Page_Main
- * 			  MemCopy
- * 			  Read_Page_Main_Spare
- * do not have flag because they were not implemented prior to this
- * They are not being added to keep changes to a minimum for now.
- * Currently, they are not required (only reqd for Wr_P_M_S.)
- * Later on, these NEED to be changed.
- */
-
-extern void GLOB_LLD_ECC_Control(int enable);
-
-extern u16 GLOB_LLD_Flash_Reset(void);
-
-extern u16 GLOB_LLD_Read_Device_ID(void);
-
-extern u16 GLOB_LLD_UnlockArrayAll(void);
-
-extern u16 GLOB_LLD_Flash_Init(void);
-
-extern int GLOB_LLD_Flash_Release(void);
-
-extern u16 GLOB_LLD_Erase_Block(u32 block_add);
-
-extern u16 GLOB_LLD_Write_Page_Main(u8 *write_data,
-	u32 block, u16 Page, u16 PageCount);
-
-extern u16 GLOB_LLD_Read_Page_Main(u8 *read_data,
-	u32 block, u16 page, u16 page_count);
-
-extern u16 GLOB_LLD_Read_Page_Main_Polling(u8 *read_data,
-	u32 block, u16 page, u16 page_count);
-
-extern u16 GLOB_LLD_Write_Page_Main_Spare(u8 *write_data,
-	u32 block, u16 Page, u16 PageCount);
-
-extern u16 GLOB_LLD_Write_Page_Spare(u8 *write_data,
-	u32 block, u16 Page, u16 PageCount);
-
-extern u16 GLOB_LLD_Read_Page_Main_Spare(u8 *read_data,
-	u32 block, u16 page, u16 page_count);
-
-extern u16 GLOB_LLD_Read_Page_Spare(u8 *read_data,
-	u32 block, u16 Page, u16 PageCount);
-
-extern u16  GLOB_LLD_Get_Bad_Block(u32 block);
-
-extern u16 GLOB_LLD_Event_Status(void);
-
-extern u16 GLOB_LLD_MemCopy_CMD(u8 *dest, u8 *src, u32 ByteCount, u16 flag);
-
-extern u16 glob_lld_execute_cmds(void);
-
-extern u16 GLOB_LLD_Erase_Block_cdma(u32 block, u16 flags);
-
-extern u16 GLOB_LLD_Write_Page_Main_cdma(u8 *data,
-	u32 block, u16 page, u16 count);
-
-extern u16 GLOB_LLD_Read_Page_Main_cdma(u8 *data,
-	u32 block, u16 page, u16 count, u16 flags);
-
-extern u16 GLOB_LLD_Write_Page_Main_Spare_cdma(u8 *data,
-	u32 block, u16 page, u16 count, u16 flags);
-
-extern u16 GLOB_LLD_Read_Page_Main_Spare_cdma(u8 *data,
-	u32 block, u16 page, u16 count);
-
-#define LLD_CMD_FLAG_ORDER_BEFORE_REST		(0x1)
-#define LLD_CMD_FLAG_MODE_CDMA			(0x8)
-
-
-#endif /*_LLD_ */
-
-
diff --git a/drivers/staging/spectra/lld_cdma.c b/drivers/staging/spectra/lld_cdma.c
deleted file mode 100644
index c6e76103d43c..000000000000
--- a/drivers/staging/spectra/lld_cdma.c
+++ /dev/null
@@ -1,910 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#include <linux/fs.h>
-#include <linux/slab.h>
-
-#include "spectraswconfig.h"
-#include "lld.h"
-#include "lld_nand.h"
-#include "lld_cdma.h"
-#include "lld_emu.h"
-#include "flash.h"
-#include "nand_regs.h"
-
-#define MAX_PENDING_CMDS    4
-#define MODE_02             (0x2 << 26)
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     CDMA_Data_Cmd
-* Inputs:   cmd code (aligned for hw)
-*               data: pointer to source or destination
-*               block: block address
-*               page: page address
-*               num: num pages to transfer
-* Outputs:      PASS
-* Description:  This function takes the parameters and puts them
-*                   into the "pending commands" array.
-*               It does not parse or validate the parameters.
-*               The array index is same as the tag.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 CDMA_Data_CMD(u8 cmd, u8 *data, u32 block, u16 page, u16 num, u16 flags)
-{
-	u8 bank;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (0 == cmd)
-		nand_dbg_print(NAND_DBG_DEBUG,
-		"%s, Line %d, Illegal cmd (0)\n", __FILE__, __LINE__);
-
-	/* If a command of another bank comes, then first execute */
-	/* pending commands of the current bank, then set the new */
-	/* bank as current bank */
-	bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-	if (bank != info.flash_bank) {
-		nand_dbg_print(NAND_DBG_WARN,
-			"Will access new bank. old bank: %d, new bank: %d\n",
-			info.flash_bank, bank);
-		if (CDMA_Execute_CMDs()) {
-			printk(KERN_ERR "CDMA_Execute_CMDs fail!\n");
-			return FAIL;
-		}
-		info.flash_bank = bank;
-	}
-
-	info.pcmds[info.pcmds_num].CMD = cmd;
-	info.pcmds[info.pcmds_num].DataAddr = data;
-	info.pcmds[info.pcmds_num].Block = block;
-	info.pcmds[info.pcmds_num].Page = page;
-	info.pcmds[info.pcmds_num].PageCount = num;
-	info.pcmds[info.pcmds_num].DataDestAddr = 0;
-	info.pcmds[info.pcmds_num].DataSrcAddr = 0;
-	info.pcmds[info.pcmds_num].MemCopyByteCnt = 0;
-	info.pcmds[info.pcmds_num].Flags = flags;
-	info.pcmds[info.pcmds_num].Status = 0xB0B;
-
-	switch (cmd) {
-	case WRITE_MAIN_SPARE_CMD:
-		Conv_Main_Spare_Data_Log2Phy_Format(data, num);
-		break;
-	case WRITE_SPARE_CMD:
-		Conv_Spare_Data_Log2Phy_Format(data);
-		break;
-	default:
-		break;
-	}
-
-	info.pcmds_num++;
-
-	if (info.pcmds_num >= MAX_PENDING_CMDS) {
-		if (CDMA_Execute_CMDs()) {
-			printk(KERN_ERR "CDMA_Execute_CMDs fail!\n");
-			return FAIL;
-		}
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     CDMA_MemCopy_CMD
-* Inputs:       dest: pointer to destination
-*               src:  pointer to source
-*               count: num bytes to transfer
-* Outputs:      PASS
-* Description:  This function takes the parameters and puts them
-*                   into the "pending commands" array.
-*               It does not parse or validate the parameters.
-*               The array index is same as the tag.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 CDMA_MemCopy_CMD(u8 *dest, u8 *src, u32 byte_cnt, u16 flags)
-{
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	info.pcmds[info.pcmds_num].CMD = MEMCOPY_CMD;
-	info.pcmds[info.pcmds_num].DataAddr = 0;
-	info.pcmds[info.pcmds_num].Block = 0;
-	info.pcmds[info.pcmds_num].Page = 0;
-	info.pcmds[info.pcmds_num].PageCount = 0;
-	info.pcmds[info.pcmds_num].DataDestAddr = dest;
-	info.pcmds[info.pcmds_num].DataSrcAddr = src;
-	info.pcmds[info.pcmds_num].MemCopyByteCnt = byte_cnt;
-	info.pcmds[info.pcmds_num].Flags = flags;
-	info.pcmds[info.pcmds_num].Status = 0xB0B;
-
-	info.pcmds_num++;
-
-	if (info.pcmds_num >= MAX_PENDING_CMDS) {
-		if (CDMA_Execute_CMDs()) {
-			printk(KERN_ERR "CDMA_Execute_CMDs fail!\n");
-			return FAIL;
-		}
-	}
-
-	return PASS;
-}
-
-#if 0
-/* Prints the PendingCMDs array */
-void print_pending_cmds(void)
-{
-	u16 i;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	for (i = 0; i < info.pcmds_num; i++) {
-		nand_dbg_print(NAND_DBG_DEBUG, "\ni: %d\n", i);
-		switch (info.pcmds[i].CMD) {
-		case ERASE_CMD:
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Erase Command (0x%x)\n",
-				info.pcmds[i].CMD);
-			break;
-		case WRITE_MAIN_CMD:
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Write Main Command (0x%x)\n",
-				info.pcmds[i].CMD);
-			break;
-		case WRITE_MAIN_SPARE_CMD:
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Write Main Spare Command (0x%x)\n",
-				info.pcmds[i].CMD);
-			break;
-		case READ_MAIN_SPARE_CMD:
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Read Main Spare Command (0x%x)\n",
-				info.pcmds[i].CMD);
-			break;
-		case READ_MAIN_CMD:
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Read Main Command (0x%x)\n",
-				info.pcmds[i].CMD);
-			break;
-		case MEMCOPY_CMD:
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Memcopy Command (0x%x)\n",
-				info.pcmds[i].CMD);
-			break;
-		case DUMMY_CMD:
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Dummy Command (0x%x)\n",
-				info.pcmds[i].CMD);
-			break;
-		default:
-			nand_dbg_print(NAND_DBG_DEBUG,
-				"Illegal Command (0x%x)\n",
-				info.pcmds[i].CMD);
-			break;
-		}
-
-		nand_dbg_print(NAND_DBG_DEBUG, "DataAddr: 0x%x\n",
-			(u32)info.pcmds[i].DataAddr);
-		nand_dbg_print(NAND_DBG_DEBUG, "Block: %d\n",
-			info.pcmds[i].Block);
-		nand_dbg_print(NAND_DBG_DEBUG, "Page: %d\n",
-			info.pcmds[i].Page);
-		nand_dbg_print(NAND_DBG_DEBUG, "PageCount: %d\n",
-			info.pcmds[i].PageCount);
-		nand_dbg_print(NAND_DBG_DEBUG, "DataDestAddr: 0x%x\n",
-			(u32)info.pcmds[i].DataDestAddr);
-		nand_dbg_print(NAND_DBG_DEBUG, "DataSrcAddr: 0x%x\n",
-			(u32)info.pcmds[i].DataSrcAddr);
-		nand_dbg_print(NAND_DBG_DEBUG, "MemCopyByteCnt: %d\n",
-			info.pcmds[i].MemCopyByteCnt);
-		nand_dbg_print(NAND_DBG_DEBUG, "Flags: 0x%x\n",
-			info.pcmds[i].Flags);
-		nand_dbg_print(NAND_DBG_DEBUG, "Status: 0x%x\n",
-			info.pcmds[i].Status);
-	}
-}
-
-/* Print the CDMA descriptors */
-void print_cdma_descriptors(void)
-{
-	struct cdma_descriptor *pc;
-	int i;
-
-	pc = (struct cdma_descriptor *)info.cdma_desc_buf;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "\nWill dump cdma descriptors:\n");
-
-	for (i = 0; i < info.cdma_num; i++) {
-		nand_dbg_print(NAND_DBG_DEBUG, "\ni: %d\n", i);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"NxtPointerHi: 0x%x, NxtPointerLo: 0x%x\n",
-			pc[i].NxtPointerHi, pc[i].NxtPointerLo);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"FlashPointerHi: 0x%x, FlashPointerLo: 0x%x\n",
-			pc[i].FlashPointerHi, pc[i].FlashPointerLo);
-		nand_dbg_print(NAND_DBG_DEBUG, "CommandType: 0x%x\n",
-			pc[i].CommandType);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"MemAddrHi: 0x%x, MemAddrLo: 0x%x\n",
-			pc[i].MemAddrHi, pc[i].MemAddrLo);
-		nand_dbg_print(NAND_DBG_DEBUG, "CommandFlags: 0x%x\n",
-			pc[i].CommandFlags);
-		nand_dbg_print(NAND_DBG_DEBUG, "Channel: %d, Status: 0x%x\n",
-			pc[i].Channel, pc[i].Status);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"MemCopyPointerHi: 0x%x, MemCopyPointerLo: 0x%x\n",
-			pc[i].MemCopyPointerHi, pc[i].MemCopyPointerLo);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Reserved12: 0x%x, Reserved13: 0x%x, "
-			"Reserved14: 0x%x, pcmd: %d\n",
-			pc[i].Reserved12, pc[i].Reserved13,
-			pc[i].Reserved14, pc[i].pcmd);
-	}
-}
-
-/* Print the Memory copy descriptors */
-static void print_memcp_descriptors(void)
-{
-	struct memcpy_descriptor *pm;
-	int i;
-
-	pm = (struct memcpy_descriptor *)info.memcp_desc_buf;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "\nWill dump mem_cpy descriptors:\n");
-
-	for (i = 0; i < info.cdma_num; i++) {
-		nand_dbg_print(NAND_DBG_DEBUG, "\ni: %d\n", i);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"NxtPointerHi: 0x%x, NxtPointerLo: 0x%x\n",
-			pm[i].NxtPointerHi, pm[i].NxtPointerLo);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"SrcAddrHi: 0x%x, SrcAddrLo: 0x%x\n",
-			pm[i].SrcAddrHi, pm[i].SrcAddrLo);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"DestAddrHi: 0x%x, DestAddrLo: 0x%x\n",
-			pm[i].DestAddrHi, pm[i].DestAddrLo);
-		nand_dbg_print(NAND_DBG_DEBUG, "XferSize: %d\n",
-			pm[i].XferSize);
-		nand_dbg_print(NAND_DBG_DEBUG, "MemCopyFlags: 0x%x\n",
-			pm[i].MemCopyFlags);
-		nand_dbg_print(NAND_DBG_DEBUG, "MemCopyStatus: %d\n",
-			pm[i].MemCopyStatus);
-		nand_dbg_print(NAND_DBG_DEBUG, "reserved9: 0x%x\n",
-			pm[i].reserved9);
-		nand_dbg_print(NAND_DBG_DEBUG, "reserved10: 0x%x\n",
-			pm[i].reserved10);
-		nand_dbg_print(NAND_DBG_DEBUG, "reserved11: 0x%x\n",
-			pm[i].reserved11);
-		nand_dbg_print(NAND_DBG_DEBUG, "reserved12: 0x%x\n",
-			pm[i].reserved12);
-		nand_dbg_print(NAND_DBG_DEBUG, "reserved13: 0x%x\n",
-			pm[i].reserved13);
-		nand_dbg_print(NAND_DBG_DEBUG, "reserved14: 0x%x\n",
-			pm[i].reserved14);
-		nand_dbg_print(NAND_DBG_DEBUG, "reserved15: 0x%x\n",
-			pm[i].reserved15);
-	}
-}
-#endif
-
-/* Reset cdma_descriptor chain to 0 */
-static void reset_cdma_desc(int i)
-{
-	struct cdma_descriptor *ptr;
-
-	BUG_ON(i >= MAX_DESCS);
-
-	ptr = (struct cdma_descriptor *)info.cdma_desc_buf;
-
-	ptr[i].NxtPointerHi = 0;
-	ptr[i].NxtPointerLo = 0;
-	ptr[i].FlashPointerHi = 0;
-	ptr[i].FlashPointerLo = 0;
-	ptr[i].CommandType = 0;
-	ptr[i].MemAddrHi = 0;
-	ptr[i].MemAddrLo = 0;
-	ptr[i].CommandFlags = 0;
-	ptr[i].Channel = 0;
-	ptr[i].Status = 0;
-	ptr[i].MemCopyPointerHi = 0;
-	ptr[i].MemCopyPointerLo = 0;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     CDMA_UpdateEventStatus
-* Inputs:       none
-* Outputs:      none
-* Description:  This function update the event status of all the channels
-*               when an error condition is reported.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-void CDMA_UpdateEventStatus(void)
-{
-	int i, j, active_chan;
-	struct cdma_descriptor *ptr;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	ptr = (struct cdma_descriptor *)info.cdma_desc_buf;
-
-	for (j = 0; j < info.cdma_num; j++) {
-		/* Check for the descriptor with failure */
-		if ((ptr[j].Status & CMD_DMA_DESC_FAIL))
-			break;
-
-	}
-
-	/* All the previous cmd's status for this channel must be good */
-	for (i = 0; i < j; i++) {
-		if (ptr[i].pcmd != 0xff)
-			info.pcmds[ptr[i].pcmd].Status = CMD_PASS;
-	}
-
-	/* Abort the channel with type 0 reset command. It resets the */
-	/* selected channel after the descriptor completes the flash */
-	/* operation and status has been updated for the descriptor. */
-	/* Memory Copy and Sync associated with this descriptor will */
-	/* not be executed */
-	active_chan = ioread32(FlashReg + CHNL_ACTIVE);
-	if ((active_chan & (1 << info.flash_bank)) == (1 << info.flash_bank)) {
-		iowrite32(MODE_02 | (0 << 4), FlashMem); /* Type 0 reset */
-		iowrite32((0xF << 4) | info.flash_bank, FlashMem + 0x10);
-	} else { /* Should not reached here */
-		printk(KERN_ERR "Error! Used bank is not set in"
-			" reg CHNL_ACTIVE\n");
-	}
-}
-
-static void cdma_trans(u16 chan)
-{
-	u32 addr;
-
-	addr = info.cdma_desc;
-
-	iowrite32(MODE_10 | (chan << 24), FlashMem);
-	iowrite32((1 << 7) | chan, FlashMem + 0x10);
-
-	iowrite32(MODE_10 | (chan << 24) | ((0x0FFFF & (addr >> 16)) << 8),
-		FlashMem);
-	iowrite32((1 << 7) | (1 << 4) | 0, FlashMem + 0x10);
-
-	iowrite32(MODE_10 | (chan << 24) | ((0x0FFFF & addr) << 8), FlashMem);
-	iowrite32((1 << 7) | (1 << 5) | 0, FlashMem + 0x10);
-
-	iowrite32(MODE_10 | (chan << 24), FlashMem);
-	iowrite32((1 << 7) | (1 << 5) | (1 << 4) | 0, FlashMem + 0x10);
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     CDMA_Execute_CMDs (for use with CMD_DMA)
-* Inputs:       tag_count:  the number of pending cmds to do
-* Outputs:      PASS/FAIL
-* Description:  Build the SDMA chain(s) by making one CMD-DMA descriptor
-*               for each pending command, start the CDMA engine, and return.
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 CDMA_Execute_CMDs(void)
-{
-	int i, ret;
-	u64 flash_add;
-	u32 ptr;
-	dma_addr_t map_addr, next_ptr;
-	u16 status = PASS;
-	u16 tmp_c;
-	struct cdma_descriptor *pc;
-	struct memcpy_descriptor *pm;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	/* No pending cmds to execute, just exit */
-	if (0 == info.pcmds_num) {
-		nand_dbg_print(NAND_DBG_TRACE,
-			"No pending cmds to execute. Just exit.\n");
-		return PASS;
-	}
-
-	for (i = 0; i < MAX_DESCS; i++)
-		reset_cdma_desc(i);
-
-	pc = (struct cdma_descriptor *)info.cdma_desc_buf;
-	pm = (struct memcpy_descriptor *)info.memcp_desc_buf;
-
-	info.cdma_desc = virt_to_bus(info.cdma_desc_buf);
-	info.memcp_desc = virt_to_bus(info.memcp_desc_buf);
-	next_ptr = info.cdma_desc;
-	info.cdma_num = 0;
-
-	for (i = 0; i < info.pcmds_num; i++) {
-		if (info.pcmds[i].Block >= DeviceInfo.wTotalBlocks) {
-			info.pcmds[i].Status = CMD_NOT_DONE;
-			continue;
-		}
-
-		next_ptr += sizeof(struct cdma_descriptor);
-		pc[info.cdma_num].NxtPointerHi = next_ptr >> 16;
-		pc[info.cdma_num].NxtPointerLo = next_ptr & 0xffff;
-
-		/* Use the Block offset within a bank */
-		tmp_c = info.pcmds[i].Block /
-			(DeviceInfo.wTotalBlocks / totalUsedBanks);
-		flash_add = (u64)(info.pcmds[i].Block - tmp_c *
-			(DeviceInfo.wTotalBlocks / totalUsedBanks)) *
-			DeviceInfo.wBlockDataSize +
-			(u64)(info.pcmds[i].Page) *
-			DeviceInfo.wPageDataSize;
-
-		ptr = MODE_10 | (info.flash_bank << 24) |
-			(u32)GLOB_u64_Div(flash_add,
-				DeviceInfo.wPageDataSize);
-		pc[info.cdma_num].FlashPointerHi = ptr >> 16;
-		pc[info.cdma_num].FlashPointerLo = ptr & 0xffff;
-
-		if ((info.pcmds[i].CMD == WRITE_MAIN_SPARE_CMD) ||
-			(info.pcmds[i].CMD == READ_MAIN_SPARE_CMD)) {
-			/* Descriptor to set Main+Spare Access Mode */
-			pc[info.cdma_num].CommandType = 0x43;
-			pc[info.cdma_num].CommandFlags =
-				(0 << 10) | (1 << 9) | (0 << 8) | 0x40;
-			pc[info.cdma_num].MemAddrHi = 0;
-			pc[info.cdma_num].MemAddrLo = 0;
-			pc[info.cdma_num].Channel = 0;
-			pc[info.cdma_num].Status = 0;
-			pc[info.cdma_num].pcmd = i;
-
-			info.cdma_num++;
-			BUG_ON(info.cdma_num >= MAX_DESCS);
-
-			reset_cdma_desc(info.cdma_num);
-			next_ptr += sizeof(struct cdma_descriptor);
-			pc[info.cdma_num].NxtPointerHi = next_ptr >> 16;
-			pc[info.cdma_num].NxtPointerLo = next_ptr & 0xffff;
-			pc[info.cdma_num].FlashPointerHi = ptr >> 16;
-			pc[info.cdma_num].FlashPointerLo = ptr & 0xffff;
-		}
-
-		switch (info.pcmds[i].CMD) {
-		case ERASE_CMD:
-			pc[info.cdma_num].CommandType = 1;
-			pc[info.cdma_num].CommandFlags =
-				(0 << 10) | (1 << 9) | (0 << 8) | 0x40;
-			pc[info.cdma_num].MemAddrHi = 0;
-			pc[info.cdma_num].MemAddrLo = 0;
-			break;
-
-		case WRITE_MAIN_CMD:
-			pc[info.cdma_num].CommandType =
-				0x2100 | info.pcmds[i].PageCount;
-			pc[info.cdma_num].CommandFlags =
-				(0 << 10) | (1 << 9) | (0 << 8) | 0x40;
-			map_addr = virt_to_bus(info.pcmds[i].DataAddr);
-			pc[info.cdma_num].MemAddrHi = map_addr >> 16;
-			pc[info.cdma_num].MemAddrLo = map_addr & 0xffff;
-			break;
-
-		case READ_MAIN_CMD:
-			pc[info.cdma_num].CommandType =
-				0x2000 | info.pcmds[i].PageCount;
-			pc[info.cdma_num].CommandFlags =
-				(0 << 10) | (1 << 9) | (0 << 8) | 0x40;
-			map_addr = virt_to_bus(info.pcmds[i].DataAddr);
-			pc[info.cdma_num].MemAddrHi = map_addr >> 16;
-			pc[info.cdma_num].MemAddrLo = map_addr & 0xffff;
-			break;
-
-		case WRITE_MAIN_SPARE_CMD:
-			pc[info.cdma_num].CommandType =
-				0x2100 | info.pcmds[i].PageCount;
-			pc[info.cdma_num].CommandFlags =
-				(0 << 10) | (1 << 9) | (0 << 8) | 0x40;
-			map_addr = virt_to_bus(info.pcmds[i].DataAddr);
-			pc[info.cdma_num].MemAddrHi = map_addr >> 16;
-			pc[info.cdma_num].MemAddrLo = map_addr & 0xffff;
-			break;
-
-		case READ_MAIN_SPARE_CMD:
-			pc[info.cdma_num].CommandType =
-				0x2000 | info.pcmds[i].PageCount;
-			pc[info.cdma_num].CommandFlags =
-				(0 << 10) | (1 << 9) | (0 << 8) | 0x40;
-			map_addr = virt_to_bus(info.pcmds[i].DataAddr);
-			pc[info.cdma_num].MemAddrHi = map_addr >> 16;
-			pc[info.cdma_num].MemAddrLo = map_addr & 0xffff;
-			break;
-
-		case MEMCOPY_CMD:
-			pc[info.cdma_num].CommandType = 0xFFFF; /* NOP cmd */
-			/* Set bit 11 to let the CDMA engine continue to */
-			/* execute only after it has finished processing   */
-			/* the memcopy descriptor.                        */
-			/* Also set bit 10 and bit 9 to 1                  */
-			pc[info.cdma_num].CommandFlags = 0x0E40;
-			map_addr = info.memcp_desc + info.cdma_num *
-					sizeof(struct memcpy_descriptor);
-			pc[info.cdma_num].MemCopyPointerHi = map_addr >> 16;
-			pc[info.cdma_num].MemCopyPointerLo = map_addr & 0xffff;
-
-			pm[info.cdma_num].NxtPointerHi = 0;
-			pm[info.cdma_num].NxtPointerLo = 0;
-
-			map_addr = virt_to_bus(info.pcmds[i].DataSrcAddr);
-			pm[info.cdma_num].SrcAddrHi = map_addr >> 16;
-			pm[info.cdma_num].SrcAddrLo = map_addr & 0xffff;
-			map_addr = virt_to_bus(info.pcmds[i].DataDestAddr);
-			pm[info.cdma_num].DestAddrHi = map_addr >> 16;
-			pm[info.cdma_num].DestAddrLo = map_addr & 0xffff;
-
-			pm[info.cdma_num].XferSize =
-				info.pcmds[i].MemCopyByteCnt;
-			pm[info.cdma_num].MemCopyFlags =
-				(0 << 15 | 0 << 14 | 27 << 8 | 0x40);
-			pm[info.cdma_num].MemCopyStatus = 0;
-			break;
-
-		case DUMMY_CMD:
-		default:
-			pc[info.cdma_num].CommandType = 0XFFFF;
-			pc[info.cdma_num].CommandFlags =
-				(0 << 10) | (1 << 9) | (0 << 8) | 0x40;
-			pc[info.cdma_num].MemAddrHi = 0;
-			pc[info.cdma_num].MemAddrLo = 0;
-			break;
-		}
-
-		pc[info.cdma_num].Channel = 0;
-		pc[info.cdma_num].Status = 0;
-		pc[info.cdma_num].pcmd = i;
-
-		info.cdma_num++;
-		BUG_ON(info.cdma_num >= MAX_DESCS);
-
-		if ((info.pcmds[i].CMD == WRITE_MAIN_SPARE_CMD) ||
-			(info.pcmds[i].CMD == READ_MAIN_SPARE_CMD)) {
-			/* Descriptor to set back Main Area Access Mode */
-			reset_cdma_desc(info.cdma_num);
-			next_ptr += sizeof(struct cdma_descriptor);
-			pc[info.cdma_num].NxtPointerHi = next_ptr >> 16;
-			pc[info.cdma_num].NxtPointerLo = next_ptr & 0xffff;
-
-			pc[info.cdma_num].FlashPointerHi = ptr >> 16;
-			pc[info.cdma_num].FlashPointerLo = ptr & 0xffff;
-
-			pc[info.cdma_num].CommandType = 0x42;
-			pc[info.cdma_num].CommandFlags =
-				(0 << 10) | (1 << 9) | (0 << 8) | 0x40;
-			pc[info.cdma_num].MemAddrHi = 0;
-			pc[info.cdma_num].MemAddrLo = 0;
-
-			pc[info.cdma_num].Channel = 0;
-			pc[info.cdma_num].Status = 0;
-			pc[info.cdma_num].pcmd = i;
-
-			info.cdma_num++;
-			BUG_ON(info.cdma_num >= MAX_DESCS);
-		}
-	}
-
-	/* Add a dummy descriptor at end of the CDMA chain */
-	reset_cdma_desc(info.cdma_num);
-	ptr = MODE_10 | (info.flash_bank << 24);
-	pc[info.cdma_num].FlashPointerHi = ptr >> 16;
-	pc[info.cdma_num].FlashPointerLo = ptr & 0xffff;
-	pc[info.cdma_num].CommandType = 0xFFFF; /* NOP command */
-	/* Set Command Flags for the last CDMA descriptor: */
-	/* set Continue bit (bit 9) to 0 and Interrupt bit (bit 8) to 1 */
-	pc[info.cdma_num].CommandFlags =
-		(0 << 10) | (0 << 9) | (1 << 8) | 0x40;
-	pc[info.cdma_num].pcmd = 0xff; /* Set it to an illegal value */
-	info.cdma_num++;
-	BUG_ON(info.cdma_num >= MAX_DESCS);
-
-	iowrite32(1, FlashReg + GLOBAL_INT_ENABLE);  /* Enable Interrupt */
-
-	iowrite32(1, FlashReg + DMA_ENABLE);
-	/* Wait for DMA to be enabled before issuing the next command */
-	while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-	cdma_trans(info.flash_bank);
-
-	ret = wait_for_completion_timeout(&info.complete, 50 * HZ);
-	if (!ret)
-		printk(KERN_ERR "Wait for completion timeout "
-			"in %s, Line %d\n", __FILE__, __LINE__);
-	status = info.ret;
-
-	info.pcmds_num = 0; /* Clear the pending cmds number to 0 */
-
-	return status;
-}
-
-int is_cdma_interrupt(void)
-{
-	u32 ints_b0, ints_b1, ints_b2, ints_b3, ints_cdma;
-	u32 int_en_mask;
-	u32 cdma_int_en_mask;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	/* Set the global Enable masks for only those interrupts
-	 * that are supported */
-	cdma_int_en_mask = (DMA_INTR__DESC_COMP_CHANNEL0 |
-			DMA_INTR__DESC_COMP_CHANNEL1 |
-			DMA_INTR__DESC_COMP_CHANNEL2 |
-			DMA_INTR__DESC_COMP_CHANNEL3 |
-			DMA_INTR__MEMCOPY_DESC_COMP);
-
-	int_en_mask = (INTR_STATUS0__ECC_ERR |
-		INTR_STATUS0__PROGRAM_FAIL |
-		INTR_STATUS0__ERASE_FAIL);
-
-	ints_b0 = ioread32(FlashReg + INTR_STATUS0) & int_en_mask;
-	ints_b1 = ioread32(FlashReg + INTR_STATUS1) & int_en_mask;
-	ints_b2 = ioread32(FlashReg + INTR_STATUS2) & int_en_mask;
-	ints_b3 = ioread32(FlashReg + INTR_STATUS3) & int_en_mask;
-	ints_cdma = ioread32(FlashReg + DMA_INTR) & cdma_int_en_mask;
-
-	nand_dbg_print(NAND_DBG_WARN, "ints_bank0 to ints_bank3: "
-			"0x%x, 0x%x, 0x%x, 0x%x, ints_cdma: 0x%x\n",
-			ints_b0, ints_b1, ints_b2, ints_b3, ints_cdma);
-
-	if (ints_b0 || ints_b1 || ints_b2 || ints_b3 || ints_cdma) {
-		return 1;
-	} else {
-		iowrite32(ints_b0, FlashReg + INTR_STATUS0);
-		iowrite32(ints_b1, FlashReg + INTR_STATUS1);
-		iowrite32(ints_b2, FlashReg + INTR_STATUS2);
-		iowrite32(ints_b3, FlashReg + INTR_STATUS3);
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Not a NAND controller interrupt! Ignore it.\n");
-		return 0;
-	}
-}
-
-static void update_event_status(void)
-{
-	int i;
-	struct cdma_descriptor *ptr;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	ptr = (struct cdma_descriptor *)info.cdma_desc_buf;
-
-	for (i = 0; i < info.cdma_num; i++) {
-		if (ptr[i].pcmd != 0xff)
-			info.pcmds[ptr[i].pcmd].Status = CMD_PASS;
-		if ((ptr[i].CommandType == 0x41) ||
-			(ptr[i].CommandType == 0x42) ||
-			(ptr[i].CommandType == 0x43))
-			continue;
-
-		switch (info.pcmds[ptr[i].pcmd].CMD) {
-		case READ_MAIN_SPARE_CMD:
-			Conv_Main_Spare_Data_Phy2Log_Format(
-				info.pcmds[ptr[i].pcmd].DataAddr,
-				info.pcmds[ptr[i].pcmd].PageCount);
-			break;
-		case READ_SPARE_CMD:
-			Conv_Spare_Data_Phy2Log_Format(
-				info.pcmds[ptr[i].pcmd].DataAddr);
-			break;
-		}
-	}
-}
-
-static u16 do_ecc_for_desc(u32 ch, u8 *buf, u16 page)
-{
-	u16 event = EVENT_NONE;
-	u16 err_byte;
-	u16 err_page = 0;
-	u8 err_sector;
-	u8 err_device;
-	u16 ecc_correction_info;
-	u16 err_address;
-	u32 eccSectorSize;
-	u8 *err_pos;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	eccSectorSize = ECC_SECTOR_SIZE * (DeviceInfo.wDevicesConnected);
-
-	do {
-		if (0 == ch)
-			err_page = ioread32(FlashReg + ERR_PAGE_ADDR0);
-		else if (1 == ch)
-			err_page = ioread32(FlashReg + ERR_PAGE_ADDR1);
-		else if (2 == ch)
-			err_page = ioread32(FlashReg + ERR_PAGE_ADDR2);
-		else if (3 == ch)
-			err_page = ioread32(FlashReg + ERR_PAGE_ADDR3);
-
-		err_address = ioread32(FlashReg + ECC_ERROR_ADDRESS);
-		err_byte = err_address & ECC_ERROR_ADDRESS__OFFSET;
-		err_sector = ((err_address &
-			ECC_ERROR_ADDRESS__SECTOR_NR) >> 12);
-
-		ecc_correction_info = ioread32(FlashReg + ERR_CORRECTION_INFO);
-		err_device = ((ecc_correction_info &
-			ERR_CORRECTION_INFO__DEVICE_NR) >> 8);
-
-		if (ecc_correction_info & ERR_CORRECTION_INFO__ERROR_TYPE) {
-			event = EVENT_UNCORRECTABLE_DATA_ERROR;
-		} else {
-			event = EVENT_CORRECTABLE_DATA_ERROR_FIXED;
-			if (err_byte < ECC_SECTOR_SIZE) {
-				err_pos = buf +
-					(err_page - page) *
-					DeviceInfo.wPageDataSize +
-					err_sector * eccSectorSize +
-					err_byte *
-					DeviceInfo.wDevicesConnected +
-					err_device;
-				*err_pos ^= ecc_correction_info &
-					ERR_CORRECTION_INFO__BYTEMASK;
-			}
-		}
-	} while (!(ecc_correction_info & ERR_CORRECTION_INFO__LAST_ERR_INFO));
-
-	return event;
-}
-
-static u16 process_ecc_int(u32 c, u16 *p_desc_num)
-{
-	struct cdma_descriptor *ptr;
-	u16 j;
-	int event = EVENT_PASS;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (c != info.flash_bank)
-		printk(KERN_ERR "Error!info.flash_bank is %d, while c is %d\n",
-				info.flash_bank, c);
-
-	ptr = (struct cdma_descriptor *)info.cdma_desc_buf;
-
-	for (j = 0; j < info.cdma_num; j++)
-		if ((ptr[j].Status & CMD_DMA_DESC_COMP) != CMD_DMA_DESC_COMP)
-			break;
-
-	*p_desc_num = j; /* Pass the descripter number found here */
-
-	if (j >= info.cdma_num) {
-		printk(KERN_ERR "Can not find the correct descriptor number "
-			"when ecc interrupt triggered!"
-			"info.cdma_num: %d, j: %d\n", info.cdma_num, j);
-		return EVENT_UNCORRECTABLE_DATA_ERROR;
-	}
-
-	event = do_ecc_for_desc(c, info.pcmds[ptr[j].pcmd].DataAddr,
-		info.pcmds[ptr[j].pcmd].Page);
-
-	if (EVENT_UNCORRECTABLE_DATA_ERROR == event) {
-		printk(KERN_ERR "Uncorrectable ECC error!"
-			"info.cdma_num: %d, j: %d, "
-			"pending cmd CMD: 0x%x, "
-			"Block: 0x%x, Page: 0x%x, PageCount: 0x%x\n",
-			info.cdma_num, j,
-			info.pcmds[ptr[j].pcmd].CMD,
-			info.pcmds[ptr[j].pcmd].Block,
-			info.pcmds[ptr[j].pcmd].Page,
-			info.pcmds[ptr[j].pcmd].PageCount);
-
-		if (ptr[j].pcmd != 0xff)
-			info.pcmds[ptr[j].pcmd].Status = CMD_FAIL;
-		CDMA_UpdateEventStatus();
-	}
-
-	return event;
-}
-
-static void process_prog_erase_fail_int(u16 desc_num)
-{
-	struct cdma_descriptor *ptr;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	ptr = (struct cdma_descriptor *)info.cdma_desc_buf;
-
-	if (ptr[desc_num].pcmd != 0xFF)
-		info.pcmds[ptr[desc_num].pcmd].Status = CMD_FAIL;
-
-	CDMA_UpdateEventStatus();
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     CDMA_Event_Status (for use with CMD_DMA)
-* Inputs:       none
-* Outputs:      Event_Status code
-* Description:  This function is called after an interrupt has happened
-*               It reads the HW status register and ...tbd
-*               It returns the appropriate event status
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16  CDMA_Event_Status(void)
-{
-	u32 ints_addr[4] = {INTR_STATUS0, INTR_STATUS1,
-		INTR_STATUS2, INTR_STATUS3};
-	u32 dma_intr_bit[4] = {DMA_INTR__DESC_COMP_CHANNEL0,
-		DMA_INTR__DESC_COMP_CHANNEL1,
-		DMA_INTR__DESC_COMP_CHANNEL2,
-		DMA_INTR__DESC_COMP_CHANNEL3};
-	u32 cdma_int_status, int_status;
-	u32 ecc_enable = 0;
-	u16 event = EVENT_PASS;
-	u16 cur_desc = 0;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	ecc_enable = ioread32(FlashReg + ECC_ENABLE);
-
-	while (1) {
-		int_status = ioread32(FlashReg + ints_addr[info.flash_bank]);
-		if (ecc_enable && (int_status & INTR_STATUS0__ECC_ERR)) {
-			event = process_ecc_int(info.flash_bank, &cur_desc);
-			iowrite32(INTR_STATUS0__ECC_ERR,
-				FlashReg + ints_addr[info.flash_bank]);
-			if (EVENT_UNCORRECTABLE_DATA_ERROR == event) {
-				nand_dbg_print(NAND_DBG_WARN,
-					"ints_bank0 to ints_bank3: "
-					"0x%x, 0x%x, 0x%x, 0x%x, "
-					"ints_cdma: 0x%x\n",
-					ioread32(FlashReg + INTR_STATUS0),
-					ioread32(FlashReg + INTR_STATUS1),
-					ioread32(FlashReg + INTR_STATUS2),
-					ioread32(FlashReg + INTR_STATUS3),
-					ioread32(FlashReg + DMA_INTR));
-				break;
-			}
-		} else if (int_status & INTR_STATUS0__PROGRAM_FAIL) {
-			printk(KERN_ERR "NAND program fail interrupt!\n");
-			process_prog_erase_fail_int(cur_desc);
-			event = EVENT_PROGRAM_FAILURE;
-			break;
-		} else if (int_status & INTR_STATUS0__ERASE_FAIL) {
-			printk(KERN_ERR "NAND erase fail interrupt!\n");
-			process_prog_erase_fail_int(cur_desc);
-			event = EVENT_ERASE_FAILURE;
-			break;
-		} else {
-			cdma_int_status = ioread32(FlashReg + DMA_INTR);
-			if (cdma_int_status & dma_intr_bit[info.flash_bank]) {
-				iowrite32(dma_intr_bit[info.flash_bank],
-					FlashReg + DMA_INTR);
-				update_event_status();
-				event = EVENT_PASS;
-				break;
-			}
-		}
-	}
-
-	int_status = ioread32(FlashReg + ints_addr[info.flash_bank]);
-	iowrite32(int_status, FlashReg + ints_addr[info.flash_bank]);
-	cdma_int_status = ioread32(FlashReg + DMA_INTR);
-	iowrite32(cdma_int_status, FlashReg + DMA_INTR);
-
-	iowrite32(0, FlashReg + DMA_ENABLE);
-	while ((ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	return event;
-}
-
-
-
diff --git a/drivers/staging/spectra/lld_cdma.h b/drivers/staging/spectra/lld_cdma.h
deleted file mode 100644
index 854ea066f0c4..000000000000
--- a/drivers/staging/spectra/lld_cdma.h
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-/* header for LLD_CDMA.c module */
-
-#ifndef _LLD_CDMA_
-#define _LLD_CDMA_
-
-#include "flash.h"
-
-#define  DEBUG_SYNC    1
-
-/*///////////   CDMA specific MACRO definition */
-#define MAX_DESCS         (255)
-#define MAX_CHANS  (4)
-#define MAX_SYNC_POINTS         (16)
-#define MAX_DESC_PER_CHAN     (MAX_DESCS * 3 + MAX_SYNC_POINTS + 2)
-
-#define CHANNEL_SYNC_MASK       (0x000F)
-#define CHANNEL_DMA_MASK        (0x00F0)
-#define CHANNEL_ID_MASK         (0x0300)
-#define CHANNEL_CONT_MASK       (0x4000)
-#define CHANNEL_INTR_MASK       (0x8000)
-
-#define CHANNEL_SYNC_OFFSET     (0)
-#define CHANNEL_DMA_OFFSET      (4)
-#define CHANNEL_ID_OFFSET       (8)
-#define CHANNEL_CONT_OFFSET     (14)
-#define CHANNEL_INTR_OFFSET     (15)
-
-u16 CDMA_Data_CMD(u8 cmd, u8 *data, u32 block, u16 page, u16 num, u16 flags);
-u16 CDMA_MemCopy_CMD(u8 *dest, u8 *src, u32 byte_cnt, u16 flags);
-u16 CDMA_Execute_CMDs(void);
-void print_pending_cmds(void);
-void print_cdma_descriptors(void);
-
-extern u8 g_SBDCmdIndex;
-extern struct mrst_nand_info info;
-
-
-/*///////////   prototypes: APIs for LLD_CDMA */
-int is_cdma_interrupt(void);
-u16 CDMA_Event_Status(void);
-
-/* CMD-DMA Descriptor Struct.  These are defined by the CMD_DMA HW */
-struct cdma_descriptor {
-	u32 NxtPointerHi;
-	u32 NxtPointerLo;
-	u32 FlashPointerHi;
-	u32 FlashPointerLo;
-	u32 CommandType;
-	u32 MemAddrHi;
-	u32 MemAddrLo;
-	u32 CommandFlags;
-	u32 Channel;
-	u32 Status;
-	u32 MemCopyPointerHi;
-	u32 MemCopyPointerLo;
-	u32 Reserved12;
-	u32 Reserved13;
-	u32 Reserved14;
-	u32 pcmd; /* pending cmd num related to this descriptor */
-};
-
-/* This struct holds one MemCopy descriptor as defined by the HW */
-struct memcpy_descriptor {
-	u32 NxtPointerHi;
-	u32 NxtPointerLo;
-	u32 SrcAddrHi;
-	u32 SrcAddrLo;
-	u32 DestAddrHi;
-	u32 DestAddrLo;
-	u32 XferSize;
-	u32 MemCopyFlags;
-	u32 MemCopyStatus;
-	u32 reserved9;
-	u32 reserved10;
-	u32 reserved11;
-	u32 reserved12;
-	u32 reserved13;
-	u32 reserved14;
-	u32 reserved15;
-};
-
-/* Pending CMD table entries (includes MemCopy parameters */
-struct pending_cmd {
-	u8 CMD;
-	u8 *DataAddr;
-	u32 Block;
-	u16 Page;
-	u16 PageCount;
-	u8 *DataDestAddr;
-	u8 *DataSrcAddr;
-	u32 MemCopyByteCnt;
-	u16 Flags;
-	u16 Status;
-};
-
-#if DEBUG_SYNC
-extern u32 debug_sync_cnt;
-#endif
-
-/* Definitions for CMD DMA descriptor chain fields */
-#define     CMD_DMA_DESC_COMP   0x8000
-#define     CMD_DMA_DESC_FAIL   0x4000
-
-#endif /*_LLD_CDMA_*/
diff --git a/drivers/staging/spectra/lld_emu.c b/drivers/staging/spectra/lld_emu.c
deleted file mode 100644
index 095f2f0c2e5b..000000000000
--- a/drivers/staging/spectra/lld_emu.c
+++ /dev/null
@@ -1,776 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#include <linux/fs.h>
-#include <linux/slab.h>
-#include "flash.h"
-#include "ffsdefs.h"
-#include "lld_emu.h"
-#include "lld.h"
-#if CMD_DMA
-#include "lld_cdma.h"
-#if FLASH_EMU
-u32 totalUsedBanks;
-u32 valid_banks[MAX_CHANS];
-#endif
-#endif
-
-#define GLOB_LLD_PAGES           64
-#define GLOB_LLD_PAGE_SIZE       (512+16)
-#define GLOB_LLD_PAGE_DATA_SIZE  512
-#define GLOB_LLD_BLOCKS          2048
-
-#if FLASH_EMU			/* This is for entire module */
-
-static u8 *flash_memory[GLOB_LLD_BLOCKS * GLOB_LLD_PAGES];
-
-/* Read nand emu file and then fill it's content to flash_memory */
-int emu_load_file_to_mem(void)
-{
-	mm_segment_t fs;
-	struct file *nef_filp = NULL;
-	struct inode *inode = NULL;
-	loff_t nef_size = 0;
-	loff_t tmp_file_offset, file_offset;
-	ssize_t nread;
-	int i, rc = -EINVAL;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	fs = get_fs();
-	set_fs(get_ds());
-
-	nef_filp = filp_open("/root/nand_emu_file", O_RDWR | O_LARGEFILE, 0);
-	if (IS_ERR(nef_filp)) {
-		printk(KERN_ERR "filp_open error: "
-		       "Unable to open nand emu file!\n");
-		return PTR_ERR(nef_filp);
-	}
-
-	if (nef_filp->f_path.dentry) {
-		inode = nef_filp->f_path.dentry->d_inode;
-	} else {
-		printk(KERN_ERR "Can not get valid inode!\n");
-		goto out;
-	}
-
-	nef_size = i_size_read(inode->i_mapping->host);
-	if (nef_size <= 0) {
-		printk(KERN_ERR "Invalid nand emu file size: "
-		       "0x%llx\n", nef_size);
-		goto out;
-	} else {
-		nand_dbg_print(NAND_DBG_DEBUG, "nand emu file size: %lld\n",
-			       nef_size);
-	}
-
-	file_offset = 0;
-	for (i = 0; i < GLOB_LLD_BLOCKS * GLOB_LLD_PAGES; i++) {
-		tmp_file_offset = file_offset;
-		nread = vfs_read(nef_filp,
-				 (char __user *)flash_memory[i],
-				 GLOB_LLD_PAGE_SIZE, &tmp_file_offset);
-		if (nread < GLOB_LLD_PAGE_SIZE) {
-			printk(KERN_ERR "%s, Line %d - "
-			       "nand emu file partial read: "
-			       "%d bytes\n", __FILE__, __LINE__, (int)nread);
-			goto out;
-		}
-		file_offset += GLOB_LLD_PAGE_SIZE;
-	}
-	rc = 0;
-
-out:
-	filp_close(nef_filp, current->files);
-	set_fs(fs);
-	return rc;
-}
-
-/* Write contents of flash_memory to nand emu file */
-int emu_write_mem_to_file(void)
-{
-	mm_segment_t fs;
-	struct file *nef_filp = NULL;
-	struct inode *inode = NULL;
-	loff_t nef_size = 0;
-	loff_t tmp_file_offset, file_offset;
-	ssize_t nwritten;
-	int i, rc = -EINVAL;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	fs = get_fs();
-	set_fs(get_ds());
-
-	nef_filp = filp_open("/root/nand_emu_file", O_RDWR | O_LARGEFILE, 0);
-	if (IS_ERR(nef_filp)) {
-		printk(KERN_ERR "filp_open error: "
-		       "Unable to open nand emu file!\n");
-		return PTR_ERR(nef_filp);
-	}
-
-	if (nef_filp->f_path.dentry) {
-		inode = nef_filp->f_path.dentry->d_inode;
-	} else {
-		printk(KERN_ERR "Invalid " "nef_filp->f_path.dentry value!\n");
-		goto out;
-	}
-
-	nef_size = i_size_read(inode->i_mapping->host);
-	if (nef_size <= 0) {
-		printk(KERN_ERR "Invalid "
-		       "nand emu file size: 0x%llx\n", nef_size);
-		goto out;
-	} else {
-		nand_dbg_print(NAND_DBG_DEBUG, "nand emu file size: "
-			       "%lld\n", nef_size);
-	}
-
-	file_offset = 0;
-	for (i = 0; i < GLOB_LLD_BLOCKS * GLOB_LLD_PAGES; i++) {
-		tmp_file_offset = file_offset;
-		nwritten = vfs_write(nef_filp,
-				     (char __user *)flash_memory[i],
-				     GLOB_LLD_PAGE_SIZE, &tmp_file_offset);
-		if (nwritten < GLOB_LLD_PAGE_SIZE) {
-			printk(KERN_ERR "%s, Line %d - "
-			       "nand emu file partial write: "
-			       "%d bytes\n", __FILE__, __LINE__, (int)nwritten);
-			goto out;
-		}
-		file_offset += GLOB_LLD_PAGE_SIZE;
-	}
-	rc = 0;
-
-out:
-	filp_close(nef_filp, current->files);
-	set_fs(fs);
-	return rc;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Flash_Init
-* Inputs:       none
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:  Creates & initializes the flash RAM array.
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Flash_Init(void)
-{
-	int i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	flash_memory[0] = vmalloc(GLOB_LLD_PAGE_SIZE * GLOB_LLD_BLOCKS *
-				  GLOB_LLD_PAGES * sizeof(u8));
-	if (!flash_memory[0]) {
-		printk(KERN_ERR "Fail to allocate memory "
-		       "for nand emulator!\n");
-		return ERR;
-	}
-
-	memset((char *)(flash_memory[0]), 0xFF,
-	       GLOB_LLD_PAGE_SIZE * GLOB_LLD_BLOCKS * GLOB_LLD_PAGES *
-	       sizeof(u8));
-
-	for (i = 1; i < GLOB_LLD_BLOCKS * GLOB_LLD_PAGES; i++)
-		flash_memory[i] = flash_memory[i - 1] + GLOB_LLD_PAGE_SIZE;
-
-	emu_load_file_to_mem(); /* Load nand emu file to mem */
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Flash_Release
-* Inputs:       none
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Releases the flash.
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int emu_Flash_Release(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	emu_write_mem_to_file();  /* Write back mem to nand emu file */
-
-	vfree(flash_memory[0]);
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Read_Device_ID
-* Inputs:       none
-* Outputs:      PASS=1 FAIL=0
-* Description:  Reads the info from the controller registers.
-*               Sets up DeviceInfo structure with device parameters
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-
-u16 emu_Read_Device_ID(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	DeviceInfo.wDeviceMaker = 0;
-	DeviceInfo.wDeviceType = 8;
-	DeviceInfo.wSpectraStartBlock = 36;
-	DeviceInfo.wSpectraEndBlock = GLOB_LLD_BLOCKS - 1;
-	DeviceInfo.wTotalBlocks = GLOB_LLD_BLOCKS;
-	DeviceInfo.wPagesPerBlock = GLOB_LLD_PAGES;
-	DeviceInfo.wPageSize = GLOB_LLD_PAGE_SIZE;
-	DeviceInfo.wPageDataSize = GLOB_LLD_PAGE_DATA_SIZE;
-	DeviceInfo.wPageSpareSize = GLOB_LLD_PAGE_SIZE -
-	    GLOB_LLD_PAGE_DATA_SIZE;
-	DeviceInfo.wBlockSize = DeviceInfo.wPageSize * GLOB_LLD_PAGES;
-	DeviceInfo.wBlockDataSize = DeviceInfo.wPageDataSize * GLOB_LLD_PAGES;
-	DeviceInfo.wDataBlockNum = (u32) (DeviceInfo.wSpectraEndBlock -
-						DeviceInfo.wSpectraStartBlock
-						+ 1);
-	DeviceInfo.MLCDevice = 1; /* Emulate MLC device */
-	DeviceInfo.nBitsInPageNumber =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wPagesPerBlock);
-	DeviceInfo.nBitsInPageDataSize =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wPageDataSize);
-	DeviceInfo.nBitsInBlockDataSize =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wBlockDataSize);
-
-#if CMD_DMA
-	totalUsedBanks = 4;
-	valid_banks[0] = 1;
-	valid_banks[1] = 1;
-	valid_banks[2] = 1;
-	valid_banks[3] = 1;
-#endif
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Flash_Reset
-* Inputs:       none
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Reset the flash
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Flash_Reset(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Erase_Block
-* Inputs:       Address
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Erase a block
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Erase_Block(u32 block_add)
-{
-	int i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (block_add >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "emu_Erase_Block error! "
-		       "Too big block address: %d\n", block_add);
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Erasing block %d\n",
-		(int)block_add);
-
-	for (i = block_add * GLOB_LLD_PAGES;
-	     i < ((block_add + 1) * GLOB_LLD_PAGES); i++) {
-		if (flash_memory[i]) {
-			memset((u8 *)(flash_memory[i]), 0xFF,
-			       DeviceInfo.wPageSize * sizeof(u8));
-		}
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Write_Page_Main
-* Inputs:       Write buffer address pointer
-*               Block number
-*               Page  number
-*               Number of pages to process
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:  Write the data in the buffer to main area of flash
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Write_Page_Main(u8 *write_data, u32 Block,
-			   u16 Page, u16 PageCount)
-{
-	int i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks)
-		return FAIL;
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock)
-		return FAIL;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "emu_Write_Page_Main: "
-		       "lba %u Page %u PageCount %u\n",
-		       (unsigned int)Block,
-		       (unsigned int)Page, (unsigned int)PageCount);
-
-	for (i = 0; i < PageCount; i++) {
-		if (NULL == flash_memory[Block * GLOB_LLD_PAGES + Page]) {
-			printk(KERN_ERR "Run out of memory\n");
-			return FAIL;
-		}
-		memcpy((u8 *) (flash_memory[Block * GLOB_LLD_PAGES + Page]),
-		       write_data, DeviceInfo.wPageDataSize);
-		write_data += DeviceInfo.wPageDataSize;
-		Page++;
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Read_Page_Main
-* Inputs:       Read buffer address pointer
-*               Block number
-*               Page  number
-*               Number of pages to process
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:  Read the data from the flash main area to the buffer
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Read_Page_Main(u8 *read_data, u32 Block,
-			  u16 Page, u16 PageCount)
-{
-	int i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks)
-		return FAIL;
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock)
-		return FAIL;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "emu_Read_Page_Main: "
-		       "lba %u Page %u PageCount %u\n",
-		       (unsigned int)Block,
-		       (unsigned int)Page, (unsigned int)PageCount);
-
-	for (i = 0; i < PageCount; i++) {
-		if (NULL == flash_memory[Block * GLOB_LLD_PAGES + Page]) {
-			memset(read_data, 0xFF, DeviceInfo.wPageDataSize);
-		} else {
-			memcpy(read_data,
-			       (u8 *) (flash_memory[Block * GLOB_LLD_PAGES
-						      + Page]),
-			       DeviceInfo.wPageDataSize);
-		}
-		read_data += DeviceInfo.wPageDataSize;
-		Page++;
-	}
-
-	return PASS;
-}
-
-#ifndef ELDORA
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Read_Page_Main_Spare
-* Inputs:       Write Buffer
-*                       Address
-*                       Buffer size
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Read from flash main+spare area
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Read_Page_Main_Spare(u8 *read_data, u32 Block,
-				u16 Page, u16 PageCount)
-{
-	int i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "Read Page Main+Spare "
-		       "Error: Block Address too big\n");
-		return FAIL;
-	}
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock) {
-		printk(KERN_ERR "Read Page Main+Spare "
-		       "Error: Page number too big\n");
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Read Page Main + Spare - "
-		       "No. of pages %u block %u start page %u\n",
-		       (unsigned int)PageCount,
-		       (unsigned int)Block, (unsigned int)Page);
-
-	for (i = 0; i < PageCount; i++) {
-		if (NULL == flash_memory[Block * GLOB_LLD_PAGES + Page]) {
-			memset(read_data, 0xFF, DeviceInfo.wPageSize);
-		} else {
-			memcpy(read_data, (u8 *) (flash_memory[Block *
-								 GLOB_LLD_PAGES
-								 + Page]),
-			       DeviceInfo.wPageSize);
-		}
-
-		read_data += DeviceInfo.wPageSize;
-		Page++;
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Write_Page_Main_Spare
-* Inputs:       Write buffer
-*                       address
-*                       buffer length
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Write the buffer to main+spare area of flash
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Write_Page_Main_Spare(u8 *write_data, u32 Block,
-				 u16 Page, u16 page_count)
-{
-	u16 i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "Write Page Main + Spare "
-		       "Error: Block Address too big\n");
-		return FAIL;
-	}
-
-	if (Page + page_count > DeviceInfo.wPagesPerBlock) {
-		printk(KERN_ERR "Write Page Main + Spare "
-		       "Error: Page number too big\n");
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Write Page Main+Spare - "
-		       "No. of pages %u block %u start page %u\n",
-		       (unsigned int)page_count,
-		       (unsigned int)Block, (unsigned int)Page);
-
-	for (i = 0; i < page_count; i++) {
-		if (NULL == flash_memory[Block * GLOB_LLD_PAGES + Page]) {
-			printk(KERN_ERR "Run out of memory!\n");
-			return FAIL;
-		}
-		memcpy((u8 *) (flash_memory[Block * GLOB_LLD_PAGES + Page]),
-		       write_data, DeviceInfo.wPageSize);
-		write_data += DeviceInfo.wPageSize;
-		Page++;
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Write_Page_Spare
-* Inputs:       Write buffer
-*                       Address
-*                       buffer size
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Write the buffer in the spare area
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Write_Page_Spare(u8 *write_data, u32 Block,
-			    u16 Page, u16 PageCount)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "Read Page Spare Error: "
-		       "Block Address too big\n");
-		return FAIL;
-	}
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock) {
-		printk(KERN_ERR "Read Page Spare Error: "
-		       "Page number too big\n");
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Write Page Spare- "
-		       "block %u page %u\n",
-		       (unsigned int)Block, (unsigned int)Page);
-
-	if (NULL == flash_memory[Block * GLOB_LLD_PAGES + Page]) {
-		printk(KERN_ERR "Run out of memory!\n");
-		return FAIL;
-	}
-
-	memcpy((u8 *) (flash_memory[Block * GLOB_LLD_PAGES + Page] +
-			 DeviceInfo.wPageDataSize), write_data,
-	       (DeviceInfo.wPageSize - DeviceInfo.wPageDataSize));
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Read_Page_Spare
-* Inputs:       Write Buffer
-*                       Address
-*                       Buffer size
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Read data from the spare area
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_Read_Page_Spare(u8 *write_data, u32 Block,
-			   u16 Page, u16 PageCount)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "Read Page Spare "
-		       "Error: Block Address too big\n");
-		return FAIL;
-	}
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock) {
-		printk(KERN_ERR "Read Page Spare "
-		       "Error: Page number too big\n");
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Read Page Spare- "
-		       "block %u page %u\n",
-		       (unsigned int)Block, (unsigned int)Page);
-
-	if (NULL == flash_memory[Block * GLOB_LLD_PAGES + Page]) {
-		memset(write_data, 0xFF,
-		       (DeviceInfo.wPageSize - DeviceInfo.wPageDataSize));
-	} else {
-		memcpy(write_data,
-		       (u8 *) (flash_memory[Block * GLOB_LLD_PAGES + Page]
-				 + DeviceInfo.wPageDataSize),
-		       (DeviceInfo.wPageSize - DeviceInfo.wPageDataSize));
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Enable_Disable_Interrupts
-* Inputs:       enable or disable
-* Outputs:      none
-* Description:  NOP
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-void emu_Enable_Disable_Interrupts(u16 INT_ENABLE)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-}
-
-u16 emu_Get_Bad_Block(u32 block)
-{
-	return 0;
-}
-
-#if CMD_DMA
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Support for CDMA functions
-************************************
-*       emu_CDMA_Flash_Init
-*           CDMA_process_data command   (use LLD_CDMA)
-*           CDMA_MemCopy_CMD            (use LLD_CDMA)
-*       emu_CDMA_execute all commands
-*       emu_CDMA_Event_Status
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_CDMA_Flash_Init(void)
-{
-	u16 i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	for (i = 0; i < MAX_DESCS + MAX_CHANS; i++) {
-		PendingCMD[i].CMD = 0;
-		PendingCMD[i].Tag = 0;
-		PendingCMD[i].DataAddr = 0;
-		PendingCMD[i].Block = 0;
-		PendingCMD[i].Page = 0;
-		PendingCMD[i].PageCount = 0;
-		PendingCMD[i].DataDestAddr = 0;
-		PendingCMD[i].DataSrcAddr = 0;
-		PendingCMD[i].MemCopyByteCnt = 0;
-		PendingCMD[i].ChanSync[0] = 0;
-		PendingCMD[i].ChanSync[1] = 0;
-		PendingCMD[i].ChanSync[2] = 0;
-		PendingCMD[i].ChanSync[3] = 0;
-		PendingCMD[i].ChanSync[4] = 0;
-		PendingCMD[i].Status = 3;
-	}
-
-	return PASS;
-}
-
-static void emu_isr(int irq, void *dev_id)
-{
-	/* TODO:  ... */
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     CDMA_Execute_CMDs
-* Inputs:       tag_count:  the number of pending cmds to do
-* Outputs:      PASS/FAIL
-* Description:  execute each command in the pending CMD array
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_CDMA_Execute_CMDs(u16 tag_count)
-{
-	u16 i, j;
-	u8 CMD;		/* cmd parameter */
-	u8 *data;
-	u32 block;
-	u16 page;
-	u16 count;
-	u16 status = PASS;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	nand_dbg_print(NAND_DBG_TRACE, "At start of Execute CMDs: "
-		       "Tag Count %u\n", tag_count);
-
-	for (i = 0; i < totalUsedBanks; i++) {
-		PendingCMD[i].CMD = DUMMY_CMD;
-		PendingCMD[i].Tag = 0xFF;
-		PendingCMD[i].Block =
-		    (DeviceInfo.wTotalBlocks / totalUsedBanks) * i;
-
-		for (j = 0; j <= MAX_CHANS; j++)
-			PendingCMD[i].ChanSync[j] = 0;
-	}
-
-	CDMA_Execute_CMDs(tag_count);
-
-	print_pending_cmds(tag_count);
-
-#if DEBUG_SYNC
-	}
-	debug_sync_cnt++;
-#endif
-
-	for (i = MAX_CHANS;
-	     i < tag_count + MAX_CHANS; i++) {
-		CMD = PendingCMD[i].CMD;
-		data = PendingCMD[i].DataAddr;
-		block = PendingCMD[i].Block;
-		page = PendingCMD[i].Page;
-		count = PendingCMD[i].PageCount;
-
-		switch (CMD) {
-		case ERASE_CMD:
-			emu_Erase_Block(block);
-			PendingCMD[i].Status = PASS;
-			break;
-		case WRITE_MAIN_CMD:
-			emu_Write_Page_Main(data, block, page, count);
-			PendingCMD[i].Status = PASS;
-			break;
-		case WRITE_MAIN_SPARE_CMD:
-			emu_Write_Page_Main_Spare(data, block, page, count);
-			PendingCMD[i].Status = PASS;
-			break;
-		case READ_MAIN_CMD:
-			emu_Read_Page_Main(data, block, page, count);
-			PendingCMD[i].Status = PASS;
-			break;
-		case MEMCOPY_CMD:
-			memcpy(PendingCMD[i].DataDestAddr,
-			       PendingCMD[i].DataSrcAddr,
-			       PendingCMD[i].MemCopyByteCnt);
-		case DUMMY_CMD:
-			PendingCMD[i].Status = PASS;
-			break;
-		default:
-			PendingCMD[i].Status = FAIL;
-			break;
-		}
-	}
-
-	/*
-	 * Temperory adding code to reset PendingCMD array for basic testing.
-	 * It should be done at the end of  event status function.
-	 */
-	for (i = tag_count + MAX_CHANS; i < MAX_DESCS; i++) {
-		PendingCMD[i].CMD = 0;
-		PendingCMD[i].Tag = 0;
-		PendingCMD[i].DataAddr = 0;
-		PendingCMD[i].Block = 0;
-		PendingCMD[i].Page = 0;
-		PendingCMD[i].PageCount = 0;
-		PendingCMD[i].DataDestAddr = 0;
-		PendingCMD[i].DataSrcAddr = 0;
-		PendingCMD[i].MemCopyByteCnt = 0;
-		PendingCMD[i].ChanSync[0] = 0;
-		PendingCMD[i].ChanSync[1] = 0;
-		PendingCMD[i].ChanSync[2] = 0;
-		PendingCMD[i].ChanSync[3] = 0;
-		PendingCMD[i].ChanSync[4] = 0;
-		PendingCMD[i].Status = CMD_NOT_DONE;
-	}
-
-	nand_dbg_print(NAND_DBG_TRACE, "At end of Execute CMDs.\n");
-
-	emu_isr(0, 0); /* This is a null isr now. Need fill it in future */
-
-	return status;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     emu_Event_Status
-* Inputs:       none
-* Outputs:      Event_Status code
-* Description:  This function can also be used to force errors
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 emu_CDMA_Event_Status(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	return EVENT_PASS;
-}
-
-#endif /* CMD_DMA */
-#endif /* !ELDORA */
-#endif /* FLASH_EMU */
diff --git a/drivers/staging/spectra/lld_emu.h b/drivers/staging/spectra/lld_emu.h
deleted file mode 100644
index 63f84c38d3c1..000000000000
--- a/drivers/staging/spectra/lld_emu.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#ifndef _LLD_EMU_
-#define _LLD_EMU_
-
-#include "ffsport.h"
-#include "ffsdefs.h"
-
-/* prototypes: emulator API functions */
-extern u16 emu_Flash_Reset(void);
-extern u16 emu_Flash_Init(void);
-extern int emu_Flash_Release(void);
-extern u16 emu_Read_Device_ID(void);
-extern u16 emu_Erase_Block(u32 block_addr);
-extern u16 emu_Write_Page_Main(u8 *write_data, u32 Block,
-				u16 Page, u16 PageCount);
-extern u16 emu_Read_Page_Main(u8 *read_data, u32 Block, u16 Page,
-				 u16 PageCount);
-extern u16 emu_Event_Status(void);
-extern void emu_Enable_Disable_Interrupts(u16 INT_ENABLE);
-extern u16 emu_Write_Page_Main_Spare(u8 *write_data, u32 Block,
-					u16 Page, u16 PageCount);
-extern u16 emu_Write_Page_Spare(u8 *write_data, u32 Block,
-					u16 Page, u16 PageCount);
-extern u16 emu_Read_Page_Main_Spare(u8 *read_data, u32 Block,
-				       u16 Page, u16 PageCount);
-extern u16 emu_Read_Page_Spare(u8 *read_data, u32 Block, u16 Page,
-				  u16 PageCount);
-extern u16 emu_Get_Bad_Block(u32 block);
-
-u16 emu_CDMA_Flash_Init(void);
-u16 emu_CDMA_Execute_CMDs(u16 tag_count);
-u16 emu_CDMA_Event_Status(void);
-#endif /*_LLD_EMU_*/
diff --git a/drivers/staging/spectra/lld_mtd.c b/drivers/staging/spectra/lld_mtd.c
deleted file mode 100644
index a9c309a167c2..000000000000
--- a/drivers/staging/spectra/lld_mtd.c
+++ /dev/null
@@ -1,683 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#include <linux/fs.h>
-#include <linux/slab.h>
-#include <linux/mtd/mtd.h>
-#include "flash.h"
-#include "ffsdefs.h"
-#include "lld_emu.h"
-#include "lld.h"
-#if CMD_DMA
-#include "lld_cdma.h"
-u32 totalUsedBanks;
-u32 valid_banks[MAX_CHANS];
-#endif
-
-#define GLOB_LLD_PAGES           64
-#define GLOB_LLD_PAGE_SIZE       (512+16)
-#define GLOB_LLD_PAGE_DATA_SIZE  512
-#define GLOB_LLD_BLOCKS          2048
-
-static struct mtd_info *spectra_mtd;
-static int mtddev = -1;
-module_param(mtddev, int, 0);
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Flash_Init
-* Inputs:       none
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:  Creates & initializes the flash RAM array.
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Flash_Init(void)
-{
-	if (mtddev == -1) {
-		printk(KERN_ERR "No MTD device specified. Give mtddev parameter\n");
-		return FAIL;
-	}
-
-	spectra_mtd = get_mtd_device(NULL, mtddev);
-	if (!spectra_mtd) {
-		printk(KERN_ERR "Failed to obtain MTD device #%d\n", mtddev);
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Flash_Release
-* Inputs:       none
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Releases the flash.
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-int mtd_Flash_Release(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-	if (!spectra_mtd)
-		return PASS;
-
-	put_mtd_device(spectra_mtd);
-	spectra_mtd = NULL;
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Read_Device_ID
-* Inputs:       none
-* Outputs:      PASS=1 FAIL=0
-* Description:  Reads the info from the controller registers.
-*               Sets up DeviceInfo structure with device parameters
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-
-u16 mtd_Read_Device_ID(void)
-{
-	uint64_t tmp;
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (!spectra_mtd)
-		return FAIL;
-
-	DeviceInfo.wDeviceMaker = 0;
-	DeviceInfo.wDeviceType = 8;
-	DeviceInfo.wSpectraStartBlock = SPECTRA_START_BLOCK;
-	tmp = spectra_mtd->size;
-	do_div(tmp, spectra_mtd->erasesize);
-	DeviceInfo.wTotalBlocks = tmp;
-	DeviceInfo.wSpectraEndBlock = DeviceInfo.wTotalBlocks - 1;
-	DeviceInfo.wPagesPerBlock = spectra_mtd->erasesize / spectra_mtd->writesize;
-	DeviceInfo.wPageSize = spectra_mtd->writesize + spectra_mtd->oobsize;
-	DeviceInfo.wPageDataSize = spectra_mtd->writesize;
-	DeviceInfo.wPageSpareSize = spectra_mtd->oobsize;
-	DeviceInfo.wBlockSize = DeviceInfo.wPageSize * DeviceInfo.wPagesPerBlock;
-	DeviceInfo.wBlockDataSize = DeviceInfo.wPageDataSize * DeviceInfo.wPagesPerBlock;
-	DeviceInfo.wDataBlockNum = (u32) (DeviceInfo.wSpectraEndBlock -
-						DeviceInfo.wSpectraStartBlock
-						+ 1);
-	DeviceInfo.MLCDevice = 0;//spectra_mtd->celltype & NAND_CI_CELLTYPE_MSK;
-	DeviceInfo.nBitsInPageNumber =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wPagesPerBlock);
-	DeviceInfo.nBitsInPageDataSize =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wPageDataSize);
-	DeviceInfo.nBitsInBlockDataSize =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wBlockDataSize);
-
-#if CMD_DMA
-	totalUsedBanks = 4;
-	valid_banks[0] = 1;
-	valid_banks[1] = 1;
-	valid_banks[2] = 1;
-	valid_banks[3] = 1;
-#endif
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Flash_Reset
-* Inputs:       none
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Reset the flash
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Flash_Reset(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	return PASS;
-}
-
-void erase_callback(struct erase_info *e)
-{
-	complete((void *)e->priv);
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Erase_Block
-* Inputs:       Address
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Erase a block
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Erase_Block(u32 block_add)
-{
-	struct erase_info erase;
-	DECLARE_COMPLETION_ONSTACK(comp);
-	int ret;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (block_add >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "mtd_Erase_Block error! "
-		       "Too big block address: %d\n", block_add);
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Erasing block %d\n",
-		(int)block_add);
-
-	erase.mtd = spectra_mtd;
-	erase.callback = erase_callback;
-	erase.addr = block_add * spectra_mtd->erasesize;
-	erase.len = spectra_mtd->erasesize;
-	erase.priv = (unsigned long)&comp;
-
-	ret = spectra_mtd->erase(spectra_mtd, &erase);
-	if (!ret) {
-		wait_for_completion(&comp);
-		if (erase.state != MTD_ERASE_DONE)
-			ret = -EIO;
-	}
-	if (ret) {
-		printk(KERN_WARNING "mtd_Erase_Block error! "
-		       "erase of region [0x%llx, 0x%llx] failed\n",
-		       erase.addr, erase.len);
-		return FAIL;
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Write_Page_Main
-* Inputs:       Write buffer address pointer
-*               Block number
-*               Page  number
-*               Number of pages to process
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:  Write the data in the buffer to main area of flash
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Write_Page_Main(u8 *write_data, u32 Block,
-			   u16 Page, u16 PageCount)
-{
-	size_t retlen;
-	int ret = 0;
-
-	if (Block >= DeviceInfo.wTotalBlocks)
-		return FAIL;
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock)
-		return FAIL;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "mtd_Write_Page_Main: "
-		       "lba %u Page %u PageCount %u\n",
-		       (unsigned int)Block,
-		       (unsigned int)Page, (unsigned int)PageCount);
-
-
-	while (PageCount) {
-		ret = spectra_mtd->write(spectra_mtd,
-					 (Block * spectra_mtd->erasesize) + (Page * spectra_mtd->writesize),
-					 DeviceInfo.wPageDataSize, &retlen, write_data);
-		if (ret) {
-			printk(KERN_ERR "%s failed %d\n", __func__, ret);
-			return FAIL;
-		}
-		write_data += DeviceInfo.wPageDataSize;
-		Page++;
-		PageCount--;
-	}
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Read_Page_Main
-* Inputs:       Read buffer address pointer
-*               Block number
-*               Page  number
-*               Number of pages to process
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:  Read the data from the flash main area to the buffer
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Read_Page_Main(u8 *read_data, u32 Block,
-			  u16 Page, u16 PageCount)
-{
-	size_t retlen;
-	int ret = 0;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks)
-		return FAIL;
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock)
-		return FAIL;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "mtd_Read_Page_Main: "
-		       "lba %u Page %u PageCount %u\n",
-		       (unsigned int)Block,
-		       (unsigned int)Page, (unsigned int)PageCount);
-
-
-	while (PageCount) {
-		ret = spectra_mtd->read(spectra_mtd,
-					(Block * spectra_mtd->erasesize) + (Page * spectra_mtd->writesize),
-					DeviceInfo.wPageDataSize, &retlen, read_data);
-		if (ret) {
-			printk(KERN_ERR "%s failed %d\n", __func__, ret);
-			return FAIL;
-		}
-		read_data += DeviceInfo.wPageDataSize;
-		Page++;
-		PageCount--;
-	}
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	return PASS;
-}
-
-#ifndef ELDORA
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Read_Page_Main_Spare
-* Inputs:       Write Buffer
-*                       Address
-*                       Buffer size
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Read from flash main+spare area
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Read_Page_Main_Spare(u8 *read_data, u32 Block,
-				u16 Page, u16 PageCount)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "Read Page Main+Spare "
-		       "Error: Block Address too big\n");
-		return FAIL;
-	}
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock) {
-		printk(KERN_ERR "Read Page Main+Spare "
-		       "Error: Page number %d+%d too big in block %d\n",
-		       Page, PageCount, Block);
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Read Page Main + Spare - "
-		       "No. of pages %u block %u start page %u\n",
-		       (unsigned int)PageCount,
-		       (unsigned int)Block, (unsigned int)Page);
-
-
-	while (PageCount) {
-		struct mtd_oob_ops ops;
-		int ret;
-
-		ops.mode = MTD_OPS_AUTO_OOB;
-		ops.datbuf = read_data;
-		ops.len = DeviceInfo.wPageDataSize;
-		ops.oobbuf = read_data + DeviceInfo.wPageDataSize + BTSIG_OFFSET;
-		ops.ooblen = BTSIG_BYTES;
-		ops.ooboffs = 0;
-
-		ret = spectra_mtd->read_oob(spectra_mtd,
-					    (Block * spectra_mtd->erasesize) + (Page * spectra_mtd->writesize),
-					    &ops);
-		if (ret) {
-			printk(KERN_ERR "%s failed %d\n", __func__, ret);
-			return FAIL;
-		}
-		read_data += DeviceInfo.wPageSize;
-		Page++;
-		PageCount--;
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Write_Page_Main_Spare
-* Inputs:       Write buffer
-*                       address
-*                       buffer length
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Write the buffer to main+spare area of flash
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Write_Page_Main_Spare(u8 *write_data, u32 Block,
-				 u16 Page, u16 page_count)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "Write Page Main + Spare "
-		       "Error: Block Address too big\n");
-		return FAIL;
-	}
-
-	if (Page + page_count > DeviceInfo.wPagesPerBlock) {
-		printk(KERN_ERR "Write Page Main + Spare "
-		       "Error: Page number %d+%d too big in block %d\n",
-		       Page, page_count, Block);
-		WARN_ON(1);
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Write Page Main+Spare - "
-		       "No. of pages %u block %u start page %u\n",
-		       (unsigned int)page_count,
-		       (unsigned int)Block, (unsigned int)Page);
-
-	while (page_count) {
-		struct mtd_oob_ops ops;
-		int ret;
-
-		ops.mode = MTD_OPS_AUTO_OOB;
-		ops.datbuf = write_data;
-		ops.len = DeviceInfo.wPageDataSize;
-		ops.oobbuf = write_data + DeviceInfo.wPageDataSize + BTSIG_OFFSET;
-		ops.ooblen = BTSIG_BYTES;
-		ops.ooboffs = 0;
-
-		ret = spectra_mtd->write_oob(spectra_mtd,
-					     (Block * spectra_mtd->erasesize) + (Page * spectra_mtd->writesize),
-					     &ops);
-		if (ret) {
-			printk(KERN_ERR "%s failed %d\n", __func__, ret);
-			return FAIL;
-		}
-		write_data += DeviceInfo.wPageSize;
-		Page++;
-		page_count--;
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Write_Page_Spare
-* Inputs:       Write buffer
-*                       Address
-*                       buffer size
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Write the buffer in the spare area
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Write_Page_Spare(u8 *write_data, u32 Block,
-			    u16 Page, u16 PageCount)
-{
-	WARN_ON(1);
-	return FAIL;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Read_Page_Spare
-* Inputs:       Write Buffer
-*                       Address
-*                       Buffer size
-* Outputs:      PASS=0 (notice 0=ok here)
-* Description:          Read data from the spare area
-*
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_Read_Page_Spare(u8 *read_data, u32 Block,
-			   u16 Page, u16 PageCount)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (Block >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "Read Page Spare "
-		       "Error: Block Address too big\n");
-		return FAIL;
-	}
-
-	if (Page + PageCount > DeviceInfo.wPagesPerBlock) {
-		printk(KERN_ERR "Read Page Spare "
-		       "Error: Page number too big\n");
-		return FAIL;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Read Page Spare- "
-		       "block %u page %u (%u pages)\n",
-		       (unsigned int)Block, (unsigned int)Page, PageCount);
-
-	while (PageCount) {
-		struct mtd_oob_ops ops;
-		int ret;
-
-		ops.mode = MTD_OPS_AUTO_OOB;
-		ops.datbuf = NULL;
-		ops.len = 0;
-		ops.oobbuf = read_data;
-		ops.ooblen = BTSIG_BYTES;
-		ops.ooboffs = 0;
-
-		ret = spectra_mtd->read_oob(spectra_mtd,
-					    (Block * spectra_mtd->erasesize) + (Page * spectra_mtd->writesize),
-					    &ops);
-		if (ret) {
-			printk(KERN_ERR "%s failed %d\n", __func__, ret);
-			return FAIL;
-		}
-
-		read_data += DeviceInfo.wPageSize;
-		Page++;
-		PageCount--;
-	}
-
-	return PASS;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Enable_Disable_Interrupts
-* Inputs:       enable or disable
-* Outputs:      none
-* Description:  NOP
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-void mtd_Enable_Disable_Interrupts(u16 INT_ENABLE)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-}
-
-u16 mtd_Get_Bad_Block(u32 block)
-{
-	return 0;
-}
-
-#if CMD_DMA
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Support for CDMA functions
-************************************
-*       mtd_CDMA_Flash_Init
-*           CDMA_process_data command   (use LLD_CDMA)
-*           CDMA_MemCopy_CMD            (use LLD_CDMA)
-*       mtd_CDMA_execute all commands
-*       mtd_CDMA_Event_Status
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_CDMA_Flash_Init(void)
-{
-	u16 i;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	for (i = 0; i < MAX_DESCS + MAX_CHANS; i++) {
-		PendingCMD[i].CMD = 0;
-		PendingCMD[i].Tag = 0;
-		PendingCMD[i].DataAddr = 0;
-		PendingCMD[i].Block = 0;
-		PendingCMD[i].Page = 0;
-		PendingCMD[i].PageCount = 0;
-		PendingCMD[i].DataDestAddr = 0;
-		PendingCMD[i].DataSrcAddr = 0;
-		PendingCMD[i].MemCopyByteCnt = 0;
-		PendingCMD[i].ChanSync[0] = 0;
-		PendingCMD[i].ChanSync[1] = 0;
-		PendingCMD[i].ChanSync[2] = 0;
-		PendingCMD[i].ChanSync[3] = 0;
-		PendingCMD[i].ChanSync[4] = 0;
-		PendingCMD[i].Status = 3;
-	}
-
-	return PASS;
-}
-
-static void mtd_isr(int irq, void *dev_id)
-{
-	/* TODO:  ... */
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     CDMA_Execute_CMDs
-* Inputs:       tag_count:  the number of pending cmds to do
-* Outputs:      PASS/FAIL
-* Description:  execute each command in the pending CMD array
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_CDMA_Execute_CMDs(u16 tag_count)
-{
-	u16 i, j;
-	u8 CMD;		/* cmd parameter */
-	u8 *data;
-	u32 block;
-	u16 page;
-	u16 count;
-	u16 status = PASS;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	nand_dbg_print(NAND_DBG_TRACE, "At start of Execute CMDs: "
-		       "Tag Count %u\n", tag_count);
-
-	for (i = 0; i < totalUsedBanks; i++) {
-		PendingCMD[i].CMD = DUMMY_CMD;
-		PendingCMD[i].Tag = 0xFF;
-		PendingCMD[i].Block =
-		    (DeviceInfo.wTotalBlocks / totalUsedBanks) * i;
-
-		for (j = 0; j <= MAX_CHANS; j++)
-			PendingCMD[i].ChanSync[j] = 0;
-	}
-
-	CDMA_Execute_CMDs(tag_count);
-
-#ifdef VERBOSE
-		print_pending_cmds(tag_count);
-#endif
-#if DEBUG_SYNC
-	}
-	debug_sync_cnt++;
-#endif
-
-	for (i = MAX_CHANS;
-	     i < tag_count + MAX_CHANS; i++) {
-		CMD = PendingCMD[i].CMD;
-		data = PendingCMD[i].DataAddr;
-		block = PendingCMD[i].Block;
-		page = PendingCMD[i].Page;
-		count = PendingCMD[i].PageCount;
-
-		switch (CMD) {
-		case ERASE_CMD:
-			mtd_Erase_Block(block);
-			PendingCMD[i].Status = PASS;
-			break;
-		case WRITE_MAIN_CMD:
-			mtd_Write_Page_Main(data, block, page, count);
-			PendingCMD[i].Status = PASS;
-			break;
-		case WRITE_MAIN_SPARE_CMD:
-			mtd_Write_Page_Main_Spare(data, block, page, count);
-			PendingCMD[i].Status = PASS;
-			break;
-		case READ_MAIN_CMD:
-			mtd_Read_Page_Main(data, block, page, count);
-			PendingCMD[i].Status = PASS;
-			break;
-		case MEMCOPY_CMD:
-			memcpy(PendingCMD[i].DataDestAddr,
-			       PendingCMD[i].DataSrcAddr,
-			       PendingCMD[i].MemCopyByteCnt);
-		case DUMMY_CMD:
-			PendingCMD[i].Status = PASS;
-			break;
-		default:
-			PendingCMD[i].Status = FAIL;
-			break;
-		}
-	}
-
-	/*
-	 * Temperory adding code to reset PendingCMD array for basic testing.
-	 * It should be done at the end of  event status function.
-	 */
-	for (i = tag_count + MAX_CHANS; i < MAX_DESCS; i++) {
-		PendingCMD[i].CMD = 0;
-		PendingCMD[i].Tag = 0;
-		PendingCMD[i].DataAddr = 0;
-		PendingCMD[i].Block = 0;
-		PendingCMD[i].Page = 0;
-		PendingCMD[i].PageCount = 0;
-		PendingCMD[i].DataDestAddr = 0;
-		PendingCMD[i].DataSrcAddr = 0;
-		PendingCMD[i].MemCopyByteCnt = 0;
-		PendingCMD[i].ChanSync[0] = 0;
-		PendingCMD[i].ChanSync[1] = 0;
-		PendingCMD[i].ChanSync[2] = 0;
-		PendingCMD[i].ChanSync[3] = 0;
-		PendingCMD[i].ChanSync[4] = 0;
-		PendingCMD[i].Status = CMD_NOT_DONE;
-	}
-
-	nand_dbg_print(NAND_DBG_TRACE, "At end of Execute CMDs.\n");
-
-	mtd_isr(0, 0); /* This is a null isr now. Need fill it in future */
-
-	return status;
-}
-
-/*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
-* Function:     mtd_Event_Status
-* Inputs:       none
-* Outputs:      Event_Status code
-* Description:  This function can also be used to force errors
-*&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
-u16 mtd_CDMA_Event_Status(void)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	return EVENT_PASS;
-}
-
-#endif /* CMD_DMA */
-#endif /* !ELDORA */
diff --git a/drivers/staging/spectra/lld_mtd.h b/drivers/staging/spectra/lld_mtd.h
deleted file mode 100644
index 4e81ee87b53d..000000000000
--- a/drivers/staging/spectra/lld_mtd.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#ifndef _LLD_MTD_
-#define _LLD_MTD_
-
-#include "ffsport.h"
-#include "ffsdefs.h"
-
-/* prototypes: MTD API functions */
-extern u16 mtd_Flash_Reset(void);
-extern u16 mtd_Flash_Init(void);
-extern int mtd_Flash_Release(void);
-extern u16 mtd_Read_Device_ID(void);
-extern u16 mtd_Erase_Block(u32 block_addr);
-extern u16 mtd_Write_Page_Main(u8 *write_data, u32 Block,
-				u16 Page, u16 PageCount);
-extern u16 mtd_Read_Page_Main(u8 *read_data, u32 Block, u16 Page,
-				 u16 PageCount);
-extern u16 mtd_Event_Status(void);
-extern void mtd_Enable_Disable_Interrupts(u16 INT_ENABLE);
-extern u16 mtd_Write_Page_Main_Spare(u8 *write_data, u32 Block,
-					u16 Page, u16 PageCount);
-extern u16 mtd_Write_Page_Spare(u8 *write_data, u32 Block,
-					u16 Page, u16 PageCount);
-extern u16 mtd_Read_Page_Main_Spare(u8 *read_data, u32 Block,
-				       u16 Page, u16 PageCount);
-extern u16 mtd_Read_Page_Spare(u8 *read_data, u32 Block, u16 Page,
-				  u16 PageCount);
-extern u16 mtd_Get_Bad_Block(u32 block);
-
-u16 mtd_CDMA_Flash_Init(void);
-u16 mtd_CDMA_Execute_CMDs(u16 tag_count);
-u16 mtd_CDMA_Event_Status(void);
-#endif /*_LLD_MTD_*/
diff --git a/drivers/staging/spectra/lld_nand.c b/drivers/staging/spectra/lld_nand.c
deleted file mode 100644
index 60a14ff26c7f..000000000000
--- a/drivers/staging/spectra/lld_nand.c
+++ /dev/null
@@ -1,2619 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#include "lld.h"
-#include "lld_nand.h"
-#include "lld_cdma.h"
-
-#include "spectraswconfig.h"
-#include "flash.h"
-#include "ffsdefs.h"
-
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/wait.h>
-#include <linux/mutex.h>
-
-#include "nand_regs.h"
-
-#define SPECTRA_NAND_NAME    "nd"
-
-#define CEIL_DIV(X, Y) (((X)%(Y)) ? ((X)/(Y)+1) : ((X)/(Y)))
-#define MAX_PAGES_PER_RW        128
-
-#define INT_IDLE_STATE                 0
-#define INT_READ_PAGE_MAIN    0x01
-#define INT_WRITE_PAGE_MAIN    0x02
-#define INT_PIPELINE_READ_AHEAD    0x04
-#define INT_PIPELINE_WRITE_AHEAD    0x08
-#define INT_MULTI_PLANE_READ    0x10
-#define INT_MULTI_PLANE_WRITE    0x11
-
-static u32 enable_ecc;
-
-struct mrst_nand_info info;
-
-int totalUsedBanks;
-u32 GLOB_valid_banks[LLD_MAX_FLASH_BANKS];
-
-void __iomem *FlashReg;
-void __iomem *FlashMem;
-
-u16 conf_parameters[] = {
-	0x0000,
-	0x0000,
-	0x01F4,
-	0x01F4,
-	0x01F4,
-	0x01F4,
-	0x0000,
-	0x0000,
-	0x0001,
-	0x0000,
-	0x0000,
-	0x0000,
-	0x0000,
-	0x0040,
-	0x0001,
-	0x000A,
-	0x000A,
-	0x000A,
-	0x0000,
-	0x0000,
-	0x0005,
-	0x0012,
-	0x000C
-};
-
-u16   NAND_Get_Bad_Block(u32 block)
-{
-	u32 status = PASS;
-	u32 flag_bytes  = 0;
-	u32 skip_bytes  = DeviceInfo.wSpareSkipBytes;
-	u32 page, i;
-	u8 *pReadSpareBuf = buf_get_bad_block;
-
-	if (enable_ecc)
-		flag_bytes = DeviceInfo.wNumPageSpareFlag;
-
-	for (page = 0; page < 2; page++) {
-		status = NAND_Read_Page_Spare(pReadSpareBuf, block, page, 1);
-		if (status != PASS)
-			return READ_ERROR;
-		for (i = flag_bytes; i < (flag_bytes + skip_bytes); i++)
-			if (pReadSpareBuf[i] != 0xff)
-				return DEFECTIVE_BLOCK;
-	}
-
-	for (page = 1; page < 3; page++) {
-		status = NAND_Read_Page_Spare(pReadSpareBuf, block,
-			DeviceInfo.wPagesPerBlock - page , 1);
-		if (status != PASS)
-			return READ_ERROR;
-		for (i = flag_bytes; i < (flag_bytes + skip_bytes); i++)
-			if (pReadSpareBuf[i] != 0xff)
-				return DEFECTIVE_BLOCK;
-	}
-
-	return GOOD_BLOCK;
-}
-
-
-u16 NAND_Flash_Reset(void)
-{
-	u32 i;
-	u32 intr_status_rst_comp[4] = {INTR_STATUS0__RST_COMP,
-		INTR_STATUS1__RST_COMP,
-		INTR_STATUS2__RST_COMP,
-		INTR_STATUS3__RST_COMP};
-	u32 intr_status_time_out[4] = {INTR_STATUS0__TIME_OUT,
-		INTR_STATUS1__TIME_OUT,
-		INTR_STATUS2__TIME_OUT,
-		INTR_STATUS3__TIME_OUT};
-	u32 intr_status[4] = {INTR_STATUS0, INTR_STATUS1,
-		INTR_STATUS2, INTR_STATUS3};
-	u32 device_reset_banks[4] = {DEVICE_RESET__BANK0,
-		DEVICE_RESET__BANK1,
-		DEVICE_RESET__BANK2,
-		DEVICE_RESET__BANK3};
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	for (i = 0 ; i < LLD_MAX_FLASH_BANKS; i++)
-		iowrite32(intr_status_rst_comp[i] | intr_status_time_out[i],
-		FlashReg + intr_status[i]);
-
-	for (i = 0 ; i < LLD_MAX_FLASH_BANKS; i++) {
-		iowrite32(device_reset_banks[i], FlashReg + DEVICE_RESET);
-		while (!(ioread32(FlashReg + intr_status[i]) &
-			(intr_status_rst_comp[i] | intr_status_time_out[i])))
-			;
-		if (ioread32(FlashReg + intr_status[i]) &
-			intr_status_time_out[i])
-			nand_dbg_print(NAND_DBG_WARN,
-			"NAND Reset operation timed out on bank %d\n", i);
-	}
-
-	for (i = 0; i < LLD_MAX_FLASH_BANKS; i++)
-		iowrite32(intr_status_rst_comp[i] | intr_status_time_out[i],
-			FlashReg + intr_status[i]);
-
-	return PASS;
-}
-
-static void NAND_ONFi_Timing_Mode(u16 mode)
-{
-	u16 Trea[6] = {40, 30, 25, 20, 20, 16};
-	u16 Trp[6] = {50, 25, 17, 15, 12, 10};
-	u16 Treh[6] = {30, 15, 15, 10, 10, 7};
-	u16 Trc[6] = {100, 50, 35, 30, 25, 20};
-	u16 Trhoh[6] = {0, 15, 15, 15, 15, 15};
-	u16 Trloh[6] = {0, 0, 0, 0, 5, 5};
-	u16 Tcea[6] = {100, 45, 30, 25, 25, 25};
-	u16 Tadl[6] = {200, 100, 100, 100, 70, 70};
-	u16 Trhw[6] = {200, 100, 100, 100, 100, 100};
-	u16 Trhz[6] = {200, 100, 100, 100, 100, 100};
-	u16 Twhr[6] = {120, 80, 80, 60, 60, 60};
-	u16 Tcs[6] = {70, 35, 25, 25, 20, 15};
-
-	u16 TclsRising = 1;
-	u16 data_invalid_rhoh, data_invalid_rloh, data_invalid;
-	u16 dv_window = 0;
-	u16 en_lo, en_hi;
-	u16 acc_clks;
-	u16 addr_2_data, re_2_we, re_2_re, we_2_re, cs_cnt;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	en_lo = CEIL_DIV(Trp[mode], CLK_X);
-	en_hi = CEIL_DIV(Treh[mode], CLK_X);
-
-#if ONFI_BLOOM_TIME
-	if ((en_hi * CLK_X) < (Treh[mode] + 2))
-		en_hi++;
-#endif
-
-	if ((en_lo + en_hi) * CLK_X < Trc[mode])
-		en_lo += CEIL_DIV((Trc[mode] - (en_lo + en_hi) * CLK_X), CLK_X);
-
-	if ((en_lo + en_hi) < CLK_MULTI)
-		en_lo += CLK_MULTI - en_lo - en_hi;
-
-	while (dv_window < 8) {
-		data_invalid_rhoh = en_lo * CLK_X + Trhoh[mode];
-
-		data_invalid_rloh = (en_lo + en_hi) * CLK_X + Trloh[mode];
-
-		data_invalid =
-		    data_invalid_rhoh <
-		    data_invalid_rloh ? data_invalid_rhoh : data_invalid_rloh;
-
-		dv_window = data_invalid - Trea[mode];
-
-		if (dv_window < 8)
-			en_lo++;
-	}
-
-	acc_clks = CEIL_DIV(Trea[mode], CLK_X);
-
-	while (((acc_clks * CLK_X) - Trea[mode]) < 3)
-		acc_clks++;
-
-	if ((data_invalid - acc_clks * CLK_X) < 2)
-		nand_dbg_print(NAND_DBG_WARN, "%s, Line %d: Warning!\n",
-			__FILE__, __LINE__);
-
-	addr_2_data = CEIL_DIV(Tadl[mode], CLK_X);
-	re_2_we = CEIL_DIV(Trhw[mode], CLK_X);
-	re_2_re = CEIL_DIV(Trhz[mode], CLK_X);
-	we_2_re = CEIL_DIV(Twhr[mode], CLK_X);
-	cs_cnt = CEIL_DIV((Tcs[mode] - Trp[mode]), CLK_X);
-	if (!TclsRising)
-		cs_cnt = CEIL_DIV(Tcs[mode], CLK_X);
-	if (cs_cnt == 0)
-		cs_cnt = 1;
-
-	if (Tcea[mode]) {
-		while (((cs_cnt * CLK_X) + Trea[mode]) < Tcea[mode])
-			cs_cnt++;
-	}
-
-#if MODE5_WORKAROUND
-	if (mode == 5)
-		acc_clks = 5;
-#endif
-
-	/* Sighting 3462430: Temporary hack for MT29F128G08CJABAWP:B */
-	if ((ioread32(FlashReg + MANUFACTURER_ID) == 0) &&
-		(ioread32(FlashReg + DEVICE_ID) == 0x88))
-		acc_clks = 6;
-
-	iowrite32(acc_clks, FlashReg + ACC_CLKS);
-	iowrite32(re_2_we, FlashReg + RE_2_WE);
-	iowrite32(re_2_re, FlashReg + RE_2_RE);
-	iowrite32(we_2_re, FlashReg + WE_2_RE);
-	iowrite32(addr_2_data, FlashReg + ADDR_2_DATA);
-	iowrite32(en_lo, FlashReg + RDWR_EN_LO_CNT);
-	iowrite32(en_hi, FlashReg + RDWR_EN_HI_CNT);
-	iowrite32(cs_cnt, FlashReg + CS_SETUP_CNT);
-}
-
-static void index_addr(u32 address, u32 data)
-{
-	iowrite32(address, FlashMem);
-	iowrite32(data, FlashMem + 0x10);
-}
-
-static void index_addr_read_data(u32 address, u32 *pdata)
-{
-	iowrite32(address, FlashMem);
-	*pdata = ioread32(FlashMem + 0x10);
-}
-
-static void set_ecc_config(void)
-{
-#if SUPPORT_8BITECC
-	if ((ioread32(FlashReg + DEVICE_MAIN_AREA_SIZE) < 4096) ||
-		(ioread32(FlashReg + DEVICE_SPARE_AREA_SIZE) <= 128))
-		iowrite32(8, FlashReg + ECC_CORRECTION);
-#endif
-
-	if ((ioread32(FlashReg + ECC_CORRECTION) & ECC_CORRECTION__VALUE)
-		== 1) {
-		DeviceInfo.wECCBytesPerSector = 4;
-		DeviceInfo.wECCBytesPerSector *= DeviceInfo.wDevicesConnected;
-		DeviceInfo.wNumPageSpareFlag =
-			DeviceInfo.wPageSpareSize -
-			DeviceInfo.wPageDataSize /
-			(ECC_SECTOR_SIZE * DeviceInfo.wDevicesConnected) *
-			DeviceInfo.wECCBytesPerSector
-			- DeviceInfo.wSpareSkipBytes;
-	} else {
-		DeviceInfo.wECCBytesPerSector =
-			(ioread32(FlashReg + ECC_CORRECTION) &
-			ECC_CORRECTION__VALUE) * 13 / 8;
-		if ((DeviceInfo.wECCBytesPerSector) % 2 == 0)
-			DeviceInfo.wECCBytesPerSector += 2;
-		else
-			DeviceInfo.wECCBytesPerSector += 1;
-
-		DeviceInfo.wECCBytesPerSector *= DeviceInfo.wDevicesConnected;
-		DeviceInfo.wNumPageSpareFlag = DeviceInfo.wPageSpareSize -
-			DeviceInfo.wPageDataSize /
-			(ECC_SECTOR_SIZE * DeviceInfo.wDevicesConnected) *
-			DeviceInfo.wECCBytesPerSector
-			- DeviceInfo.wSpareSkipBytes;
-	}
-}
-
-static u16 get_onfi_nand_para(void)
-{
-	int i;
-	u16 blks_lun_l, blks_lun_h, n_of_luns;
-	u32 blockperlun, id;
-
-	iowrite32(DEVICE_RESET__BANK0, FlashReg + DEVICE_RESET);
-
-	while (!((ioread32(FlashReg + INTR_STATUS0) &
-		INTR_STATUS0__RST_COMP) |
-		(ioread32(FlashReg + INTR_STATUS0) &
-		INTR_STATUS0__TIME_OUT)))
-		;
-
-	if (ioread32(FlashReg + INTR_STATUS0) & INTR_STATUS0__RST_COMP) {
-		iowrite32(DEVICE_RESET__BANK1, FlashReg + DEVICE_RESET);
-		while (!((ioread32(FlashReg + INTR_STATUS1) &
-			INTR_STATUS1__RST_COMP) |
-			(ioread32(FlashReg + INTR_STATUS1) &
-			INTR_STATUS1__TIME_OUT)))
-			;
-
-		if (ioread32(FlashReg + INTR_STATUS1) &
-			INTR_STATUS1__RST_COMP) {
-			iowrite32(DEVICE_RESET__BANK2,
-				FlashReg + DEVICE_RESET);
-			while (!((ioread32(FlashReg + INTR_STATUS2) &
-				INTR_STATUS2__RST_COMP) |
-				(ioread32(FlashReg + INTR_STATUS2) &
-				INTR_STATUS2__TIME_OUT)))
-				;
-
-			if (ioread32(FlashReg + INTR_STATUS2) &
-				INTR_STATUS2__RST_COMP) {
-				iowrite32(DEVICE_RESET__BANK3,
-					FlashReg + DEVICE_RESET);
-				while (!((ioread32(FlashReg + INTR_STATUS3) &
-					INTR_STATUS3__RST_COMP) |
-					(ioread32(FlashReg + INTR_STATUS3) &
-					INTR_STATUS3__TIME_OUT)))
-					;
-			} else {
-				printk(KERN_ERR "Getting a time out for bank 2!\n");
-			}
-		} else {
-			printk(KERN_ERR "Getting a time out for bank 1!\n");
-		}
-	}
-
-	iowrite32(INTR_STATUS0__TIME_OUT, FlashReg + INTR_STATUS0);
-	iowrite32(INTR_STATUS1__TIME_OUT, FlashReg + INTR_STATUS1);
-	iowrite32(INTR_STATUS2__TIME_OUT, FlashReg + INTR_STATUS2);
-	iowrite32(INTR_STATUS3__TIME_OUT, FlashReg + INTR_STATUS3);
-
-	DeviceInfo.wONFIDevFeatures =
-		ioread32(FlashReg + ONFI_DEVICE_FEATURES);
-	DeviceInfo.wONFIOptCommands =
-		ioread32(FlashReg + ONFI_OPTIONAL_COMMANDS);
-	DeviceInfo.wONFITimingMode =
-		ioread32(FlashReg + ONFI_TIMING_MODE);
-	DeviceInfo.wONFIPgmCacheTimingMode =
-		ioread32(FlashReg + ONFI_PGM_CACHE_TIMING_MODE);
-
-	n_of_luns = ioread32(FlashReg + ONFI_DEVICE_NO_OF_LUNS) &
-		ONFI_DEVICE_NO_OF_LUNS__NO_OF_LUNS;
-	blks_lun_l = ioread32(FlashReg + ONFI_DEVICE_NO_OF_BLOCKS_PER_LUN_L);
-	blks_lun_h = ioread32(FlashReg + ONFI_DEVICE_NO_OF_BLOCKS_PER_LUN_U);
-
-	blockperlun = (blks_lun_h << 16) | blks_lun_l;
-
-	DeviceInfo.wTotalBlocks = n_of_luns * blockperlun;
-
-	if (!(ioread32(FlashReg + ONFI_TIMING_MODE) &
-		ONFI_TIMING_MODE__VALUE))
-		return FAIL;
-
-	for (i = 5; i > 0; i--) {
-		if (ioread32(FlashReg + ONFI_TIMING_MODE) & (0x01 << i))
-			break;
-	}
-
-	NAND_ONFi_Timing_Mode(i);
-
-	index_addr(MODE_11 | 0, 0x90);
-	index_addr(MODE_11 | 1, 0);
-
-	for (i = 0; i < 3; i++)
-		index_addr_read_data(MODE_11 | 2, &id);
-
-	nand_dbg_print(NAND_DBG_DEBUG, "3rd ID: 0x%x\n", id);
-
-	DeviceInfo.MLCDevice = id & 0x0C;
-
-	/* By now, all the ONFI devices we know support the page cache */
-	/* rw feature. So here we enable the pipeline_rw_ahead feature */
-	/* iowrite32(1, FlashReg + CACHE_WRITE_ENABLE); */
-	/* iowrite32(1, FlashReg + CACHE_READ_ENABLE);  */
-
-	return PASS;
-}
-
-static void get_samsung_nand_para(void)
-{
-	u8 no_of_planes;
-	u32 blk_size;
-	u64 plane_size, capacity;
-	u32 id_bytes[5];
-	int i;
-
-	index_addr((u32)(MODE_11 | 0), 0x90);
-	index_addr((u32)(MODE_11 | 1), 0);
-	for (i = 0; i < 5; i++)
-		index_addr_read_data((u32)(MODE_11 | 2), &id_bytes[i]);
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		"ID bytes: 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\n",
-		id_bytes[0], id_bytes[1], id_bytes[2],
-		id_bytes[3], id_bytes[4]);
-
-	if ((id_bytes[1] & 0xff) == 0xd3) { /* Samsung K9WAG08U1A */
-		/* Set timing register values according to datasheet */
-		iowrite32(5, FlashReg + ACC_CLKS);
-		iowrite32(20, FlashReg + RE_2_WE);
-		iowrite32(12, FlashReg + WE_2_RE);
-		iowrite32(14, FlashReg + ADDR_2_DATA);
-		iowrite32(3, FlashReg + RDWR_EN_LO_CNT);
-		iowrite32(2, FlashReg + RDWR_EN_HI_CNT);
-		iowrite32(2, FlashReg + CS_SETUP_CNT);
-	}
-
-	no_of_planes = 1 << ((id_bytes[4] & 0x0c) >> 2);
-	plane_size  = (u64)64 << ((id_bytes[4] & 0x70) >> 4);
-	blk_size = 64 << ((ioread32(FlashReg + DEVICE_PARAM_1) & 0x30) >> 4);
-	capacity = (u64)128 * plane_size * no_of_planes;
-
-	DeviceInfo.wTotalBlocks = (u32)GLOB_u64_Div(capacity, blk_size);
-}
-
-static void get_toshiba_nand_para(void)
-{
-	void __iomem *scratch_reg;
-	u32 tmp;
-
-	/* Workaround to fix a controller bug which reports a wrong */
-	/* spare area size for some kind of Toshiba NAND device */
-	if ((ioread32(FlashReg + DEVICE_MAIN_AREA_SIZE) == 4096) &&
-		(ioread32(FlashReg + DEVICE_SPARE_AREA_SIZE) == 64)) {
-		iowrite32(216, FlashReg + DEVICE_SPARE_AREA_SIZE);
-		tmp = ioread32(FlashReg + DEVICES_CONNECTED) *
-			ioread32(FlashReg + DEVICE_SPARE_AREA_SIZE);
-		iowrite32(tmp, FlashReg + LOGICAL_PAGE_SPARE_SIZE);
-#if SUPPORT_15BITECC
-		iowrite32(15, FlashReg + ECC_CORRECTION);
-#elif SUPPORT_8BITECC
-		iowrite32(8, FlashReg + ECC_CORRECTION);
-#endif
-	}
-
-	/* As Toshiba NAND can not provide it's block number, */
-	/* so here we need user to provide the correct block */
-	/* number in a scratch register before the Linux NAND */
-	/* driver is loaded. If no valid value found in the scratch */
-	/* register, then we use default block number value */
-	scratch_reg = ioremap_nocache(SCRATCH_REG_ADDR, SCRATCH_REG_SIZE);
-	if (!scratch_reg) {
-		printk(KERN_ERR "Spectra: ioremap failed in %s, Line %d",
-			__FILE__, __LINE__);
-		DeviceInfo.wTotalBlocks = GLOB_HWCTL_DEFAULT_BLKS;
-	} else {
-		nand_dbg_print(NAND_DBG_WARN,
-			"Spectra: ioremap reg address: 0x%p\n", scratch_reg);
-		DeviceInfo.wTotalBlocks = 1 << ioread8(scratch_reg);
-		if (DeviceInfo.wTotalBlocks < 512)
-			DeviceInfo.wTotalBlocks = GLOB_HWCTL_DEFAULT_BLKS;
-		iounmap(scratch_reg);
-	}
-}
-
-static void get_hynix_nand_para(void)
-{
-	void __iomem *scratch_reg;
-	u32 main_size, spare_size;
-
-	switch (DeviceInfo.wDeviceID) {
-	case 0xD5: /* Hynix H27UAG8T2A, H27UBG8U5A or H27UCG8VFA */
-	case 0xD7: /* Hynix H27UDG8VEM, H27UCG8UDM or H27UCG8V5A */
-		iowrite32(128, FlashReg + PAGES_PER_BLOCK);
-		iowrite32(4096, FlashReg + DEVICE_MAIN_AREA_SIZE);
-		iowrite32(224, FlashReg + DEVICE_SPARE_AREA_SIZE);
-		main_size = 4096 * ioread32(FlashReg + DEVICES_CONNECTED);
-		spare_size = 224 * ioread32(FlashReg + DEVICES_CONNECTED);
-		iowrite32(main_size, FlashReg + LOGICAL_PAGE_DATA_SIZE);
-		iowrite32(spare_size, FlashReg + LOGICAL_PAGE_SPARE_SIZE);
-		iowrite32(0, FlashReg + DEVICE_WIDTH);
-#if SUPPORT_15BITECC
-		iowrite32(15, FlashReg + ECC_CORRECTION);
-#elif SUPPORT_8BITECC
-		iowrite32(8, FlashReg + ECC_CORRECTION);
-#endif
-		DeviceInfo.MLCDevice  = 1;
-		break;
-	default:
-		nand_dbg_print(NAND_DBG_WARN,
-			"Spectra: Unknown Hynix NAND (Device ID: 0x%x)."
-			"Will use default parameter values instead.\n",
-			DeviceInfo.wDeviceID);
-	}
-
-	scratch_reg = ioremap_nocache(SCRATCH_REG_ADDR, SCRATCH_REG_SIZE);
-	if (!scratch_reg) {
-		printk(KERN_ERR "Spectra: ioremap failed in %s, Line %d",
-			__FILE__, __LINE__);
-		DeviceInfo.wTotalBlocks = GLOB_HWCTL_DEFAULT_BLKS;
-	} else {
-		nand_dbg_print(NAND_DBG_WARN,
-			"Spectra: ioremap reg address: 0x%p\n", scratch_reg);
-		DeviceInfo.wTotalBlocks = 1 << ioread8(scratch_reg);
-		if (DeviceInfo.wTotalBlocks < 512)
-			DeviceInfo.wTotalBlocks = GLOB_HWCTL_DEFAULT_BLKS;
-		iounmap(scratch_reg);
-	}
-}
-
-static void find_valid_banks(void)
-{
-	u32 id[LLD_MAX_FLASH_BANKS];
-	int i;
-
-	totalUsedBanks = 0;
-	for (i = 0; i < LLD_MAX_FLASH_BANKS; i++) {
-		index_addr((u32)(MODE_11 | (i << 24) | 0), 0x90);
-		index_addr((u32)(MODE_11 | (i << 24) | 1), 0);
-		index_addr_read_data((u32)(MODE_11 | (i << 24) | 2), &id[i]);
-
-		nand_dbg_print(NAND_DBG_DEBUG,
-			"Return 1st ID for bank[%d]: %x\n", i, id[i]);
-
-		if (i == 0) {
-			if (id[i] & 0x0ff)
-				GLOB_valid_banks[i] = 1;
-		} else {
-			if ((id[i] & 0x0ff) == (id[0] & 0x0ff))
-				GLOB_valid_banks[i] = 1;
-		}
-
-		totalUsedBanks += GLOB_valid_banks[i];
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		"totalUsedBanks: %d\n", totalUsedBanks);
-}
-
-static void detect_partition_feature(void)
-{
-	if (ioread32(FlashReg + FEATURES) & FEATURES__PARTITION) {
-		if ((ioread32(FlashReg + PERM_SRC_ID_1) &
-			PERM_SRC_ID_1__SRCID) == SPECTRA_PARTITION_ID) {
-			DeviceInfo.wSpectraStartBlock =
-			    ((ioread32(FlashReg + MIN_MAX_BANK_1) &
-			      MIN_MAX_BANK_1__MIN_VALUE) *
-			     DeviceInfo.wTotalBlocks)
-			    +
-			    (ioread32(FlashReg + MIN_BLK_ADDR_1) &
-			    MIN_BLK_ADDR_1__VALUE);
-
-			DeviceInfo.wSpectraEndBlock =
-			    (((ioread32(FlashReg + MIN_MAX_BANK_1) &
-			       MIN_MAX_BANK_1__MAX_VALUE) >> 2) *
-			     DeviceInfo.wTotalBlocks)
-			    +
-			    (ioread32(FlashReg + MAX_BLK_ADDR_1) &
-			    MAX_BLK_ADDR_1__VALUE);
-
-			DeviceInfo.wTotalBlocks *= totalUsedBanks;
-
-			if (DeviceInfo.wSpectraEndBlock >=
-			    DeviceInfo.wTotalBlocks) {
-				DeviceInfo.wSpectraEndBlock =
-				    DeviceInfo.wTotalBlocks - 1;
-			}
-
-			DeviceInfo.wDataBlockNum =
-				DeviceInfo.wSpectraEndBlock -
-				DeviceInfo.wSpectraStartBlock + 1;
-		} else {
-			DeviceInfo.wTotalBlocks *= totalUsedBanks;
-			DeviceInfo.wSpectraStartBlock = SPECTRA_START_BLOCK;
-			DeviceInfo.wSpectraEndBlock =
-				DeviceInfo.wTotalBlocks - 1;
-			DeviceInfo.wDataBlockNum =
-				DeviceInfo.wSpectraEndBlock -
-				DeviceInfo.wSpectraStartBlock + 1;
-		}
-	} else {
-		DeviceInfo.wTotalBlocks *= totalUsedBanks;
-		DeviceInfo.wSpectraStartBlock = SPECTRA_START_BLOCK;
-		DeviceInfo.wSpectraEndBlock = DeviceInfo.wTotalBlocks - 1;
-		DeviceInfo.wDataBlockNum =
-			DeviceInfo.wSpectraEndBlock -
-			DeviceInfo.wSpectraStartBlock + 1;
-	}
-}
-
-static void dump_device_info(void)
-{
-	nand_dbg_print(NAND_DBG_DEBUG, "DeviceInfo:\n");
-	nand_dbg_print(NAND_DBG_DEBUG, "DeviceMaker: 0x%x\n",
-		DeviceInfo.wDeviceMaker);
-	nand_dbg_print(NAND_DBG_DEBUG, "DeviceID: 0x%x\n",
-		DeviceInfo.wDeviceID);
-	nand_dbg_print(NAND_DBG_DEBUG, "DeviceType: 0x%x\n",
-		DeviceInfo.wDeviceType);
-	nand_dbg_print(NAND_DBG_DEBUG, "SpectraStartBlock: %d\n",
-		DeviceInfo.wSpectraStartBlock);
-	nand_dbg_print(NAND_DBG_DEBUG, "SpectraEndBlock: %d\n",
-		DeviceInfo.wSpectraEndBlock);
-	nand_dbg_print(NAND_DBG_DEBUG, "TotalBlocks: %d\n",
-		DeviceInfo.wTotalBlocks);
-	nand_dbg_print(NAND_DBG_DEBUG, "PagesPerBlock: %d\n",
-		DeviceInfo.wPagesPerBlock);
-	nand_dbg_print(NAND_DBG_DEBUG, "PageSize: %d\n",
-		DeviceInfo.wPageSize);
-	nand_dbg_print(NAND_DBG_DEBUG, "PageDataSize: %d\n",
-		DeviceInfo.wPageDataSize);
-	nand_dbg_print(NAND_DBG_DEBUG, "PageSpareSize: %d\n",
-		DeviceInfo.wPageSpareSize);
-	nand_dbg_print(NAND_DBG_DEBUG, "NumPageSpareFlag: %d\n",
-		DeviceInfo.wNumPageSpareFlag);
-	nand_dbg_print(NAND_DBG_DEBUG, "ECCBytesPerSector: %d\n",
-		DeviceInfo.wECCBytesPerSector);
-	nand_dbg_print(NAND_DBG_DEBUG, "BlockSize: %d\n",
-		DeviceInfo.wBlockSize);
-	nand_dbg_print(NAND_DBG_DEBUG, "BlockDataSize: %d\n",
-		DeviceInfo.wBlockDataSize);
-	nand_dbg_print(NAND_DBG_DEBUG, "DataBlockNum: %d\n",
-		DeviceInfo.wDataBlockNum);
-	nand_dbg_print(NAND_DBG_DEBUG, "PlaneNum: %d\n",
-		DeviceInfo.bPlaneNum);
-	nand_dbg_print(NAND_DBG_DEBUG, "DeviceMainAreaSize: %d\n",
-		DeviceInfo.wDeviceMainAreaSize);
-	nand_dbg_print(NAND_DBG_DEBUG, "DeviceSpareAreaSize: %d\n",
-		DeviceInfo.wDeviceSpareAreaSize);
-	nand_dbg_print(NAND_DBG_DEBUG, "DevicesConnected: %d\n",
-		DeviceInfo.wDevicesConnected);
-	nand_dbg_print(NAND_DBG_DEBUG, "DeviceWidth: %d\n",
-		DeviceInfo.wDeviceWidth);
-	nand_dbg_print(NAND_DBG_DEBUG, "HWRevision: 0x%x\n",
-		DeviceInfo.wHWRevision);
-	nand_dbg_print(NAND_DBG_DEBUG, "HWFeatures: 0x%x\n",
-		DeviceInfo.wHWFeatures);
-	nand_dbg_print(NAND_DBG_DEBUG, "ONFIDevFeatures: 0x%x\n",
-		DeviceInfo.wONFIDevFeatures);
-	nand_dbg_print(NAND_DBG_DEBUG, "ONFIOptCommands: 0x%x\n",
-		DeviceInfo.wONFIOptCommands);
-	nand_dbg_print(NAND_DBG_DEBUG, "ONFITimingMode: 0x%x\n",
-		DeviceInfo.wONFITimingMode);
-	nand_dbg_print(NAND_DBG_DEBUG, "ONFIPgmCacheTimingMode: 0x%x\n",
-		DeviceInfo.wONFIPgmCacheTimingMode);
-	nand_dbg_print(NAND_DBG_DEBUG, "MLCDevice: %s\n",
-		DeviceInfo.MLCDevice ? "Yes" : "No");
-	nand_dbg_print(NAND_DBG_DEBUG, "SpareSkipBytes: %d\n",
-		DeviceInfo.wSpareSkipBytes);
-	nand_dbg_print(NAND_DBG_DEBUG, "BitsInPageNumber: %d\n",
-		DeviceInfo.nBitsInPageNumber);
-	nand_dbg_print(NAND_DBG_DEBUG, "BitsInPageDataSize: %d\n",
-		DeviceInfo.nBitsInPageDataSize);
-	nand_dbg_print(NAND_DBG_DEBUG, "BitsInBlockDataSize: %d\n",
-		DeviceInfo.nBitsInBlockDataSize);
-}
-
-u16 NAND_Read_Device_ID(void)
-{
-	u16 status = PASS;
-	u8 no_of_planes;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	iowrite32(0x02, FlashReg + SPARE_AREA_SKIP_BYTES);
-	iowrite32(0xffff, FlashReg + SPARE_AREA_MARKER);
-	DeviceInfo.wDeviceMaker = ioread32(FlashReg + MANUFACTURER_ID);
-	DeviceInfo.wDeviceID = ioread32(FlashReg + DEVICE_ID);
-	DeviceInfo.MLCDevice = ioread32(FlashReg + DEVICE_PARAM_0) & 0x0c;
-
-	if (ioread32(FlashReg + ONFI_DEVICE_NO_OF_LUNS) &
-		ONFI_DEVICE_NO_OF_LUNS__ONFI_DEVICE) { /* ONFI 1.0 NAND */
-		if (FAIL == get_onfi_nand_para())
-			return FAIL;
-	} else if (DeviceInfo.wDeviceMaker == 0xEC) { /* Samsung NAND */
-		get_samsung_nand_para();
-	} else if (DeviceInfo.wDeviceMaker == 0x98) { /* Toshiba NAND */
-		get_toshiba_nand_para();
-	} else if (DeviceInfo.wDeviceMaker == 0xAD) { /* Hynix NAND */
-		get_hynix_nand_para();
-	} else {
-		DeviceInfo.wTotalBlocks = GLOB_HWCTL_DEFAULT_BLKS;
-	}
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Dump timing register values:"
-			"acc_clks: %d, re_2_we: %d, we_2_re: %d,"
-			"addr_2_data: %d, rdwr_en_lo_cnt: %d, "
-			"rdwr_en_hi_cnt: %d, cs_setup_cnt: %d\n",
-			ioread32(FlashReg + ACC_CLKS),
-			ioread32(FlashReg + RE_2_WE),
-			ioread32(FlashReg + WE_2_RE),
-			ioread32(FlashReg + ADDR_2_DATA),
-			ioread32(FlashReg + RDWR_EN_LO_CNT),
-			ioread32(FlashReg + RDWR_EN_HI_CNT),
-			ioread32(FlashReg + CS_SETUP_CNT));
-
-	DeviceInfo.wHWRevision = ioread32(FlashReg + REVISION);
-	DeviceInfo.wHWFeatures = ioread32(FlashReg + FEATURES);
-
-	DeviceInfo.wDeviceMainAreaSize =
-		ioread32(FlashReg + DEVICE_MAIN_AREA_SIZE);
-	DeviceInfo.wDeviceSpareAreaSize =
-		ioread32(FlashReg + DEVICE_SPARE_AREA_SIZE);
-
-	DeviceInfo.wPageDataSize =
-		ioread32(FlashReg + LOGICAL_PAGE_DATA_SIZE);
-
-	/* Note: When using the Micon 4K NAND device, the controller will report
-	 * Page Spare Size as 216 bytes. But Micron's Spec say it's 218 bytes.
-	 * And if force set it to 218 bytes, the controller can not work
-	 * correctly. So just let it be. But keep in mind that this bug may
-	 * cause
-	 * other problems in future.       - Yunpeng  2008-10-10
-	 */
-	DeviceInfo.wPageSpareSize =
-		ioread32(FlashReg + LOGICAL_PAGE_SPARE_SIZE);
-
-	DeviceInfo.wPagesPerBlock = ioread32(FlashReg + PAGES_PER_BLOCK);
-
-	DeviceInfo.wPageSize =
-	    DeviceInfo.wPageDataSize + DeviceInfo.wPageSpareSize;
-	DeviceInfo.wBlockSize =
-	    DeviceInfo.wPageSize * DeviceInfo.wPagesPerBlock;
-	DeviceInfo.wBlockDataSize =
-	    DeviceInfo.wPagesPerBlock * DeviceInfo.wPageDataSize;
-
-	DeviceInfo.wDeviceWidth = ioread32(FlashReg + DEVICE_WIDTH);
-	DeviceInfo.wDeviceType =
-		((ioread32(FlashReg + DEVICE_WIDTH) > 0) ? 16 : 8);
-
-	DeviceInfo.wDevicesConnected = ioread32(FlashReg + DEVICES_CONNECTED);
-
-	DeviceInfo.wSpareSkipBytes =
-		ioread32(FlashReg + SPARE_AREA_SKIP_BYTES) *
-		DeviceInfo.wDevicesConnected;
-
-	DeviceInfo.nBitsInPageNumber =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wPagesPerBlock);
-	DeviceInfo.nBitsInPageDataSize =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wPageDataSize);
-	DeviceInfo.nBitsInBlockDataSize =
-		(u8)GLOB_Calc_Used_Bits(DeviceInfo.wBlockDataSize);
-
-	set_ecc_config();
-
-	no_of_planes = ioread32(FlashReg + NUMBER_OF_PLANES) &
-		NUMBER_OF_PLANES__VALUE;
-
-	switch (no_of_planes) {
-	case 0:
-	case 1:
-	case 3:
-	case 7:
-		DeviceInfo.bPlaneNum = no_of_planes + 1;
-		break;
-	default:
-		status = FAIL;
-		break;
-	}
-
-	find_valid_banks();
-
-	detect_partition_feature();
-
-	dump_device_info();
-
-	return status;
-}
-
-u16 NAND_UnlockArrayAll(void)
-{
-	u64 start_addr, end_addr;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	start_addr = 0;
-	end_addr = ((u64)DeviceInfo.wBlockSize *
-		(DeviceInfo.wTotalBlocks - 1)) >>
-		DeviceInfo.nBitsInPageDataSize;
-
-	index_addr((u32)(MODE_10 | (u32)start_addr), 0x10);
-	index_addr((u32)(MODE_10 | (u32)end_addr), 0x11);
-
-	return PASS;
-}
-
-void NAND_LLD_Enable_Disable_Interrupts(u16 INT_ENABLE)
-{
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (INT_ENABLE)
-		iowrite32(1, FlashReg + GLOBAL_INT_ENABLE);
-	else
-		iowrite32(0, FlashReg + GLOBAL_INT_ENABLE);
-}
-
-u16 NAND_Erase_Block(u32 block)
-{
-	u16 status = PASS;
-	u64 flash_add;
-	u16 flash_bank;
-	u32 intr_status = 0;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		* DeviceInfo.wBlockDataSize;
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	if (block >= DeviceInfo.wTotalBlocks)
-		status = FAIL;
-
-	if (status == PASS) {
-		intr_status = intr_status_addresses[flash_bank];
-
-		iowrite32(INTR_STATUS0__ERASE_COMP | INTR_STATUS0__ERASE_FAIL,
-			FlashReg + intr_status);
-
-		index_addr((u32)(MODE_10 | (flash_bank << 24) |
-			(flash_add >> DeviceInfo.nBitsInPageDataSize)), 1);
-
-		while (!(ioread32(FlashReg + intr_status) &
-			(INTR_STATUS0__ERASE_COMP | INTR_STATUS0__ERASE_FAIL)))
-			;
-
-		if (ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__ERASE_FAIL)
-			status = FAIL;
-
-		iowrite32(INTR_STATUS0__ERASE_COMP | INTR_STATUS0__ERASE_FAIL,
-			FlashReg + intr_status);
-	}
-
-	return status;
-}
-
-static u32 Boundary_Check_Block_Page(u32 block, u16 page,
-						u16 page_count)
-{
-	u32 status = PASS;
-
-	if (block >= DeviceInfo.wTotalBlocks)
-		status = FAIL;
-
-	if (page + page_count > DeviceInfo.wPagesPerBlock)
-		status = FAIL;
-
-	return status;
-}
-
-u16 NAND_Read_Page_Spare(u8 *read_data, u32 block, u16 page,
-			    u16 page_count)
-{
-	u32 status = PASS;
-	u32 i;
-	u64 flash_add;
-	u32 PageSpareSize = DeviceInfo.wPageSpareSize;
-	u32 spareFlagBytes = DeviceInfo.wNumPageSpareFlag;
-	u32 flash_bank;
-	u32 intr_status = 0;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	u8 *page_spare = buf_read_page_spare;
-
-	if (block >= DeviceInfo.wTotalBlocks) {
-		printk(KERN_ERR "block too big: %d\n", (int)block);
-		status = FAIL;
-	}
-
-	if (page >= DeviceInfo.wPagesPerBlock) {
-		printk(KERN_ERR "page too big: %d\n", page);
-		status = FAIL;
-	}
-
-	if (page_count > 1) {
-		printk(KERN_ERR "page count too big: %d\n", page_count);
-		status = FAIL;
-	}
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		* DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	if (status == PASS) {
-		intr_status = intr_status_addresses[flash_bank];
-		iowrite32(ioread32(FlashReg + intr_status),
-			FlashReg + intr_status);
-
-		index_addr((u32)(MODE_10 | (flash_bank << 24) |
-			(flash_add >> DeviceInfo.nBitsInPageDataSize)),
-			0x41);
-		index_addr((u32)(MODE_10 | (flash_bank << 24) |
-			(flash_add >> DeviceInfo.nBitsInPageDataSize)),
-			0x2000 | page_count);
-		while (!(ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__LOAD_COMP))
-			;
-
-		iowrite32((u32)(MODE_01 | (flash_bank << 24) |
-			(flash_add >> DeviceInfo.nBitsInPageDataSize)),
-			FlashMem);
-
-		for (i = 0; i < (PageSpareSize / 4); i++)
-			*((u32 *)page_spare + i) =
-					ioread32(FlashMem + 0x10);
-
-		if (enable_ecc) {
-			for (i = 0; i < spareFlagBytes; i++)
-				read_data[i] =
-					page_spare[PageSpareSize -
-						spareFlagBytes + i];
-			for (i = 0; i < (PageSpareSize - spareFlagBytes); i++)
-				read_data[spareFlagBytes + i] =
-							page_spare[i];
-		} else {
-			for (i = 0; i < PageSpareSize; i++)
-				read_data[i] = page_spare[i];
-		}
-
-		index_addr((u32)(MODE_10 | (flash_bank << 24) |
-			(flash_add >> DeviceInfo.nBitsInPageDataSize)), 0x42);
-	}
-
-	return status;
-}
-
-/* No use function. Should be removed later */
-u16 NAND_Write_Page_Spare(u8 *write_data, u32 block, u16 page,
-			     u16 page_count)
-{
-	printk(KERN_ERR
-	       "Error! This function (NAND_Write_Page_Spare) should never"
-		" be called!\n");
-	return ERR;
-}
-
-/* op value:  0 - DDMA read;  1 - DDMA write */
-static void ddma_trans(u8 *data, u64 flash_add,
-			u32 flash_bank, int op, u32 numPages)
-{
-	u32 data_addr;
-
-	/* Map virtual address to bus address for DDMA */
-	data_addr = virt_to_bus(data);
-
-	index_addr((u32)(MODE_10 | (flash_bank << 24) |
-		(flash_add >> DeviceInfo.nBitsInPageDataSize)),
-		(u16)(2 << 12) | (op << 8) | numPages);
-
-	index_addr((u32)(MODE_10 | (flash_bank << 24) |
-		((u16)(0x0FFFF & (data_addr >> 16)) << 8)),
-		(u16)(2 << 12) | (2 << 8) | 0);
-
-	index_addr((u32)(MODE_10 | (flash_bank << 24) |
-		((u16)(0x0FFFF & data_addr) << 8)),
-		(u16)(2 << 12) | (3 << 8) | 0);
-
-	index_addr((u32)(MODE_10 | (flash_bank << 24) |
-		(1 << 16) | (0x40 << 8)),
-		(u16)(2 << 12) | (4 << 8) | 0);
-}
-
-/* If data in buf are all 0xff, then return 1; otherwise return 0 */
-static int check_all_1(u8 *buf)
-{
-	int i, j, cnt;
-
-	for (i = 0; i < DeviceInfo.wPageDataSize; i++) {
-		if (buf[i] != 0xff) {
-			cnt = 0;
-			nand_dbg_print(NAND_DBG_WARN,
-				"the first non-0xff data byte is: %d\n", i);
-			for (j = i; j < DeviceInfo.wPageDataSize; j++) {
-				nand_dbg_print(NAND_DBG_WARN, "0x%x ", buf[j]);
-				cnt++;
-				if (cnt > 8)
-					break;
-			}
-			nand_dbg_print(NAND_DBG_WARN, "\n");
-			return 0;
-		}
-	}
-
-	return 1;
-}
-
-static int do_ecc_new(unsigned long bank, u8 *buf,
-				u32 block, u16 page)
-{
-	int status = PASS;
-	u16 err_page = 0;
-	u16 err_byte;
-	u8 err_sect;
-	u8 err_dev;
-	u16 err_fix_info;
-	u16 err_addr;
-	u32 ecc_sect_size;
-	u8 *err_pos;
-	u32 err_page_addr[4] = {ERR_PAGE_ADDR0,
-		ERR_PAGE_ADDR1, ERR_PAGE_ADDR2, ERR_PAGE_ADDR3};
-
-	ecc_sect_size = ECC_SECTOR_SIZE * (DeviceInfo.wDevicesConnected);
-
-	do {
-		err_page = ioread32(FlashReg + err_page_addr[bank]);
-		err_addr = ioread32(FlashReg + ECC_ERROR_ADDRESS);
-		err_byte = err_addr & ECC_ERROR_ADDRESS__OFFSET;
-		err_sect = ((err_addr & ECC_ERROR_ADDRESS__SECTOR_NR) >> 12);
-		err_fix_info = ioread32(FlashReg + ERR_CORRECTION_INFO);
-		err_dev = ((err_fix_info & ERR_CORRECTION_INFO__DEVICE_NR)
-			>> 8);
-		if (err_fix_info & ERR_CORRECTION_INFO__ERROR_TYPE) {
-			nand_dbg_print(NAND_DBG_WARN,
-				"%s, Line %d Uncorrectable ECC error "
-				"when read block %d page %d."
-				"PTN_INTR register: 0x%x "
-				"err_page: %d, err_sect: %d, err_byte: %d, "
-				"err_dev: %d, ecc_sect_size: %d, "
-				"err_fix_info: 0x%x\n",
-				__FILE__, __LINE__, block, page,
-				ioread32(FlashReg + PTN_INTR),
-				err_page, err_sect, err_byte, err_dev,
-				ecc_sect_size, (u32)err_fix_info);
-
-			if (check_all_1(buf))
-				nand_dbg_print(NAND_DBG_WARN, "%s, Line %d"
-					       "All 0xff!\n",
-					       __FILE__, __LINE__);
-			else
-				nand_dbg_print(NAND_DBG_WARN, "%s, Line %d"
-					       "Not all 0xff!\n",
-					       __FILE__, __LINE__);
-			status = FAIL;
-		} else {
-			nand_dbg_print(NAND_DBG_WARN,
-				"%s, Line %d Found ECC error "
-				"when read block %d page %d."
-				"err_page: %d, err_sect: %d, err_byte: %d, "
-				"err_dev: %d, ecc_sect_size: %d, "
-				"err_fix_info: 0x%x\n",
-				__FILE__, __LINE__, block, page,
-				err_page, err_sect, err_byte, err_dev,
-				ecc_sect_size, (u32)err_fix_info);
-			if (err_byte < ECC_SECTOR_SIZE) {
-				err_pos = buf +
-					(err_page - page) *
-					DeviceInfo.wPageDataSize +
-					err_sect * ecc_sect_size +
-					err_byte *
-					DeviceInfo.wDevicesConnected +
-					err_dev;
-
-				*err_pos ^= err_fix_info &
-					ERR_CORRECTION_INFO__BYTEMASK;
-			}
-		}
-	} while (!(err_fix_info & ERR_CORRECTION_INFO__LAST_ERR_INFO));
-
-	return status;
-}
-
-u16 NAND_Read_Page_Main_Polling(u8 *read_data,
-		u32 block, u16 page, u16 page_count)
-{
-	u32 status = PASS;
-	u64 flash_add;
-	u32 intr_status = 0;
-	u32 flash_bank;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	u8 *read_data_l;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-	if (status != PASS)
-		return status;
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		* DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-	intr_status = intr_status_addresses[flash_bank];
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	if (page_count > 1) {
-		read_data_l = read_data;
-		while (page_count > MAX_PAGES_PER_RW) {
-			if (ioread32(FlashReg + MULTIPLANE_OPERATION))
-				status = NAND_Multiplane_Read(read_data_l,
-					block, page, MAX_PAGES_PER_RW);
-			else
-				status = NAND_Pipeline_Read_Ahead_Polling(
-					read_data_l, block, page,
-					MAX_PAGES_PER_RW);
-
-			if (status == FAIL)
-				return status;
-
-			read_data_l += DeviceInfo.wPageDataSize *
-					MAX_PAGES_PER_RW;
-			page_count -= MAX_PAGES_PER_RW;
-			page += MAX_PAGES_PER_RW;
-		}
-		if (ioread32(FlashReg + MULTIPLANE_OPERATION))
-			status = NAND_Multiplane_Read(read_data_l,
-					block, page, page_count);
-		else
-			status = NAND_Pipeline_Read_Ahead_Polling(
-					read_data_l, block, page, page_count);
-
-		return status;
-	}
-
-	iowrite32(1, FlashReg + DMA_ENABLE);
-	while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	ddma_trans(read_data, flash_add, flash_bank, 0, 1);
-
-	if (enable_ecc) {
-		while (!(ioread32(FlashReg + intr_status) &
-			(INTR_STATUS0__ECC_TRANSACTION_DONE |
-			INTR_STATUS0__ECC_ERR)))
-			;
-
-		if (ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__ECC_ERR) {
-			iowrite32(INTR_STATUS0__ECC_ERR,
-				FlashReg + intr_status);
-			status = do_ecc_new(flash_bank, read_data,
-					block, page);
-		}
-
-		if (ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__ECC_TRANSACTION_DONE &
-			INTR_STATUS0__ECC_ERR)
-			iowrite32(INTR_STATUS0__ECC_TRANSACTION_DONE |
-				INTR_STATUS0__ECC_ERR,
-				FlashReg + intr_status);
-		else if (ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__ECC_TRANSACTION_DONE)
-			iowrite32(INTR_STATUS0__ECC_TRANSACTION_DONE,
-				FlashReg + intr_status);
-		else if (ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__ECC_ERR)
-			iowrite32(INTR_STATUS0__ECC_ERR,
-				FlashReg + intr_status);
-	} else {
-		while (!(ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__DMA_CMD_COMP))
-			;
-		iowrite32(INTR_STATUS0__DMA_CMD_COMP, FlashReg + intr_status);
-	}
-
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(0, FlashReg + DMA_ENABLE);
-	while ((ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	return status;
-}
-
-u16 NAND_Pipeline_Read_Ahead_Polling(u8 *read_data,
-			u32 block, u16 page, u16 page_count)
-{
-	u32 status = PASS;
-	u32 NumPages = page_count;
-	u64 flash_add;
-	u32 flash_bank;
-	u32 intr_status = 0;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	u32 ecc_done_OR_dma_comp;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-
-	if (page_count < 2)
-		status = FAIL;
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		*DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	if (status == PASS) {
-		intr_status = intr_status_addresses[flash_bank];
-		iowrite32(ioread32(FlashReg + intr_status),
-			FlashReg + intr_status);
-
-		iowrite32(1, FlashReg + DMA_ENABLE);
-		while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-			;
-
-		iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-		index_addr((u32)(MODE_10 | (flash_bank << 24) |
-			(flash_add >> DeviceInfo.nBitsInPageDataSize)), 0x42);
-		ddma_trans(read_data, flash_add, flash_bank, 0, NumPages);
-
-		ecc_done_OR_dma_comp = 0;
-		while (1) {
-			if (enable_ecc) {
-				while (!ioread32(FlashReg + intr_status))
-					;
-
-				if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__ECC_ERR) {
-					iowrite32(INTR_STATUS0__ECC_ERR,
-						FlashReg + intr_status);
-					status = do_ecc_new(flash_bank,
-						read_data, block, page);
-				} else if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__DMA_CMD_COMP) {
-					iowrite32(INTR_STATUS0__DMA_CMD_COMP,
-						FlashReg + intr_status);
-
-					if (1 == ecc_done_OR_dma_comp)
-						break;
-
-					ecc_done_OR_dma_comp = 1;
-				} else if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__ECC_TRANSACTION_DONE) {
-					iowrite32(
-					INTR_STATUS0__ECC_TRANSACTION_DONE,
-					FlashReg + intr_status);
-
-					if (1 == ecc_done_OR_dma_comp)
-						break;
-
-					ecc_done_OR_dma_comp = 1;
-				}
-			} else {
-				while (!(ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__DMA_CMD_COMP))
-					;
-
-				iowrite32(INTR_STATUS0__DMA_CMD_COMP,
-					FlashReg + intr_status);
-				break;
-			}
-
-			iowrite32((~INTR_STATUS0__ECC_ERR) &
-				(~INTR_STATUS0__ECC_TRANSACTION_DONE) &
-				(~INTR_STATUS0__DMA_CMD_COMP),
-				FlashReg + intr_status);
-
-		}
-
-		iowrite32(ioread32(FlashReg + intr_status),
-			FlashReg + intr_status);
-
-		iowrite32(0, FlashReg + DMA_ENABLE);
-
-		while ((ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-			;
-	}
-	return status;
-}
-
-u16 NAND_Read_Page_Main(u8 *read_data, u32 block, u16 page,
-			   u16 page_count)
-{
-	u32 status = PASS;
-	u64 flash_add;
-	u32 intr_status = 0;
-	u32 flash_bank;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	int ret;
-	u8 *read_data_l;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-	if (status != PASS)
-		return status;
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		* DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-	intr_status = intr_status_addresses[flash_bank];
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	if (page_count > 1) {
-		read_data_l = read_data;
-		while (page_count > MAX_PAGES_PER_RW) {
-			if (ioread32(FlashReg + MULTIPLANE_OPERATION))
-				status = NAND_Multiplane_Read(read_data_l,
-					block, page, MAX_PAGES_PER_RW);
-			else
-				status = NAND_Pipeline_Read_Ahead(
-					read_data_l, block, page,
-					MAX_PAGES_PER_RW);
-
-			if (status == FAIL)
-				return status;
-
-			read_data_l += DeviceInfo.wPageDataSize *
-					MAX_PAGES_PER_RW;
-			page_count -= MAX_PAGES_PER_RW;
-			page += MAX_PAGES_PER_RW;
-		}
-		if (ioread32(FlashReg + MULTIPLANE_OPERATION))
-			status = NAND_Multiplane_Read(read_data_l,
-					block, page, page_count);
-		else
-			status = NAND_Pipeline_Read_Ahead(
-					read_data_l, block, page, page_count);
-
-		return status;
-	}
-
-	iowrite32(1, FlashReg + DMA_ENABLE);
-	while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	/* Fill the mrst_nand_info structure */
-	info.state = INT_READ_PAGE_MAIN;
-	info.read_data = read_data;
-	info.flash_bank = flash_bank;
-	info.block = block;
-	info.page = page;
-	info.ret = PASS;
-
-	ddma_trans(read_data, flash_add, flash_bank, 0, 1);
-
-	iowrite32(1, FlashReg + GLOBAL_INT_ENABLE); /* Enable Interrupt */
-
-	ret = wait_for_completion_timeout(&info.complete, 10 * HZ);
-	if (!ret) {
-		printk(KERN_ERR "Wait for completion timeout "
-			"in %s, Line %d\n", __FILE__, __LINE__);
-		status = ERR;
-	} else {
-		status = info.ret;
-	}
-
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(0, FlashReg + DMA_ENABLE);
-	while ((ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	return status;
-}
-
-void Conv_Spare_Data_Log2Phy_Format(u8 *data)
-{
-	int i;
-	const u32 spareFlagBytes = DeviceInfo.wNumPageSpareFlag;
-	const u32 PageSpareSize  = DeviceInfo.wPageSpareSize;
-
-	if (enable_ecc) {
-		for (i = spareFlagBytes - 1; i >= 0; i--)
-			data[PageSpareSize - spareFlagBytes + i] = data[i];
-	}
-}
-
-void Conv_Spare_Data_Phy2Log_Format(u8 *data)
-{
-	int i;
-	const u32 spareFlagBytes = DeviceInfo.wNumPageSpareFlag;
-	const u32 PageSpareSize = DeviceInfo.wPageSpareSize;
-
-	if (enable_ecc) {
-		for (i = 0; i < spareFlagBytes; i++)
-			data[i] = data[PageSpareSize - spareFlagBytes + i];
-	}
-}
-
-
-void Conv_Main_Spare_Data_Log2Phy_Format(u8 *data, u16 page_count)
-{
-	const u32 PageSize = DeviceInfo.wPageSize;
-	const u32 PageDataSize = DeviceInfo.wPageDataSize;
-	const u32 eccBytes = DeviceInfo.wECCBytesPerSector;
-	const u32 spareSkipBytes = DeviceInfo.wSpareSkipBytes;
-	const u32 spareFlagBytes = DeviceInfo.wNumPageSpareFlag;
-	u32 eccSectorSize;
-	u32 page_offset;
-	int i, j;
-
-	eccSectorSize = ECC_SECTOR_SIZE * (DeviceInfo.wDevicesConnected);
-	if (enable_ecc) {
-		while (page_count > 0) {
-			page_offset = (page_count - 1) * PageSize;
-			j = (DeviceInfo.wPageDataSize / eccSectorSize);
-			for (i = spareFlagBytes - 1; i >= 0; i--)
-				data[page_offset +
-					(eccSectorSize + eccBytes) * j + i] =
-					data[page_offset + PageDataSize + i];
-			for (j--; j >= 1; j--) {
-				for (i = eccSectorSize - 1; i >= 0; i--)
-					data[page_offset +
-					(eccSectorSize + eccBytes) * j + i] =
-						data[page_offset +
-						eccSectorSize * j + i];
-			}
-			for (i = (PageSize - spareSkipBytes) - 1;
-				i >= PageDataSize; i--)
-				data[page_offset + i + spareSkipBytes] =
-					data[page_offset + i];
-			page_count--;
-		}
-	}
-}
-
-void Conv_Main_Spare_Data_Phy2Log_Format(u8 *data, u16 page_count)
-{
-	const u32 PageSize = DeviceInfo.wPageSize;
-	const u32 PageDataSize = DeviceInfo.wPageDataSize;
-	const u32 eccBytes = DeviceInfo.wECCBytesPerSector;
-	const u32 spareSkipBytes = DeviceInfo.wSpareSkipBytes;
-	const u32 spareFlagBytes = DeviceInfo.wNumPageSpareFlag;
-	u32 eccSectorSize;
-	u32 page_offset;
-	int i, j;
-
-	eccSectorSize = ECC_SECTOR_SIZE * (DeviceInfo.wDevicesConnected);
-	if (enable_ecc) {
-		while (page_count > 0) {
-			page_offset = (page_count - 1) * PageSize;
-			for (i = PageDataSize;
-				i < PageSize - spareSkipBytes;
-				i++)
-				data[page_offset + i] =
-					data[page_offset + i +
-					spareSkipBytes];
-			for (j = 1;
-			j < DeviceInfo.wPageDataSize / eccSectorSize;
-			j++) {
-				for (i = 0; i < eccSectorSize; i++)
-					data[page_offset +
-					eccSectorSize * j + i] =
-						data[page_offset +
-						(eccSectorSize + eccBytes) * j
-						+ i];
-			}
-			for (i = 0; i < spareFlagBytes; i++)
-				data[page_offset + PageDataSize + i] =
-					data[page_offset +
-					(eccSectorSize + eccBytes) * j + i];
-			page_count--;
-		}
-	}
-}
-
-/* Un-tested function */
-u16 NAND_Multiplane_Read(u8 *read_data, u32 block, u16 page,
-			    u16 page_count)
-{
-	u32 status = PASS;
-	u32 NumPages = page_count;
-	u64 flash_add;
-	u32 flash_bank;
-	u32 intr_status = 0;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	u32 ecc_done_OR_dma_comp;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		* DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	if (status == PASS) {
-		intr_status = intr_status_addresses[flash_bank];
-		iowrite32(ioread32(FlashReg + intr_status),
-			FlashReg + intr_status);
-
-		iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-		iowrite32(0x01, FlashReg + MULTIPLANE_OPERATION);
-
-		iowrite32(1, FlashReg + DMA_ENABLE);
-		while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-			;
-		index_addr((u32)(MODE_10 | (flash_bank << 24) |
-			(flash_add >> DeviceInfo.nBitsInPageDataSize)), 0x42);
-		ddma_trans(read_data, flash_add, flash_bank, 0, NumPages);
-
-		ecc_done_OR_dma_comp = 0;
-		while (1) {
-			if (enable_ecc) {
-				while (!ioread32(FlashReg + intr_status))
-					;
-
-				if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__ECC_ERR) {
-					iowrite32(INTR_STATUS0__ECC_ERR,
-						FlashReg + intr_status);
-					status = do_ecc_new(flash_bank,
-						read_data, block, page);
-				} else if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__DMA_CMD_COMP) {
-					iowrite32(INTR_STATUS0__DMA_CMD_COMP,
-						FlashReg + intr_status);
-
-					if (1 == ecc_done_OR_dma_comp)
-						break;
-
-					ecc_done_OR_dma_comp = 1;
-				} else if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__ECC_TRANSACTION_DONE) {
-					iowrite32(
-					INTR_STATUS0__ECC_TRANSACTION_DONE,
-					FlashReg + intr_status);
-
-					if (1 == ecc_done_OR_dma_comp)
-						break;
-
-					ecc_done_OR_dma_comp = 1;
-				}
-			} else {
-				while (!(ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__DMA_CMD_COMP))
-					;
-				iowrite32(INTR_STATUS0__DMA_CMD_COMP,
-					FlashReg + intr_status);
-				break;
-			}
-
-			iowrite32((~INTR_STATUS0__ECC_ERR) &
-				(~INTR_STATUS0__ECC_TRANSACTION_DONE) &
-				(~INTR_STATUS0__DMA_CMD_COMP),
-				FlashReg + intr_status);
-
-		}
-
-		iowrite32(ioread32(FlashReg + intr_status),
-			FlashReg + intr_status);
-
-		iowrite32(0, FlashReg + DMA_ENABLE);
-
-		while ((ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-			;
-
-		iowrite32(0, FlashReg + MULTIPLANE_OPERATION);
-	}
-
-	return status;
-}
-
-u16 NAND_Pipeline_Read_Ahead(u8 *read_data, u32 block,
-				u16 page, u16 page_count)
-{
-	u32 status = PASS;
-	u32 NumPages = page_count;
-	u64 flash_add;
-	u32 flash_bank;
-	u32 intr_status = 0;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	int ret;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-
-	if (page_count < 2)
-		status = FAIL;
-
-	if (status != PASS)
-		return status;
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		*DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	intr_status = intr_status_addresses[flash_bank];
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(1, FlashReg + DMA_ENABLE);
-	while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-	/* Fill the mrst_nand_info structure */
-	info.state = INT_PIPELINE_READ_AHEAD;
-	info.read_data = read_data;
-	info.flash_bank = flash_bank;
-	info.block = block;
-	info.page = page;
-	info.ret = PASS;
-
-	index_addr((u32)(MODE_10 | (flash_bank << 24) |
-		(flash_add >> DeviceInfo.nBitsInPageDataSize)), 0x42);
-
-	ddma_trans(read_data, flash_add, flash_bank, 0, NumPages);
-
-	iowrite32(1, FlashReg + GLOBAL_INT_ENABLE); /* Enable Interrupt */
-
-	ret = wait_for_completion_timeout(&info.complete, 10 * HZ);
-	if (!ret) {
-		printk(KERN_ERR "Wait for completion timeout "
-			"in %s, Line %d\n", __FILE__, __LINE__);
-		status = ERR;
-	} else {
-		status = info.ret;
-	}
-
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(0, FlashReg + DMA_ENABLE);
-
-	while ((ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	return status;
-}
-
-
-u16 NAND_Write_Page_Main(u8 *write_data, u32 block, u16 page,
-			    u16 page_count)
-{
-	u32 status = PASS;
-	u64 flash_add;
-	u32 intr_status = 0;
-	u32 flash_bank;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	int ret;
-	u8 *write_data_l;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-	if (status != PASS)
-		return status;
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		* DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	intr_status = intr_status_addresses[flash_bank];
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-	iowrite32(INTR_STATUS0__PROGRAM_COMP |
-		INTR_STATUS0__PROGRAM_FAIL, FlashReg + intr_status);
-
-	if (page_count > 1) {
-		write_data_l = write_data;
-		while (page_count > MAX_PAGES_PER_RW) {
-			if (ioread32(FlashReg + MULTIPLANE_OPERATION))
-				status = NAND_Multiplane_Write(write_data_l,
-					block, page, MAX_PAGES_PER_RW);
-			else
-				status = NAND_Pipeline_Write_Ahead(
-					write_data_l, block, page,
-					MAX_PAGES_PER_RW);
-			if (status == FAIL)
-				return status;
-
-			write_data_l += DeviceInfo.wPageDataSize *
-					MAX_PAGES_PER_RW;
-			page_count -= MAX_PAGES_PER_RW;
-			page += MAX_PAGES_PER_RW;
-		}
-		if (ioread32(FlashReg + MULTIPLANE_OPERATION))
-			status = NAND_Multiplane_Write(write_data_l,
-				block, page, page_count);
-		else
-			status = NAND_Pipeline_Write_Ahead(write_data_l,
-				block, page, page_count);
-
-		return status;
-	}
-
-	iowrite32(1, FlashReg + DMA_ENABLE);
-	while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	/* Fill the mrst_nand_info structure */
-	info.state = INT_WRITE_PAGE_MAIN;
-	info.write_data = write_data;
-	info.flash_bank = flash_bank;
-	info.block = block;
-	info.page = page;
-	info.ret = PASS;
-
-	ddma_trans(write_data, flash_add, flash_bank, 1, 1);
-
-	iowrite32(1, FlashReg + GLOBAL_INT_ENABLE); /* Enable interrupt */
-
-	ret = wait_for_completion_timeout(&info.complete, 10 * HZ);
-	if (!ret) {
-		printk(KERN_ERR "Wait for completion timeout "
-			"in %s, Line %d\n", __FILE__, __LINE__);
-		status = ERR;
-	} else {
-		status = info.ret;
-	}
-
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(0, FlashReg + DMA_ENABLE);
-	while (ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG)
-		;
-
-	return status;
-}
-
-void NAND_ECC_Ctrl(int enable)
-{
-	if (enable) {
-		nand_dbg_print(NAND_DBG_WARN,
-			"Will enable ECC in %s, Line %d, Function: %s\n",
-			__FILE__, __LINE__, __func__);
-		iowrite32(1, FlashReg + ECC_ENABLE);
-		enable_ecc = 1;
-	} else {
-		nand_dbg_print(NAND_DBG_WARN,
-			"Will disable ECC in %s, Line %d, Function: %s\n",
-			__FILE__, __LINE__, __func__);
-		iowrite32(0, FlashReg + ECC_ENABLE);
-		enable_ecc = 0;
-	}
-}
-
-u16 NAND_Write_Page_Main_Spare(u8 *write_data, u32 block,
-					u16 page, u16 page_count)
-{
-	u32 status = PASS;
-	u32 i, j, page_num = 0;
-	u32 PageSize = DeviceInfo.wPageSize;
-	u32 PageDataSize = DeviceInfo.wPageDataSize;
-	u32 eccBytes = DeviceInfo.wECCBytesPerSector;
-	u32 spareFlagBytes = DeviceInfo.wNumPageSpareFlag;
-	u32 spareSkipBytes  = DeviceInfo.wSpareSkipBytes;
-	u64 flash_add;
-	u32 eccSectorSize;
-	u32 flash_bank;
-	u32 intr_status = 0;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	u8 *page_main_spare = buf_write_page_main_spare;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	eccSectorSize = ECC_SECTOR_SIZE * (DeviceInfo.wDevicesConnected);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	if (status == PASS) {
-		intr_status = intr_status_addresses[flash_bank];
-
-		iowrite32(1, FlashReg + TRANSFER_SPARE_REG);
-
-		while ((status != FAIL) && (page_count > 0)) {
-			flash_add = (u64)(block %
-			(DeviceInfo.wTotalBlocks / totalUsedBanks)) *
-			DeviceInfo.wBlockDataSize +
-			(u64)page * DeviceInfo.wPageDataSize;
-
-			iowrite32(ioread32(FlashReg + intr_status),
-				FlashReg + intr_status);
-
-			iowrite32((u32)(MODE_01 | (flash_bank << 24) |
-				(flash_add >>
-				DeviceInfo.nBitsInPageDataSize)),
-				FlashMem);
-
-			if (enable_ecc) {
-				for (j = 0;
-				     j <
-				     DeviceInfo.wPageDataSize / eccSectorSize;
-				     j++) {
-					for (i = 0; i < eccSectorSize; i++)
-						page_main_spare[(eccSectorSize +
-								 eccBytes) * j +
-								i] =
-						    write_data[eccSectorSize *
-							       j + i];
-
-					for (i = 0; i < eccBytes; i++)
-						page_main_spare[(eccSectorSize +
-								 eccBytes) * j +
-								eccSectorSize +
-								i] =
-						    write_data[PageDataSize +
-							       spareFlagBytes +
-							       eccBytes * j +
-							       i];
-				}
-
-				for (i = 0; i < spareFlagBytes; i++)
-					page_main_spare[(eccSectorSize +
-							 eccBytes) * j + i] =
-					    write_data[PageDataSize + i];
-
-				for (i = PageSize - 1; i >= PageDataSize +
-							spareSkipBytes; i--)
-					page_main_spare[i] = page_main_spare[i -
-								spareSkipBytes];
-
-				for (i = PageDataSize; i < PageDataSize +
-							spareSkipBytes; i++)
-					page_main_spare[i] = 0xff;
-
-				for (i = 0; i < PageSize / 4; i++)
-					iowrite32(
-					*((u32 *)page_main_spare + i),
-					FlashMem + 0x10);
-			} else {
-
-				for (i = 0; i < PageSize / 4; i++)
-					iowrite32(*((u32 *)write_data + i),
-						FlashMem + 0x10);
-			}
-
-			while (!(ioread32(FlashReg + intr_status) &
-				(INTR_STATUS0__PROGRAM_COMP |
-				INTR_STATUS0__PROGRAM_FAIL)))
-				;
-
-			if (ioread32(FlashReg + intr_status) &
-				INTR_STATUS0__PROGRAM_FAIL)
-				status = FAIL;
-
-			iowrite32(ioread32(FlashReg + intr_status),
-					FlashReg + intr_status);
-
-			page_num++;
-			page_count--;
-			write_data += PageSize;
-		}
-
-		iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-	}
-
-	return status;
-}
-
-u16 NAND_Read_Page_Main_Spare(u8 *read_data, u32 block, u16 page,
-				 u16 page_count)
-{
-	u32 status = PASS;
-	u32 i, j;
-	u64 flash_add = 0;
-	u32 PageSize = DeviceInfo.wPageSize;
-	u32 PageDataSize = DeviceInfo.wPageDataSize;
-	u32 PageSpareSize = DeviceInfo.wPageSpareSize;
-	u32 eccBytes = DeviceInfo.wECCBytesPerSector;
-	u32 spareFlagBytes = DeviceInfo.wNumPageSpareFlag;
-	u32 spareSkipBytes  = DeviceInfo.wSpareSkipBytes;
-	u32 eccSectorSize;
-	u32 flash_bank;
-	u32 intr_status = 0;
-	u8 *read_data_l = read_data;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	u8 *page_main_spare = buf_read_page_main_spare;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	eccSectorSize = ECC_SECTOR_SIZE * (DeviceInfo.wDevicesConnected);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	if (status == PASS) {
-		intr_status = intr_status_addresses[flash_bank];
-
-		iowrite32(1, FlashReg + TRANSFER_SPARE_REG);
-
-		iowrite32(ioread32(FlashReg + intr_status),
-				FlashReg + intr_status);
-
-		while ((status != FAIL) && (page_count > 0)) {
-			flash_add = (u64)(block %
-				(DeviceInfo.wTotalBlocks / totalUsedBanks))
-				* DeviceInfo.wBlockDataSize +
-				(u64)page * DeviceInfo.wPageDataSize;
-
-			index_addr((u32)(MODE_10 | (flash_bank << 24) |
-				(flash_add >> DeviceInfo.nBitsInPageDataSize)),
-				0x43);
-			index_addr((u32)(MODE_10 | (flash_bank << 24) |
-				(flash_add >> DeviceInfo.nBitsInPageDataSize)),
-				0x2000 | page_count);
-
-			while (!(ioread32(FlashReg + intr_status) &
-				INTR_STATUS0__LOAD_COMP))
-				;
-
-			iowrite32((u32)(MODE_01 | (flash_bank << 24) |
-				(flash_add >>
-				DeviceInfo.nBitsInPageDataSize)),
-				FlashMem);
-
-			for (i = 0; i < PageSize / 4; i++)
-				*(((u32 *)page_main_spare) + i) =
-					ioread32(FlashMem + 0x10);
-
-			if (enable_ecc) {
-				for (i = PageDataSize;  i < PageSize -
-							spareSkipBytes; i++)
-					page_main_spare[i] = page_main_spare[i +
-								spareSkipBytes];
-
-				for (j = 0;
-				j < DeviceInfo.wPageDataSize / eccSectorSize;
-				j++) {
-
-					for (i = 0; i < eccSectorSize; i++)
-						read_data_l[eccSectorSize * j +
-							    i] =
-						    page_main_spare[
-							(eccSectorSize +
-							eccBytes) * j + i];
-
-					for (i = 0; i < eccBytes; i++)
-						read_data_l[PageDataSize +
-							    spareFlagBytes +
-							    eccBytes * j + i] =
-						    page_main_spare[
-							(eccSectorSize +
-							eccBytes) * j +
-							eccSectorSize + i];
-				}
-
-				for (i = 0; i < spareFlagBytes; i++)
-					read_data_l[PageDataSize + i] =
-					    page_main_spare[(eccSectorSize +
-							     eccBytes) * j + i];
-			} else {
-				for (i = 0; i < (PageDataSize + PageSpareSize);
-				     i++)
-					read_data_l[i] = page_main_spare[i];
-
-			}
-
-			if (enable_ecc) {
-				while (!(ioread32(FlashReg + intr_status) &
-					(INTR_STATUS0__ECC_TRANSACTION_DONE |
-					INTR_STATUS0__ECC_ERR)))
-					;
-
-				if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__ECC_ERR) {
-					iowrite32(INTR_STATUS0__ECC_ERR,
-						FlashReg + intr_status);
-					status = do_ecc_new(flash_bank,
-						read_data, block, page);
-				}
-
-				if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__ECC_TRANSACTION_DONE &
-					INTR_STATUS0__ECC_ERR) {
-					iowrite32(INTR_STATUS0__ECC_ERR |
-					INTR_STATUS0__ECC_TRANSACTION_DONE,
-					FlashReg + intr_status);
-				} else if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__ECC_TRANSACTION_DONE) {
-					iowrite32(
-					INTR_STATUS0__ECC_TRANSACTION_DONE,
-					FlashReg + intr_status);
-				} else if (ioread32(FlashReg + intr_status) &
-					INTR_STATUS0__ECC_ERR) {
-					iowrite32(INTR_STATUS0__ECC_ERR,
-						FlashReg + intr_status);
-				}
-			}
-
-			page++;
-			page_count--;
-			read_data_l += PageSize;
-		}
-	}
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-	index_addr((u32)(MODE_10 | (flash_bank << 24) |
-		(flash_add >> DeviceInfo.nBitsInPageDataSize)), 0x42);
-
-	return status;
-}
-
-u16 NAND_Pipeline_Write_Ahead(u8 *write_data, u32 block,
-			u16 page, u16 page_count)
-{
-	u16 status = PASS;
-	u32 NumPages = page_count;
-	u64 flash_add;
-	u32 flash_bank;
-	u32 intr_status = 0;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	int ret;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-
-	if (page_count < 2)
-		status = FAIL;
-
-	if (status != PASS)
-		return status;
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		* DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	intr_status = intr_status_addresses[flash_bank];
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(1, FlashReg + DMA_ENABLE);
-	while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-	/* Fill the mrst_nand_info structure */
-	info.state = INT_PIPELINE_WRITE_AHEAD;
-	info.write_data = write_data;
-	info.flash_bank = flash_bank;
-	info.block = block;
-	info.page = page;
-	info.ret = PASS;
-
-	index_addr((u32)(MODE_10 | (flash_bank << 24) |
-		(flash_add >> DeviceInfo.nBitsInPageDataSize)), 0x42);
-
-	ddma_trans(write_data, flash_add, flash_bank, 1, NumPages);
-
-	iowrite32(1, FlashReg + GLOBAL_INT_ENABLE); /* Enable interrupt */
-
-	ret = wait_for_completion_timeout(&info.complete, 10 * HZ);
-	if (!ret) {
-		printk(KERN_ERR "Wait for completion timeout "
-			"in %s, Line %d\n", __FILE__, __LINE__);
-		status = ERR;
-	} else {
-		status = info.ret;
-	}
-
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(0, FlashReg + DMA_ENABLE);
-	while ((ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	return status;
-}
-
-/* Un-tested function */
-u16 NAND_Multiplane_Write(u8 *write_data, u32 block, u16 page,
-			     u16 page_count)
-{
-	u16 status = PASS;
-	u32 NumPages = page_count;
-	u64 flash_add;
-	u32 flash_bank;
-	u32 intr_status = 0;
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	u16 status2 = PASS;
-	u32 t;
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	status = Boundary_Check_Block_Page(block, page, page_count);
-	if (status != PASS)
-		return status;
-
-	flash_add = (u64)(block % (DeviceInfo.wTotalBlocks / totalUsedBanks))
-		* DeviceInfo.wBlockDataSize +
-		(u64)page * DeviceInfo.wPageDataSize;
-
-	flash_bank = block / (DeviceInfo.wTotalBlocks / totalUsedBanks);
-
-	intr_status = intr_status_addresses[flash_bank];
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-	iowrite32(0x01, FlashReg + MULTIPLANE_OPERATION);
-
-	iowrite32(1, FlashReg + DMA_ENABLE);
-	while (!(ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	iowrite32(0, FlashReg + TRANSFER_SPARE_REG);
-
-	index_addr((u32)(MODE_10 | (flash_bank << 24) |
-		(flash_add >> DeviceInfo.nBitsInPageDataSize)), 0x42);
-
-	ddma_trans(write_data, flash_add, flash_bank, 1, NumPages);
-
-	while (1) {
-		while (!ioread32(FlashReg + intr_status))
-			;
-
-		if (ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__DMA_CMD_COMP) {
-			iowrite32(INTR_STATUS0__DMA_CMD_COMP,
-				FlashReg + intr_status);
-			status = PASS;
-			if (status2 == FAIL)
-				status = FAIL;
-			break;
-		} else if (ioread32(FlashReg + intr_status) &
-				INTR_STATUS0__PROGRAM_FAIL) {
-			status2 = FAIL;
-			status = FAIL;
-			t = ioread32(FlashReg + intr_status) &
-				INTR_STATUS0__PROGRAM_FAIL;
-			iowrite32(t, FlashReg + intr_status);
-		} else {
-			iowrite32((~INTR_STATUS0__PROGRAM_FAIL) &
-				(~INTR_STATUS0__DMA_CMD_COMP),
-				FlashReg + intr_status);
-		}
-	}
-
-	iowrite32(ioread32(FlashReg + intr_status), FlashReg + intr_status);
-
-	iowrite32(0, FlashReg + DMA_ENABLE);
-
-	while ((ioread32(FlashReg + DMA_ENABLE) & DMA_ENABLE__FLAG))
-		;
-
-	iowrite32(0, FlashReg + MULTIPLANE_OPERATION);
-
-	return status;
-}
-
-
-#if CMD_DMA
-static irqreturn_t cdma_isr(int irq, void *dev_id)
-{
-	struct mrst_nand_info *dev = dev_id;
-	int first_failed_cmd;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	if (!is_cdma_interrupt())
-		return IRQ_NONE;
-
-	/* Disable controller interrupts */
-	iowrite32(0, FlashReg + GLOBAL_INT_ENABLE);
-	GLOB_FTL_Event_Status(&first_failed_cmd);
-	complete(&dev->complete);
-
-	return IRQ_HANDLED;
-}
-#else
-static void handle_nand_int_read(struct mrst_nand_info *dev)
-{
-	u32 intr_status_addresses[4] = {INTR_STATUS0,
-		INTR_STATUS1, INTR_STATUS2, INTR_STATUS3};
-	u32 intr_status;
-	u32 ecc_done_OR_dma_comp = 0;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	dev->ret = PASS;
-	intr_status = intr_status_addresses[dev->flash_bank];
-
-	while (1) {
-		if (enable_ecc) {
-			if (ioread32(FlashReg + intr_status) &
-				INTR_STATUS0__ECC_ERR) {
-				iowrite32(INTR_STATUS0__ECC_ERR,
-					FlashReg + intr_status);
-				dev->ret = do_ecc_new(dev->flash_bank,
-						dev->read_data,
-						dev->block, dev->page);
-			} else if (ioread32(FlashReg + intr_status) &
-				INTR_STATUS0__DMA_CMD_COMP) {
-				iowrite32(INTR_STATUS0__DMA_CMD_COMP,
-					FlashReg + intr_status);
-				if (1 == ecc_done_OR_dma_comp)
-					break;
-				ecc_done_OR_dma_comp = 1;
-			} else if (ioread32(FlashReg + intr_status) &
-				INTR_STATUS0__ECC_TRANSACTION_DONE) {
-				iowrite32(INTR_STATUS0__ECC_TRANSACTION_DONE,
-					FlashReg + intr_status);
-				if (1 == ecc_done_OR_dma_comp)
-					break;
-				ecc_done_OR_dma_comp = 1;
-			}
-		} else {
-			if (ioread32(FlashReg + intr_status) &
-				INTR_STATUS0__DMA_CMD_COMP) {
-				iowrite32(INTR_STATUS0__DMA_CMD_COMP,
-					FlashReg + intr_status);
-				break;
-			} else {
-				printk(KERN_ERR "Illegal INTS "
-					"(offset addr 0x%x) value: 0x%x\n",
-					intr_status,
-					ioread32(FlashReg + intr_status));
-			}
-		}
-
-		iowrite32((~INTR_STATUS0__ECC_ERR) &
-		(~INTR_STATUS0__ECC_TRANSACTION_DONE) &
-		(~INTR_STATUS0__DMA_CMD_COMP),
-		FlashReg + intr_status);
-	}
-}
-
-static void handle_nand_int_write(struct mrst_nand_info *dev)
-{
-	u32 intr_status;
-	u32 intr[4] = {INTR_STATUS0, INTR_STATUS1,
-		INTR_STATUS2, INTR_STATUS3};
-	int status = PASS;
-
-	nand_dbg_print(NAND_DBG_DEBUG, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	dev->ret = PASS;
-	intr_status = intr[dev->flash_bank];
-
-	while (1) {
-		while (!ioread32(FlashReg + intr_status))
-			;
-
-		if (ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__DMA_CMD_COMP) {
-			iowrite32(INTR_STATUS0__DMA_CMD_COMP,
-				FlashReg + intr_status);
-			if (FAIL == status)
-				dev->ret = FAIL;
-			break;
-		} else if (ioread32(FlashReg + intr_status) &
-			INTR_STATUS0__PROGRAM_FAIL) {
-			status = FAIL;
-			iowrite32(INTR_STATUS0__PROGRAM_FAIL,
-				FlashReg + intr_status);
-		} else {
-			iowrite32((~INTR_STATUS0__PROGRAM_FAIL) &
-				(~INTR_STATUS0__DMA_CMD_COMP),
-				FlashReg + intr_status);
-		}
-	}
-}
-
-static irqreturn_t ddma_isr(int irq, void *dev_id)
-{
-	struct mrst_nand_info *dev = dev_id;
-	u32 int_mask, ints0, ints1, ints2, ints3, ints_offset;
-	u32 intr[4] = {INTR_STATUS0, INTR_STATUS1,
-		INTR_STATUS2, INTR_STATUS3};
-
-	int_mask = INTR_STATUS0__DMA_CMD_COMP |
-		INTR_STATUS0__ECC_TRANSACTION_DONE |
-		INTR_STATUS0__ECC_ERR |
-		INTR_STATUS0__PROGRAM_FAIL |
-		INTR_STATUS0__ERASE_FAIL;
-
-	ints0 = ioread32(FlashReg + INTR_STATUS0);
-	ints1 = ioread32(FlashReg + INTR_STATUS1);
-	ints2 = ioread32(FlashReg + INTR_STATUS2);
-	ints3 = ioread32(FlashReg + INTR_STATUS3);
-
-	ints_offset = intr[dev->flash_bank];
-
-	nand_dbg_print(NAND_DBG_DEBUG,
-		"INTR0: 0x%x, INTR1: 0x%x, INTR2: 0x%x, INTR3: 0x%x, "
-		"DMA_INTR: 0x%x, "
-		"dev->state: 0x%x, dev->flash_bank: %d\n",
-		ints0, ints1, ints2, ints3,
-		ioread32(FlashReg + DMA_INTR),
-		dev->state, dev->flash_bank);
-
-	if (!(ioread32(FlashReg + ints_offset) & int_mask)) {
-		iowrite32(ints0, FlashReg + INTR_STATUS0);
-		iowrite32(ints1, FlashReg + INTR_STATUS1);
-		iowrite32(ints2, FlashReg + INTR_STATUS2);
-		iowrite32(ints3, FlashReg + INTR_STATUS3);
-		nand_dbg_print(NAND_DBG_WARN,
-			"ddma_isr: Invalid interrupt for NAND controller. "
-			"Ignore it\n");
-		return IRQ_NONE;
-	}
-
-	switch (dev->state) {
-	case INT_READ_PAGE_MAIN:
-	case INT_PIPELINE_READ_AHEAD:
-		/* Disable controller interrupts */
-		iowrite32(0, FlashReg + GLOBAL_INT_ENABLE);
-		handle_nand_int_read(dev);
-		break;
-	case INT_WRITE_PAGE_MAIN:
-	case INT_PIPELINE_WRITE_AHEAD:
-		iowrite32(0, FlashReg + GLOBAL_INT_ENABLE);
-		handle_nand_int_write(dev);
-		break;
-	default:
-		printk(KERN_ERR "ddma_isr - Illegal state: 0x%x\n",
-			dev->state);
-		return IRQ_NONE;
-	}
-
-	dev->state = INT_IDLE_STATE;
-	complete(&dev->complete);
-	return IRQ_HANDLED;
-}
-#endif
-
-static const struct pci_device_id nand_pci_ids[] = {
-	{
-	 .vendor = 0x8086,
-	 .device = 0x0809,
-	 .subvendor = PCI_ANY_ID,
-	 .subdevice = PCI_ANY_ID,
-	 },
-	{ /* end: all zeroes */ }
-};
-
-static int nand_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
-{
-	int ret = -ENODEV;
-	unsigned long csr_base;
-	unsigned long csr_len;
-	struct mrst_nand_info *pndev = &info;
-	u32 int_mask;
-
-	ret = pci_enable_device(dev);
-	if (ret) {
-		printk(KERN_ERR "Spectra: pci_enable_device failed.\n");
-		return ret;
-	}
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	FlashReg = ioremap_nocache(GLOB_HWCTL_REG_BASE,
-			GLOB_HWCTL_REG_SIZE);
-	if (!FlashReg) {
-		printk(KERN_ERR "Spectra: ioremap_nocache failed!");
-		goto failed_disable;
-	}
-	nand_dbg_print(NAND_DBG_WARN,
-		"Spectra: Remapped reg base address: "
-		"0x%p, len: %d\n",
-		FlashReg, GLOB_HWCTL_REG_SIZE);
-
-	FlashMem = ioremap_nocache(GLOB_HWCTL_MEM_BASE,
-			GLOB_HWCTL_MEM_SIZE);
-	if (!FlashMem) {
-		printk(KERN_ERR "Spectra: ioremap_nocache failed!");
-		iounmap(FlashReg);
-		goto failed_disable;
-	}
-	nand_dbg_print(NAND_DBG_WARN,
-		"Spectra: Remapped flash base address: "
-		"0x%p, len: %d\n",
-		(void *)FlashMem, GLOB_HWCTL_MEM_SIZE);
-
-	nand_dbg_print(NAND_DBG_DEBUG, "Dump timing register values:"
-			"acc_clks: %d, re_2_we: %d, we_2_re: %d,"
-			"addr_2_data: %d, rdwr_en_lo_cnt: %d, "
-			"rdwr_en_hi_cnt: %d, cs_setup_cnt: %d\n",
-			ioread32(FlashReg + ACC_CLKS),
-			ioread32(FlashReg + RE_2_WE),
-			ioread32(FlashReg + WE_2_RE),
-			ioread32(FlashReg + ADDR_2_DATA),
-			ioread32(FlashReg + RDWR_EN_LO_CNT),
-			ioread32(FlashReg + RDWR_EN_HI_CNT),
-			ioread32(FlashReg + CS_SETUP_CNT));
-
-	NAND_Flash_Reset();
-
-	iowrite32(0, FlashReg + GLOBAL_INT_ENABLE);
-
-#if CMD_DMA
-	info.pcmds_num = 0;
-	info.flash_bank = 0;
-	info.cdma_num = 0;
-	int_mask = (DMA_INTR__DESC_COMP_CHANNEL0 |
-		DMA_INTR__DESC_COMP_CHANNEL1 |
-		DMA_INTR__DESC_COMP_CHANNEL2 |
-		DMA_INTR__DESC_COMP_CHANNEL3 |
-		DMA_INTR__MEMCOPY_DESC_COMP);
-	iowrite32(int_mask, FlashReg + DMA_INTR_EN);
-	iowrite32(0xFFFF, FlashReg + DMA_INTR);
-
-	int_mask = (INTR_STATUS0__ECC_ERR |
-		INTR_STATUS0__PROGRAM_FAIL |
-		INTR_STATUS0__ERASE_FAIL);
-#else
-	int_mask = INTR_STATUS0__DMA_CMD_COMP |
-		INTR_STATUS0__ECC_TRANSACTION_DONE |
-		INTR_STATUS0__ECC_ERR |
-		INTR_STATUS0__PROGRAM_FAIL |
-		INTR_STATUS0__ERASE_FAIL;
-#endif
-	iowrite32(int_mask, FlashReg + INTR_EN0);
-	iowrite32(int_mask, FlashReg + INTR_EN1);
-	iowrite32(int_mask, FlashReg + INTR_EN2);
-	iowrite32(int_mask, FlashReg + INTR_EN3);
-
-	/* Clear all status bits */
-	iowrite32(0xFFFF, FlashReg + INTR_STATUS0);
-	iowrite32(0xFFFF, FlashReg + INTR_STATUS1);
-	iowrite32(0xFFFF, FlashReg + INTR_STATUS2);
-	iowrite32(0xFFFF, FlashReg + INTR_STATUS3);
-
-	iowrite32(0x0F, FlashReg + RB_PIN_ENABLED);
-	iowrite32(CHIP_EN_DONT_CARE__FLAG, FlashReg + CHIP_ENABLE_DONT_CARE);
-
-	/* Should set value for these registers when init */
-	iowrite32(0, FlashReg + TWO_ROW_ADDR_CYCLES);
-	iowrite32(1, FlashReg + ECC_ENABLE);
-	enable_ecc = 1;
-
-	pci_set_master(dev);
-	pndev->dev = dev;
-
-	csr_base = pci_resource_start(dev, 0);
-	if (!csr_base) {
-		printk(KERN_ERR "Spectra: pci_resource_start failed!\n");
-		ret = -ENODEV;
-		goto failed_req_csr;
-	}
-
-	csr_len = pci_resource_len(dev, 0);
-	if (!csr_len) {
-		printk(KERN_ERR "Spectra: pci_resource_len failed!\n");
-		ret = -ENODEV;
-		goto failed_req_csr;
-	}
-
-	ret = pci_request_regions(dev, SPECTRA_NAND_NAME);
-	if (ret) {
-		printk(KERN_ERR "Spectra: Unable to request "
-		       "memory region\n");
-		goto failed_req_csr;
-	}
-
-	pndev->ioaddr = ioremap_nocache(csr_base, csr_len);
-	if (!pndev->ioaddr) {
-		printk(KERN_ERR "Spectra: Unable to remap memory region\n");
-		ret = -ENOMEM;
-		goto failed_remap_csr;
-	}
-	nand_dbg_print(NAND_DBG_DEBUG, "Spectra: CSR 0x%08lx -> 0x%p (0x%lx)\n",
-		       csr_base, pndev->ioaddr, csr_len);
-
-	init_completion(&pndev->complete);
-	nand_dbg_print(NAND_DBG_DEBUG, "Spectra: IRQ %d\n", dev->irq);
-
-#if CMD_DMA
-	if (request_irq(dev->irq, cdma_isr, IRQF_SHARED,
-			SPECTRA_NAND_NAME, &info)) {
-		printk(KERN_ERR "Spectra: Unable to allocate IRQ\n");
-		ret = -ENODEV;
-		iounmap(pndev->ioaddr);
-		goto failed_remap_csr;
-	}
-#else
-	if (request_irq(dev->irq, ddma_isr, IRQF_SHARED,
-			SPECTRA_NAND_NAME, &info)) {
-		printk(KERN_ERR "Spectra: Unable to allocate IRQ\n");
-		ret = -ENODEV;
-		iounmap(pndev->ioaddr);
-		goto failed_remap_csr;
-	}
-#endif
-
-	pci_set_drvdata(dev, pndev);
-
-	ret = GLOB_LLD_Read_Device_ID();
-	if (ret) {
-		iounmap(pndev->ioaddr);
-		goto failed_remap_csr;
-	}
-
-	ret = register_spectra_ftl();
-	if (ret) {
-		iounmap(pndev->ioaddr);
-		goto failed_remap_csr;
-	}
-
-	return 0;
-
-failed_remap_csr:
-	pci_release_regions(dev);
-failed_req_csr:
-	iounmap(FlashMem);
-	iounmap(FlashReg);
-failed_disable:
-	pci_disable_device(dev);
-
-	return ret;
-}
-
-static void nand_pci_remove(struct pci_dev *dev)
-{
-	struct mrst_nand_info *pndev = pci_get_drvdata(dev);
-
-	nand_dbg_print(NAND_DBG_WARN, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-#if CMD_DMA
-	free_irq(dev->irq, pndev);
-#endif
-	iounmap(pndev->ioaddr);
-	pci_release_regions(dev);
-	pci_disable_device(dev);
-}
-
-MODULE_DEVICE_TABLE(pci, nand_pci_ids);
-
-static struct pci_driver nand_pci_driver = {
-	.name = SPECTRA_NAND_NAME,
-	.id_table = nand_pci_ids,
-	.probe = nand_pci_probe,
-	.remove = nand_pci_remove,
-};
-
-int NAND_Flash_Init(void)
-{
-	int retval;
-
-	nand_dbg_print(NAND_DBG_TRACE, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
-
-	retval = pci_register_driver(&nand_pci_driver);
-	if (retval)
-		return -ENOMEM;
-
-	return PASS;
-}
-
-/* Free memory */
-int nand_release_spectra(void)
-{
-	pci_unregister_driver(&nand_pci_driver);
-	iounmap(FlashMem);
-	iounmap(FlashReg);
-
-	return 0;
-}
-
-
-
diff --git a/drivers/staging/spectra/lld_nand.h b/drivers/staging/spectra/lld_nand.h
deleted file mode 100644
index d08388287da8..000000000000
--- a/drivers/staging/spectra/lld_nand.h
+++ /dev/null
@@ -1,131 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#ifndef _LLD_NAND_
-#define _LLD_NAND_
-
-#ifdef ELDORA
-#include "defs.h"
-#else
-#include "flash.h"
-#include "ffsport.h"
-#endif
-
-#define MODE_00    0x00000000
-#define MODE_01    0x04000000
-#define MODE_10    0x08000000
-#define MODE_11    0x0C000000
-
-
-#define DATA_TRANSFER_MODE              0
-#define PROTECTION_PER_BLOCK            1
-#define LOAD_WAIT_COUNT                 2
-#define PROGRAM_WAIT_COUNT              3
-#define ERASE_WAIT_COUNT                4
-#define INT_MONITOR_CYCLE_COUNT         5
-#define READ_BUSY_PIN_ENABLED           6
-#define MULTIPLANE_OPERATION_SUPPORT    7
-#define PRE_FETCH_MODE                  8
-#define CE_DONT_CARE_SUPPORT            9
-#define COPYBACK_SUPPORT                10
-#define CACHE_WRITE_SUPPORT             11
-#define CACHE_READ_SUPPORT              12
-#define NUM_PAGES_IN_BLOCK              13
-#define ECC_ENABLE_SELECT               14
-#define WRITE_ENABLE_2_READ_ENABLE      15
-#define ADDRESS_2_DATA                  16
-#define READ_ENABLE_2_WRITE_ENABLE      17
-#define TWO_ROW_ADDRESS_CYCLES          18
-#define MULTIPLANE_ADDRESS_RESTRICT     19
-#define ACC_CLOCKS                      20
-#define READ_WRITE_ENABLE_LOW_COUNT     21
-#define READ_WRITE_ENABLE_HIGH_COUNT    22
-
-#define ECC_SECTOR_SIZE     512
-#define LLD_MAX_FLASH_BANKS     4
-
-struct mrst_nand_info {
-	struct pci_dev *dev;
-	u32 state;
-	u32 flash_bank;
-	u8 *read_data;
-	u8 *write_data;
-	u32 block;
-	u16 page;
-	u32 use_dma;
-	void __iomem *ioaddr;  /* Mapped io reg base address */
-	int ret;
-	u32 pcmds_num;
-	struct pending_cmd *pcmds;
-	int cdma_num;           /* CDMA descriptor number in this chan */
-	u8 *cdma_desc_buf;	/* CDMA descriptor table */
-	u8 *memcp_desc_buf;	/* Memory copy descriptor table */
-	dma_addr_t cdma_desc;	/* Mapped CDMA descriptor table */
-	dma_addr_t memcp_desc;	/* Mapped memory copy descriptor table */
-	struct completion complete;
-};
-
-int NAND_Flash_Init(void);
-int nand_release_spectra(void);
-u16  NAND_Flash_Reset(void);
-u16  NAND_Read_Device_ID(void);
-u16  NAND_Erase_Block(u32 flash_add);
-u16  NAND_Write_Page_Main(u8 *write_data, u32 block, u16 page,
-				u16 page_count);
-u16  NAND_Read_Page_Main(u8 *read_data, u32 block, u16 page,
-				u16 page_count);
-u16  NAND_UnlockArrayAll(void);
-u16  NAND_Write_Page_Main_Spare(u8 *write_data, u32 block,
-				u16 page, u16 page_count);
-u16  NAND_Write_Page_Spare(u8 *read_data, u32 block, u16 page,
-				u16 page_count);
-u16  NAND_Read_Page_Main_Spare(u8 *read_data, u32 block, u16 page,
-				u16 page_count);
-u16  NAND_Read_Page_Spare(u8 *read_data, u32 block, u16 page,
-				u16 page_count);
-void NAND_LLD_Enable_Disable_Interrupts(u16 INT_ENABLE);
-u16  NAND_Get_Bad_Block(u32 block);
-u16  NAND_Pipeline_Read_Ahead(u8 *read_data, u32 block, u16 page,
-				u16 page_count);
-u16  NAND_Pipeline_Write_Ahead(u8 *write_data, u32 block,
-				u16 page, u16 page_count);
-u16  NAND_Multiplane_Read(u8 *read_data, u32 block, u16 page,
-				u16 page_count);
-u16  NAND_Multiplane_Write(u8 *write_data, u32 block, u16 page,
-				u16 page_count);
-void NAND_ECC_Ctrl(int enable);
-u16 NAND_Read_Page_Main_Polling(u8 *read_data,
-		u32 block, u16 page, u16 page_count);
-u16 NAND_Pipeline_Read_Ahead_Polling(u8 *read_data,
-			u32 block, u16 page, u16 page_count);
-void Conv_Spare_Data_Log2Phy_Format(u8 *data);
-void Conv_Spare_Data_Phy2Log_Format(u8 *data);
-void Conv_Main_Spare_Data_Log2Phy_Format(u8 *data, u16 page_count);
-void Conv_Main_Spare_Data_Phy2Log_Format(u8 *data, u16 page_count);
-
-extern void __iomem *FlashReg;
-extern void __iomem *FlashMem;
-
-extern int totalUsedBanks;
-extern u32 GLOB_valid_banks[LLD_MAX_FLASH_BANKS];
-
-#endif /*_LLD_NAND_*/
-
-
-
diff --git a/drivers/staging/spectra/nand_regs.h b/drivers/staging/spectra/nand_regs.h
deleted file mode 100644
index e192e4ae8c1e..000000000000
--- a/drivers/staging/spectra/nand_regs.h
+++ /dev/null
@@ -1,619 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#define DEVICE_RESET				0x0
-#define     DEVICE_RESET__BANK0				0x0001
-#define     DEVICE_RESET__BANK1				0x0002
-#define     DEVICE_RESET__BANK2				0x0004
-#define     DEVICE_RESET__BANK3				0x0008
-
-#define TRANSFER_SPARE_REG			0x10
-#define     TRANSFER_SPARE_REG__FLAG			0x0001
-
-#define LOAD_WAIT_CNT				0x20
-#define     LOAD_WAIT_CNT__VALUE				0xffff
-
-#define PROGRAM_WAIT_CNT			0x30
-#define     PROGRAM_WAIT_CNT__VALUE			0xffff
-
-#define ERASE_WAIT_CNT				0x40
-#define     ERASE_WAIT_CNT__VALUE			0xffff
-
-#define INT_MON_CYCCNT				0x50
-#define     INT_MON_CYCCNT__VALUE			0xffff
-
-#define RB_PIN_ENABLED				0x60
-#define     RB_PIN_ENABLED__BANK0			0x0001
-#define     RB_PIN_ENABLED__BANK1			0x0002
-#define     RB_PIN_ENABLED__BANK2			0x0004
-#define     RB_PIN_ENABLED__BANK3			0x0008
-
-#define MULTIPLANE_OPERATION			0x70
-#define     MULTIPLANE_OPERATION__FLAG			0x0001
-
-#define MULTIPLANE_READ_ENABLE			0x80
-#define     MULTIPLANE_READ_ENABLE__FLAG		0x0001
-
-#define COPYBACK_DISABLE			0x90
-#define     COPYBACK_DISABLE__FLAG			0x0001
-
-#define CACHE_WRITE_ENABLE			0xa0
-#define     CACHE_WRITE_ENABLE__FLAG			0x0001
-
-#define CACHE_READ_ENABLE			0xb0
-#define     CACHE_READ_ENABLE__FLAG			0x0001
-
-#define PREFETCH_MODE				0xc0
-#define     PREFETCH_MODE__PREFETCH_EN			0x0001
-#define     PREFETCH_MODE__PREFETCH_BURST_LENGTH	0xfff0
-
-#define CHIP_ENABLE_DONT_CARE			0xd0
-#define     CHIP_EN_DONT_CARE__FLAG			0x01
-
-#define ECC_ENABLE				0xe0
-#define     ECC_ENABLE__FLAG				0x0001
-
-#define GLOBAL_INT_ENABLE			0xf0
-#define     GLOBAL_INT_EN_FLAG				0x01
-
-#define WE_2_RE					0x100
-#define     WE_2_RE__VALUE				0x003f
-
-#define ADDR_2_DATA				0x110
-#define     ADDR_2_DATA__VALUE				0x003f
-
-#define RE_2_WE					0x120
-#define     RE_2_WE__VALUE				0x003f
-
-#define ACC_CLKS    				0x130
-#define     ACC_CLKS__VALUE				0x000f
-
-#define NUMBER_OF_PLANES			0x140
-#define     NUMBER_OF_PLANES__VALUE			0x0007
-
-#define PAGES_PER_BLOCK				0x150
-#define     PAGES_PER_BLOCK__VALUE			0xffff
-
-#define DEVICE_WIDTH				0x160
-#define     DEVICE_WIDTH__VALUE				0x0003
-
-#define DEVICE_MAIN_AREA_SIZE			0x170
-#define     DEVICE_MAIN_AREA_SIZE__VALUE		0xffff
-
-#define DEVICE_SPARE_AREA_SIZE			0x180
-#define     DEVICE_SPARE_AREA_SIZE__VALUE		0xffff
-
-#define TWO_ROW_ADDR_CYCLES			0x190
-#define     TWO_ROW_ADDR_CYCLES__FLAG			0x0001
-
-#define MULTIPLANE_ADDR_RESTRICT		0x1a0
-#define     MULTIPLANE_ADDR_RESTRICT__FLAG		0x0001
-
-#define ECC_CORRECTION				0x1b0
-#define     ECC_CORRECTION__VALUE			0x001f
-
-#define READ_MODE				0x1c0
-#define     READ_MODE__VALUE				0x000f
-
-#define WRITE_MODE				0x1d0
-#define     WRITE_MODE__VALUE				0x000f
-
-#define COPYBACK_MODE				0x1e0
-#define     COPYBACK_MODE__VALUE			0x000f
-
-#define RDWR_EN_LO_CNT				0x1f0
-#define     RDWR_EN_LO_CNT__VALUE			0x001f
-
-#define RDWR_EN_HI_CNT				0x200
-#define     RDWR_EN_HI_CNT__VALUE			0x001f
-
-#define MAX_RD_DELAY				0x210
-#define     MAX_RD_DELAY__VALUE				0x000f
-
-#define CS_SETUP_CNT				0x220
-#define     CS_SETUP_CNT__VALUE				0x001f
-
-#define SPARE_AREA_SKIP_BYTES			0x230
-#define     SPARE_AREA_SKIP_BYTES__VALUE		0x003f
-
-#define SPARE_AREA_MARKER			0x240
-#define     SPARE_AREA_MARKER__VALUE			0xffff
-
-#define DEVICES_CONNECTED			0x250
-#define     DEVICES_CONNECTED__VALUE			0x0007
-
-#define DIE_MASK					0x260
-#define     DIE_MASK__VALUE				0x00ff
-
-#define FIRST_BLOCK_OF_NEXT_PLANE		0x270
-#define     FIRST_BLOCK_OF_NEXT_PLANE__VALUE		0xffff
-
-#define WRITE_PROTECT				0x280
-#define     WRITE_PROTECT__FLAG				0x0001
-
-#define RE_2_RE					0x290
-#define     RE_2_RE__VALUE				0x003f
-
-#define MANUFACTURER_ID			0x300
-#define     MANUFACTURER_ID__VALUE			0x00ff
-
-#define DEVICE_ID				0x310
-#define     DEVICE_ID__VALUE				0x00ff
-
-#define DEVICE_PARAM_0				0x320
-#define     DEVICE_PARAM_0__VALUE			0x00ff
-
-#define DEVICE_PARAM_1				0x330
-#define     DEVICE_PARAM_1__VALUE			0x00ff
-
-#define DEVICE_PARAM_2				0x340
-#define     DEVICE_PARAM_2__VALUE			0x00ff
-
-#define LOGICAL_PAGE_DATA_SIZE			0x350
-#define     LOGICAL_PAGE_DATA_SIZE__VALUE		0xffff
-
-#define LOGICAL_PAGE_SPARE_SIZE			0x360
-#define     LOGICAL_PAGE_SPARE_SIZE__VALUE		0xffff
-
-#define REVISION					0x370
-#define     REVISION__VALUE				0xffff
-
-#define ONFI_DEVICE_FEATURES			0x380
-#define     ONFI_DEVICE_FEATURES__VALUE			0x003f
-
-#define ONFI_OPTIONAL_COMMANDS		0x390
-#define     ONFI_OPTIONAL_COMMANDS__VALUE		0x003f
-
-#define ONFI_TIMING_MODE			0x3a0
-#define     ONFI_TIMING_MODE__VALUE			0x003f
-
-#define ONFI_PGM_CACHE_TIMING_MODE		0x3b0
-#define     ONFI_PGM_CACHE_TIMING_MODE__VALUE		0x003f
-
-#define ONFI_DEVICE_NO_OF_LUNS			0x3c0
-#define     ONFI_DEVICE_NO_OF_LUNS__NO_OF_LUNS		0x00ff
-#define     ONFI_DEVICE_NO_OF_LUNS__ONFI_DEVICE		0x0100
-
-#define ONFI_DEVICE_NO_OF_BLOCKS_PER_LUN_L	0x3d0
-#define     ONFI_DEVICE_NO_OF_BLOCKS_PER_LUN_L__VALUE	0xffff
-
-#define ONFI_DEVICE_NO_OF_BLOCKS_PER_LUN_U	0x3e0
-#define     ONFI_DEVICE_NO_OF_BLOCKS_PER_LUN_U__VALUE	0xffff
-
-#define FEATURES					0x3f0
-#define     FEATURES__N_BANKS				0x0003
-#define     FEATURES__ECC_MAX_ERR			0x003c
-#define     FEATURES__DMA					0x0040
-#define     FEATURES__CMD_DMA				0x0080
-#define     FEATURES__PARTITION				0x0100
-#define     FEATURES__XDMA_SIDEBAND			0x0200
-#define     FEATURES__GPREG				0x0400
-#define     FEATURES__INDEX_ADDR				0x0800
-
-#define TRANSFER_MODE				0x400
-#define     TRANSFER_MODE__VALUE			0x0003
-
-#define INTR_STATUS0				0x410
-#define     INTR_STATUS0__ECC_TRANSACTION_DONE		0x0001
-#define     INTR_STATUS0__ECC_ERR			0x0002
-#define     INTR_STATUS0__DMA_CMD_COMP			0x0004
-#define     INTR_STATUS0__TIME_OUT			0x0008
-#define     INTR_STATUS0__PROGRAM_FAIL			0x0010
-#define     INTR_STATUS0__ERASE_FAIL			0x0020
-#define     INTR_STATUS0__LOAD_COMP			0x0040
-#define     INTR_STATUS0__PROGRAM_COMP			0x0080
-#define     INTR_STATUS0__ERASE_COMP			0x0100
-#define     INTR_STATUS0__PIPE_CPYBCK_CMD_COMP		0x0200
-#define     INTR_STATUS0__LOCKED_BLK			0x0400
-#define     INTR_STATUS0__UNSUP_CMD			0x0800
-#define     INTR_STATUS0__INT_ACT			0x1000
-#define     INTR_STATUS0__RST_COMP			0x2000
-#define     INTR_STATUS0__PIPE_CMD_ERR			0x4000
-#define     INTR_STATUS0__PAGE_XFER_INC			0x8000
-
-#define INTR_EN0					0x420
-#define     INTR_EN0__ECC_TRANSACTION_DONE		0x0001
-#define     INTR_EN0__ECC_ERR				0x0002
-#define     INTR_EN0__DMA_CMD_COMP			0x0004
-#define     INTR_EN0__TIME_OUT				0x0008
-#define     INTR_EN0__PROGRAM_FAIL			0x0010
-#define     INTR_EN0__ERASE_FAIL				0x0020
-#define     INTR_EN0__LOAD_COMP				0x0040
-#define     INTR_EN0__PROGRAM_COMP			0x0080
-#define     INTR_EN0__ERASE_COMP				0x0100
-#define     INTR_EN0__PIPE_CPYBCK_CMD_COMP		0x0200
-#define     INTR_EN0__LOCKED_BLK				0x0400
-#define     INTR_EN0__UNSUP_CMD				0x0800
-#define     INTR_EN0__INT_ACT				0x1000
-#define     INTR_EN0__RST_COMP				0x2000
-#define     INTR_EN0__PIPE_CMD_ERR			0x4000
-#define     INTR_EN0__PAGE_XFER_INC			0x8000
-
-#define PAGE_CNT0				0x430
-#define     PAGE_CNT0__VALUE				0x00ff
-
-#define ERR_PAGE_ADDR0				0x440
-#define     ERR_PAGE_ADDR0__VALUE			0xffff
-
-#define ERR_BLOCK_ADDR0			0x450
-#define     ERR_BLOCK_ADDR0__VALUE			0xffff
-
-#define INTR_STATUS1				0x460
-#define     INTR_STATUS1__ECC_TRANSACTION_DONE		0x0001
-#define     INTR_STATUS1__ECC_ERR			0x0002
-#define     INTR_STATUS1__DMA_CMD_COMP			0x0004
-#define     INTR_STATUS1__TIME_OUT			0x0008
-#define     INTR_STATUS1__PROGRAM_FAIL			0x0010
-#define     INTR_STATUS1__ERASE_FAIL			0x0020
-#define     INTR_STATUS1__LOAD_COMP			0x0040
-#define     INTR_STATUS1__PROGRAM_COMP			0x0080
-#define     INTR_STATUS1__ERASE_COMP			0x0100
-#define     INTR_STATUS1__PIPE_CPYBCK_CMD_COMP		0x0200
-#define     INTR_STATUS1__LOCKED_BLK			0x0400
-#define     INTR_STATUS1__UNSUP_CMD			0x0800
-#define     INTR_STATUS1__INT_ACT			0x1000
-#define     INTR_STATUS1__RST_COMP			0x2000
-#define     INTR_STATUS1__PIPE_CMD_ERR			0x4000
-#define     INTR_STATUS1__PAGE_XFER_INC			0x8000
-
-#define INTR_EN1					0x470
-#define     INTR_EN1__ECC_TRANSACTION_DONE		0x0001
-#define     INTR_EN1__ECC_ERR				0x0002
-#define     INTR_EN1__DMA_CMD_COMP			0x0004
-#define     INTR_EN1__TIME_OUT				0x0008
-#define     INTR_EN1__PROGRAM_FAIL			0x0010
-#define     INTR_EN1__ERASE_FAIL				0x0020
-#define     INTR_EN1__LOAD_COMP				0x0040
-#define     INTR_EN1__PROGRAM_COMP			0x0080
-#define     INTR_EN1__ERASE_COMP				0x0100
-#define     INTR_EN1__PIPE_CPYBCK_CMD_COMP		0x0200
-#define     INTR_EN1__LOCKED_BLK				0x0400
-#define     INTR_EN1__UNSUP_CMD				0x0800
-#define     INTR_EN1__INT_ACT				0x1000
-#define     INTR_EN1__RST_COMP				0x2000
-#define     INTR_EN1__PIPE_CMD_ERR			0x4000
-#define     INTR_EN1__PAGE_XFER_INC			0x8000
-
-#define PAGE_CNT1				0x480
-#define     PAGE_CNT1__VALUE				0x00ff
-
-#define ERR_PAGE_ADDR1				0x490
-#define     ERR_PAGE_ADDR1__VALUE			0xffff
-
-#define ERR_BLOCK_ADDR1			0x4a0
-#define     ERR_BLOCK_ADDR1__VALUE			0xffff
-
-#define INTR_STATUS2				0x4b0
-#define     INTR_STATUS2__ECC_TRANSACTION_DONE		0x0001
-#define     INTR_STATUS2__ECC_ERR			0x0002
-#define     INTR_STATUS2__DMA_CMD_COMP			0x0004
-#define     INTR_STATUS2__TIME_OUT			0x0008
-#define     INTR_STATUS2__PROGRAM_FAIL			0x0010
-#define     INTR_STATUS2__ERASE_FAIL			0x0020
-#define     INTR_STATUS2__LOAD_COMP			0x0040
-#define     INTR_STATUS2__PROGRAM_COMP			0x0080
-#define     INTR_STATUS2__ERASE_COMP			0x0100
-#define     INTR_STATUS2__PIPE_CPYBCK_CMD_COMP		0x0200
-#define     INTR_STATUS2__LOCKED_BLK			0x0400
-#define     INTR_STATUS2__UNSUP_CMD			0x0800
-#define     INTR_STATUS2__INT_ACT			0x1000
-#define     INTR_STATUS2__RST_COMP			0x2000
-#define     INTR_STATUS2__PIPE_CMD_ERR			0x4000
-#define     INTR_STATUS2__PAGE_XFER_INC			0x8000
-
-#define INTR_EN2					0x4c0
-#define     INTR_EN2__ECC_TRANSACTION_DONE		0x0001
-#define     INTR_EN2__ECC_ERR				0x0002
-#define     INTR_EN2__DMA_CMD_COMP			0x0004
-#define     INTR_EN2__TIME_OUT				0x0008
-#define     INTR_EN2__PROGRAM_FAIL			0x0010
-#define     INTR_EN2__ERASE_FAIL				0x0020
-#define     INTR_EN2__LOAD_COMP				0x0040
-#define     INTR_EN2__PROGRAM_COMP			0x0080
-#define     INTR_EN2__ERASE_COMP				0x0100
-#define     INTR_EN2__PIPE_CPYBCK_CMD_COMP		0x0200
-#define     INTR_EN2__LOCKED_BLK				0x0400
-#define     INTR_EN2__UNSUP_CMD				0x0800
-#define     INTR_EN2__INT_ACT				0x1000
-#define     INTR_EN2__RST_COMP				0x2000
-#define     INTR_EN2__PIPE_CMD_ERR			0x4000
-#define     INTR_EN2__PAGE_XFER_INC			0x8000
-
-#define PAGE_CNT2				0x4d0
-#define     PAGE_CNT2__VALUE				0x00ff
-
-#define ERR_PAGE_ADDR2				0x4e0
-#define     ERR_PAGE_ADDR2__VALUE			0xffff
-
-#define ERR_BLOCK_ADDR2			0x4f0
-#define     ERR_BLOCK_ADDR2__VALUE			0xffff
-
-#define INTR_STATUS3				0x500
-#define     INTR_STATUS3__ECC_TRANSACTION_DONE		0x0001
-#define     INTR_STATUS3__ECC_ERR			0x0002
-#define     INTR_STATUS3__DMA_CMD_COMP			0x0004
-#define     INTR_STATUS3__TIME_OUT			0x0008
-#define     INTR_STATUS3__PROGRAM_FAIL			0x0010
-#define     INTR_STATUS3__ERASE_FAIL			0x0020
-#define     INTR_STATUS3__LOAD_COMP			0x0040
-#define     INTR_STATUS3__PROGRAM_COMP			0x0080
-#define     INTR_STATUS3__ERASE_COMP			0x0100
-#define     INTR_STATUS3__PIPE_CPYBCK_CMD_COMP		0x0200
-#define     INTR_STATUS3__LOCKED_BLK			0x0400
-#define     INTR_STATUS3__UNSUP_CMD			0x0800
-#define     INTR_STATUS3__INT_ACT			0x1000
-#define     INTR_STATUS3__RST_COMP			0x2000
-#define     INTR_STATUS3__PIPE_CMD_ERR			0x4000
-#define     INTR_STATUS3__PAGE_XFER_INC			0x8000
-
-#define INTR_EN3					0x510
-#define     INTR_EN3__ECC_TRANSACTION_DONE		0x0001
-#define     INTR_EN3__ECC_ERR				0x0002
-#define     INTR_EN3__DMA_CMD_COMP			0x0004
-#define     INTR_EN3__TIME_OUT				0x0008
-#define     INTR_EN3__PROGRAM_FAIL			0x0010
-#define     INTR_EN3__ERASE_FAIL				0x0020
-#define     INTR_EN3__LOAD_COMP				0x0040
-#define     INTR_EN3__PROGRAM_COMP			0x0080
-#define     INTR_EN3__ERASE_COMP				0x0100
-#define     INTR_EN3__PIPE_CPYBCK_CMD_COMP		0x0200
-#define     INTR_EN3__LOCKED_BLK				0x0400
-#define     INTR_EN3__UNSUP_CMD				0x0800
-#define     INTR_EN3__INT_ACT				0x1000
-#define     INTR_EN3__RST_COMP				0x2000
-#define     INTR_EN3__PIPE_CMD_ERR			0x4000
-#define     INTR_EN3__PAGE_XFER_INC			0x8000
-
-#define PAGE_CNT3				0x520
-#define     PAGE_CNT3__VALUE				0x00ff
-
-#define ERR_PAGE_ADDR3				0x530
-#define     ERR_PAGE_ADDR3__VALUE			0xffff
-
-#define ERR_BLOCK_ADDR3			0x540
-#define     ERR_BLOCK_ADDR3__VALUE			0xffff
-
-#define DATA_INTR				0x550
-#define     DATA_INTR__WRITE_SPACE_AV			0x0001
-#define     DATA_INTR__READ_DATA_AV			0x0002
-
-#define DATA_INTR_EN				0x560
-#define     DATA_INTR_EN__WRITE_SPACE_AV		0x0001
-#define     DATA_INTR_EN__READ_DATA_AV			0x0002
-
-#define GPREG_0					0x570
-#define     GPREG_0__VALUE				0xffff
-
-#define GPREG_1					0x580
-#define     GPREG_1__VALUE				0xffff
-
-#define GPREG_2					0x590
-#define     GPREG_2__VALUE				0xffff
-
-#define GPREG_3					0x5a0
-#define     GPREG_3__VALUE				0xffff
-
-#define ECC_THRESHOLD				0x600
-#define     ECC_THRESHOLD__VALUE				0x03ff
-
-#define ECC_ERROR_BLOCK_ADDRESS		0x610
-#define     ECC_ERROR_BLOCK_ADDRESS__VALUE		0xffff
-
-#define ECC_ERROR_PAGE_ADDRESS			0x620
-#define     ECC_ERROR_PAGE_ADDRESS__VALUE		0x0fff
-#define     ECC_ERROR_PAGE_ADDRESS__BANK		0xf000
-
-#define ECC_ERROR_ADDRESS			0x630
-#define     ECC_ERROR_ADDRESS__OFFSET			0x0fff
-#define     ECC_ERROR_ADDRESS__SECTOR_NR		0xf000
-
-#define ERR_CORRECTION_INFO			0x640
-#define     ERR_CORRECTION_INFO__BYTEMASK		0x00ff
-#define     ERR_CORRECTION_INFO__DEVICE_NR		0x0f00
-#define     ERR_CORRECTION_INFO__ERROR_TYPE		0x4000
-#define     ERR_CORRECTION_INFO__LAST_ERR_INFO		0x8000
-
-#define DMA_ENABLE				0x700
-#define     DMA_ENABLE__FLAG				0x0001
-
-#define IGNORE_ECC_DONE				0x710
-#define     IGNORE_ECC_DONE__FLAG			0x0001
-
-#define DMA_INTR				0x720
-#define     DMA_INTR__TARGET_ERROR			0x0001
-#define     DMA_INTR__DESC_COMP_CHANNEL0		0x0002
-#define     DMA_INTR__DESC_COMP_CHANNEL1		0x0004
-#define     DMA_INTR__DESC_COMP_CHANNEL2		0x0008
-#define     DMA_INTR__DESC_COMP_CHANNEL3		0x0010
-#define     DMA_INTR__MEMCOPY_DESC_COMP		0x0020
-
-#define DMA_INTR_EN				0x730
-#define     DMA_INTR_EN__TARGET_ERROR			0x0001
-#define     DMA_INTR_EN__DESC_COMP_CHANNEL0		0x0002
-#define     DMA_INTR_EN__DESC_COMP_CHANNEL1		0x0004
-#define     DMA_INTR_EN__DESC_COMP_CHANNEL2		0x0008
-#define     DMA_INTR_EN__DESC_COMP_CHANNEL3		0x0010
-#define     DMA_INTR_EN__MEMCOPY_DESC_COMP		0x0020
-
-#define TARGET_ERR_ADDR_LO			0x740
-#define     TARGET_ERR_ADDR_LO__VALUE			0xffff
-
-#define TARGET_ERR_ADDR_HI			0x750
-#define     TARGET_ERR_ADDR_HI__VALUE			0xffff
-
-#define CHNL_ACTIVE				0x760
-#define     CHNL_ACTIVE__CHANNEL0			0x0001
-#define     CHNL_ACTIVE__CHANNEL1			0x0002
-#define     CHNL_ACTIVE__CHANNEL2			0x0004
-#define     CHNL_ACTIVE__CHANNEL3			0x0008
-
-#define ACTIVE_SRC_ID				0x800
-#define     ACTIVE_SRC_ID__VALUE				0x00ff
-
-#define PTN_INTR					0x810
-#define     PTN_INTR__CONFIG_ERROR			0x0001
-#define     PTN_INTR__ACCESS_ERROR_BANK0		0x0002
-#define     PTN_INTR__ACCESS_ERROR_BANK1		0x0004
-#define     PTN_INTR__ACCESS_ERROR_BANK2		0x0008
-#define     PTN_INTR__ACCESS_ERROR_BANK3		0x0010
-#define     PTN_INTR__REG_ACCESS_ERROR			0x0020
-
-#define PTN_INTR_EN				0x820
-#define     PTN_INTR_EN__CONFIG_ERROR			0x0001
-#define     PTN_INTR_EN__ACCESS_ERROR_BANK0		0x0002
-#define     PTN_INTR_EN__ACCESS_ERROR_BANK1		0x0004
-#define     PTN_INTR_EN__ACCESS_ERROR_BANK2		0x0008
-#define     PTN_INTR_EN__ACCESS_ERROR_BANK3		0x0010
-#define     PTN_INTR_EN__REG_ACCESS_ERROR		0x0020
-
-#define PERM_SRC_ID_0				0x830
-#define     PERM_SRC_ID_0__SRCID				0x00ff
-#define     PERM_SRC_ID_0__DIRECT_ACCESS_ACTIVE		0x0800
-#define     PERM_SRC_ID_0__WRITE_ACTIVE			0x2000
-#define     PERM_SRC_ID_0__READ_ACTIVE			0x4000
-#define     PERM_SRC_ID_0__PARTITION_VALID		0x8000
-
-#define MIN_BLK_ADDR_0				0x840
-#define     MIN_BLK_ADDR_0__VALUE			0xffff
-
-#define MAX_BLK_ADDR_0				0x850
-#define     MAX_BLK_ADDR_0__VALUE			0xffff
-
-#define MIN_MAX_BANK_0				0x860
-#define     MIN_MAX_BANK_0__MIN_VALUE			0x0003
-#define     MIN_MAX_BANK_0__MAX_VALUE			0x000c
-
-#define PERM_SRC_ID_1				0x870
-#define     PERM_SRC_ID_1__SRCID				0x00ff
-#define     PERM_SRC_ID_1__DIRECT_ACCESS_ACTIVE		0x0800
-#define     PERM_SRC_ID_1__WRITE_ACTIVE			0x2000
-#define     PERM_SRC_ID_1__READ_ACTIVE			0x4000
-#define     PERM_SRC_ID_1__PARTITION_VALID		0x8000
-
-#define MIN_BLK_ADDR_1				0x880
-#define     MIN_BLK_ADDR_1__VALUE			0xffff
-
-#define MAX_BLK_ADDR_1				0x890
-#define     MAX_BLK_ADDR_1__VALUE			0xffff
-
-#define MIN_MAX_BANK_1				0x8a0
-#define     MIN_MAX_BANK_1__MIN_VALUE			0x0003
-#define     MIN_MAX_BANK_1__MAX_VALUE			0x000c
-
-#define PERM_SRC_ID_2				0x8b0
-#define     PERM_SRC_ID_2__SRCID				0x00ff
-#define     PERM_SRC_ID_2__DIRECT_ACCESS_ACTIVE		0x0800
-#define     PERM_SRC_ID_2__WRITE_ACTIVE			0x2000
-#define     PERM_SRC_ID_2__READ_ACTIVE			0x4000
-#define     PERM_SRC_ID_2__PARTITION_VALID		0x8000
-
-#define MIN_BLK_ADDR_2				0x8c0
-#define     MIN_BLK_ADDR_2__VALUE			0xffff
-
-#define MAX_BLK_ADDR_2				0x8d0
-#define     MAX_BLK_ADDR_2__VALUE			0xffff
-
-#define MIN_MAX_BANK_2				0x8e0
-#define     MIN_MAX_BANK_2__MIN_VALUE			0x0003
-#define     MIN_MAX_BANK_2__MAX_VALUE			0x000c
-
-#define PERM_SRC_ID_3				0x8f0
-#define     PERM_SRC_ID_3__SRCID				0x00ff
-#define     PERM_SRC_ID_3__DIRECT_ACCESS_ACTIVE		0x0800
-#define     PERM_SRC_ID_3__WRITE_ACTIVE			0x2000
-#define     PERM_SRC_ID_3__READ_ACTIVE			0x4000
-#define     PERM_SRC_ID_3__PARTITION_VALID		0x8000
-
-#define MIN_BLK_ADDR_3				0x900
-#define     MIN_BLK_ADDR_3__VALUE			0xffff
-
-#define MAX_BLK_ADDR_3				0x910
-#define     MAX_BLK_ADDR_3__VALUE			0xffff
-
-#define MIN_MAX_BANK_3				0x920
-#define     MIN_MAX_BANK_3__MIN_VALUE			0x0003
-#define     MIN_MAX_BANK_3__MAX_VALUE			0x000c
-
-#define PERM_SRC_ID_4				0x930
-#define     PERM_SRC_ID_4__SRCID				0x00ff
-#define     PERM_SRC_ID_4__DIRECT_ACCESS_ACTIVE		0x0800
-#define     PERM_SRC_ID_4__WRITE_ACTIVE			0x2000
-#define     PERM_SRC_ID_4__READ_ACTIVE			0x4000
-#define     PERM_SRC_ID_4__PARTITION_VALID		0x8000
-
-#define MIN_BLK_ADDR_4				0x940
-#define     MIN_BLK_ADDR_4__VALUE			0xffff
-
-#define MAX_BLK_ADDR_4				0x950
-#define     MAX_BLK_ADDR_4__VALUE			0xffff
-
-#define MIN_MAX_BANK_4				0x960
-#define     MIN_MAX_BANK_4__MIN_VALUE			0x0003
-#define     MIN_MAX_BANK_4__MAX_VALUE			0x000c
-
-#define PERM_SRC_ID_5				0x970
-#define     PERM_SRC_ID_5__SRCID				0x00ff
-#define     PERM_SRC_ID_5__DIRECT_ACCESS_ACTIVE		0x0800
-#define     PERM_SRC_ID_5__WRITE_ACTIVE			0x2000
-#define     PERM_SRC_ID_5__READ_ACTIVE			0x4000
-#define     PERM_SRC_ID_5__PARTITION_VALID		0x8000
-
-#define MIN_BLK_ADDR_5				0x980
-#define     MIN_BLK_ADDR_5__VALUE			0xffff
-
-#define MAX_BLK_ADDR_5				0x990
-#define     MAX_BLK_ADDR_5__VALUE			0xffff
-
-#define MIN_MAX_BANK_5				0x9a0
-#define     MIN_MAX_BANK_5__MIN_VALUE			0x0003
-#define     MIN_MAX_BANK_5__MAX_VALUE			0x000c
-
-#define PERM_SRC_ID_6				0x9b0
-#define     PERM_SRC_ID_6__SRCID				0x00ff
-#define     PERM_SRC_ID_6__DIRECT_ACCESS_ACTIVE		0x0800
-#define     PERM_SRC_ID_6__WRITE_ACTIVE			0x2000
-#define     PERM_SRC_ID_6__READ_ACTIVE			0x4000
-#define     PERM_SRC_ID_6__PARTITION_VALID		0x8000
-
-#define MIN_BLK_ADDR_6				0x9c0
-#define     MIN_BLK_ADDR_6__VALUE			0xffff
-
-#define MAX_BLK_ADDR_6				0x9d0
-#define     MAX_BLK_ADDR_6__VALUE			0xffff
-
-#define MIN_MAX_BANK_6				0x9e0
-#define     MIN_MAX_BANK_6__MIN_VALUE			0x0003
-#define     MIN_MAX_BANK_6__MAX_VALUE			0x000c
-
-#define PERM_SRC_ID_7				0x9f0
-#define     PERM_SRC_ID_7__SRCID				0x00ff
-#define     PERM_SRC_ID_7__DIRECT_ACCESS_ACTIVE		0x0800
-#define     PERM_SRC_ID_7__WRITE_ACTIVE			0x2000
-#define     PERM_SRC_ID_7__READ_ACTIVE			0x4000
-#define     PERM_SRC_ID_7__PARTITION_VALID		0x8000
-
-#define MIN_BLK_ADDR_7				0xa00
-#define     MIN_BLK_ADDR_7__VALUE			0xffff
-
-#define MAX_BLK_ADDR_7				0xa10
-#define     MAX_BLK_ADDR_7__VALUE			0xffff
-
-#define MIN_MAX_BANK_7				0xa20
-#define     MIN_MAX_BANK_7__MIN_VALUE			0x0003
-#define     MIN_MAX_BANK_7__MAX_VALUE			0x000c
diff --git a/drivers/staging/spectra/spectraswconfig.h b/drivers/staging/spectra/spectraswconfig.h
deleted file mode 100644
index 17259469e955..000000000000
--- a/drivers/staging/spectra/spectraswconfig.h
+++ /dev/null
@@ -1,82 +0,0 @@
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#ifndef _SPECTRASWCONFIG_
-#define _SPECTRASWCONFIG_
-
-/* NAND driver version */
-#define GLOB_VERSION          "driver version 20100311"
-
-
-/***** Common Parameters *****/
-#define RETRY_TIMES                   3
-
-#define READ_BADBLOCK_INFO            1
-#define READBACK_VERIFY               0
-#define AUTO_FORMAT_FLASH             0
-
-/***** Cache Parameters *****/
-#define CACHE_ITEM_NUM            128
-#define BLK_NUM_FOR_L2_CACHE        16
-
-/***** Block Table Parameters *****/
-#define BLOCK_TABLE_INDEX             0
-
-/***** Wear Leveling Parameters *****/
-#define WEAR_LEVELING_GATE         0x10
-#define WEAR_LEVELING_BLOCK_NUM      10
-
-#define DEBUG_BNDRY             0
-
-/***** Product Feature Support *****/
-#define FLASH_EMU               defined(CONFIG_SPECTRA_EMU)
-#define FLASH_NAND              defined(CONFIG_SPECTRA_MRST_HW)
-#define FLASH_MTD               defined(CONFIG_SPECTRA_MTD)
-#define CMD_DMA                 defined(CONFIG_SPECTRA_MRST_HW_DMA)
-
-#define SPECTRA_PARTITION_ID    0
-
-/* Enable this macro if the number of flash blocks is larger than 16K. */
-#define SUPPORT_LARGE_BLOCKNUM  1
-
-/**** Block Table and Reserved Block Parameters *****/
-#define SPECTRA_START_BLOCK     3
-//#define NUM_FREE_BLOCKS_GATE    30
-#define NUM_FREE_BLOCKS_GATE    60
-
-/**** Hardware Parameters ****/
-#define GLOB_HWCTL_REG_BASE     0xFFA40000
-#define GLOB_HWCTL_REG_SIZE     4096
-
-#define GLOB_HWCTL_MEM_BASE     0xFFA48000
-#define GLOB_HWCTL_MEM_SIZE     4096
-
-/* KBV - Updated to LNW scratch register address */
-#define SCRATCH_REG_ADDR    0xFF108018
-#define SCRATCH_REG_SIZE    64
-
-#define GLOB_HWCTL_DEFAULT_BLKS    2048
-
-#define SUPPORT_15BITECC        1
-#define SUPPORT_8BITECC         1
-
-#define ONFI_BLOOM_TIME         0
-#define MODE5_WORKAROUND        1
-
-#endif /*_SPECTRASWCONFIG_*/
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/staging/usbip/stub_dev.c
index 55c0b510889c..03420e25d9c6 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/staging/usbip/stub_dev.c
@@ -109,11 +109,6 @@ static ssize_t store_sockfd(struct device *dev, struct device_attribute *attr,
 			spin_unlock(&sdev->ud.lock);
 			return -EINVAL;
 		}
-#if 0
-		setnodelay(socket);
-		setkeepalive(socket);
-		setreuse(socket);
-#endif
 		sdev->ud.tcp_socket = socket;
 
 		spin_unlock(&sdev->ud.lock);
diff --git a/drivers/staging/usbip/stub_rx.c b/drivers/staging/usbip/stub_rx.c
index 6b4e3e182de8..27ac363d1cfa 100644
--- a/drivers/staging/usbip/stub_rx.c
+++ b/drivers/staging/usbip/stub_rx.c
@@ -564,7 +564,7 @@ static void stub_rx_pdu(struct usbip_device *ud)
 	memset(&pdu, 0, sizeof(pdu));
 
 	/* 1. receive a pdu header */
-	ret = usbip_xmit(0, ud->tcp_socket, (char *) &pdu, sizeof(pdu), 0);
+	ret = usbip_recv(ud->tcp_socket, &pdu, sizeof(pdu));
 	if (ret != sizeof(pdu)) {
 		dev_err(dev, "recv a header, %d\n", ret);
 		usbip_event_add(ud, SDEV_EVENT_ERROR_TCP);
diff --git a/drivers/staging/usbip/usbip_common.c b/drivers/staging/usbip/usbip_common.c
index 3b7a847f4657..d93e7f1f7973 100644
--- a/drivers/staging/usbip/usbip_common.c
+++ b/drivers/staging/usbip/usbip_common.c
@@ -334,9 +334,8 @@ void usbip_dump_header(struct usbip_header *pdu)
 }
 EXPORT_SYMBOL_GPL(usbip_dump_header);
 
-/* Send/receive messages over TCP/IP. I refer drivers/block/nbd.c */
-int usbip_xmit(int send, struct socket *sock, char *buf, int size,
-	       int msg_flags)
+/* Receive data over TCP/IP. */
+int usbip_recv(struct socket *sock, void *buf, int size)
 {
 	int result;
 	struct msghdr msg;
@@ -355,19 +354,6 @@ int usbip_xmit(int send, struct socket *sock, char *buf, int size,
 		return -EINVAL;
 	}
 
-	if (usbip_dbg_flag_xmit) {
-		if (send) {
-			if (!in_interrupt())
-				pr_debug("%-10s:", current->comm);
-			else
-				pr_debug("interrupt  :");
-
-			pr_debug("sending... , sock %p, buf %p, size %d, "
-				 "msg_flags %d\n", sock, buf, size, msg_flags);
-			usbip_dump_buffer(buf, size);
-		}
-	}
-
 	do {
 		sock->sk->sk_allocation = GFP_NOIO;
 		iov.iov_base    = buf;
@@ -377,42 +363,30 @@ int usbip_xmit(int send, struct socket *sock, char *buf, int size,
 		msg.msg_control = NULL;
 		msg.msg_controllen = 0;
 		msg.msg_namelen    = 0;
-		msg.msg_flags      = msg_flags | MSG_NOSIGNAL;
-
-		if (send)
-			result = kernel_sendmsg(sock, &msg, &iov, 1, size);
-		else
-			result = kernel_recvmsg(sock, &msg, &iov, 1, size,
-						MSG_WAITALL);
+		msg.msg_flags      = MSG_NOSIGNAL;
 
+		result = kernel_recvmsg(sock, &msg, &iov, 1, size, MSG_WAITALL);
 		if (result <= 0) {
-			pr_debug("%s sock %p buf %p size %u ret %d total %d\n",
-				 send ? "send" : "receive", sock, buf, size,
-				 result, total);
+			pr_debug("receive sock %p buf %p size %u ret %d total %d\n",
+				 sock, buf, size, result, total);
 			goto err;
 		}
 
 		size -= result;
 		buf += result;
 		total += result;
-
 	} while (size > 0);
 
 	if (usbip_dbg_flag_xmit) {
-		if (!send) {
-			if (!in_interrupt())
-				pr_debug("%-10s:", current->comm);
-			else
-				pr_debug("interrupt  :");
-
-			pr_debug("receiving....\n");
-			usbip_dump_buffer(bp, osize);
-			pr_debug("received, osize %d ret %d size %d total %d\n",
-				 osize, result, size, total);
-		}
+		if (!in_interrupt())
+			pr_debug("%-10s:", current->comm);
+		else
+			pr_debug("interrupt  :");
 
-		if (send)
-			pr_debug("send, total %d\n", total);
+		pr_debug("receiving....\n");
+		usbip_dump_buffer(bp, osize);
+		pr_debug("received, osize %d ret %d size %d total %d\n",
+			osize, result, size, total);
 	}
 
 	return total;
@@ -420,7 +394,7 @@ int usbip_xmit(int send, struct socket *sock, char *buf, int size,
 err:
 	return result;
 }
-EXPORT_SYMBOL_GPL(usbip_xmit);
+EXPORT_SYMBOL_GPL(usbip_recv);
 
 struct socket *sockfd_to_socket(unsigned int sockfd)
 {
@@ -712,7 +686,7 @@ int usbip_recv_iso(struct usbip_device *ud, struct urb *urb)
 	if (!buff)
 		return -ENOMEM;
 
-	ret = usbip_xmit(0, ud->tcp_socket, buff, size, 0);
+	ret = usbip_recv(ud->tcp_socket, buff, size);
 	if (ret != size) {
 		dev_err(&urb->dev->dev, "recv iso_frame_descriptor, %d\n",
 			ret);
@@ -823,8 +797,7 @@ int usbip_recv_xbuff(struct usbip_device *ud, struct urb *urb)
 	if (!(size > 0))
 		return 0;
 
-	ret = usbip_xmit(0, ud->tcp_socket, (char *)urb->transfer_buffer,
-			 size, 0);
+	ret = usbip_recv(ud->tcp_socket, urb->transfer_buffer, size);
 	if (ret != size) {
 		dev_err(&urb->dev->dev, "recv xbuf, %d\n", ret);
 		if (ud->side == USBIP_STUB) {
diff --git a/drivers/staging/usbip/usbip_common.h b/drivers/staging/usbip/usbip_common.h
index be216175ae87..b8f8c48b8a72 100644
--- a/drivers/staging/usbip/usbip_common.h
+++ b/drivers/staging/usbip/usbip_common.h
@@ -292,21 +292,11 @@ struct usbip_device {
 	} eh_ops;
 };
 
-#if 0
-int usbip_sendmsg(struct socket *, struct msghdr *, int);
-int set_sockaddr(struct socket *socket, struct sockaddr_storage *ss);
-int setnodelay(struct socket *);
-int setquickack(struct socket *);
-int setkeepalive(struct socket *socket);
-void setreuse(struct socket *);
-#endif
-
 /* usbip_common.c */
 void usbip_dump_urb(struct urb *purb);
 void usbip_dump_header(struct usbip_header *pdu);
 
-int usbip_xmit(int send, struct socket *sock, char *buf, int size,
-	       int msg_flags);
+int usbip_recv(struct socket *sock, void *buf, int size);
 struct socket *sockfd_to_socket(unsigned int sockfd);
 
 void usbip_pack_pdu(struct usbip_header *pdu, struct urb *urb, int cmd,
@@ -337,9 +327,4 @@ static inline int interface_to_devnum(struct usb_interface *interface)
 	return udev->devnum;
 }
 
-static inline int interface_to_infnum(struct usb_interface *interface)
-{
-	return interface->cur_altsetting->desc.bInterfaceNumber;
-}
-
 #endif /* __USBIP_COMMON_H */
diff --git a/drivers/staging/usbip/vhci_rx.c b/drivers/staging/usbip/vhci_rx.c
index 3872b8cccdcf..3f511b47563d 100644
--- a/drivers/staging/usbip/vhci_rx.c
+++ b/drivers/staging/usbip/vhci_rx.c
@@ -206,7 +206,7 @@ static void vhci_rx_pdu(struct usbip_device *ud)
 	memset(&pdu, 0, sizeof(pdu));
 
 	/* 1. receive a pdu header */
-	ret = usbip_xmit(0, ud->tcp_socket, (char *) &pdu, sizeof(pdu), 0);
+	ret = usbip_recv(ud->tcp_socket, &pdu, sizeof(pdu));
 	if (ret < 0) {
 		if (ret == -ECONNRESET)
 			pr_info("connection reset by peer\n");
diff --git a/drivers/staging/vme/TODO b/drivers/staging/vme/TODO
index 82c222b4a146..79f00333e7ef 100644
--- a/drivers/staging/vme/TODO
+++ b/drivers/staging/vme/TODO
@@ -1,70 +1,5 @@
 				TODO
 				====
 
-API
-===
-
-Master window broadcast select mask
------------------------------------
-
-API currently provides no method to set or get Broadcast Select mask. Suggest
-somthing like:
-
-	int vme_master_bmsk_set (struct vme_resource *res, int mask);
-	int vme_master_bmsk_get (struct vme_resource *res, int *mask);
-
-
-Interrupt Generation
---------------------
-
-Add optional timeout when waiting for an IACK.
-
-
-CR/CSR Buffer
--------------
-
-The VME API provides no functions to access the buffer mapped into the CR/CSR
-space.
-
-
-Mailboxes
----------
-
-Whilst not part of the VME specification, they are provided by a number of
-chips. They are currently not supported at all by the API.
-
-
-Core
-====
-
-- Improve generic sanity checks (Such as does an offset and size fit within a
-  window and parameter checking).
-
-Bridge Support
-==============
-
-Tempe (tsi148)
---------------
-
-- 2eSST Broadcast mode.
-- Mailboxes unsupported.
-- Improve error detection.
-- Control of prefetch size, threshold.
-- Arbiter control
-- Requestor control
-
-Universe II (ca91c142)
-----------------------
-
-- Mailboxes unsupported.
-- Error Detection.
-- Control of prefetch size, threshold.
-- Arbiter control
-- Requestor control
-- Slot detection
-
-Universe I (ca91x042)
----------------------
-
-Currently completely unsupported.
+- Add one or more device drivers which use the VME framework.
 
diff --git a/drivers/staging/vme/bridges/vme_ca91cx42.c b/drivers/staging/vme/bridges/vme_ca91cx42.c
index 0e4feac138eb..515b8b8e32a8 100644
--- a/drivers/staging/vme/bridges/vme_ca91cx42.c
+++ b/drivers/staging/vme/bridges/vme_ca91cx42.c
@@ -338,7 +338,7 @@ static int ca91cx42_irq_generate(struct vme_bridge *ca91cx42_bridge, int level,
 
 static int ca91cx42_slave_set(struct vme_slave_resource *image, int enabled,
 	unsigned long long vme_base, unsigned long long size,
-	dma_addr_t pci_base, vme_address_t aspace, vme_cycle_t cycle)
+	dma_addr_t pci_base, u32 aspace, u32 cycle)
 {
 	unsigned int i, addr = 0, granularity;
 	unsigned int temp_ctl = 0;
@@ -444,7 +444,7 @@ static int ca91cx42_slave_set(struct vme_slave_resource *image, int enabled,
 
 static int ca91cx42_slave_get(struct vme_slave_resource *image, int *enabled,
 	unsigned long long *vme_base, unsigned long long *size,
-	dma_addr_t *pci_base, vme_address_t *aspace, vme_cycle_t *cycle)
+	dma_addr_t *pci_base, u32 *aspace, u32 *cycle)
 {
 	unsigned int i, granularity = 0, ctl = 0;
 	unsigned long long vme_bound, pci_offset;
@@ -595,8 +595,8 @@ static void ca91cx42_free_resource(struct vme_master_resource *image)
 
 
 static int ca91cx42_master_set(struct vme_master_resource *image, int enabled,
-	unsigned long long vme_base, unsigned long long size,
-	vme_address_t aspace, vme_cycle_t cycle, vme_width_t dwidth)
+	unsigned long long vme_base, unsigned long long size, u32 aspace,
+	u32 cycle, u32 dwidth)
 {
 	int retval = 0;
 	unsigned int i, granularity = 0;
@@ -753,7 +753,7 @@ err_window:
 
 static int __ca91cx42_master_get(struct vme_master_resource *image,
 	int *enabled, unsigned long long *vme_base, unsigned long long *size,
-	vme_address_t *aspace, vme_cycle_t *cycle, vme_width_t *dwidth)
+	u32 *aspace, u32 *cycle, u32 *dwidth)
 {
 	unsigned int i, ctl;
 	unsigned long long pci_base, pci_bound, vme_offset;
@@ -839,8 +839,8 @@ static int __ca91cx42_master_get(struct vme_master_resource *image,
 }
 
 static int ca91cx42_master_get(struct vme_master_resource *image, int *enabled,
-	unsigned long long *vme_base, unsigned long long *size,
-	vme_address_t *aspace, vme_cycle_t *cycle, vme_width_t *dwidth)
+	unsigned long long *vme_base, unsigned long long *size, u32 *aspace,
+	u32 *cycle, u32 *dwidth)
 {
 	int retval;
 
@@ -876,13 +876,13 @@ static ssize_t ca91cx42_master_read(struct vme_master_resource *image,
 	 * maximal configured data cycle is used and splits it
 	 * automatically for non-aligned addresses.
 	 */
-	if ((int)addr & 0x1) {
+	if ((uintptr_t)addr & 0x1) {
 		*(u8 *)buf = ioread8(addr);
 		done += 1;
 		if (done == count)
 			goto out;
 	}
-	if ((int)addr & 0x2) {
+	if ((uintptr_t)addr & 0x2) {
 		if ((count - done) < 2) {
 			*(u8 *)(buf + done) = ioread8(addr + done);
 			done += 1;
@@ -930,13 +930,13 @@ static ssize_t ca91cx42_master_write(struct vme_master_resource *image,
 	/* Here we apply for the same strategy we do in master_read
 	 * function in order to assure D16 cycle when required.
 	 */
-	if ((int)addr & 0x1) {
+	if ((uintptr_t)addr & 0x1) {
 		iowrite8(*(u8 *)buf, addr);
 		done += 1;
 		if (done == count)
 			goto out;
 	}
-	if ((int)addr & 0x2) {
+	if ((uintptr_t)addr & 0x2) {
 		if ((count - done) < 2) {
 			iowrite8(*(u8 *)(buf + done), addr + done);
 			done += 1;
@@ -973,7 +973,8 @@ static unsigned int ca91cx42_master_rmw(struct vme_master_resource *image,
 	unsigned int mask, unsigned int compare, unsigned int swap,
 	loff_t offset)
 {
-	u32 pci_addr, result;
+	u32 result;
+	uintptr_t pci_addr;
 	int i;
 	struct ca91cx42_driver *bridge;
 	struct device *dev;
@@ -990,7 +991,7 @@ static unsigned int ca91cx42_master_rmw(struct vme_master_resource *image,
 	/* Lock image */
 	spin_lock(&image->lock);
 
-	pci_addr = (u32)image->kern_base + offset;
+	pci_addr = (uintptr_t)image->kern_base + offset;
 
 	/* Address must be 4-byte aligned */
 	if (pci_addr & 0x3) {
@@ -1291,7 +1292,7 @@ static int ca91cx42_dma_list_empty(struct vme_dma_list *list)
  * callback is attached and disabled when the last callback is removed.
  */
 static int ca91cx42_lm_set(struct vme_lm_resource *lm,
-	unsigned long long lm_base, vme_address_t aspace, vme_cycle_t cycle)
+	unsigned long long lm_base, u32 aspace, u32 cycle)
 {
 	u32 temp_base, lm_ctl = 0;
 	int i;
@@ -1359,7 +1360,7 @@ static int ca91cx42_lm_set(struct vme_lm_resource *lm,
  * or disabled.
  */
 static int ca91cx42_lm_get(struct vme_lm_resource *lm,
-	unsigned long long *lm_base, vme_address_t *aspace, vme_cycle_t *cycle)
+	unsigned long long *lm_base, u32 *aspace, u32 *cycle)
 {
 	u32 lm_ctl, enabled = 0;
 	struct ca91cx42_driver *bridge;
diff --git a/drivers/staging/vme/bridges/vme_tsi148.c b/drivers/staging/vme/bridges/vme_tsi148.c
index 6c1167c2bea9..08a449b4abf9 100644
--- a/drivers/staging/vme/bridges/vme_tsi148.c
+++ b/drivers/staging/vme/bridges/vme_tsi148.c
@@ -483,7 +483,7 @@ static int tsi148_irq_generate(struct vme_bridge *tsi148_bridge, int level,
  * Find the first error in this address range
  */
 static struct vme_bus_error *tsi148_find_error(struct vme_bridge *tsi148_bridge,
-	vme_address_t aspace, unsigned long long address, size_t count)
+	u32 aspace, unsigned long long address, size_t count)
 {
 	struct list_head *err_pos;
 	struct vme_bus_error *vme_err, *valid = NULL;
@@ -517,7 +517,7 @@ static struct vme_bus_error *tsi148_find_error(struct vme_bridge *tsi148_bridge,
  * Clear errors in the provided address range.
  */
 static void tsi148_clear_errors(struct vme_bridge *tsi148_bridge,
-	vme_address_t aspace, unsigned long long address, size_t count)
+	u32 aspace, unsigned long long address, size_t count)
 {
 	struct list_head *err_pos, *temp;
 	struct vme_bus_error *vme_err;
@@ -551,7 +551,7 @@ static void tsi148_clear_errors(struct vme_bridge *tsi148_bridge,
  */
 static int tsi148_slave_set(struct vme_slave_resource *image, int enabled,
 	unsigned long long vme_base, unsigned long long size,
-	dma_addr_t pci_base, vme_address_t aspace, vme_cycle_t cycle)
+	dma_addr_t pci_base, u32 aspace, u32 cycle)
 {
 	unsigned int i, addr = 0, granularity = 0;
 	unsigned int temp_ctl = 0;
@@ -701,7 +701,7 @@ static int tsi148_slave_set(struct vme_slave_resource *image, int enabled,
  */
 static int tsi148_slave_get(struct vme_slave_resource *image, int *enabled,
 	unsigned long long *vme_base, unsigned long long *size,
-	dma_addr_t *pci_base, vme_address_t *aspace, vme_cycle_t *cycle)
+	dma_addr_t *pci_base, u32 *aspace, u32 *cycle)
 {
 	unsigned int i, granularity = 0, ctl = 0;
 	unsigned int vme_base_low, vme_base_high;
@@ -893,8 +893,8 @@ static void tsi148_free_resource(struct vme_master_resource *image)
  * Set the attributes of an outbound window.
  */
 static int tsi148_master_set(struct vme_master_resource *image, int enabled,
-	unsigned long long vme_base, unsigned long long size,
-	vme_address_t aspace, vme_cycle_t cycle, vme_width_t dwidth)
+	unsigned long long vme_base, unsigned long long size, u32 aspace,
+	u32 cycle, u32 dwidth)
 {
 	int retval = 0;
 	unsigned int i;
@@ -1129,8 +1129,8 @@ err_window:
  * XXX Not parsing prefetch information.
  */
 static int __tsi148_master_get(struct vme_master_resource *image, int *enabled,
-	unsigned long long *vme_base, unsigned long long *size,
-	vme_address_t *aspace, vme_cycle_t *cycle, vme_width_t *dwidth)
+	unsigned long long *vme_base, unsigned long long *size, u32 *aspace,
+	u32 *cycle, u32 *dwidth)
 {
 	unsigned int i, ctl;
 	unsigned int pci_base_low, pci_base_high;
@@ -1239,8 +1239,8 @@ static int __tsi148_master_get(struct vme_master_resource *image, int *enabled,
 
 
 static int tsi148_master_get(struct vme_master_resource *image, int *enabled,
-	unsigned long long *vme_base, unsigned long long *size,
-	vme_address_t *aspace, vme_cycle_t *cycle, vme_width_t *dwidth)
+	unsigned long long *vme_base, unsigned long long *size, u32 *aspace,
+	u32 *cycle, u32 *dwidth)
 {
 	int retval;
 
@@ -1259,9 +1259,7 @@ static ssize_t tsi148_master_read(struct vme_master_resource *image, void *buf,
 {
 	int retval, enabled;
 	unsigned long long vme_base, size;
-	vme_address_t aspace;
-	vme_cycle_t cycle;
-	vme_width_t dwidth;
+	u32 aspace, cycle, dwidth;
 	struct vme_bus_error *vme_err = NULL;
 	struct vme_bridge *tsi148_bridge;
 
@@ -1301,9 +1299,7 @@ static ssize_t tsi148_master_write(struct vme_master_resource *image, void *buf,
 {
 	int retval = 0, enabled;
 	unsigned long long vme_base, size;
-	vme_address_t aspace;
-	vme_cycle_t cycle;
-	vme_width_t dwidth;
+	u32 aspace, cycle, dwidth;
 
 	struct vme_bus_error *vme_err = NULL;
 	struct vme_bridge *tsi148_bridge;
@@ -1420,7 +1416,7 @@ static unsigned int tsi148_master_rmw(struct vme_master_resource *image,
 }
 
 static int tsi148_dma_set_vme_src_attributes(struct device *dev, u32 *attr,
-	vme_address_t aspace, vme_cycle_t cycle, vme_width_t dwidth)
+	u32 aspace, u32 cycle, u32 dwidth)
 {
 	/* Setup 2eSST speeds */
 	switch (cycle & (VME_2eSST160 | VME_2eSST267 | VME_2eSST320)) {
@@ -1514,7 +1510,7 @@ static int tsi148_dma_set_vme_src_attributes(struct device *dev, u32 *attr,
 }
 
 static int tsi148_dma_set_vme_dest_attributes(struct device *dev, u32 *attr,
-	vme_address_t aspace, vme_cycle_t cycle, vme_width_t dwidth)
+	u32 aspace, u32 cycle, u32 dwidth)
 {
 	/* Setup 2eSST speeds */
 	switch (cycle & (VME_2eSST160 | VME_2eSST267 | VME_2eSST320)) {
@@ -1886,7 +1882,7 @@ static int tsi148_dma_list_empty(struct vme_dma_list *list)
  * callback is attached and disabled when the last callback is removed.
  */
 static int tsi148_lm_set(struct vme_lm_resource *lm, unsigned long long lm_base,
-	vme_address_t aspace, vme_cycle_t cycle)
+	u32 aspace, u32 cycle)
 {
 	u32 lm_base_high, lm_base_low, lm_ctl = 0;
 	int i;
@@ -1953,7 +1949,7 @@ static int tsi148_lm_set(struct vme_lm_resource *lm, unsigned long long lm_base,
  * or disabled.
  */
 static int tsi148_lm_get(struct vme_lm_resource *lm,
-	unsigned long long *lm_base, vme_address_t *aspace, vme_cycle_t *cycle)
+	unsigned long long *lm_base, u32 *aspace, u32 *cycle)
 {
 	u32 lm_base_high, lm_base_low, lm_ctl, enabled = 0;
 	struct tsi148_driver *bridge;
diff --git a/drivers/staging/vme/devices/Kconfig b/drivers/staging/vme/devices/Kconfig
index ca5ba89e2d8c..55ec30cb1fa2 100644
--- a/drivers/staging/vme/devices/Kconfig
+++ b/drivers/staging/vme/devices/Kconfig
@@ -6,3 +6,16 @@ config VME_USER
 	  If you say Y here you want to be able to access a limited number of
 	  VME windows in a manner at least semi-compatible with the interface
 	  provided with the original driver at http://vmelinux.org/.
+
+config VME_PIO2
+	tristate "GE PIO2 VME"
+	depends on GPIOLIB
+	help
+	  Say Y here to include support for the GE PIO2. The PIO2 is a 6U VME
+	  slave card, implementing 32 solid-state relay switched IO lines, in
+	  4 groups of 8. Each bank of IO lines is built to function as input,
+	  output or both depending on the variant of the card.
+
+	  To compile this driver as a module, choose M here. The module will
+	  be called vme_pio2. If unsure, say N.
+
diff --git a/drivers/staging/vme/devices/Makefile b/drivers/staging/vme/devices/Makefile
index 459742a75283..172512cb5dbf 100644
--- a/drivers/staging/vme/devices/Makefile
+++ b/drivers/staging/vme/devices/Makefile
@@ -3,3 +3,6 @@
 #
 
 obj-$(CONFIG_VME_USER)		+= vme_user.o
+
+vme_pio2-objs	:= vme_pio2_cntr.o vme_pio2_gpio.o vme_pio2_core.o
+obj-$(CONFIG_VME_PIO2)         += vme_pio2.o
diff --git a/drivers/staging/vme/devices/vme_pio2.h b/drivers/staging/vme/devices/vme_pio2.h
new file mode 100644
index 000000000000..3c5931364535
--- /dev/null
+++ b/drivers/staging/vme/devices/vme_pio2.h
@@ -0,0 +1,249 @@
+#ifndef _VME_PIO2_H_
+#define _VME_PIO2_H_
+
+#define PIO2_CARDS_MAX			32
+
+#define PIO2_VARIANT_LENGTH		5
+
+#define PIO2_NUM_CHANNELS		32
+#define PIO2_NUM_IRQS			11
+#define PIO2_NUM_CNTRS			6
+
+#define PIO2_REGS_SIZE			0x40
+
+#define PIO2_REGS_DATA0			0x0
+#define PIO2_REGS_DATA1			0x1
+#define PIO2_REGS_DATA2			0x2
+#define PIO2_REGS_DATA3			0x3
+
+static const int PIO2_REGS_DATA[4] = { PIO2_REGS_DATA0, PIO2_REGS_DATA1,
+					PIO2_REGS_DATA2, PIO2_REGS_DATA3 };
+
+#define PIO2_REGS_INT_STAT0		0x8
+#define PIO2_REGS_INT_STAT1		0x9
+#define PIO2_REGS_INT_STAT2		0xa
+#define PIO2_REGS_INT_STAT3		0xb
+
+static const int PIO2_REGS_INT_STAT[4] = { PIO2_REGS_INT_STAT0,
+					PIO2_REGS_INT_STAT1,
+					PIO2_REGS_INT_STAT2,
+					PIO2_REGS_INT_STAT3 };
+
+#define PIO2_REGS_INT_STAT_CNTR		0xc
+#define PIO2_REGS_INT_MASK0		0x10
+#define PIO2_REGS_INT_MASK1		0x11
+#define PIO2_REGS_INT_MASK2		0x12
+#define PIO2_REGS_INT_MASK3		0x13
+#define PIO2_REGS_INT_MASK4		0x14
+#define PIO2_REGS_INT_MASK5		0x15
+#define PIO2_REGS_INT_MASK6		0x16
+#define PIO2_REGS_INT_MASK7		0x17
+
+static const int PIO2_REGS_INT_MASK[8] = { PIO2_REGS_INT_MASK0,
+					PIO2_REGS_INT_MASK1,
+					PIO2_REGS_INT_MASK2,
+					PIO2_REGS_INT_MASK3,
+					PIO2_REGS_INT_MASK4,
+					PIO2_REGS_INT_MASK5,
+					PIO2_REGS_INT_MASK6,
+					PIO2_REGS_INT_MASK7 };
+
+
+
+#define PIO2_REGS_CTRL			0x18
+#define PIO2_REGS_VME_VECTOR		0x19
+#define PIO2_REGS_CNTR0			0x20
+#define PIO2_REGS_CNTR1			0x22
+#define PIO2_REGS_CNTR2			0x24
+#define PIO2_REGS_CTRL_WRD0		0x26
+#define PIO2_REGS_CNTR3			0x28
+#define PIO2_REGS_CNTR4			0x2a
+#define PIO2_REGS_CNTR5			0x2c
+#define PIO2_REGS_CTRL_WRD1		0x2e
+
+#define PIO2_REGS_ID			0x30
+
+
+/* PIO2_REGS_DATAx (0x0 - 0x3) */
+
+static const int PIO2_CHANNEL_BANK[32] = { 0, 0, 0, 0, 0, 0, 0, 0,
+					1, 1, 1, 1, 1, 1, 1, 1,
+					2, 2, 2, 2, 2, 2, 2, 2,
+					3, 3, 3, 3, 3, 3, 3, 3 };
+
+#define PIO2_CHANNEL0_BIT		(1 << 0)
+#define PIO2_CHANNEL1_BIT		(1 << 1)
+#define PIO2_CHANNEL2_BIT		(1 << 2)
+#define PIO2_CHANNEL3_BIT		(1 << 3)
+#define PIO2_CHANNEL4_BIT		(1 << 4)
+#define PIO2_CHANNEL5_BIT		(1 << 5)
+#define PIO2_CHANNEL6_BIT		(1 << 6)
+#define PIO2_CHANNEL7_BIT		(1 << 7)
+#define PIO2_CHANNEL8_BIT		(1 << 0)
+#define PIO2_CHANNEL9_BIT		(1 << 1)
+#define PIO2_CHANNEL10_BIT		(1 << 2)
+#define PIO2_CHANNEL11_BIT		(1 << 3)
+#define PIO2_CHANNEL12_BIT		(1 << 4)
+#define PIO2_CHANNEL13_BIT		(1 << 5)
+#define PIO2_CHANNEL14_BIT		(1 << 6)
+#define PIO2_CHANNEL15_BIT		(1 << 7)
+#define PIO2_CHANNEL16_BIT		(1 << 0)
+#define PIO2_CHANNEL17_BIT		(1 << 1)
+#define PIO2_CHANNEL18_BIT		(1 << 2)
+#define PIO2_CHANNEL19_BIT		(1 << 3)
+#define PIO2_CHANNEL20_BIT		(1 << 4)
+#define PIO2_CHANNEL21_BIT		(1 << 5)
+#define PIO2_CHANNEL22_BIT		(1 << 6)
+#define PIO2_CHANNEL23_BIT		(1 << 7)
+#define PIO2_CHANNEL24_BIT		(1 << 0)
+#define PIO2_CHANNEL25_BIT		(1 << 1)
+#define PIO2_CHANNEL26_BIT		(1 << 2)
+#define PIO2_CHANNEL27_BIT		(1 << 3)
+#define PIO2_CHANNEL28_BIT		(1 << 4)
+#define PIO2_CHANNEL29_BIT		(1 << 5)
+#define PIO2_CHANNEL30_BIT		(1 << 6)
+#define PIO2_CHANNEL31_BIT		(1 << 7)
+
+static const int PIO2_CHANNEL_BIT[32] = { PIO2_CHANNEL0_BIT, PIO2_CHANNEL1_BIT,
+					PIO2_CHANNEL2_BIT, PIO2_CHANNEL3_BIT,
+					PIO2_CHANNEL4_BIT, PIO2_CHANNEL5_BIT,
+					PIO2_CHANNEL6_BIT, PIO2_CHANNEL7_BIT,
+					PIO2_CHANNEL8_BIT, PIO2_CHANNEL9_BIT,
+					PIO2_CHANNEL10_BIT, PIO2_CHANNEL11_BIT,
+					PIO2_CHANNEL12_BIT, PIO2_CHANNEL13_BIT,
+					PIO2_CHANNEL14_BIT, PIO2_CHANNEL15_BIT,
+					PIO2_CHANNEL16_BIT, PIO2_CHANNEL17_BIT,
+					PIO2_CHANNEL18_BIT, PIO2_CHANNEL19_BIT,
+					PIO2_CHANNEL20_BIT, PIO2_CHANNEL21_BIT,
+					PIO2_CHANNEL22_BIT, PIO2_CHANNEL23_BIT,
+					PIO2_CHANNEL24_BIT, PIO2_CHANNEL25_BIT,
+					PIO2_CHANNEL26_BIT, PIO2_CHANNEL27_BIT,
+					PIO2_CHANNEL28_BIT, PIO2_CHANNEL29_BIT,
+					PIO2_CHANNEL30_BIT, PIO2_CHANNEL31_BIT
+					};
+
+/* PIO2_REGS_INT_STAT_CNTR (0xc) */
+#define PIO2_COUNTER0			(1 << 0)
+#define PIO2_COUNTER1			(1 << 1)
+#define PIO2_COUNTER2			(1 << 2)
+#define PIO2_COUNTER3			(1 << 3)
+#define PIO2_COUNTER4			(1 << 4)
+#define PIO2_COUNTER5			(1 << 5)
+
+static const int PIO2_COUNTER[6] = { PIO2_COUNTER0, PIO2_COUNTER1,
+					PIO2_COUNTER2, PIO2_COUNTER3,
+					PIO2_COUNTER4, PIO2_COUNTER5 };
+
+/* PIO2_REGS_CTRL (0x18) */
+#define PIO2_VME_INT_MASK		0x7
+#define PIO2_LED			(1 << 6)
+#define PIO2_LOOP			(1 << 7)
+
+/* PIO2_REGS_VME_VECTOR (0x19) */
+#define PIO2_VME_VECTOR_SPUR		0x0
+#define PIO2_VME_VECTOR_BANK0		0x1
+#define PIO2_VME_VECTOR_BANK1		0x2
+#define PIO2_VME_VECTOR_BANK2		0x3
+#define PIO2_VME_VECTOR_BANK3		0x4
+#define PIO2_VME_VECTOR_CNTR0		0x5
+#define PIO2_VME_VECTOR_CNTR1		0x6
+#define PIO2_VME_VECTOR_CNTR2		0x7
+#define PIO2_VME_VECTOR_CNTR3		0x8
+#define PIO2_VME_VECTOR_CNTR4		0x9
+#define PIO2_VME_VECTOR_CNTR5		0xa
+
+#define PIO2_VME_VECTOR_MASK		0xf0
+
+static const int PIO2_VECTOR_BANK[4] = { PIO2_VME_VECTOR_BANK0,
+					PIO2_VME_VECTOR_BANK1,
+					PIO2_VME_VECTOR_BANK2,
+					PIO2_VME_VECTOR_BANK3 };
+
+static const int PIO2_VECTOR_CNTR[6] = { PIO2_VME_VECTOR_CNTR0,
+					PIO2_VME_VECTOR_CNTR1,
+					PIO2_VME_VECTOR_CNTR2,
+					PIO2_VME_VECTOR_CNTR3,
+					PIO2_VME_VECTOR_CNTR4,
+					PIO2_VME_VECTOR_CNTR5 };
+
+/* PIO2_REGS_CNTRx (0x20 - 0x24 & 0x28 - 0x2c) */
+
+static const int PIO2_CNTR_DATA[6] = { PIO2_REGS_CNTR0, PIO2_REGS_CNTR1,
+					PIO2_REGS_CNTR2, PIO2_REGS_CNTR3,
+					PIO2_REGS_CNTR4, PIO2_REGS_CNTR5 };
+
+/* PIO2_REGS_CTRL_WRDx (0x26 & 0x2e) */
+
+static const int PIO2_CNTR_CTRL[6] = { PIO2_REGS_CTRL_WRD0,
+					PIO2_REGS_CTRL_WRD0,
+					PIO2_REGS_CTRL_WRD0,
+					PIO2_REGS_CTRL_WRD1,
+					PIO2_REGS_CTRL_WRD1,
+					PIO2_REGS_CTRL_WRD1 };
+
+#define PIO2_CNTR_SC_DEV0		0
+#define PIO2_CNTR_SC_DEV1		(1 << 6)
+#define PIO2_CNTR_SC_DEV2		(2 << 6)
+#define PIO2_CNTR_SC_RDBACK		(3 << 6)
+
+static const int PIO2_CNTR_SC_DEV[6] = { PIO2_CNTR_SC_DEV0, PIO2_CNTR_SC_DEV1,
+					PIO2_CNTR_SC_DEV2, PIO2_CNTR_SC_DEV0,
+					PIO2_CNTR_SC_DEV1, PIO2_CNTR_SC_DEV2 };
+
+#define PIO2_CNTR_RW_LATCH		0
+#define PIO2_CNTR_RW_LSB		(1 << 4)
+#define PIO2_CNTR_RW_MSB		(2 << 4)
+#define PIO2_CNTR_RW_BOTH		(3 << 4)
+
+#define PIO2_CNTR_MODE0			0
+#define PIO2_CNTR_MODE1			(1 << 1)
+#define PIO2_CNTR_MODE2			(2 << 1)
+#define PIO2_CNTR_MODE3			(3 << 1)
+#define PIO2_CNTR_MODE4			(4 << 1)
+#define PIO2_CNTR_MODE5			(5 << 1)
+
+#define PIO2_CNTR_BCD			1
+
+
+
+enum pio2_bank_config { NOFIT, INPUT, OUTPUT, BOTH };
+enum pio2_int_config { NONE = 0, LOW2HIGH = 1, HIGH2LOW = 2, EITHER = 4 };
+
+/* Bank configuration structure */
+struct pio2_io_bank {
+	enum pio2_bank_config config;
+	u8 value;
+	enum pio2_int_config irq[8];
+};
+
+/* Counter configuration structure */
+struct pio2_cntr {
+	int mode;
+	int count;
+};
+
+struct pio2_card {
+	int id;
+	int bus;
+	long base;
+	int irq_vector;
+	int irq_level;
+	char variant[6];
+	int led;
+
+	struct vme_dev *vdev;
+	struct vme_resource *window;
+
+	struct gpio_chip gc;
+	struct pio2_io_bank bank[4];
+
+	struct pio2_cntr cntr[6];
+};
+
+int pio2_cntr_reset(struct pio2_card *);
+
+int pio2_gpio_reset(struct pio2_card *);
+int __init pio2_gpio_init(struct pio2_card *);
+void __exit pio2_gpio_exit(struct pio2_card *);
+
+#endif /* _VME_PIO2_H_ */
diff --git a/drivers/staging/vme/devices/vme_pio2_cntr.c b/drivers/staging/vme/devices/vme_pio2_cntr.c
new file mode 100644
index 000000000000..08e0d59806ca
--- /dev/null
+++ b/drivers/staging/vme/devices/vme_pio2_cntr.c
@@ -0,0 +1,71 @@
+/*
+ * GE PIO2 Counter Driver
+ *
+ * Author: Martyn Welch <martyn.welch@ge.com>
+ * Copyright 2009 GE Intelligent Platforms Embedded Systems, Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * The PIO-2 has 6 counters, currently this code just disables the interrupts
+ * and leaves them alone.
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/gpio.h>
+
+#include "../vme.h"
+#include "vme_pio2.h"
+
+static int pio2_cntr_irq_set(struct pio2_card *card, int id)
+{
+	int retval;
+	u8 data;
+
+	data = PIO2_CNTR_SC_DEV[id] | PIO2_CNTR_RW_BOTH | card->cntr[id].mode;
+	retval = vme_master_write(card->window, &data, 1, PIO2_CNTR_CTRL[id]);
+	if (retval < 0)
+		return retval;
+
+	data = card->cntr[id].count & 0xFF;
+	retval = vme_master_write(card->window, &data, 1, PIO2_CNTR_DATA[id]);
+	if (retval < 0)
+		return retval;
+
+	data = (card->cntr[id].count >> 8) & 0xFF;
+	retval = vme_master_write(card->window, &data, 1, PIO2_CNTR_DATA[id]);
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+int pio2_cntr_reset(struct pio2_card *card)
+{
+	int i, retval = 0;
+	u8 reg;
+
+	/* Clear down all timers */
+	for (i = 0; i < 6; i++) {
+		card->cntr[i].mode = PIO2_CNTR_MODE5;
+		card->cntr[i].count = 0;
+		retval = pio2_cntr_irq_set(card, i);
+		if (retval < 0)
+			return retval;
+	}
+
+	/* Ensure all counter interrupts are cleared */
+	do {
+		retval = vme_master_read(card->window, &reg, 1,
+			PIO2_REGS_INT_STAT_CNTR);
+		if (retval < 0)
+			return retval;
+	} while (reg != 0);
+
+	return retval;
+}
+
diff --git a/drivers/staging/vme/devices/vme_pio2_core.c b/drivers/staging/vme/devices/vme_pio2_core.c
new file mode 100644
index 000000000000..9fedc442a779
--- /dev/null
+++ b/drivers/staging/vme/devices/vme_pio2_core.c
@@ -0,0 +1,524 @@
+/*
+ * GE PIO2 6U VME I/O Driver
+ *
+ * Author: Martyn Welch <martyn.welch@ge.com>
+ * Copyright 2009 GE Intelligent Platforms Embedded Systems, Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/device.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+
+#include "../vme.h"
+#include "vme_pio2.h"
+
+
+static const char driver_name[] = "pio2";
+
+static int bus[PIO2_CARDS_MAX];
+static int bus_num;
+static long base[PIO2_CARDS_MAX];
+static int base_num;
+static int vector[PIO2_CARDS_MAX];
+static int vector_num;
+static int level[PIO2_CARDS_MAX];
+static int level_num;
+static const char *variant[PIO2_CARDS_MAX];
+static int variant_num;
+
+static int loopback;
+
+static int pio2_match(struct vme_dev *);
+static int __devinit pio2_probe(struct vme_dev *);
+static int __devexit pio2_remove(struct vme_dev *);
+
+static int pio2_get_led(struct pio2_card *card)
+{
+	/* Can't read hardware, state saved in structure */
+	return card->led;
+}
+
+static int pio2_set_led(struct pio2_card *card, int state)
+{
+	u8 reg;
+	int retval;
+
+	reg = card->irq_level;
+
+	/* Register state inverse of led state */
+	if (!state)
+		reg |= PIO2_LED;
+
+	if (loopback)
+		reg |= PIO2_LOOP;
+
+	retval = vme_master_write(card->window, &reg, 1, PIO2_REGS_CTRL);
+	if (retval < 0)
+		return retval;
+
+	card->led = state ? 1 : 0;
+
+	return 0;
+}
+
+static void pio2_int(int level, int vector, void *ptr)
+{
+	int vec, i, channel, retval;
+	u8 reg;
+	struct pio2_card *card  = ptr;
+
+	vec = vector & ~PIO2_VME_VECTOR_MASK;
+
+	switch (vec) {
+	case 0:
+		dev_warn(&card->vdev->dev, "Spurious Interrupt\n");
+		break;
+	case 1:
+	case 2:
+	case 3:
+	case 4:
+		/* Channels 0 to 7 */
+		retval = vme_master_read(card->window, &reg, 1,
+			PIO2_REGS_INT_STAT[vec - 1]);
+		if (retval < 0) {
+			dev_err(&card->vdev->dev,
+				"Unable to read IRQ status register\n");
+			return;
+		}
+		for (i = 0; i < 8; i++) {
+			channel = ((vec - 1) * 8) + i;
+			if (reg & PIO2_CHANNEL_BIT[channel])
+				dev_info(&card->vdev->dev,
+					"Interrupt on I/O channel %d\n",
+					channel);
+		}
+		break;
+	case 5:
+	case 6:
+	case 7:
+	case 8:
+	case 9:
+	case 10:
+		/* Counters are dealt with by their own handler */
+		dev_err(&card->vdev->dev,
+			"Counter interrupt\n");
+		break;
+	}
+}
+
+
+/*
+ * We return whether this has been successful - this is used in the probe to
+ * ensure we have a valid card.
+ */
+static int pio2_reset_card(struct pio2_card *card)
+{
+	int retval = 0;
+	u8 data = 0;
+
+	/* Clear main register*/
+	retval = vme_master_write(card->window, &data, 1, PIO2_REGS_CTRL);
+	if (retval < 0)
+		return retval;
+
+	/* Clear VME vector */
+	retval = vme_master_write(card->window, &data, 1, PIO2_REGS_VME_VECTOR);
+	if (retval < 0)
+		return retval;
+
+	/* Reset GPIO */
+	retval = pio2_gpio_reset(card);
+	if (retval < 0)
+		return retval;
+
+	/* Reset counters */
+	retval = pio2_cntr_reset(card);
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+static struct vme_driver pio2_driver = {
+	.name = driver_name,
+	.match = pio2_match,
+	.probe = pio2_probe,
+	.remove = __devexit_p(pio2_remove),
+};
+
+
+static int __init pio2_init(void)
+{
+	int retval = 0;
+
+	if (bus_num == 0) {
+		printk(KERN_ERR "%s: No cards, skipping registration\n",
+			driver_name);
+		goto err_nocard;
+	}
+
+	if (bus_num > PIO2_CARDS_MAX) {
+		printk(KERN_ERR
+			"%s: Driver only able to handle %d PIO2 Cards\n",
+			driver_name, PIO2_CARDS_MAX);
+		bus_num = PIO2_CARDS_MAX;
+	}
+
+	/* Register the PIO2 driver */
+	retval = vme_register_driver(&pio2_driver, bus_num);
+	if (retval != 0)
+		goto err_reg;
+
+	return retval;
+
+err_reg:
+err_nocard:
+	return retval;
+}
+
+static int pio2_match(struct vme_dev *vdev)
+{
+
+	if (vdev->num >= bus_num) {
+		dev_err(&vdev->dev,
+			"The enumeration of the VMEbus to which the board is connected must be specified");
+		return 0;
+	}
+
+	if (vdev->num >= base_num) {
+		dev_err(&vdev->dev,
+			"The VME address for the cards registers must be specified");
+		return 0;
+	}
+
+	if (vdev->num >= vector_num) {
+		dev_err(&vdev->dev,
+			"The IRQ vector used by the card must be specified");
+		return 0;
+	}
+
+	if (vdev->num >= level_num) {
+		dev_err(&vdev->dev,
+			"The IRQ level used by the card must be specified");
+		return 0;
+	}
+
+	if (vdev->num >= variant_num) {
+		dev_err(&vdev->dev, "The variant of the card must be specified");
+		return 0;
+	}
+
+	return 1;
+}
+
+static int __devinit pio2_probe(struct vme_dev *vdev)
+{
+	struct pio2_card *card;
+	int retval;
+	int i;
+	u8 reg;
+	int vec;
+
+	card = kzalloc(sizeof(struct pio2_card), GFP_KERNEL);
+	if (card == NULL) {
+		dev_err(&vdev->dev, "Unable to allocate card structure\n");
+		retval = -ENOMEM;
+		goto err_struct;
+	}
+
+	card->id = vdev->num;
+	card->bus = bus[card->id];
+	card->base = base[card->id];
+	card->irq_vector = vector[card->id];
+	card->irq_level = level[card->id] & PIO2_VME_INT_MASK;
+	strncpy(card->variant, variant[card->id], PIO2_VARIANT_LENGTH);
+	card->vdev = vdev;
+
+	for (i = 0; i < PIO2_VARIANT_LENGTH; i++) {
+
+		if (isdigit(card->variant[i]) == 0) {
+			dev_err(&card->vdev->dev, "Variant invalid\n");
+			retval = -EINVAL;
+			goto err_variant;
+		}
+	}
+
+	/*
+	 * Bottom 4 bits of VME interrupt vector used to determine source,
+	 * provided vector should only use upper 4 bits.
+	 */
+	if (card->irq_vector & ~PIO2_VME_VECTOR_MASK) {
+		dev_err(&card->vdev->dev,
+			"Invalid VME IRQ Vector, vector must not use lower 4 bits\n");
+		retval = -EINVAL;
+		goto err_vector;
+	}
+
+	/*
+	 * There is no way to determine the build variant or whether each bank
+	 * is input, output or both at run time. The inputs are also inverted
+	 * if configured as both.
+	 *
+	 * We pass in the board variant and use that to determine the
+	 * configuration of the banks.
+	 */
+	for (i = 1; i < PIO2_VARIANT_LENGTH; i++) {
+		switch (card->variant[i]) {
+		case '0':
+			card->bank[i-1].config = NOFIT;
+			break;
+		case '1':
+		case '2':
+		case '3':
+		case '4':
+			card->bank[i-1].config = INPUT;
+			break;
+		case '5':
+			card->bank[i-1].config = OUTPUT;
+			break;
+		case '6':
+		case '7':
+		case '8':
+		case '9':
+			card->bank[i-1].config = BOTH;
+			break;
+		}
+	}
+
+	/* Get a master window and position over regs */
+	card->window = vme_master_request(vdev, VME_A24, VME_SCT, VME_D16);
+	if (card->window == NULL) {
+		dev_err(&card->vdev->dev,
+			"Unable to assign VME master resource\n");
+		retval = -EIO;
+		goto err_window;
+	}
+
+	retval = vme_master_set(card->window, 1, card->base, 0x10000, VME_A24,
+		(VME_SCT | VME_USER | VME_DATA), VME_D16);
+	if (retval) {
+		dev_err(&card->vdev->dev,
+			"Unable to configure VME master resource\n");
+		goto err_set;
+	}
+
+	/*
+	 * There is also no obvious register which we can probe to determine
+	 * whether the provided base is valid. If we can read the "ID Register"
+	 * offset and the reset function doesn't error, assume we have a valid
+	 * location.
+	 */
+	retval = vme_master_read(card->window, &reg, 1, PIO2_REGS_ID);
+	if (retval < 0) {
+		dev_err(&card->vdev->dev, "Unable to read from device\n");
+		goto err_read;
+	}
+
+	dev_dbg(&card->vdev->dev, "ID Register:%x\n", reg);
+
+	/*
+	 * Ensure all the I/O is cleared. We can't read back the states, so
+	 * this is the only method we have to ensure that the I/O is in a known
+	 * state.
+	 */
+	retval = pio2_reset_card(card);
+	if (retval) {
+		dev_err(&card->vdev->dev,
+			"Failed to reset card, is location valid?");
+		retval = -ENODEV;
+		goto err_reset;
+	}
+
+	/* Configure VME Interrupts */
+	reg = card->irq_level;
+	if (pio2_get_led(card))
+		reg |= PIO2_LED;
+	if (loopback)
+		reg |= PIO2_LOOP;
+	retval = vme_master_write(card->window, &reg, 1, PIO2_REGS_CTRL);
+	if (retval < 0)
+		return retval;
+
+	/* Set VME vector */
+	retval = vme_master_write(card->window, &card->irq_vector, 1,
+		PIO2_REGS_VME_VECTOR);
+	if (retval < 0)
+		return retval;
+
+	/* Attach spurious interrupt handler. */
+	vec = card->irq_vector | PIO2_VME_VECTOR_SPUR;
+
+	retval = vme_irq_request(vdev, card->irq_level, vec,
+		&pio2_int, (void *)card);
+	if (retval < 0) {
+		dev_err(&card->vdev->dev,
+			"Unable to attach VME interrupt vector0x%x, level 0x%x\n",
+			 vec, card->irq_level);
+		goto err_irq;
+	}
+
+	/* Attach GPIO interrupt handlers. */
+	for (i = 0; i < 4; i++) {
+		vec = card->irq_vector | PIO2_VECTOR_BANK[i];
+
+		retval = vme_irq_request(vdev, card->irq_level, vec,
+			&pio2_int, (void *)card);
+		if (retval < 0) {
+			dev_err(&card->vdev->dev,
+				"Unable to attach VME interrupt vector0x%x, level 0x%x\n",
+				 vec, card->irq_level);
+			goto err_gpio_irq;
+		}
+	}
+
+	/* Attach counter interrupt handlers. */
+	for (i = 0; i < 6; i++) {
+		vec = card->irq_vector | PIO2_VECTOR_CNTR[i];
+
+		retval = vme_irq_request(vdev, card->irq_level, vec,
+			&pio2_int, (void *)card);
+		if (retval < 0) {
+			dev_err(&card->vdev->dev,
+				"Unable to attach VME interrupt vector0x%x, level 0x%x\n",
+				vec, card->irq_level);
+			goto err_cntr_irq;
+		}
+	}
+
+	/* Register IO */
+	retval = pio2_gpio_init(card);
+	if (retval < 0) {
+		dev_err(&card->vdev->dev,
+			"Unable to register with GPIO framework\n");
+		goto err_gpio;
+	}
+
+	/* Set LED - This also sets interrupt level */
+	retval = pio2_set_led(card, 0);
+	if (retval < 0) {
+		dev_err(&card->vdev->dev, "Unable to set LED\n");
+		goto err_led;
+	}
+
+	dev_set_drvdata(&card->vdev->dev, card);
+
+	dev_info(&card->vdev->dev,
+		"PIO2 (variant %s) configured at 0x%lx\n", card->variant,
+		card->base);
+
+	return 0;
+
+err_led:
+	pio2_gpio_exit(card);
+err_gpio:
+	i = 6;
+err_cntr_irq:
+	while (i > 0) {
+		i--;
+		vec = card->irq_vector | PIO2_VECTOR_CNTR[i];
+		vme_irq_free(vdev, card->irq_level, vec);
+	}
+
+	i = 4;
+err_gpio_irq:
+	while (i > 0) {
+		i--;
+		vec = card->irq_vector | PIO2_VECTOR_BANK[i];
+		vme_irq_free(vdev, card->irq_level, vec);
+	}
+
+	vec = (card->irq_vector & PIO2_VME_VECTOR_MASK) | PIO2_VME_VECTOR_SPUR;
+	vme_irq_free(vdev, card->irq_level, vec);
+err_irq:
+	 pio2_reset_card(card);
+err_reset:
+err_read:
+	vme_master_set(card->window, 0, 0, 0, VME_A16, 0, VME_D16);
+err_set:
+	vme_master_free(card->window);
+err_window:
+err_vector:
+err_variant:
+	kfree(card);
+err_struct:
+	return retval;
+}
+
+static int __devexit pio2_remove(struct vme_dev *vdev)
+{
+	int vec;
+	int i;
+
+	struct pio2_card *card = dev_get_drvdata(&vdev->dev);
+
+	pio2_gpio_exit(card);
+
+	for (i = 0; i < 6; i++) {
+		vec = card->irq_vector | PIO2_VECTOR_CNTR[i];
+		vme_irq_free(vdev, card->irq_level, vec);
+	}
+
+	for (i = 0; i < 4; i++) {
+		vec = card->irq_vector | PIO2_VECTOR_BANK[i];
+		vme_irq_free(vdev, card->irq_level, vec);
+	}
+
+	vec = (card->irq_vector & PIO2_VME_VECTOR_MASK) | PIO2_VME_VECTOR_SPUR;
+	vme_irq_free(vdev, card->irq_level, vec);
+
+	pio2_reset_card(card);
+
+	vme_master_set(card->window, 0, 0, 0, VME_A16, 0, VME_D16);
+
+	vme_master_free(card->window);
+
+	kfree(card);
+
+	return 0;
+}
+
+static void __exit pio2_exit(void)
+{
+	vme_unregister_driver(&pio2_driver);
+}
+
+
+/* These are required for each board */
+MODULE_PARM_DESC(bus, "Enumeration of VMEbus to which the board is connected");
+module_param_array(bus, int, &bus_num, S_IRUGO);
+
+MODULE_PARM_DESC(base, "Base VME address for PIO2 Registers");
+module_param_array(base, long, &base_num, S_IRUGO);
+
+MODULE_PARM_DESC(vector, "VME IRQ Vector (Lower 4 bits masked)");
+module_param_array(vector, int, &vector_num, S_IRUGO);
+
+MODULE_PARM_DESC(level, "VME IRQ Level");
+module_param_array(level, int, &level_num, S_IRUGO);
+
+MODULE_PARM_DESC(variant, "Last 4 characters of PIO2 board variant");
+module_param_array(variant, charp, &variant_num, S_IRUGO);
+
+/* This is for debugging */
+MODULE_PARM_DESC(loopback, "Enable loopback mode on all cards");
+module_param(loopback, bool, S_IRUGO);
+
+MODULE_DESCRIPTION("GE PIO2 6U VME I/O Driver");
+MODULE_AUTHOR("Martyn Welch <martyn.welch@ge.com");
+MODULE_LICENSE("GPL");
+
+module_init(pio2_init);
+module_exit(pio2_exit);
+
diff --git a/drivers/staging/vme/devices/vme_pio2_gpio.c b/drivers/staging/vme/devices/vme_pio2_gpio.c
new file mode 100644
index 000000000000..dc837deb99dd
--- /dev/null
+++ b/drivers/staging/vme/devices/vme_pio2_gpio.c
@@ -0,0 +1,232 @@
+/*
+ * GE PIO2 GPIO Driver
+ *
+ * Author: Martyn Welch <martyn.welch@ge.com>
+ * Copyright 2009 GE Intelligent Platforms Embedded Systems, Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/ctype.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+
+#include "../vme.h"
+#include "vme_pio2.h"
+
+static const char driver_name[] = "pio2_gpio";
+
+static struct pio2_card *gpio_to_pio2_card(struct gpio_chip *chip)
+{
+	return container_of(chip, struct pio2_card, gc);
+}
+
+static int pio2_gpio_get(struct gpio_chip *chip, unsigned int offset)
+{
+	u8 reg;
+	int retval;
+	struct pio2_card *card = gpio_to_pio2_card(chip);
+
+	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == OUTPUT) |
+		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
+
+		dev_err(&card->vdev->dev, "Channel not available as input\n");
+		return 0;
+	}
+
+	retval = vme_master_read(card->window, &reg, 1,
+		PIO2_REGS_DATA[PIO2_CHANNEL_BANK[offset]]);
+	if (retval < 0) {
+		dev_err(&card->vdev->dev, "Unable to read from GPIO\n");
+		return 0;
+	}
+
+	/*
+	 * Remember, input on channels configured as both input and output
+	 * are inverted!
+	 */
+	if (reg & PIO2_CHANNEL_BIT[offset]) {
+		if (card->bank[PIO2_CHANNEL_BANK[offset]].config != BOTH)
+			return 0;
+		else
+			return 1;
+	} else {
+		if (card->bank[PIO2_CHANNEL_BANK[offset]].config != BOTH)
+			return 1;
+		else
+			return 0;
+	}
+}
+
+static void pio2_gpio_set(struct gpio_chip *chip, unsigned int offset,
+	int value)
+{
+	u8 reg;
+	int retval;
+	struct pio2_card *card = gpio_to_pio2_card(chip);
+
+	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == INPUT) |
+		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
+
+		dev_err(&card->vdev->dev, "Channel not availabe as output\n");
+		return;
+	}
+
+	if (value)
+		reg = card->bank[PIO2_CHANNEL_BANK[offset]].value |
+			PIO2_CHANNEL_BIT[offset];
+	else
+		reg = card->bank[PIO2_CHANNEL_BANK[offset]].value &
+			~PIO2_CHANNEL_BIT[offset];
+
+	retval = vme_master_write(card->window, &reg, 1,
+		PIO2_REGS_DATA[PIO2_CHANNEL_BANK[offset]]);
+	if (retval < 0) {
+		dev_err(&card->vdev->dev, "Unable to write to GPIO\n");
+		return;
+	}
+
+	card->bank[PIO2_CHANNEL_BANK[offset]].value = reg;
+}
+
+/* Directionality configured at board build - send appropriate response */
+static int pio2_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
+{
+	int data;
+	struct pio2_card *card = gpio_to_pio2_card(chip);
+
+	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == OUTPUT) |
+		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
+		dev_err(&card->vdev->dev,
+			"Channel directionality not configurable at runtine\n");
+
+		data = -EINVAL;
+	} else {
+		data = 0;
+	}
+
+	return data;
+}
+
+/* Directionality configured at board build - send appropriate response */
+static int pio2_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int value)
+{
+	int data;
+	struct pio2_card *card = gpio_to_pio2_card(chip);
+
+	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == INPUT) |
+		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
+		dev_err(&card->vdev->dev,
+			"Channel directionality not configurable at runtine\n");
+
+		data = -EINVAL;
+	} else {
+		data = 0;
+	}
+
+	return data;
+}
+
+/*
+ * We return whether this has been successful - this is used in the probe to
+ * ensure we have a valid card.
+ */
+int pio2_gpio_reset(struct pio2_card *card)
+{
+	int retval = 0;
+	int i, j;
+
+	u8 data = 0;
+
+	/* Zero output registers */
+	for (i = 0; i < 4; i++) {
+		retval = vme_master_write(card->window, &data, 1,
+			PIO2_REGS_DATA[i]);
+		if (retval < 0)
+			return retval;
+		card->bank[i].value = 0;
+	}
+
+	/* Set input interrupt masks */
+	for (i = 0; i < 4; i++) {
+		retval = vme_master_write(card->window, &data, 1,
+			PIO2_REGS_INT_MASK[i * 2]);
+		if (retval < 0)
+			return retval;
+
+		retval = vme_master_write(card->window, &data, 1,
+			PIO2_REGS_INT_MASK[(i * 2) + 1]);
+		if (retval < 0)
+			return retval;
+
+		for (j = 0; j < 8; j++)
+			card->bank[i].irq[j] = NONE;
+	}
+
+	/* Ensure all I/O interrupts are cleared */
+	for (i = 0; i < 4; i++) {
+		do {
+			retval = vme_master_read(card->window, &data, 1,
+				PIO2_REGS_INT_STAT[i]);
+			if (retval < 0)
+				return retval;
+		} while (data != 0);
+	}
+
+	return 0;
+}
+
+int __init pio2_gpio_init(struct pio2_card *card)
+{
+	int retval = 0;
+	char *label;
+
+	label = kmalloc(PIO2_NUM_CHANNELS, GFP_KERNEL);
+	if (label == NULL) {
+		dev_err(&card->vdev->dev, "Unable to allocate GPIO label\n");
+		return -ENOMEM;
+	}
+
+	sprintf(label, "%s@%s", driver_name, dev_name(&card->vdev->dev));
+	card->gc.label = label;
+
+	card->gc.ngpio = PIO2_NUM_CHANNELS;
+	/* Dynamic allocation of base */
+	card->gc.base = -1;
+	/* Setup pointers to chip functions */
+	card->gc.direction_input = pio2_gpio_dir_in;
+	card->gc.direction_output = pio2_gpio_dir_out;
+	card->gc.get = pio2_gpio_get;
+	card->gc.set = pio2_gpio_set;
+
+	/* This function adds a memory mapped GPIO chip */
+	retval = gpiochip_add(&(card->gc));
+	if (retval) {
+		dev_err(&card->vdev->dev, "Unable to register GPIO\n");
+		kfree(card->gc.label);
+	}
+
+	return retval;
+};
+
+void __exit pio2_gpio_exit(struct pio2_card *card)
+{
+	const char *label = card->gc.label;
+
+	if (gpiochip_remove(&(card->gc)))
+		dev_err(&card->vdev->dev, "Failed to remove GPIO");
+
+	kfree(label);
+}
+
diff --git a/drivers/staging/vme/devices/vme_user.h b/drivers/staging/vme/devices/vme_user.h
index d85a1e9dbe3a..7d24cd6343e4 100644
--- a/drivers/staging/vme/devices/vme_user.h
+++ b/drivers/staging/vme/devices/vme_user.h
@@ -10,9 +10,9 @@ struct vme_master {
 	int enable;			/* State of Window */
 	unsigned long long vme_addr;	/* Starting Address on the VMEbus */
 	unsigned long long size;	/* Window Size */
-	vme_address_t aspace;		/* Address Space */
-	vme_cycle_t cycle;		/* Cycle properties */
-	vme_width_t dwidth;		/* Maximum Data Width */
+	u32 aspace;			/* Address Space */
+	u32 cycle;		/* Cycle properties */
+	u32 dwidth;		/* Maximum Data Width */
 #if 0
 	char prefetchEnable;		/* Prefetch Read Enable State */
 	int prefetchSize;		/* Prefetch Read Size (Cache Lines) */
@@ -34,8 +34,8 @@ struct vme_slave {
 	int enable;			/* State of Window */
 	unsigned long long vme_addr;	/* Starting Address on the VMEbus */
 	unsigned long long size;	/* Window Size */
-	vme_address_t aspace;		/* Address Space */
-	vme_cycle_t cycle;		/* Cycle properties */
+	u32 aspace;			/* Address Space */
+	u32 cycle;		/* Cycle properties */
 #if 0
 	char wrPostEnable;		/* Write Post State */
 	char rmwLock;			/* Lock PCI during RMW Cycles */
diff --git a/drivers/staging/vme/vme.c b/drivers/staging/vme/vme.c
index b04b4688f705..70722ae52321 100644
--- a/drivers/staging/vme/vme.c
+++ b/drivers/staging/vme/vme.c
@@ -153,9 +153,7 @@ size_t vme_get_size(struct vme_resource *resource)
 	int enabled, retval;
 	unsigned long long base, size;
 	dma_addr_t buf_base;
-	vme_address_t aspace;
-	vme_cycle_t cycle;
-	vme_width_t dwidth;
+	u32 aspace, cycle, dwidth;
 
 	switch (resource->type) {
 	case VME_MASTER:
@@ -181,7 +179,7 @@ size_t vme_get_size(struct vme_resource *resource)
 }
 EXPORT_SYMBOL(vme_get_size);
 
-static int vme_check_window(vme_address_t aspace, unsigned long long vme_base,
+static int vme_check_window(u32 aspace, unsigned long long vme_base,
 	unsigned long long size)
 {
 	int retval = 0;
@@ -232,8 +230,8 @@ static int vme_check_window(vme_address_t aspace, unsigned long long vme_base,
  * Request a slave image with specific attributes, return some unique
  * identifier.
  */
-struct vme_resource *vme_slave_request(struct vme_dev *vdev,
-	vme_address_t address, vme_cycle_t cycle)
+struct vme_resource *vme_slave_request(struct vme_dev *vdev, u32 address,
+	u32 cycle)
 {
 	struct vme_bridge *bridge;
 	struct list_head *slave_pos = NULL;
@@ -298,7 +296,7 @@ EXPORT_SYMBOL(vme_slave_request);
 
 int vme_slave_set(struct vme_resource *resource, int enabled,
 	unsigned long long vme_base, unsigned long long size,
-	dma_addr_t buf_base, vme_address_t aspace, vme_cycle_t cycle)
+	dma_addr_t buf_base, u32 aspace, u32 cycle)
 {
 	struct vme_bridge *bridge = find_bridge(resource);
 	struct vme_slave_resource *image;
@@ -333,7 +331,7 @@ EXPORT_SYMBOL(vme_slave_set);
 
 int vme_slave_get(struct vme_resource *resource, int *enabled,
 	unsigned long long *vme_base, unsigned long long *size,
-	dma_addr_t *buf_base, vme_address_t *aspace, vme_cycle_t *cycle)
+	dma_addr_t *buf_base, u32 *aspace, u32 *cycle)
 {
 	struct vme_bridge *bridge = find_bridge(resource);
 	struct vme_slave_resource *image;
@@ -388,8 +386,8 @@ EXPORT_SYMBOL(vme_slave_free);
  * Request a master image with specific attributes, return some unique
  * identifier.
  */
-struct vme_resource *vme_master_request(struct vme_dev *vdev,
-	vme_address_t address, vme_cycle_t cycle, vme_width_t dwidth)
+struct vme_resource *vme_master_request(struct vme_dev *vdev, u32 address,
+	u32 cycle, u32 dwidth)
 {
 	struct vme_bridge *bridge;
 	struct list_head *master_pos = NULL;
@@ -456,8 +454,8 @@ err_bus:
 EXPORT_SYMBOL(vme_master_request);
 
 int vme_master_set(struct vme_resource *resource, int enabled,
-	unsigned long long vme_base, unsigned long long size,
-	vme_address_t aspace, vme_cycle_t cycle, vme_width_t dwidth)
+	unsigned long long vme_base, unsigned long long size, u32 aspace,
+	u32 cycle, u32 dwidth)
 {
 	struct vme_bridge *bridge = find_bridge(resource);
 	struct vme_master_resource *image;
@@ -492,8 +490,8 @@ int vme_master_set(struct vme_resource *resource, int enabled,
 EXPORT_SYMBOL(vme_master_set);
 
 int vme_master_get(struct vme_resource *resource, int *enabled,
-	unsigned long long *vme_base, unsigned long long *size,
-	vme_address_t *aspace, vme_cycle_t *cycle, vme_width_t *dwidth)
+	unsigned long long *vme_base, unsigned long long *size, u32 *aspace,
+	u32 *cycle, u32 *dwidth)
 {
 	struct vme_bridge *bridge = find_bridge(resource);
 	struct vme_master_resource *image;
@@ -646,8 +644,7 @@ EXPORT_SYMBOL(vme_master_free);
  * Request a DMA controller with specific attributes, return some unique
  * identifier.
  */
-struct vme_resource *vme_dma_request(struct vme_dev *vdev,
-	vme_dma_route_t route)
+struct vme_resource *vme_dma_request(struct vme_dev *vdev, u32 route)
 {
 	struct vme_bridge *bridge;
 	struct list_head *dma_pos = NULL;
@@ -743,8 +740,7 @@ EXPORT_SYMBOL(vme_new_dma_list);
 /*
  * Create "Pattern" type attributes
  */
-struct vme_dma_attr *vme_dma_pattern_attribute(u32 pattern,
-	vme_pattern_t type)
+struct vme_dma_attr *vme_dma_pattern_attribute(u32 pattern, u32 type)
 {
 	struct vme_dma_attr *attributes;
 	struct vme_dma_pattern *pattern_attr;
@@ -822,7 +818,7 @@ EXPORT_SYMBOL(vme_dma_pci_attribute);
  * Create "VME" type attributes
  */
 struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long address,
-	vme_address_t aspace, vme_cycle_t cycle, vme_width_t dwidth)
+	u32 aspace, u32 cycle, u32 dwidth)
 {
 	struct vme_dma_attr *attributes;
 	struct vme_dma_vme *vme_attr;
@@ -1173,7 +1169,7 @@ int vme_lm_count(struct vme_resource *resource)
 EXPORT_SYMBOL(vme_lm_count);
 
 int vme_lm_set(struct vme_resource *resource, unsigned long long lm_base,
-	vme_address_t aspace, vme_cycle_t cycle)
+	u32 aspace, u32 cycle)
 {
 	struct vme_bridge *bridge = find_bridge(resource);
 	struct vme_lm_resource *lm;
@@ -1195,7 +1191,7 @@ int vme_lm_set(struct vme_resource *resource, unsigned long long lm_base,
 EXPORT_SYMBOL(vme_lm_set);
 
 int vme_lm_get(struct vme_resource *resource, unsigned long long *lm_base,
-	vme_address_t *aspace, vme_cycle_t *cycle)
+	u32 *aspace, u32 *cycle)
 {
 	struct vme_bridge *bridge = find_bridge(resource);
 	struct vme_lm_resource *lm;
@@ -1307,7 +1303,12 @@ EXPORT_SYMBOL(vme_slot_get);
 
 /* - Bridge Registration --------------------------------------------------- */
 
-static int vme_add_bus(struct vme_bridge *bridge)
+static void vme_dev_release(struct device *dev)
+{
+	kfree(dev_to_vme_dev(dev));
+}
+
+int vme_register_bridge(struct vme_bridge *bridge)
 {
 	int i;
 	int ret = -1;
@@ -1327,8 +1328,9 @@ static int vme_add_bus(struct vme_bridge *bridge)
 
 	return ret;
 }
+EXPORT_SYMBOL(vme_register_bridge);
 
-static void vme_remove_bus(struct vme_bridge *bridge)
+void vme_unregister_bridge(struct vme_bridge *bridge)
 {
 	struct vme_dev *vdev;
 	struct vme_dev *tmp;
@@ -1343,22 +1345,6 @@ static void vme_remove_bus(struct vme_bridge *bridge)
 	list_del(&bridge->bus_list);
 	mutex_unlock(&vme_buses_lock);
 }
-
-static void vme_dev_release(struct device *dev)
-{
-	kfree(dev_to_vme_dev(dev));
-}
-
-int vme_register_bridge(struct vme_bridge *bridge)
-{
-	return vme_add_bus(bridge);
-}
-EXPORT_SYMBOL(vme_register_bridge);
-
-void vme_unregister_bridge(struct vme_bridge *bridge)
-{
-	vme_remove_bus(bridge);
-}
 EXPORT_SYMBOL(vme_unregister_bridge);
 
 /* - Driver Registration --------------------------------------------------- */
@@ -1421,10 +1407,7 @@ static int __vme_register_driver(struct vme_driver *drv, unsigned int ndevs)
 		 * and if the bridge is removed, it will have to go through
 		 * vme_unregister_bridge() to do it (which calls remove() on
 		 * the bridge which in turn tries to acquire vme_buses_lock and
-		 * will have to wait). The probe() called after device
-		 * registration in __vme_register_driver below will also fail
-		 * as the bridge is being removed (since the probe() calls
-		 * vme_bridge_get()).
+		 * will have to wait).
 		 */
 		err = __vme_register_driver_bus(drv, bridge, ndevs);
 		if (err)
diff --git a/drivers/staging/vme/vme.h b/drivers/staging/vme/vme.h
index e3828badca61..9d38ceed60e2 100644
--- a/drivers/staging/vme/vme.h
+++ b/drivers/staging/vme/vme.h
@@ -10,7 +10,6 @@ enum vme_resource_type {
 };
 
 /* VME Address Spaces */
-typedef u32 vme_address_t;
 #define VME_A16		0x1
 #define VME_A24		0x2
 #define	VME_A32		0x4
@@ -29,7 +28,6 @@ typedef u32 vme_address_t;
 
 
 /* VME Cycle Types */
-typedef u32 vme_cycle_t;
 #define VME_SCT		0x1
 #define VME_BLT		0x2
 #define VME_MBLT	0x4
@@ -47,28 +45,23 @@ typedef u32 vme_cycle_t;
 #define	VME_DATA	0x8000
 
 /* VME Data Widths */
-typedef u32 vme_width_t;
 #define VME_D8		0x1
 #define VME_D16		0x2
 #define VME_D32		0x4
 #define VME_D64		0x8
 
 /* Arbitration Scheduling Modes */
-typedef u32 vme_arbitration_t;
 #define VME_R_ROBIN_MODE	0x1
 #define VME_PRIORITY_MODE	0x2
 
-typedef u32 vme_dma_t;
 #define VME_DMA_PATTERN			(1<<0)
 #define VME_DMA_PCI			(1<<1)
 #define VME_DMA_VME			(1<<2)
 
-typedef u32 vme_pattern_t;
 #define VME_DMA_PATTERN_BYTE		(1<<0)
 #define VME_DMA_PATTERN_WORD		(1<<1)
 #define VME_DMA_PATTERN_INCREMENT	(1<<2)
 
-typedef u32 vme_dma_route_t;
 #define VME_DMA_VME_TO_MEM		(1<<0)
 #define VME_DMA_MEM_TO_VME		(1<<1)
 #define VME_DMA_VME_TO_VME		(1<<2)
@@ -77,7 +70,7 @@ typedef u32 vme_dma_route_t;
 #define VME_DMA_PATTERN_TO_MEM		(1<<5)
 
 struct vme_dma_attr {
-	vme_dma_t type;
+	u32 type;
 	void *private;
 };
 
@@ -97,7 +90,7 @@ extern struct bus_type vme_bus_type;
 
 /**
  * Structure representing a VME device
- * @id: The ID of the device (currently the bus and slot number)
+ * @num: The device number
  * @bridge: Pointer to the bridge device this device is on
  * @dev: Internal device structure
  * @drv_list: List of devices (per driver)
@@ -128,32 +121,29 @@ void vme_free_consistent(struct vme_resource *, size_t,  void *,
 
 size_t vme_get_size(struct vme_resource *);
 
-struct vme_resource *vme_slave_request(struct vme_dev *, vme_address_t,
-	vme_cycle_t);
+struct vme_resource *vme_slave_request(struct vme_dev *, u32, u32);
 int vme_slave_set(struct vme_resource *, int, unsigned long long,
-	unsigned long long, dma_addr_t, vme_address_t, vme_cycle_t);
+	unsigned long long, dma_addr_t, u32, u32);
 int vme_slave_get(struct vme_resource *, int *, unsigned long long *,
-	unsigned long long *, dma_addr_t *, vme_address_t *, vme_cycle_t *);
+	unsigned long long *, dma_addr_t *, u32 *, u32 *);
 void vme_slave_free(struct vme_resource *);
 
-struct vme_resource *vme_master_request(struct vme_dev *, vme_address_t,
-	vme_cycle_t, vme_width_t);
+struct vme_resource *vme_master_request(struct vme_dev *, u32, u32, u32);
 int vme_master_set(struct vme_resource *, int, unsigned long long,
-	unsigned long long, vme_address_t, vme_cycle_t, vme_width_t);
+	unsigned long long, u32, u32, u32);
 int vme_master_get(struct vme_resource *, int *, unsigned long long *,
-	unsigned long long *, vme_address_t *, vme_cycle_t *, vme_width_t *);
+	unsigned long long *, u32 *, u32 *, u32 *);
 ssize_t vme_master_read(struct vme_resource *, void *, size_t, loff_t);
 ssize_t vme_master_write(struct vme_resource *, void *, size_t, loff_t);
 unsigned int vme_master_rmw(struct vme_resource *, unsigned int, unsigned int,
 	unsigned int, loff_t);
 void vme_master_free(struct vme_resource *);
 
-struct vme_resource *vme_dma_request(struct vme_dev *, vme_dma_route_t);
+struct vme_resource *vme_dma_request(struct vme_dev *, u32);
 struct vme_dma_list *vme_new_dma_list(struct vme_resource *);
-struct vme_dma_attr *vme_dma_pattern_attribute(u32, vme_pattern_t);
+struct vme_dma_attr *vme_dma_pattern_attribute(u32, u32);
 struct vme_dma_attr *vme_dma_pci_attribute(dma_addr_t);
-struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long, vme_address_t,
-	vme_cycle_t, vme_width_t);
+struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long, u32, u32, u32);
 void vme_dma_free_attribute(struct vme_dma_attr *);
 int vme_dma_list_add(struct vme_dma_list *, struct vme_dma_attr *,
 	struct vme_dma_attr *, size_t);
@@ -168,10 +158,8 @@ int vme_irq_generate(struct vme_dev *, int, int);
 
 struct vme_resource * vme_lm_request(struct vme_dev *);
 int vme_lm_count(struct vme_resource *);
-int vme_lm_set(struct vme_resource *, unsigned long long, vme_address_t,
-	vme_cycle_t);
-int vme_lm_get(struct vme_resource *, unsigned long long *, vme_address_t *,
-	vme_cycle_t *);
+int vme_lm_set(struct vme_resource *, unsigned long long, u32, u32);
+int vme_lm_get(struct vme_resource *, unsigned long long *, u32 *, u32 *);
 int vme_lm_attach(struct vme_resource *, int, void (*callback)(int));
 int vme_lm_detach(struct vme_resource *, int);
 void vme_lm_free(struct vme_resource *);
diff --git a/drivers/staging/vme/vme_api.txt b/drivers/staging/vme/vme_api.txt
index e8ff2151a487..856efa35f6e3 100644
--- a/drivers/staging/vme/vme_api.txt
+++ b/drivers/staging/vme/vme_api.txt
@@ -86,26 +86,26 @@ driver allows a resource to be assigned based on the required attributes of the
 driver in question:
 
 	struct vme_resource * vme_master_request(struct vme_dev *dev,
-		vme_address_t aspace, vme_cycle_t cycle, vme_width_t width);
-
-	struct vme_resource * vme_slave_request(struct vme_dev *dev,
-		vme_address_t aspace, vme_cycle_t cycle);
-
-	struct vme_resource *vme_dma_request(struct vme_dev *dev,
-		vme_dma_route_t route);
-
-For slave windows these attributes are split into those of type 'vme_address_t'
-and 'vme_cycle_t'. Master windows add a further set of attributes
-'vme_cycle_t'.  These attributes are defined as bitmasks and as such any
-combination of the attributes can be requested for a single window, the core
-will assign a window that meets the requirements, returning a pointer of type
-vme_resource that should be used to identify the allocated resource when it is
-used. For DMA controllers, the request function requires the potential
-direction of any transfers to be provided in the route attributes. This is
-typically VME-to-MEM and/or MEM-to-VME, though some hardware can support
-VME-to-VME and MEM-to-MEM transfers as well as test pattern generation. If an
-unallocated window fitting the requirements can not be found a NULL pointer
-will be returned.
+		u32 aspace, u32 cycle, u32 width);
+
+	struct vme_resource * vme_slave_request(struct vme_dev *dev, u32 aspace,
+		u32 cycle);
+
+	struct vme_resource *vme_dma_request(struct vme_dev *dev, u32 route);
+
+For slave windows these attributes are split into the VME address spaces that
+need to be accessed in 'aspace' and VME bus cycle types required in 'cycle'.
+Master windows add a further set of attributes in 'width' specifying the
+required data transfer widths. These attributes are defined as bitmasks and as
+such any combination of the attributes can be requested for a single window,
+the core will assign a window that meets the requirements, returning a pointer
+of type vme_resource that should be used to identify the allocated resource
+when it is used. For DMA controllers, the request function requires the
+potential direction of any transfers to be provided in the route attributes.
+This is typically VME-to-MEM and/or MEM-to-VME, though some hardware can
+support VME-to-VME and MEM-to-MEM transfers as well as test pattern generation.
+If an unallocated window fitting the requirements can not be found a NULL
+pointer will be returned.
 
 Functions are also provided to free window allocations once they are no longer
 required. These functions should be passed the pointer to the resource provided
@@ -133,12 +133,12 @@ Once a master window has been assigned the following functions can be used to
 configure it and retrieve the current settings:
 
 	int vme_master_set (struct vme_resource *res, int enabled,
-		unsigned long long base, unsigned long long size,
-		vme_address_t aspace, vme_cycle_t cycle, vme_width_t width);
+		unsigned long long base, unsigned long long size, u32 aspace,
+		u32 cycle, u32 width);
 
 	int vme_master_get (struct vme_resource *res, int *enabled,
-		unsigned long long *base, unsigned long long *size,
-		vme_address_t *aspace, vme_cycle_t *cycle, vme_width_t *width);
+		unsigned long long *base, unsigned long long *size, u32 *aspace,
+		u32 *cycle, u32 *width);
 
 The address spaces, transfer widths and cycle types are the same as described
 under resource management, however some of the options are mutually exclusive.
@@ -189,11 +189,11 @@ configure it and retrieve the current settings:
 
 	int vme_slave_set (struct vme_resource *res, int enabled,
 		unsigned long long base, unsigned long long size,
-		dma_addr_t mem, vme_address_t aspace, vme_cycle_t cycle);
+		dma_addr_t mem, u32 aspace, u32 cycle);
 
 	int vme_slave_get (struct vme_resource *res, int *enabled,
 		unsigned long long *base, unsigned long long *size,
-		dma_addr_t *mem, vme_address_t *aspace, vme_cycle_t *cycle);
+		dma_addr_t *mem, u32 *aspace, u32 *cycle);
 
 The address spaces, transfer widths and cycle types are the same as described
 under resource management, however some of the options are mutually exclusive.
@@ -273,8 +273,7 @@ and pattern sources and destinations (where appropriate):
 
 Pattern source:
 
-	struct vme_dma_attr *vme_dma_pattern_attribute(u32 pattern,
-		vme_pattern_t type);
+	struct vme_dma_attr *vme_dma_pattern_attribute(u32 pattern, u32 type);
 
 PCI source or destination:
 
@@ -283,7 +282,7 @@ PCI source or destination:
 VME source or destination:
 
 	struct vme_dma_attr *vme_dma_vme_attribute(unsigned long long base,
-		vme_address_t aspace, vme_cycle_t cycle, vme_width_t width);
+		u32 aspace, u32 cycle, u32 width);
 
 The following function should be used to free an attribute:
 
@@ -366,10 +365,10 @@ Once a bank of location monitors has been allocated, the following functions
 are provided to configure the location and mode of the location monitor:
 
 	int vme_lm_set(struct vme_resource *res, unsigned long long base,
-		vme_address_t aspace, vme_cycle_t cycle);
+		u32 aspace, u32 cycle);
 
 	int vme_lm_get(struct vme_resource *res, unsigned long long *base,
-		vme_address_t *aspace, vme_cycle_t *cycle);
+		u32 *aspace, u32 *cycle);
 
 
 Location Monitor Use
diff --git a/drivers/staging/vme/vme_bridge.h b/drivers/staging/vme/vme_bridge.h
index c2deda2c38df..934949abd745 100644
--- a/drivers/staging/vme/vme_bridge.h
+++ b/drivers/staging/vme/vme_bridge.h
@@ -15,9 +15,9 @@ struct vme_master_resource {
 	spinlock_t lock;
 	int locked;
 	int number;
-	vme_address_t address_attr;
-	vme_cycle_t cycle_attr;
-	vme_width_t width_attr;
+	u32 address_attr;
+	u32 cycle_attr;
+	u32 width_attr;
 	struct resource bus_resource;
 	void __iomem *kern_base;
 };
@@ -28,13 +28,13 @@ struct vme_slave_resource {
 	struct mutex mtx;
 	int locked;
 	int number;
-	vme_address_t address_attr;
-	vme_cycle_t cycle_attr;
+	u32 address_attr;
+	u32 cycle_attr;
 };
 
 struct vme_dma_pattern {
 	u32 pattern;
-	vme_pattern_t type;
+	u32 type;
 };
 
 struct vme_dma_pci {
@@ -43,9 +43,9 @@ struct vme_dma_pci {
 
 struct vme_dma_vme {
 	unsigned long long address;
-	vme_address_t aspace;
-	vme_cycle_t cycle;
-	vme_width_t dwidth;
+	u32 aspace;
+	u32 cycle;
+	u32 dwidth;
 };
 
 struct vme_dma_list {
@@ -63,7 +63,7 @@ struct vme_dma_resource {
 	int number;
 	struct list_head pending;
 	struct list_head running;
-	vme_dma_route_t route_attr;
+	u32 route_attr;
 };
 
 struct vme_lm_resource {
@@ -122,17 +122,16 @@ struct vme_bridge {
 	/* Slave Functions */
 	int (*slave_get) (struct vme_slave_resource *, int *,
 		unsigned long long *, unsigned long long *, dma_addr_t *,
-		vme_address_t *, vme_cycle_t *);
+		u32 *, u32 *);
 	int (*slave_set) (struct vme_slave_resource *, int, unsigned long long,
-		unsigned long long, dma_addr_t, vme_address_t, vme_cycle_t);
+		unsigned long long, dma_addr_t, u32, u32);
 
 	/* Master Functions */
 	int (*master_get) (struct vme_master_resource *, int *,
-		unsigned long long *, unsigned long long *, vme_address_t *,
-		vme_cycle_t *, vme_width_t *);
+		unsigned long long *, unsigned long long *, u32 *, u32 *,
+		u32 *);
 	int (*master_set) (struct vme_master_resource *, int,
-		unsigned long long, unsigned long long,  vme_address_t,
-		vme_cycle_t, vme_width_t);
+		unsigned long long, unsigned long long,  u32, u32, u32);
 	ssize_t (*master_read) (struct vme_master_resource *, void *, size_t,
 		loff_t);
 	ssize_t (*master_write) (struct vme_master_resource *, void *, size_t,
@@ -151,10 +150,9 @@ struct vme_bridge {
 	int (*irq_generate) (struct vme_bridge *, int, int);
 
 	/* Location monitor functions */
-	int (*lm_set) (struct vme_lm_resource *, unsigned long long,
-		vme_address_t, vme_cycle_t);
-	int (*lm_get) (struct vme_lm_resource *, unsigned long long *,
-		vme_address_t *, vme_cycle_t *);
+	int (*lm_set) (struct vme_lm_resource *, unsigned long long, u32, u32);
+	int (*lm_get) (struct vme_lm_resource *, unsigned long long *, u32 *,
+		u32 *);
 	int (*lm_attach) (struct vme_lm_resource *, int, void (*callback)(int));
 	int (*lm_detach) (struct vme_lm_resource *, int);
 
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index d8dd7846447d..3e8283c2dc73 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -3153,11 +3153,7 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 		break;
 
 	case SIOCGIWNWID:     //0x8b03  support
-	#ifdef  WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-          rc = iwctl_giwnwid(dev, NULL, &(wrq->u.nwid), NULL);
-	#else
-        rc = -EOPNOTSUPP;
-	#endif
+		rc = -EOPNOTSUPP;
 		break;
 
 		// Set frequency/channel
diff --git a/drivers/staging/vt6655/ioctl.c b/drivers/staging/vt6655/ioctl.c
index 432a20993c6e..7fd5cc5a55f6 100644
--- a/drivers/staging/vt6655/ioctl.c
+++ b/drivers/staging/vt6655/ioctl.c
@@ -300,6 +300,10 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EFAULT;
 			break;
 		}
+		if (sList.uItem > (ULONG_MAX - sizeof(SBSSIDList)) / sizeof(SBSSIDItem)) {
+			result = -EINVAL;
+			break;
+		}
 		pList = (PSBSSIDList)kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)), (int)GFP_ATOMIC);
 		if (pList == NULL) {
 			result = -ENOMEM;
@@ -571,6 +575,10 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EFAULT;
 			break;
 		}
+		if (sNodeList.uItem > (ULONG_MAX - sizeof(SNodeList)) / sizeof(SNodeItem)) {
+			result = -EINVAL;
+			break;
+		}
 		pNodeList = (PSNodeList)kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)), (int)GFP_ATOMIC);
 		if (pNodeList == NULL) {
 			result = -ENOMEM;
diff --git a/drivers/staging/vt6655/iwctl.c b/drivers/staging/vt6655/iwctl.c
index 5e425d1476b8..87288db21785 100644
--- a/drivers/staging/vt6655/iwctl.c
+++ b/drivers/staging/vt6655/iwctl.c
@@ -151,18 +151,6 @@ int iwctl_giwname(struct net_device *dev,
 	return 0;
 }
 
-int iwctl_giwnwid(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-                   char *extra)
-{
- 	//wrq->value = 0x100;
-	//wrq->disabled = 0;
-	//wrq->fixed = 1;
-	//return 0;
-  return -EOPNOTSUPP;
-}
-
 /*
  * Wireless Handler : set scan
  */
diff --git a/drivers/staging/vt6655/iwctl.h b/drivers/staging/vt6655/iwctl.h
index 3096de0ba1bd..d224f913a624 100644
--- a/drivers/staging/vt6655/iwctl.h
+++ b/drivers/staging/vt6655/iwctl.h
@@ -79,11 +79,6 @@ int iwctl_giwname(struct net_device *dev,
 			 char *wrq,
 			 char *extra);
 
-int iwctl_giwnwid(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-                   char *extra) ;
-
 int iwctl_giwsens(struct net_device *dev,
 			 struct iw_request_info *info,
 			 struct iw_param *wrq,
diff --git a/drivers/staging/vt6656/80211mgr.c b/drivers/staging/vt6656/80211mgr.c
index fceec4999c36..39f98423dc02 100644
--- a/drivers/staging/vt6656/80211mgr.c
+++ b/drivers/staging/vt6656/80211mgr.c
@@ -224,8 +224,6 @@ vMgrDecodeBeacon(
         }
         pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 + pItem->len);
     }
-
-    return;
 }
 
 
@@ -248,8 +246,6 @@ vMgrEncodeIBSSATIM(
 {
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
     pFrame->len = WLAN_HDR_ADDR3_LEN;
-
-    return;
 }
 
 
@@ -270,8 +266,6 @@ vMgrDecodeIBSSATIM(
     )
 {
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
-
-    return;
 }
 
 
@@ -298,8 +292,6 @@ vMgrEncodeDisassociation(
     pFrame->pwReason = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_DISASSOC_OFF_REASON);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DISASSOC_OFF_REASON + sizeof(*(pFrame->pwReason));
-
-    return;
 }
 
 
@@ -324,8 +316,6 @@ vMgrDecodeDisassociation(
     /* Fixed Fields */
     pFrame->pwReason = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_DISASSOC_OFF_REASON);
-
-    return;
 }
 
 /*+
@@ -352,7 +342,6 @@ vMgrEncodeAssocRequest(
     pFrame->pwListenInterval = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_ASSOCREQ_OFF_LISTEN_INT);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCREQ_OFF_LISTEN_INT + sizeof(*(pFrame->pwListenInterval));
-    return;
 }
 
 
@@ -418,7 +407,6 @@ vMgrDecodeAssocRequest(
         }
         pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 + pItem->len);
     }
-    return;
 }
 
 /*+
@@ -448,8 +436,6 @@ vMgrEncodeAssocResponse(
                             + WLAN_ASSOCRESP_OFF_AID);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_ASSOCRESP_OFF_AID
                   + sizeof(*(pFrame->pwAid));
-
-    return;
 }
 
 
@@ -491,10 +477,8 @@ vMgrDecodeAssocResponse(
     if ((((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
         pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pFrame->pExtSuppRates=[%p].\n", pItem);
-    } else {
+	} else
         pFrame->pExtSuppRates = NULL;
-    }
-    return;
 }
 
 
@@ -524,8 +508,6 @@ vMgrEncodeReassocRequest(
     pFrame->pAddrCurrAP = (PIEEE_ADDR)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                        + WLAN_REASSOCREQ_OFF_CURR_AP);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCREQ_OFF_CURR_AP + sizeof(*(pFrame->pAddrCurrAP));
-
-    return;
 }
 
 
@@ -578,10 +560,9 @@ vMgrDecodeReassocRequest(
                 pFrame->pRSN = (PWLAN_IE_RSN)pItem;
             break;
         case WLAN_EID_RSN_WPA:
-            if (pFrame->pRSNWPA == NULL) {
+		if (pFrame->pRSNWPA == NULL)
                 if (WPAb_Is_RSN((PWLAN_IE_RSN_EXT)pItem) == TRUE)
                     pFrame->pRSNWPA = (PWLAN_IE_RSN_EXT)pItem;
-            }
             break;
 
         case WLAN_EID_EXTSUPP_RATES:
@@ -595,7 +576,6 @@ vMgrDecodeReassocRequest(
         }
         pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 + pItem->len);
     }
-    return;
 }
 
 
@@ -619,7 +599,6 @@ vMgrEncodeProbeRequest(
 {
     pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
     pFrame->len = WLAN_HDR_ADDR3_LEN;
-    return;
 }
 
 /*+
@@ -670,7 +649,6 @@ vMgrDecodeProbeRequest(
 
         pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 +  pItem->len);
     }
-    return;
 }
 
 
@@ -703,8 +681,6 @@ vMgrEncodeProbeResponse(
 
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_PROBERESP_OFF_CAP_INFO +
                   sizeof(*(pFrame->pwCapInfo));
-
-    return;
 }
 
 
@@ -818,7 +794,6 @@ vMgrDecodeProbeResponse(
 
         pItem = (PWLAN_IE)(((PBYTE)pItem) + 2 +  pItem->len);
     }
-    return;
 }
 
 
@@ -848,7 +823,6 @@ vMgrEncodeAuthen(
     pFrame->pwStatus = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_AUTHEN_OFF_STATUS);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_AUTHEN_OFF_STATUS + sizeof(*(pFrame->pwStatus));
-    return;
 }
 
 
@@ -886,7 +860,6 @@ vMgrDecodeAuthen(
 
     if ((((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_CHALLENGE))
         pFrame->pChallenge = (PWLAN_IE_CHALLENGE)pItem;
-    return;
 }
 
 
@@ -912,7 +885,6 @@ vMgrEncodeDeauthen(
     pFrame->pwReason = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_DEAUTHEN_OFF_REASON);
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_DEAUTHEN_OFF_REASON + sizeof(*(pFrame->pwReason));
-    return;
 }
 
 
@@ -937,7 +909,6 @@ vMgrDecodeDeauthen(
     /* Fixed Fields */
     pFrame->pwReason = (PWORD)(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3))
                                + WLAN_DEAUTHEN_OFF_REASON);
-    return;
 }
 
 
@@ -968,7 +939,6 @@ vMgrEncodeReassocResponse(
                             + WLAN_REASSOCRESP_OFF_AID);
 
     pFrame->len = WLAN_HDR_ADDR3_LEN + WLAN_REASSOCRESP_OFF_AID + sizeof(*(pFrame->pwAid));
-    return;
 }
 
 
@@ -1010,5 +980,4 @@ vMgrDecodeReassocResponse(
 
     if ((((PBYTE)pItem) < (pFrame->pBuf + pFrame->len)) && (pItem->byElementID == WLAN_EID_EXTSUPP_RATES))
         pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-    return;
 }
diff --git a/drivers/staging/vt6656/baseband.c b/drivers/staging/vt6656/baseband.c
index 0d11147f91c1..06f27f624db4 100644
--- a/drivers/staging/vt6656/baseband.c
+++ b/drivers/staging/vt6656/baseband.c
@@ -932,30 +932,8 @@ BBvCaculateParameter (
 void
 BBvSetAntennaMode (PSDevice pDevice, BYTE byAntennaMode)
 {
-    //{{ RobertYu: 20041124, ABG Mode, VC1/VC2 define, make the ANT_A, ANT_B inverted
-    /*if ( (pDevice->byRFType == RF_MAXIM2829) ||
-         (pDevice->byRFType == RF_UW2452) ||
-         (pDevice->byRFType == RF_AIROHA7230) ) { // RobertYu: 20041210, 20050104
-
-        switch (byAntennaMode) {
-            case ANT_TXA:
-                byAntennaMode = ANT_TXB;
-                break;
-            case ANT_TXB:
-                byAntennaMode = ANT_TXA;
-                break;
-            case ANT_RXA:
-                byAntennaMode = ANT_RXB;
-                break;
-            case ANT_RXB:
-                byAntennaMode = ANT_RXA;
-                break;
-        }
-    }*/
-
     switch (byAntennaMode) {
         case ANT_TXA:
-            break;
         case ANT_TXB:
             break;
         case ANT_RXA:
@@ -1249,8 +1227,7 @@ void BBvLoopbackOff (PSDevice pDevice)
         // Set the CR33 Bit2 to disable internal Loopback.
         ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x21, &byData);//CR33
         ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x21, (BYTE)(byData & 0xFE));//CR33
-    }
-    else { // OFDM
+	} else { /* OFDM */
         ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0x9A, &byData);//CR154
         ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x9A, (BYTE)(byData & 0xFE));//CR154
     }
@@ -1277,19 +1254,16 @@ BBvSetShortSlotTime (PSDevice pDevice)
 {
     BYTE byBBVGA=0;
 
-    if (pDevice->bShortSlotTime) {
+	if (pDevice->bShortSlotTime)
         pDevice->byBBRxConf &= 0xDF;//1101 1111
-    } else {
+	else
         pDevice->byBBRxConf |= 0x20;//0010 0000
-    }
 
     ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0xE7, &byBBVGA);
-    if (byBBVGA == pDevice->abyBBVGA[0]) {
+	if (byBBVGA == pDevice->abyBBVGA[0])
         pDevice->byBBRxConf |= 0x20;//0010 0000
-    }
 
     ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0A, pDevice->byBBRxConf);
-
 }
 
 
@@ -1299,13 +1273,11 @@ void BBvSetVGAGainOffset(PSDevice pDevice, BYTE byData)
     ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xE7, byData);
 
     // patch for 3253B0 Baseband with Cardbus module
-    if (byData == pDevice->abyBBVGA[0]) {
-        pDevice->byBBRxConf |= 0x20;//0010 0000
-    } else if (pDevice->bShortSlotTime) {
-        pDevice->byBBRxConf &= 0xDF;//1101 1111
-    } else {
-        pDevice->byBBRxConf |= 0x20;//0010 0000
-    }
+	if (pDevice->bShortSlotTime)
+		pDevice->byBBRxConf &= 0xDF; /* 1101 1111 */
+	else
+		pDevice->byBBRxConf |= 0x20; /* 0010 0000 */
+
     ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0A, pDevice->byBBRxConf);//CR10
 }
 
@@ -1365,15 +1337,14 @@ static unsigned long s_ulGetLowSQ3(PSDevice pDevice)
 	unsigned long ulMaxPacket;
 
     ulMaxPacket = pDevice->aulPktNum[RATE_54M];
-    if ( pDevice->aulPktNum[RATE_54M] != 0 ) {
+	if (pDevice->aulPktNum[RATE_54M] != 0)
         ulSQ3 = pDevice->aulSQ3Val[RATE_54M] / pDevice->aulPktNum[RATE_54M];
-    }
-    for ( ii=RATE_48M;ii>=RATE_6M;ii-- ) {
-        if ( pDevice->aulPktNum[ii] > ulMaxPacket ) {
+
+	for (ii = RATE_48M; ii >= RATE_6M; ii--)
+		if (pDevice->aulPktNum[ii] > ulMaxPacket) {
             ulMaxPacket = pDevice->aulPktNum[ii];
             ulSQ3 = pDevice->aulSQ3Val[ii] / pDevice->aulPktNum[ii];
         }
-    }
 
     return ulSQ3;
 }
@@ -1392,7 +1363,7 @@ static unsigned long s_ulGetRatio(PSDevice pDevice)
         ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
         ulRatio += TOP_RATE_54M;
     }
-    for ( ii=RATE_48M;ii>=RATE_1M;ii-- ) {
+	for (ii = RATE_48M; ii >= RATE_1M; ii--)
         if ( pDevice->aulPktNum[ii] > ulMaxPacket ) {
             ulPacketNum = 0;
             for ( jj=RATE_54M;jj>=ii;jj--)
@@ -1402,8 +1373,6 @@ static unsigned long s_ulGetRatio(PSDevice pDevice)
             ulMaxPacket = pDevice->aulPktNum[ii];
         }
 
-    }
-
     return ulRatio;
 }
 
@@ -1589,7 +1558,6 @@ void TimerSQ3CallBack(void *hDeviceContext)
 
 
     spin_unlock_irq(&pDevice->lock);
-    return;
 }
 
 
@@ -1637,7 +1605,6 @@ void TimerSQ3Tmax3CallBack(void *hDeviceContext)
     add_timer(&pDevice->TimerSQ3Tmax1);
 
     spin_unlock_irq(&pDevice->lock);
-    return;
 }
 
 void
diff --git a/drivers/staging/vt6656/bssdb.c b/drivers/staging/vt6656/bssdb.c
index af006df4c8e9..32c67ed8435a 100644
--- a/drivers/staging/vt6656/bssdb.c
+++ b/drivers/staging/vt6656/bssdb.c
@@ -216,26 +216,6 @@ PKnownBSS BSSpSearchBSSList(void *hDeviceContext,
                         continue;
                     }
                 }
-/*
-                if (pMgmt->eAuthenMode < WMAC_AUTH_WPA) {
-                    if (pCurrBSS->bWPAValid == TRUE) {
-                        // WPA AP will reject connection of station without WPA enable.
-                        continue;
-                    }
-                } else if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
-                           (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)) {
-                    if (pCurrBSS->bWPAValid == FALSE) {
-                        // station with WPA enable can't join NonWPA AP.
-                        continue;
-                    }
-                } else if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
-                           (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
-                    if (pCurrBSS->bWPA2Valid == FALSE) {
-                        // station with WPA2 enable can't join NonWPA2 AP.
-                        continue;
-                    }
-                }
-*/
 
         pMgmt->pSameBSS[jj].uChannel = pCurrBSS->uChannel;
         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BSSpSearchBSSList pSelect1[%02X %02X %02X-%02X %02X %02X]\n",*pCurrBSS->abyBSSID,*(pCurrBSS->abyBSSID+1),*(pCurrBSS->abyBSSID+2),*(pCurrBSS->abyBSSID+3),*(pCurrBSS->abyBSSID+4),*(pCurrBSS->abyBSSID+5));
@@ -300,18 +280,11 @@ void BSSvClearBSSList(void *hDeviceContext, BOOL bKeepCurrBSSID)
                 continue;
             }
         }
-/*
-        if ((pMgmt->sBSSList[ii].bActive) && (pMgmt->sBSSList[ii].uClearCount < BSS_CLEAR_COUNT)) {
-             pMgmt->sBSSList[ii].uClearCount ++;
-             continue;
-        }
-*/
-        pMgmt->sBSSList[ii].bActive = FALSE;
+
+	pMgmt->sBSSList[ii].bActive = FALSE;
         memset(&pMgmt->sBSSList[ii], 0, sizeof(KnownBSS));
     }
     BSSvClearAnyBSSJoinRecord(pDevice);
-
-    return;
 }
 
 
@@ -524,46 +497,6 @@ BOOL BSSbInsertToBSSList(void *hDeviceContext,
             pBSSList->ldBmAverage[ii] = 0;
     }
 
-/*
-    if ((pIE_Country != NULL) &&
-        (pMgmt->b11hEnable == TRUE)) {
-        CARDvSetCountryInfo(pMgmt->pAdapter,
-                            pBSSList->eNetworkTypeInUse,
-                            pIE_Country);
-    }
-
-    if ((bParsingQuiet == TRUE) && (pIE_Quiet != NULL)) {
-        if ((((PWLAN_IE_QUIET)pIE_Quiet)->len == 8) &&
-            (((PWLAN_IE_QUIET)pIE_Quiet)->byQuietCount != 0)) {
-            // valid EID
-            if (pQuiet == NULL) {
-                pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-                CARDbSetQuiet(  pMgmt->pAdapter,
-                                TRUE,
-                                pQuiet->byQuietCount,
-                                pQuiet->byQuietPeriod,
-                                *((PWORD)pQuiet->abyQuietDuration),
-                                *((PWORD)pQuiet->abyQuietOffset)
-                                );
-            } else {
-                pQuiet = (PWLAN_IE_QUIET)pIE_Quiet;
-                CARDbSetQuiet(  pMgmt->pAdapter,
-                                FALSE,
-                                pQuiet->byQuietCount,
-                                pQuiet->byQuietPeriod,
-                                *((PWORD)pQuiet->abyQuietDuration),
-                                *((PWORD)pQuiet->abyQuietOffset)
-                                );
-            }
-        }
-    }
-
-    if ((bParsingQuiet == TRUE) &&
-        (pQuiet != NULL)) {
-        CARDbStartQuiet(pMgmt->pAdapter);
-    }
-*/
-
     pBSSList->uIELength = uIELength;
     if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
         pBSSList->uIELength = WLAN_BEACON_FR_MAXLEN;
@@ -609,8 +542,6 @@ BOOL BSSbUpdateToBSSList(void *hDeviceContext,
     PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
     signed long            ldBm, ldBmSum;
     BOOL            bParsingQuiet = FALSE;
-  //  BYTE            abyTmpSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
-
 
     if (pBSSList == NULL)
         return FALSE;
@@ -622,7 +553,6 @@ BOOL BSSbUpdateToBSSList(void *hDeviceContext,
     pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
     pBSSList->uClearCount = 0;
     pBSSList->uChannel = byCurrChannel;
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BSSbUpdateToBSSList: pBSSList->uChannel: %d\n", pBSSList->uChannel);
 
     if (pSSID->len > WLAN_SSID_MAXLEN)
         pSSID->len = WLAN_SSID_MAXLEN;
@@ -809,7 +739,6 @@ void BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex)
     pMgmt->sNodeDBTable[*puNodeIndex].byAuthSequence = 0;
     pMgmt->sNodeDBTable[*puNodeIndex].wEnQueueCnt = 0;
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create node index = %d\n", ii);
-    return;
 };
 
 
@@ -840,8 +769,6 @@ void BSSvRemoveOneNode(void *hDeviceContext, unsigned int uNodeIndex)
     memset(&pMgmt->sNodeDBTable[uNodeIndex], 0, sizeof(KnownNodeDB));
     // clear tx bit map
     pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[uNodeIndex].wAID >> 3] &=  ~byMask[pMgmt->sNodeDBTable[uNodeIndex].wAID & 7];
-
-    return;
 };
 /*+
  *
@@ -1054,10 +981,6 @@ if((pMgmt->eCurrState!=WMAC_STATE_ASSOC) &&
 
             // Rate fallback check
             if (!pDevice->bFixRate) {
-/*
-                if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (ii == 0))
-                    RATEvTxRateFallBack(pDevice, &(pMgmt->sNodeDBTable[ii]));
-*/
                 if (ii > 0) {
                     // ii = 0 for multicast node (AP & Adhoc)
 			RATEvTxRateFallBack((void *)pDevice,
@@ -1152,7 +1075,6 @@ if((pMgmt->eCurrState!=WMAC_STATE_ASSOC) &&
         (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)) {
 
         if (pMgmt->sNodeDBTable[0].bActive) { // Assoc with BSS
-           // DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Callback inactive Count = [%d]\n", pMgmt->sNodeDBTable[0].uInActiveCount);
 
             if (pDevice->bUpdateBBVGA) {
 		/* s_vCheckSensitivity((void *) pDevice); */
@@ -1194,7 +1116,6 @@ if((pMgmt->eCurrState!=WMAC_STATE_ASSOC) &&
              pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
          }
    #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-  // if(pDevice->bWPASuppWextEnabled == TRUE)
       {
 	union iwreq_data  wrqu;
 	memset(&wrqu, 0, sizeof (wrqu));
@@ -1223,7 +1144,6 @@ DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bIsRoaming %d, !\n", pDevice->bIsRoaming );
                 pDevice->uIsroamingTime = 0;
                 pDevice->bRoaming = FALSE;
 
-//            if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
              wpahdr = (viawget_wpa_header *)pDevice->skb->data;
              wpahdr->type = VIAWGET_CCKM_ROAM_MSG;
              wpahdr->resp_ie_len = 0;
@@ -1237,7 +1157,6 @@ DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bIsRoaming %d, !\n", pDevice->bIsRoaming );
              netif_rx(pDevice->skb);
             pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
 
-//         }
           }
       else if ((pDevice->bRoaming == FALSE)&&(pDevice->bIsRoaming == TRUE)) {
                             pDevice->uIsroamingTime++;
@@ -1315,7 +1234,6 @@ else {
 
     pMgmt->sTimerSecondCallback.expires = RUN_AT(HZ);
     add_timer(&pMgmt->sTimerSecondCallback);
-    return;
 }
 
 /*+
@@ -1364,7 +1282,6 @@ void BSSvUpdateNodeTxCounter(void *hDeviceContext,
 
     // Only Unicast using support rates
     if (wFIFOCtl & FIFOCTL_NEEDACK) {
-        //DBG_PRN_GRP21(("Device %08X, wRate %04X, byTSR %02X\n", hDeviceContext, wRate, byTSR));
         if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
             pMgmt->sNodeDBTable[0].uTxAttempts += 1;
             if ( !(byTSR & (TSR_TMO | TSR_RETRYTMO))) {
@@ -1475,10 +1392,6 @@ void BSSvUpdateNodeTxCounter(void *hDeviceContext,
             }
         }
     }
-
-    return;
-
-
 }
 
 /*+
@@ -1519,8 +1432,6 @@ void BSSvClearNodeDBTable(void *hDeviceContext,
             memset(&pMgmt->sNodeDBTable[ii], 0, sizeof(KnownNodeDB));
         }
     }
-
-    return;
 };
 
 void s_vCheckSensitivity(void *hDeviceContext)
@@ -1584,7 +1495,6 @@ RxOkRatio = (RxCnt < 6) ? 2000:((pDevice->scStatistic.RxOkCnt * 2000) / RxCnt);
 //decide link quality
 if(pDevice->bLinkPass !=TRUE)
 {
- //  printk("s_uCalculateLinkQual-->Link disconnect and Poor quality**\n");
    pDevice->scStatistic.LinkQuality = 0;
    pDevice->scStatistic.SignalStren = 0;
 }
@@ -1608,7 +1518,6 @@ else
    pDevice->scStatistic.TxFailCount = 0;
    pDevice->scStatistic.TxNoRetryOkCount = 0;
    pDevice->scStatistic.TxRetryOkCount = 0;
-   return;
 }
 
 void BSSvClearAnyBSSJoinRecord(void *hDeviceContext)
@@ -1617,10 +1526,8 @@ void BSSvClearAnyBSSJoinRecord(void *hDeviceContext)
     PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
     unsigned int            ii;
 
-    for (ii = 0; ii < MAX_BSS_NUM; ii++) {
+	for (ii = 0; ii < MAX_BSS_NUM; ii++)
         pMgmt->sBSSList[ii].bSelected = FALSE;
-    }
-    return;
 }
 
 void s_vCheckPreEDThreshold(void *hDeviceContext)
@@ -1637,6 +1544,5 @@ void s_vCheckPreEDThreshold(void *hDeviceContext)
             BBvUpdatePreEDThreshold(pDevice, FALSE);
         }
     }
-    return;
 }
 
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index a49053bd7c65..9d09e9fd8e18 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -91,15 +91,10 @@ const WORD cwRXBCNTSFOff[MAX_RATE] =
  *      uConnectionChannel  - Channel to be set
  *  Out:
  *      none
- *
- * Return Value: TRUE if succeeded; FALSE if failed.
- *
  */
-BOOL CARDbSetMediaChannel(void *pDeviceHandler, unsigned int uConnectionChannel)
+void CARDbSetMediaChannel(void *pDeviceHandler, unsigned int uConnectionChannel)
 {
 PSDevice            pDevice = (PSDevice) pDeviceHandler;
-BOOL                bResult = TRUE;
-
 
     if (pDevice->byBBType == BB_TYPE_11A) { // 15 ~ 38
         if ((uConnectionChannel < (CB_MAX_CHANNEL_24G+1)) || (uConnectionChannel > CB_MAX_CHANNEL))
@@ -140,7 +135,6 @@ BOOL                bResult = TRUE;
         RFbRawSetPower(pDevice, pDevice->abyCCKPwrTbl[uConnectionChannel-1], RATE_1M);
     }
     ControlvWriteByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_CHANNEL,(BYTE)(uConnectionChannel|0x80));
-    return(bResult);
 }
 
 /*
@@ -607,7 +601,7 @@ BYTE ii;
  * Return Value: TRUE if succeeded; FALSE if failed.
  *
  */
-BOOL CARDbAddBasicRate(void *pDeviceHandler, WORD wRateIdx)
+void CARDbAddBasicRate(void *pDeviceHandler, WORD wRateIdx)
 {
 PSDevice    pDevice = (PSDevice) pDeviceHandler;
 WORD wRate = (WORD)(1<<wRateIdx);
@@ -616,8 +610,6 @@ WORD wRate = (WORD)(1<<wRateIdx);
 
     //Determines the highest basic rate.
     CARDvUpdateBasicTopRate(pDevice);
-
-    return(TRUE);
 }
 
 BOOL CARDbIsOFDMinBasicRate(void *pDeviceHandler)
@@ -1090,7 +1082,7 @@ CARDbChannelSwitch (
 
     if (byCount == 0) {
         pDevice->sMgmtObj.uCurrChannel = byNewChannel;
-        bResult = CARDbSetMediaChannel(pDevice, byNewChannel);
+	CARDbSetMediaChannel(pDevice, byNewChannel);
 
 	return bResult;
     }
diff --git a/drivers/staging/vt6656/card.h b/drivers/staging/vt6656/card.h
index 6c91343d0d14..9cf71a3d8801 100644
--- a/drivers/staging/vt6656/card.h
+++ b/drivers/staging/vt6656/card.h
@@ -60,12 +60,12 @@ typedef enum _CARD_OP_MODE {
 
 /*---------------------  Export Functions  --------------------------*/
 
-BOOL CARDbSetMediaChannel(void *pDeviceHandler,
+void CARDbSetMediaChannel(void *pDeviceHandler,
 			  unsigned int uConnectionChannel);
 void CARDvSetRSPINF(void *pDeviceHandler, BYTE byBBType);
 void vUpdateIFS(void *pDeviceHandler);
 void CARDvUpdateBasicTopRate(void *pDeviceHandler);
-BOOL CARDbAddBasicRate(void *pDeviceHandler, WORD wRateIdx);
+void CARDbAddBasicRate(void *pDeviceHandler, WORD wRateIdx);
 BOOL CARDbIsOFDMinBasicRate(void *pDeviceHandler);
 void CARDvAdjustTSF(void *pDeviceHandler, BYTE byRxRate,
 		    QWORD qwBSSTimestamp, QWORD qwLocalTSF);
diff --git a/drivers/staging/vt6656/int.c b/drivers/staging/vt6656/int.c
index c95833ac58e0..0a114231145f 100644
--- a/drivers/staging/vt6656/int.c
+++ b/drivers/staging/vt6656/int.c
@@ -92,9 +92,8 @@ void INTvWorkItem(void *Context)
 	spin_unlock_irq(&pDevice->lock);
 }
 
-int INTnsProcessData(PSDevice pDevice)
+void INTnsProcessData(PSDevice pDevice)
 {
-	int status = STATUS_SUCCESS;
 	PSINTData	pINTData;
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
 	struct net_device_stats *pStats = &pDevice->stats;
@@ -218,6 +217,4 @@ int INTnsProcessData(PSDevice pDevice)
 			pDevice->scStatistic.ullTxBroadcastBytes;
 	pStats->tx_errors = pDevice->scStatistic.dwTsrErr;
 	pStats->tx_dropped = pDevice->scStatistic.dwTsrErr;
-
-	return status;
 }
diff --git a/drivers/staging/vt6656/int.h b/drivers/staging/vt6656/int.h
index 3176c8d08d6d..a5d96b968176 100644
--- a/drivers/staging/vt6656/int.h
+++ b/drivers/staging/vt6656/int.h
@@ -68,6 +68,6 @@ SINTData, *PSINTData;
 /*---------------------  Export Functions  --------------------------*/
 
 void INTvWorkItem(void *Context);
-int INTnsProcessData(PSDevice pDevice);
+void INTnsProcessData(PSDevice pDevice);
 
 #endif /* __INT_H__ */
diff --git a/drivers/staging/vt6656/ioctl.c b/drivers/staging/vt6656/ioctl.c
index 49390026dea3..1463d76895f0 100644
--- a/drivers/staging/vt6656/ioctl.c
+++ b/drivers/staging/vt6656/ioctl.c
@@ -295,6 +295,10 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EFAULT;
 			break;
 		}
+		if (sList.uItem > (ULONG_MAX - sizeof(SBSSIDList)) / sizeof(SBSSIDItem)) {
+			result = -EINVAL;
+			break;
+		}
 		pList = (PSBSSIDList)kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)), (int)GFP_ATOMIC);
 		if (pList == NULL) {
 			result = -ENOMEM;
@@ -557,6 +561,10 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EFAULT;
 			break;
 		}
+		if (sNodeList.uItem > (ULONG_MAX - sizeof(SNodeList)) / sizeof(SNodeItem)) {
+			result = -ENOMEM;
+			break;
+		}
 		pNodeList = (PSNodeList)kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)), (int)GFP_ATOMIC);
 		if (pNodeList == NULL) {
 			result = -ENOMEM;
diff --git a/drivers/staging/vt6656/iwctl.c b/drivers/staging/vt6656/iwctl.c
index 2121205a912b..ecfda5272fa1 100644
--- a/drivers/staging/vt6656/iwctl.c
+++ b/drivers/staging/vt6656/iwctl.c
@@ -128,17 +128,6 @@ int iwctl_giwname(struct net_device *dev,
 	return 0;
 }
 
-int iwctl_giwnwid(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-                   char *extra)
-{
- 	//wrq->value = 0x100;
-	//wrq->disabled = 0;
-	//wrq->fixed = 1;
-	//return 0;
-  return -EOPNOTSUPP;
-}
 /*
  * Wireless Handler : set scan
  */
@@ -1939,7 +1928,6 @@ static const iw_handler		iwctl_handler[] =
 	(iw_handler) iwctl_commit,      // SIOCSIWCOMMIT
 	(iw_handler) iwctl_giwname,     // SIOCGIWNAME
 	(iw_handler) NULL,				// SIOCSIWNWID
-	(iw_handler) NULL,				// SIOCGIWNWID
 	(iw_handler) iwctl_siwfreq,		// SIOCSIWFREQ
 	(iw_handler) iwctl_giwfreq,		// SIOCGIWFREQ
 	(iw_handler) iwctl_siwmode,		// SIOCSIWMODE
diff --git a/drivers/staging/vt6656/iwctl.h b/drivers/staging/vt6656/iwctl.h
index cc48954783fc..10a240e65012 100644
--- a/drivers/staging/vt6656/iwctl.h
+++ b/drivers/staging/vt6656/iwctl.h
@@ -77,11 +77,6 @@ int iwctl_giwname(struct net_device *dev,
 			 char *wrq,
 			 char *extra);
 
-int iwctl_giwnwid(struct net_device *dev,
-             struct iw_request_info *info,
-			 struct iw_param *wrq,
-                   char *extra) ;
-
 int iwctl_giwsens(struct net_device *dev,
 			 struct iw_request_info *info,
 			 struct iw_param *wrq,
diff --git a/drivers/staging/vt6656/mac.c b/drivers/staging/vt6656/mac.c
index 26c19d1408c4..af4a29d14775 100644
--- a/drivers/staging/vt6656/mac.c
+++ b/drivers/staging/vt6656/mac.c
@@ -133,10 +133,9 @@ void MACvWriteMultiAddr(PSDevice pDevice, unsigned int uByteIdx, BYTE byData)
  *  Out:
  *      none
  *
- * Return Value: TRUE if success; otherwise FALSE
  *
  */
-BOOL MACbShutdown (PSDevice pDevice)
+void MACbShutdown(PSDevice pDevice)
 {
     CONTROLnsRequestOutAsyn(pDevice,
                         MESSAGE_TYPE_MACSHUTDOWN,
@@ -145,7 +144,6 @@ BOOL MACbShutdown (PSDevice pDevice)
                         0,
                         NULL
                         );
-    return TRUE;
 }
 
 void MACvSetBBType(PSDevice pDevice,BYTE byType)
diff --git a/drivers/staging/vt6656/mac.h b/drivers/staging/vt6656/mac.h
index 491ff5ecd04b..147ac50218d3 100644
--- a/drivers/staging/vt6656/mac.h
+++ b/drivers/staging/vt6656/mac.h
@@ -422,7 +422,7 @@
 
 void MACvSetMultiAddrByHash(PSDevice pDevice, BYTE byHashIdx);
 void MACvWriteMultiAddr(PSDevice pDevice, unsigned int uByteIdx, BYTE byData);
-BOOL MACbShutdown(PSDevice pDevice);
+void MACbShutdown(PSDevice pDevice);
 void MACvSetBBType(PSDevice pDevice, BYTE byType);
 void MACvSetMISCFifo(PSDevice pDevice, WORD wOffset, DWORD dwData);
 void MACvDisableKeyEntry(PSDevice pDevice, unsigned int uEntryIdx);
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 541f9aa8ef6d..6a708f447651 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -900,7 +900,7 @@ static BOOL device_alloc_bufs(PSDevice pDevice) {
     }
 
     // allocate rcb mem
-    pDevice->pRCBMem = kmalloc((sizeof(RCB) * pDevice->cbRD), GFP_KERNEL);
+	pDevice->pRCBMem = kzalloc((sizeof(RCB) * pDevice->cbRD), GFP_KERNEL);
     if (pDevice->pRCBMem == NULL) {
         DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s : alloc rx usb context failed\n", pDevice->dev->name);
         goto free_tx;
@@ -912,7 +912,6 @@ static BOOL device_alloc_bufs(PSDevice pDevice) {
     pDevice->FirstRecvMngList = NULL;
     pDevice->LastRecvMngList = NULL;
     pDevice->NumRecvFreeList = 0;
-    memset(pDevice->pRCBMem, 0, (sizeof(RCB) * pDevice->cbRD));
     pRCB = (PRCB) pDevice->pRCBMem;
 
     for (ii = 0; ii < pDevice->cbRD; ii++) {
@@ -1618,15 +1617,8 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 		break;
 
 	case SIOCSIWNWID:
-        rc = -EOPNOTSUPP;
-		break;
-
 	case SIOCGIWNWID:     //0x8b03  support
-	#ifdef  WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-          rc = iwctl_giwnwid(dev, NULL, &(wrq->u.nwid), NULL);
-	#else
-        rc = -EOPNOTSUPP;
-	#endif
+		rc = -EOPNOTSUPP;
 		break;
 
 		// Set frequency/channel
diff --git a/drivers/staging/wlags49_h2/wl_pci.c b/drivers/staging/wlags49_h2/wl_pci.c
index 1f1d98679171..2bd9b84ace8e 100644
--- a/drivers/staging/wlags49_h2/wl_pci.c
+++ b/drivers/staging/wlags49_h2/wl_pci.c
@@ -112,17 +112,10 @@ extern dbg_info_t *DbgInfo;
 #endif  // DBG
 
 /* define the PCI device Table Cardname and id tables */
-enum hermes_pci_versions {
-	CH_Agere_Systems_Mini_PCI_V1 = 0,
-};
-
 static struct pci_device_id wl_pci_tbl[] __devinitdata = {
-	{ PCI_VENDOR_ID_WL_LKM, PCI_DEVICE_ID_WL_LKM_0,
-	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, CH_Agere_Systems_Mini_PCI_V1 },
-	{ PCI_VENDOR_ID_WL_LKM, PCI_DEVICE_ID_WL_LKM_1,
-	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, CH_Agere_Systems_Mini_PCI_V1 },
-	{ PCI_VENDOR_ID_WL_LKM, PCI_DEVICE_ID_WL_LKM_2,
-	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, CH_Agere_Systems_Mini_PCI_V1 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_WL_LKM, PCI_DEVICE_ID_WL_LKM_0), },
+	{ PCI_DEVICE(PCI_VENDOR_ID_WL_LKM, PCI_DEVICE_ID_WL_LKM_1), },
+	{ PCI_DEVICE(PCI_VENDOR_ID_WL_LKM, PCI_DEVICE_ID_WL_LKM_2), },
 
 	{ }			/* Terminating entry */
 };
diff --git a/drivers/staging/xgifb/Makefile b/drivers/staging/xgifb/Makefile
index 3c8c7de9eadd..55e519905346 100644
--- a/drivers/staging/xgifb/Makefile
+++ b/drivers/staging/xgifb/Makefile
@@ -1,4 +1,4 @@
 obj-$(CONFIG_FB_XGI)  += xgifb.o
 
-xgifb-y := XGI_main_26.o vb_init.o vb_setmode.o vb_util.o vb_ext.o
+xgifb-y := XGI_main_26.o vb_init.o vb_setmode.o vb_util.o
 
diff --git a/drivers/staging/xgifb/XGI_main.h b/drivers/staging/xgifb/XGI_main.h
index 71aebe3e84d8..35f7b2a485e1 100644
--- a/drivers/staging/xgifb/XGI_main.h
+++ b/drivers/staging/xgifb/XGI_main.h
@@ -32,14 +32,10 @@
 #endif
 
 static DEFINE_PCI_DEVICE_TABLE(xgifb_pci_table) = {
-	{PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_20, PCI_ANY_ID, PCI_ANY_ID,
-	 0, 0, 0},
-	{PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_27, PCI_ANY_ID, PCI_ANY_ID,
-	 0, 0, 1},
-	{PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_40, PCI_ANY_ID, PCI_ANY_ID,
-	 0, 0, 2},
-	{PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_42, PCI_ANY_ID, PCI_ANY_ID,
-	 0, 0, 3},
+	{PCI_DEVICE(PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_20)},
+	{PCI_DEVICE(PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_27)},
+	{PCI_DEVICE(PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_40)},
+	{PCI_DEVICE(PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_42)},
 	{0}
 };
 
@@ -128,7 +124,6 @@ MODULE_DEVICE_TABLE(pci, xgifb_pci_table);
 /* display status */
 static int XGIfb_crt1off;
 static int XGIfb_forcecrt1 = -1;
-static int XGIfb_userom ;
 
 /* global flags */
 static int XGIfb_tvmode;
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 277e408c39c5..2502c49c9c5b 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -21,7 +21,6 @@
 #include <linux/ioport.h>
 #include <linux/init.h>
 #include <linux/pci.h>
-#include <linux/vmalloc.h>
 #include <linux/vt_kern.h>
 #include <linux/capability.h>
 #include <linux/fs.h>
@@ -46,8 +45,7 @@
 #define GPIOG_EN    (1<<6)
 #define GPIOG_READ  (1<<1)
 
-#define XGIFB_ROM_SIZE	65536
-
+static char *forcecrt2type;
 static char *mode;
 static int vesa = -1;
 static unsigned int refresh_rate;
@@ -159,7 +157,6 @@ static int XGIfb_mode_rate_to_dclock(struct vb_device_info *XGI_Pr,
 
 	/* unsigned long  temp = 0; */
 	int Clock;
-	XGI_Pr->ROMAddr = HwDeviceExtension->pjVirtualRomBase;
 	InitTo330Pointer(HwDeviceExtension->jChipType, XGI_Pr);
 
 	RefreshRateTableIndex = XGI_GetRatePtrCRT2(HwDeviceExtension, ModeNo,
@@ -199,7 +196,6 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 	unsigned char sr_data, cr_data, cr_data2;
 	unsigned long cr_data3;
 	int A, B, C, D, E, F, temp, j;
-	XGI_Pr->ROMAddr = HwDeviceExtension->pjVirtualRomBase;
 	InitTo330Pointer(HwDeviceExtension->jChipType, XGI_Pr);
 	RefreshRateTableIndex = XGI_GetRatePtrCRT2(HwDeviceExtension, ModeNo,
 			ModeIdIndex, XGI_Pr);
@@ -387,7 +383,7 @@ static void XGIRegInit(struct vb_device_info *XGI_Pr, unsigned long BaseAddr)
 
 /* ------------------ Internal helper routines ----------------- */
 
-static int XGIfb_GetXG21DefaultLVDSModeIdx(void)
+static int XGIfb_GetXG21DefaultLVDSModeIdx(struct xgifb_video_info *xgifb_info)
 {
 
 	int found_mode = 0;
@@ -396,11 +392,11 @@ static int XGIfb_GetXG21DefaultLVDSModeIdx(void)
 	found_mode = 0;
 	while ((XGIbios_mode[XGIfb_mode_idx].mode_no != 0)
 			&& (XGIbios_mode[XGIfb_mode_idx].xres
-					<= XGI21_LCDCapList[0].LVDSHDE)) {
+					<= xgifb_info->lvds_data.LVDSHDE)) {
 		if ((XGIbios_mode[XGIfb_mode_idx].xres
-				== XGI21_LCDCapList[0].LVDSHDE)
+				== xgifb_info->lvds_data.LVDSHDE)
 				&& (XGIbios_mode[XGIfb_mode_idx].yres
-						== XGI21_LCDCapList[0].LVDSVDE)
+					== xgifb_info->lvds_data.LVDSVDE)
 				&& (XGIbios_mode[XGIfb_mode_idx].bpp == 8)) {
 			found_mode = 1;
 			break;
@@ -456,51 +452,6 @@ invalid:
 		printk(KERN_INFO "XGIfb: Invalid VESA mode 0x%x'\n", vesamode);
 }
 
-static int XGIfb_GetXG21LVDSData(struct xgifb_video_info *xgifb_info)
-{
-	u8 tmp;
-	void __iomem *data = xgifb_info->mmio_vbase + 0x20000;
-	int i, j, k;
-
-	tmp = xgifb_reg_get(XGISR, 0x1e);
-	xgifb_reg_set(XGISR, 0x1e, tmp | 4);
-
-	if ((readb(data) == 0x55) &&
-	    (readb(data + 1) == 0xAA) &&
-	    (readb(data + 0x65) & 0x1)) {
-		i = readw(data + 0x316);
-		j = readb(data + i - 1);
-		if (j == 0xff)
-			j = 1;
-
-		k = 0;
-		do {
-			XGI21_LCDCapList[k].LVDS_Capability = readw(data + i);
-			XGI21_LCDCapList[k].LVDSHT = readw(data + i + 2);
-			XGI21_LCDCapList[k].LVDSVT = readw(data + i + 4);
-			XGI21_LCDCapList[k].LVDSHDE = readw(data + i + 6);
-			XGI21_LCDCapList[k].LVDSVDE = readw(data + i + 8);
-			XGI21_LCDCapList[k].LVDSHFP = readw(data + i + 10);
-			XGI21_LCDCapList[k].LVDSVFP = readw(data + i + 12);
-			XGI21_LCDCapList[k].LVDSHSYNC = readw(data + i + 14);
-			XGI21_LCDCapList[k].LVDSVSYNC = readw(data + i + 16);
-			XGI21_LCDCapList[k].VCLKData1 = readb(data + i + 18);
-			XGI21_LCDCapList[k].VCLKData2 = readb(data + i + 19);
-			XGI21_LCDCapList[k].PSC_S1 = readb(data + i + 20);
-			XGI21_LCDCapList[k].PSC_S2 = readb(data + i + 21);
-			XGI21_LCDCapList[k].PSC_S3 = readb(data + i + 22);
-			XGI21_LCDCapList[k].PSC_S4 = readb(data + i + 23);
-			XGI21_LCDCapList[k].PSC_S5 = readb(data + i + 24);
-			i += 25;
-			j--;
-			k++;
-		} while ((j > 0) && (k < (sizeof(XGI21_LCDCapList)
-				/ sizeof(struct XGI21_LVDSCapStruct))));
-		return 1;
-	}
-	return 0;
-}
-
 static int XGIfb_validate_mode(struct xgifb_video_info *xgifb_info, int myindex)
 {
 	u16 xres, yres;
@@ -508,8 +459,8 @@ static int XGIfb_validate_mode(struct xgifb_video_info *xgifb_info, int myindex)
 
 	if (xgifb_info->chip == XG21) {
 		if (xgifb_info->display2 == XGIFB_DISP_LCD) {
-			xres = XGI21_LCDCapList[0].LVDSHDE;
-			yres = XGI21_LCDCapList[0].LVDSVDE;
+			xres = xgifb_info->lvds_data.LVDSHDE;
+			yres = xgifb_info->lvds_data.LVDSVDE;
 			if (XGIbios_mode[myindex].xres > xres)
 				return -1;
 			if (XGIbios_mode[myindex].yres > yres)
@@ -1223,7 +1174,7 @@ static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
 	if (isactive) {
 
 		XGIfb_pre_setmode(xgifb_info);
-		if (XGISetModeNew(hw_info,
+		if (XGISetModeNew(xgifb_info, hw_info,
 				  XGIbios_mode[xgifb_info->mode_idx].mode_no)
 					== 0) {
 			printk(KERN_ERR "XGIfb: Setting mode[0x%x] failed\n",
@@ -1794,17 +1745,16 @@ static void XGIfb_detect_VB(struct xgifb_video_info *xgifb_info)
 			XGIfb_crt1off = 0;
 	}
 
-	if (XGIfb_crt2type != -1)
-		/* TW: Override with option */
-		xgifb_info->display2 = XGIfb_crt2type;
-	else if (cr32 & XGI_VB_TV)
-		xgifb_info->display2 = XGIFB_DISP_TV;
-	else if (cr32 & XGI_VB_LCD)
-		xgifb_info->display2 = XGIFB_DISP_LCD;
-	else if (cr32 & XGI_VB_CRT2)
-		xgifb_info->display2 = XGIFB_DISP_CRT;
-	else
-		xgifb_info->display2 = XGIFB_DISP_NONE;
+	if (!xgifb_info->display2_force) {
+		if (cr32 & XGI_VB_TV)
+			xgifb_info->display2 = XGIFB_DISP_TV;
+		else if (cr32 & XGI_VB_LCD)
+			xgifb_info->display2 = XGIFB_DISP_LCD;
+		else if (cr32 & XGI_VB_CRT2)
+			xgifb_info->display2 = XGIFB_DISP_CRT;
+		else
+			xgifb_info->display2 = XGIFB_DISP_NONE;
+	}
 
 	if (XGIfb_tvplug != -1)
 		/* PR/TW: Override with option */
@@ -1925,8 +1875,6 @@ static int __init XGIfb_setup(char *options)
 			XGIfb_crt2type = XGIFB_DISP_LCD;
 		} else if (!strncmp(this_opt, "noypan", 6)) {
 			XGIfb_ypan = 0;
-		} else if (!strncmp(this_opt, "userom:", 7)) {
-			XGIfb_userom = xgifb_optval(this_opt, 7);
 		} else {
 			mode = this_opt;
 		}
@@ -1934,35 +1882,12 @@ static int __init XGIfb_setup(char *options)
 	return 0;
 }
 
-static unsigned char *xgifb_copy_rom(struct pci_dev *dev)
-{
-	void __iomem *rom_address;
-	unsigned char *rom_copy;
-	size_t rom_size;
-
-	rom_address = pci_map_rom(dev, &rom_size);
-	if (rom_address == NULL)
-		return NULL;
-
-	rom_copy = vzalloc(XGIFB_ROM_SIZE);
-	if (rom_copy == NULL)
-		goto done;
-
-	rom_size = min_t(size_t, rom_size, XGIFB_ROM_SIZE);
-	memcpy_fromio(rom_copy, rom_address, rom_size);
-
-done:
-	pci_unmap_rom(dev, rom_address);
-	return rom_copy;
-}
-
 static int __devinit xgifb_probe(struct pci_dev *pdev,
 		const struct pci_device_id *ent)
 {
 	u8 reg, reg1;
 	u8 CR48, CR38;
 	int ret;
-	bool xgi21_drvlcdcaplist = false;
 	struct fb_info *fb_info;
 	struct xgifb_video_info *xgifb_info;
 	struct xgi_hw_device_info *hw_info;
@@ -2001,6 +1926,11 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		goto error;
 	}
 
+	if (XGIfb_crt2type != -1) {
+		xgifb_info->display2 = XGIfb_crt2type;
+		xgifb_info->display2_force = true;
+	}
+
 	XGIRegInit(&xgifb_info->dev_info, (unsigned long)hw_info->pjIOAddress);
 
 	xgifb_reg_set(XGISR, IND_XGI_PASSWORD, XGI_PASSWORD);
@@ -2041,18 +1971,6 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 	printk("XGIfb:chipid = %x\n", xgifb_info->chip);
 	hw_info->jChipType = xgifb_info->chip;
 
-	if ((xgifb_info->chip == XG21) || (XGIfb_userom)) {
-		hw_info->pjVirtualRomBase = xgifb_copy_rom(pdev);
-		if (hw_info->pjVirtualRomBase)
-			printk(KERN_INFO "XGIfb: Video ROM found and mapped to %p\n",
-			       hw_info->pjVirtualRomBase);
-		else
-			printk(KERN_INFO "XGIfb: Video ROM not found\n");
-	} else {
-		hw_info->pjVirtualRomBase = NULL;
-		printk(KERN_INFO "XGIfb: Video ROM usage disabled\n");
-	}
-
 	if (XGIfb_get_dram_size(xgifb_info)) {
 		printk(KERN_INFO "XGIfb: Fatal error: Unable to determine RAM size.\n");
 		ret = -ENODEV;
@@ -2117,8 +2035,6 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		CR38 = xgifb_reg_get(XGICR, 0x38);
 		if ((CR38&0xE0) == 0xC0) {
 			xgifb_info->display2 = XGIFB_DISP_LCD;
-			if (!XGIfb_GetXG21LVDSData(xgifb_info))
-				xgi21_drvlcdcaplist = true;
 		} else if ((CR38&0xE0) == 0x60) {
 			xgifb_info->hasVB = HASVB_CHRONTEL;
 		} else {
@@ -2193,6 +2109,8 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 
 	if (xgifb_info->hasVB != HASVB_NONE)
 		XGIfb_detect_VB(xgifb_info);
+	else if (xgifb_info->chip != XG21)
+		xgifb_info->display2 = XGIFB_DISP_NONE;
 
 	if (xgifb_info->display2 == XGIFB_DISP_LCD) {
 		if (!enable_dstn) {
@@ -2254,7 +2172,7 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		if (xgifb_info->display2 == XGIFB_DISP_LCD &&
 		    xgifb_info->chip == XG21)
 			xgifb_info->mode_idx =
-				XGIfb_GetXG21DefaultLVDSModeIdx();
+				XGIfb_GetXG21DefaultLVDSModeIdx(xgifb_info);
 		else
 			xgifb_info->mode_idx = DEFAULT_MODE;
 	}
@@ -2264,21 +2182,6 @@ static int __devinit xgifb_probe(struct pci_dev *pdev,
 		goto error_1;
 	}
 
-	if (xgi21_drvlcdcaplist) {
-		int m;
-
-		for (m = 0; m < ARRAY_SIZE(XGI21_LCDCapList); m++)
-			if ((XGI21_LCDCapList[m].LVDSHDE ==
-				XGIbios_mode[xgifb_info->mode_idx].xres) &&
-			    (XGI21_LCDCapList[m].LVDSVDE ==
-				XGIbios_mode[xgifb_info->mode_idx].yres)) {
-				xgifb_reg_set(xgifb_info->dev_info.P3d4,
-					      0x36,
-					      m);
-				break;
-			}
-	}
-
 	/* yilin set default refresh rate */
 	xgifb_info->refresh_rate = refresh_rate;
 	if (xgifb_info->refresh_rate == 0)
@@ -2418,7 +2321,6 @@ error_1:
 error_0:
 	release_mem_region(xgifb_info->video_base, xgifb_info->video_size);
 error:
-	vfree(hw_info->pjVirtualRomBase);
 	framebuffer_release(fb_info);
 	return ret;
 }
@@ -2442,7 +2344,6 @@ static void __devexit xgifb_remove(struct pci_dev *pdev)
 	iounmap(xgifb_info->video_vbase);
 	release_mem_region(xgifb_info->mmio_base, xgifb_info->mmio_size);
 	release_mem_region(xgifb_info->video_base, xgifb_info->video_size);
-	vfree(xgifb_info->hw_info.pjVirtualRomBase);
 	framebuffer_release(fb_info);
 	pci_set_drvdata(pdev, NULL);
 }
@@ -2458,6 +2359,8 @@ static int __init xgifb_init(void)
 {
 	char *option = NULL;
 
+	if (forcecrt2type != NULL)
+		XGIfb_search_crt2type(forcecrt2type);
 	if (fb_get_options("xgifb", &option))
 		return -ENODEV;
 	XGIfb_setup(option);
@@ -2480,6 +2383,11 @@ MODULE_AUTHOR("XGITECH , Others");
 module_param(mode, charp, 0);
 module_param(vesa, int, 0);
 module_param(filter, int, 0);
+module_param(forcecrt2type, charp, 0);
+
+MODULE_PARM_DESC(forcecrt2type,
+	"\nForce the second display output type. Possible values are NONE,\n"
+	"LCD, TV, VGA, SVIDEO or COMPOSITE.\n");
 
 MODULE_PARM_DESC(mode,
 	"\nSelects the desired default display mode in the format XxYxDepth,\n"
diff --git a/drivers/staging/xgifb/XGIfb.h b/drivers/staging/xgifb/XGIfb.h
index 7611846a7039..2c866bb65a00 100644
--- a/drivers/staging/xgifb/XGIfb.h
+++ b/drivers/staging/xgifb/XGIfb.h
@@ -86,10 +86,13 @@ struct xgifb_video_info {
 	unsigned int refresh_rate;
 
 	enum xgifb_display_type display2; /* the second display output type */
+	bool display2_force;
 	unsigned char hasVB;
 	unsigned char TV_type;
 	unsigned char TV_plug;
 
+	struct XGI21_LVDSCapStruct lvds_data;
+
 	enum XGI_CHIP_TYPE chip;
 	unsigned char revision_id;
 
diff --git a/drivers/staging/xgifb/vb_ext.c b/drivers/staging/xgifb/vb_ext.c
deleted file mode 100644
index b1a25730b7ca..000000000000
--- a/drivers/staging/xgifb/vb_ext.c
+++ /dev/null
@@ -1,444 +0,0 @@
-#include <linux/io.h>
-#include <linux/types.h>
-#include "XGIfb.h"
-
-#include "vb_def.h"
-#include "vgatypes.h"
-#include "vb_struct.h"
-#include "vb_util.h"
-#include "vb_setmode.h"
-#include "vb_ext.h"
-
-/**************************************************************
- *********************** Dynamic Sense ************************
- *************************************************************/
-
-static unsigned char XGINew_Is301B(struct vb_device_info *pVBInfo)
-{
-	unsigned short flag;
-
-	flag = xgifb_reg_get(pVBInfo->Part4Port, 0x01);
-
-	if (flag > 0x0B0)
-		return 0; /* 301b */
-	else
-		return 1;
-}
-
-static unsigned char XGINew_Sense(unsigned short tempbx,
-				  unsigned short tempcx,
-				  struct vb_device_info *pVBInfo)
-{
-	unsigned short temp, i, tempch;
-
-	temp = tempbx & 0xFF;
-	xgifb_reg_set(pVBInfo->Part4Port, 0x11, temp);
-	temp = (tempbx & 0xFF00) >> 8;
-	temp |= (tempcx & 0x00FF);
-	xgifb_reg_and_or(pVBInfo->Part4Port, 0x10, ~0x1F, temp);
-
-	for (i = 0; i < 10; i++)
-		XGI_LongWait(pVBInfo);
-
-	tempch = (tempcx & 0x7F00) >> 8;
-	temp = xgifb_reg_get(pVBInfo->Part4Port, 0x03);
-	temp = temp ^ (0x0E);
-	temp &= tempch;
-
-	if (temp > 0)
-		return 1;
-	else
-		return 0;
-}
-
-static unsigned char
-XGINew_GetLCDDDCInfo(struct xgi_hw_device_info *HwDeviceExtension,
-		     struct vb_device_info *pVBInfo)
-{
-	unsigned short temp;
-
-	/* add lcd sense */
-	if (HwDeviceExtension->ulCRT2LCDType == LCD_UNKNOWN) {
-		return 0;
-	} else {
-		temp = (unsigned short) HwDeviceExtension->ulCRT2LCDType;
-		switch (HwDeviceExtension->ulCRT2LCDType) {
-		case LCD_INVALID:
-		case LCD_800x600:
-		case LCD_1024x768:
-		case LCD_1280x1024:
-			break;
-
-		case LCD_640x480:
-		case LCD_1024x600:
-		case LCD_1152x864:
-		case LCD_1280x960:
-		case LCD_1152x768:
-			temp = 0;
-			break;
-
-		case LCD_1400x1050:
-		case LCD_1280x768:
-		case LCD_1600x1200:
-			break;
-
-		case LCD_1920x1440:
-		case LCD_2048x1536:
-			temp = 0;
-			break;
-
-		default:
-			break;
-		}
-		xgifb_reg_and_or(pVBInfo->P3d4, 0x36, 0xF0, temp);
-		return 1;
-	}
-}
-
-static unsigned char XGINew_GetPanelID(struct vb_device_info *pVBInfo)
-{
-	unsigned short PanelTypeTable[16] = { SyncNN | PanelRGB18Bit
-			| Panel800x600  | _PanelType00, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType01, SyncNN | PanelRGB18Bit
-			| Panel800x600  | _PanelType02, SyncNN | PanelRGB18Bit
-			| Panel640x480  | _PanelType03, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType04, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType05, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType06, SyncNN | PanelRGB24Bit
-			| Panel1024x768 | _PanelType07, SyncNN | PanelRGB18Bit
-			| Panel800x600  | _PanelType08, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType09, SyncNN | PanelRGB18Bit
-			| Panel800x600  | _PanelType0A, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType0B, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType0C, SyncNN | PanelRGB24Bit
-			| Panel1024x768 | _PanelType0D, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType0E, SyncNN | PanelRGB18Bit
-			| Panel1024x768 | _PanelType0F };
-	unsigned short tempax, tempbx, temp;
-	/* unsigned short return_flag; */
-
-	tempax = xgifb_reg_get(pVBInfo->P3c4, 0x1A);
-	tempbx = tempax & 0x1E;
-
-	if (tempax == 0)
-		return 0;
-	else {
-		/*
-		if (!(tempax & 0x10)) {
-			if (pVBInfo->IF_DEF_LVDS == 1) {
-				tempbx = 0;
-				temp = xgifb_reg_get(pVBInfo->P3c4, 0x38);
-				if (temp & 0x40)
-					tempbx |= 0x08;
-				if (temp & 0x20)
-					tempbx |= 0x02;
-				if (temp & 0x01)
-					tempbx |= 0x01;
-
-				temp = xgifb_reg_get(pVBInfo->P3c4, 0x39);
-				if (temp & 0x80)
-					tempbx |= 0x04;
-			 } else {
-				return(0);
-			 }
-		}
-		*/
-
-		tempbx = tempbx >> 1;
-		temp = tempbx & 0x00F;
-		xgifb_reg_set(pVBInfo->P3d4, 0x36, temp);
-		tempbx--;
-		tempbx = PanelTypeTable[tempbx];
-
-		temp = (tempbx & 0xFF00) >> 8;
-		xgifb_reg_and_or(pVBInfo->P3d4, 0x37, ~(LCDSyncBit
-				| LCDRGB18Bit), temp);
-		return 1;
-	}
-}
-
-static unsigned char
-XGINew_BridgeIsEnable(struct xgi_hw_device_info *HwDeviceExtension,
-		      struct vb_device_info *pVBInfo)
-{
-	unsigned short flag;
-
-	if (XGI_BridgeIsOn(pVBInfo) == 0) {
-		flag = xgifb_reg_get(pVBInfo->Part1Port, 0x0);
-
-		if (flag & 0x050)
-			return 1;
-		else
-			return 0;
-
-	}
-	return 0;
-}
-
-static unsigned char
-XGINew_SenseHiTV(struct xgi_hw_device_info *HwDeviceExtension,
-		 struct vb_device_info *pVBInfo)
-{
-	unsigned short tempbx, tempcx, temp, i, tempch;
-
-	tempbx = *pVBInfo->pYCSenseData2;
-
-	tempcx = 0x0604;
-
-	temp = tempbx & 0xFF;
-	xgifb_reg_set(pVBInfo->Part4Port, 0x11, temp);
-	temp = (tempbx & 0xFF00) >> 8;
-	temp |= (tempcx & 0x00FF);
-	xgifb_reg_and_or(pVBInfo->Part4Port, 0x10, ~0x1F, temp);
-
-	for (i = 0; i < 10; i++)
-		XGI_LongWait(pVBInfo);
-
-	tempch = (tempcx & 0xFF00) >> 8;
-	temp = xgifb_reg_get(pVBInfo->Part4Port, 0x03);
-	temp = temp ^ (0x0E);
-	temp &= tempch;
-
-	if (temp != tempch)
-		return 0;
-
-	tempbx = *pVBInfo->pVideoSenseData2;
-
-	tempcx = 0x0804;
-	temp = tempbx & 0xFF;
-	xgifb_reg_set(pVBInfo->Part4Port, 0x11, temp);
-	temp = (tempbx & 0xFF00) >> 8;
-	temp |= (tempcx & 0x00FF);
-	xgifb_reg_and_or(pVBInfo->Part4Port, 0x10, ~0x1F, temp);
-
-	for (i = 0; i < 10; i++)
-		XGI_LongWait(pVBInfo);
-
-	tempch = (tempcx & 0xFF00) >> 8;
-	temp = xgifb_reg_get(pVBInfo->Part4Port, 0x03);
-	temp = temp ^ (0x0E);
-	temp &= tempch;
-
-	if (temp != tempch) {
-		return 0;
-	} else {
-		tempbx = 0x3FF;
-		tempcx = 0x0804;
-		temp = tempbx & 0xFF;
-		xgifb_reg_set(pVBInfo->Part4Port, 0x11, temp);
-		temp = (tempbx & 0xFF00) >> 8;
-		temp |= (tempcx & 0x00FF);
-		xgifb_reg_and_or(pVBInfo->Part4Port, 0x10, ~0x1F, temp);
-
-		for (i = 0; i < 10; i++)
-			XGI_LongWait(pVBInfo);
-
-		tempch = (tempcx & 0xFF00) >> 8;
-		temp = xgifb_reg_get(pVBInfo->Part4Port, 0x03);
-		temp = temp ^ (0x0E);
-		temp &= tempch;
-
-		if (temp != tempch)
-			return 1;
-		else
-			return 0;
-	}
-}
-
-void XGI_GetSenseStatus(struct xgi_hw_device_info *HwDeviceExtension,
-			struct vb_device_info *pVBInfo)
-{
-	unsigned short  tempax = 0, tempbx, tempcx, temp,
-			P2reg0 = 0, SenseModeNo = 0,
-			OutputSelect = *pVBInfo->pOutputSelect,
-			ModeIdIndex, i;
-	pVBInfo->BaseAddr = (unsigned long) HwDeviceExtension->pjIOAddress;
-
-	if (pVBInfo->IF_DEF_LVDS == 1) {
-		/* ynlai 02/27/2002 */
-		tempax = xgifb_reg_get(pVBInfo->P3c4, 0x1A);
-		tempbx = xgifb_reg_get(pVBInfo->P3c4, 0x1B);
-		tempax = ((tempax & 0xFE) >> 1) | (tempbx << 8);
-		if (tempax == 0x00) { /* Get Panel id from DDC */
-			temp = XGINew_GetLCDDDCInfo(HwDeviceExtension, pVBInfo);
-			if (temp == 1) { /* LCD connect */
-				/* set CR39 bit0="1" */
-				xgifb_reg_and_or(pVBInfo->P3d4,
-						 0x39, 0xFF, 0x01);
-				/* clean CR37 bit4="0" */
-				xgifb_reg_and_or(pVBInfo->P3d4,
-						 0x37, 0xEF, 0x00);
-				temp = LCDSense;
-			} else { /* LCD don't connect */
-				temp = 0;
-			}
-		} else {
-			XGINew_GetPanelID(pVBInfo);
-			temp = LCDSense;
-		}
-
-		tempbx = ~(LCDSense | AVIDEOSense | SVIDEOSense);
-		xgifb_reg_and_or(pVBInfo->P3d4, 0x32, tempbx, temp);
-	} else { /* for 301 */
-		if (pVBInfo->VBInfo & SetCRT2ToHiVisionTV) { /* for HiVision */
-			tempax = xgifb_reg_get(pVBInfo->P3c4, 0x38);
-			temp = tempax & 0x01;
-			tempax = xgifb_reg_get(pVBInfo->P3c4, 0x3A);
-			temp = temp | (tempax & 0x02);
-			xgifb_reg_and_or(pVBInfo->P3d4, 0x32, 0xA0, temp);
-		} else {
-			if (XGI_BridgeIsOn(pVBInfo)) {
-				P2reg0 = xgifb_reg_get(pVBInfo->Part2Port,
-						       0x00);
-				if (!XGINew_BridgeIsEnable(HwDeviceExtension,
-							   pVBInfo)) {
-					SenseModeNo = 0x2e;
-				/* xgifb_reg_set(pVBInfo->P3d4, 0x30, 0x41);
-				 * XGISetModeNew(HwDeviceExtension, 0x2e);
-				 * // ynlai InitMode */
-
-					temp = XGI_SearchModeID(SenseModeNo,
-								&ModeIdIndex,
-								pVBInfo);
-					XGI_GetVGAType(HwDeviceExtension,
-						       pVBInfo);
-					XGI_GetVBType(pVBInfo);
-					pVBInfo->SetFlag = 0x00;
-					pVBInfo->ModeType = ModeVGA;
-					pVBInfo->VBInfo = SetCRT2ToRAMDAC |
-							  LoadDACFlag |
-							  SetInSlaveMode;
-					XGI_GetLCDInfo(0x2e,
-						       ModeIdIndex,
-						       pVBInfo);
-					XGI_GetTVInfo(0x2e,
-						      ModeIdIndex,
-						      pVBInfo);
-					XGI_EnableBridge(HwDeviceExtension,
-							 pVBInfo);
-					XGI_SetCRT2Group301(SenseModeNo,
-							    HwDeviceExtension,
-							    pVBInfo);
-					XGI_SetCRT2ModeRegs(0x2e,
-							    HwDeviceExtension,
-							    pVBInfo);
-					/* XGI_DisableBridge(HwDeviceExtension,
-					 *		     pVBInfo ) ; */
-					/* Display Off 0212 */
-					xgifb_reg_and_or(pVBInfo->P3c4,
-							 0x01,
-							 0xDF,
-							 0x20);
-					for (i = 0; i < 20; i++)
-						XGI_LongWait(pVBInfo);
-				}
-				xgifb_reg_set(pVBInfo->Part2Port, 0x00, 0x1c);
-				tempax = 0;
-				tempbx = *pVBInfo->pRGBSenseData;
-
-				if (!(XGINew_Is301B(pVBInfo)))
-					tempbx = *pVBInfo->pRGBSenseData2;
-
-				tempcx = 0x0E08;
-				if (XGINew_Sense(tempbx, tempcx, pVBInfo)) {
-					if (XGINew_Sense(tempbx,
-							 tempcx,
-							 pVBInfo))
-						tempax |= Monitor2Sense;
-				}
-
-				if (pVBInfo->VBType & VB_XGI301C)
-					xgifb_reg_or(pVBInfo->Part4Port,
-						     0x0d,
-						     0x04);
-
-				/* add by kuku for Multi-adapter sense HiTV */
-				if (XGINew_SenseHiTV(HwDeviceExtension,
-						     pVBInfo)) {
-					tempax |= HiTVSense;
-					if ((pVBInfo->VBType & VB_XGI301C))
-						tempax ^= (HiTVSense |
-							   YPbPrSense);
-				}
-
-				/* start */
-				if (!(tempax & (HiTVSense | YPbPrSense))) {
-					tempbx = *pVBInfo->pYCSenseData;
-					if (!(XGINew_Is301B(pVBInfo)))
-						tempbx = *pVBInfo->pYCSenseData2;
-					tempcx = 0x0604;
-					if (XGINew_Sense(tempbx,
-							 tempcx,
-							 pVBInfo)) {
-						if (XGINew_Sense(tempbx,
-								 tempcx,
-								 pVBInfo))
-							tempax |= SVIDEOSense;
-					}
-
-					if (OutputSelect & BoardTVType) {
-						tempbx = *pVBInfo->pVideoSenseData;
-
-						if (!(XGINew_Is301B(pVBInfo)))
-							tempbx = *pVBInfo->pVideoSenseData2;
-
-						tempcx = 0x0804;
-						if (XGINew_Sense(tempbx,
-								 tempcx,
-								 pVBInfo)) {
-							if (XGINew_Sense(tempbx,
-									 tempcx,
-									 pVBInfo))
-								tempax |= AVIDEOSense;
-						}
-					} else {
-						if (!(tempax & SVIDEOSense)) {
-							tempbx = *pVBInfo->pVideoSenseData;
-
-							if (!(XGINew_Is301B(pVBInfo)))
-								tempbx = *pVBInfo->pVideoSenseData2;
-
-							tempcx = 0x0804;
-							if (XGINew_Sense(tempbx,
-									 tempcx,
-									 pVBInfo)) {
-								if (XGINew_Sense(tempbx, tempcx, pVBInfo))
-									tempax |= AVIDEOSense;
-							}
-						}
-					}
-				}
-			} /* end */
-			if (!(tempax & Monitor2Sense)) {
-				if (XGINew_SenseLCD(HwDeviceExtension, pVBInfo))
-					tempax |= LCDSense;
-			}
-			tempbx = 0;
-			tempcx = 0;
-			XGINew_Sense(tempbx, tempcx, pVBInfo);
-
-			xgifb_reg_and_or(pVBInfo->P3d4, 0x32, ~0xDF, tempax);
-			xgifb_reg_set(pVBInfo->Part2Port, 0x00, P2reg0);
-
-			if (!(P2reg0 & 0x20)) {
-				pVBInfo->VBInfo = DisableCRT2Display;
-				/* XGI_SetCRT2Group301(SenseModeNo,
-				 *		       HwDeviceExtension,
-				 *		       pVBInfo); */
-			}
-		}
-	}
-	XGI_DisableBridge(HwDeviceExtension, pVBInfo); /* shampoo 0226 */
-
-}
-
-unsigned short XGINew_SenseLCD(struct xgi_hw_device_info *HwDeviceExtension,
-			       struct vb_device_info *pVBInfo)
-{
-	/* unsigned short SoftSetting ; */
-	unsigned short temp;
-
-	temp = XGINew_GetLCDDDCInfo(HwDeviceExtension, pVBInfo);
-
-	return temp;
-}
diff --git a/drivers/staging/xgifb/vb_ext.h b/drivers/staging/xgifb/vb_ext.h
deleted file mode 100644
index 0b1f55b4242f..000000000000
--- a/drivers/staging/xgifb/vb_ext.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _VBEXT_
-#define _VBEXT_
-
-extern void XGI_GetSenseStatus(struct xgi_hw_device_info *HwDeviceExtension,
-			       struct vb_device_info *pVBInfo);
-extern unsigned short XGINew_SenseLCD(struct xgi_hw_device_info *,
-				      struct vb_device_info *pVBInfo);
-
-#endif
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
index 9e890a17fbc2..4ccd988ffd7c 100644
--- a/drivers/staging/xgifb/vb_init.c
+++ b/drivers/staging/xgifb/vb_init.c
@@ -1,6 +1,7 @@
 #include <linux/types.h>
 #include <linux/delay.h> /* udelay */
 #include <linux/pci.h>
+#include <linux/vmalloc.h>
 
 #include "vgatypes.h"
 #include "XGIfb.h"
@@ -10,7 +11,6 @@
 #include "vb_util.h"
 #include "vb_setmode.h"
 #include "vb_init.h"
-#include "vb_ext.h"
 
 
 #include <linux/io.h>
@@ -35,6 +35,8 @@ static const unsigned short XGINew_DDRDRAM_TYPE20[12][5] = {
 	{ 2, 12,  9,  8, 0x35},
 	{ 2, 12,  8,  4, 0x31} };
 
+#define XGIFB_ROM_SIZE	65536
+
 static unsigned char
 XGINew_GetXG20DRAMType(struct xgi_hw_device_info *HwDeviceExtension,
 		       struct vb_device_info *pVBInfo)
@@ -1068,20 +1070,20 @@ static int XGINew_DDRSizing340(struct xgi_hw_device_info *HwDeviceExtension,
 	return 0;
 }
 
-static void XGINew_SetDRAMSize_340(struct xgi_hw_device_info *HwDeviceExtension,
+static void XGINew_SetDRAMSize_340(struct xgifb_video_info *xgifb_info,
+		struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned short data;
 
-	pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase;
 	pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress;
 
-	XGISetModeNew(HwDeviceExtension, 0x2e);
+	XGISetModeNew(xgifb_info, HwDeviceExtension, 0x2e);
 
 	data = xgifb_reg_get(pVBInfo->P3c4, 0x21);
 	/* disable read cache */
 	xgifb_reg_set(pVBInfo->P3c4, 0x21, (unsigned short) (data & 0xDF));
-	XGI_DisplayOff(HwDeviceExtension, pVBInfo);
+	XGI_DisplayOff(xgifb_info, HwDeviceExtension, pVBInfo);
 
 	/* data = xgifb_reg_get(pVBInfo->P3c4, 0x1); */
 	/* data |= 0x20 ; */
@@ -1092,118 +1094,100 @@ static void XGINew_SetDRAMSize_340(struct xgi_hw_device_info *HwDeviceExtension,
 	xgifb_reg_set(pVBInfo->P3c4, 0x21, (unsigned short) (data | 0x20));
 }
 
-static void ReadVBIOSTablData(unsigned char ChipType,
+static u8 *xgifb_copy_rom(struct pci_dev *dev, size_t *rom_size)
+{
+	void __iomem *rom_address;
+	u8 *rom_copy;
+
+	rom_address = pci_map_rom(dev, rom_size);
+	if (rom_address == NULL)
+		return NULL;
+
+	rom_copy = vzalloc(XGIFB_ROM_SIZE);
+	if (rom_copy == NULL)
+		goto done;
+
+	*rom_size = min_t(size_t, *rom_size, XGIFB_ROM_SIZE);
+	memcpy_fromio(rom_copy, rom_address, *rom_size);
+
+done:
+	pci_unmap_rom(dev, rom_address);
+	return rom_copy;
+}
+
+static void xgifb_read_vbios(struct pci_dev *pdev,
 			      struct vb_device_info *pVBInfo)
 {
-	volatile unsigned char *pVideoMemory =
-		(unsigned char *) pVBInfo->ROMAddr;
+	struct xgifb_video_info *xgifb_info = pci_get_drvdata(pdev);
+	u8 *vbios;
 	unsigned long i;
-	unsigned char j, k;
-	/* Volari customize data area end */
-
-	if (ChipType == XG21) {
-		pVBInfo->IF_DEF_LVDS = 0;
-		if (pVideoMemory[0x65] & 0x1) {
-			pVBInfo->IF_DEF_LVDS = 1;
-			i = pVideoMemory[0x316] | (pVideoMemory[0x317] << 8);
-			j = pVideoMemory[i - 1];
-			if (j != 0xff) {
-				k = 0;
-				do {
-					pVBInfo->XG21_LVDSCapList[k].
-						 LVDS_Capability
-						= pVideoMemory[i] |
-						 (pVideoMemory[i + 1] << 8);
-					pVBInfo->XG21_LVDSCapList[k].LVDSHT
-						= pVideoMemory[i + 2] |
-						  (pVideoMemory[i + 3] << 8);
-					pVBInfo->XG21_LVDSCapList[k].LVDSVT
-						= pVideoMemory[i + 4] |
-						  (pVideoMemory[i + 5] << 8);
-					pVBInfo->XG21_LVDSCapList[k].LVDSHDE
-						= pVideoMemory[i + 6] |
-						  (pVideoMemory[i + 7] << 8);
-					pVBInfo->XG21_LVDSCapList[k].LVDSVDE
-						= pVideoMemory[i + 8] |
-						  (pVideoMemory[i + 9] << 8);
-					pVBInfo->XG21_LVDSCapList[k].LVDSHFP
-						= pVideoMemory[i + 10] |
-						  (pVideoMemory[i + 11] << 8);
-					pVBInfo->XG21_LVDSCapList[k].LVDSVFP
-						= pVideoMemory[i + 12] |
-						  (pVideoMemory[i + 13] << 8);
-					pVBInfo->XG21_LVDSCapList[k].LVDSHSYNC
-						= pVideoMemory[i + 14] |
-						  (pVideoMemory[i + 15] << 8);
-					pVBInfo->XG21_LVDSCapList[k].LVDSVSYNC
-						= pVideoMemory[i + 16] |
-						  (pVideoMemory[i + 17] << 8);
-					pVBInfo->XG21_LVDSCapList[k].VCLKData1
-						= pVideoMemory[i + 18];
-					pVBInfo->XG21_LVDSCapList[k].VCLKData2
-						= pVideoMemory[i + 19];
-					pVBInfo->XG21_LVDSCapList[k].PSC_S1
-						= pVideoMemory[i + 20];
-					pVBInfo->XG21_LVDSCapList[k].PSC_S2
-						= pVideoMemory[i + 21];
-					pVBInfo->XG21_LVDSCapList[k].PSC_S3
-						= pVideoMemory[i + 22];
-					pVBInfo->XG21_LVDSCapList[k].PSC_S4
-						= pVideoMemory[i + 23];
-					pVBInfo->XG21_LVDSCapList[k].PSC_S5
-						= pVideoMemory[i + 24];
-					i += 25;
-					j--;
-					k++;
-				} while ((j > 0) &&
-					 (k < (sizeof(XGI21_LCDCapList) /
-					       sizeof(struct
-							XGI21_LVDSCapStruct))));
-			} else {
-				pVBInfo->XG21_LVDSCapList[0].LVDS_Capability
-						= pVideoMemory[i] |
-						  (pVideoMemory[i + 1] << 8);
-				pVBInfo->XG21_LVDSCapList[0].LVDSHT
-						= pVideoMemory[i + 2] |
-						  (pVideoMemory[i + 3] << 8);
-				pVBInfo->XG21_LVDSCapList[0].LVDSVT
-						= pVideoMemory[i + 4] |
-						  (pVideoMemory[i + 5] << 8);
-				pVBInfo->XG21_LVDSCapList[0].LVDSHDE
-						= pVideoMemory[i + 6] |
-						  (pVideoMemory[i + 7] << 8);
-				pVBInfo->XG21_LVDSCapList[0].LVDSVDE
-						= pVideoMemory[i + 8] |
-						  (pVideoMemory[i + 9] << 8);
-				pVBInfo->XG21_LVDSCapList[0].LVDSHFP
-						= pVideoMemory[i + 10] |
-						  (pVideoMemory[i + 11] << 8);
-				pVBInfo->XG21_LVDSCapList[0].LVDSVFP
-						= pVideoMemory[i + 12] |
-						  (pVideoMemory[i + 13] << 8);
-				pVBInfo->XG21_LVDSCapList[0].LVDSHSYNC
-						= pVideoMemory[i + 14] |
-						  (pVideoMemory[i + 15] << 8);
-				pVBInfo->XG21_LVDSCapList[0].LVDSVSYNC
-						= pVideoMemory[i + 16] |
-						  (pVideoMemory[i + 17] << 8);
-				pVBInfo->XG21_LVDSCapList[0].VCLKData1
-						= pVideoMemory[i + 18];
-				pVBInfo->XG21_LVDSCapList[0].VCLKData2
-						= pVideoMemory[i + 19];
-				pVBInfo->XG21_LVDSCapList[0].PSC_S1
-						= pVideoMemory[i + 20];
-				pVBInfo->XG21_LVDSCapList[0].PSC_S2
-						= pVideoMemory[i + 21];
-				pVBInfo->XG21_LVDSCapList[0].PSC_S3
-						= pVideoMemory[i + 22];
-				pVBInfo->XG21_LVDSCapList[0].PSC_S4
-						= pVideoMemory[i + 23];
-				pVBInfo->XG21_LVDSCapList[0].PSC_S5
-						= pVideoMemory[i + 24];
-			}
-		}
+	unsigned char j;
+	struct XGI21_LVDSCapStruct *lvds;
+	size_t vbios_size;
+	int entry;
+
+	if (xgifb_info->chip != XG21)
+		return;
+	pVBInfo->IF_DEF_LVDS = 0;
+	vbios = xgifb_copy_rom(pdev, &vbios_size);
+	if (vbios == NULL) {
+		dev_err(&pdev->dev, "video BIOS not available\n");
+		return;
+	}
+	if (vbios_size <= 0x65)
+		goto error;
+	/*
+	 * The user can ignore the LVDS bit in the BIOS and force the display
+	 * type.
+	 */
+	if (!(vbios[0x65] & 0x1) &&
+	    (!xgifb_info->display2_force ||
+	     xgifb_info->display2 != XGIFB_DISP_LCD)) {
+		vfree(vbios);
+		return;
 	}
+	if (vbios_size <= 0x317)
+		goto error;
+	i = vbios[0x316] | (vbios[0x317] << 8);
+	if (vbios_size <= i - 1)
+		goto error;
+	j = vbios[i - 1];
+	if (j == 0)
+		goto error;
+	if (j == 0xff)
+		j = 1;
+	/*
+	 * Read the LVDS table index scratch register set by the BIOS.
+	 */
+	entry = xgifb_reg_get(xgifb_info->dev_info.P3d4, 0x36);
+	if (entry >= j)
+		entry = 0;
+	i += entry * 25;
+	lvds = &xgifb_info->lvds_data;
+	if (vbios_size <= i + 24)
+		goto error;
+	lvds->LVDS_Capability	= vbios[i]	| (vbios[i + 1] << 8);
+	lvds->LVDSHT		= vbios[i + 2]	| (vbios[i + 3] << 8);
+	lvds->LVDSVT		= vbios[i + 4]	| (vbios[i + 5] << 8);
+	lvds->LVDSHDE		= vbios[i + 6]	| (vbios[i + 7] << 8);
+	lvds->LVDSVDE		= vbios[i + 8]	| (vbios[i + 9] << 8);
+	lvds->LVDSHFP		= vbios[i + 10]	| (vbios[i + 11] << 8);
+	lvds->LVDSVFP		= vbios[i + 12]	| (vbios[i + 13] << 8);
+	lvds->LVDSHSYNC		= vbios[i + 14]	| (vbios[i + 15] << 8);
+	lvds->LVDSVSYNC		= vbios[i + 16]	| (vbios[i + 17] << 8);
+	lvds->VCLKData1		= vbios[i + 18];
+	lvds->VCLKData2		= vbios[i + 19];
+	lvds->PSC_S1		= vbios[i + 20];
+	lvds->PSC_S2		= vbios[i + 21];
+	lvds->PSC_S3		= vbios[i + 22];
+	lvds->PSC_S4		= vbios[i + 23];
+	lvds->PSC_S5		= vbios[i + 24];
+	vfree(vbios);
+	pVBInfo->IF_DEF_LVDS = 1;
+	return;
+error:
+	dev_err(&pdev->dev, "video BIOS corrupted\n");
+	vfree(vbios);
 }
 
 static void XGINew_ChkSenseStatus(struct xgi_hw_device_info *HwDeviceExtension,
@@ -1336,18 +1320,57 @@ static void XGINew_SetModeScratch(struct xgi_hw_device_info *HwDeviceExtension,
 
 }
 
+static unsigned short XGINew_SenseLCD(struct xgi_hw_device_info
+							*HwDeviceExtension,
+				      struct vb_device_info *pVBInfo)
+{
+	unsigned short temp;
+
+	/* add lcd sense */
+	if (HwDeviceExtension->ulCRT2LCDType == LCD_UNKNOWN) {
+		return 0;
+	} else {
+		temp = (unsigned short) HwDeviceExtension->ulCRT2LCDType;
+		switch (HwDeviceExtension->ulCRT2LCDType) {
+		case LCD_INVALID:
+		case LCD_800x600:
+		case LCD_1024x768:
+		case LCD_1280x1024:
+			break;
+
+		case LCD_640x480:
+		case LCD_1024x600:
+		case LCD_1152x864:
+		case LCD_1280x960:
+		case LCD_1152x768:
+			temp = 0;
+			break;
+
+		case LCD_1400x1050:
+		case LCD_1280x768:
+		case LCD_1600x1200:
+			break;
+
+		case LCD_1920x1440:
+		case LCD_2048x1536:
+			temp = 0;
+			break;
+
+		default:
+			break;
+		}
+		xgifb_reg_and_or(pVBInfo->P3d4, 0x36, 0xF0, temp);
+		return 1;
+	}
+}
+
 static void XGINew_GetXG21Sense(struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned char Temp;
-	volatile unsigned char *pVideoMemory =
-			(unsigned char *) pVBInfo->ROMAddr;
-
-	pVBInfo->IF_DEF_LVDS = 0;
 
 #if 1
-	if ((pVideoMemory[0x65] & 0x01)) { /* For XG21 LVDS */
-		pVBInfo->IF_DEF_LVDS = 1;
+	if (pVBInfo->IF_DEF_LVDS) { /* For XG21 LVDS */
 		xgifb_reg_or(pVBInfo->P3d4, 0x32, LCDSense);
 		/* LVDS on chip */
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x38, ~0xE0, 0xC0);
@@ -1393,7 +1416,6 @@ static void XGINew_GetXG27Sense(struct xgi_hw_device_info *HwDeviceExtension,
 	xgifb_reg_set(pVBInfo->P3d4, 0x4A, bCR4A);
 
 	if (Temp <= 0x02) {
-		pVBInfo->IF_DEF_LVDS = 1;
 		/* LVDS setting */
 		xgifb_reg_and_or(pVBInfo->P3d4, 0x38, ~0xE0, 0xC0);
 		xgifb_reg_set(pVBInfo->P3d4, 0x30, 0x21);
@@ -1451,24 +1473,15 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 	struct vb_device_info *pVBInfo = &VBINF;
 	unsigned char i, temp = 0, temp1;
 	/* VBIOSVersion[5]; */
-	volatile unsigned char *pVideoMemory;
 
 	/* unsigned long j, k; */
 
-	pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase;
-
 	pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress;
 
 	pVBInfo->BaseAddr = (unsigned long) HwDeviceExtension->pjIOAddress;
 
-	pVideoMemory = (unsigned char *) pVBInfo->ROMAddr;
-
 	/* Newdebugcode(0x99); */
 
-
-	/* if (pVBInfo->ROMAddr == 0) */
-	/* return(0); */
-
 	if (pVBInfo->FBAddr == NULL) {
 		printk("\n pVBInfo->FBAddr == 0 ");
 		return 0;
@@ -1516,8 +1529,7 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 
 	InitTo330Pointer(HwDeviceExtension->jChipType, pVBInfo);
 
-	/* ReadVBIOSData */
-	ReadVBIOSTablData(HwDeviceExtension->jChipType, pVBInfo);
+	xgifb_read_vbios(pdev, pVBInfo);
 
 	/* 1.Openkey */
 	xgifb_reg_set(pVBInfo->P3c4, 0x05, 0x86);
@@ -1774,7 +1786,7 @@ unsigned char XGIInitNew(struct pci_dev *pdev)
 					 pVBInfo);
 
 	printk("20");
-	XGINew_SetDRAMSize_340(HwDeviceExtension, pVBInfo);
+	XGINew_SetDRAMSize_340(xgifb_info, HwDeviceExtension, pVBInfo);
 	printk("21");
 
 	printk("22");
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index 81c0cc41bb42..67a316c3c108 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -67,11 +67,6 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 	pVBInfo->XGINEWUB_CRT1Table
 			= (struct XGI_CRT1TableStruct *) XGI_CRT1Table;
 
-	/* add for new UNIVGABIOS */
-	/* XGINew_UBLCDDataTable =
-	 *	(struct XGI_LCDDataTablStruct *) XGI_LCDDataTable; */
-	/* XGINew_UBTVDataTable = (XGI_TVDataTablStruct *) XGI_TVDataTable; */
-
 	pVBInfo->MCLKData = (struct XGI_MCLKDataStruct *) XGI340New_MCLKData;
 	pVBInfo->ECLKData = (struct XGI_ECLKDataStruct *) XGI340_ECLKData;
 	pVBInfo->VCLKData = (struct XGI_VCLKDataStruct *) XGI_VCLKData;
@@ -148,9 +143,6 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 	else
 		pVBInfo->LCDCapList = XGI_LCDCapList;
 
-	if ((ChipType == XG21) || (ChipType == XG27))
-		pVBInfo->XG21_LVDSCapList = XGI21_LCDCapList;
-
 	pVBInfo->XGI_TVDelayList = XGI301TVDelayList;
 	pVBInfo->XGI_TVDelayList2 = XGI301TVDelayList2;
 
@@ -236,28 +228,6 @@ static void XGI_SetSeqRegs(unsigned short ModeNo,
 	}
 }
 
-static void XGI_SetMiscRegs(unsigned short StandTableIndex,
-			    struct vb_device_info *pVBInfo)
-{
-	unsigned char Miscdata;
-
-	/* Get Misc from file */
-	Miscdata = pVBInfo->StandTable[StandTableIndex].MISC;
-	/*
-	if (pVBInfo->VBType & (VB_XGI301B |
-			       VB_XGI302B |
-			       VB_XGI301LV |
-			       VB_XGI302LV |
-			       VB_XGI301C)) {
-		if (pVBInfo->VBInfo & SetCRT2ToLCDA) {
-			Miscdata |= 0x0C;
-		}
-	}
-	*/
-
-	outb(Miscdata, pVBInfo->P3c2); /* Set Misc(3c2) */
-}
-
 static void XGI_SetCRTCRegs(struct xgi_hw_device_info *HwDeviceExtension,
 			    unsigned short StandTableIndex,
 			    struct vb_device_info *pVBInfo)
@@ -274,16 +244,6 @@ static void XGI_SetCRTCRegs(struct xgi_hw_device_info *HwDeviceExtension,
 		CRTCdata = pVBInfo->StandTable[StandTableIndex].CRTC[i];
 		xgifb_reg_set(pVBInfo->P3d4, i, CRTCdata); /* Set CRTC(3d4) */
 	}
-	/*
-	if ((HwDeviceExtension->jChipType == XGI_630) &&
-	    (HwDeviceExtension->jChipRevision == 0x30)) {
-		if (pVBInfo->VBInfo & SetInSlaveMode) {
-			if (pVBInfo->VBInfo & (SetCRT2ToLCD | SetCRT2ToTV)) {
-				xgifb_reg_set(pVBInfo->P3d4, 0x18, 0xFE);
-			}
-		}
-	}
-	*/
 }
 
 static void XGI_SetATTRegs(unsigned short ModeNo,
@@ -530,10 +490,6 @@ static void XGI_SetCRT1Timing_H(struct vb_device_info *pVBInfo,
 	unsigned char data, data1, pushax;
 	unsigned short i, j;
 
-	/* xgifb_reg_set(pVBInfo->P3d4, 0x51, 0); */
-	/* xgifb_reg_set(pVBInfo->P3d4, 0x56, 0); */
-	/* xgifb_reg_and_or(pVBInfo->P3d4, 0x11, 0x7f, 0x00); */
-
 	/* unlock cr0-7 */
 	data = (unsigned char) xgifb_reg_get(pVBInfo->P3d4, 0x11);
 	data &= 0x7F;
@@ -595,10 +551,6 @@ static void XGI_SetCRT1Timing_V(unsigned short ModeIdIndex,
 	unsigned char data;
 	unsigned short i, j;
 
-	/* xgifb_reg_set(pVBInfo->P3d4, 0x51, 0); */
-	/* xgifb_reg_set(pVBInfo->P3d4, 0x56, 0); */
-	/* xgifb_reg_and_or(pVBInfo->P3d4, 0x11, 0x7f, 0x00); */
-
 	for (i = 0x00; i <= 0x01; i++) {
 		data = pVBInfo->TimingV[0].data[i];
 		xgifb_reg_set(pVBInfo->P3d4, (unsigned short) (i + 6), data);
@@ -976,6 +928,20 @@ static void XGI_SetXG27CRTC(unsigned short ModeNo,
 	}
 }
 
+static void XGI_SetXG27FPBits(struct vb_device_info *pVBInfo)
+{
+	unsigned char temp;
+
+	/* D[1:0] 01: 18bit, 00: dual 12, 10: single 24 */
+	temp = xgifb_reg_get(pVBInfo->P3d4, 0x37);
+	temp = (temp & 3) << 6;
+	/* SR06[7]0: dual 12/1: single 24 [6] 18bit Dither <= 0 h/w recommend */
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x06, ~0xc0, temp & 0x80);
+	/* SR09[7] enable FP output, SR09[6] 1: sigle 18bits, 0: 24bits */
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x09, ~0xc0, temp | 0x80);
+
+}
+
 static void xgifb_set_lcd(int chip_id,
 			  struct vb_device_info *pVBInfo,
 			  unsigned short RefreshRateTableIndex,
@@ -1088,6 +1054,20 @@ static void XGI_UpdateXG21CRTC(unsigned short ModeNo,
 	}
 }
 
+static unsigned short XGI_GetResInfo(unsigned short ModeNo,
+		unsigned short ModeIdIndex, struct vb_device_info *pVBInfo)
+{
+	unsigned short resindex;
+
+	if (ModeNo <= 0x13)
+		/* si+St_ResInfo */
+		resindex = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
+	else
+		/* si+Ext_ResInfo */
+		resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
+	return resindex;
+}
+
 static void XGI_SetCRT1DE(struct xgi_hw_device_info *HwDeviceExtension,
 		unsigned short ModeNo, unsigned short ModeIdIndex,
 		unsigned short RefreshRateTableIndex,
@@ -1127,9 +1107,6 @@ static void XGI_SetCRT1DE(struct xgi_hw_device_info *HwDeviceExtension,
 
 	tempcx = 8;
 
-	/* if (!(modeflag & Charx8Dot)) */
-	/* tempcx = 9; */
-
 	tempax /= tempcx;
 	tempax -= 1;
 	tempbx -= 1;
@@ -1163,20 +1140,6 @@ static void XGI_SetCRT1DE(struct xgi_hw_device_info *HwDeviceExtension,
 	xgifb_reg_set(pVBInfo->P3d4, 0x11, temp);
 }
 
-unsigned short XGI_GetResInfo(unsigned short ModeNo,
-		unsigned short ModeIdIndex, struct vb_device_info *pVBInfo)
-{
-	unsigned short resindex;
-
-	if (ModeNo <= 0x13)
-		/* si+St_ResInfo */
-		resindex = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
-	else
-		/* si+Ext_ResInfo */
-		resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
-	return resindex;
-}
-
 static void XGI_SetCRT1Offset(unsigned short ModeNo,
 			      unsigned short ModeIdIndex,
 			      unsigned short RefreshRateTableIndex,
@@ -1308,77 +1271,55 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 				VCLKIndex = LCDXlat2VCLK[CRT2Index];
 			else
 				VCLKIndex = LCDXlat1VCLK[CRT2Index];
-		} else { /* for TV */
-			if (pVBInfo->VBInfo & SetCRT2ToTV) {
-				if (pVBInfo->VBInfo & SetCRT2ToHiVisionTV) {
-					if (pVBInfo->SetFlag & RPLLDIV2XO) {
-						VCLKIndex = HiTVVCLKDIV2;
-						VCLKIndex += 25;
-					} else {
-						VCLKIndex = HiTVVCLK;
-						VCLKIndex += 25;
-					}
-
-					if (pVBInfo->SetFlag & TVSimuMode) {
-						if (modeflag & Charx8Dot) {
-							VCLKIndex =
-								HiTVSimuVCLK;
-							VCLKIndex += 25;
-						} else {
-							VCLKIndex =
-								HiTVTextVCLK;
-							VCLKIndex += 25;
-						}
-					}
+		} else if (pVBInfo->VBInfo & SetCRT2ToHiVisionTV) {
+			if (pVBInfo->SetFlag & RPLLDIV2XO) {
+				VCLKIndex = HiTVVCLKDIV2;
+				VCLKIndex += 25;
+			} else {
+				VCLKIndex = HiTVVCLK;
+				VCLKIndex += 25;
+			}
 
-					/* 301lv */
-					if (pVBInfo->VBType & VB_XGI301LV) {
-						if (!(pVBInfo->VBExtInfo ==
-						     VB_YPbPr1080i)) {
-							VCLKIndex =
-								YPbPr750pVCLK;
-							if (!(pVBInfo->VBExtInfo
-									==
-							     VB_YPbPr750p)) {
-								VCLKIndex =
-								  YPbPr525pVCLK;
-								if (!(pVBInfo->VBExtInfo
-										== VB_YPbPr525p)) {
-									VCLKIndex
-											= YPbPr525iVCLK_2;
-									if (!(pVBInfo->SetFlag
-											& RPLLDIV2XO))
-										VCLKIndex
-												= YPbPr525iVCLK;
-								}
-							}
-						}
-					}
+			if (pVBInfo->SetFlag & TVSimuMode) {
+				if (modeflag & Charx8Dot) {
+					VCLKIndex = HiTVSimuVCLK;
+					VCLKIndex += 25;
 				} else {
-					if (pVBInfo->VBInfo & SetCRT2ToTV) {
-						if (pVBInfo->SetFlag &
-						    RPLLDIV2XO) {
-							VCLKIndex = TVVCLKDIV2;
-							VCLKIndex += 25;
-						} else {
-							VCLKIndex = TVVCLK;
-							VCLKIndex += 25;
-						}
-					}
+					VCLKIndex = HiTVTextVCLK;
+					VCLKIndex += 25;
 				}
-			} else { /* for CRT2 */
-				/* Port 3cch */
-				VCLKIndex = (unsigned char) inb(
-						(pVBInfo->P3ca + 0x02));
-				VCLKIndex = ((VCLKIndex >> 2) & 0x03);
-				if (ModeNo > 0x13) {
-					/* di+Ext_CRTVCLK */
-					VCLKIndex =
-						pVBInfo->RefIndex[
+			}
+
+			/* 301lv */
+			if ((pVBInfo->VBType & VB_XGI301LV) &&
+			    !(pVBInfo->VBExtInfo == VB_YPbPr1080i)) {
+				if (pVBInfo->VBExtInfo == VB_YPbPr750p)
+					VCLKIndex = YPbPr750pVCLK;
+				else if (pVBInfo->VBExtInfo == VB_YPbPr525p)
+					VCLKIndex = YPbPr525pVCLK;
+				else if (pVBInfo->SetFlag & RPLLDIV2XO)
+					VCLKIndex = YPbPr525iVCLK_2;
+				else
+					VCLKIndex = YPbPr525iVCLK;
+			}
+		} else if (pVBInfo->VBInfo & SetCRT2ToTV) {
+			if (pVBInfo->SetFlag & RPLLDIV2XO) {
+				VCLKIndex = TVVCLKDIV2;
+				VCLKIndex += 25;
+			} else {
+				VCLKIndex = TVVCLK;
+				VCLKIndex += 25;
+			}
+		} else { /* for CRT2 */
+			/* Port 3cch */
+			VCLKIndex = (unsigned char) inb((pVBInfo->P3ca + 0x02));
+			VCLKIndex = ((VCLKIndex >> 2) & 0x03);
+			if (ModeNo > 0x13) {
+				/* di+Ext_CRTVCLK */
+				VCLKIndex = pVBInfo->RefIndex[
 							RefreshRateTableIndex].
 								Ext_CRTVCLK;
-					VCLKIndex &= IndexMask;
-				}
+				VCLKIndex &= IndexMask;
 			}
 		}
 	} else { /* LVDS */
@@ -1397,7 +1338,6 @@ static unsigned short XGI_GetVCLK2Ptr(unsigned short ModeNo,
 		else
 			VCLKIndex = LVDSXlat3VCLK[VCLKIndex];
 	}
-	/* VCLKIndex = VCLKIndex&IndexMask; */
 
 	return VCLKIndex;
 }
@@ -1461,6 +1401,19 @@ static void XGI_SetCRT1VCLK(unsigned short ModeNo,
 	}
 }
 
+static void XGI_SetXG21FPBits(struct vb_device_info *pVBInfo)
+{
+	unsigned char temp;
+
+	temp = xgifb_reg_get(pVBInfo->P3d4, 0x37); /* D[0] 1: 18bit */
+	temp = (temp & 1) << 6;
+	/* SR06[6] 18bit Dither */
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x06, ~0x40, temp);
+	/* SR09[7] enable FP output, SR09[6] 1: sigle 18bits, 0: dual 12bits */
+	xgifb_reg_and_or(pVBInfo->P3c4, 0x09, ~0xc0, temp | 0x80);
+
+}
+
 static void XGI_SetCRT1FIFO(unsigned short ModeNo,
 		struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
@@ -1532,16 +1485,6 @@ static void XGI_SetVCLKState(struct xgi_hw_device_info *HwDeviceExtension,
 		xgifb_reg_set(pVBInfo->P3c4, 0x1F, data);
 	}
 
-	/*  Jong for Adavantech LCD ripple issue
-	if ((VCLK >= 0) && (VCLK < 135))
-		data2 = 0x03;
-	else if ((VCLK >= 135) && (VCLK < 160))
-		data2 = 0x02;
-	else if ((VCLK >= 160) && (VCLK < 260))
-		data2 = 0x01;
-	else if (VCLK > 260)
-		data2 = 0x00;
-	*/
 	data2 = 0x00;
 
 	xgifb_reg_and_or(pVBInfo->P3c4, 0x07, 0xFC, data2);
@@ -1591,7 +1534,6 @@ static void XGI_SetCRT1ModeRegs(struct xgi_hw_device_info *HwDeviceExtension,
 		data2 |= 0x20;
 
 	xgifb_reg_and_or(pVBInfo->P3c4, 0x06, ~0x3F, data2);
-	/* xgifb_reg_set(pVBInfo->P3c4,0x06,data2); */
 	resindex = XGI_GetResInfo(ModeNo, ModeIdIndex, pVBInfo);
 	if (ModeNo <= 0x13)
 		xres = pVBInfo->StResInfo[resindex].HTotal;
@@ -1636,11 +1578,6 @@ static void XGI_SetCRT1ModeRegs(struct xgi_hw_device_info *HwDeviceExtension,
 	XGI_SetVCLKState(HwDeviceExtension, ModeNo, RefreshRateTableIndex,
 			pVBInfo);
 
-	/* if (modeflag&HalfDCLK) //030305 fix lowresolution bug */
-	/* if (XGINew_IF_DEF_NEW_LOWRES) */
-	/* XGI_VesaLowResolution(ModeNo, ModeIdIndex);
-	 * //030305 fix lowresolution bug */
-
 	data = xgifb_reg_get(pVBInfo->P3d4, 0x31);
 
 	if (HwDeviceExtension->jChipType == XG27) {
@@ -1803,11 +1740,6 @@ static void XGI_GetLVDSResInfo(unsigned short ModeNo,
 		/* si+Ext_ResInfo */
 		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 
-	/* if (ModeNo > 0x13) */
-	/*	modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag; */
-	/* else */
-	/*	modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag; */
-
 	if (ModeNo <= 0x13)
 		/* si+St_ResInfo */
 		resindex = pVBInfo->SModeIDTable[ModeIdIndex].St_ResInfo;
@@ -1815,8 +1747,6 @@ static void XGI_GetLVDSResInfo(unsigned short ModeNo,
 		/* si+Ext_ResInfo */
 		resindex = pVBInfo->EModeIDTable[ModeIdIndex].Ext_RESINFO;
 
-	/* resindex = XGI_GetResInfo(ModeNo, ModeIdIndex, pVBInfo); */
-
 	if (ModeNo <= 0x13) {
 		xres = pVBInfo->StResInfo[resindex].HTotal;
 		yres = pVBInfo->StResInfo[resindex].VTotal;
@@ -1831,13 +1761,10 @@ static void XGI_GetLVDSResInfo(unsigned short ModeNo,
 		if (modeflag & DoubleScanMode)
 			yres = yres << 1;
 	}
-	/* if (modeflag & Charx8Dot) */
-	/* { */
 
 	if (xres == 720)
 		xres = 640;
 
-	/* } */
 	pVBInfo->VGAHDE = xres;
 	pVBInfo->HDE = xres;
 	pVBInfo->VGAVDE = yres;
@@ -1890,7 +1817,7 @@ static void *XGI_GetLcdPtr(unsigned short BX, unsigned short ModeNo,
 		tempal = (tempal & 0x0f);
 	}
 
-	tempcx = LCDLenList[tempbx]; /* mov cl,byte ptr cs:LCDLenList[bx] */
+	tempcx = LCDLenList[tempbx];
 
 	if (pVBInfo->LCDInfo & EnableScalingLCD) { /* ScaleLCD */
 		if ((tempbx == 5) || (tempbx) == 7)
@@ -1898,9 +1825,6 @@ static void *XGI_GetLcdPtr(unsigned short BX, unsigned short ModeNo,
 		else if ((tempbx == 3) || (tempbx == 8))
 			tempcx = LVDSDesDataLen2;
 	}
-	/* mov di, word ptr cs:LCDDataList[bx] */
-	/* tempdi = pVideoMemory[LCDDataList + tempbx * 2] |
-		    (pVideoMemory[LCDDataList + tempbx * 2 + 1] << 8); */
 
 	switch (tempbx) {
 	case 0:
@@ -2321,10 +2245,10 @@ static void *XGI_GetTVPtr(unsigned short BX, unsigned short ModeNo,
 
 	switch (tempbx) {
 	case 0:
-		tempdi = NULL; /*EPLCHTVCRT1Ptr_H;*/
+		tempdi = NULL;
 		break;
 	case 1:
-		tempdi = NULL; /*EPLCHTVCRT1Ptr_V;*/
+		tempdi = NULL;
 		break;
 	case 2:
 	case 6:
@@ -2363,9 +2287,7 @@ static void *XGI_GetTVPtr(unsigned short BX, unsigned short ModeNo,
 	}
 
 	/* 07/05/22 */
-	if (table == 0x00) {
-	} else if (table == 0x01) {
-	} else if (table == 0x04) {
+	if (table == 0x04) {
 		switch (tempdi[i].DATAPTR) {
 		case 0:
 			return &XGI_ExtPALData[tempal];
@@ -2429,7 +2351,6 @@ static void *XGI_GetTVPtr(unsigned short BX, unsigned short ModeNo,
 		default:
 			break;
 		}
-	} else if (table == 0x06) {
 	}
 	return NULL;
 }
@@ -2741,7 +2662,6 @@ static void XGI_SetLVDSRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	else
 		tempbx = LCDPtr->LCDVRS;
 
-	/* tempbx = tempbx >> 4; */
 	tempcx = push1;
 
 	if (pVBInfo->LCDInfo & EnableScalingLCD)
@@ -2881,7 +2801,6 @@ static void XGI_GetLCDVCLKPtr(unsigned char *di_0, unsigned char *di_1,
 	unsigned short index;
 
 	if (pVBInfo->VBInfo & (SetCRT2ToLCD | SetCRT2ToLCDA)) {
-		/* index = XGI_GetLCDCapPtr(pVBInfo); */
 		index = XGI_GetLCDCapPtr1(pVBInfo);
 
 		if (pVBInfo->VBInfo & SetCRT2ToLCD) { /* LCDB */
@@ -3105,19 +3024,6 @@ static void XGI_UpdateModeInfo(struct xgi_hw_device_info *HwDeviceExtension,
 	}
 }
 
-void XGI_GetVGAType(struct xgi_hw_device_info *HwDeviceExtension,
-		struct vb_device_info *pVBInfo)
-{
-	/*
-	if ( HwDeviceExtension->jChipType >= XG20 ) {
-		pVBInfo->Set_VGAType = XG20;
-	} else {
-		pVBInfo->Set_VGAType = VGA_XGI340;
-	}
-	*/
-	pVBInfo->Set_VGAType = HwDeviceExtension->jChipType;
-}
-
 void XGI_GetVBType(struct vb_device_info *pVBInfo)
 {
 	unsigned short flag, tempbx, tempah;
@@ -3160,7 +3066,7 @@ void XGI_GetVBType(struct vb_device_info *pVBInfo)
 	}
 }
 
-void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
+static void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
@@ -3193,14 +3099,9 @@ void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 		if (pVBInfo->IF_DEF_LCDA == 1) {
 
-			if ((pVBInfo->Set_VGAType >= XG20)
-					|| (pVBInfo->Set_VGAType >= XG40)) {
+			if ((HwDeviceExtension->jChipType >= XG20) ||
+			    (HwDeviceExtension->jChipType >= XG40)) {
 				if (pVBInfo->IF_DEF_LVDS == 0) {
-					/* if ((pVBInfo->VBType & VB_XGI302B)
-					    || (pVBInfo->VBType & VB_XGI301LV)
-					    || (pVBInfo->VBType & VB_XGI302LV)
-					    || (pVBInfo->VBType & VB_XGI301C))
-					*/
 					if (pVBInfo->VBType &
 					    (VB_XGI302B |
 					     VB_XGI301LV |
@@ -3225,7 +3126,7 @@ void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 			    ((pVBInfo->VBType & VB_XGI301LV) ||
 			    (pVBInfo->VBType & VB_XGI302LV) ||
 			    (pVBInfo->VBType & VB_XGI301C)))) {
-				if (temp & SetYPbPr) { /* temp = CR38 */
+				if (temp & SetYPbPr) {
 					if (pVBInfo->IF_DEF_HiVision == 1) {
 						/* shampoo add for new
 						 * scratch */
@@ -3242,8 +3143,6 @@ void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 							 SetCRT2ToYPbPr;
 						}
 					}
-
-					/* tempbx |= SetCRT2ToYPbPr; */
 				}
 			}
 		}
@@ -3368,7 +3267,7 @@ void XGI_GetVBInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 	pVBInfo->VBInfo = tempbx;
 }
 
-void XGI_GetTVInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
+static void XGI_GetTVInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned short temp, tempbx = 0, resinfo = 0, modeflag, index1;
@@ -3404,17 +3303,6 @@ void XGI_GetTVInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 				tempbx &= (SetCHTVOverScan |
 					   SetNTSCJ |
 					   SetPALTV);
-			/*
-			if (pVBInfo->IF_DEF_LVDS == 0) {
-				//PAL-M/PAL-N Info
-				index1 = xgifb_reg_get(pVBInfo->P3d4, 0x38);
-				//00:PAL, 01:PAL-M, 10:PAL-N
-				temp2 = (index1 & 0xC0) >> 5;
-				tempbx |= temp2;
-				if (temp2 & 0x02) //PAL-M
-					tempbx &= (~SetPALTV);
-			}
-			*/
 		}
 
 		if (pVBInfo->IF_DEF_LVDS == 0) {
@@ -3476,8 +3364,8 @@ void XGI_GetTVInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 	pVBInfo->TVInfo = tempbx;
 }
 
-unsigned char XGI_GetLCDInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
-		struct vb_device_info *pVBInfo)
+static unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
+		unsigned short ModeIdIndex, struct vb_device_info *pVBInfo)
 {
 	unsigned short temp, tempax, tempbx, modeflag, resinfo = 0, LCDIdIndex;
 
@@ -3553,15 +3441,8 @@ unsigned char XGI_GetLCDInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 			tempbx |= SetLCDtoNonExpanding;
 	}
 
-	/*
-	if (tempax & LCDBToA) {
-		tempbx |= SetLCDBToA;
-	}
-	*/
-
 	if (pVBInfo->IF_DEF_ExpLink == 1) {
 		if (modeflag & HalfDCLK) {
-			/* if (!(pVBInfo->LCDInfo&LCDNonExpanding)) */
 			if (!(tempbx & SetLCDtoNonExpanding)) {
 				tempbx |= EnableLVDSDDA;
 			} else {
@@ -3604,25 +3485,6 @@ unsigned char XGI_GetLCDInfo(unsigned short ModeNo, unsigned short ModeIdIndex,
 		}
 	}
 
-	/*
-	if (pVBInfo->IF_DEF_LVDS == 0) {
-		if (tempax & (LockLCDBToA | StLCDBToA)) {
-			if (pVBInfo->VBInfo & SetInSlaveMode) {
-				if (!((!(tempax & LockLCDBToA)) &&
-				    (ModeNo > 0x13))) {
-					pVBInfo->VBInfo &=
-						~(SetSimuScanMode |
-						  SetInSlaveMode |
-						  SetCRT2ToLCD);
-					pVBInfo->VBInfo |=
-						SetCRT2ToLCDA |
-						SetCRT2ToDualEdge;
-				}
-			}
-		}
-	}
-	*/
-
 	return 1;
 }
 
@@ -3632,10 +3494,6 @@ unsigned char XGI_SearchModeID(unsigned short ModeNo,
 	if (ModeNo <= 5)
 		ModeNo |= 1;
 	if (ModeNo <= 0x13) {
-		/* for (*ModeIdIndex=0;
-			*ModeIdIndex < sizeof(pVBInfo->SModeIDTable)
-				/ sizeof(struct XGI_StStruct);
-			(*ModeIdIndex)++) */
 		for (*ModeIdIndex = 0;; (*ModeIdIndex)++) {
 			if (pVBInfo->SModeIDTable[*ModeIdIndex].St_ModeID ==
 			    ModeNo)
@@ -3651,10 +3509,6 @@ unsigned char XGI_SearchModeID(unsigned short ModeNo,
 			(*ModeIdIndex) += 2; /* 400 lines */
 		/* else 350 lines */
 	} else {
-		/* for (*ModeIdIndex=0;
-			*ModeIdIndex < sizeof(pVBInfo->EModeIDTable)
-				/ sizeof(struct XGI_ExtStruct);
-			(*ModeIdIndex)++) */
 		for (*ModeIdIndex = 0;; (*ModeIdIndex)++) {
 			if (pVBInfo->EModeIDTable[*ModeIdIndex].Ext_ModeID ==
 			    ModeNo)
@@ -3675,7 +3529,6 @@ static unsigned char XG21GPIODataTransfer(unsigned char ujDate)
 
 	for (i = 0; i < 8; i++) {
 		ujRet = ujRet << 1;
-		/* ujRet |= GETBITS(ujDate >> i, 0:0); */
 		ujRet |= (ujDate >> i) & 1;
 	}
 
@@ -3726,7 +3579,101 @@ static unsigned char XGI_XG27GetPSCValue(struct vb_device_info *pVBInfo)
 	return temp;
 }
 
-void XGI_DisplayOn(struct xgi_hw_device_info *pXGIHWDE,
+/*----------------------------------------------------------------------------*/
+/* input                                                                      */
+/*      bl[5] : 1;LVDS signal on                                              */
+/*      bl[1] : 1;LVDS backlight on                                           */
+/*      bl[0] : 1:LVDS VDD on                                                 */
+/*      bh: 100000b : clear bit 5, to set bit5                                */
+/*          000010b : clear bit 1, to set bit1                                */
+/*          000001b : clear bit 0, to set bit0                                */
+/*----------------------------------------------------------------------------*/
+static void XGI_XG21BLSignalVDD(unsigned short tempbh, unsigned short tempbl,
+		struct vb_device_info *pVBInfo)
+{
+	unsigned char CR4A, temp;
+
+	CR4A = xgifb_reg_get(pVBInfo->P3d4, 0x4A);
+	tempbh &= 0x23;
+	tempbl &= 0x23;
+	xgifb_reg_and(pVBInfo->P3d4, 0x4A, ~tempbh); /* enable GPIO write */
+
+	if (tempbh & 0x20) {
+		temp = (tempbl >> 4) & 0x02;
+
+		/* CR B4[1] */
+		xgifb_reg_and_or(pVBInfo->P3d4, 0xB4, ~0x02, temp);
+
+	}
+
+	temp = xgifb_reg_get(pVBInfo->P3d4, 0x48);
+
+	temp = XG21GPIODataTransfer(temp);
+	temp &= ~tempbh;
+	temp |= tempbl;
+	xgifb_reg_set(pVBInfo->P3d4, 0x48, temp);
+}
+
+static void XGI_XG27BLSignalVDD(unsigned short tempbh, unsigned short tempbl,
+		struct vb_device_info *pVBInfo)
+{
+	unsigned char CR4A, temp;
+	unsigned short tempbh0, tempbl0;
+
+	tempbh0 = tempbh;
+	tempbl0 = tempbl;
+	tempbh0 &= 0x20;
+	tempbl0 &= 0x20;
+	tempbh0 >>= 3;
+	tempbl0 >>= 3;
+
+	if (tempbh & 0x20) {
+		temp = (tempbl >> 4) & 0x02;
+
+		/* CR B4[1] */
+		xgifb_reg_and_or(pVBInfo->P3d4, 0xB4, ~0x02, temp);
+
+	}
+	xgifb_reg_and_or(pVBInfo->P3d4, 0xB4, ~tempbh0, tempbl0);
+
+	CR4A = xgifb_reg_get(pVBInfo->P3d4, 0x4A);
+	tempbh &= 0x03;
+	tempbl &= 0x03;
+	tempbh <<= 2;
+	tempbl <<= 2; /* GPIOC,GPIOD */
+	xgifb_reg_and(pVBInfo->P3d4, 0x4A, ~tempbh); /* enable GPIO write */
+	xgifb_reg_and_or(pVBInfo->P3d4, 0x48, ~tempbh, tempbl);
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_XG21SetPanelDelay */
+/* Input : */
+/* Output : */
+/* Description : */
+/* I/P : bl : 1 ; T1 : the duration between CPL on and signal on */
+/* : bl : 2 ; T2 : the duration signal on and Vdd on */
+/* : bl : 3 ; T3 : the duration between CPL off and signal off */
+/* : bl : 4 ; T4 : the duration signal off and Vdd off */
+/* --------------------------------------------------------------------- */
+static void XGI_XG21SetPanelDelay(struct xgifb_video_info *xgifb_info,
+		unsigned short tempbl,
+		struct vb_device_info *pVBInfo)
+{
+	if (tempbl == 1)
+		mdelay(xgifb_info->lvds_data.PSC_S1);
+
+	if (tempbl == 2)
+		mdelay(xgifb_info->lvds_data.PSC_S2);
+
+	if (tempbl == 3)
+		mdelay(xgifb_info->lvds_data.PSC_S3);
+
+	if (tempbl == 4)
+		mdelay(xgifb_info->lvds_data.PSC_S4);
+}
+
+static void XGI_DisplayOn(struct xgifb_video_info *xgifb_info,
+		struct xgi_hw_device_info *pXGIHWDE,
 		struct vb_device_info *pVBInfo)
 {
 
@@ -3736,12 +3683,12 @@ void XGI_DisplayOn(struct xgi_hw_device_info *pXGIHWDE,
 			if (!(XGI_XG21GetPSCValue(pVBInfo) & 0x1)) {
 				/* LVDS VDD on */
 				XGI_XG21BLSignalVDD(0x01, 0x01, pVBInfo);
-				XGI_XG21SetPanelDelay(2, pVBInfo);
+				XGI_XG21SetPanelDelay(xgifb_info, 2, pVBInfo);
 			}
 			if (!(XGI_XG21GetPSCValue(pVBInfo) & 0x20))
 				/* LVDS signal on */
 				XGI_XG21BLSignalVDD(0x20, 0x20, pVBInfo);
-			XGI_XG21SetPanelDelay(3, pVBInfo);
+			XGI_XG21SetPanelDelay(xgifb_info, 3, pVBInfo);
 			/* LVDS backlight on */
 			XGI_XG21BLSignalVDD(0x02, 0x02, pVBInfo);
 		} else {
@@ -3756,12 +3703,12 @@ void XGI_DisplayOn(struct xgi_hw_device_info *pXGIHWDE,
 			if (!(XGI_XG27GetPSCValue(pVBInfo) & 0x1)) {
 				/* LVDS VDD on */
 				XGI_XG27BLSignalVDD(0x01, 0x01, pVBInfo);
-				XGI_XG21SetPanelDelay(2, pVBInfo);
+				XGI_XG21SetPanelDelay(xgifb_info, 2, pVBInfo);
 			}
 			if (!(XGI_XG27GetPSCValue(pVBInfo) & 0x20))
 				/* LVDS signal on */
 				XGI_XG27BLSignalVDD(0x20, 0x20, pVBInfo);
-			XGI_XG21SetPanelDelay(3, pVBInfo);
+			XGI_XG21SetPanelDelay(xgifb_info, 3, pVBInfo);
 			/* LVDS backlight on */
 			XGI_XG27BLSignalVDD(0x02, 0x02, pVBInfo);
 		} else {
@@ -3772,7 +3719,8 @@ void XGI_DisplayOn(struct xgi_hw_device_info *pXGIHWDE,
 	}
 }
 
-void XGI_DisplayOff(struct xgi_hw_device_info *pXGIHWDE,
+void XGI_DisplayOff(struct xgifb_video_info *xgifb_info,
+		struct xgi_hw_device_info *pXGIHWDE,
 		struct vb_device_info *pVBInfo)
 {
 
@@ -3780,7 +3728,7 @@ void XGI_DisplayOff(struct xgi_hw_device_info *pXGIHWDE,
 		if (pVBInfo->IF_DEF_LVDS == 1) {
 			/* LVDS backlight off */
 			XGI_XG21BLSignalVDD(0x02, 0x00, pVBInfo);
-			XGI_XG21SetPanelDelay(3, pVBInfo);
+			XGI_XG21SetPanelDelay(xgifb_info, 3, pVBInfo);
 		} else {
 			/* DVO/DVI signal off */
 			XGI_XG21BLSignalVDD(0x20, 0x00, pVBInfo);
@@ -3791,7 +3739,7 @@ void XGI_DisplayOff(struct xgi_hw_device_info *pXGIHWDE,
 		if ((XGI_XG27GetPSCValue(pVBInfo) & 0x2)) {
 			/* LVDS backlight off */
 			XGI_XG27BLSignalVDD(0x02, 0x00, pVBInfo);
-			XGI_XG21SetPanelDelay(3, pVBInfo);
+			XGI_XG21SetPanelDelay(xgifb_info, 3, pVBInfo);
 		}
 
 		if (pVBInfo->IF_DEF_LVDS == 0)
@@ -3838,26 +3786,17 @@ static void XGI_GetCRT2ResInfo(unsigned short ModeNo,
 	if (ModeNo <= 0x13) {
 		xres = pVBInfo->StResInfo[resindex].HTotal;
 		yres = pVBInfo->StResInfo[resindex].VTotal;
-		/* si+St_ResInfo */
-		/* modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;*/
 	} else {
 		xres = pVBInfo->ModeResInfo[resindex].HTotal; /* xres->ax */
 		yres = pVBInfo->ModeResInfo[resindex].VTotal; /* yres->bx */
 		/* si+St_ModeFlag */
 		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 
-		/*
-		if (pVBInfo->IF_DEF_FSTN) {
-			xres *= 2;
-			yres *= 2;
-		 } else {
-		*/
 		if (modeflag & HalfDCLK)
 			xres *= 2;
 
 		if (modeflag & DoubleScanMode)
 			yres *= 2;
-		/* } */
 	}
 
 	if (pVBInfo->VBInfo & SetCRT2ToLCD) {
@@ -4028,8 +3967,6 @@ static void XGI_GetCRT2Data(unsigned short ModeNo, unsigned short ModeIdIndex,
 					tempbx = 775;
 				else if (pVBInfo->VGAVDE == 600)
 					tempbx = 775;
-				/* else if (pVBInfo->VGAVDE==350) tempbx=560; */
-				/* else if (pVBInfo->VGAVDE==400) tempbx=640; */
 				else
 					tempbx = 768;
 			} else
@@ -4294,7 +4231,6 @@ static void XGI_PreSetGroup1(unsigned short ModeNo, unsigned short ModeIdIndex,
 	XGI_SetCRT2Offset(ModeNo, ModeIdIndex, RefreshRateTableIndex,
 			HwDeviceExtension, pVBInfo);
 	XGI_SetCRT2FIFO(pVBInfo);
-	/* XGI_SetCRT2Sync(ModeNo,RefreshRateTableIndex); */
 
 	for (tempcx = 4; tempcx < 7; tempcx++)
 		xgifb_reg_set(pVBInfo->Part1Port, tempcx, 0x0);
@@ -4497,9 +4433,6 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 
 	temp = 0xFF; /* set MAX HT */
 	xgifb_reg_set(pVBInfo->Part1Port, 0x03, temp);
-	/* if (modeflag & Charx8Dot) */
-	/*	tempcx = 0x08; */
-	/* else */
 	tempcx = 0x08;
 
 	if (pVBInfo->VBType & (VB_XGI301LV | VB_XGI302LV | VB_XGI301C))
@@ -4565,7 +4498,6 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 			}
 		}
 	} else {
-		/* tempcx = tempbx & 0x00FF ; */
 		tempbx = (tempbx & 0xFF00) >> 8;
 		tempcx = (tempcx + tempbx) >> 1;
 		temp = (tempcx & 0x00FF) + 2;
@@ -4579,36 +4511,23 @@ static void XGI_SetLockRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 						temp -= 6;
 				}
 			}
-		} else {
-			if (!(modeflag & HalfDCLK)) {
-				temp -= 4;
-				if (pVBInfo->LCDResInfo != Panel1280x960) {
-					if (pVBInfo->VGAHDE >= 800) {
-						temp -= 7;
-						if (pVBInfo->ModeType ==
-							ModeEGA) {
-							if (pVBInfo->VGAVDE ==
-							    1024) {
-								temp += 15;
-								if (pVBInfo->LCDResInfo != Panel1280x1024) {
-									temp +=
-									    7;
-								}
-							}
-						}
-
-						if (pVBInfo->VGAHDE >= 1280) {
-							if (pVBInfo->LCDResInfo
-									!= Panel1280x960) {
-								if (pVBInfo->LCDInfo
-										& LCDNonExpanding) {
-									temp
-											+= 28;
-								}
-							}
-						}
-					}
+		} else if (!(modeflag & HalfDCLK)) {
+			temp -= 4;
+			if (pVBInfo->LCDResInfo != Panel1280x960 &&
+			    pVBInfo->VGAHDE >= 800) {
+				temp -= 7;
+				if (pVBInfo->ModeType == ModeEGA &&
+				    pVBInfo->VGAVDE == 1024) {
+					temp += 15;
+					if (pVBInfo->LCDResInfo !=
+						Panel1280x1024)
+						temp += 7;
 				}
+
+				if (pVBInfo->VGAHDE >= 1280 &&
+				    pVBInfo->LCDResInfo != Panel1280x960 &&
+				    (pVBInfo->LCDInfo & LCDNonExpanding))
+					temp += 28;
 			}
 		}
 	}
@@ -5297,7 +5216,6 @@ static void XGI_SetGroup2(unsigned short ModeNo, unsigned short ModeIdIndex,
 		tempax--;
 		xgifb_reg_and(pVBInfo->Part2Port, 0x01, tempax);
 
-		/* if ( !( pVBInfo->VBType & VB_XGI301C ) ) */
 		xgifb_reg_and(pVBInfo->Part2Port, 0x00, 0xEF);
 	}
 
@@ -5436,7 +5354,6 @@ static void XGI_SetLCDRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	tempax = pVBInfo->VT;
 	tempbx = pVBInfo->LCDVRS;
 
-	/* if (SetLCD_Info & EnableScalingLCD) */
 	tempcx += tempbx;
 	if (tempcx >= tempax)
 		tempcx -= tempax;
@@ -5478,12 +5395,10 @@ static void XGI_SetLCDRegs(unsigned short ModeNo, unsigned short ModeIdIndex,
 	temp = (tempcx & 0xFF00) >> 8;
 	xgifb_reg_set(pVBInfo->Part2Port, 0x25, temp);
 
-	/* getlcdsync() */
 	XGI_GetLCDSync(&tempax, &tempbx, pVBInfo);
 	tempcx = tempax;
 	tempax = pVBInfo->HT;
 	tempbx = pVBInfo->LCDHRS;
-	/* if ( SetLCD_Info & EnableScalingLCD) */
 	if (XGI_IsLCDDualLink(pVBInfo)) {
 		tempax = tempax >> 1;
 		tempbx = tempbx >> 1;
@@ -5801,9 +5716,6 @@ static void XGI_SetGroup4(unsigned short ModeNo, unsigned short ModeIdIndex,
 		if (XGI_IsLCDDualLink(pVBInfo))
 			tempax = tempax >> 1;
 
-		/* if((pVBInfo->VBInfo&(SetCRT2ToLCD)) ||
-		      ((pVBInfo->TVInfo&SetYPbPrMode525p) ||
-		      (pVBInfo->TVInfo&SetYPbPrMode750p))) { */
 		if (pVBInfo->VBInfo & SetCRT2ToLCD) {
 			if (tempax > 800)
 				tempax -= 800;
@@ -5817,33 +5729,6 @@ static void XGI_SetGroup4(unsigned short ModeNo, unsigned short ModeIdIndex,
 		}
 		tempax -= 1;
 
-		/*
-		if (pVBInfo->VBInfo & (SetCRT2ToTV | SetCRT2ToHiVisionTV)) {
-			if (pVBInfo->VBType & VB_XGI301LV) {
-				if (!(pVBInfo->TVInfo &
-				      (SetYPbPrMode525p |
-				       SetYPbPrMode750p |
-				       SetYPbPrMode1080i))) {
-					if (pVBInfo->VGAHDE > 800) {
-						if (pVBInfo->VGAHDE == 1024)
-							tempax =(tempax * 25 /
-								 32) - 1;
-						else
-							tempax = (tempax * 20 /
-								  32) - 1;
-					}
-				}
-			} else {
-				if (pVBInfo->VGAHDE > 800) {
-					if (pVBInfo->VGAHDE == 1024)
-						tempax = (tempax * 25 / 32) - 1;
-					else
-						tempax = (tempax * 20 / 32) - 1;
-				}
-			}
-		}
-		*/
-
 		temp = (tempax & 0xFF00) >> 8;
 		temp = ((temp & 0x0003) << 4);
 		xgifb_reg_set(pVBInfo->Part4Port, 0x1E, temp);
@@ -5902,7 +5787,6 @@ static void XGI_SetGroup5(unsigned short ModeNo, unsigned short ModeIdIndex,
 		if (!(pVBInfo->VBInfo & (SetInSlaveMode | LoadDACFlag
 				| CRT2DisplayFlag))) {
 			XGINew_EnableCRT2(pVBInfo);
-			/* LoadDAC2(pVBInfo->Part5Port, ModeNo, ModeIdIndex); */
 		}
 	}
 	return;
@@ -5921,118 +5805,11 @@ static void XGI_DisableGatingCRT(struct xgi_hw_device_info *HwDeviceExtension,
 	xgifb_reg_and_or(pVBInfo->P3d4, 0x63, 0xBF, 0x00);
 }
 
-/*----------------------------------------------------------------------------*/
-/* input                                                                      */
-/*      bl[5] : 1;LVDS signal on                                              */
-/*      bl[1] : 1;LVDS backlight on                                           */
-/*      bl[0] : 1:LVDS VDD on                                                 */
-/*      bh: 100000b : clear bit 5, to set bit5                                */
-/*          000010b : clear bit 1, to set bit1                                */
-/*          000001b : clear bit 0, to set bit0                                */
-/*----------------------------------------------------------------------------*/
-void XGI_XG21BLSignalVDD(unsigned short tempbh, unsigned short tempbl,
-		struct vb_device_info *pVBInfo)
-{
-	unsigned char CR4A, temp;
-
-	CR4A = xgifb_reg_get(pVBInfo->P3d4, 0x4A);
-	tempbh &= 0x23;
-	tempbl &= 0x23;
-	xgifb_reg_and(pVBInfo->P3d4, 0x4A, ~tempbh); /* enable GPIO write */
-
-	if (tempbh & 0x20) {
-		temp = (tempbl >> 4) & 0x02;
-
-		/* CR B4[1] */
-		xgifb_reg_and_or(pVBInfo->P3d4, 0xB4, ~0x02, temp);
-
-	}
-
-	temp = xgifb_reg_get(pVBInfo->P3d4, 0x48);
-
-	temp = XG21GPIODataTransfer(temp);
-	temp &= ~tempbh;
-	temp |= tempbl;
-	xgifb_reg_set(pVBInfo->P3d4, 0x48, temp);
-}
-
-void XGI_XG27BLSignalVDD(unsigned short tempbh, unsigned short tempbl,
-		struct vb_device_info *pVBInfo)
-{
-	unsigned char CR4A, temp;
-	unsigned short tempbh0, tempbl0;
-
-	tempbh0 = tempbh;
-	tempbl0 = tempbl;
-	tempbh0 &= 0x20;
-	tempbl0 &= 0x20;
-	tempbh0 >>= 3;
-	tempbl0 >>= 3;
-
-	if (tempbh & 0x20) {
-		temp = (tempbl >> 4) & 0x02;
-
-		/* CR B4[1] */
-		xgifb_reg_and_or(pVBInfo->P3d4, 0xB4, ~0x02, temp);
-
-	}
-	xgifb_reg_and_or(pVBInfo->P3d4, 0xB4, ~tempbh0, tempbl0);
-
-	CR4A = xgifb_reg_get(pVBInfo->P3d4, 0x4A);
-	tempbh &= 0x03;
-	tempbl &= 0x03;
-	tempbh <<= 2;
-	tempbl <<= 2; /* GPIOC,GPIOD */
-	xgifb_reg_and(pVBInfo->P3d4, 0x4A, ~tempbh); /* enable GPIO write */
-	xgifb_reg_and_or(pVBInfo->P3d4, 0x48, ~tempbh, tempbl);
-}
-
-/* --------------------------------------------------------------------- */
-unsigned short XGI_GetLVDSOEMTableIndex(struct vb_device_info *pVBInfo)
-{
-	unsigned short index;
-
-	index = xgifb_reg_get(pVBInfo->P3d4, 0x36);
-	if (index < sizeof(XGI21_LCDCapList)
-			/ sizeof(struct XGI21_LVDSCapStruct))
-		return index;
-	return 0;
-}
-
-/* --------------------------------------------------------------------- */
-/* Function : XGI_XG21SetPanelDelay */
-/* Input : */
-/* Output : */
-/* Description : */
-/* I/P : bl : 1 ; T1 : the duration between CPL on and signal on */
-/* : bl : 2 ; T2 : the duration signal on and Vdd on */
-/* : bl : 3 ; T3 : the duration between CPL off and signal off */
-/* : bl : 4 ; T4 : the duration signal off and Vdd off */
-/* --------------------------------------------------------------------- */
-void XGI_XG21SetPanelDelay(unsigned short tempbl,
+static unsigned char XGI_XG21CheckLVDSMode(struct xgifb_video_info *xgifb_info,
+		unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
 {
-	unsigned short index;
-
-	index = XGI_GetLVDSOEMTableIndex(pVBInfo);
-	if (tempbl == 1)
-		mdelay(pVBInfo->XG21_LVDSCapList[index].PSC_S1);
-
-	if (tempbl == 2)
-		mdelay(pVBInfo->XG21_LVDSCapList[index].PSC_S2);
-
-	if (tempbl == 3)
-		mdelay(pVBInfo->XG21_LVDSCapList[index].PSC_S3);
-
-	if (tempbl == 4)
-		mdelay(pVBInfo->XG21_LVDSCapList[index].PSC_S4);
-}
-
-unsigned char XGI_XG21CheckLVDSMode(unsigned short ModeNo,
-		unsigned short ModeIdIndex, struct vb_device_info *pVBInfo)
-{
-	unsigned short xres, yres, colordepth, modeflag, resindex,
-			lvdstableindex;
+	unsigned short xres, yres, colordepth, modeflag, resindex;
 
 	resindex = XGI_GetResInfo(ModeNo, ModeIdIndex, pVBInfo);
 	if (ModeNo <= 0x13) {
@@ -6061,18 +5838,15 @@ unsigned char XGI_XG21CheckLVDSMode(unsigned short ModeNo,
 
 	}
 
-	lvdstableindex = XGI_GetLVDSOEMTableIndex(pVBInfo);
-	if (xres > (pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE))
+	if (xres > xgifb_info->lvds_data.LVDSHDE)
 		return 0;
 
-	if (yres > (pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE))
+	if (yres > xgifb_info->lvds_data.LVDSVDE)
 		return 0;
 
 	if (ModeNo > 0x13) {
-		if ((xres != (pVBInfo->XG21_LVDSCapList[lvdstableindex].
-				LVDSHDE)) ||
-		    (yres != (pVBInfo->XG21_LVDSCapList[lvdstableindex].
-				LVDSVDE))) {
+		if (xres != xgifb_info->lvds_data.LVDSHDE ||
+		    yres != xgifb_info->lvds_data.LVDSVDE) {
 			colordepth = XGI_GetColorDepth(ModeNo,
 						       ModeIdIndex,
 						       pVBInfo);
@@ -6084,55 +5858,26 @@ unsigned char XGI_XG21CheckLVDSMode(unsigned short ModeNo,
 	return 1;
 }
 
-void XGI_SetXG21FPBits(struct vb_device_info *pVBInfo)
-{
-	unsigned char temp;
-
-	temp = xgifb_reg_get(pVBInfo->P3d4, 0x37); /* D[0] 1: 18bit */
-	temp = (temp & 1) << 6;
-	/* SR06[6] 18bit Dither */
-	xgifb_reg_and_or(pVBInfo->P3c4, 0x06, ~0x40, temp);
-	/* SR09[7] enable FP output, SR09[6] 1: sigle 18bits, 0: dual 12bits */
-	xgifb_reg_and_or(pVBInfo->P3c4, 0x09, ~0xc0, temp | 0x80);
-
-}
-
-void XGI_SetXG27FPBits(struct vb_device_info *pVBInfo)
-{
-	unsigned char temp;
-
-	/* D[1:0] 01: 18bit, 00: dual 12, 10: single 24 */
-	temp = xgifb_reg_get(pVBInfo->P3d4, 0x37);
-	temp = (temp & 3) << 6;
-	/* SR06[7]0: dual 12/1: single 24 [6] 18bit Dither <= 0 h/w recommend */
-	xgifb_reg_and_or(pVBInfo->P3c4, 0x06, ~0xc0, temp & 0x80);
-	/* SR09[7] enable FP output, SR09[6] 1: sigle 18bits, 0: 24bits */
-	xgifb_reg_and_or(pVBInfo->P3c4, 0x09, ~0xc0, temp | 0x80);
-
-}
-
-static void xgifb_set_lvds(int chip_id,
+static void xgifb_set_lvds(struct xgifb_video_info *xgifb_info,
+			   int chip_id,
 			   unsigned short ModeNo,
 			   unsigned short ModeIdIndex,
 			   struct vb_device_info *pVBInfo)
 {
 	unsigned char temp, Miscdata;
-	unsigned short xres, yres, modeflag, resindex, lvdstableindex;
+	unsigned short xres, yres, modeflag, resindex;
 	unsigned short LVDSHT, LVDSHBS, LVDSHRS, LVDSHRE, LVDSHBE;
 	unsigned short LVDSVT, LVDSVBS, LVDSVRS, LVDSVRE, LVDSVBE;
 	unsigned short value;
 
-	lvdstableindex = XGI_GetLVDSOEMTableIndex(pVBInfo);
-	temp = (unsigned char) ((pVBInfo->XG21_LVDSCapList[lvdstableindex].
-					LVDS_Capability &
+	temp = (unsigned char) ((xgifb_info->lvds_data.LVDS_Capability &
 				(LCDPolarity << 8)) >> 8);
 	temp &= LCDPolarity;
 	Miscdata = (unsigned char) inb(pVBInfo->P3cc);
 
 	outb((Miscdata & 0x3F) | temp, pVBInfo->P3c2);
 
-	temp = (unsigned char) (pVBInfo->XG21_LVDSCapList[lvdstableindex].
-					LVDS_Capability & LCDPolarity);
+	temp = xgifb_info->lvds_data.LVDS_Capability & LCDPolarity;
 	/* SR35[7] FP VSync polarity */
 	xgifb_reg_and_or(pVBInfo->P3c4, 0x35, ~0x80, temp & 0x80);
 	/* SR30[5] FP HSync polarity */
@@ -6159,48 +5904,43 @@ static void xgifb_set_lvds(int chip_id,
 	if (!(modeflag & Charx8Dot))
 		xres = xres * 8 / 9;
 
-	LVDSHT = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHT;
+	LVDSHT = xgifb_info->lvds_data.LVDSHT;
 
-	LVDSHBS = xres + (pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE
-			- xres) / 2;
+	LVDSHBS = xres + (xgifb_info->lvds_data.LVDSHDE - xres) / 2;
 	if ((ModeNo <= 0x13) && (modeflag & HalfDCLK))
 		LVDSHBS -= xres / 4;
 
 	if (LVDSHBS > LVDSHT)
 		LVDSHBS -= LVDSHT;
 
-	LVDSHRS = LVDSHBS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHFP;
+	LVDSHRS = LVDSHBS + xgifb_info->lvds_data.LVDSHFP;
 	if (LVDSHRS > LVDSHT)
 		LVDSHRS -= LVDSHT;
 
-	LVDSHRE = LVDSHRS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHSYNC;
+	LVDSHRE = LVDSHRS + xgifb_info->lvds_data.LVDSHSYNC;
 	if (LVDSHRE > LVDSHT)
 		LVDSHRE -= LVDSHT;
 
-	LVDSHBE = LVDSHBS + LVDSHT
-			- pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE;
+	LVDSHBE = LVDSHBS + LVDSHT - xgifb_info->lvds_data.LVDSHDE;
 
-	LVDSVT = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVT;
+	LVDSVT = xgifb_info->lvds_data.LVDSVT;
 
-	LVDSVBS = yres + (pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE
-			- yres) / 2;
+	LVDSVBS = yres + (xgifb_info->lvds_data.LVDSVDE - yres) / 2;
 	if ((ModeNo > 0x13) && (modeflag & DoubleScanMode))
 		LVDSVBS += yres / 2;
 
 	if (LVDSVBS > LVDSVT)
 		LVDSVBS -= LVDSVT;
 
-	LVDSVRS = LVDSVBS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVFP;
+	LVDSVRS = LVDSVBS + xgifb_info->lvds_data.LVDSVFP;
 	if (LVDSVRS > LVDSVT)
 		LVDSVRS -= LVDSVT;
 
-	LVDSVRE = LVDSVRS + pVBInfo->XG21_LVDSCapList[lvdstableindex].
-					LVDSVSYNC;
+	LVDSVRE = LVDSVRS + xgifb_info->lvds_data.LVDSVSYNC;
 	if (LVDSVRE > LVDSVT)
 		LVDSVRE -= LVDSVT;
 
-	LVDSVBE = LVDSVBS + LVDSVT
-			- pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE;
+	LVDSVBE = LVDSVBS + LVDSVT - xgifb_info->lvds_data.LVDSVDE;
 
 	temp = (unsigned char) xgifb_reg_get(pVBInfo->P3d4, 0x11);
 	xgifb_reg_set(pVBInfo->P3d4, 0x11, temp & 0x7f); /* Unlock CRTC */
@@ -6300,13 +6040,9 @@ static void xgifb_set_lvds(int chip_id,
 
 		xgifb_reg_and_or(pVBInfo->P3c4, 0x31, ~0x30, value);
 		xgifb_reg_set(pVBInfo->P3c4,
-			      0x2B,
-			      pVBInfo->XG21_LVDSCapList[lvdstableindex].
-					VCLKData1);
+			      0x2B, xgifb_info->lvds_data.VCLKData1);
 		xgifb_reg_set(pVBInfo->P3c4,
-			      0x2C,
-			      pVBInfo->XG21_LVDSCapList[lvdstableindex].
-					VCLKData2);
+			      0x2C, xgifb_info->lvds_data.VCLKData2);
 		value += 0x10;
 	}
 
@@ -6398,7 +6134,8 @@ static unsigned char XGI_EnableChISLCD(struct vb_device_info *pVBInfo)
 	return 0;
 }
 
-void XGI_DisableBridge(struct xgi_hw_device_info *HwDeviceExtension,
+static void XGI_DisableBridge(struct xgifb_video_info *xgifb_info,
+		struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned short tempah = 0;
@@ -6442,7 +6179,7 @@ void XGI_DisableBridge(struct xgi_hw_device_info *HwDeviceExtension,
 						| SetSimuScanMode))) {
 			if (pVBInfo->SetFlag & GatingCRT)
 				XGI_EnableGatingCRT(HwDeviceExtension, pVBInfo);
-			XGI_DisplayOff(HwDeviceExtension, pVBInfo);
+			XGI_DisplayOff(xgifb_info, HwDeviceExtension, pVBInfo);
 		}
 
 		if (pVBInfo->VBInfo & SetCRT2ToLCDA) {
@@ -6464,7 +6201,6 @@ void XGI_DisableBridge(struct xgi_hw_device_info *HwDeviceExtension,
 		    ((!(pVBInfo->VBInfo & SetCRT2ToLCDA)) &&
 		    (pVBInfo->VBInfo &
 			(SetCRT2ToRAMDAC | SetCRT2ToLCD | SetCRT2ToTV))))
-			/* BScreenOff=1 */
 			xgifb_reg_or(pVBInfo->Part1Port, 0x00, 0x80);
 
 		if ((pVBInfo->SetFlag & DisableChB) ||
@@ -6484,7 +6220,6 @@ void XGI_DisableBridge(struct xgi_hw_device_info *HwDeviceExtension,
 		}
 	} else { /* {301} */
 		if (pVBInfo->VBInfo & (SetCRT2ToLCD | SetCRT2ToTV)) {
-			/* BScreenOff=1 */
 			xgifb_reg_or(pVBInfo->Part1Port, 0x00, 0x80);
 			/* Disable CRT2 */
 			xgifb_reg_and(pVBInfo->Part1Port, 0x1E, 0xDF);
@@ -6494,7 +6229,7 @@ void XGI_DisableBridge(struct xgi_hw_device_info *HwDeviceExtension,
 
 		if (pVBInfo->VBInfo & (DisableCRT2Display | SetCRT2ToLCDA
 				| SetSimuScanMode))
-			XGI_DisplayOff(HwDeviceExtension, pVBInfo);
+			XGI_DisplayOff(xgifb_info, HwDeviceExtension, pVBInfo);
 	}
 }
 
@@ -6610,17 +6345,6 @@ static void XGI_SetDelayComp(struct vb_device_info *pVBInfo)
 
 			if (pVBInfo->VBInfo & SetCRT2ToDualEdge)
 				tempbl = tempbl >> 4;
-			/*
-			if (pVBInfo->VBInfo & SetCRT2ToRAMDAC)
-				tempbl = CRT2Delay1;	// Get CRT2 Delay
-			if (pVBInfo->VBType &
-			    (VB_XGI301B |
-			     VB_XGI302B |
-			     VB_XGI301LV |
-			     VB_XGI302LV |
-			     VB_XGI301C))
-				tempbl = CRT2Delay2;
-			*/
 			if (pVBInfo->VBInfo & (SetCRT2ToLCD | SetCRT2ToLCDA)) {
 				/* Get LCD Delay */
 				index = XGI_GetLCDCapPtr(pVBInfo);
@@ -6680,23 +6404,6 @@ static void XGI_SetLCDCap_A(unsigned short tempcx,
 				(unsigned short) (0x30 | (tempcx & 0x00C0)));
 		xgifb_reg_and_or(pVBInfo->Part1Port, 0x1A, 0x7F, 0x00);
 	}
-
-	/*
-	if (tempcx & EnableLCD24bpp) {	// 24bits
-		xgifb_reg_and_or(pVBInfo->Part1Port,
-				 0x19,
-				 0x0F,
-				 (unsigned short)(0x30 | (tempcx&0x00C0)));
-		xgifb_reg_and_or(pVBInfo->Part1Port, 0x1A, 0x7F, 0x00);
-	} else {
-		xgifb_reg_and_or(pVBInfo->Part1Port,
-				 0x19,
-				 0x0F,
-				 // Enable Dither
-				 (unsigned short)(0x20 | (tempcx&0x00C0)));
-		xgifb_reg_and_or(pVBInfo->Part1Port, 0x1A, 0x7F, 0x80);
-	}
-	*/
 }
 
 /* --------------------------------------------------------------------- */
@@ -6718,6 +6425,25 @@ static void XGI_SetLCDCap_B(unsigned short tempcx,
 						| 0x18)); /* Enable Dither */
 }
 
+static void XGI_LongWait(struct vb_device_info *pVBInfo)
+{
+	unsigned short i;
+
+	i = xgifb_reg_get(pVBInfo->P3c4, 0x1F);
+
+	if (!(i & 0xC0)) {
+		for (i = 0; i < 0xFFFF; i++) {
+			if (!(inb(pVBInfo->P3da) & 0x08))
+				break;
+		}
+
+		for (i = 0; i < 0xFFFF; i++) {
+			if ((inb(pVBInfo->P3da) & 0x08))
+				break;
+		}
+	}
+}
+
 static void SetSpectrum(struct vb_device_info *pVBInfo)
 {
 	unsigned short index;
@@ -6940,14 +6666,12 @@ static void XGI_OEM310Setting(unsigned short ModeNo,
 			      unsigned short ModeIdIndex,
 			      struct vb_device_info *pVBInfo)
 {
-	/* GetPart1IO(); */
 	XGI_SetDelayComp(pVBInfo);
 
 	if (pVBInfo->VBInfo & (SetCRT2ToLCD | SetCRT2ToLCDA))
 		XGI_SetLCDCap(pVBInfo);
 
 	if (pVBInfo->VBInfo & SetCRT2ToTV) {
-		/* GetPart2IO() */
 		XGI_SetPhaseIncr(pVBInfo);
 		XGI_SetYFilter(ModeNo, ModeIdIndex, pVBInfo);
 		XGI_SetAntiFlicker(ModeNo, ModeIdIndex, pVBInfo);
@@ -6963,7 +6687,7 @@ static void XGI_OEM310Setting(unsigned short ModeNo,
 /* Output : */
 /* Description : Origin code for crt2group */
 /* --------------------------------------------------------------------- */
-void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
+static void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 		struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
@@ -6972,8 +6696,6 @@ void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 
 	unsigned char tempah;
 
-	/* // fix write part1 index 0 BTDRAM bit Bug
-	 * xgifb_reg_set(pVBInfo->Part1Port, 0x03, 0x00); */
 	tempah = 0;
 	if (!(pVBInfo->VBInfo & DisableCRT2Display)) {
 		tempah = xgifb_reg_get(pVBInfo->Part1Port, 0x00);
@@ -6999,32 +6721,6 @@ void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 		}
 	}
 
-	/* 0210 shampoo
-	if (pVBInfo->VBInfo & DisableCRT2Display) {
-		tempah = 0;
-	}
-
-	xgifb_reg_set(pVBInfo->Part1Port, 0x00, tempah);
-	if (pVBInfo->VBInfo & (SetCRT2ToRAMDAC | SetCRT2ToTV | SetCRT2ToLCD)) {
-		tempcl = pVBInfo->ModeType;
-		if (ModeNo > 0x13) {
-			tempcl -= ModeVGA;
-			if ((tempcl > 0) || (tempcl == 0)) {
-				tempah=(0x008>>tempcl) ;
-				if (tempah == 0)
-					tempah = 1;
-				tempah |= 0x040;
-			}
-		} else {
-			tempah = 0x040;
-		}
-
-		if (pVBInfo->VBInfo & SetInSlaveMode) {
-			tempah = (tempah ^ 0x050);
-		}
-	}
-	*/
-
 	xgifb_reg_set(pVBInfo->Part1Port, 0x00, tempah);
 	tempah = 0x08;
 	tempbl = 0xf0;
@@ -7093,14 +6789,11 @@ void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 		tempah |= 0x080;
 
 		if (pVBInfo->VBInfo & SetCRT2ToTV) {
-			/* if (!(pVBInfo->TVInfo &
-				 (SetYPbPrMode525p | SetYPbPrMode750p))) { */
 			tempah |= 0x020;
 			if (ModeNo > 0x13) {
 				if (pVBInfo->VBInfo & DriverMode)
 					tempah = tempah ^ 0x20;
 			}
-			/* } */
 		}
 
 		xgifb_reg_and_or(pVBInfo->Part4Port, 0x0D, ~0x0BF, tempah);
@@ -7110,12 +6803,8 @@ void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
 			tempah |= 0x40;
 
 		if (pVBInfo->VBInfo & SetCRT2ToTV) {
-			/* if ((!(pVBInfo->VBInfo & SetCRT2ToHiVisionTV)) &&
-			       (!(pVBInfo->TVInfo &
-				  (SetYPbPrMode525p | SetYPbPrMode750p)))) { */
 			if (pVBInfo->TVInfo & RPLLDIV2XO)
 				tempah |= 0x40;
-			/* } */
 		}
 
 		if ((pVBInfo->LCDResInfo == Panel1280x1024)
@@ -7219,57 +6908,6 @@ unsigned char XGI_BridgeIsOn(struct vb_device_info *pVBInfo)
 	}
 }
 
-void XGI_LongWait(struct vb_device_info *pVBInfo)
-{
-	unsigned short i;
-
-	i = xgifb_reg_get(pVBInfo->P3c4, 0x1F);
-
-	if (!(i & 0xC0)) {
-		for (i = 0; i < 0xFFFF; i++) {
-			if (!(inb(pVBInfo->P3da) & 0x08))
-				break;
-		}
-
-		for (i = 0; i < 0xFFFF; i++) {
-			if ((inb(pVBInfo->P3da) & 0x08))
-				break;
-		}
-	}
-}
-
-static void XGI_VBLongWait(struct vb_device_info *pVBInfo)
-{
-	unsigned short tempal, temp, i, j;
-	return;
-	if (!(pVBInfo->VBInfo & SetCRT2ToTV)) {
-		temp = 0;
-		for (i = 0; i < 3; i++) {
-			for (j = 0; j < 100; j++) {
-				tempal = inb(pVBInfo->P3da);
-				if (temp & 0x01) { /* VBWaitMode2 */
-					if ((tempal & 0x08))
-						continue;
-
-					if (!(tempal & 0x08))
-						break;
-
-				} else { /* VBWaitMode1 */
-					if (!(tempal & 0x08))
-						continue;
-
-					if ((tempal & 0x08))
-						break;
-				}
-			}
-			temp = temp ^ 0x01;
-		}
-	} else {
-		XGI_LongWait(pVBInfo);
-	}
-	return;
-}
-
 unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 		unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
@@ -7322,12 +6960,6 @@ unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 	RefreshRateTableIndex = pVBInfo->EModeIDTable[ModeIdIndex].REFindex;
 	ModeNo = pVBInfo->RefIndex[RefreshRateTableIndex].ModeID;
 	if (pXGIHWDE->jChipType >= XG20) { /* for XG20, XG21, XG27 */
-		/*
-		if (pVBInfo->RefIndex[RefreshRateTableIndex].Ext_InfoFlag &
-		    XG2xNotSupport) {
-			index++;
-		}
-		*/
 		if ((pVBInfo->RefIndex[RefreshRateTableIndex].XRes == 800) &&
 		    (pVBInfo->RefIndex[RefreshRateTableIndex].YRes == 600)) {
 			index++;
@@ -7371,7 +7003,7 @@ unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 		temp = XGI_AjustCRT2Rate(ModeNo, ModeIdIndex,
 				RefreshRateTableIndex, &i, pVBInfo);
 	}
-	return RefreshRateTableIndex + i; /* return (0x01 | (temp1<<1)); */
+	return RefreshRateTableIndex + i;
 }
 
 static void XGI_SetLCDAGroup(unsigned short ModeNo, unsigned short ModeIdIndex,
@@ -7379,9 +7011,6 @@ static void XGI_SetLCDAGroup(unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned short RefreshRateTableIndex;
-	/* unsigned short temp ; */
-
-	/* pVBInfo->SelectCRT2Rate = 0; */
 
 	pVBInfo->SetFlag |= ProgrammingCRT2;
 	RefreshRateTableIndex = XGI_GetRatePtrCRT2(HwDeviceExtension, ModeNo,
@@ -7394,7 +7023,7 @@ static void XGI_SetLCDAGroup(unsigned short ModeNo, unsigned short ModeIdIndex,
 	XGI_SetCRT2ECLK(ModeNo, ModeIdIndex, RefreshRateTableIndex, pVBInfo);
 }
 
-unsigned char XGI_SetCRT2Group301(unsigned short ModeNo,
+static unsigned char XGI_SetCRT2Group301(unsigned short ModeNo,
 		struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
@@ -7499,10 +7128,6 @@ void XGI_SenseCRT1(struct vb_device_info *pVBInfo)
 		outb((unsigned char) DAC_TEST_PARMS[2], (pVBInfo->P3c8 + 1));
 	}
 
-	XGI_VBLongWait(pVBInfo);
-	XGI_VBLongWait(pVBInfo);
-	XGI_VBLongWait(pVBInfo);
-
 	mdelay(1);
 
 	XGI_WaitDisply(pVBInfo);
@@ -7532,7 +7157,8 @@ void XGI_SenseCRT1(struct vb_device_info *pVBInfo)
 	xgifb_reg_set(pVBInfo->P3c4, 0x1F, (unsigned char) SR1F);
 }
 
-void XGI_EnableBridge(struct xgi_hw_device_info *HwDeviceExtension,
+static void XGI_EnableBridge(struct xgifb_video_info *xgifb_info,
+		struct xgi_hw_device_info *HwDeviceExtension,
 		struct vb_device_info *pVBInfo)
 {
 	unsigned short tempah;
@@ -7544,7 +7170,6 @@ void XGI_EnableBridge(struct xgi_hw_device_info *HwDeviceExtension,
 				/* Power on */
 				xgifb_reg_set(pVBInfo->Part1Port, 0x1E, 0x20);
 			} else {
-				/* SetCRT2ToLCDA ) */
 				if (pVBInfo->VBInfo & SetCRT2ToDualEdge) {
 					/* Power on */
 					xgifb_reg_set(pVBInfo->Part1Port,
@@ -7572,10 +7197,8 @@ void XGI_EnableBridge(struct xgi_hw_device_info *HwDeviceExtension,
 						pVBInfo->Part1Port, 0x2E);
 
 				if (!(tempah & 0x80))
-					/* BVBDOENABLE = 1 */
 					xgifb_reg_or(pVBInfo->Part1Port,
 							0x2E, 0x80);
-				/* BScreenOFF = 0 */
 				xgifb_reg_and(pVBInfo->Part1Port, 0x00, 0x7F);
 			}
 		}
@@ -7638,12 +7261,11 @@ void XGI_EnableBridge(struct xgi_hw_device_info *HwDeviceExtension,
 		xgifb_reg_or(pVBInfo->Part4Port, 0x1F, tempah);
 
 		if (!(pVBInfo->SetFlag & DisableChA)) {
-			XGI_VBLongWait(pVBInfo);
 			if (!(pVBInfo->SetFlag & GatingCRT)) {
 				XGI_DisableGatingCRT(HwDeviceExtension,
 						     pVBInfo);
-				XGI_DisplayOn(HwDeviceExtension, pVBInfo);
-				XGI_VBLongWait(pVBInfo);
+				XGI_DisplayOn(xgifb_info, HwDeviceExtension,
+						pVBInfo);
 			}
 		}
 	} /* 301 */
@@ -7656,15 +7278,15 @@ void XGI_EnableBridge(struct xgi_hw_device_info *HwDeviceExtension,
 		tempah = (unsigned char) xgifb_reg_get(pVBInfo->Part1Port,
 				0x2E);
 		if (!(tempah & 0x80))
-			/* BVBDOENABLE = 1 */
 			xgifb_reg_or(pVBInfo->Part1Port, 0x2E, 0x80);
 
 		xgifb_reg_and(pVBInfo->Part1Port, 0x00, 0x7F);
-		XGI_DisplayOn(HwDeviceExtension, pVBInfo);
+		XGI_DisplayOn(xgifb_info, HwDeviceExtension, pVBInfo);
 	} /* End of VB */
 }
 
-static void XGI_SetCRT1Group(struct xgi_hw_device_info *HwDeviceExtension,
+static void XGI_SetCRT1Group(struct xgifb_video_info *xgifb_info,
+		struct xgi_hw_device_info *HwDeviceExtension,
 		unsigned short ModeNo, unsigned short ModeIdIndex,
 		struct vb_device_info *pVBInfo)
 {
@@ -7672,18 +7294,14 @@ static void XGI_SetCRT1Group(struct xgi_hw_device_info *HwDeviceExtension,
 
 	unsigned short XGINew_P3cc = pVBInfo->P3cc;
 
-	/* XGINew_CRT1Mode = ModeNo; // SaveModeID */
 	StandTableIndex = XGI_GetModePtr(ModeNo, ModeIdIndex, pVBInfo);
-	/* XGI_SetBIOSData(ModeNo, ModeIdIndex); */
-	/* XGI_ClearBankRegs(ModeNo, ModeIdIndex); */
 	XGI_SetSeqRegs(ModeNo, StandTableIndex, ModeIdIndex, pVBInfo);
-	XGI_SetMiscRegs(StandTableIndex, pVBInfo);
+	outb(pVBInfo->StandTable[StandTableIndex].MISC, pVBInfo->P3c2);
 	XGI_SetCRTCRegs(HwDeviceExtension, StandTableIndex, pVBInfo);
 	XGI_SetATTRegs(ModeNo, StandTableIndex, ModeIdIndex, pVBInfo);
 	XGI_SetGRCRegs(StandTableIndex, pVBInfo);
 	XGI_ClearExt1Regs(pVBInfo);
 
-	/* if (pVBInfo->IF_DEF_ExpLink) */
 	if (HwDeviceExtension->jChipType == XG27) {
 		if (pVBInfo->IF_DEF_LVDS == 0)
 			XGI_SetDefaultVCLK(pVBInfo);
@@ -7735,11 +7353,6 @@ static void XGI_SetCRT1Group(struct xgi_hw_device_info *HwDeviceExtension,
 		temp = xgifb_reg_get(pVBInfo->P3d4, 0x38);
 		if (temp & 0xA0) {
 
-			/* Enable write GPIOF */
-			/* xgifb_reg_and(pVBInfo->P3d4, 0x4A, ~0x20); */
-			/* P. DWN */
-			/* xgifb_reg_and(pVBInfo->P3d4, 0x48, ~0x20); */
-			/* XG21 CRT1 Timing */
 			if (HwDeviceExtension->jChipType == XG27)
 				XGI_SetXG27CRTC(ModeNo, ModeIdIndex,
 						RefreshRateTableIndex, pVBInfo);
@@ -7754,10 +7367,9 @@ static void XGI_SetCRT1Group(struct xgi_hw_device_info *HwDeviceExtension,
 					pVBInfo, RefreshRateTableIndex, ModeNo);
 
 			if (pVBInfo->IF_DEF_LVDS == 1)
-				xgifb_set_lvds(HwDeviceExtension->jChipType,
+				xgifb_set_lvds(xgifb_info,
+						HwDeviceExtension->jChipType,
 						ModeNo, ModeIdIndex, pVBInfo);
-			/* P. ON */
-			/* xgifb_reg_or(pVBInfo->P3d4, 0x48, 0x20); */
 		}
 	}
 
@@ -7765,22 +7377,16 @@ static void XGI_SetCRT1Group(struct xgi_hw_device_info *HwDeviceExtension,
 	XGI_SetCRT1FIFO(ModeNo, HwDeviceExtension, pVBInfo);
 	XGI_SetCRT1ModeRegs(HwDeviceExtension, ModeNo, ModeIdIndex,
 			RefreshRateTableIndex, pVBInfo);
-
-	/* XGI_LoadCharacter(); //dif ifdef TVFont */
-
 	XGI_LoadDAC(ModeNo, ModeIdIndex, pVBInfo);
-	/* XGI_ClearBuffer(HwDeviceExtension, ModeNo, pVBInfo); */
 }
 
-unsigned char XGISetModeNew(struct xgi_hw_device_info *HwDeviceExtension,
+unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
+			struct xgi_hw_device_info *HwDeviceExtension,
 			unsigned short ModeNo)
 {
 	unsigned short ModeIdIndex;
-	/* unsigned char *pVBInfo->FBAddr =
-				HwDeviceExtension->pjVideoMemoryAddress; */
 	struct vb_device_info VBINF;
 	struct vb_device_info *pVBInfo = &VBINF;
-	pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase;
 	pVBInfo->BaseAddr = (unsigned long) HwDeviceExtension->pjIOAddress;
 	pVBInfo->IF_DEF_LVDS = 0;
 	pVBInfo->IF_DEF_LCDA = 1;
@@ -7831,14 +7437,8 @@ unsigned char XGISetModeNew(struct xgi_hw_device_info *HwDeviceExtension,
 		XGI_GetVBType(pVBInfo);
 
 	InitTo330Pointer(HwDeviceExtension->jChipType, pVBInfo);
-	if (ModeNo & 0x80) {
+	if (ModeNo & 0x80)
 		ModeNo = ModeNo & 0x7F;
-		/* XGINew_flag_clearbuffer = 0; */
-	}
-	/* else {
-		XGINew_flag_clearbuffer = 1;
-	}
-	*/
 	xgifb_reg_set(pVBInfo->P3c4, 0x05, 0x86);
 
 	if (HwDeviceExtension->jChipType < XG20) /* kuku 2004/06/25 1.Openkey */
@@ -7846,16 +7446,14 @@ unsigned char XGISetModeNew(struct xgi_hw_device_info *HwDeviceExtension,
 
 	XGI_SearchModeID(ModeNo, &ModeIdIndex, pVBInfo);
 
-	XGI_GetVGAType(HwDeviceExtension, pVBInfo);
-
 	if (HwDeviceExtension->jChipType < XG20) { /* kuku 2004/06/25 */
 		XGI_GetVBInfo(ModeNo, ModeIdIndex, HwDeviceExtension, pVBInfo);
 		XGI_GetTVInfo(ModeNo, ModeIdIndex, pVBInfo);
 		XGI_GetLCDInfo(ModeNo, ModeIdIndex, pVBInfo);
-		XGI_DisableBridge(HwDeviceExtension, pVBInfo);
+		XGI_DisableBridge(xgifb_info, HwDeviceExtension, pVBInfo);
 
 		if (pVBInfo->VBInfo & (SetSimuScanMode | SetCRT2ToLCDA)) {
-			XGI_SetCRT1Group(HwDeviceExtension, ModeNo,
+			XGI_SetCRT1Group(xgifb_info, HwDeviceExtension, ModeNo,
 					ModeIdIndex, pVBInfo);
 
 			if (pVBInfo->VBInfo & SetCRT2ToLCDA) {
@@ -7864,7 +7462,8 @@ unsigned char XGISetModeNew(struct xgi_hw_device_info *HwDeviceExtension,
 			}
 		} else {
 			if (!(pVBInfo->VBInfo & SwitchToCRT2)) {
-				XGI_SetCRT1Group(HwDeviceExtension, ModeNo,
+				XGI_SetCRT1Group(xgifb_info,
+						HwDeviceExtension, ModeNo,
 						ModeIdIndex, pVBInfo);
 				if (pVBInfo->VBInfo & SetCRT2ToLCDA) {
 					XGI_SetLCDAGroup(ModeNo, ModeIdIndex,
@@ -7894,11 +7493,11 @@ unsigned char XGISetModeNew(struct xgi_hw_device_info *HwDeviceExtension,
 		XGI_SetCRT2ModeRegs(ModeNo, HwDeviceExtension, pVBInfo);
 		XGI_OEM310Setting(ModeNo, ModeIdIndex, pVBInfo); /*0212*/
 		XGI_CloseCRTC(HwDeviceExtension, pVBInfo);
-		XGI_EnableBridge(HwDeviceExtension, pVBInfo);
+		XGI_EnableBridge(xgifb_info, HwDeviceExtension, pVBInfo);
 	} /* !XG20 */
 	else {
 		if (pVBInfo->IF_DEF_LVDS == 1)
-			if (!XGI_XG21CheckLVDSMode(ModeNo,
+			if (!XGI_XG21CheckLVDSMode(xgifb_info, ModeNo,
 						   ModeIdIndex,
 						   pVBInfo))
 				return 0;
@@ -7914,39 +7513,13 @@ unsigned char XGISetModeNew(struct xgi_hw_device_info *HwDeviceExtension,
 		pVBInfo->SetFlag = 0;
 		pVBInfo->VBInfo = DisableCRT2Display;
 
-		XGI_DisplayOff(HwDeviceExtension, pVBInfo);
+		XGI_DisplayOff(xgifb_info, HwDeviceExtension, pVBInfo);
 
-		XGI_SetCRT1Group(HwDeviceExtension, ModeNo, ModeIdIndex,
-				pVBInfo);
+		XGI_SetCRT1Group(xgifb_info, HwDeviceExtension, ModeNo,
+				ModeIdIndex, pVBInfo);
 
-		XGI_DisplayOn(HwDeviceExtension, pVBInfo);
-		/*
-		if (HwDeviceExtension->jChipType == XG21)
-			xgifb_reg_and_or(pVBInfo->P3c4, 0x09, ~0x80, 0x80);
-		*/
-	}
-
-	/*
-	if (ModeNo <= 0x13) {
-		modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;
-	} else {
-		modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;
+		XGI_DisplayOn(xgifb_info, HwDeviceExtension, pVBInfo);
 	}
-	pVBInfo->ModeType = modeflag&ModeInfoFlag;
-	pVBInfo->SetFlag = 0x00;
-	pVBInfo->VBInfo = DisableCRT2Display;
-	temp = XGINew_CheckMemorySize(HwDeviceExtension,
-				      ModeNo,
-				      ModeIdIndex,
-				      pVBInfo);
-
-	if (temp == 0)
-		return (0);
-
-	XGI_DisplayOff(HwDeviceExtension, pVBInfo) ;
-	XGI_SetCRT1Group(HwDeviceExtension, ModeNo, ModeIdIndex, pVBInfo);
-	XGI_DisplayOn(HwDeviceExtension, pVBInfo);
-	*/
 
 	XGI_UpdateModeInfo(HwDeviceExtension, pVBInfo);
 
diff --git a/drivers/staging/xgifb/vb_setmode.h b/drivers/staging/xgifb/vb_setmode.h
index 1bd8667ff5cf..552482858c1c 100644
--- a/drivers/staging/xgifb/vb_setmode.h
+++ b/drivers/staging/xgifb/vb_setmode.h
@@ -6,66 +6,22 @@ extern void XGI_UnLockCRT2(struct xgi_hw_device_info *HwDeviceExtension,
 			   struct vb_device_info *);
 extern void XGI_LockCRT2(struct xgi_hw_device_info *HwDeviceExtension,
 			 struct vb_device_info *);
-extern void XGI_LongWait(struct vb_device_info *);
-extern void XGI_SetCRT2ModeRegs(unsigned short ModeNo,
-				struct xgi_hw_device_info *,
-				struct vb_device_info *);
-extern void XGI_DisableBridge(struct xgi_hw_device_info *HwDeviceExtension,
-			      struct vb_device_info *);
-extern void XGI_EnableBridge(struct xgi_hw_device_info *HwDeviceExtension,
-			     struct vb_device_info *);
-extern void XGI_DisplayOff(struct xgi_hw_device_info *,
+extern void XGI_DisplayOff(struct xgifb_video_info *,
+			   struct xgi_hw_device_info *,
 			   struct vb_device_info *);
-extern void XGI_DisplayOn(struct xgi_hw_device_info *,
-			  struct vb_device_info *);
 extern void XGI_GetVBType(struct vb_device_info *);
 extern void XGI_SenseCRT1(struct vb_device_info *);
-extern void XGI_GetVGAType(struct xgi_hw_device_info *HwDeviceExtension,
-			   struct vb_device_info *);
-extern void XGI_GetVBInfo(unsigned short ModeNo,
-			  unsigned short ModeIdIndex,
-			  struct xgi_hw_device_info *HwDeviceExtension,
-			  struct vb_device_info *);
-extern void XGI_GetTVInfo(unsigned short ModeNo,
-			  unsigned short ModeIdIndex,
-			  struct vb_device_info *);
-extern unsigned short XGI_GetResInfo(unsigned short ModeNo,
-				     unsigned short ModeIdIndex,
-				     struct vb_device_info *pVBInfo);
-
-extern unsigned char XGISetModeNew(struct xgi_hw_device_info *HwDeviceExtension,
+extern unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
+				   struct xgi_hw_device_info *HwDeviceExtension,
 				   unsigned short ModeNo) ;
 
 extern unsigned char XGI_SearchModeID(unsigned short ModeNo,
 				      unsigned short *ModeIdIndex,
 				      struct vb_device_info *);
-extern unsigned char XGI_GetLCDInfo(unsigned short ModeNo,
-				    unsigned short ModeIdIndex,
-				    struct vb_device_info *);
 extern unsigned char XGI_BridgeIsOn(struct vb_device_info *);
-
-extern unsigned char
-XGI_SetCRT2Group301(unsigned short ModeNo,
-		    struct xgi_hw_device_info *HwDeviceExtension,
-		    struct vb_device_info *);
 extern unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 					 unsigned short ModeNo,
 					 unsigned short ModeIdIndex,
 					 struct vb_device_info *);
 
-extern void XGI_SetXG21FPBits(struct vb_device_info *pVBInfo);
-extern void XGI_SetXG27FPBits(struct vb_device_info *pVBInfo);
-extern void XGI_XG21BLSignalVDD(unsigned short tempbh,
-				unsigned short tempbl,
-				struct vb_device_info *pVBInfo);
-extern void XGI_XG27BLSignalVDD(unsigned short tempbh,
-				unsigned short tempbl,
-				struct vb_device_info *pVBInfo);
-extern void XGI_XG21SetPanelDelay(unsigned short tempbl,
-				  struct vb_device_info *pVBInfo);
-extern unsigned char XGI_XG21CheckLVDSMode(unsigned short ModeNo,
-					   unsigned short ModeIdIndex,
-					   struct vb_device_info *pVBInfo);
-extern unsigned short XGI_GetLVDSOEMTableIndex(struct vb_device_info *pVBInfo);
-
 #endif
diff --git a/drivers/staging/xgifb/vb_struct.h b/drivers/staging/xgifb/vb_struct.h
index f9ade6f9f7ee..6556a0d6ff82 100644
--- a/drivers/staging/xgifb/vb_struct.h
+++ b/drivers/staging/xgifb/vb_struct.h
@@ -293,13 +293,12 @@ struct vb_device_info {
 	unsigned short   IF_DEF_ExpLink;
 	unsigned short   IF_DEF_HiVision;
 	unsigned short   LCDResInfo, LCDTypeInfo, VBType;/*301b*/
-	unsigned short   VBInfo, TVInfo, LCDInfo, Set_VGAType;
+	unsigned short   VBInfo, TVInfo, LCDInfo;
 	unsigned short   VBExtInfo;/*301lv*/
 	unsigned short   SetFlag;
 	unsigned short   NewFlickerMode;
 	unsigned short   SelectCRT2Rate;
 
-	unsigned char *ROMAddr;
 	void __iomem *FBAddr;
 	unsigned long BaseAddr;
 	unsigned long RelIO;
@@ -376,7 +375,6 @@ struct vb_device_info {
 	unsigned char   *pXGINew_CR97 ;
 
 	struct XGI330_LCDCapStruct  *LCDCapList;
-	struct XGI21_LVDSCapStruct  *XG21_LVDSCapList;
 
 	struct XGI_TimingHStruct  *TimingH;
 	struct XGI_TimingVStruct  *TimingV;
diff --git a/drivers/staging/xgifb/vb_table.h b/drivers/staging/xgifb/vb_table.h
index b81ac7726d1c..e7946f1c1143 100644
--- a/drivers/staging/xgifb/vb_table.h
+++ b/drivers/staging/xgifb/vb_table.h
@@ -2569,33 +2569,6 @@ static struct XGI330_LCDCapStruct  XGI_LCDCapList[] = {
 	0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
 };
 
-struct XGI21_LVDSCapStruct XGI21_LCDCapList[] = {
-	{DisableLCD24bpp + LCDPolarity,
-	 2160, 1250, 1600, 1200,   64,    1,  192,    3,
-	 0x70, 0x24, 0x20, 0x04, 0x0A, 0x02, 0xC8
-	},
-	{DisableLCD24bpp + LCDPolarity,
-	 1688, 1066, 1280, 1024,   48,    1,   112,    3,
-	 0x70, 0x44, 0x20, 0x04, 0x0A, 0x02, 0xC8
-	},
-	{DisableLCD24bpp + LCDPolarity + (LCDPolarity << 8),
-	 1344,  806, 1024,  768,   24,    3,   136,    6,
-	 0x6C, 0x65, 0x20, 0x04, 0x0A, 0x02, 0xC8
-	},
-	{DisableLCD24bpp + LCDPolarity,
-	 1056,  628,  800,  600,   40,    1,  128,    4,
-	 0x42, 0xE2, 0x20, 0x14, 0x0A, 0x02, 0x00
-	},
-	{DisableLCD24bpp + LCDPolarity,
-	  928,  525,  800,  480,   40,   13,   48,    3,
-	 0x52, 0xC5, 0x20, 0x14, 0x0A, 0x02, 0x00
-	},
-	{DisableLCD24bpp + LCDPolarity + (LCDPolarity << 8),
-	  800,  525,  640,  480,   16,   10,   96,    2,
-	 0x1B, 0xE1, 0x20, 0x04, 0x0A, 0x02, 0xC8
-	}
-};
-
 static struct XGI_Ext2Struct XGI330_RefIndex[] = {
 	{Support32Bpp + SupportAllCRT2 + SyncPN, RES320x200, VCLK25_175,
 	0x00, 0x10, 0x59, 320, 200},/* 00 */
diff --git a/drivers/staging/xgifb/vgatypes.h b/drivers/staging/xgifb/vgatypes.h
index 9b939b75c309..9e166bbb00c4 100644
--- a/drivers/staging/xgifb/vgatypes.h
+++ b/drivers/staging/xgifb/vgatypes.h
@@ -51,8 +51,6 @@ struct xgi_hw_device_info {
 	unsigned long ulExternalChip; /* NO VB or other video bridge*/
 				      /* if ujVBChipID = VB_CHIP_UNKNOWN, */
 
-	unsigned char *pjVirtualRomBase; /* ROM image */
-
 	void __iomem *pjVideoMemoryAddress;/* base virtual memory address */
 					    /* of Linear VGA memory */
 
diff --git a/drivers/staging/zcache/zcache-main.c b/drivers/staging/zcache/zcache-main.c
index 56c1f9c80dc1..642840c612ac 100644
--- a/drivers/staging/zcache/zcache-main.c
+++ b/drivers/staging/zcache/zcache-main.c
@@ -787,7 +787,7 @@ static ssize_t zv_max_zsize_store(struct kobject *kobj,
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
-	err = strict_strtoul(buf, 10, &val);
+	err = kstrtoul(buf, 10, &val);
 	if (err || (val == 0) || (val > (PAGE_SIZE / 8) * 7))
 		return -EINVAL;
 	zv_max_zsize = val;
@@ -819,7 +819,7 @@ static ssize_t zv_max_mean_zsize_store(struct kobject *kobj,
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
-	err = strict_strtoul(buf, 10, &val);
+	err = kstrtoul(buf, 10, &val);
 	if (err || (val == 0) || (val > (PAGE_SIZE / 8) * 7))
 		return -EINVAL;
 	zv_max_mean_zsize = val;
@@ -853,7 +853,7 @@ static ssize_t zv_page_count_policy_percent_store(struct kobject *kobj,
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
-	err = strict_strtoul(buf, 10, &val);
+	err = kstrtoul(buf, 10, &val);
 	if (err || (val == 0) || (val > 150))
 		return -EINVAL;
 	zv_page_count_policy_percent = val;
diff --git a/drivers/staging/zram/zram_drv.c b/drivers/staging/zram/zram_drv.c
index 09de99fbb7e0..2a2a92d389e6 100644
--- a/drivers/staging/zram/zram_drv.c
+++ b/drivers/staging/zram/zram_drv.c
@@ -653,7 +653,8 @@ int zram_init_device(struct zram *zram)
 		goto fail_no_table;
 	}
 
-	zram->compress_buffer = (void *)__get_free_pages(__GFP_ZERO, 1);
+	zram->compress_buffer =
+		(void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, 1);
 	if (!zram->compress_buffer) {
 		pr_err("Error allocating compressor buffer space\n");
 		ret = -ENOMEM;
diff --git a/drivers/staging/zram/zram_sysfs.c b/drivers/staging/zram/zram_sysfs.c
index 0ea8ed296a9d..d521122826f6 100644
--- a/drivers/staging/zram/zram_sysfs.c
+++ b/drivers/staging/zram/zram_sysfs.c
@@ -58,7 +58,7 @@ static ssize_t disksize_store(struct device *dev,
 	u64 disksize;
 	struct zram *zram = dev_to_zram(dev);
 
-	ret = strict_strtoull(buf, 10, &disksize);
+	ret = kstrtoull(buf, 10, &disksize);
 	if (ret)
 		return ret;
 
@@ -88,7 +88,7 @@ static ssize_t reset_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t len)
 {
 	int ret;
-	unsigned long do_reset;
+	unsigned short do_reset;
 	struct zram *zram;
 	struct block_device *bdev;
 
@@ -99,7 +99,7 @@ static ssize_t reset_store(struct device *dev,
 	if (bdev->bd_holders)
 		return -EBUSY;
 
-	ret = strict_strtoul(buf, 10, &do_reset);
+	ret = kstrtou16(buf, 10, &do_reset);
 	if (ret)
 		return ret;
 
diff --git a/drivers/tty/n_hdlc.c b/drivers/tty/n_hdlc.c
index cea56033b34c..a09ce3ef5d74 100644
--- a/drivers/tty/n_hdlc.c
+++ b/drivers/tty/n_hdlc.c
@@ -417,7 +417,7 @@ static void n_hdlc_send_frames(struct n_hdlc *n_hdlc, struct tty_struct *tty)
 				__FILE__,__LINE__,tbuf,tbuf->count);
 			
 		/* Send the next block of data to device */
-		tty->flags |= (1 << TTY_DO_WRITE_WAKEUP);
+		set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
 		actual = tty->ops->write(tty, tbuf->buf, tbuf->count);
 
 		/* rollback was possible and has been done */
@@ -459,7 +459,7 @@ static void n_hdlc_send_frames(struct n_hdlc *n_hdlc, struct tty_struct *tty)
 	}
 	
 	if (!tbuf)
-		tty->flags  &= ~(1 << TTY_DO_WRITE_WAKEUP);
+		clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
 	
 	/* Clear the re-entry flag */
 	spin_lock_irqsave(&n_hdlc->tx_buf_list.spinlock, flags);
@@ -491,7 +491,7 @@ static void n_hdlc_tty_wakeup(struct tty_struct *tty)
 		return;
 
 	if (tty != n_hdlc->tty) {
-		tty->flags &= ~(1 << TTY_DO_WRITE_WAKEUP);
+		clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
 		return;
 	}
 
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index 39d6ab6551e0..d2256d08ee7e 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -61,7 +61,7 @@
  * controlling the space in the read buffer.
  */
 #define TTY_THRESHOLD_THROTTLE		128 /* now based on remaining room */
-#define TTY_THRESHOLD_UNTHROTTLE 	128
+#define TTY_THRESHOLD_UNTHROTTLE	128
 
 /*
  * Special byte codes used in the echo buffer to represent operations
@@ -405,7 +405,7 @@ static ssize_t process_output_block(struct tty_struct *tty,
 				    const unsigned char *buf, unsigned int nr)
 {
 	int	space;
-	int 	i;
+	int	i;
 	const unsigned char *cp;
 
 	mutex_lock(&tty->output_lock);
@@ -1607,7 +1607,7 @@ static inline int input_available_p(struct tty_struct *tty, int amt)
 }
 
 /**
- * 	copy_from_read_buf	-	copy read data directly
+ *	copy_from_read_buf	-	copy read data directly
  *	@tty: terminal device
  *	@b: user data
  *	@nr: size of data
@@ -1909,7 +1909,7 @@ do_it_again:
 		if (nr)
 			clear_bit(TTY_PUSH, &tty->flags);
 	} else if (test_and_clear_bit(TTY_PUSH, &tty->flags))
-		 goto do_it_again;
+		goto do_it_again;
 
 	n_tty_set_room(tty);
 	return retval;
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index e18604b3fc7d..d8653ab6f498 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -446,19 +446,8 @@ static inline void legacy_pty_init(void) { }
 int pty_limit = NR_UNIX98_PTY_DEFAULT;
 static int pty_limit_min;
 static int pty_limit_max = NR_UNIX98_PTY_MAX;
-static int tty_count;
 static int pty_count;
 
-static inline void pty_inc_count(void)
-{
-	pty_count = (++tty_count) / 2;
-}
-
-static inline void pty_dec_count(void)
-{
-	pty_count = (--tty_count) / 2;
-}
-
 static struct cdev ptmx_cdev;
 
 static struct ctl_table pty_table[] = {
@@ -600,8 +589,7 @@ static int pty_unix98_install(struct tty_driver *driver, struct tty_struct *tty)
 	 */
 	tty_driver_kref_get(driver);
 	tty->count++;
-	pty_inc_count(); /* tty */
-	pty_inc_count(); /* tty->link */
+	pty_count++;
 	return 0;
 err_free_mem:
 	deinitialize_tty_struct(o_tty);
@@ -613,15 +601,19 @@ err_free_tty:
 	return -ENOMEM;
 }
 
-static void pty_unix98_remove(struct tty_driver *driver, struct tty_struct *tty)
+static void ptm_unix98_remove(struct tty_driver *driver, struct tty_struct *tty)
+{
+	pty_count--;
+}
+
+static void pts_unix98_remove(struct tty_driver *driver, struct tty_struct *tty)
 {
-	pty_dec_count();
 }
 
 static const struct tty_operations ptm_unix98_ops = {
 	.lookup = ptm_unix98_lookup,
 	.install = pty_unix98_install,
-	.remove = pty_unix98_remove,
+	.remove = ptm_unix98_remove,
 	.open = pty_open,
 	.close = pty_close,
 	.write = pty_write,
@@ -638,7 +630,7 @@ static const struct tty_operations ptm_unix98_ops = {
 static const struct tty_operations pty_unix98_ops = {
 	.lookup = pts_unix98_lookup,
 	.install = pty_unix98_install,
-	.remove = pty_unix98_remove,
+	.remove = pts_unix98_remove,
 	.open = pty_open,
 	.close = pty_close,
 	.write = pty_write,
diff --git a/drivers/tty/serial/8250.c b/drivers/tty/serial/8250.c
index eeadf1b8e093..9f50c4e3c2be 100644
--- a/drivers/tty/serial/8250.c
+++ b/drivers/tty/serial/8250.c
@@ -129,32 +129,6 @@ static unsigned long probe_rsa[PORT_RSA_MAX];
 static unsigned int probe_rsa_count;
 #endif /* CONFIG_SERIAL_8250_RSA  */
 
-struct uart_8250_port {
-	struct uart_port	port;
-	struct timer_list	timer;		/* "no irq" timer */
-	struct list_head	list;		/* ports on this IRQ */
-	unsigned short		capabilities;	/* port capabilities */
-	unsigned short		bugs;		/* port bugs */
-	unsigned int		tx_loadsz;	/* transmit fifo load size */
-	unsigned char		acr;
-	unsigned char		ier;
-	unsigned char		lcr;
-	unsigned char		mcr;
-	unsigned char		mcr_mask;	/* mask of user bits */
-	unsigned char		mcr_force;	/* mask of forced bits */
-	unsigned char		cur_iotype;	/* Running I/O type */
-
-	/*
-	 * Some bits in registers are cleared on a read, so they must
-	 * be saved whenever the register is read but the bits will not
-	 * be immediately processed.
-	 */
-#define LSR_SAVE_FLAGS UART_LSR_BRK_ERROR_BITS
-	unsigned char		lsr_saved_flags;
-#define MSR_SAVE_FLAGS UART_MSR_ANY_DELTA
-	unsigned char		msr_saved_flags;
-};
-
 struct irq_info {
 	struct			hlist_node node;
 	int			irq;
@@ -1326,8 +1300,6 @@ static void serial8250_stop_tx(struct uart_port *port)
 	}
 }
 
-static void transmit_chars(struct uart_8250_port *up);
-
 static void serial8250_start_tx(struct uart_port *port)
 {
 	struct uart_8250_port *up =
@@ -1344,7 +1316,7 @@ static void serial8250_start_tx(struct uart_port *port)
 			if ((up->port.type == PORT_RM9000) ?
 				(lsr & UART_LSR_THRE) :
 				(lsr & UART_LSR_TEMT))
-				transmit_chars(up);
+				serial8250_tx_chars(up);
 		}
 	}
 
@@ -1401,11 +1373,16 @@ static void clear_rx_fifo(struct uart_8250_port *up)
 	} while (1);
 }
 
-static void
-receive_chars(struct uart_8250_port *up, unsigned int *status)
+/*
+ * serial8250_rx_chars: processes according to the passed in LSR
+ * value, and returns the remaining LSR bits not handled
+ * by this Rx routine.
+ */
+unsigned char
+serial8250_rx_chars(struct uart_8250_port *up, unsigned char lsr)
 {
 	struct tty_struct *tty = up->port.state->port.tty;
-	unsigned char ch, lsr = *status;
+	unsigned char ch;
 	int max_count = 256;
 	char flag;
 
@@ -1481,10 +1458,11 @@ ignore_char:
 	spin_unlock(&up->port.lock);
 	tty_flip_buffer_push(tty);
 	spin_lock(&up->port.lock);
-	*status = lsr;
+	return lsr;
 }
+EXPORT_SYMBOL_GPL(serial8250_rx_chars);
 
-static void transmit_chars(struct uart_8250_port *up)
+void serial8250_tx_chars(struct uart_8250_port *up)
 {
 	struct circ_buf *xmit = &up->port.state->xmit;
 	int count;
@@ -1521,8 +1499,9 @@ static void transmit_chars(struct uart_8250_port *up)
 	if (uart_circ_empty(xmit))
 		__stop_tx(up);
 }
+EXPORT_SYMBOL_GPL(serial8250_tx_chars);
 
-static unsigned int check_modem_status(struct uart_8250_port *up)
+unsigned int serial8250_modem_status(struct uart_8250_port *up)
 {
 	unsigned int status = serial_in(up, UART_MSR);
 
@@ -1544,14 +1523,20 @@ static unsigned int check_modem_status(struct uart_8250_port *up)
 
 	return status;
 }
+EXPORT_SYMBOL_GPL(serial8250_modem_status);
 
 /*
  * This handles the interrupt from one port.
  */
-static void serial8250_handle_port(struct uart_8250_port *up)
+int serial8250_handle_irq(struct uart_port *port, unsigned int iir)
 {
-	unsigned int status;
+	unsigned char status;
 	unsigned long flags;
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+
+	if (iir & UART_IIR_NO_INT)
+		return 0;
 
 	spin_lock_irqsave(&up->port.lock, flags);
 
@@ -1560,25 +1545,13 @@ static void serial8250_handle_port(struct uart_8250_port *up)
 	DEBUG_INTR("status = %x...", status);
 
 	if (status & (UART_LSR_DR | UART_LSR_BI))
-		receive_chars(up, &status);
-	check_modem_status(up);
+		status = serial8250_rx_chars(up, status);
+	serial8250_modem_status(up);
 	if (status & UART_LSR_THRE)
-		transmit_chars(up);
+		serial8250_tx_chars(up);
 
 	spin_unlock_irqrestore(&up->port.lock, flags);
-}
-
-int serial8250_handle_irq(struct uart_port *port, unsigned int iir)
-{
-	struct uart_8250_port *up =
-		container_of(port, struct uart_8250_port, port);
-
-	if (!(iir & UART_IIR_NO_INT)) {
-		serial8250_handle_port(up);
-		return 1;
-	}
-
-	return 0;
+	return 1;
 }
 EXPORT_SYMBOL_GPL(serial8250_handle_irq);
 
@@ -1619,11 +1592,13 @@ static irqreturn_t serial8250_interrupt(int irq, void *dev_id)
 	do {
 		struct uart_8250_port *up;
 		struct uart_port *port;
+		bool skip;
 
 		up = list_entry(l, struct uart_8250_port, list);
 		port = &up->port;
+		skip = pass_counter && up->port.flags & UPF_IIR_ONCE;
 
-		if (port->handle_irq(port)) {
+		if (!skip && port->handle_irq(port)) {
 			handled = 1;
 			end = NULL;
 		} else if (end == NULL)
@@ -1758,11 +1733,8 @@ static void serial_unlink_irq_chain(struct uart_8250_port *up)
 static void serial8250_timeout(unsigned long data)
 {
 	struct uart_8250_port *up = (struct uart_8250_port *)data;
-	unsigned int iir;
 
-	iir = serial_in(up, UART_IIR);
-	if (!(iir & UART_IIR_NO_INT))
-		serial8250_handle_port(up);
+	up->port.handle_irq(&up->port);
 	mod_timer(&up->timer, jiffies + uart_poll_timeout(&up->port));
 }
 
@@ -1801,7 +1773,7 @@ static void serial8250_backup_timeout(unsigned long data)
 	}
 
 	if (!(iir & UART_IIR_NO_INT))
-		transmit_chars(up);
+		serial8250_tx_chars(up);
 
 	if (is_real_interrupt(up->port.irq))
 		serial_out(up, UART_IER, ier);
@@ -1835,7 +1807,7 @@ static unsigned int serial8250_get_mctrl(struct uart_port *port)
 	unsigned int status;
 	unsigned int ret;
 
-	status = check_modem_status(up);
+	status = serial8250_modem_status(up);
 
 	ret = 0;
 	if (status & UART_MSR_DCD)
@@ -2000,7 +1972,7 @@ static int serial8250_startup(struct uart_port *port)
 		serial_outp(up, UART_IER, 0);
 		serial_outp(up, UART_LCR, 0);
 		serial_icr_write(up, UART_CSR, 0); /* Reset the UART */
-		serial_outp(up, UART_LCR, 0xBF);
+		serial_outp(up, UART_LCR, UART_LCR_CONF_MODE_B);
 		serial_outp(up, UART_EFR, UART_EFR_ECB);
 		serial_outp(up, UART_LCR, 0);
 	}
@@ -2848,7 +2820,7 @@ serial8250_console_write(struct console *co, const char *s, unsigned int count)
 
 	local_irq_save(flags);
 	if (up->port.sysrq) {
-		/* serial8250_handle_port() already took the lock */
+		/* serial8250_handle_irq() already took the lock */
 		locked = 0;
 	} else if (oops_in_progress) {
 		locked = spin_trylock(&up->port.lock);
@@ -2882,7 +2854,7 @@ serial8250_console_write(struct console *co, const char *s, unsigned int count)
 	 *	while processing with interrupts off.
 	 */
 	if (up->msr_saved_flags)
-		check_modem_status(up);
+		serial8250_modem_status(up);
 
 	if (locked)
 		spin_unlock(&up->port.lock);
diff --git a/drivers/tty/serial/8250.h b/drivers/tty/serial/8250.h
index 6edf4a6a22d4..ae027be57e25 100644
--- a/drivers/tty/serial/8250.h
+++ b/drivers/tty/serial/8250.h
@@ -13,6 +13,32 @@
 
 #include <linux/serial_8250.h>
 
+struct uart_8250_port {
+	struct uart_port	port;
+	struct timer_list	timer;		/* "no irq" timer */
+	struct list_head	list;		/* ports on this IRQ */
+	unsigned short		capabilities;	/* port capabilities */
+	unsigned short		bugs;		/* port bugs */
+	unsigned int		tx_loadsz;	/* transmit fifo load size */
+	unsigned char		acr;
+	unsigned char		ier;
+	unsigned char		lcr;
+	unsigned char		mcr;
+	unsigned char		mcr_mask;	/* mask of user bits */
+	unsigned char		mcr_force;	/* mask of forced bits */
+	unsigned char		cur_iotype;	/* Running I/O type */
+
+	/*
+	 * Some bits in registers are cleared on a read, so they must
+	 * be saved whenever the register is read but the bits will not
+	 * be immediately processed.
+	 */
+#define LSR_SAVE_FLAGS UART_LSR_BRK_ERROR_BITS
+	unsigned char		lsr_saved_flags;
+#define MSR_SAVE_FLAGS UART_MSR_ANY_DELTA
+	unsigned char		msr_saved_flags;
+};
+
 struct old_serial_port {
 	unsigned int uart;
 	unsigned int baud_base;
diff --git a/drivers/tty/serial/8250_dw.c b/drivers/tty/serial/8250_dw.c
index bf1fba640c2d..f574eef3075f 100644
--- a/drivers/tty/serial/8250_dw.c
+++ b/drivers/tty/serial/8250_dw.c
@@ -177,17 +177,7 @@ static struct platform_driver dw8250_platform_driver = {
 	.remove			= __devexit_p(dw8250_remove),
 };
 
-static int __init dw8250_init(void)
-{
-	return platform_driver_register(&dw8250_platform_driver);
-}
-module_init(dw8250_init);
-
-static void __exit dw8250_exit(void)
-{
-	platform_driver_unregister(&dw8250_platform_driver);
-}
-module_exit(dw8250_exit);
+module_platform_driver(dw8250_platform_driver);
 
 MODULE_AUTHOR("Jamie Iles");
 MODULE_LICENSE("GPL");
diff --git a/drivers/tty/serial/8250_fsl.c b/drivers/tty/serial/8250_fsl.c
new file mode 100644
index 000000000000..f4d3c47b88e8
--- /dev/null
+++ b/drivers/tty/serial/8250_fsl.c
@@ -0,0 +1,63 @@
+#include <linux/serial_reg.h>
+#include <linux/serial_8250.h>
+
+#include "8250.h"
+
+/*
+ * Freescale 16550 UART "driver", Copyright (C) 2011 Paul Gortmaker.
+ *
+ * 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 isn't a full driver; it just provides an alternate IRQ
+ * handler to deal with an errata.  Everything else is just
+ * using the bog standard 8250 support.
+ *
+ * We follow code flow of serial8250_default_handle_irq() but add
+ * a check for a break and insert a dummy read on the Rx for the
+ * immediately following IRQ event.
+ *
+ * We re-use the already existing "bug handling" lsr_saved_flags
+ * field to carry the "what we just did" information from the one
+ * IRQ event to the next one.
+ */
+
+int fsl8250_handle_irq(struct uart_port *port)
+{
+	unsigned char lsr, orig_lsr;
+	unsigned long flags;
+	unsigned int iir;
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+
+	spin_lock_irqsave(&up->port.lock, flags);
+
+	iir = port->serial_in(port, UART_IIR);
+	if (iir & UART_IIR_NO_INT) {
+		spin_unlock_irqrestore(&up->port.lock, flags);
+		return 0;
+	}
+
+	/* This is the WAR; if last event was BRK, then read and return */
+	if (unlikely(up->lsr_saved_flags & UART_LSR_BI)) {
+		up->lsr_saved_flags &= ~UART_LSR_BI;
+		port->serial_in(port, UART_RX);
+		spin_unlock_irqrestore(&up->port.lock, flags);
+		return 1;
+	}
+
+	lsr = orig_lsr = up->port.serial_in(&up->port, UART_LSR);
+
+	if (lsr & (UART_LSR_DR | UART_LSR_BI))
+		lsr = serial8250_rx_chars(up, lsr);
+
+	serial8250_modem_status(up);
+
+	if (lsr & UART_LSR_THRE)
+		serial8250_tx_chars(up);
+
+	up->lsr_saved_flags = orig_lsr;
+	spin_unlock_irqrestore(&up->port.lock, flags);
+	return 1;
+}
diff --git a/drivers/tty/serial/8250_pci.c b/drivers/tty/serial/8250_pci.c
index 825937a5f210..da2b0b0a183f 100644
--- a/drivers/tty/serial/8250_pci.c
+++ b/drivers/tty/serial/8250_pci.c
@@ -1092,6 +1092,14 @@ static int skip_tx_en_setup(struct serial_private *priv,
 	return pci_default_setup(priv, board, port, idx);
 }
 
+static int kt_serial_setup(struct serial_private *priv,
+			   const struct pciserial_board *board,
+			   struct uart_port *port, int idx)
+{
+	port->flags |= UPF_IIR_ONCE;
+	return skip_tx_en_setup(priv, board, port, idx);
+}
+
 static int pci_eg20t_init(struct pci_dev *dev)
 {
 #if defined(CONFIG_SERIAL_PCH_UART) || defined(CONFIG_SERIAL_PCH_UART_MODULE)
@@ -1110,7 +1118,18 @@ pci_xr17c154_setup(struct serial_private *priv,
 	return pci_default_setup(priv, board, port, idx);
 }
 
-/* This should be in linux/pci_ids.h */
+static int try_enable_msi(struct pci_dev *dev)
+{
+	/* use msi if available, but fallback to legacy otherwise */
+	pci_enable_msi(dev);
+	return 0;
+}
+
+static void disable_msi(struct pci_dev *dev)
+{
+	pci_disable_msi(dev);
+}
+
 #define PCI_VENDOR_ID_SBSMODULARIO	0x124B
 #define PCI_SUBVENDOR_ID_SBSMODULARIO	0x124B
 #define PCI_DEVICE_ID_OCTPRO		0x0001
@@ -1133,9 +1152,14 @@ pci_xr17c154_setup(struct serial_private *priv,
 #define PCI_DEVICE_ID_TITAN_800E	0xA014
 #define PCI_DEVICE_ID_TITAN_200EI	0xA016
 #define PCI_DEVICE_ID_TITAN_200EISI	0xA017
+#define PCI_DEVICE_ID_TITAN_400V3	0xA310
+#define PCI_DEVICE_ID_TITAN_410V3	0xA312
+#define PCI_DEVICE_ID_TITAN_800V3	0xA314
+#define PCI_DEVICE_ID_TITAN_800V3B	0xA315
 #define PCI_DEVICE_ID_OXSEMI_16PCI958	0x9538
 #define PCIE_DEVICE_ID_NEO_2_OX_IBM	0x00F6
 #define PCI_DEVICE_ID_PLX_CRONYX_OMEGA	0xc001
+#define PCI_DEVICE_ID_INTEL_PATSBURG_KT 0x1d3d
 
 /* Unknown vendors/cards - this should not be in linux/pci_ids.h */
 #define PCI_SUBDEVICE_ID_UNKNOWN_0x1584	0x1584
@@ -1220,6 +1244,15 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subdevice	= PCI_ANY_ID,
 		.setup		= ce4100_serial_setup,
 	},
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_PATSBURG_KT,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.init		= try_enable_msi,
+		.setup		= kt_serial_setup,
+		.exit		= disable_msi,
+	},
 	/*
 	 * ITE
 	 */
@@ -3414,6 +3447,18 @@ static struct pci_device_id serial_pci_tbl[] = {
 	{	PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_200EISI,
 		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
 		pbn_oxsemi_2_4000000 },
+	{	PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_400V3,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		pbn_b0_4_921600 },
+	{	PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_410V3,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		pbn_b0_4_921600 },
+	{	PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_800V3,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		pbn_b0_4_921600 },
+	{	PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_800V3B,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		pbn_b0_4_921600 },
 
 	{	PCI_VENDOR_ID_SIIG, PCI_DEVICE_ID_SIIG_1S_10x_550,
 		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 95b21a619900..f32a2ea70100 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -97,6 +97,11 @@ config SERIAL_8250_PNP
 	  This builds standard PNP serial support. You may be able to
 	  disable this feature if you only need legacy serial support.
 
+config SERIAL_8250_FSL
+	bool
+	depends on SERIAL_8250_CONSOLE && PPC_UDBG_16550
+	default PPC
+
 config SERIAL_8250_HP300
 	tristate
 	depends on SERIAL_8250 && HP300
@@ -457,7 +462,7 @@ config SERIAL_SAMSUNG
 config SERIAL_SAMSUNG_UARTS_4
 	bool
 	depends on ARM && PLAT_SAMSUNG
-	default y if CPU_S3C2443
+	default y if !(CPU_S3C2410 || SERIAL_S3C2412 || CPU_S3C2440 || CPU_S3C2442)
 	help
 	  Internal node for the common case of 4 Samsung compatible UARTs
 
@@ -465,7 +470,7 @@ config SERIAL_SAMSUNG_UARTS
 	int
 	depends on ARM && PLAT_SAMSUNG
 	default 6 if ARCH_S5P6450
-	default 4 if SERIAL_SAMSUNG_UARTS_4
+	default 4 if SERIAL_SAMSUNG_UARTS_4 || CPU_S3C2416
 	default 3
 	help
 	  Select the number of available UART ports for the Samsung S3C
@@ -495,46 +500,27 @@ config SERIAL_SAMSUNG_CONSOLE
 	  your boot loader about how to pass options to the kernel at
 	  boot time.)
 
-config SERIAL_S3C2410
-	tristate "Samsung S3C2410 Serial port support"
-	depends on SERIAL_SAMSUNG && CPU_S3C2410
-	default y if CPU_S3C2410
-	help
-	  Serial port support for the Samsung S3C2410 SoC
-
-config SERIAL_S3C2412
-	tristate "Samsung S3C2412/S3C2413 Serial port support"
-	depends on SERIAL_SAMSUNG && CPU_S3C2412
-	default y if CPU_S3C2412
-	help
-	  Serial port support for the Samsung S3C2412 and S3C2413 SoC
-
-config SERIAL_S3C2440
-	tristate "Samsung S3C2440/S3C2442/S3C2416 Serial port support"
-	depends on SERIAL_SAMSUNG && (CPU_S3C2440 || CPU_S3C2442 || CPU_S3C2416)
-	default y if CPU_S3C2440
-	default y if CPU_S3C2442
-	select SERIAL_SAMSUNG_UARTS_4 if CPU_S3C2416
-	help
-	  Serial port support for the Samsung S3C2440, S3C2416 and S3C2442 SoC
-
-config SERIAL_S3C6400
-	tristate "Samsung S3C6400/S3C6410/S5P6440/S5P6450/S5PC100 Serial port support"
-	depends on SERIAL_SAMSUNG && (CPU_S3C6400 || CPU_S3C6410 || CPU_S5P6440 || CPU_S5P6450 || CPU_S5PC100)
-	select SERIAL_SAMSUNG_UARTS_4
-	default y
-	help
-	  Serial port support for the Samsung S3C6400, S3C6410, S5P6440, S5P6450
-	  and S5PC100 SoCs
-
-config SERIAL_S5PV210
-	tristate "Samsung S5PV210 Serial port support"
-	depends on SERIAL_SAMSUNG && (CPU_S5PV210 || CPU_EXYNOS4210 || SOC_EXYNOS4212)
-	select SERIAL_SAMSUNG_UARTS_4 if (CPU_S5PV210 || CPU_EXYNOS4210 || SOC_EXYNOS4212)
-	default y
-	help
-	  Serial port support for Samsung's S5P Family of SoC's
-
+config SERIAL_SIRFSOC
+        tristate "SiRF SoC Platform Serial port support"
+        depends on ARM && ARCH_PRIMA2
+        select SERIAL_CORE
+        help
+          Support for the on-chip UART on the CSR SiRFprimaII series,
+          providing /dev/ttySiRF0, 1 and 2 (note, some machines may not
+          provide all of these ports, depending on how the serial port
+          pins are configured).
+
+config SERIAL_SIRFSOC_CONSOLE
+        bool "Support for console on SiRF SoC serial port"
+        depends on SERIAL_SIRFSOC=y
+        select SERIAL_CORE_CONSOLE
+        help
+          Even if you say Y here, the currently visible virtual console
+          (/dev/tty0) will still be used as the system console by default, but
+          you can alter that using a kernel command line option such as
+          "console=ttySiRFx". (Try "man bootparam" or see the documentation of
+          your boot loader about how to pass options to the kernel at
+          boot time.)
 
 config SERIAL_MAX3100
 	tristate "MAX3100 support"
@@ -1324,7 +1310,7 @@ config SERIAL_OF_PLATFORM
 
 config SERIAL_OMAP
 	tristate "OMAP serial port support"
-	depends on ARCH_OMAP2 || ARCH_OMAP3 || ARCH_OMAP4
+	depends on ARCH_OMAP2PLUS
 	select SERIAL_CORE
 	help
 	  If you have a machine based on an Texas Instruments OMAP CPU you
@@ -1575,6 +1561,15 @@ config SERIAL_PCH_UART
 	  ML7213/ML7223/ML7831 is companion chip for Intel Atom E6xx series.
 	  ML7213/ML7223/ML7831 is completely compatible for Intel EG20T PCH.
 
+config SERIAL_PCH_UART_CONSOLE
+	bool "Support for console on Intel EG20T PCH UART/OKI SEMICONDUCTOR ML7213 IOH"
+	depends on SERIAL_PCH_UART=y
+	select SERIAL_CORE_CONSOLE
+	help
+	  Say Y here if you wish to use the PCH UART as the system console
+	  (the system  console is the device which receives all kernel messages and
+	  warnings and which allows logins in single user mode).
+
 config SERIAL_MSM_SMD
 	bool "Enable tty device interface for some SMD ports"
 	default n
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index e10cf5b54b6d..07e0494c6830 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -28,6 +28,7 @@ obj-$(CONFIG_SERIAL_8250_BOCA) += 8250_boca.o
 obj-$(CONFIG_SERIAL_8250_EXAR_ST16C554) += 8250_exar_st16c554.o
 obj-$(CONFIG_SERIAL_8250_HUB6) += 8250_hub6.o
 obj-$(CONFIG_SERIAL_8250_MCA) += 8250_mca.o
+obj-$(CONFIG_SERIAL_8250_FSL) += 8250_fsl.o
 obj-$(CONFIG_SERIAL_8250_DW) += 8250_dw.o
 obj-$(CONFIG_SERIAL_AMBA_PL010) += amba-pl010.o
 obj-$(CONFIG_SERIAL_AMBA_PL011) += amba-pl011.o
@@ -39,11 +40,6 @@ obj-$(CONFIG_SERIAL_BCM63XX) += bcm63xx_uart.o
 obj-$(CONFIG_SERIAL_BFIN) += bfin_uart.o
 obj-$(CONFIG_SERIAL_BFIN_SPORT) += bfin_sport_uart.o
 obj-$(CONFIG_SERIAL_SAMSUNG) += samsung.o
-obj-$(CONFIG_SERIAL_S3C2410) += s3c2410.o
-obj-$(CONFIG_SERIAL_S3C2412) += s3c2412.o
-obj-$(CONFIG_SERIAL_S3C2440) += s3c2440.o
-obj-$(CONFIG_SERIAL_S3C6400) += s3c6400.o
-obj-$(CONFIG_SERIAL_S5PV210) += s5pv210.o
 obj-$(CONFIG_SERIAL_MAX3100) += max3100.o
 obj-$(CONFIG_SERIAL_MAX3107) += max3107.o
 obj-$(CONFIG_SERIAL_MAX3107_AAVA) += max3107-aava.o
@@ -94,3 +90,4 @@ obj-$(CONFIG_SERIAL_MSM_SMD)	+= msm_smd_tty.o
 obj-$(CONFIG_SERIAL_MXS_AUART) += mxs-auart.o
 obj-$(CONFIG_SERIAL_LANTIQ)	+= lantiq.o
 obj-$(CONFIG_SERIAL_XILINX_PS_UART) += xilinx_uartps.o
+obj-$(CONFIG_SERIAL_SIRFSOC) += sirfsoc_uart.o
diff --git a/drivers/tty/serial/apbuart.c b/drivers/tty/serial/apbuart.c
index 77554fd68d1f..7162f70d9260 100644
--- a/drivers/tty/serial/apbuart.c
+++ b/drivers/tty/serial/apbuart.c
@@ -577,7 +577,7 @@ static int __devinit apbuart_probe(struct platform_device *op)
 	return 0;
 }
 
-static struct of_device_id __initdata apbuart_match[] = {
+static struct of_device_id apbuart_match[] = {
 	{
 	 .name = "GAISLER_APBUART",
 	 },
@@ -597,7 +597,7 @@ static struct platform_driver grlib_apbuart_of_driver = {
 };
 
 
-static int grlib_apbuart_configure(void)
+static int __init grlib_apbuart_configure(void)
 {
 	struct device_node *np;
 	int line = 0;
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index 4c823f341d98..10605ecc99ab 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -212,8 +212,9 @@ void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 {
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 	unsigned int mode;
+	unsigned long flags;
 
-	spin_lock(&port->lock);
+	spin_lock_irqsave(&port->lock, flags);
 
 	/* Disable interrupts */
 	UART_PUT_IDR(port, atmel_port->tx_done_mask);
@@ -244,7 +245,7 @@ void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 	/* Enable interrupts */
 	UART_PUT_IER(port, atmel_port->tx_done_mask);
 
-	spin_unlock(&port->lock);
+	spin_unlock_irqrestore(&port->lock, flags);
 
 }
 
@@ -1256,12 +1257,7 @@ static void atmel_set_termios(struct uart_port *port, struct ktermios *termios,
 
 static void atmel_set_ldisc(struct uart_port *port, int new)
 {
-	int line = port->line;
-
-	if (line >= port->state->port.tty->driver->num)
-		return;
-
-	if (port->state->port.tty->ldisc->ops->num == N_PPS) {
+	if (new == N_PPS) {
 		port->flags |= UPF_HARDPPS_CD;
 		atmel_enable_ms(port);
 	} else {
diff --git a/drivers/tty/serial/bfin_sport_uart.c b/drivers/tty/serial/bfin_sport_uart.c
index ee101c0d358f..7fbc3a08f10d 100644
--- a/drivers/tty/serial/bfin_sport_uart.c
+++ b/drivers/tty/serial/bfin_sport_uart.c
@@ -299,8 +299,13 @@ static int sport_startup(struct uart_port *port)
 			dev_info(port->dev, "Unable to attach BlackFin UART over SPORT CTS interrupt. So, disable it.\n");
 		}
 	}
-	if (up->rts_pin >= 0)
-		gpio_direction_output(up->rts_pin, 0);
+	if (up->rts_pin >= 0) {
+		if (gpio_request(up->rts_pin, DRV_NAME)) {
+			dev_info(port->dev, "fail to request RTS PIN at GPIO_%d\n", up->rts_pin);
+			up->rts_pin = -1;
+		} else
+			gpio_direction_output(up->rts_pin, 0);
+	}
 #endif
 
 	return 0;
@@ -445,6 +450,8 @@ static void sport_shutdown(struct uart_port *port)
 #ifdef CONFIG_SERIAL_BFIN_SPORT_CTSRTS
 	if (up->cts_pin >= 0)
 		free_irq(gpio_to_irq(up->cts_pin), up);
+	if (up->rts_pin >= 0)
+		gpio_free(up->rts_pin);
 #endif
 }
 
@@ -803,17 +810,16 @@ static int __devinit sport_uart_probe(struct platform_device *pdev)
 		res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 		if (res == NULL)
 			sport->cts_pin = -1;
-		else
+		else {
 			sport->cts_pin = res->start;
+			sport->port.flags |= ASYNC_CTS_FLOW;
+		}
 
 		res = platform_get_resource(pdev, IORESOURCE_IO, 1);
 		if (res == NULL)
 			sport->rts_pin = -1;
 		else
 			sport->rts_pin = res->start;
-
-		if (sport->rts_pin >= 0)
-			gpio_request(sport->rts_pin, DRV_NAME);
 #endif
 	}
 
@@ -853,10 +859,6 @@ static int __devexit sport_uart_remove(struct platform_device *pdev)
 
 	if (sport) {
 		uart_remove_one_port(&sport_uart_reg, &sport->port);
-#ifdef CONFIG_SERIAL_BFIN_CTSRTS
-		if (sport->rts_pin >= 0)
-			gpio_free(sport->rts_pin);
-#endif
 		iounmap(sport->port.membase);
 		peripheral_free_list(
 			(unsigned short *)pdev->dev.platform_data);
diff --git a/drivers/tty/serial/bfin_sport_uart.h b/drivers/tty/serial/bfin_sport_uart.h
index 6d06ce1d5675..e4510ea135ce 100644
--- a/drivers/tty/serial/bfin_sport_uart.h
+++ b/drivers/tty/serial/bfin_sport_uart.h
@@ -45,11 +45,12 @@
 #define SPORT_GET_RX32(sport) \
 ({ \
 	unsigned int __ret; \
+	unsigned long flags; \
 	if (ANOMALY_05000473) \
-		local_irq_disable(); \
+		local_irq_save(flags); \
 	__ret = bfin_read32((sport)->port.membase + OFFSET_RX); \
 	if (ANOMALY_05000473) \
-		local_irq_enable(); \
+		local_irq_restore(flags); \
 	__ret; \
 })
 #define SPORT_GET_RCR1(sport)		bfin_read16(((sport)->port.membase + OFFSET_RCR1))
diff --git a/drivers/tty/serial/bfin_uart.c b/drivers/tty/serial/bfin_uart.c
index 66afb98b77b5..26953bfa6922 100644
--- a/drivers/tty/serial/bfin_uart.c
+++ b/drivers/tty/serial/bfin_uart.c
@@ -116,15 +116,22 @@ static void bfin_serial_set_mctrl(struct uart_port *port, unsigned int mctrl)
 static irqreturn_t bfin_serial_mctrl_cts_int(int irq, void *dev_id)
 {
 	struct bfin_serial_port *uart = dev_id;
-	unsigned int status;
-
-	status = bfin_serial_get_mctrl(&uart->port);
-	uart_handle_cts_change(&uart->port, status & TIOCM_CTS);
+	unsigned int status = bfin_serial_get_mctrl(&uart->port);
 #ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
-	uart->scts = 1;
+	struct tty_struct *tty = uart->port.state->port.tty;
+
 	UART_CLEAR_SCTS(uart);
-	UART_CLEAR_IER(uart, EDSSI);
+	if (tty->hw_stopped) {
+		if (status) {
+			tty->hw_stopped = 0;
+			uart_write_wakeup(&uart->port);
+		}
+	} else {
+		if (!status)
+			tty->hw_stopped = 1;
+	}
 #endif
+	uart_handle_cts_change(&uart->port, status & TIOCM_CTS);
 
 	return IRQ_HANDLED;
 }
@@ -175,13 +182,6 @@ static void bfin_serial_start_tx(struct uart_port *port)
 	struct bfin_serial_port *uart = (struct bfin_serial_port *)port;
 	struct tty_struct *tty = uart->port.state->port.tty;
 
-#ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
-	if (uart->scts && !(bfin_serial_get_mctrl(&uart->port) & TIOCM_CTS)) {
-		uart->scts = 0;
-		uart_handle_cts_change(&uart->port, uart->scts);
-	}
-#endif
-
 	/*
 	 * To avoid losting RX interrupt, we reset IR function
 	 * before sending data.
@@ -380,12 +380,6 @@ static irqreturn_t bfin_serial_tx_int(int irq, void *dev_id)
 {
 	struct bfin_serial_port *uart = dev_id;
 
-#ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
-	if (uart->scts && !(bfin_serial_get_mctrl(&uart->port) & TIOCM_CTS)) {
-		uart->scts = 0;
-		uart_handle_cts_change(&uart->port, uart->scts);
-	}
-#endif
 	spin_lock(&uart->port.lock);
 	if (UART_GET_LSR(uart) & THRE)
 		bfin_serial_tx_chars(uart);
@@ -531,13 +525,6 @@ static irqreturn_t bfin_serial_dma_tx_int(int irq, void *dev_id)
 	struct bfin_serial_port *uart = dev_id;
 	struct circ_buf *xmit = &uart->port.state->xmit;
 
-#ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
-	if (uart->scts && !(bfin_serial_get_mctrl(&uart->port)&TIOCM_CTS)) {
-		uart->scts = 0;
-		uart_handle_cts_change(&uart->port, uart->scts);
-	}
-#endif
-
 	spin_lock(&uart->port.lock);
 	if (!(get_dma_curr_irqstat(uart->tx_dma_channel)&DMA_RUN)) {
 		disable_dma(uart->tx_dma_channel);
@@ -739,20 +726,26 @@ static int bfin_serial_startup(struct uart_port *port)
 			pr_info("Unable to attach BlackFin UART CTS interrupt. So, disable it.\n");
 		}
 	}
-	if (uart->rts_pin >= 0)
-		gpio_direction_output(uart->rts_pin, 0);
+	if (uart->rts_pin >= 0) {
+		if (gpio_request(uart->rts_pin, DRIVER_NAME)) {
+			pr_info("fail to request RTS PIN at GPIO_%d\n", uart->rts_pin);
+			uart->rts_pin = -1;
+		} else
+			gpio_direction_output(uart->rts_pin, 0);
+	}
 #endif
 #ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
-	if (uart->cts_pin >= 0 && request_irq(uart->status_irq,
-		bfin_serial_mctrl_cts_int,
-		0, "BFIN_UART_MODEM_STATUS", uart)) {
-		uart->cts_pin = -1;
-		pr_info("Unable to attach BlackFin UART Modem Status interrupt.\n");
-	}
+	if (uart->cts_pin >= 0) {
+		if (request_irq(uart->status_irq, bfin_serial_mctrl_cts_int,
+			IRQF_DISABLED, "BFIN_UART_MODEM_STATUS", uart)) {
+			uart->cts_pin = -1;
+			dev_info(port->dev, "Unable to attach BlackFin UART Modem Status interrupt.\n");
+		}
 
-	/* CTS RTS PINs are negative assertive. */
-	UART_PUT_MCR(uart, ACTS);
-	UART_SET_IER(uart, EDSSI);
+		/* CTS RTS PINs are negative assertive. */
+		UART_PUT_MCR(uart, ACTS);
+		UART_SET_IER(uart, EDSSI);
+	}
 #endif
 
 	UART_SET_IER(uart, ERBFI);
@@ -792,6 +785,8 @@ static void bfin_serial_shutdown(struct uart_port *port)
 #ifdef CONFIG_SERIAL_BFIN_CTSRTS
 	if (uart->cts_pin >= 0)
 		free_irq(gpio_to_irq(uart->cts_pin), uart);
+	if (uart->rts_pin >= 0)
+		gpio_free(uart->rts_pin);
 #endif
 #ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
 	if (uart->cts_pin >= 0)
@@ -1370,18 +1365,18 @@ static int bfin_serial_probe(struct platform_device *pdev)
 		res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 		if (res == NULL)
 			uart->cts_pin = -1;
-		else
+		else {
 			uart->cts_pin = res->start;
+#ifdef CONFIG_SERIAL_BFIN_CTSRTS
+			uart->port.flags |= ASYNC_CTS_FLOW;
+#endif
+		}
 
 		res = platform_get_resource(pdev, IORESOURCE_IO, 1);
 		if (res == NULL)
 			uart->rts_pin = -1;
 		else
 			uart->rts_pin = res->start;
-# if defined(CONFIG_SERIAL_BFIN_CTSRTS)
-		if (uart->rts_pin >= 0)
-			gpio_request(uart->rts_pin, DRIVER_NAME);
-# endif
 #endif
 	}
 
@@ -1421,10 +1416,6 @@ static int __devexit bfin_serial_remove(struct platform_device *pdev)
 
 	if (uart) {
 		uart_remove_one_port(&bfin_serial_reg, &uart->port);
-#ifdef CONFIG_SERIAL_BFIN_CTSRTS
-		if (uart->rts_pin >= 0)
-			gpio_free(uart->rts_pin);
-#endif
 		iounmap(uart->port.membase);
 		peripheral_free_list(
 			(unsigned short *)pdev->dev.platform_data);
diff --git a/drivers/tty/serial/ifx6x60.c b/drivers/tty/serial/ifx6x60.c
index 426434e5eb7c..7e925e20cbaa 100644
--- a/drivers/tty/serial/ifx6x60.c
+++ b/drivers/tty/serial/ifx6x60.c
@@ -1334,7 +1334,6 @@ MODULE_DEVICE_TABLE(spi, ifx_id_table);
 static const struct spi_driver ifx_spi_driver = {
 	.driver = {
 		.name = DRVNAME,
-		.bus = &spi_bus_type,
 		.pm = &ifx_spi_pm,
 		.owner = THIS_MODULE},
 	.probe = ifx_spi_spi_probe,
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 163fc9021f5a..0b7fed746b27 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -102,6 +102,7 @@
 #define  UCR2_STPB       (1<<6)	 /* Stop */
 #define  UCR2_WS         (1<<5)	 /* Word size */
 #define  UCR2_RTSEN      (1<<4)	 /* Request to send interrupt enable */
+#define  UCR2_ATEN       (1<<3)  /* Aging Timer Enable */
 #define  UCR2_TXEN       (1<<2)	 /* Transmitter enabled */
 #define  UCR2_RXEN       (1<<1)	 /* Receiver enabled */
 #define  UCR2_SRST 	 (1<<0)	 /* SW reset */
@@ -207,6 +208,12 @@ struct imx_port {
 	struct imx_uart_data	*devdata;
 };
 
+struct imx_port_ucrs {
+	unsigned int	ucr1;
+	unsigned int	ucr2;
+	unsigned int	ucr3;
+};
+
 #ifdef CONFIG_IRDA
 #define USE_IRDA(sport)	((sport)->use_irda)
 #else
@@ -260,6 +267,27 @@ static inline int is_imx21_uart(struct imx_port *sport)
 }
 
 /*
+ * Save and restore functions for UCR1, UCR2 and UCR3 registers
+ */
+static void imx_port_ucrs_save(struct uart_port *port,
+			       struct imx_port_ucrs *ucr)
+{
+	/* save control registers */
+	ucr->ucr1 = readl(port->membase + UCR1);
+	ucr->ucr2 = readl(port->membase + UCR2);
+	ucr->ucr3 = readl(port->membase + UCR3);
+}
+
+static void imx_port_ucrs_restore(struct uart_port *port,
+				  struct imx_port_ucrs *ucr)
+{
+	/* restore control registers */
+	writel(ucr->ucr1, port->membase + UCR1);
+	writel(ucr->ucr2, port->membase + UCR2);
+	writel(ucr->ucr3, port->membase + UCR3);
+}
+
+/*
  * Handle any change of modem status signal since we were last called.
  */
 static void imx_mctrl_check(struct imx_port *sport)
@@ -566,6 +594,9 @@ static irqreturn_t imx_int(int irq, void *dev_id)
 	if (sts & USR1_RTSD)
 		imx_rtsint(irq, dev_id);
 
+	if (sts & USR1_AWAKE)
+		writel(USR1_AWAKE, sport->port.membase + USR1);
+
 	return IRQ_HANDLED;
 }
 
@@ -901,6 +932,8 @@ imx_set_termios(struct uart_port *port, struct ktermios *termios,
 			ucr2 |= UCR2_PROE;
 	}
 
+	del_timer_sync(&sport->timer);
+
 	/*
 	 * Ask the core to calculate the divisor for us.
 	 */
@@ -931,8 +964,6 @@ imx_set_termios(struct uart_port *port, struct ktermios *termios,
 			sport->port.ignore_status_mask |= URXD_OVRRUN;
 	}
 
-	del_timer_sync(&sport->timer);
-
 	/*
 	 * Update the per-port timeout.
 	 */
@@ -1079,6 +1110,70 @@ imx_verify_port(struct uart_port *port, struct serial_struct *ser)
 	return ret;
 }
 
+#if defined(CONFIG_CONSOLE_POLL)
+static int imx_poll_get_char(struct uart_port *port)
+{
+	struct imx_port_ucrs old_ucr;
+	unsigned int status;
+	unsigned char c;
+
+	/* save control registers */
+	imx_port_ucrs_save(port, &old_ucr);
+
+	/* disable interrupts */
+	writel(UCR1_UARTEN, port->membase + UCR1);
+	writel(old_ucr.ucr2 & ~(UCR2_ATEN | UCR2_RTSEN | UCR2_ESCI),
+	       port->membase + UCR2);
+	writel(old_ucr.ucr3 & ~(UCR3_DCD | UCR3_RI | UCR3_DTREN),
+	       port->membase + UCR3);
+
+	/* poll */
+	do {
+		status = readl(port->membase + USR2);
+	} while (~status & USR2_RDR);
+
+	/* read */
+	c = readl(port->membase + URXD0);
+
+	/* restore control registers */
+	imx_port_ucrs_restore(port, &old_ucr);
+
+	return c;
+}
+
+static void imx_poll_put_char(struct uart_port *port, unsigned char c)
+{
+	struct imx_port_ucrs old_ucr;
+	unsigned int status;
+
+	/* save control registers */
+	imx_port_ucrs_save(port, &old_ucr);
+
+	/* disable interrupts */
+	writel(UCR1_UARTEN, port->membase + UCR1);
+	writel(old_ucr.ucr2 & ~(UCR2_ATEN | UCR2_RTSEN | UCR2_ESCI),
+	       port->membase + UCR2);
+	writel(old_ucr.ucr3 & ~(UCR3_DCD | UCR3_RI | UCR3_DTREN),
+	       port->membase + UCR3);
+
+	/* drain */
+	do {
+		status = readl(port->membase + USR1);
+	} while (~status & USR1_TRDY);
+
+	/* write */
+	writel(c, port->membase + URTX0);
+
+	/* flush */
+	do {
+		status = readl(port->membase + USR2);
+	} while (~status & USR2_TXDC);
+
+	/* restore control registers */
+	imx_port_ucrs_restore(port, &old_ucr);
+}
+#endif
+
 static struct uart_ops imx_pops = {
 	.tx_empty	= imx_tx_empty,
 	.set_mctrl	= imx_set_mctrl,
@@ -1096,6 +1191,10 @@ static struct uart_ops imx_pops = {
 	.request_port	= imx_request_port,
 	.config_port	= imx_config_port,
 	.verify_port	= imx_verify_port,
+#if defined(CONFIG_CONSOLE_POLL)
+	.poll_get_char  = imx_poll_get_char,
+	.poll_put_char  = imx_poll_put_char,
+#endif
 };
 
 static struct imx_port *imx_ports[UART_NR];
@@ -1118,13 +1217,14 @@ static void
 imx_console_write(struct console *co, const char *s, unsigned int count)
 {
 	struct imx_port *sport = imx_ports[co->index];
-	unsigned int old_ucr1, old_ucr2, ucr1;
+	struct imx_port_ucrs old_ucr;
+	unsigned int ucr1;
 
 	/*
-	 *	First, save UCR1/2 and then disable interrupts
+	 *	First, save UCR1/2/3 and then disable interrupts
 	 */
-	ucr1 = old_ucr1 = readl(sport->port.membase + UCR1);
-	old_ucr2 = readl(sport->port.membase + UCR2);
+	imx_port_ucrs_save(&sport->port, &old_ucr);
+	ucr1 = old_ucr.ucr1;
 
 	if (is_imx1_uart(sport))
 		ucr1 |= IMX1_UCR1_UARTCLKEN;
@@ -1133,18 +1233,17 @@ imx_console_write(struct console *co, const char *s, unsigned int count)
 
 	writel(ucr1, sport->port.membase + UCR1);
 
-	writel(old_ucr2 | UCR2_TXEN, sport->port.membase + UCR2);
+	writel(old_ucr.ucr2 | UCR2_TXEN, sport->port.membase + UCR2);
 
 	uart_console_write(&sport->port, s, count, imx_console_putchar);
 
 	/*
 	 *	Finally, wait for transmitter to become empty
-	 *	and restore UCR1/2
+	 *	and restore UCR1/2/3
 	 */
 	while (!(readl(sport->port.membase + USR2) & USR2_TXDC));
 
-	writel(old_ucr1, sport->port.membase + UCR1);
-	writel(old_ucr2, sport->port.membase + UCR2);
+	imx_port_ucrs_restore(&sport->port, &old_ucr);
 }
 
 /*
@@ -1269,6 +1368,12 @@ static struct uart_driver imx_reg = {
 static int serial_imx_suspend(struct platform_device *dev, pm_message_t state)
 {
 	struct imx_port *sport = platform_get_drvdata(dev);
+	unsigned int val;
+
+	/* enable wakeup from i.MX UART */
+	val = readl(sport->port.membase + UCR3);
+	val |= UCR3_AWAKEN;
+	writel(val, sport->port.membase + UCR3);
 
 	if (sport)
 		uart_suspend_port(&imx_reg, &sport->port);
@@ -1279,6 +1384,12 @@ static int serial_imx_suspend(struct platform_device *dev, pm_message_t state)
 static int serial_imx_resume(struct platform_device *dev)
 {
 	struct imx_port *sport = platform_get_drvdata(dev);
+	unsigned int val;
+
+	/* disable wakeup from i.MX UART */
+	val = readl(sport->port.membase + UCR3);
+	val &= ~UCR3_AWAKEN;
+	writel(val, sport->port.membase + UCR3);
 
 	if (sport)
 		uart_resume_port(&imx_reg, &sport->port);
@@ -1287,6 +1398,10 @@ static int serial_imx_resume(struct platform_device *dev)
 }
 
 #ifdef CONFIG_OF
+/*
+ * This function returns 1 iff pdev isn't a device instatiated by dt, 0 iff it
+ * could successfully get all information from dt or a negative errno.
+ */
 static int serial_imx_probe_dt(struct imx_port *sport,
 		struct platform_device *pdev)
 {
@@ -1296,12 +1411,13 @@ static int serial_imx_probe_dt(struct imx_port *sport,
 	int ret;
 
 	if (!np)
-		return -ENODEV;
+		/* no device tree device */
+		return 1;
 
 	ret = of_alias_get_id(np, "serial");
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to get alias id, errno %d\n", ret);
-		return -ENODEV;
+		return ret;
 	}
 	sport->port.line = ret;
 
@@ -1319,7 +1435,7 @@ static int serial_imx_probe_dt(struct imx_port *sport,
 static inline int serial_imx_probe_dt(struct imx_port *sport,
 		struct platform_device *pdev)
 {
-	return -ENODEV;
+	return 1;
 }
 #endif
 
@@ -1354,8 +1470,10 @@ static int serial_imx_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	ret = serial_imx_probe_dt(sport, pdev);
-	if (ret == -ENODEV)
+	if (ret > 0)
 		serial_imx_probe_pdata(sport, pdev);
+	else if (ret < 0)
+		goto free;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
@@ -1476,7 +1594,7 @@ static int __init imx_serial_init(void)
 	if (ret != 0)
 		uart_unregister_driver(&imx_reg);
 
-	return 0;
+	return ret;
 }
 
 static void __exit imx_serial_exit(void)
diff --git a/drivers/tty/serial/m32r_sio.c b/drivers/tty/serial/m32r_sio.c
index 08018934e013..94a6792bf97b 100644
--- a/drivers/tty/serial/m32r_sio.c
+++ b/drivers/tty/serial/m32r_sio.c
@@ -1000,11 +1000,8 @@ static void __init m32r_sio_register_ports(struct uart_driver *drv)
 		init_timer(&up->timer);
 		up->timer.function = m32r_sio_timeout;
 
-		/*
-		 * ALPHA_KLUDGE_MCR needs to be killed.
-		 */
-		up->mcr_mask = ~ALPHA_KLUDGE_MCR;
-		up->mcr_force = ALPHA_KLUDGE_MCR;
+		up->mcr_mask = ~0;
+		up->mcr_force = 0;
 
 		uart_add_one_port(drv, &up->port);
 	}
diff --git a/drivers/tty/serial/max3100.c b/drivers/tty/serial/max3100.c
index 8a6cc8c30b5a..b4902b99cfd2 100644
--- a/drivers/tty/serial/max3100.c
+++ b/drivers/tty/serial/max3100.c
@@ -901,7 +901,6 @@ static int max3100_resume(struct spi_device *spi)
 static struct spi_driver max3100_driver = {
 	.driver = {
 		.name		= "max3100",
-		.bus		= &spi_bus_type,
 		.owner		= THIS_MODULE,
 	},
 
diff --git a/drivers/tty/serial/max3107-aava.c b/drivers/tty/serial/max3107-aava.c
index 90c40f22ec70..aae772a71de6 100644
--- a/drivers/tty/serial/max3107-aava.c
+++ b/drivers/tty/serial/max3107-aava.c
@@ -315,7 +315,6 @@ static int __devinit max3107_probe_aava(struct spi_device *spi)
 static struct spi_driver max3107_driver = {
 	.driver = {
 		.name		= "aava-max3107",
-		.bus		= &spi_bus_type,
 		.owner		= THIS_MODULE,
 	},
 	.probe		= max3107_probe_aava,
diff --git a/drivers/tty/serial/max3107.c b/drivers/tty/serial/max3107.c
index 7827000db4f5..17c7ba805d98 100644
--- a/drivers/tty/serial/max3107.c
+++ b/drivers/tty/serial/max3107.c
@@ -1181,7 +1181,6 @@ static int max3107_probe_generic(struct spi_device *spi)
 static struct spi_driver max3107_driver = {
 	.driver = {
 		.name		= "max3107",
-		.bus		= &spi_bus_type,
 		.owner		= THIS_MODULE,
 	},
 	.probe		= max3107_probe_generic,
diff --git a/drivers/tty/serial/mfd.c b/drivers/tty/serial/mfd.c
index e272d3919c67..a9234ba8f8d5 100644
--- a/drivers/tty/serial/mfd.c
+++ b/drivers/tty/serial/mfd.c
@@ -1154,7 +1154,6 @@ serial_hsu_console_setup(struct console *co, char *options)
 	int bits = 8;
 	int parity = 'n';
 	int flow = 'n';
-	int ret;
 
 	if (co->index == -1 || co->index >= serial_hsu_reg.nr)
 		co->index = 0;
@@ -1165,9 +1164,7 @@ serial_hsu_console_setup(struct console *co, char *options)
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
 
-	ret = uart_set_options(&up->port, co, baud, parity, bits, flow);
-
-	return ret;
+	return uart_set_options(&up->port, co, baud, parity, bits, flow);
 }
 
 static struct console serial_hsu_console = {
@@ -1176,9 +1173,13 @@ static struct console serial_hsu_console = {
 	.device		= uart_console_device,
 	.setup		= serial_hsu_console_setup,
 	.flags		= CON_PRINTBUFFER,
-	.index		= 2,
+	.index		= -1,
 	.data		= &serial_hsu_reg,
 };
+
+#define SERIAL_HSU_CONSOLE	(&serial_hsu_console)
+#else
+#define SERIAL_HSU_CONSOLE	NULL
 #endif
 
 struct uart_ops serial_hsu_pops = {
@@ -1208,6 +1209,7 @@ static struct uart_driver serial_hsu_reg = {
 	.major		= TTY_MAJOR,
 	.minor		= 128,
 	.nr		= 3,
+	.cons		= SERIAL_HSU_CONSOLE,
 };
 
 #ifdef CONFIG_PM
@@ -1342,12 +1344,6 @@ static int serial_hsu_probe(struct pci_dev *pdev,
 		}
 		uart_add_one_port(&serial_hsu_reg, &uport->port);
 
-#ifdef CONFIG_SERIAL_MFD_HSU_CONSOLE
-		if (index == 2) {
-			register_console(&serial_hsu_console);
-			uport->port.cons = &serial_hsu_console;
-		}
-#endif
 		pci_set_drvdata(pdev, uport);
 	}
 
diff --git a/drivers/tty/serial/mrst_max3110.c b/drivers/tty/serial/mrst_max3110.c
index 4c309e869903..df2a2240a3ae 100644
--- a/drivers/tty/serial/mrst_max3110.c
+++ b/drivers/tty/serial/mrst_max3110.c
@@ -876,7 +876,6 @@ static int __devexit serial_m3110_remove(struct spi_device *dev)
 static struct spi_driver uart_max3110_driver = {
 	.driver = {
 			.name	= "spi_max3111",
-			.bus	= &spi_bus_type,
 			.owner	= THIS_MODULE,
 	},
 	.probe		= serial_m3110_probe,
diff --git a/drivers/tty/serial/msm_serial_hs.c b/drivers/tty/serial/msm_serial_hs.c
index 60c6eb850265..5e85e1e14c44 100644
--- a/drivers/tty/serial/msm_serial_hs.c
+++ b/drivers/tty/serial/msm_serial_hs.c
@@ -422,9 +422,9 @@ static int __devexit msm_hs_remove(struct platform_device *pdev)
 		      msm_uport->rx.rbuffer);
 	dma_pool_destroy(msm_uport->rx.pool);
 
-	dma_unmap_single(dev, msm_uport->rx.cmdptr_dmaaddr, sizeof(u32 *),
+	dma_unmap_single(dev, msm_uport->rx.cmdptr_dmaaddr, sizeof(u32),
 			 DMA_TO_DEVICE);
-	dma_unmap_single(dev, msm_uport->tx.mapped_cmd_ptr_ptr, sizeof(u32 *),
+	dma_unmap_single(dev, msm_uport->tx.mapped_cmd_ptr_ptr, sizeof(u32),
 			 DMA_TO_DEVICE);
 	dma_unmap_single(dev, msm_uport->tx.mapped_cmd_ptr, sizeof(dmov_box),
 			 DMA_TO_DEVICE);
@@ -812,7 +812,7 @@ static void msm_hs_submit_tx_locked(struct uart_port *uport)
 	*tx->command_ptr_ptr = CMD_PTR_LP | DMOV_CMD_ADDR(tx->mapped_cmd_ptr);
 
 	dma_sync_single_for_device(uport->dev, tx->mapped_cmd_ptr_ptr,
-				   sizeof(u32 *), DMA_TO_DEVICE);
+				   sizeof(u32), DMA_TO_DEVICE);
 
 	/* Save tx_count to use in Callback */
 	tx->tx_count = tx_count;
@@ -1087,12 +1087,10 @@ static void msm_hs_config_port(struct uart_port *uport, int cfg_flags)
 }
 
 /*  Handle CTS changes (Called from interrupt handler) */
-static void msm_hs_handle_delta_cts(struct uart_port *uport)
+static void msm_hs_handle_delta_cts_locked(struct uart_port *uport)
 {
-	unsigned long flags;
 	struct msm_hs_port *msm_uport = UARTDM_TO_MSM(uport);
 
-	spin_lock_irqsave(&uport->lock, flags);
 	clk_enable(msm_uport->clk);
 
 	/* clear interrupt */
@@ -1100,7 +1098,6 @@ static void msm_hs_handle_delta_cts(struct uart_port *uport)
 	uport->icount.cts++;
 
 	clk_disable(msm_uport->clk);
-	spin_unlock_irqrestore(&uport->lock, flags);
 
 	/* clear the IOCTL TIOCMIWAIT if called */
 	wake_up_interruptible(&uport->state->port.delta_msr_wait);
@@ -1248,7 +1245,7 @@ static irqreturn_t msm_hs_isr(int irq, void *dev)
 
 	/* Change in CTS interrupt */
 	if (isr_status & UARTDM_ISR_DELTA_CTS_BMSK)
-		msm_hs_handle_delta_cts(uport);
+		msm_hs_handle_delta_cts_locked(uport);
 
 	spin_unlock_irqrestore(&uport->lock, flags);
 
@@ -1537,7 +1534,7 @@ static int __devinit uartdm_init_port(struct uart_port *uport)
 	if (!tx->command_ptr)
 		return -ENOMEM;
 
-	tx->command_ptr_ptr = kmalloc(sizeof(u32 *), GFP_KERNEL | __GFP_DMA);
+	tx->command_ptr_ptr = kmalloc(sizeof(u32), GFP_KERNEL | __GFP_DMA);
 	if (!tx->command_ptr_ptr) {
 		ret = -ENOMEM;
 		goto err_tx_command_ptr_ptr;
@@ -1547,7 +1544,7 @@ static int __devinit uartdm_init_port(struct uart_port *uport)
 					    sizeof(dmov_box), DMA_TO_DEVICE);
 	tx->mapped_cmd_ptr_ptr = dma_map_single(uport->dev,
 						tx->command_ptr_ptr,
-						sizeof(u32 *), DMA_TO_DEVICE);
+						sizeof(u32), DMA_TO_DEVICE);
 	tx->xfer.cmdptr = DMOV_CMD_ADDR(tx->mapped_cmd_ptr_ptr);
 
 	init_waitqueue_head(&rx->wait);
@@ -1575,7 +1572,7 @@ static int __devinit uartdm_init_port(struct uart_port *uport)
 		goto err_rx_command_ptr;
 	}
 
-	rx->command_ptr_ptr = kmalloc(sizeof(u32 *), GFP_KERNEL | __GFP_DMA);
+	rx->command_ptr_ptr = kmalloc(sizeof(u32), GFP_KERNEL | __GFP_DMA);
 	if (!rx->command_ptr_ptr) {
 		pr_err("%s(): cannot allocate rx->command_ptr_ptr", __func__);
 		ret = -ENOMEM;
@@ -1593,7 +1590,7 @@ static int __devinit uartdm_init_port(struct uart_port *uport)
 	*rx->command_ptr_ptr = CMD_PTR_LP | DMOV_CMD_ADDR(rx->mapped_cmd_ptr);
 
 	rx->cmdptr_dmaaddr = dma_map_single(uport->dev, rx->command_ptr_ptr,
-					    sizeof(u32 *), DMA_TO_DEVICE);
+					    sizeof(u32), DMA_TO_DEVICE);
 	rx->xfer.cmdptr = DMOV_CMD_ADDR(rx->cmdptr_dmaaddr);
 
 	INIT_WORK(&rx->tty_work, msm_hs_tty_flip_buffer_work);
@@ -1609,7 +1606,7 @@ err_dma_pool_alloc:
 	dma_pool_destroy(msm_uport->rx.pool);
 err_dma_pool_create:
 	dma_unmap_single(uport->dev, msm_uport->tx.mapped_cmd_ptr_ptr,
-				sizeof(u32 *), DMA_TO_DEVICE);
+				sizeof(u32), DMA_TO_DEVICE);
 	dma_unmap_single(uport->dev, msm_uport->tx.mapped_cmd_ptr,
 				sizeof(dmov_box), DMA_TO_DEVICE);
 	kfree(msm_uport->tx.command_ptr_ptr);
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index 7e02c9c344fe..55fd362b9879 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -145,11 +145,12 @@ static inline void mxs_auart_tx_chars(struct mxs_auart_port *s)
 			writel(xmit->buf[xmit->tail],
 				     s->port.membase + AUART_DATA);
 			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
-			if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
-				uart_write_wakeup(&s->port);
 		} else
 			break;
 	}
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(&s->port);
+
 	if (uart_circ_empty(&(s->port.state->xmit)))
 		writel(AUART_INTR_TXIEN,
 			     s->port.membase + AUART_INTR_CLR);
@@ -424,7 +425,7 @@ static int mxs_auart_startup(struct uart_port *u)
 {
 	struct mxs_auart_port *s = to_auart_port(u);
 
-	clk_enable(s->clk);
+	clk_prepare_enable(s->clk);
 
 	writel(AUART_CTRL0_CLKGATE, u->membase + AUART_CTRL0_CLR);
 
@@ -453,7 +454,7 @@ static void mxs_auart_shutdown(struct uart_port *u)
 	writel(AUART_INTR_RXIEN | AUART_INTR_RTIEN | AUART_INTR_CTSMIEN,
 			u->membase + AUART_INTR_CLR);
 
-	clk_disable(s->clk);
+	clk_disable_unprepare(s->clk);
 }
 
 static unsigned int mxs_auart_tx_empty(struct uart_port *u)
@@ -634,7 +635,7 @@ auart_console_setup(struct console *co, char *options)
 	if (!s)
 		return -ENODEV;
 
-	clk_enable(s->clk);
+	clk_prepare_enable(s->clk);
 
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
@@ -643,7 +644,7 @@ auart_console_setup(struct console *co, char *options)
 
 	ret = uart_set_options(&s->port, co, baud, parity, bits, flow);
 
-	clk_disable(s->clk);
+	clk_disable_unprepare(s->clk);
 
 	return ret;
 }
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index 5e713d3ef1f4..d192dcbb82f5 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -37,17 +37,24 @@
 #include <linux/clk.h>
 #include <linux/serial_core.h>
 #include <linux/irq.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
 
 #include <plat/dma.h>
 #include <plat/dmtimer.h>
 #include <plat/omap-serial.h>
 
+#define DEFAULT_CLK_SPEED 48000000 /* 48Mhz*/
+
 static struct uart_omap_port *ui[OMAP_MAX_HSUART_PORTS];
 
 /* Forward declaration of functions */
 static void uart_tx_dma_callback(int lch, u16 ch_status, void *data);
-static void serial_omap_rx_timeout(unsigned long uart_no);
+static void serial_omap_rxdma_poll(unsigned long uart_no);
 static int serial_omap_start_rxdma(struct uart_omap_port *up);
+static void serial_omap_mdr1_errataset(struct uart_omap_port *up, u8 mdr1);
+
+static struct workqueue_struct *serial_omap_uart_wq;
 
 static inline unsigned int serial_in(struct uart_omap_port *up, int offset)
 {
@@ -102,6 +109,8 @@ static void serial_omap_stop_rxdma(struct uart_omap_port *up)
 		omap_free_dma(up->uart_dma.rx_dma_channel);
 		up->uart_dma.rx_dma_channel = OMAP_UART_DMA_CH_FREE;
 		up->uart_dma.rx_dma_used = false;
+		pm_runtime_mark_last_busy(&up->pdev->dev);
+		pm_runtime_put_autosuspend(&up->pdev->dev);
 	}
 }
 
@@ -109,9 +118,12 @@ static void serial_omap_enable_ms(struct uart_port *port)
 {
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
 
-	dev_dbg(up->port.dev, "serial_omap_enable_ms+%d\n", up->pdev->id);
+	dev_dbg(up->port.dev, "serial_omap_enable_ms+%d\n", up->port.line);
+
+	pm_runtime_get_sync(&up->pdev->dev);
 	up->ier |= UART_IER_MSI;
 	serial_out(up, UART_IER, up->ier);
+	pm_runtime_put(&up->pdev->dev);
 }
 
 static void serial_omap_stop_tx(struct uart_port *port)
@@ -129,30 +141,40 @@ static void serial_omap_stop_tx(struct uart_port *port)
 		omap_stop_dma(up->uart_dma.tx_dma_channel);
 		omap_free_dma(up->uart_dma.tx_dma_channel);
 		up->uart_dma.tx_dma_channel = OMAP_UART_DMA_CH_FREE;
+		pm_runtime_mark_last_busy(&up->pdev->dev);
+		pm_runtime_put_autosuspend(&up->pdev->dev);
 	}
 
+	pm_runtime_get_sync(&up->pdev->dev);
 	if (up->ier & UART_IER_THRI) {
 		up->ier &= ~UART_IER_THRI;
 		serial_out(up, UART_IER, up->ier);
 	}
+
+	pm_runtime_mark_last_busy(&up->pdev->dev);
+	pm_runtime_put_autosuspend(&up->pdev->dev);
 }
 
 static void serial_omap_stop_rx(struct uart_port *port)
 {
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
 
+	pm_runtime_get_sync(&up->pdev->dev);
 	if (up->use_dma)
 		serial_omap_stop_rxdma(up);
 	up->ier &= ~UART_IER_RLSI;
 	up->port.read_status_mask &= ~UART_LSR_DR;
 	serial_out(up, UART_IER, up->ier);
+	pm_runtime_mark_last_busy(&up->pdev->dev);
+	pm_runtime_put_autosuspend(&up->pdev->dev);
 }
 
-static inline void receive_chars(struct uart_omap_port *up, int *status)
+static inline void receive_chars(struct uart_omap_port *up,
+		unsigned int *status)
 {
 	struct tty_struct *tty = up->port.state->port.tty;
-	unsigned int flag;
-	unsigned char ch, lsr = *status;
+	unsigned int flag, lsr = *status;
+	unsigned char ch = 0;
 	int max_count = 256;
 
 	do {
@@ -262,7 +284,10 @@ static void serial_omap_start_tx(struct uart_port *port)
 	int ret = 0;
 
 	if (!up->use_dma) {
+		pm_runtime_get_sync(&up->pdev->dev);
 		serial_omap_enable_ier_thri(up);
+		pm_runtime_mark_last_busy(&up->pdev->dev);
+		pm_runtime_put_autosuspend(&up->pdev->dev);
 		return;
 	}
 
@@ -272,6 +297,7 @@ static void serial_omap_start_tx(struct uart_port *port)
 	xmit = &up->port.state->xmit;
 
 	if (up->uart_dma.tx_dma_channel == OMAP_UART_DMA_CH_FREE) {
+		pm_runtime_get_sync(&up->pdev->dev);
 		ret = omap_request_dma(up->uart_dma.uart_dma_tx,
 				"UART Tx DMA",
 				(void *)uart_tx_dma_callback, up,
@@ -354,9 +380,13 @@ static inline irqreturn_t serial_omap_irq(int irq, void *dev_id)
 	unsigned int iir, lsr;
 	unsigned long flags;
 
+	pm_runtime_get_sync(&up->pdev->dev);
 	iir = serial_in(up, UART_IIR);
-	if (iir & UART_IIR_NO_INT)
+	if (iir & UART_IIR_NO_INT) {
+		pm_runtime_mark_last_busy(&up->pdev->dev);
+		pm_runtime_put_autosuspend(&up->pdev->dev);
 		return IRQ_NONE;
+	}
 
 	spin_lock_irqsave(&up->port.lock, flags);
 	lsr = serial_in(up, UART_LSR);
@@ -378,6 +408,9 @@ static inline irqreturn_t serial_omap_irq(int irq, void *dev_id)
 		transmit_chars(up);
 
 	spin_unlock_irqrestore(&up->port.lock, flags);
+	pm_runtime_mark_last_busy(&up->pdev->dev);
+	pm_runtime_put_autosuspend(&up->pdev->dev);
+
 	up->port_activity = jiffies;
 	return IRQ_HANDLED;
 }
@@ -388,22 +421,26 @@ static unsigned int serial_omap_tx_empty(struct uart_port *port)
 	unsigned long flags = 0;
 	unsigned int ret = 0;
 
-	dev_dbg(up->port.dev, "serial_omap_tx_empty+%d\n", up->pdev->id);
+	pm_runtime_get_sync(&up->pdev->dev);
+	dev_dbg(up->port.dev, "serial_omap_tx_empty+%d\n", up->port.line);
 	spin_lock_irqsave(&up->port.lock, flags);
 	ret = serial_in(up, UART_LSR) & UART_LSR_TEMT ? TIOCSER_TEMT : 0;
 	spin_unlock_irqrestore(&up->port.lock, flags);
-
+	pm_runtime_put(&up->pdev->dev);
 	return ret;
 }
 
 static unsigned int serial_omap_get_mctrl(struct uart_port *port)
 {
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
-	unsigned char status;
+	unsigned int status;
 	unsigned int ret = 0;
 
+	pm_runtime_get_sync(&up->pdev->dev);
 	status = check_modem_status(up);
-	dev_dbg(up->port.dev, "serial_omap_get_mctrl+%d\n", up->pdev->id);
+	pm_runtime_put(&up->pdev->dev);
+
+	dev_dbg(up->port.dev, "serial_omap_get_mctrl+%d\n", up->port.line);
 
 	if (status & UART_MSR_DCD)
 		ret |= TIOCM_CAR;
@@ -421,7 +458,7 @@ static void serial_omap_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
 	unsigned char mcr = 0;
 
-	dev_dbg(up->port.dev, "serial_omap_set_mctrl+%d\n", up->pdev->id);
+	dev_dbg(up->port.dev, "serial_omap_set_mctrl+%d\n", up->port.line);
 	if (mctrl & TIOCM_RTS)
 		mcr |= UART_MCR_RTS;
 	if (mctrl & TIOCM_DTR)
@@ -433,8 +470,11 @@ static void serial_omap_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	if (mctrl & TIOCM_LOOP)
 		mcr |= UART_MCR_LOOP;
 
-	mcr |= up->mcr;
-	serial_out(up, UART_MCR, mcr);
+	pm_runtime_get_sync(&up->pdev->dev);
+	up->mcr = serial_in(up, UART_MCR);
+	up->mcr |= mcr;
+	serial_out(up, UART_MCR, up->mcr);
+	pm_runtime_put(&up->pdev->dev);
 }
 
 static void serial_omap_break_ctl(struct uart_port *port, int break_state)
@@ -442,7 +482,8 @@ static void serial_omap_break_ctl(struct uart_port *port, int break_state)
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
 	unsigned long flags = 0;
 
-	dev_dbg(up->port.dev, "serial_omap_break_ctl+%d\n", up->pdev->id);
+	dev_dbg(up->port.dev, "serial_omap_break_ctl+%d\n", up->port.line);
+	pm_runtime_get_sync(&up->pdev->dev);
 	spin_lock_irqsave(&up->port.lock, flags);
 	if (break_state == -1)
 		up->lcr |= UART_LCR_SBC;
@@ -450,6 +491,7 @@ static void serial_omap_break_ctl(struct uart_port *port, int break_state)
 		up->lcr &= ~UART_LCR_SBC;
 	serial_out(up, UART_LCR, up->lcr);
 	spin_unlock_irqrestore(&up->port.lock, flags);
+	pm_runtime_put(&up->pdev->dev);
 }
 
 static int serial_omap_startup(struct uart_port *port)
@@ -466,8 +508,9 @@ static int serial_omap_startup(struct uart_port *port)
 	if (retval)
 		return retval;
 
-	dev_dbg(up->port.dev, "serial_omap_startup+%d\n", up->pdev->id);
+	dev_dbg(up->port.dev, "serial_omap_startup+%d\n", up->port.line);
 
+	pm_runtime_get_sync(&up->pdev->dev);
 	/*
 	 * Clear the FIFO buffers and disable them.
 	 * (they will be reenabled in set_termios())
@@ -505,8 +548,8 @@ static int serial_omap_startup(struct uart_port *port)
 			(dma_addr_t *)&(up->uart_dma.tx_buf_dma_phys),
 			0);
 		init_timer(&(up->uart_dma.rx_timer));
-		up->uart_dma.rx_timer.function = serial_omap_rx_timeout;
-		up->uart_dma.rx_timer.data = up->pdev->id;
+		up->uart_dma.rx_timer.function = serial_omap_rxdma_poll;
+		up->uart_dma.rx_timer.data = up->port.line;
 		/* Currently the buffer size is 4KB. Can increase it */
 		up->uart_dma.rx_buf = dma_alloc_coherent(NULL,
 			up->uart_dma.rx_buf_size,
@@ -523,6 +566,8 @@ static int serial_omap_startup(struct uart_port *port)
 	/* Enable module level wake up */
 	serial_out(up, UART_OMAP_WER, OMAP_UART_WER_MOD_WKUP);
 
+	pm_runtime_mark_last_busy(&up->pdev->dev);
+	pm_runtime_put_autosuspend(&up->pdev->dev);
 	up->port_activity = jiffies;
 	return 0;
 }
@@ -532,7 +577,9 @@ static void serial_omap_shutdown(struct uart_port *port)
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
 	unsigned long flags = 0;
 
-	dev_dbg(up->port.dev, "serial_omap_shutdown+%d\n", up->pdev->id);
+	dev_dbg(up->port.dev, "serial_omap_shutdown+%d\n", up->port.line);
+
+	pm_runtime_get_sync(&up->pdev->dev);
 	/*
 	 * Disable interrupts from this port
 	 */
@@ -566,6 +613,8 @@ static void serial_omap_shutdown(struct uart_port *port)
 			up->uart_dma.rx_buf_dma_phys);
 		up->uart_dma.rx_buf = NULL;
 	}
+
+	pm_runtime_put(&up->pdev->dev);
 	free_irq(up->port.irq, up);
 }
 
@@ -573,8 +622,6 @@ static inline void
 serial_omap_configure_xonxoff
 		(struct uart_omap_port *up, struct ktermios *termios)
 {
-	unsigned char efr = 0;
-
 	up->lcr = serial_in(up, UART_LCR);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
 	up->efr = serial_in(up, UART_EFR);
@@ -584,8 +631,7 @@ serial_omap_configure_xonxoff
 	serial_out(up, UART_XOFF1, termios->c_cc[VSTOP]);
 
 	/* clear SW control mode bits */
-	efr = up->efr;
-	efr &= OMAP_UART_SW_CLR;
+	up->efr &= OMAP_UART_SW_CLR;
 
 	/*
 	 * IXON Flag:
@@ -593,7 +639,7 @@ serial_omap_configure_xonxoff
 	 * Transmit XON1, XOFF1
 	 */
 	if (termios->c_iflag & IXON)
-		efr |= OMAP_UART_SW_TX;
+		up->efr |= OMAP_UART_SW_TX;
 
 	/*
 	 * IXOFF Flag:
@@ -601,7 +647,7 @@ serial_omap_configure_xonxoff
 	 * Receiver compares XON1, XOFF1.
 	 */
 	if (termios->c_iflag & IXOFF)
-		efr |= OMAP_UART_SW_RX;
+		up->efr |= OMAP_UART_SW_RX;
 
 	serial_out(up, UART_EFR, up->efr | UART_EFR_ECB);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_A);
@@ -624,13 +670,21 @@ serial_omap_configure_xonxoff
 	 * load the new software flow control mode IXON or IXOFF
 	 * and restore the UARTi.EFR_REG[4] ENHANCED_EN value.
 	 */
-	serial_out(up, UART_EFR, efr | UART_EFR_SCD);
+	serial_out(up, UART_EFR, up->efr | UART_EFR_SCD);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_A);
 
 	serial_out(up, UART_MCR, up->mcr & ~UART_MCR_TCRTLR);
 	serial_out(up, UART_LCR, up->lcr);
 }
 
+static void serial_omap_uart_qos_work(struct work_struct *work)
+{
+	struct uart_omap_port *up = container_of(work, struct uart_omap_port,
+						qos_work);
+
+	pm_qos_update_request(&up->pm_qos_request, up->latency);
+}
+
 static void
 serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 			struct ktermios *old)
@@ -671,6 +725,16 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 	baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk/13);
 	quot = serial_omap_get_divisor(port, baud);
 
+	/* calculate wakeup latency constraint */
+	up->calc_latency = (1000000 * up->port.fifosize) /
+				(1000 * baud / 8);
+	up->latency = up->calc_latency;
+	schedule_work(&up->qos_work);
+
+	up->dll = quot & 0xff;
+	up->dlh = quot >> 8;
+	up->mdr1 = UART_OMAP_MDR1_DISABLE;
+
 	up->fcr = UART_FCR_R_TRIG_01 | UART_FCR_T_TRIG_01 |
 			UART_FCR_ENABLE_FIFO;
 	if (up->use_dma)
@@ -680,6 +744,7 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 	 * Ok, we're now changing the port state. Do it with
 	 * interrupts disabled.
 	 */
+	pm_runtime_get_sync(&up->pdev->dev);
 	spin_lock_irqsave(&up->port.lock, flags);
 
 	/*
@@ -723,6 +788,8 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 		up->ier |= UART_IER_MSI;
 	serial_out(up, UART_IER, up->ier);
 	serial_out(up, UART_LCR, cval);		/* reset DLAB */
+	up->lcr = cval;
+	up->scr = OMAP_UART_SCR_TX_EMPTY;
 
 	/* FIFOs and DMA Settings */
 
@@ -749,17 +816,22 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	if (up->use_dma) {
 		serial_out(up, UART_TI752_TLR, 0);
-		serial_out(up, UART_OMAP_SCR,
-			(UART_FCR_TRIGGER_4 | UART_FCR_TRIGGER_8));
+		up->scr |= (UART_FCR_TRIGGER_4 | UART_FCR_TRIGGER_8);
 	}
 
+	serial_out(up, UART_OMAP_SCR, up->scr);
+
 	serial_out(up, UART_EFR, up->efr);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_A);
 	serial_out(up, UART_MCR, up->mcr);
 
 	/* Protocol, Baud Rate, and Interrupt Settings */
 
-	serial_out(up, UART_OMAP_MDR1, UART_OMAP_MDR1_DISABLE);
+	if (up->errata & UART_ERRATA_i202_MDR1_ACCESS)
+		serial_omap_mdr1_errataset(up, up->mdr1);
+	else
+		serial_out(up, UART_OMAP_MDR1, up->mdr1);
+
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
 
 	up->efr = serial_in(up, UART_EFR);
@@ -769,8 +841,8 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 	serial_out(up, UART_IER, 0);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
 
-	serial_out(up, UART_DLL, quot & 0xff);          /* LS of divisor */
-	serial_out(up, UART_DLM, quot >> 8);            /* MS of divisor */
+	serial_out(up, UART_DLL, up->dll);	/* LS of divisor */
+	serial_out(up, UART_DLM, up->dlh);	/* MS of divisor */
 
 	serial_out(up, UART_LCR, 0);
 	serial_out(up, UART_IER, up->ier);
@@ -780,9 +852,14 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 	serial_out(up, UART_LCR, cval);
 
 	if (baud > 230400 && baud != 3000000)
-		serial_out(up, UART_OMAP_MDR1, UART_OMAP_MDR1_13X_MODE);
+		up->mdr1 = UART_OMAP_MDR1_13X_MODE;
 	else
-		serial_out(up, UART_OMAP_MDR1, UART_OMAP_MDR1_16X_MODE);
+		up->mdr1 = UART_OMAP_MDR1_16X_MODE;
+
+	if (up->errata & UART_ERRATA_i202_MDR1_ACCESS)
+		serial_omap_mdr1_errataset(up, up->mdr1);
+	else
+		serial_out(up, UART_OMAP_MDR1, up->mdr1);
 
 	/* Hardware Flow Control Configuration */
 
@@ -809,7 +886,8 @@ serial_omap_set_termios(struct uart_port *port, struct ktermios *termios,
 	serial_omap_configure_xonxoff(up, termios);
 
 	spin_unlock_irqrestore(&up->port.lock, flags);
-	dev_dbg(up->port.dev, "serial_omap_set_termios+%d\n", up->pdev->id);
+	pm_runtime_put(&up->pdev->dev);
+	dev_dbg(up->port.dev, "serial_omap_set_termios+%d\n", up->port.line);
 }
 
 static void
@@ -819,7 +897,9 @@ serial_omap_pm(struct uart_port *port, unsigned int state,
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
 	unsigned char efr;
 
-	dev_dbg(up->port.dev, "serial_omap_pm+%d\n", up->pdev->id);
+	dev_dbg(up->port.dev, "serial_omap_pm+%d\n", up->port.line);
+
+	pm_runtime_get_sync(&up->pdev->dev);
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
 	efr = serial_in(up, UART_EFR);
 	serial_out(up, UART_EFR, efr | UART_EFR_ECB);
@@ -829,6 +909,15 @@ serial_omap_pm(struct uart_port *port, unsigned int state,
 	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B);
 	serial_out(up, UART_EFR, efr);
 	serial_out(up, UART_LCR, 0);
+
+	if (!device_may_wakeup(&up->pdev->dev)) {
+		if (!state)
+			pm_runtime_forbid(&up->pdev->dev);
+		else
+			pm_runtime_allow(&up->pdev->dev);
+	}
+
+	pm_runtime_put(&up->pdev->dev);
 }
 
 static void serial_omap_release_port(struct uart_port *port)
@@ -847,7 +936,7 @@ static void serial_omap_config_port(struct uart_port *port, int flags)
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
 
 	dev_dbg(up->port.dev, "serial_omap_config_port+%d\n",
-							up->pdev->id);
+							up->port.line);
 	up->port.type = PORT_OMAP;
 }
 
@@ -864,7 +953,7 @@ serial_omap_type(struct uart_port *port)
 {
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
 
-	dev_dbg(up->port.dev, "serial_omap_type+%d\n", up->pdev->id);
+	dev_dbg(up->port.dev, "serial_omap_type+%d\n", up->port.line);
 	return up->name;
 }
 
@@ -906,19 +995,26 @@ static inline void wait_for_xmitr(struct uart_omap_port *up)
 static void serial_omap_poll_put_char(struct uart_port *port, unsigned char ch)
 {
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
+
+	pm_runtime_get_sync(&up->pdev->dev);
 	wait_for_xmitr(up);
 	serial_out(up, UART_TX, ch);
+	pm_runtime_put(&up->pdev->dev);
 }
 
 static int serial_omap_poll_get_char(struct uart_port *port)
 {
 	struct uart_omap_port *up = (struct uart_omap_port *)port;
-	unsigned int status = serial_in(up, UART_LSR);
+	unsigned int status;
 
+	pm_runtime_get_sync(&up->pdev->dev);
+	status = serial_in(up, UART_LSR);
 	if (!(status & UART_LSR_DR))
 		return NO_POLL_CHAR;
 
-	return serial_in(up, UART_RX);
+	status = serial_in(up, UART_RX);
+	pm_runtime_put(&up->pdev->dev);
+	return status;
 }
 
 #endif /* CONFIG_CONSOLE_POLL */
@@ -946,6 +1042,8 @@ serial_omap_console_write(struct console *co, const char *s,
 	unsigned int ier;
 	int locked = 1;
 
+	pm_runtime_get_sync(&up->pdev->dev);
+
 	local_irq_save(flags);
 	if (up->port.sysrq)
 		locked = 0;
@@ -978,6 +1076,8 @@ serial_omap_console_write(struct console *co, const char *s,
 	if (up->msr_saved_flags)
 		check_modem_status(up);
 
+	pm_runtime_mark_last_busy(&up->pdev->dev);
+	pm_runtime_put_autosuspend(&up->pdev->dev);
 	if (locked)
 		spin_unlock(&up->port.lock);
 	local_irq_restore(flags);
@@ -1014,7 +1114,7 @@ static struct console serial_omap_console = {
 
 static void serial_omap_add_console_port(struct uart_omap_port *up)
 {
-	serial_omap_console_ports[up->pdev->id] = up;
+	serial_omap_console_ports[up->port.line] = up;
 }
 
 #define OMAP_CONSOLE	(&serial_omap_console)
@@ -1060,26 +1160,30 @@ static struct uart_driver serial_omap_reg = {
 	.cons		= OMAP_CONSOLE,
 };
 
-static int
-serial_omap_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_SUSPEND
+static int serial_omap_suspend(struct device *dev)
 {
-	struct uart_omap_port *up = platform_get_drvdata(pdev);
+	struct uart_omap_port *up = dev_get_drvdata(dev);
 
-	if (up)
+	if (up) {
 		uart_suspend_port(&serial_omap_reg, &up->port);
+		flush_work_sync(&up->qos_work);
+	}
+
 	return 0;
 }
 
-static int serial_omap_resume(struct platform_device *dev)
+static int serial_omap_resume(struct device *dev)
 {
-	struct uart_omap_port *up = platform_get_drvdata(dev);
+	struct uart_omap_port *up = dev_get_drvdata(dev);
 
 	if (up)
 		uart_resume_port(&serial_omap_reg, &up->port);
 	return 0;
 }
+#endif
 
-static void serial_omap_rx_timeout(unsigned long uart_no)
+static void serial_omap_rxdma_poll(unsigned long uart_no)
 {
 	struct uart_omap_port *up = ui[uart_no];
 	unsigned int curr_dma_pos, curr_transmitted_size;
@@ -1089,9 +1193,9 @@ static void serial_omap_rx_timeout(unsigned long uart_no)
 	if ((curr_dma_pos == up->uart_dma.prev_rx_dma_pos) ||
 			     (curr_dma_pos == 0)) {
 		if (jiffies_to_msecs(jiffies - up->port_activity) <
-							RX_TIMEOUT) {
+						up->uart_dma.rx_timeout) {
 			mod_timer(&up->uart_dma.rx_timer, jiffies +
-				usecs_to_jiffies(up->uart_dma.rx_timeout));
+				usecs_to_jiffies(up->uart_dma.rx_poll_rate));
 		} else {
 			serial_omap_stop_rxdma(up);
 			up->ier |= (UART_IER_RDI | UART_IER_RLSI);
@@ -1120,7 +1224,7 @@ static void serial_omap_rx_timeout(unsigned long uart_no)
 		}
 	} else  {
 		mod_timer(&up->uart_dma.rx_timer, jiffies +
-			usecs_to_jiffies(up->uart_dma.rx_timeout));
+			usecs_to_jiffies(up->uart_dma.rx_poll_rate));
 	}
 	up->port_activity = jiffies;
 }
@@ -1135,6 +1239,7 @@ static int serial_omap_start_rxdma(struct uart_omap_port *up)
 	int ret = 0;
 
 	if (up->uart_dma.rx_dma_channel == -1) {
+		pm_runtime_get_sync(&up->pdev->dev);
 		ret = omap_request_dma(up->uart_dma.uart_dma_rx,
 				"UART Rx DMA",
 				(void *)uart_rx_dma_callback, up,
@@ -1158,7 +1263,7 @@ static int serial_omap_start_rxdma(struct uart_omap_port *up)
 	/* FIXME: Cache maintenance needed here? */
 	omap_start_dma(up->uart_dma.rx_dma_channel);
 	mod_timer(&up->uart_dma.rx_timer, jiffies +
-				usecs_to_jiffies(up->uart_dma.rx_timeout));
+				usecs_to_jiffies(up->uart_dma.rx_poll_rate));
 	up->uart_dma.rx_dma_used = true;
 	return ret;
 }
@@ -1221,6 +1326,19 @@ static void uart_tx_dma_callback(int lch, u16 ch_status, void *data)
 	return;
 }
 
+static struct omap_uart_port_info *of_get_uart_port_info(struct device *dev)
+{
+	struct omap_uart_port_info *omap_up_info;
+
+	omap_up_info = devm_kzalloc(dev, sizeof(*omap_up_info), GFP_KERNEL);
+	if (!omap_up_info)
+		return NULL; /* out of memory */
+
+	of_property_read_u32(dev->of_node, "clock-frequency",
+					 &omap_up_info->uartclk);
+	return omap_up_info;
+}
+
 static int serial_omap_probe(struct platform_device *pdev)
 {
 	struct uart_omap_port	*up;
@@ -1228,6 +1346,9 @@ static int serial_omap_probe(struct platform_device *pdev)
 	struct omap_uart_port_info *omap_up_info = pdev->dev.platform_data;
 	int ret = -ENOSPC;
 
+	if (pdev->dev.of_node)
+		omap_up_info = of_get_uart_port_info(&pdev->dev);
+
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!mem) {
 		dev_err(&pdev->dev, "no mem resource?\n");
@@ -1263,7 +1384,6 @@ static int serial_omap_probe(struct platform_device *pdev)
 		ret = -ENOMEM;
 		goto do_release_region;
 	}
-	sprintf(up->name, "OMAP UART%d", pdev->id);
 	up->pdev = pdev;
 	up->port.dev = &pdev->dev;
 	up->port.type = PORT_OMAP;
@@ -1273,34 +1393,74 @@ static int serial_omap_probe(struct platform_device *pdev)
 	up->port.regshift = 2;
 	up->port.fifosize = 64;
 	up->port.ops = &serial_omap_pops;
-	up->port.line = pdev->id;
 
-	up->port.membase = omap_up_info->membase;
-	up->port.mapbase = omap_up_info->mapbase;
+	if (pdev->dev.of_node)
+		up->port.line = of_alias_get_id(pdev->dev.of_node, "serial");
+	else
+		up->port.line = pdev->id;
+
+	if (up->port.line < 0) {
+		dev_err(&pdev->dev, "failed to get alias/pdev id, errno %d\n",
+								up->port.line);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	sprintf(up->name, "OMAP UART%d", up->port.line);
+	up->port.mapbase = mem->start;
+	up->port.membase = ioremap(mem->start, resource_size(mem));
+	if (!up->port.membase) {
+		dev_err(&pdev->dev, "can't ioremap UART\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
 	up->port.flags = omap_up_info->flags;
-	up->port.irqflags = omap_up_info->irqflags;
 	up->port.uartclk = omap_up_info->uartclk;
+	if (!up->port.uartclk) {
+		up->port.uartclk = DEFAULT_CLK_SPEED;
+		dev_warn(&pdev->dev, "No clock speed specified: using default:"
+						"%d\n", DEFAULT_CLK_SPEED);
+	}
 	up->uart_dma.uart_base = mem->start;
+	up->errata = omap_up_info->errata;
 
 	if (omap_up_info->dma_enabled) {
 		up->uart_dma.uart_dma_tx = dma_tx->start;
 		up->uart_dma.uart_dma_rx = dma_rx->start;
 		up->use_dma = 1;
-		up->uart_dma.rx_buf_size = 4096;
-		up->uart_dma.rx_timeout = 2;
+		up->uart_dma.rx_buf_size = omap_up_info->dma_rx_buf_size;
+		up->uart_dma.rx_timeout = omap_up_info->dma_rx_timeout;
+		up->uart_dma.rx_poll_rate = omap_up_info->dma_rx_poll_rate;
 		spin_lock_init(&(up->uart_dma.tx_lock));
 		spin_lock_init(&(up->uart_dma.rx_lock));
 		up->uart_dma.tx_dma_channel = OMAP_UART_DMA_CH_FREE;
 		up->uart_dma.rx_dma_channel = OMAP_UART_DMA_CH_FREE;
 	}
 
-	ui[pdev->id] = up;
+	up->latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
+	up->calc_latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
+	pm_qos_add_request(&up->pm_qos_request,
+		PM_QOS_CPU_DMA_LATENCY, up->latency);
+	serial_omap_uart_wq = create_singlethread_workqueue(up->name);
+	INIT_WORK(&up->qos_work, serial_omap_uart_qos_work);
+
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_autosuspend_delay(&pdev->dev,
+			omap_up_info->autosuspend_timeout);
+
+	pm_runtime_irq_safe(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
+
+	ui[up->port.line] = up;
 	serial_omap_add_console_port(up);
 
 	ret = uart_add_one_port(&serial_omap_reg, &up->port);
 	if (ret != 0)
 		goto do_release_region;
 
+	pm_runtime_put(&pdev->dev);
 	platform_set_drvdata(pdev, up);
 	return 0;
 err:
@@ -1315,22 +1475,168 @@ static int serial_omap_remove(struct platform_device *dev)
 {
 	struct uart_omap_port *up = platform_get_drvdata(dev);
 
-	platform_set_drvdata(dev, NULL);
 	if (up) {
+		pm_runtime_disable(&up->pdev->dev);
 		uart_remove_one_port(&serial_omap_reg, &up->port);
+		pm_qos_remove_request(&up->pm_qos_request);
+
 		kfree(up);
 	}
+
+	platform_set_drvdata(dev, NULL);
 	return 0;
 }
 
+/*
+ * Work Around for Errata i202 (2430, 3430, 3630, 4430 and 4460)
+ * The access to uart register after MDR1 Access
+ * causes UART to corrupt data.
+ *
+ * Need a delay =
+ * 5 L4 clock cycles + 5 UART functional clock cycle (@48MHz = ~0.2uS)
+ * give 10 times as much
+ */
+static void serial_omap_mdr1_errataset(struct uart_omap_port *up, u8 mdr1)
+{
+	u8 timeout = 255;
+
+	serial_out(up, UART_OMAP_MDR1, mdr1);
+	udelay(2);
+	serial_out(up, UART_FCR, up->fcr | UART_FCR_CLEAR_XMIT |
+			UART_FCR_CLEAR_RCVR);
+	/*
+	 * Wait for FIFO to empty: when empty, RX_FIFO_E bit is 0 and
+	 * TX_FIFO_E bit is 1.
+	 */
+	while (UART_LSR_THRE != (serial_in(up, UART_LSR) &
+				(UART_LSR_THRE | UART_LSR_DR))) {
+		timeout--;
+		if (!timeout) {
+			/* Should *never* happen. we warn and carry on */
+			dev_crit(&up->pdev->dev, "Errata i202: timedout %x\n",
+						serial_in(up, UART_LSR));
+			break;
+		}
+		udelay(1);
+	}
+}
+
+static void serial_omap_restore_context(struct uart_omap_port *up)
+{
+	if (up->errata & UART_ERRATA_i202_MDR1_ACCESS)
+		serial_omap_mdr1_errataset(up, UART_OMAP_MDR1_DISABLE);
+	else
+		serial_out(up, UART_OMAP_MDR1, UART_OMAP_MDR1_DISABLE);
+
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B); /* Config B mode */
+	serial_out(up, UART_EFR, UART_EFR_ECB);
+	serial_out(up, UART_LCR, 0x0); /* Operational mode */
+	serial_out(up, UART_IER, 0x0);
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B); /* Config B mode */
+	serial_out(up, UART_DLL, up->dll);
+	serial_out(up, UART_DLM, up->dlh);
+	serial_out(up, UART_LCR, 0x0); /* Operational mode */
+	serial_out(up, UART_IER, up->ier);
+	serial_out(up, UART_FCR, up->fcr);
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_A);
+	serial_out(up, UART_MCR, up->mcr);
+	serial_out(up, UART_LCR, UART_LCR_CONF_MODE_B); /* Config B mode */
+	serial_out(up, UART_OMAP_SCR, up->scr);
+	serial_out(up, UART_EFR, up->efr);
+	serial_out(up, UART_LCR, up->lcr);
+	if (up->errata & UART_ERRATA_i202_MDR1_ACCESS)
+		serial_omap_mdr1_errataset(up, up->mdr1);
+	else
+		serial_out(up, UART_OMAP_MDR1, up->mdr1);
+}
+
+#ifdef CONFIG_PM_RUNTIME
+static int serial_omap_runtime_suspend(struct device *dev)
+{
+	struct uart_omap_port *up = dev_get_drvdata(dev);
+	struct omap_uart_port_info *pdata = dev->platform_data;
+
+	if (!up)
+		return -EINVAL;
+
+	if (!pdata || !pdata->enable_wakeup)
+		return 0;
+
+	if (pdata->get_context_loss_count)
+		up->context_loss_cnt = pdata->get_context_loss_count(dev);
+
+	if (device_may_wakeup(dev)) {
+		if (!up->wakeups_enabled) {
+			pdata->enable_wakeup(up->pdev, true);
+			up->wakeups_enabled = true;
+		}
+	} else {
+		if (up->wakeups_enabled) {
+			pdata->enable_wakeup(up->pdev, false);
+			up->wakeups_enabled = false;
+		}
+	}
+
+	/* Errata i291 */
+	if (up->use_dma && pdata->set_forceidle &&
+			(up->errata & UART_ERRATA_i291_DMA_FORCEIDLE))
+		pdata->set_forceidle(up->pdev);
+
+	up->latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
+	schedule_work(&up->qos_work);
+
+	return 0;
+}
+
+static int serial_omap_runtime_resume(struct device *dev)
+{
+	struct uart_omap_port *up = dev_get_drvdata(dev);
+	struct omap_uart_port_info *pdata = dev->platform_data;
+
+	if (up) {
+		if (pdata->get_context_loss_count) {
+			u32 loss_cnt = pdata->get_context_loss_count(dev);
+
+			if (up->context_loss_cnt != loss_cnt)
+				serial_omap_restore_context(up);
+		}
+
+		/* Errata i291 */
+		if (up->use_dma && pdata->set_noidle &&
+				(up->errata & UART_ERRATA_i291_DMA_FORCEIDLE))
+			pdata->set_noidle(up->pdev);
+
+		up->latency = up->calc_latency;
+		schedule_work(&up->qos_work);
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops serial_omap_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(serial_omap_suspend, serial_omap_resume)
+	SET_RUNTIME_PM_OPS(serial_omap_runtime_suspend,
+				serial_omap_runtime_resume, NULL)
+};
+
+#if defined(CONFIG_OF)
+static const struct of_device_id omap_serial_of_match[] = {
+	{ .compatible = "ti,omap2-uart" },
+	{ .compatible = "ti,omap3-uart" },
+	{ .compatible = "ti,omap4-uart" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap_serial_of_match);
+#endif
+
 static struct platform_driver serial_omap_driver = {
 	.probe          = serial_omap_probe,
 	.remove         = serial_omap_remove,
-
-	.suspend	= serial_omap_suspend,
-	.resume		= serial_omap_resume,
 	.driver		= {
 		.name	= DRIVER_NAME,
+		.pm	= &serial_omap_dev_pm_ops,
+		.of_match_table = of_match_ptr(omap_serial_of_match),
 	},
 };
 
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index d6aba8c087e4..de0f613ed6f5 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -25,6 +25,9 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/dmi.h>
+#include <linux/console.h>
+#include <linux/nmi.h>
+#include <linux/delay.h>
 
 #include <linux/dmaengine.h>
 #include <linux/pch_dma.h>
@@ -198,6 +201,10 @@ enum {
 
 #define PCI_VENDOR_ID_ROHM		0x10DB
 
+#define BOTH_EMPTY (UART_LSR_TEMT | UART_LSR_THRE)
+
+#define DEFAULT_BAUD_RATE 1843200 /* 1.8432MHz */
+
 struct pch_uart_buffer {
 	unsigned char *buf;
 	int size;
@@ -276,6 +283,9 @@ static struct pch_uart_driver_data drv_dat[] = {
 	[pch_ml7831_uart1] = {PCH_UART_2LINE, 1},
 };
 
+#ifdef CONFIG_SERIAL_PCH_UART_CONSOLE
+static struct eg20t_port *pch_uart_ports[PCH_UART_NR];
+#endif
 static unsigned int default_baud = 9600;
 static const int trigger_level_256[4] = { 1, 64, 128, 224 };
 static const int trigger_level_64[4] = { 1, 16, 32, 56 };
@@ -1385,6 +1395,143 @@ static struct uart_ops pch_uart_ops = {
 	.verify_port = pch_uart_verify_port
 };
 
+#ifdef CONFIG_SERIAL_PCH_UART_CONSOLE
+
+/*
+ *	Wait for transmitter & holding register to empty
+ */
+static void wait_for_xmitr(struct eg20t_port *up, int bits)
+{
+	unsigned int status, tmout = 10000;
+
+	/* Wait up to 10ms for the character(s) to be sent. */
+	for (;;) {
+		status = ioread8(up->membase + UART_LSR);
+
+		if ((status & bits) == bits)
+			break;
+		if (--tmout == 0)
+			break;
+		udelay(1);
+	}
+
+	/* Wait up to 1s for flow control if necessary */
+	if (up->port.flags & UPF_CONS_FLOW) {
+		unsigned int tmout;
+		for (tmout = 1000000; tmout; tmout--) {
+			unsigned int msr = ioread8(up->membase + UART_MSR);
+			if (msr & UART_MSR_CTS)
+				break;
+			udelay(1);
+			touch_nmi_watchdog();
+		}
+	}
+}
+
+static void pch_console_putchar(struct uart_port *port, int ch)
+{
+	struct eg20t_port *priv =
+		container_of(port, struct eg20t_port, port);
+
+	wait_for_xmitr(priv, UART_LSR_THRE);
+	iowrite8(ch, priv->membase + PCH_UART_THR);
+}
+
+/*
+ *	Print a string to the serial port trying not to disturb
+ *	any possible real use of the port...
+ *
+ *	The console_lock must be held when we get here.
+ */
+static void
+pch_console_write(struct console *co, const char *s, unsigned int count)
+{
+	struct eg20t_port *priv;
+
+	unsigned long flags;
+	u8 ier;
+	int locked = 1;
+
+	priv = pch_uart_ports[co->index];
+
+	touch_nmi_watchdog();
+
+	local_irq_save(flags);
+	if (priv->port.sysrq) {
+		/* serial8250_handle_port() already took the lock */
+		locked = 0;
+	} else if (oops_in_progress) {
+		locked = spin_trylock(&priv->port.lock);
+	} else
+		spin_lock(&priv->port.lock);
+
+	/*
+	 *	First save the IER then disable the interrupts
+	 */
+	ier = ioread8(priv->membase + UART_IER);
+
+	pch_uart_hal_disable_interrupt(priv, PCH_UART_HAL_ALL_INT);
+
+	uart_console_write(&priv->port, s, count, pch_console_putchar);
+
+	/*
+	 *	Finally, wait for transmitter to become empty
+	 *	and restore the IER
+	 */
+	wait_for_xmitr(priv, BOTH_EMPTY);
+	iowrite8(ier, priv->membase + UART_IER);
+
+	if (locked)
+		spin_unlock(&priv->port.lock);
+	local_irq_restore(flags);
+}
+
+static int __init pch_console_setup(struct console *co, char *options)
+{
+	struct uart_port *port;
+	int baud = 9600;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+
+	/*
+	 * Check whether an invalid uart number has been specified, and
+	 * if so, search for the first available port that does have
+	 * console support.
+	 */
+	if (co->index >= PCH_UART_NR)
+		co->index = 0;
+	port = &pch_uart_ports[co->index]->port;
+
+	if (!port || (!port->iobase && !port->membase))
+		return -ENODEV;
+
+	/* setup uartclock */
+	port->uartclk = DEFAULT_BAUD_RATE;
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+
+	return uart_set_options(port, co, baud, parity, bits, flow);
+}
+
+static struct uart_driver pch_uart_driver;
+
+static struct console pch_console = {
+	.name		= PCH_UART_DRIVER_DEVICE,
+	.write		= pch_console_write,
+	.device		= uart_console_device,
+	.setup		= pch_console_setup,
+	.flags		= CON_PRINTBUFFER | CON_ANYTIME,
+	.index		= -1,
+	.data		= &pch_uart_driver,
+};
+
+#define PCH_CONSOLE	(&pch_console)
+#else
+#define PCH_CONSOLE	NULL
+#endif
+
 static struct uart_driver pch_uart_driver = {
 	.owner = THIS_MODULE,
 	.driver_name = KBUILD_MODNAME,
@@ -1392,6 +1539,7 @@ static struct uart_driver pch_uart_driver = {
 	.major = 0,
 	.minor = 0,
 	.nr = PCH_UART_NR,
+	.cons = PCH_CONSOLE,
 };
 
 static struct eg20t_port *pch_uart_init_port(struct pci_dev *pdev,
@@ -1418,7 +1566,7 @@ static struct eg20t_port *pch_uart_init_port(struct pci_dev *pdev,
 	if (!rxbuf)
 		goto init_port_free_txbuf;
 
-	base_baud = 1843200; /* 1.8432MHz */
+	base_baud = DEFAULT_BAUD_RATE;
 
 	/* quirk for CM-iTC board */
 	board_name = dmi_get_system_info(DMI_BOARD_NAME);
@@ -1468,6 +1616,9 @@ static struct eg20t_port *pch_uart_init_port(struct pci_dev *pdev,
 	pci_set_drvdata(pdev, priv);
 	pch_uart_hal_request(pdev, fifosize, base_baud);
 
+#ifdef CONFIG_SERIAL_PCH_UART_CONSOLE
+	pch_uart_ports[board->line_no] = priv;
+#endif
 	ret = uart_add_one_port(&pch_uart_driver, &priv->port);
 	if (ret < 0)
 		goto init_port_hal_free;
@@ -1475,6 +1626,9 @@ static struct eg20t_port *pch_uart_init_port(struct pci_dev *pdev,
 	return priv;
 
 init_port_hal_free:
+#ifdef CONFIG_SERIAL_PCH_UART_CONSOLE
+	pch_uart_ports[board->line_no] = NULL;
+#endif
 	free_page((unsigned long)rxbuf);
 init_port_free_txbuf:
 	kfree(priv);
@@ -1497,6 +1651,10 @@ static void pch_uart_pci_remove(struct pci_dev *pdev)
 	priv = (struct eg20t_port *)pci_get_drvdata(pdev);
 
 	pci_disable_msi(pdev);
+
+#ifdef CONFIG_SERIAL_PCH_UART_CONSOLE
+	pch_uart_ports[priv->port.line] = NULL;
+#endif
 	pch_uart_exit_port(priv);
 	pci_disable_device(pdev);
 	kfree(priv);
diff --git a/drivers/tty/serial/s3c2410.c b/drivers/tty/serial/s3c2410.c
deleted file mode 100644
index b1d7e7c1849d..000000000000
--- a/drivers/tty/serial/s3c2410.c
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
- * Driver for Samsung S3C2410 SoC onboard UARTs.
- *
- * Ben Dooks, Copyright (c) 2003-2008 Simtec Electronics
- *	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.
-*/
-
-#include <linux/module.h>
-#include <linux/ioport.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/init.h>
-#include <linux/serial_core.h>
-#include <linux/serial.h>
-
-#include <asm/irq.h>
-#include <mach/hardware.h>
-
-#include <plat/regs-serial.h>
-#include <mach/regs-gpio.h>
-
-#include "samsung.h"
-
-static int s3c2410_serial_setsource(struct uart_port *port,
-				    struct s3c24xx_uart_clksrc *clk)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	if (strcmp(clk->name, "uclk") == 0)
-		ucon |= S3C2410_UCON_UCLK;
-	else
-		ucon &= ~S3C2410_UCON_UCLK;
-
-	wr_regl(port, S3C2410_UCON, ucon);
-	return 0;
-}
-
-static int s3c2410_serial_getsource(struct uart_port *port,
-				    struct s3c24xx_uart_clksrc *clk)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	clk->divisor = 1;
-	clk->name = (ucon & S3C2410_UCON_UCLK) ? "uclk" : "pclk";
-
-	return 0;
-}
-
-static int s3c2410_serial_resetport(struct uart_port *port,
-				    struct s3c2410_uartcfg *cfg)
-{
-	dbg("s3c2410_serial_resetport: port=%p (%08lx), cfg=%p\n",
-	    port, port->mapbase, cfg);
-
-	wr_regl(port, S3C2410_UCON,  cfg->ucon);
-	wr_regl(port, S3C2410_ULCON, cfg->ulcon);
-
-	/* reset both fifos */
-
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon | S3C2410_UFCON_RESETBOTH);
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon);
-
-	return 0;
-}
-
-static struct s3c24xx_uart_info s3c2410_uart_inf = {
-	.name		= "Samsung S3C2410 UART",
-	.type		= PORT_S3C2410,
-	.fifosize	= 16,
-	.rx_fifomask	= S3C2410_UFSTAT_RXMASK,
-	.rx_fifoshift	= S3C2410_UFSTAT_RXSHIFT,
-	.rx_fifofull	= S3C2410_UFSTAT_RXFULL,
-	.tx_fifofull	= S3C2410_UFSTAT_TXFULL,
-	.tx_fifomask	= S3C2410_UFSTAT_TXMASK,
-	.tx_fifoshift	= S3C2410_UFSTAT_TXSHIFT,
-	.get_clksrc	= s3c2410_serial_getsource,
-	.set_clksrc	= s3c2410_serial_setsource,
-	.reset_port	= s3c2410_serial_resetport,
-};
-
-static int s3c2410_serial_probe(struct platform_device *dev)
-{
-	return s3c24xx_serial_probe(dev, &s3c2410_uart_inf);
-}
-
-static struct platform_driver s3c2410_serial_driver = {
-	.probe		= s3c2410_serial_probe,
-	.remove		= __devexit_p(s3c24xx_serial_remove),
-	.driver		= {
-		.name	= "s3c2410-uart",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init s3c2410_serial_init(void)
-{
-	return s3c24xx_serial_init(&s3c2410_serial_driver, &s3c2410_uart_inf);
-}
-
-static void __exit s3c2410_serial_exit(void)
-{
-	platform_driver_unregister(&s3c2410_serial_driver);
-}
-
-module_init(s3c2410_serial_init);
-module_exit(s3c2410_serial_exit);
-
-MODULE_LICENSE("GPL v2");
-MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
-MODULE_DESCRIPTION("Samsung S3C2410 SoC Serial port driver");
-MODULE_ALIAS("platform:s3c2410-uart");
diff --git a/drivers/tty/serial/s3c2412.c b/drivers/tty/serial/s3c2412.c
deleted file mode 100644
index 2234bf9ced45..000000000000
--- a/drivers/tty/serial/s3c2412.c
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- * Driver for Samsung S3C2412 and S3C2413 SoC onboard UARTs.
- *
- * Ben Dooks, Copyright (c) 2003-2008 Simtec Electronics
- *	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.
-*/
-
-#include <linux/module.h>
-#include <linux/ioport.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/init.h>
-#include <linux/serial_core.h>
-#include <linux/serial.h>
-
-#include <asm/irq.h>
-#include <mach/hardware.h>
-
-#include <plat/regs-serial.h>
-#include <mach/regs-gpio.h>
-
-#include "samsung.h"
-
-static int s3c2412_serial_setsource(struct uart_port *port,
-				     struct s3c24xx_uart_clksrc *clk)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	ucon &= ~S3C2412_UCON_CLKMASK;
-
-	if (strcmp(clk->name, "uclk") == 0)
-		ucon |= S3C2440_UCON_UCLK;
-	else if (strcmp(clk->name, "pclk") == 0)
-		ucon |= S3C2440_UCON_PCLK;
-	else if (strcmp(clk->name, "usysclk") == 0)
-		ucon |= S3C2412_UCON_USYSCLK;
-	else {
-		printk(KERN_ERR "unknown clock source %s\n", clk->name);
-		return -EINVAL;
-	}
-
-	wr_regl(port, S3C2410_UCON, ucon);
-	return 0;
-}
-
-
-static int s3c2412_serial_getsource(struct uart_port *port,
-				    struct s3c24xx_uart_clksrc *clk)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	switch (ucon & S3C2412_UCON_CLKMASK) {
-	case S3C2412_UCON_UCLK:
-		clk->divisor = 1;
-		clk->name = "uclk";
-		break;
-
-	case S3C2412_UCON_PCLK:
-	case S3C2412_UCON_PCLK2:
-		clk->divisor = 1;
-		clk->name = "pclk";
-		break;
-
-	case S3C2412_UCON_USYSCLK:
-		clk->divisor = 1;
-		clk->name = "usysclk";
-		break;
-	}
-
-	return 0;
-}
-
-static int s3c2412_serial_resetport(struct uart_port *port,
-				    struct s3c2410_uartcfg *cfg)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	dbg("%s: port=%p (%08lx), cfg=%p\n",
-	    __func__, port, port->mapbase, cfg);
-
-	/* ensure we don't change the clock settings... */
-
-	ucon &= S3C2412_UCON_CLKMASK;
-
-	wr_regl(port, S3C2410_UCON,  ucon | cfg->ucon);
-	wr_regl(port, S3C2410_ULCON, cfg->ulcon);
-
-	/* reset both fifos */
-
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon | S3C2410_UFCON_RESETBOTH);
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon);
-
-	return 0;
-}
-
-static struct s3c24xx_uart_info s3c2412_uart_inf = {
-	.name		= "Samsung S3C2412 UART",
-	.type		= PORT_S3C2412,
-	.fifosize	= 64,
-	.has_divslot	= 1,
-	.rx_fifomask	= S3C2440_UFSTAT_RXMASK,
-	.rx_fifoshift	= S3C2440_UFSTAT_RXSHIFT,
-	.rx_fifofull	= S3C2440_UFSTAT_RXFULL,
-	.tx_fifofull	= S3C2440_UFSTAT_TXFULL,
-	.tx_fifomask	= S3C2440_UFSTAT_TXMASK,
-	.tx_fifoshift	= S3C2440_UFSTAT_TXSHIFT,
-	.get_clksrc	= s3c2412_serial_getsource,
-	.set_clksrc	= s3c2412_serial_setsource,
-	.reset_port	= s3c2412_serial_resetport,
-};
-
-/* device management */
-
-static int s3c2412_serial_probe(struct platform_device *dev)
-{
-	dbg("s3c2440_serial_probe: dev=%p\n", dev);
-	return s3c24xx_serial_probe(dev, &s3c2412_uart_inf);
-}
-
-static struct platform_driver s3c2412_serial_driver = {
-	.probe		= s3c2412_serial_probe,
-	.remove		= __devexit_p(s3c24xx_serial_remove),
-	.driver		= {
-		.name	= "s3c2412-uart",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static inline int s3c2412_serial_init(void)
-{
-	return s3c24xx_serial_init(&s3c2412_serial_driver, &s3c2412_uart_inf);
-}
-
-static inline void s3c2412_serial_exit(void)
-{
-	platform_driver_unregister(&s3c2412_serial_driver);
-}
-
-module_init(s3c2412_serial_init);
-module_exit(s3c2412_serial_exit);
-
-MODULE_DESCRIPTION("Samsung S3C2412,S3C2413 SoC Serial port driver");
-MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:s3c2412-uart");
diff --git a/drivers/tty/serial/s3c2440.c b/drivers/tty/serial/s3c2440.c
deleted file mode 100644
index 1d0c324b813f..000000000000
--- a/drivers/tty/serial/s3c2440.c
+++ /dev/null
@@ -1,178 +0,0 @@
-/*
- * Driver for Samsung S3C2440 and S3C2442 SoC onboard UARTs.
- *
- * Ben Dooks, Copyright (c) 2003-2008 Simtec Electronics
- *	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.
-*/
-
-#include <linux/module.h>
-#include <linux/ioport.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/init.h>
-#include <linux/serial_core.h>
-#include <linux/serial.h>
-
-#include <asm/irq.h>
-#include <mach/hardware.h>
-
-#include <plat/regs-serial.h>
-#include <mach/regs-gpio.h>
-
-#include "samsung.h"
-
-
-static int s3c2440_serial_setsource(struct uart_port *port,
-				     struct s3c24xx_uart_clksrc *clk)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	/* todo - proper fclk<>nonfclk switch. */
-
-	ucon &= ~S3C2440_UCON_CLKMASK;
-
-	if (strcmp(clk->name, "uclk") == 0)
-		ucon |= S3C2440_UCON_UCLK;
-	else if (strcmp(clk->name, "pclk") == 0)
-		ucon |= S3C2440_UCON_PCLK;
-	else if (strcmp(clk->name, "fclk") == 0)
-		ucon |= S3C2440_UCON_FCLK;
-	else {
-		printk(KERN_ERR "unknown clock source %s\n", clk->name);
-		return -EINVAL;
-	}
-
-	wr_regl(port, S3C2410_UCON, ucon);
-	return 0;
-}
-
-
-static int s3c2440_serial_getsource(struct uart_port *port,
-				    struct s3c24xx_uart_clksrc *clk)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-	unsigned long ucon0, ucon1, ucon2;
-
-	switch (ucon & S3C2440_UCON_CLKMASK) {
-	case S3C2440_UCON_UCLK:
-		clk->divisor = 1;
-		clk->name = "uclk";
-		break;
-
-	case S3C2440_UCON_PCLK:
-	case S3C2440_UCON_PCLK2:
-		clk->divisor = 1;
-		clk->name = "pclk";
-		break;
-
-	case S3C2440_UCON_FCLK:
-		/* the fun of calculating the uart divisors on
-		 * the s3c2440 */
-
-		ucon0 = __raw_readl(S3C24XX_VA_UART0 + S3C2410_UCON);
-		ucon1 = __raw_readl(S3C24XX_VA_UART1 + S3C2410_UCON);
-		ucon2 = __raw_readl(S3C24XX_VA_UART2 + S3C2410_UCON);
-
-		printk("ucons: %08lx, %08lx, %08lx\n", ucon0, ucon1, ucon2);
-
-		ucon0 &= S3C2440_UCON0_DIVMASK;
-		ucon1 &= S3C2440_UCON1_DIVMASK;
-		ucon2 &= S3C2440_UCON2_DIVMASK;
-
-		if (ucon0 != 0) {
-			clk->divisor = ucon0 >> S3C2440_UCON_DIVSHIFT;
-			clk->divisor += 6;
-		} else if (ucon1 != 0) {
-			clk->divisor = ucon1 >> S3C2440_UCON_DIVSHIFT;
-			clk->divisor += 21;
-		} else if (ucon2 != 0) {
-			clk->divisor = ucon2 >> S3C2440_UCON_DIVSHIFT;
-			clk->divisor += 36;
-		} else {
-			/* manual calims 44, seems to be 9 */
-			clk->divisor = 9;
-		}
-
-		clk->name = "fclk";
-		break;
-	}
-
-	return 0;
-}
-
-static int s3c2440_serial_resetport(struct uart_port *port,
-				    struct s3c2410_uartcfg *cfg)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	dbg("s3c2440_serial_resetport: port=%p (%08lx), cfg=%p\n",
-	    port, port->mapbase, cfg);
-
-	/* ensure we don't change the clock settings... */
-
-	ucon &= (S3C2440_UCON0_DIVMASK | (3<<10));
-
-	wr_regl(port, S3C2410_UCON,  ucon | cfg->ucon);
-	wr_regl(port, S3C2410_ULCON, cfg->ulcon);
-
-	/* reset both fifos */
-
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon | S3C2410_UFCON_RESETBOTH);
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon);
-
-	return 0;
-}
-
-static struct s3c24xx_uart_info s3c2440_uart_inf = {
-	.name		= "Samsung S3C2440 UART",
-	.type		= PORT_S3C2440,
-	.fifosize	= 64,
-	.rx_fifomask	= S3C2440_UFSTAT_RXMASK,
-	.rx_fifoshift	= S3C2440_UFSTAT_RXSHIFT,
-	.rx_fifofull	= S3C2440_UFSTAT_RXFULL,
-	.tx_fifofull	= S3C2440_UFSTAT_TXFULL,
-	.tx_fifomask	= S3C2440_UFSTAT_TXMASK,
-	.tx_fifoshift	= S3C2440_UFSTAT_TXSHIFT,
-	.get_clksrc	= s3c2440_serial_getsource,
-	.set_clksrc	= s3c2440_serial_setsource,
-	.reset_port	= s3c2440_serial_resetport,
-};
-
-/* device management */
-
-static int s3c2440_serial_probe(struct platform_device *dev)
-{
-	dbg("s3c2440_serial_probe: dev=%p\n", dev);
-	return s3c24xx_serial_probe(dev, &s3c2440_uart_inf);
-}
-
-static struct platform_driver s3c2440_serial_driver = {
-	.probe		= s3c2440_serial_probe,
-	.remove		= __devexit_p(s3c24xx_serial_remove),
-	.driver		= {
-		.name	= "s3c2440-uart",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init s3c2440_serial_init(void)
-{
-	return s3c24xx_serial_init(&s3c2440_serial_driver, &s3c2440_uart_inf);
-}
-
-static void __exit s3c2440_serial_exit(void)
-{
-	platform_driver_unregister(&s3c2440_serial_driver);
-}
-
-module_init(s3c2440_serial_init);
-module_exit(s3c2440_serial_exit);
-
-MODULE_DESCRIPTION("Samsung S3C2440,S3C2442 SoC Serial port driver");
-MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:s3c2440-uart");
diff --git a/drivers/tty/serial/s3c6400.c b/drivers/tty/serial/s3c6400.c
deleted file mode 100644
index e2f6913d84d5..000000000000
--- a/drivers/tty/serial/s3c6400.c
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- * Driver for Samsung S3C6400 and S3C6410 SoC onboard UARTs.
- *
- * Copyright 2008 Openmoko,  Inc.
- * Copyright 2008 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.
-*/
-
-#include <linux/module.h>
-#include <linux/ioport.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/init.h>
-#include <linux/serial_core.h>
-#include <linux/serial.h>
-
-#include <asm/irq.h>
-#include <mach/hardware.h>
-
-#include <plat/regs-serial.h>
-
-#include "samsung.h"
-
-static int s3c6400_serial_setsource(struct uart_port *port,
-				    struct s3c24xx_uart_clksrc *clk)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	if (strcmp(clk->name, "uclk0") == 0) {
-		ucon &= ~S3C6400_UCON_CLKMASK;
-		ucon |= S3C6400_UCON_UCLK0;
-	} else if (strcmp(clk->name, "uclk1") == 0)
-		ucon |= S3C6400_UCON_UCLK1;
-	else if (strcmp(clk->name, "pclk") == 0) {
-		/* See notes about transitioning from UCLK to PCLK */
-		ucon &= ~S3C6400_UCON_UCLK0;
-	} else {
-		printk(KERN_ERR "unknown clock source %s\n", clk->name);
-		return -EINVAL;
-	}
-
-	wr_regl(port, S3C2410_UCON, ucon);
-	return 0;
-}
-
-
-static int s3c6400_serial_getsource(struct uart_port *port,
-				    struct s3c24xx_uart_clksrc *clk)
-{
-	u32 ucon = rd_regl(port, S3C2410_UCON);
-
-	clk->divisor = 1;
-
-	switch (ucon & S3C6400_UCON_CLKMASK) {
-	case S3C6400_UCON_UCLK0:
-		clk->name = "uclk0";
-		break;
-
-	case S3C6400_UCON_UCLK1:
-		clk->name = "uclk1";
-		break;
-
-	case S3C6400_UCON_PCLK:
-	case S3C6400_UCON_PCLK2:
-		clk->name = "pclk";
-		break;
-	}
-
-	return 0;
-}
-
-static int s3c6400_serial_resetport(struct uart_port *port,
-				    struct s3c2410_uartcfg *cfg)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	dbg("s3c6400_serial_resetport: port=%p (%08lx), cfg=%p\n",
-	    port, port->mapbase, cfg);
-
-	/* ensure we don't change the clock settings... */
-
-	ucon &= S3C6400_UCON_CLKMASK;
-
-	wr_regl(port, S3C2410_UCON,  ucon | cfg->ucon);
-	wr_regl(port, S3C2410_ULCON, cfg->ulcon);
-
-	/* reset both fifos */
-
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon | S3C2410_UFCON_RESETBOTH);
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon);
-
-	return 0;
-}
-
-static struct s3c24xx_uart_info s3c6400_uart_inf = {
-	.name		= "Samsung S3C6400 UART",
-	.type		= PORT_S3C6400,
-	.fifosize	= 64,
-	.has_divslot	= 1,
-	.rx_fifomask	= S3C2440_UFSTAT_RXMASK,
-	.rx_fifoshift	= S3C2440_UFSTAT_RXSHIFT,
-	.rx_fifofull	= S3C2440_UFSTAT_RXFULL,
-	.tx_fifofull	= S3C2440_UFSTAT_TXFULL,
-	.tx_fifomask	= S3C2440_UFSTAT_TXMASK,
-	.tx_fifoshift	= S3C2440_UFSTAT_TXSHIFT,
-	.get_clksrc	= s3c6400_serial_getsource,
-	.set_clksrc	= s3c6400_serial_setsource,
-	.reset_port	= s3c6400_serial_resetport,
-};
-
-/* device management */
-
-static int s3c6400_serial_probe(struct platform_device *dev)
-{
-	dbg("s3c6400_serial_probe: dev=%p\n", dev);
-	return s3c24xx_serial_probe(dev, &s3c6400_uart_inf);
-}
-
-static struct platform_driver s3c6400_serial_driver = {
-	.probe		= s3c6400_serial_probe,
-	.remove		= __devexit_p(s3c24xx_serial_remove),
-	.driver		= {
-		.name	= "s3c6400-uart",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init s3c6400_serial_init(void)
-{
-	return s3c24xx_serial_init(&s3c6400_serial_driver, &s3c6400_uart_inf);
-}
-
-static void __exit s3c6400_serial_exit(void)
-{
-	platform_driver_unregister(&s3c6400_serial_driver);
-}
-
-module_init(s3c6400_serial_init);
-module_exit(s3c6400_serial_exit);
-
-MODULE_DESCRIPTION("Samsung S3C6400,S3C6410 SoC Serial port driver");
-MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:s3c6400-uart");
diff --git a/drivers/tty/serial/s5pv210.c b/drivers/tty/serial/s5pv210.c
deleted file mode 100644
index 8b0b888a1b76..000000000000
--- a/drivers/tty/serial/s5pv210.c
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * Copyright (c) 2010 Samsung Electronics Co., Ltd.
- *		http://www.samsung.com/
- *
- * Based on drivers/serial/s3c6400.c
- *
- * Driver for Samsung S5PV210 SoC UARTs.
- *
- * 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/ioport.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/init.h>
-#include <linux/serial_core.h>
-#include <linux/serial.h>
-#include <linux/delay.h>
-
-#include <asm/irq.h>
-#include <mach/hardware.h>
-#include <plat/regs-serial.h>
-#include "samsung.h"
-
-static int s5pv210_serial_setsource(struct uart_port *port,
-					struct s3c24xx_uart_clksrc *clk)
-{
-	struct s3c2410_uartcfg *cfg = port->dev->platform_data;
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	if (cfg->flags & NO_NEED_CHECK_CLKSRC)
-		return 0;
-
-	if (strcmp(clk->name, "pclk") == 0)
-		ucon &= ~S5PV210_UCON_CLKMASK;
-	else if (strcmp(clk->name, "uclk1") == 0)
-		ucon |= S5PV210_UCON_CLKMASK;
-	else {
-		printk(KERN_ERR "unknown clock source %s\n", clk->name);
-		return -EINVAL;
-	}
-
-	wr_regl(port, S3C2410_UCON, ucon);
-	return 0;
-}
-
-
-static int s5pv210_serial_getsource(struct uart_port *port,
-					struct s3c24xx_uart_clksrc *clk)
-{
-	struct s3c2410_uartcfg *cfg = port->dev->platform_data;
-	u32 ucon = rd_regl(port, S3C2410_UCON);
-
-	clk->divisor = 1;
-
-	if (cfg->flags & NO_NEED_CHECK_CLKSRC)
-		return 0;
-
-	switch (ucon & S5PV210_UCON_CLKMASK) {
-	case S5PV210_UCON_PCLK:
-		clk->name = "pclk";
-		break;
-	case S5PV210_UCON_UCLK:
-		clk->name = "uclk1";
-		break;
-	}
-
-	return 0;
-}
-
-static int s5pv210_serial_resetport(struct uart_port *port,
-					struct s3c2410_uartcfg *cfg)
-{
-	unsigned long ucon = rd_regl(port, S3C2410_UCON);
-
-	ucon &= S5PV210_UCON_CLKMASK;
-	wr_regl(port, S3C2410_UCON,  ucon | cfg->ucon);
-	wr_regl(port, S3C2410_ULCON, cfg->ulcon);
-
-	/* reset both fifos */
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon | S3C2410_UFCON_RESETBOTH);
-	wr_regl(port, S3C2410_UFCON, cfg->ufcon);
-
-	/* It is need to delay When reset FIFO register */
-	udelay(1);
-
-	return 0;
-}
-
-#define S5PV210_UART_DEFAULT_INFO(fifo_size)			\
-		.name		= "Samsung S5PV210 UART0",	\
-		.type		= PORT_S3C6400,			\
-		.fifosize	= fifo_size,			\
-		.has_divslot	= 1,				\
-		.rx_fifomask	= S5PV210_UFSTAT_RXMASK,	\
-		.rx_fifoshift	= S5PV210_UFSTAT_RXSHIFT,	\
-		.rx_fifofull	= S5PV210_UFSTAT_RXFULL,	\
-		.tx_fifofull	= S5PV210_UFSTAT_TXFULL,	\
-		.tx_fifomask	= S5PV210_UFSTAT_TXMASK,	\
-		.tx_fifoshift	= S5PV210_UFSTAT_TXSHIFT,	\
-		.get_clksrc	= s5pv210_serial_getsource,	\
-		.set_clksrc	= s5pv210_serial_setsource,	\
-		.reset_port	= s5pv210_serial_resetport
-
-static struct s3c24xx_uart_info s5p_port_fifo256 = {
-	S5PV210_UART_DEFAULT_INFO(256),
-};
-
-static struct s3c24xx_uart_info s5p_port_fifo64 = {
-	S5PV210_UART_DEFAULT_INFO(64),
-};
-
-static struct s3c24xx_uart_info s5p_port_fifo16 = {
-	S5PV210_UART_DEFAULT_INFO(16),
-};
-
-static struct s3c24xx_uart_info *s5p_uart_inf[] = {
-	[0] = &s5p_port_fifo256,
-	[1] = &s5p_port_fifo64,
-	[2] = &s5p_port_fifo16,
-	[3] = &s5p_port_fifo16,
-};
-
-/* device management */
-static int s5p_serial_probe(struct platform_device *pdev)
-{
-	return s3c24xx_serial_probe(pdev, s5p_uart_inf[pdev->id]);
-}
-
-static struct platform_driver s5p_serial_driver = {
-	.probe		= s5p_serial_probe,
-	.remove		= __devexit_p(s3c24xx_serial_remove),
-	.driver		= {
-		.name	= "s5pv210-uart",
-		.owner	= THIS_MODULE,
-	},
-};
-
-static int __init s5p_serial_init(void)
-{
-	return s3c24xx_serial_init(&s5p_serial_driver, *s5p_uart_inf);
-}
-
-static void __exit s5p_serial_exit(void)
-{
-	platform_driver_unregister(&s5p_serial_driver);
-}
-
-module_init(s5p_serial_init);
-module_exit(s5p_serial_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:s5pv210-uart");
-MODULE_DESCRIPTION("Samsung S5PV210 UART Driver support");
-MODULE_AUTHOR("Thomas Abraham <thomas.ab@samsung.com>");
diff --git a/drivers/tty/serial/samsung.c b/drivers/tty/serial/samsung.c
index b31f1c3a2c4c..f96f37b5fec6 100644
--- a/drivers/tty/serial/samsung.c
+++ b/drivers/tty/serial/samsung.c
@@ -42,6 +42,7 @@
 #include <linux/delay.h>
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
+#include <linux/of.h>
 
 #include <asm/irq.h>
 
@@ -49,6 +50,7 @@
 #include <mach/map.h>
 
 #include <plat/regs-serial.h>
+#include <plat/clock.h>
 
 #include "samsung.h"
 
@@ -190,10 +192,13 @@ static inline struct s3c24xx_uart_info *s3c24xx_port_to_info(struct uart_port *p
 
 static inline struct s3c2410_uartcfg *s3c24xx_port_to_cfg(struct uart_port *port)
 {
+	struct s3c24xx_uart_port *ourport;
+
 	if (port->dev == NULL)
 		return NULL;
 
-	return (struct s3c2410_uartcfg *)port->dev->platform_data;
+	ourport = container_of(port, struct s3c24xx_uart_port, port);
+	return ourport->cfg;
 }
 
 static int s3c24xx_serial_rx_fifocnt(struct s3c24xx_uart_port *ourport,
@@ -202,7 +207,7 @@ static int s3c24xx_serial_rx_fifocnt(struct s3c24xx_uart_port *ourport,
 	struct s3c24xx_uart_info *info = ourport->info;
 
 	if (ufstat & info->rx_fifofull)
-		return info->fifosize;
+		return ourport->port.fifosize;
 
 	return (ufstat & info->rx_fifomask) >> info->rx_fifoshift;
 }
@@ -555,154 +560,98 @@ static void s3c24xx_serial_pm(struct uart_port *port, unsigned int level,
  *
 */
 
+#define MAX_CLK_NAME_LENGTH 15
 
-#define MAX_CLKS (8)
-
-static struct s3c24xx_uart_clksrc tmp_clksrc = {
-	.name		= "pclk",
-	.min_baud	= 0,
-	.max_baud	= 0,
-	.divisor	= 1,
-};
-
-static inline int
-s3c24xx_serial_getsource(struct uart_port *port, struct s3c24xx_uart_clksrc *c)
+static inline int s3c24xx_serial_getsource(struct uart_port *port)
 {
 	struct s3c24xx_uart_info *info = s3c24xx_port_to_info(port);
+	unsigned int ucon;
 
-	return (info->get_clksrc)(port, c);
-}
-
-static inline int
-s3c24xx_serial_setsource(struct uart_port *port, struct s3c24xx_uart_clksrc *c)
-{
-	struct s3c24xx_uart_info *info = s3c24xx_port_to_info(port);
+	if (info->num_clks == 1)
+		return 0;
 
-	return (info->set_clksrc)(port, c);
+	ucon = rd_regl(port, S3C2410_UCON);
+	ucon &= info->clksel_mask;
+	return ucon >> info->clksel_shift;
 }
 
-struct baud_calc {
-	struct s3c24xx_uart_clksrc	*clksrc;
-	unsigned int			 calc;
-	unsigned int			 divslot;
-	unsigned int			 quot;
-	struct clk			*src;
-};
-
-static int s3c24xx_serial_calcbaud(struct baud_calc *calc,
-				   struct uart_port *port,
-				   struct s3c24xx_uart_clksrc *clksrc,
-				   unsigned int baud)
+static void s3c24xx_serial_setsource(struct uart_port *port,
+			unsigned int clk_sel)
 {
-	struct s3c24xx_uart_port *ourport = to_ourport(port);
-	unsigned long rate;
-
-	calc->src = clk_get(port->dev, clksrc->name);
-	if (calc->src == NULL || IS_ERR(calc->src))
-		return 0;
-
-	rate = clk_get_rate(calc->src);
-	rate /= clksrc->divisor;
+	struct s3c24xx_uart_info *info = s3c24xx_port_to_info(port);
+	unsigned int ucon;
 
-	calc->clksrc = clksrc;
+	if (info->num_clks == 1)
+		return;
 
-	if (ourport->info->has_divslot) {
-		unsigned long div = rate / baud;
-
-		/* The UDIVSLOT register on the newer UARTs allows us to
-		 * get a divisor adjustment of 1/16th on the baud clock.
-		 *
-		 * We don't keep the UDIVSLOT value (the 16ths we calculated
-		 * by not multiplying the baud by 16) as it is easy enough
-		 * to recalculate.
-		 */
-
-		calc->quot = div / 16;
-		calc->calc = rate / div;
-	} else {
-		calc->quot = (rate + (8 * baud)) / (16 * baud);
-		calc->calc = (rate / (calc->quot * 16));
-	}
+	ucon = rd_regl(port, S3C2410_UCON);
+	if ((ucon & info->clksel_mask) >> info->clksel_shift == clk_sel)
+		return;
 
-	calc->quot--;
-	return 1;
+	ucon &= ~info->clksel_mask;
+	ucon |= clk_sel << info->clksel_shift;
+	wr_regl(port, S3C2410_UCON, ucon);
 }
 
-static unsigned int s3c24xx_serial_getclk(struct uart_port *port,
-					  struct s3c24xx_uart_clksrc **clksrc,
-					  struct clk **clk,
-					  unsigned int baud)
+static unsigned int s3c24xx_serial_getclk(struct s3c24xx_uart_port *ourport,
+			unsigned int req_baud, struct clk **best_clk,
+			unsigned int *clk_num)
 {
-	struct s3c2410_uartcfg *cfg = s3c24xx_port_to_cfg(port);
-	struct s3c24xx_uart_clksrc *clkp;
-	struct baud_calc res[MAX_CLKS];
-	struct baud_calc *resptr, *best, *sptr;
-	int i;
-
-	clkp = cfg->clocks;
-	best = NULL;
-
-	if (cfg->clocks_size < 2) {
-		if (cfg->clocks_size == 0)
-			clkp = &tmp_clksrc;
-
-		/* check to see if we're sourcing fclk, and if so we're
-		 * going to have to update the clock source
-		 */
-
-		if (strcmp(clkp->name, "fclk") == 0) {
-			struct s3c24xx_uart_clksrc src;
-
-			s3c24xx_serial_getsource(port, &src);
-
-			/* check that the port already using fclk, and if
-			 * not, then re-select fclk
+	struct s3c24xx_uart_info *info = ourport->info;
+	struct clk *clk;
+	unsigned long rate;
+	unsigned int cnt, baud, quot, clk_sel, best_quot = 0;
+	char clkname[MAX_CLK_NAME_LENGTH];
+	int calc_deviation, deviation = (1 << 30) - 1;
+
+	*best_clk = NULL;
+	clk_sel = (ourport->cfg->clk_sel) ? ourport->cfg->clk_sel :
+			ourport->info->def_clk_sel;
+	for (cnt = 0; cnt < info->num_clks; cnt++) {
+		if (!(clk_sel & (1 << cnt)))
+			continue;
+
+		sprintf(clkname, "clk_uart_baud%d", cnt);
+		clk = clk_get(ourport->port.dev, clkname);
+		if (IS_ERR_OR_NULL(clk))
+			continue;
+
+		rate = clk_get_rate(clk);
+		if (!rate)
+			continue;
+
+		if (ourport->info->has_divslot) {
+			unsigned long div = rate / req_baud;
+
+			/* The UDIVSLOT register on the newer UARTs allows us to
+			 * get a divisor adjustment of 1/16th on the baud clock.
+			 *
+			 * We don't keep the UDIVSLOT value (the 16ths we
+			 * calculated by not multiplying the baud by 16) as it
+			 * is easy enough to recalculate.
 			 */
 
-			if (strcmp(src.name, clkp->name) == 0) {
-				s3c24xx_serial_setsource(port, clkp);
-				s3c24xx_serial_getsource(port, &src);
-			}
-
-			clkp->divisor = src.divisor;
-		}
-
-		s3c24xx_serial_calcbaud(res, port, clkp, baud);
-		best = res;
-		resptr = best + 1;
-	} else {
-		resptr = res;
-
-		for (i = 0; i < cfg->clocks_size; i++, clkp++) {
-			if (s3c24xx_serial_calcbaud(resptr, port, clkp, baud))
-				resptr++;
+			quot = div / 16;
+			baud = rate / div;
+		} else {
+			quot = (rate + (8 * req_baud)) / (16 * req_baud);
+			baud = rate / (quot * 16);
 		}
-	}
-
-	/* ok, we now need to select the best clock we found */
-
-	if (!best) {
-		unsigned int deviation = (1<<30)|((1<<30)-1);
-		int calc_deviation;
+		quot--;
 
-		for (sptr = res; sptr < resptr; sptr++) {
-			calc_deviation = baud - sptr->calc;
-			if (calc_deviation < 0)
-				calc_deviation = -calc_deviation;
+		calc_deviation = req_baud - baud;
+		if (calc_deviation < 0)
+			calc_deviation = -calc_deviation;
 
-			if (calc_deviation < deviation) {
-				best = sptr;
-				deviation = calc_deviation;
-			}
+		if (calc_deviation < deviation) {
+			*best_clk = clk;
+			best_quot = quot;
+			*clk_num = cnt;
+			deviation = calc_deviation;
 		}
 	}
 
-	/* store results to pass back */
-
-	*clksrc = best->clksrc;
-	*clk    = best->src;
-
-	return best->quot;
+	return best_quot;
 }
 
 /* udivslot_table[]
@@ -735,10 +684,9 @@ static void s3c24xx_serial_set_termios(struct uart_port *port,
 {
 	struct s3c2410_uartcfg *cfg = s3c24xx_port_to_cfg(port);
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
-	struct s3c24xx_uart_clksrc *clksrc = NULL;
 	struct clk *clk = NULL;
 	unsigned long flags;
-	unsigned int baud, quot;
+	unsigned int baud, quot, clk_sel = 0;
 	unsigned int ulcon;
 	unsigned int umcon;
 	unsigned int udivslot = 0;
@@ -754,17 +702,16 @@ static void s3c24xx_serial_set_termios(struct uart_port *port,
 	 */
 
 	baud = uart_get_baud_rate(port, termios, old, 0, 115200*8);
-
+	quot = s3c24xx_serial_getclk(ourport, baud, &clk, &clk_sel);
 	if (baud == 38400 && (port->flags & UPF_SPD_MASK) == UPF_SPD_CUST)
 		quot = port->custom_divisor;
-	else
-		quot = s3c24xx_serial_getclk(port, &clksrc, &clk, baud);
+	if (!clk)
+		return;
 
 	/* check to see if we need  to change clock source */
 
-	if (ourport->clksrc != clksrc || ourport->baudclk != clk) {
-		dbg("selecting clock %p\n", clk);
-		s3c24xx_serial_setsource(port, clksrc);
+	if (ourport->baudclk != clk) {
+		s3c24xx_serial_setsource(port, clk_sel);
 
 		if (ourport->baudclk != NULL && !IS_ERR(ourport->baudclk)) {
 			clk_disable(ourport->baudclk);
@@ -773,7 +720,6 @@ static void s3c24xx_serial_set_termios(struct uart_port *port,
 
 		clk_enable(clk);
 
-		ourport->clksrc = clksrc;
 		ourport->baudclk = clk;
 		ourport->baudclk_rate = clk ? clk_get_rate(clk) : 0;
 	}
@@ -1020,16 +966,29 @@ static struct s3c24xx_uart_port s3c24xx_serial_ports[CONFIG_SERIAL_SAMSUNG_UARTS
 
 /* s3c24xx_serial_resetport
  *
- * wrapper to call the specific reset for this port (reset the fifos
- * and the settings)
+ * reset the fifos and other the settings.
 */
 
-static inline int s3c24xx_serial_resetport(struct uart_port *port,
-					   struct s3c2410_uartcfg *cfg)
+static void s3c24xx_serial_resetport(struct uart_port *port,
+				   struct s3c2410_uartcfg *cfg)
 {
 	struct s3c24xx_uart_info *info = s3c24xx_port_to_info(port);
+	unsigned long ucon = rd_regl(port, S3C2410_UCON);
+	unsigned int ucon_mask;
+
+	ucon_mask = info->clksel_mask;
+	if (info->type == PORT_S3C2440)
+		ucon_mask |= S3C2440_UCON0_DIVMASK;
 
-	return (info->reset_port)(port, cfg);
+	ucon &= ucon_mask;
+	wr_regl(port, S3C2410_UCON,  ucon | cfg->ucon);
+
+	/* reset both fifos */
+	wr_regl(port, S3C2410_UFCON, cfg->ufcon | S3C2410_UFCON_RESETBOTH);
+	wr_regl(port, S3C2410_UFCON, cfg->ufcon);
+
+	/* some delay is required after fifo reset */
+	udelay(1);
 }
 
 
@@ -1121,11 +1080,10 @@ static inline void s3c24xx_serial_cpufreq_deregister(struct s3c24xx_uart_port *p
  */
 
 static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
-				    struct s3c24xx_uart_info *info,
 				    struct platform_device *platdev)
 {
 	struct uart_port *port = &ourport->port;
-	struct s3c2410_uartcfg *cfg;
+	struct s3c2410_uartcfg *cfg = ourport->cfg;
 	struct resource *res;
 	int ret;
 
@@ -1134,30 +1092,16 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 	if (platdev == NULL)
 		return -ENODEV;
 
-	cfg = s3c24xx_dev_to_cfg(&platdev->dev);
-
 	if (port->mapbase != 0)
 		return 0;
 
-	if (cfg->hwport > CONFIG_SERIAL_SAMSUNG_UARTS) {
-		printk(KERN_ERR "%s: port %d bigger than %d\n", __func__,
-		       cfg->hwport, CONFIG_SERIAL_SAMSUNG_UARTS);
-		return -ERANGE;
-	}
-
 	/* setup info for port */
 	port->dev	= &platdev->dev;
-	ourport->info	= info;
 
 	/* Startup sequence is different for s3c64xx and higher SoC's */
 	if (s3c24xx_serial_has_interrupt_mask(port))
 		s3c24xx_serial_ops.startup = s3c64xx_serial_startup;
 
-	/* copy the info in from provided structure */
-	ourport->port.fifosize = info->fifosize;
-
-	dbg("s3c24xx_serial_init_port: %p (hw %d)...\n", port, cfg->hwport);
-
 	port->uartclk = 1;
 
 	if (cfg->uart_flags & UPF_CONS_FLOW) {
@@ -1215,43 +1159,74 @@ static ssize_t s3c24xx_serial_show_clksrc(struct device *dev,
 	struct uart_port *port = s3c24xx_dev_to_port(dev);
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
 
-	return snprintf(buf, PAGE_SIZE, "* %s\n", ourport->clksrc->name);
+	return snprintf(buf, PAGE_SIZE, "* %s\n", ourport->baudclk->name);
 }
 
 static DEVICE_ATTR(clock_source, S_IRUGO, s3c24xx_serial_show_clksrc, NULL);
 
+
 /* Device driver serial port probe */
 
+static const struct of_device_id s3c24xx_uart_dt_match[];
 static int probe_index;
 
-int s3c24xx_serial_probe(struct platform_device *dev,
-			 struct s3c24xx_uart_info *info)
+static inline struct s3c24xx_serial_drv_data *s3c24xx_get_driver_data(
+			struct platform_device *pdev)
+{
+#ifdef CONFIG_OF
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(s3c24xx_uart_dt_match, pdev->dev.of_node);
+		return (struct s3c24xx_serial_drv_data *)match->data;
+	}
+#endif
+	return (struct s3c24xx_serial_drv_data *)
+			platform_get_device_id(pdev)->driver_data;
+}
+
+static int s3c24xx_serial_probe(struct platform_device *pdev)
 {
 	struct s3c24xx_uart_port *ourport;
 	int ret;
 
-	dbg("s3c24xx_serial_probe(%p, %p) %d\n", dev, info, probe_index);
+	dbg("s3c24xx_serial_probe(%p) %d\n", pdev, probe_index);
 
 	ourport = &s3c24xx_serial_ports[probe_index];
+
+	ourport->drv_data = s3c24xx_get_driver_data(pdev);
+	if (!ourport->drv_data) {
+		dev_err(&pdev->dev, "could not find driver data\n");
+		return -ENODEV;
+	}
+
+	ourport->info = ourport->drv_data->info;
+	ourport->cfg = (pdev->dev.platform_data) ?
+			(struct s3c2410_uartcfg *)pdev->dev.platform_data :
+			ourport->drv_data->def_cfg;
+
+	ourport->port.fifosize = (ourport->info->fifosize) ?
+		ourport->info->fifosize :
+		ourport->drv_data->fifosize[probe_index];
+
 	probe_index++;
 
 	dbg("%s: initialising port %p...\n", __func__, ourport);
 
-	ret = s3c24xx_serial_init_port(ourport, info, dev);
+	ret = s3c24xx_serial_init_port(ourport, pdev);
 	if (ret < 0)
 		goto probe_err;
 
 	dbg("%s: adding port\n", __func__);
 	uart_add_one_port(&s3c24xx_uart_drv, &ourport->port);
-	platform_set_drvdata(dev, &ourport->port);
+	platform_set_drvdata(pdev, &ourport->port);
 
-	ret = device_create_file(&dev->dev, &dev_attr_clock_source);
+	ret = device_create_file(&pdev->dev, &dev_attr_clock_source);
 	if (ret < 0)
-		printk(KERN_ERR "%s: failed to add clksrc attr.\n", __func__);
+		dev_err(&pdev->dev, "failed to add clock source attr.\n");
 
 	ret = s3c24xx_serial_cpufreq_register(ourport);
 	if (ret < 0)
-		dev_err(&dev->dev, "failed to add cpufreq notifier\n");
+		dev_err(&pdev->dev, "failed to add cpufreq notifier\n");
 
 	return 0;
 
@@ -1259,9 +1234,7 @@ int s3c24xx_serial_probe(struct platform_device *dev,
 	return ret;
 }
 
-EXPORT_SYMBOL_GPL(s3c24xx_serial_probe);
-
-int __devexit s3c24xx_serial_remove(struct platform_device *dev)
+static int __devexit s3c24xx_serial_remove(struct platform_device *dev)
 {
 	struct uart_port *port = s3c24xx_dev_to_port(&dev->dev);
 
@@ -1274,8 +1247,6 @@ int __devexit s3c24xx_serial_remove(struct platform_device *dev)
 	return 0;
 }
 
-EXPORT_SYMBOL_GPL(s3c24xx_serial_remove);
-
 /* UART power management code */
 #ifdef CONFIG_PM_SLEEP
 static int s3c24xx_serial_suspend(struct device *dev)
@@ -1315,41 +1286,6 @@ static const struct dev_pm_ops s3c24xx_serial_pm_ops = {
 #define SERIAL_SAMSUNG_PM_OPS	NULL
 #endif /* CONFIG_PM_SLEEP */
 
-int s3c24xx_serial_init(struct platform_driver *drv,
-			struct s3c24xx_uart_info *info)
-{
-	dbg("s3c24xx_serial_init(%p,%p)\n", drv, info);
-
-	drv->driver.pm = SERIAL_SAMSUNG_PM_OPS;
-
-	return platform_driver_register(drv);
-}
-
-EXPORT_SYMBOL_GPL(s3c24xx_serial_init);
-
-/* module initialisation code */
-
-static int __init s3c24xx_serial_modinit(void)
-{
-	int ret;
-
-	ret = uart_register_driver(&s3c24xx_uart_drv);
-	if (ret < 0) {
-		printk(KERN_ERR "failed to register UART driver\n");
-		return -1;
-	}
-
-	return 0;
-}
-
-static void __exit s3c24xx_serial_modexit(void)
-{
-	uart_unregister_driver(&s3c24xx_uart_drv);
-}
-
-module_init(s3c24xx_serial_modinit);
-module_exit(s3c24xx_serial_modexit);
-
 /* Console code */
 
 #ifdef CONFIG_SERIAL_SAMSUNG_CONSOLE
@@ -1395,12 +1331,13 @@ static void __init
 s3c24xx_serial_get_options(struct uart_port *port, int *baud,
 			   int *parity, int *bits)
 {
-	struct s3c24xx_uart_clksrc clksrc;
 	struct clk *clk;
 	unsigned int ulcon;
 	unsigned int ucon;
 	unsigned int ubrdiv;
 	unsigned long rate;
+	unsigned int clk_sel;
+	char clk_name[MAX_CLK_NAME_LENGTH];
 
 	ulcon  = rd_regl(port, S3C2410_ULCON);
 	ucon   = rd_regl(port, S3C2410_UCON);
@@ -1445,44 +1382,21 @@ s3c24xx_serial_get_options(struct uart_port *port, int *baud,
 
 		/* now calculate the baud rate */
 
-		s3c24xx_serial_getsource(port, &clksrc);
+		clk_sel = s3c24xx_serial_getsource(port);
+		sprintf(clk_name, "clk_uart_baud%d", clk_sel);
 
-		clk = clk_get(port->dev, clksrc.name);
+		clk = clk_get(port->dev, clk_name);
 		if (!IS_ERR(clk) && clk != NULL)
-			rate = clk_get_rate(clk) / clksrc.divisor;
+			rate = clk_get_rate(clk);
 		else
 			rate = 1;
 
-
 		*baud = rate / (16 * (ubrdiv + 1));
 		dbg("calculated baud %d\n", *baud);
 	}
 
 }
 
-/* s3c24xx_serial_init_ports
- *
- * initialise the serial ports from the machine provided initialisation
- * data.
-*/
-
-static int s3c24xx_serial_init_ports(struct s3c24xx_uart_info **info)
-{
-	struct s3c24xx_uart_port *ptr = s3c24xx_serial_ports;
-	struct platform_device **platdev_ptr;
-	int i;
-
-	dbg("s3c24xx_serial_init_ports: initialising ports...\n");
-
-	platdev_ptr = s3c24xx_uart_devs;
-
-	for (i = 0; i < CONFIG_SERIAL_SAMSUNG_UARTS; i++, ptr++, platdev_ptr++) {
-		s3c24xx_serial_init_port(ptr, info[i], *platdev_ptr);
-	}
-
-	return 0;
-}
-
 static int __init
 s3c24xx_serial_console_setup(struct console *co, char *options)
 {
@@ -1526,11 +1440,6 @@ s3c24xx_serial_console_setup(struct console *co, char *options)
 	return uart_set_options(port, co, baud, parity, bits, flow);
 }
 
-/* s3c24xx_serial_initconsole
- *
- * initialise the console from one of the uart drivers
-*/
-
 static struct console s3c24xx_serial_console = {
 	.name		= S3C24XX_SERIAL_NAME,
 	.device		= uart_console_device,
@@ -1540,34 +1449,250 @@ static struct console s3c24xx_serial_console = {
 	.setup		= s3c24xx_serial_console_setup,
 	.data		= &s3c24xx_uart_drv,
 };
+#endif /* CONFIG_SERIAL_SAMSUNG_CONSOLE */
 
-int s3c24xx_serial_initconsole(struct platform_driver *drv,
-			       struct s3c24xx_uart_info **info)
+#ifdef CONFIG_CPU_S3C2410
+static struct s3c24xx_serial_drv_data s3c2410_serial_drv_data = {
+	.info = &(struct s3c24xx_uart_info) {
+		.name		= "Samsung S3C2410 UART",
+		.type		= PORT_S3C2410,
+		.fifosize	= 16,
+		.rx_fifomask	= S3C2410_UFSTAT_RXMASK,
+		.rx_fifoshift	= S3C2410_UFSTAT_RXSHIFT,
+		.rx_fifofull	= S3C2410_UFSTAT_RXFULL,
+		.tx_fifofull	= S3C2410_UFSTAT_TXFULL,
+		.tx_fifomask	= S3C2410_UFSTAT_TXMASK,
+		.tx_fifoshift	= S3C2410_UFSTAT_TXSHIFT,
+		.def_clk_sel	= S3C2410_UCON_CLKSEL0,
+		.num_clks	= 2,
+		.clksel_mask	= S3C2410_UCON_CLKMASK,
+		.clksel_shift	= S3C2410_UCON_CLKSHIFT,
+	},
+	.def_cfg = &(struct s3c2410_uartcfg) {
+		.ucon		= S3C2410_UCON_DEFAULT,
+		.ufcon		= S3C2410_UFCON_DEFAULT,
+	},
+};
+#define S3C2410_SERIAL_DRV_DATA ((kernel_ulong_t)&s3c2410_serial_drv_data)
+#else
+#define S3C2410_SERIAL_DRV_DATA (kernel_ulong_t)NULL
+#endif
 
-{
-	struct platform_device *dev = s3c24xx_uart_devs[0];
+#ifdef CONFIG_CPU_S3C2412
+static struct s3c24xx_serial_drv_data s3c2412_serial_drv_data = {
+	.info = &(struct s3c24xx_uart_info) {
+		.name		= "Samsung S3C2412 UART",
+		.type		= PORT_S3C2412,
+		.fifosize	= 64,
+		.has_divslot	= 1,
+		.rx_fifomask	= S3C2440_UFSTAT_RXMASK,
+		.rx_fifoshift	= S3C2440_UFSTAT_RXSHIFT,
+		.rx_fifofull	= S3C2440_UFSTAT_RXFULL,
+		.tx_fifofull	= S3C2440_UFSTAT_TXFULL,
+		.tx_fifomask	= S3C2440_UFSTAT_TXMASK,
+		.tx_fifoshift	= S3C2440_UFSTAT_TXSHIFT,
+		.def_clk_sel	= S3C2410_UCON_CLKSEL2,
+		.num_clks	= 4,
+		.clksel_mask	= S3C2412_UCON_CLKMASK,
+		.clksel_shift	= S3C2412_UCON_CLKSHIFT,
+	},
+	.def_cfg = &(struct s3c2410_uartcfg) {
+		.ucon		= S3C2410_UCON_DEFAULT,
+		.ufcon		= S3C2410_UFCON_DEFAULT,
+	},
+};
+#define S3C2412_SERIAL_DRV_DATA ((kernel_ulong_t)&s3c2412_serial_drv_data)
+#else
+#define S3C2412_SERIAL_DRV_DATA (kernel_ulong_t)NULL
+#endif
 
-	dbg("s3c24xx_serial_initconsole\n");
+#if defined(CONFIG_CPU_S3C2440) || defined(CONFIG_CPU_S3C2416) || \
+	defined(CONFIG_CPU_S3C2443)
+static struct s3c24xx_serial_drv_data s3c2440_serial_drv_data = {
+	.info = &(struct s3c24xx_uart_info) {
+		.name		= "Samsung S3C2440 UART",
+		.type		= PORT_S3C2440,
+		.fifosize	= 64,
+		.has_divslot	= 1,
+		.rx_fifomask	= S3C2440_UFSTAT_RXMASK,
+		.rx_fifoshift	= S3C2440_UFSTAT_RXSHIFT,
+		.rx_fifofull	= S3C2440_UFSTAT_RXFULL,
+		.tx_fifofull	= S3C2440_UFSTAT_TXFULL,
+		.tx_fifomask	= S3C2440_UFSTAT_TXMASK,
+		.tx_fifoshift	= S3C2440_UFSTAT_TXSHIFT,
+		.def_clk_sel	= S3C2410_UCON_CLKSEL2,
+		.num_clks	= 4,
+		.clksel_mask	= S3C2412_UCON_CLKMASK,
+		.clksel_shift	= S3C2412_UCON_CLKSHIFT,
+	},
+	.def_cfg = &(struct s3c2410_uartcfg) {
+		.ucon		= S3C2410_UCON_DEFAULT,
+		.ufcon		= S3C2410_UFCON_DEFAULT,
+	},
+};
+#define S3C2440_SERIAL_DRV_DATA ((kernel_ulong_t)&s3c2440_serial_drv_data)
+#else
+#define S3C2440_SERIAL_DRV_DATA (kernel_ulong_t)NULL
+#endif
 
-	/* select driver based on the cpu */
+#if defined(CONFIG_CPU_S3C6400) || defined(CONFIG_CPU_S3C6410) || \
+	defined(CONFIG_CPU_S5P6440) || defined(CONFIG_CPU_S5P6450) || \
+	defined(CONFIG_CPU_S5PC100)
+static struct s3c24xx_serial_drv_data s3c6400_serial_drv_data = {
+	.info = &(struct s3c24xx_uart_info) {
+		.name		= "Samsung S3C6400 UART",
+		.type		= PORT_S3C6400,
+		.fifosize	= 64,
+		.has_divslot	= 1,
+		.rx_fifomask	= S3C2440_UFSTAT_RXMASK,
+		.rx_fifoshift	= S3C2440_UFSTAT_RXSHIFT,
+		.rx_fifofull	= S3C2440_UFSTAT_RXFULL,
+		.tx_fifofull	= S3C2440_UFSTAT_TXFULL,
+		.tx_fifomask	= S3C2440_UFSTAT_TXMASK,
+		.tx_fifoshift	= S3C2440_UFSTAT_TXSHIFT,
+		.def_clk_sel	= S3C2410_UCON_CLKSEL2,
+		.num_clks	= 4,
+		.clksel_mask	= S3C6400_UCON_CLKMASK,
+		.clksel_shift	= S3C6400_UCON_CLKSHIFT,
+	},
+	.def_cfg = &(struct s3c2410_uartcfg) {
+		.ucon		= S3C2410_UCON_DEFAULT,
+		.ufcon		= S3C2410_UFCON_DEFAULT,
+	},
+};
+#define S3C6400_SERIAL_DRV_DATA ((kernel_ulong_t)&s3c6400_serial_drv_data)
+#else
+#define S3C6400_SERIAL_DRV_DATA (kernel_ulong_t)NULL
+#endif
 
-	if (dev == NULL) {
-		printk(KERN_ERR "s3c24xx: no devices for console init\n");
-		return 0;
-	}
+#ifdef CONFIG_CPU_S5PV210
+static struct s3c24xx_serial_drv_data s5pv210_serial_drv_data = {
+	.info = &(struct s3c24xx_uart_info) {
+		.name		= "Samsung S5PV210 UART",
+		.type		= PORT_S3C6400,
+		.has_divslot	= 1,
+		.rx_fifomask	= S5PV210_UFSTAT_RXMASK,
+		.rx_fifoshift	= S5PV210_UFSTAT_RXSHIFT,
+		.rx_fifofull	= S5PV210_UFSTAT_RXFULL,
+		.tx_fifofull	= S5PV210_UFSTAT_TXFULL,
+		.tx_fifomask	= S5PV210_UFSTAT_TXMASK,
+		.tx_fifoshift	= S5PV210_UFSTAT_TXSHIFT,
+		.def_clk_sel	= S3C2410_UCON_CLKSEL0,
+		.num_clks	= 2,
+		.clksel_mask	= S5PV210_UCON_CLKMASK,
+		.clksel_shift	= S5PV210_UCON_CLKSHIFT,
+	},
+	.def_cfg = &(struct s3c2410_uartcfg) {
+		.ucon		= S5PV210_UCON_DEFAULT,
+		.ufcon		= S5PV210_UFCON_DEFAULT,
+	},
+	.fifosize = { 256, 64, 16, 16 },
+};
+#define S5PV210_SERIAL_DRV_DATA ((kernel_ulong_t)&s5pv210_serial_drv_data)
+#else
+#define S5PV210_SERIAL_DRV_DATA	(kernel_ulong_t)NULL
+#endif
 
-	if (strcmp(dev->name, drv->driver.name) != 0)
-		return 0;
+#ifdef CONFIG_CPU_EXYNOS4210
+static struct s3c24xx_serial_drv_data exynos4210_serial_drv_data = {
+	.info = &(struct s3c24xx_uart_info) {
+		.name		= "Samsung Exynos4 UART",
+		.type		= PORT_S3C6400,
+		.has_divslot	= 1,
+		.rx_fifomask	= S5PV210_UFSTAT_RXMASK,
+		.rx_fifoshift	= S5PV210_UFSTAT_RXSHIFT,
+		.rx_fifofull	= S5PV210_UFSTAT_RXFULL,
+		.tx_fifofull	= S5PV210_UFSTAT_TXFULL,
+		.tx_fifomask	= S5PV210_UFSTAT_TXMASK,
+		.tx_fifoshift	= S5PV210_UFSTAT_TXSHIFT,
+		.def_clk_sel	= S3C2410_UCON_CLKSEL0,
+		.num_clks	= 1,
+		.clksel_mask	= 0,
+		.clksel_shift	= 0,
+	},
+	.def_cfg = &(struct s3c2410_uartcfg) {
+		.ucon		= S5PV210_UCON_DEFAULT,
+		.ufcon		= S5PV210_UFCON_DEFAULT,
+		.has_fracval	= 1,
+	},
+	.fifosize = { 256, 64, 16, 16 },
+};
+#define EXYNOS4210_SERIAL_DRV_DATA ((kernel_ulong_t)&exynos4210_serial_drv_data)
+#else
+#define EXYNOS4210_SERIAL_DRV_DATA (kernel_ulong_t)NULL
+#endif
 
-	s3c24xx_serial_console.data = &s3c24xx_uart_drv;
-	s3c24xx_serial_init_ports(info);
+static struct platform_device_id s3c24xx_serial_driver_ids[] = {
+	{
+		.name		= "s3c2410-uart",
+		.driver_data	= S3C2410_SERIAL_DRV_DATA,
+	}, {
+		.name		= "s3c2412-uart",
+		.driver_data	= S3C2412_SERIAL_DRV_DATA,
+	}, {
+		.name		= "s3c2440-uart",
+		.driver_data	= S3C2440_SERIAL_DRV_DATA,
+	}, {
+		.name		= "s3c6400-uart",
+		.driver_data	= S3C6400_SERIAL_DRV_DATA,
+	}, {
+		.name		= "s5pv210-uart",
+		.driver_data	= S5PV210_SERIAL_DRV_DATA,
+	}, {
+		.name		= "exynos4210-uart",
+		.driver_data	= EXYNOS4210_SERIAL_DRV_DATA,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, s3c24xx_serial_driver_ids);
 
-	register_console(&s3c24xx_serial_console);
-	return 0;
+#ifdef CONFIG_OF
+static const struct of_device_id s3c24xx_uart_dt_match[] = {
+	{ .compatible = "samsung,exynos4210-uart",
+		.data = (void *)EXYNOS4210_SERIAL_DRV_DATA },
+	{},
+};
+MODULE_DEVICE_TABLE(of, s3c24xx_uart_dt_match);
+#else
+#define s3c24xx_uart_dt_match NULL
+#endif
+
+static struct platform_driver samsung_serial_driver = {
+	.probe		= s3c24xx_serial_probe,
+	.remove		= __devexit_p(s3c24xx_serial_remove),
+	.id_table	= s3c24xx_serial_driver_ids,
+	.driver		= {
+		.name	= "samsung-uart",
+		.owner	= THIS_MODULE,
+		.pm	= SERIAL_SAMSUNG_PM_OPS,
+		.of_match_table	= s3c24xx_uart_dt_match,
+	},
+};
+
+/* module initialisation code */
+
+static int __init s3c24xx_serial_modinit(void)
+{
+	int ret;
+
+	ret = uart_register_driver(&s3c24xx_uart_drv);
+	if (ret < 0) {
+		printk(KERN_ERR "failed to register UART driver\n");
+		return -1;
+	}
+
+	return platform_driver_register(&samsung_serial_driver);
 }
 
-#endif /* CONFIG_SERIAL_SAMSUNG_CONSOLE */
+static void __exit s3c24xx_serial_modexit(void)
+{
+	uart_unregister_driver(&s3c24xx_uart_drv);
+}
+
+module_init(s3c24xx_serial_modinit);
+module_exit(s3c24xx_serial_modexit);
 
+MODULE_ALIAS("platform:samsung-uart");
 MODULE_DESCRIPTION("Samsung SoC Serial port driver");
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/samsung.h b/drivers/tty/serial/samsung.h
index 8e87b788e5c6..1a4bca3e4179 100644
--- a/drivers/tty/serial/samsung.h
+++ b/drivers/tty/serial/samsung.h
@@ -19,20 +19,25 @@ struct s3c24xx_uart_info {
 	unsigned long		tx_fifomask;
 	unsigned long		tx_fifoshift;
 	unsigned long		tx_fifofull;
+	unsigned int		def_clk_sel;
+	unsigned long		num_clks;
+	unsigned long		clksel_mask;
+	unsigned long		clksel_shift;
 
 	/* uart port features */
 
 	unsigned int		has_divslot:1;
 
-	/* clock source control */
-
-	int (*get_clksrc)(struct uart_port *, struct s3c24xx_uart_clksrc *clk);
-	int (*set_clksrc)(struct uart_port *, struct s3c24xx_uart_clksrc *clk);
-
 	/* uart controls */
 	int (*reset_port)(struct uart_port *, struct s3c2410_uartcfg *);
 };
 
+struct s3c24xx_serial_drv_data {
+	struct s3c24xx_uart_info	*info;
+	struct s3c2410_uartcfg		*def_cfg;
+	unsigned int			fifosize[CONFIG_SERIAL_SAMSUNG_UARTS];
+};
+
 struct s3c24xx_uart_port {
 	unsigned char			rx_claimed;
 	unsigned char			tx_claimed;
@@ -43,10 +48,13 @@ struct s3c24xx_uart_port {
 	unsigned int			tx_irq;
 
 	struct s3c24xx_uart_info	*info;
-	struct s3c24xx_uart_clksrc	*clksrc;
 	struct clk			*clk;
 	struct clk			*baudclk;
 	struct uart_port		port;
+	struct s3c24xx_serial_drv_data	*drv_data;
+
+	/* reference to platform data */
+	struct s3c2410_uartcfg		*cfg;
 
 #ifdef CONFIG_CPU_FREQ
 	struct notifier_block		freq_transition;
@@ -56,7 +64,6 @@ struct s3c24xx_uart_port {
 /* conversion functions */
 
 #define s3c24xx_dev_to_port(__dev) (struct uart_port *)dev_get_drvdata(__dev)
-#define s3c24xx_dev_to_cfg(__dev) (struct s3c2410_uartcfg *)((__dev)->platform_data)
 
 /* register access controls */
 
@@ -69,17 +76,6 @@ struct s3c24xx_uart_port {
 #define wr_regb(port, reg, val) __raw_writeb(val, portaddr(port, reg))
 #define wr_regl(port, reg, val) __raw_writel(val, portaddr(port, reg))
 
-extern int s3c24xx_serial_probe(struct platform_device *dev,
-				struct s3c24xx_uart_info *uart);
-
-extern int __devexit s3c24xx_serial_remove(struct platform_device *dev);
-
-extern int s3c24xx_serial_initconsole(struct platform_driver *drv,
-				      struct s3c24xx_uart_info **uart);
-
-extern int s3c24xx_serial_init(struct platform_driver *drv,
-			       struct s3c24xx_uart_info *info);
-
 #ifdef CONFIG_SERIAL_SAMSUNG_DEBUG
 
 extern void printascii(const char *);
diff --git a/drivers/tty/serial/sc26xx.c b/drivers/tty/serial/sc26xx.c
index 75038ad2b242..e0b4b0a30a5a 100644
--- a/drivers/tty/serial/sc26xx.c
+++ b/drivers/tty/serial/sc26xx.c
@@ -736,19 +736,7 @@ static struct platform_driver sc26xx_driver = {
 	},
 };
 
-static int __init sc26xx_init(void)
-{
-	return platform_driver_register(&sc26xx_driver);
-}
-
-static void __exit sc26xx_exit(void)
-{
-	platform_driver_unregister(&sc26xx_driver);
-}
-
-module_init(sc26xx_init);
-module_exit(sc26xx_exit);
-
+module_platform_driver(sc26xx_driver);
 
 MODULE_AUTHOR("Thomas Bogendörfer");
 MODULE_DESCRIPTION("SC681/SC2692 serial driver");
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index 0406d7ff505e..c7bf31a6a7e7 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -22,6 +22,7 @@
  */
 #include <linux/module.h>
 #include <linux/tty.h>
+#include <linux/tty_flip.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/console.h>
@@ -60,6 +61,8 @@ static void uart_change_speed(struct tty_struct *tty, struct uart_state *state,
 static void uart_wait_until_sent(struct tty_struct *tty, int timeout);
 static void uart_change_pm(struct uart_state *state, int pm_state);
 
+static void uart_port_shutdown(struct tty_port *port);
+
 /*
  * This routine is used by the interrupt handler to schedule processing in
  * the software interrupt portion of the driver.
@@ -128,25 +131,16 @@ uart_update_mctrl(struct uart_port *port, unsigned int set, unsigned int clear)
  * Startup the port.  This will be called once per open.  All calls
  * will be serialised by the per-port mutex.
  */
-static int uart_startup(struct tty_struct *tty, struct uart_state *state, int init_hw)
+static int uart_port_startup(struct tty_struct *tty, struct uart_state *state,
+		int init_hw)
 {
 	struct uart_port *uport = state->uart_port;
 	struct tty_port *port = &state->port;
 	unsigned long page;
 	int retval = 0;
 
-	if (port->flags & ASYNC_INITIALIZED)
-		return 0;
-
-	/*
-	 * Set the TTY IO error marker - we will only clear this
-	 * once we have successfully opened the port.  Also set
-	 * up the tty->alt_speed kludge
-	 */
-	set_bit(TTY_IO_ERROR, &tty->flags);
-
 	if (uport->type == PORT_UNKNOWN)
-		return 0;
+		return 1;
 
 	/*
 	 * Initialise and allocate the transmit and temporary
@@ -188,10 +182,6 @@ static int uart_startup(struct tty_struct *tty, struct uart_state *state, int in
 				tty->hw_stopped = 1;
 			spin_unlock_irq(&uport->lock);
 		}
-
-		set_bit(ASYNCB_INITIALIZED, &port->flags);
-
-		clear_bit(TTY_IO_ERROR, &tty->flags);
 	}
 
 	/*
@@ -200,6 +190,31 @@ static int uart_startup(struct tty_struct *tty, struct uart_state *state, int in
 	 * now.
 	 */
 	if (retval && capable(CAP_SYS_ADMIN))
+		return 1;
+
+	return retval;
+}
+
+static int uart_startup(struct tty_struct *tty, struct uart_state *state,
+		int init_hw)
+{
+	struct tty_port *port = &state->port;
+	int retval;
+
+	if (port->flags & ASYNC_INITIALIZED)
+		return 0;
+
+	/*
+	 * Set the TTY IO error marker - we will only clear this
+	 * once we have successfully opened the port.
+	 */
+	set_bit(TTY_IO_ERROR, &tty->flags);
+
+	retval = uart_port_startup(tty, state, init_hw);
+	if (!retval) {
+		set_bit(ASYNCB_INITIALIZED, &port->flags);
+		clear_bit(TTY_IO_ERROR, &tty->flags);
+	} else if (retval > 0)
 		retval = 0;
 
 	return retval;
@@ -228,24 +243,7 @@ static void uart_shutdown(struct tty_struct *tty, struct uart_state *state)
 		if (!tty || (tty->termios->c_cflag & HUPCL))
 			uart_clear_mctrl(uport, TIOCM_DTR | TIOCM_RTS);
 
-		/*
-		 * clear delta_msr_wait queue to avoid mem leaks: we may free
-		 * the irq here so the queue might never be woken up.  Note
-		 * that we won't end up waiting on delta_msr_wait again since
-		 * any outstanding file descriptors should be pointing at
-		 * hung_up_tty_fops now.
-		 */
-		wake_up_interruptible(&port->delta_msr_wait);
-
-		/*
-		 * Free the IRQ and disable the port.
-		 */
-		uport->ops->shutdown(uport);
-
-		/*
-		 * Ensure that the IRQ handler isn't running on another CPU.
-		 */
-		synchronize_irq(uport->irq);
+		uart_port_shutdown(port);
 	}
 
 	/*
@@ -423,7 +421,7 @@ uart_get_divisor(struct uart_port *port, unsigned int baud)
 	if (baud == 38400 && (port->flags & UPF_SPD_MASK) == UPF_SPD_CUST)
 		quot = port->custom_divisor;
 	else
-		quot = (port->uartclk + (8 * baud)) / (16 * baud);
+		quot = DIV_ROUND_CLOSEST(port->uartclk, 16 * baud);
 
 	return quot;
 }
@@ -658,10 +656,10 @@ static int uart_get_info(struct uart_state *state,
 	tmp.flags	    = uport->flags;
 	tmp.xmit_fifo_size  = uport->fifosize;
 	tmp.baud_base	    = uport->uartclk / 16;
-	tmp.close_delay	    = port->close_delay / 10;
+	tmp.close_delay	    = jiffies_to_msecs(port->close_delay) / 10;
 	tmp.closing_wait    = port->closing_wait == ASYNC_CLOSING_WAIT_NONE ?
 				ASYNC_CLOSING_WAIT_NONE :
-				port->closing_wait / 10;
+				jiffies_to_msecs(port->closing_wait) / 10;
 	tmp.custom_divisor  = uport->custom_divisor;
 	tmp.hub6	    = uport->hub6;
 	tmp.io_type         = uport->iotype;
@@ -695,9 +693,10 @@ static int uart_set_info(struct tty_struct *tty, struct uart_state *state,
 		new_port += (unsigned long) new_serial.port_high << HIGH_BITS_OFFSET;
 
 	new_serial.irq = irq_canonicalize(new_serial.irq);
-	close_delay = new_serial.close_delay * 10;
+	close_delay = msecs_to_jiffies(new_serial.close_delay * 10);
 	closing_wait = new_serial.closing_wait == ASYNC_CLOSING_WAIT_NONE ?
-			ASYNC_CLOSING_WAIT_NONE : new_serial.closing_wait * 10;
+			ASYNC_CLOSING_WAIT_NONE :
+			msecs_to_jiffies(new_serial.closing_wait * 10);
 
 	/*
 	 * This semaphore protects port->count.  It is also
@@ -1265,47 +1264,8 @@ static void uart_close(struct tty_struct *tty, struct file *filp)
 
 	pr_debug("uart_close(%d) called\n", uport->line);
 
-	spin_lock_irqsave(&port->lock, flags);
-
-	if (tty_hung_up_p(filp)) {
-		spin_unlock_irqrestore(&port->lock, flags);
+	if (tty_port_close_start(port, tty, filp) == 0)
 		return;
-	}
-
-	if ((tty->count == 1) && (port->count != 1)) {
-		/*
-		 * Uh, oh.  tty->count is 1, which means that the tty
-		 * structure will be freed.  port->count should always
-		 * be one in these conditions.  If it's greater than
-		 * one, we've got real problems, since it means the
-		 * serial port won't be shutdown.
-		 */
-		printk(KERN_ERR "uart_close: bad serial port count; tty->count is 1, "
-		       "port->count is %d\n", port->count);
-		port->count = 1;
-	}
-	if (--port->count < 0) {
-		printk(KERN_ERR "uart_close: bad serial port count for %s: %d\n",
-		       tty->name, port->count);
-		port->count = 0;
-	}
-	if (port->count) {
-		spin_unlock_irqrestore(&port->lock, flags);
-		return;
-	}
-
-	/*
-	 * Now we wait for the transmit buffer to clear; and we notify
-	 * the line discipline to only process XON/XOFF characters by
-	 * setting tty->closing.
-	 */
-	set_bit(ASYNCB_CLOSING, &port->flags);
-	tty->closing = 1;
-	spin_unlock_irqrestore(&port->lock, flags);
-
-	if (port->closing_wait != ASYNC_CLOSING_WAIT_NONE)
-		tty_wait_until_sent_from_close(tty,
-				msecs_to_jiffies(port->closing_wait));
 
 	/*
 	 * At this point, we stop accepting input.  To do this, we
@@ -1337,7 +1297,8 @@ static void uart_close(struct tty_struct *tty, struct file *filp)
 	if (port->blocked_open) {
 		spin_unlock_irqrestore(&port->lock, flags);
 		if (port->close_delay)
-			msleep_interruptible(port->close_delay);
+			msleep_interruptible(
+					jiffies_to_msecs(port->close_delay));
 		spin_lock_irqsave(&port->lock, flags);
 	} else if (!uart_console(uport)) {
 		spin_unlock_irqrestore(&port->lock, flags);
@@ -1441,6 +1402,36 @@ static void uart_hangup(struct tty_struct *tty)
 	mutex_unlock(&port->mutex);
 }
 
+static int uart_port_activate(struct tty_port *port, struct tty_struct *tty)
+{
+	return 0;
+}
+
+static void uart_port_shutdown(struct tty_port *port)
+{
+	struct uart_state *state = container_of(port, struct uart_state, port);
+	struct uart_port *uport = state->uart_port;
+
+	/*
+	 * clear delta_msr_wait queue to avoid mem leaks: we may free
+	 * the irq here so the queue might never be woken up.  Note
+	 * that we won't end up waiting on delta_msr_wait again since
+	 * any outstanding file descriptors should be pointing at
+	 * hung_up_tty_fops now.
+	 */
+	wake_up_interruptible(&port->delta_msr_wait);
+
+	/*
+	 * Free the IRQ and disable the port.
+	 */
+	uport->ops->shutdown(uport);
+
+	/*
+	 * Ensure that the IRQ handler isn't running on another CPU.
+	 */
+	synchronize_irq(uport->irq);
+}
+
 static int uart_carrier_raised(struct tty_port *port)
 {
 	struct uart_state *state = container_of(port, struct uart_state, port);
@@ -1466,33 +1457,6 @@ static void uart_dtr_rts(struct tty_port *port, int onoff)
 		uart_clear_mctrl(uport, TIOCM_DTR | TIOCM_RTS);
 }
 
-static struct uart_state *uart_get(struct uart_driver *drv, int line)
-{
-	struct uart_state *state;
-	struct tty_port *port;
-	int ret = 0;
-
-	state = drv->state + line;
-	port = &state->port;
-	if (mutex_lock_interruptible(&port->mutex)) {
-		ret = -ERESTARTSYS;
-		goto err;
-	}
-
-	port->count++;
-	if (!state->uart_port || state->uart_port->flags & UPF_DEAD) {
-		ret = -ENXIO;
-		goto err_unlock;
-	}
-	return state;
-
- err_unlock:
-	port->count--;
-	mutex_unlock(&port->mutex);
- err:
-	return ERR_PTR(ret);
-}
-
 /*
  * calls to uart_open are serialised by the BKL in
  *   fs/char_dev.c:chrdev_open()
@@ -1506,26 +1470,29 @@ static struct uart_state *uart_get(struct uart_driver *drv, int line)
 static int uart_open(struct tty_struct *tty, struct file *filp)
 {
 	struct uart_driver *drv = (struct uart_driver *)tty->driver->driver_state;
-	struct uart_state *state;
-	struct tty_port *port;
 	int retval, line = tty->index;
+	struct uart_state *state = drv->state + line;
+	struct tty_port *port = &state->port;
 
 	pr_debug("uart_open(%d) called\n", line);
 
 	/*
-	 * We take the semaphore inside uart_get to guarantee that we won't
-	 * be re-entered while allocating the state structure, or while we
-	 * request any IRQs that the driver may need.  This also has the nice
-	 * side-effect that it delays the action of uart_hangup, so we can
-	 * guarantee that state->port.tty will always contain something
-	 * reasonable.
+	 * We take the semaphore here to guarantee that we won't be re-entered
+	 * while allocating the state structure, or while we request any IRQs
+	 * that the driver may need.  This also has the nice side-effect that
+	 * it delays the action of uart_hangup, so we can guarantee that
+	 * state->port.tty will always contain something reasonable.
 	 */
-	state = uart_get(drv, line);
-	if (IS_ERR(state)) {
-		retval = PTR_ERR(state);
-		goto fail;
+	if (mutex_lock_interruptible(&port->mutex)) {
+		retval = -ERESTARTSYS;
+		goto end;
+	}
+
+	port->count++;
+	if (!state->uart_port || state->uart_port->flags & UPF_DEAD) {
+		retval = -ENXIO;
+		goto err_dec_count;
 	}
-	port = &state->port;
 
 	/*
 	 * Once we set tty->driver_data here, we are guaranteed that
@@ -1535,7 +1502,6 @@ static int uart_open(struct tty_struct *tty, struct file *filp)
 	tty->driver_data = state;
 	state->uart_port->state = state;
 	tty->low_latency = (state->uart_port->flags & UPF_LOW_LATENCY) ? 1 : 0;
-	tty->alt_speed = 0;
 	tty_port_tty_set(port, tty);
 
 	/*
@@ -1543,9 +1509,7 @@ static int uart_open(struct tty_struct *tty, struct file *filp)
 	 */
 	if (tty_hung_up_p(filp)) {
 		retval = -EAGAIN;
-		port->count--;
-		mutex_unlock(&port->mutex);
-		goto fail;
+		goto err_dec_count;
 	}
 
 	/*
@@ -1566,8 +1530,12 @@ static int uart_open(struct tty_struct *tty, struct file *filp)
 	if (retval == 0)
 		retval = tty_port_block_til_ready(port, tty, filp);
 
-fail:
+end:
 	return retval;
+err_dec_count:
+	port->count--;
+	mutex_unlock(&port->mutex);
+	goto end;
 }
 
 static const char *uart_type(struct uart_port *port)
@@ -1858,6 +1826,14 @@ uart_set_options(struct uart_port *port, struct console *co,
 EXPORT_SYMBOL_GPL(uart_set_options);
 #endif /* CONFIG_SERIAL_CORE_CONSOLE */
 
+/**
+ * uart_change_pm - set power state of the port
+ *
+ * @state: port descriptor
+ * @pm_state: new state
+ *
+ * Locking: port->mutex has to be held
+ */
 static void uart_change_pm(struct uart_state *state, int pm_state)
 {
 	struct uart_port *port = state->uart_port;
@@ -2214,6 +2190,8 @@ static const struct tty_operations uart_ops = {
 };
 
 static const struct tty_port_operations uart_port_ops = {
+	.activate	= uart_port_activate,
+	.shutdown	= uart_port_shutdown,
 	.carrier_raised = uart_carrier_raised,
 	.dtr_rts	= uart_dtr_rts,
 };
@@ -2275,8 +2253,8 @@ int uart_register_driver(struct uart_driver *drv)
 
 		tty_port_init(port);
 		port->ops = &uart_port_ops;
-		port->close_delay     = 500;	/* .5 seconds */
-		port->closing_wait    = 30000;	/* 30 seconds */
+		port->close_delay     = HZ / 2;	/* .5 seconds */
+		port->closing_wait    = 30 * HZ;/* 30 seconds */
 	}
 
 	retval = tty_register_driver(normal);
@@ -2467,6 +2445,99 @@ int uart_match_port(struct uart_port *port1, struct uart_port *port2)
 }
 EXPORT_SYMBOL(uart_match_port);
 
+/**
+ *	uart_handle_dcd_change - handle a change of carrier detect state
+ *	@uport: uart_port structure for the open port
+ *	@status: new carrier detect status, nonzero if active
+ */
+void uart_handle_dcd_change(struct uart_port *uport, unsigned int status)
+{
+	struct uart_state *state = uport->state;
+	struct tty_port *port = &state->port;
+	struct tty_ldisc *ld = tty_ldisc_ref(port->tty);
+	struct pps_event_time ts;
+
+	if (ld && ld->ops->dcd_change)
+		pps_get_ts(&ts);
+
+	uport->icount.dcd++;
+#ifdef CONFIG_HARD_PPS
+	if ((uport->flags & UPF_HARDPPS_CD) && status)
+		hardpps();
+#endif
+
+	if (port->flags & ASYNC_CHECK_CD) {
+		if (status)
+			wake_up_interruptible(&port->open_wait);
+		else if (port->tty)
+			tty_hangup(port->tty);
+	}
+
+	if (ld && ld->ops->dcd_change)
+		ld->ops->dcd_change(port->tty, status, &ts);
+	if (ld)
+		tty_ldisc_deref(ld);
+}
+EXPORT_SYMBOL_GPL(uart_handle_dcd_change);
+
+/**
+ *	uart_handle_cts_change - handle a change of clear-to-send state
+ *	@uport: uart_port structure for the open port
+ *	@status: new clear to send status, nonzero if active
+ */
+void uart_handle_cts_change(struct uart_port *uport, unsigned int status)
+{
+	struct tty_port *port = &uport->state->port;
+	struct tty_struct *tty = port->tty;
+
+	uport->icount.cts++;
+
+	if (port->flags & ASYNC_CTS_FLOW) {
+		if (tty->hw_stopped) {
+			if (status) {
+				tty->hw_stopped = 0;
+				uport->ops->start_tx(uport);
+				uart_write_wakeup(uport);
+			}
+		} else {
+			if (!status) {
+				tty->hw_stopped = 1;
+				uport->ops->stop_tx(uport);
+			}
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(uart_handle_cts_change);
+
+/**
+ * uart_insert_char - push a char to the uart layer
+ *
+ * User is responsible to call tty_flip_buffer_push when they are done with
+ * insertion.
+ *
+ * @port: corresponding port
+ * @status: state of the serial port RX buffer (LSR for 8250)
+ * @overrun: mask of overrun bits in @status
+ * @ch: character to push
+ * @flag: flag for the character (see TTY_NORMAL and friends)
+ */
+void uart_insert_char(struct uart_port *port, unsigned int status,
+		 unsigned int overrun, unsigned int ch, unsigned int flag)
+{
+	struct tty_struct *tty = port->state->port.tty;
+
+	if ((status & port->ignore_status_mask & ~overrun) == 0)
+		tty_insert_flip_char(tty, ch, flag);
+
+	/*
+	 * Overrun is special.  Since it's reported immediately,
+	 * it doesn't affect the current character.
+	 */
+	if (status & ~port->ignore_status_mask & overrun)
+		tty_insert_flip_char(tty, 0, TTY_OVERRUN);
+}
+EXPORT_SYMBOL_GPL(uart_insert_char);
+
 EXPORT_SYMBOL(uart_write_wakeup);
 EXPORT_SYMBOL(uart_register_driver);
 EXPORT_SYMBOL(uart_unregister_driver);
diff --git a/drivers/tty/serial/serial_cs.c b/drivers/tty/serial/serial_cs.c
index eef736ff810a..86090605a84e 100644
--- a/drivers/tty/serial/serial_cs.c
+++ b/drivers/tty/serial/serial_cs.c
@@ -317,7 +317,7 @@ static int serial_probe(struct pcmcia_device *link)
 	info->p_dev = link;
 	link->priv = info;
 
-	link->config_flags |= CONF_ENABLE_IRQ;
+	link->config_flags |= CONF_ENABLE_IRQ | CONF_AUTO_SET_IO;
 	if (do_sound)
 		link->config_flags |= CONF_ENABLE_SPKR;
 
@@ -445,7 +445,7 @@ static int simple_config(struct pcmcia_device *link)
 
 	/* First pass: look for a config entry that looks normal.
 	 * Two tries: without IO aliases, then with aliases */
-	link->config_flags |= CONF_AUTO_SET_VPP | CONF_AUTO_SET_IO;
+	link->config_flags |= CONF_AUTO_SET_VPP;
 	for (try = 0; try < 4; try++)
 		if (!pcmcia_loop_config(link, simple_config_check, &try))
 			goto found_port;
@@ -501,7 +501,8 @@ static int multi_config_check_notpicky(struct pcmcia_device *p_dev,
 {
 	int *base2 = priv_data;
 
-	if (!p_dev->resource[0]->end || !p_dev->resource[1]->end)
+	if (!p_dev->resource[0]->end || !p_dev->resource[1]->end ||
+		p_dev->resource[0]->start + 8 != p_dev->resource[1]->start)
 		return -ENODEV;
 
 	p_dev->resource[0]->end = p_dev->resource[1]->end = 8;
@@ -520,7 +521,6 @@ static int multi_config(struct pcmcia_device *link)
 	struct serial_info *info = link->priv;
 	int i, base2 = 0;
 
-	link->config_flags |= CONF_AUTO_SET_IO;
 	/* First, look for a generic full-sized window */
 	if (!pcmcia_loop_config(link, multi_config_check, &info->multi))
 		base2 = link->resource[0]->start + 8;
diff --git a/drivers/tty/serial/sirfsoc_uart.c b/drivers/tty/serial/sirfsoc_uart.c
new file mode 100644
index 000000000000..a60523fee11b
--- /dev/null
+++ b/drivers/tty/serial/sirfsoc_uart.c
@@ -0,0 +1,783 @@
+/*
+ * Driver for CSR SiRFprimaII onboard UARTs.
+ *
+ * Copyright (c) 2011 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/init.h>
+#include <linux/sysrq.h>
+#include <linux/console.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <asm/irq.h>
+#include <asm/mach/irq.h>
+#include <linux/pinctrl/pinmux.h>
+
+#include "sirfsoc_uart.h"
+
+static unsigned int
+sirfsoc_uart_pio_tx_chars(struct sirfsoc_uart_port *sirfport, int count);
+static unsigned int
+sirfsoc_uart_pio_rx_chars(struct uart_port *port, unsigned int max_rx_count);
+static struct uart_driver sirfsoc_uart_drv;
+
+static const struct sirfsoc_baudrate_to_regv baudrate_to_regv[] = {
+	{4000000, 2359296},
+	{3500000, 1310721},
+	{3000000, 1572865},
+	{2500000, 1245186},
+	{2000000, 1572866},
+	{1500000, 1245188},
+	{1152000, 1638404},
+	{1000000, 1572869},
+	{921600, 1114120},
+	{576000, 1245196},
+	{500000, 1245198},
+	{460800, 1572876},
+	{230400, 1310750},
+	{115200, 1310781},
+	{57600, 1310843},
+	{38400, 1114328},
+	{19200, 1114545},
+	{9600, 1114979},
+};
+
+static struct sirfsoc_uart_port sirfsoc_uart_ports[SIRFSOC_UART_NR] = {
+	[0] = {
+		.port = {
+			.iotype		= UPIO_MEM,
+			.flags		= UPF_BOOT_AUTOCONF,
+			.line		= 0,
+		},
+	},
+	[1] = {
+		.port = {
+			.iotype		= UPIO_MEM,
+			.flags		= UPF_BOOT_AUTOCONF,
+			.line		= 1,
+		},
+	},
+	[2] = {
+		.port = {
+			.iotype		= UPIO_MEM,
+			.flags		= UPF_BOOT_AUTOCONF,
+			.line		= 2,
+		},
+	},
+};
+
+static inline struct sirfsoc_uart_port *to_sirfport(struct uart_port *port)
+{
+	return container_of(port, struct sirfsoc_uart_port, port);
+}
+
+static inline unsigned int sirfsoc_uart_tx_empty(struct uart_port *port)
+{
+	unsigned long reg;
+	reg = rd_regl(port, SIRFUART_TX_FIFO_STATUS);
+	if (reg & SIRFUART_FIFOEMPTY_MASK(port))
+		return TIOCSER_TEMT;
+	else
+		return 0;
+}
+
+static unsigned int sirfsoc_uart_get_mctrl(struct uart_port *port)
+{
+	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
+	if (!(sirfport->ms_enabled)) {
+		goto cts_asserted;
+	} else if (sirfport->hw_flow_ctrl) {
+		if (!(rd_regl(port, SIRFUART_AFC_CTRL) &
+						SIRFUART_CTS_IN_STATUS))
+			goto cts_asserted;
+		else
+			goto cts_deasserted;
+	}
+cts_deasserted:
+	return TIOCM_CAR | TIOCM_DSR;
+cts_asserted:
+	return TIOCM_CAR | TIOCM_DSR | TIOCM_CTS;
+}
+
+static void sirfsoc_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
+	unsigned int assert = mctrl & TIOCM_RTS;
+	unsigned int val = assert ? SIRFUART_AFC_CTRL_RX_THD : 0x0;
+	unsigned int current_val;
+	if (sirfport->hw_flow_ctrl) {
+		current_val = rd_regl(port, SIRFUART_AFC_CTRL) & ~0xFF;
+		val |= current_val;
+		wr_regl(port, SIRFUART_AFC_CTRL, val);
+	}
+}
+
+static void sirfsoc_uart_stop_tx(struct uart_port *port)
+{
+	unsigned int regv;
+	regv = rd_regl(port, SIRFUART_INT_EN);
+	wr_regl(port, SIRFUART_INT_EN, regv & ~SIRFUART_TX_INT_EN);
+}
+
+void sirfsoc_uart_start_tx(struct uart_port *port)
+{
+	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
+	unsigned long regv;
+	sirfsoc_uart_pio_tx_chars(sirfport, 1);
+	wr_regl(port, SIRFUART_TX_FIFO_OP, SIRFUART_TX_FIFO_START);
+	regv = rd_regl(port, SIRFUART_INT_EN);
+	wr_regl(port, SIRFUART_INT_EN, regv | SIRFUART_TX_INT_EN);
+}
+
+static void sirfsoc_uart_stop_rx(struct uart_port *port)
+{
+	unsigned long regv;
+	wr_regl(port, SIRFUART_RX_FIFO_OP, 0);
+	regv = rd_regl(port, SIRFUART_INT_EN);
+	wr_regl(port, SIRFUART_INT_EN, regv & ~SIRFUART_RX_IO_INT_EN);
+}
+
+static void sirfsoc_uart_disable_ms(struct uart_port *port)
+{
+	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
+	unsigned long reg;
+	sirfport->ms_enabled = 0;
+	if (!sirfport->hw_flow_ctrl)
+		return;
+	reg = rd_regl(port, SIRFUART_AFC_CTRL);
+	wr_regl(port, SIRFUART_AFC_CTRL, reg & ~0x3FF);
+	reg = rd_regl(port, SIRFUART_INT_EN);
+	wr_regl(port, SIRFUART_INT_EN, reg & ~SIRFUART_CTS_INT_EN);
+}
+
+static void sirfsoc_uart_enable_ms(struct uart_port *port)
+{
+	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
+	unsigned long reg;
+	unsigned long flg;
+	if (!sirfport->hw_flow_ctrl)
+		return;
+	flg = SIRFUART_AFC_RX_EN | SIRFUART_AFC_TX_EN;
+	reg = rd_regl(port, SIRFUART_AFC_CTRL);
+	wr_regl(port, SIRFUART_AFC_CTRL, reg | flg);
+	reg = rd_regl(port, SIRFUART_INT_EN);
+	wr_regl(port, SIRFUART_INT_EN, reg | SIRFUART_CTS_INT_EN);
+	uart_handle_cts_change(port,
+		!(rd_regl(port, SIRFUART_AFC_CTRL) & SIRFUART_CTS_IN_STATUS));
+	sirfport->ms_enabled = 1;
+}
+
+static void sirfsoc_uart_break_ctl(struct uart_port *port, int break_state)
+{
+	unsigned long ulcon = rd_regl(port, SIRFUART_LINE_CTRL);
+	if (break_state)
+		ulcon |= SIRFUART_SET_BREAK;
+	else
+		ulcon &= ~SIRFUART_SET_BREAK;
+	wr_regl(port, SIRFUART_LINE_CTRL, ulcon);
+}
+
+static unsigned int
+sirfsoc_uart_pio_rx_chars(struct uart_port *port, unsigned int max_rx_count)
+{
+	unsigned int ch, rx_count = 0;
+	struct tty_struct *tty;
+
+	tty = tty_port_tty_get(&port->state->port);
+	if (!tty)
+		return -ENODEV;
+
+	while (!(rd_regl(port, SIRFUART_RX_FIFO_STATUS) &
+					SIRFUART_FIFOEMPTY_MASK(port))) {
+		ch = rd_regl(port, SIRFUART_RX_FIFO_DATA) | SIRFUART_DUMMY_READ;
+		if (unlikely(uart_handle_sysrq_char(port, ch)))
+			continue;
+		uart_insert_char(port, 0, 0, ch, TTY_NORMAL);
+		rx_count++;
+		if (rx_count >= max_rx_count)
+			break;
+	}
+
+	port->icount.rx += rx_count;
+	tty_flip_buffer_push(tty);
+	tty_kref_put(tty);
+
+	return rx_count;
+}
+
+static unsigned int
+sirfsoc_uart_pio_tx_chars(struct sirfsoc_uart_port *sirfport, int count)
+{
+	struct uart_port *port = &sirfport->port;
+	struct circ_buf *xmit = &port->state->xmit;
+	unsigned int num_tx = 0;
+	while (!uart_circ_empty(xmit) &&
+		!(rd_regl(port, SIRFUART_TX_FIFO_STATUS) &
+					SIRFUART_FIFOFULL_MASK(port)) &&
+		count--) {
+		wr_regl(port, SIRFUART_TX_FIFO_DATA, xmit->buf[xmit->tail]);
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		port->icount.tx++;
+		num_tx++;
+	}
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+	return num_tx;
+}
+
+static irqreturn_t sirfsoc_uart_isr(int irq, void *dev_id)
+{
+	unsigned long intr_status;
+	unsigned long cts_status;
+	unsigned long flag = TTY_NORMAL;
+	struct sirfsoc_uart_port *sirfport = (struct sirfsoc_uart_port *)dev_id;
+	struct uart_port *port = &sirfport->port;
+	struct uart_state *state = port->state;
+	struct circ_buf *xmit = &port->state->xmit;
+	intr_status = rd_regl(port, SIRFUART_INT_STATUS);
+	wr_regl(port, SIRFUART_INT_STATUS, intr_status);
+	intr_status &= rd_regl(port, SIRFUART_INT_EN);
+	if (unlikely(intr_status & (SIRFUART_ERR_INT_STAT))) {
+		if (intr_status & SIRFUART_RXD_BREAK) {
+			if (uart_handle_break(port))
+				goto recv_char;
+			uart_insert_char(port, intr_status,
+					SIRFUART_RX_OFLOW, 0, TTY_BREAK);
+			return IRQ_HANDLED;
+		}
+		if (intr_status & SIRFUART_RX_OFLOW)
+			port->icount.overrun++;
+		if (intr_status & SIRFUART_FRM_ERR) {
+			port->icount.frame++;
+			flag = TTY_FRAME;
+		}
+		if (intr_status & SIRFUART_PARITY_ERR)
+			flag = TTY_PARITY;
+		wr_regl(port, SIRFUART_RX_FIFO_OP, SIRFUART_RX_FIFO_RESET);
+		wr_regl(port, SIRFUART_RX_FIFO_OP, 0);
+		wr_regl(port, SIRFUART_RX_FIFO_OP, SIRFUART_RX_FIFO_START);
+		intr_status &= port->read_status_mask;
+		uart_insert_char(port, intr_status,
+					SIRFUART_RX_OFLOW_INT, 0, flag);
+	}
+recv_char:
+	if (intr_status & SIRFUART_CTS_INT_EN) {
+		cts_status = !(rd_regl(port, SIRFUART_AFC_CTRL) &
+							SIRFUART_CTS_IN_STATUS);
+		if (cts_status != 0) {
+			uart_handle_cts_change(port, 1);
+		} else {
+			uart_handle_cts_change(port, 0);
+			wake_up_interruptible(&state->port.delta_msr_wait);
+		}
+	}
+	if (intr_status & SIRFUART_RX_IO_INT_EN)
+		sirfsoc_uart_pio_rx_chars(port, SIRFSOC_UART_IO_RX_MAX_CNT);
+	if (intr_status & SIRFUART_TX_INT_EN) {
+		if (uart_circ_empty(xmit) || uart_tx_stopped(port)) {
+			return IRQ_HANDLED;
+		} else {
+			sirfsoc_uart_pio_tx_chars(sirfport,
+					SIRFSOC_UART_IO_TX_REASONABLE_CNT);
+			if ((uart_circ_empty(xmit)) &&
+				(rd_regl(port, SIRFUART_TX_FIFO_STATUS) &
+						SIRFUART_FIFOEMPTY_MASK(port)))
+				sirfsoc_uart_stop_tx(port);
+		}
+	}
+	return IRQ_HANDLED;
+}
+
+static void sirfsoc_uart_start_rx(struct uart_port *port)
+{
+	unsigned long regv;
+	regv = rd_regl(port, SIRFUART_INT_EN);
+	wr_regl(port, SIRFUART_INT_EN, regv | SIRFUART_RX_IO_INT_EN);
+	wr_regl(port, SIRFUART_RX_FIFO_OP, SIRFUART_RX_FIFO_RESET);
+	wr_regl(port, SIRFUART_RX_FIFO_OP, 0);
+	wr_regl(port, SIRFUART_RX_FIFO_OP, SIRFUART_RX_FIFO_START);
+}
+
+static unsigned int
+sirfsoc_calc_sample_div(unsigned long baud_rate,
+			unsigned long ioclk_rate, unsigned long *setted_baud)
+{
+	unsigned long min_delta = ~0UL;
+	unsigned short sample_div;
+	unsigned int regv = 0;
+	unsigned long ioclk_div;
+	unsigned long baud_tmp;
+	int temp_delta;
+
+	for (sample_div = SIRF_MIN_SAMPLE_DIV;
+			sample_div <= SIRF_MAX_SAMPLE_DIV; sample_div++) {
+		ioclk_div = (ioclk_rate / (baud_rate * (sample_div + 1))) - 1;
+		if (ioclk_div > SIRF_IOCLK_DIV_MAX)
+			continue;
+		baud_tmp = ioclk_rate / ((ioclk_div + 1) * (sample_div + 1));
+		temp_delta = baud_tmp - baud_rate;
+		temp_delta = (temp_delta > 0) ? temp_delta : -temp_delta;
+		if (temp_delta < min_delta) {
+			regv = regv & (~SIRF_IOCLK_DIV_MASK);
+			regv = regv | ioclk_div;
+			regv = regv & (~SIRF_SAMPLE_DIV_MASK);
+			regv = regv | (sample_div << SIRF_SAMPLE_DIV_SHIFT);
+			min_delta = temp_delta;
+			*setted_baud = baud_tmp;
+		}
+	}
+	return regv;
+}
+
+static void sirfsoc_uart_set_termios(struct uart_port *port,
+				       struct ktermios *termios,
+				       struct ktermios *old)
+{
+	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
+	unsigned long	ioclk_rate;
+	unsigned long	config_reg = 0;
+	unsigned long	baud_rate;
+	unsigned long	setted_baud;
+	unsigned long	flags;
+	unsigned long	ic;
+	unsigned int	clk_div_reg = 0;
+	unsigned long	temp_reg_val;
+	unsigned long	rx_time_out;
+	int		threshold_div;
+	int		temp;
+
+	ioclk_rate = 150000000;
+	switch (termios->c_cflag & CSIZE) {
+	default:
+	case CS8:
+		config_reg |= SIRFUART_DATA_BIT_LEN_8;
+		break;
+	case CS7:
+		config_reg |= SIRFUART_DATA_BIT_LEN_7;
+		break;
+	case CS6:
+		config_reg |= SIRFUART_DATA_BIT_LEN_6;
+		break;
+	case CS5:
+		config_reg |= SIRFUART_DATA_BIT_LEN_5;
+		break;
+	}
+	if (termios->c_cflag & CSTOPB)
+		config_reg |= SIRFUART_STOP_BIT_LEN_2;
+	baud_rate = uart_get_baud_rate(port, termios, old, 0, 4000000);
+	spin_lock_irqsave(&port->lock, flags);
+	port->read_status_mask = SIRFUART_RX_OFLOW_INT;
+	port->ignore_status_mask = 0;
+	/* read flags */
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |=
+			SIRFUART_FRM_ERR_INT | SIRFUART_PARITY_ERR_INT;
+	if (termios->c_iflag & (BRKINT | PARMRK))
+		port->read_status_mask |= SIRFUART_RXD_BREAK_INT;
+	/* ignore flags */
+	if (termios->c_iflag & IGNPAR)
+		port->ignore_status_mask |=
+			SIRFUART_FRM_ERR_INT | SIRFUART_PARITY_ERR_INT;
+	if ((termios->c_cflag & CREAD) == 0)
+		port->ignore_status_mask |= SIRFUART_DUMMY_READ;
+	/* enable parity if PARENB is set*/
+	if (termios->c_cflag & PARENB) {
+		if (termios->c_cflag & CMSPAR) {
+			if (termios->c_cflag & PARODD)
+				config_reg |= SIRFUART_STICK_BIT_MARK;
+			else
+				config_reg |= SIRFUART_STICK_BIT_SPACE;
+		} else if (termios->c_cflag & PARODD) {
+			config_reg |= SIRFUART_STICK_BIT_ODD;
+		} else {
+			config_reg |= SIRFUART_STICK_BIT_EVEN;
+		}
+	}
+	/* Hardware Flow Control Settings */
+	if (UART_ENABLE_MS(port, termios->c_cflag)) {
+		if (!sirfport->ms_enabled)
+			sirfsoc_uart_enable_ms(port);
+	} else {
+		if (sirfport->ms_enabled)
+			sirfsoc_uart_disable_ms(port);
+	}
+
+	/* common rate: fast calculation */
+	for (ic = 0; ic < SIRF_BAUD_RATE_SUPPORT_NR; ic++)
+		if (baud_rate == baudrate_to_regv[ic].baud_rate)
+			clk_div_reg = baudrate_to_regv[ic].reg_val;
+	setted_baud = baud_rate;
+	/* arbitary rate setting */
+	if (unlikely(clk_div_reg == 0))
+		clk_div_reg = sirfsoc_calc_sample_div(baud_rate, ioclk_rate,
+								&setted_baud);
+	wr_regl(port, SIRFUART_DIVISOR, clk_div_reg);
+
+	if (tty_termios_baud_rate(termios))
+		tty_termios_encode_baud_rate(termios, setted_baud, setted_baud);
+
+	/* set receive timeout */
+	rx_time_out = SIRFSOC_UART_RX_TIMEOUT(baud_rate, 20000);
+	rx_time_out = (rx_time_out > 0xFFFF) ? 0xFFFF : rx_time_out;
+	config_reg |= SIRFUART_RECV_TIMEOUT(rx_time_out);
+	temp_reg_val = rd_regl(port, SIRFUART_TX_FIFO_OP);
+	wr_regl(port, SIRFUART_RX_FIFO_OP, 0);
+	wr_regl(port, SIRFUART_TX_FIFO_OP,
+				temp_reg_val & ~SIRFUART_TX_FIFO_START);
+	wr_regl(port, SIRFUART_TX_DMA_IO_CTRL, SIRFUART_TX_MODE_IO);
+	wr_regl(port, SIRFUART_RX_DMA_IO_CTRL, SIRFUART_RX_MODE_IO);
+	wr_regl(port, SIRFUART_LINE_CTRL, config_reg);
+
+	/* Reset Rx/Tx FIFO Threshold level for proper baudrate */
+	if (baud_rate < 1000000)
+		threshold_div = 1;
+	else
+		threshold_div = 2;
+	temp = port->line == 1 ? 16 : 64;
+	wr_regl(port, SIRFUART_TX_FIFO_CTRL, temp / threshold_div);
+	wr_regl(port, SIRFUART_RX_FIFO_CTRL, temp / threshold_div);
+	temp_reg_val |= SIRFUART_TX_FIFO_START;
+	wr_regl(port, SIRFUART_TX_FIFO_OP, temp_reg_val);
+	uart_update_timeout(port, termios->c_cflag, baud_rate);
+	sirfsoc_uart_start_rx(port);
+	wr_regl(port, SIRFUART_TX_RX_EN, SIRFUART_TX_EN | SIRFUART_RX_EN);
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static void startup_uart_controller(struct uart_port *port)
+{
+	unsigned long temp_regv;
+	int temp;
+	temp_regv = rd_regl(port, SIRFUART_TX_DMA_IO_CTRL);
+	wr_regl(port, SIRFUART_TX_DMA_IO_CTRL, temp_regv | SIRFUART_TX_MODE_IO);
+	temp_regv = rd_regl(port, SIRFUART_RX_DMA_IO_CTRL);
+	wr_regl(port, SIRFUART_RX_DMA_IO_CTRL, temp_regv | SIRFUART_RX_MODE_IO);
+	wr_regl(port, SIRFUART_TX_DMA_IO_LEN, 0);
+	wr_regl(port, SIRFUART_RX_DMA_IO_LEN, 0);
+	wr_regl(port, SIRFUART_TX_RX_EN, SIRFUART_RX_EN | SIRFUART_TX_EN);
+	wr_regl(port, SIRFUART_TX_FIFO_OP, SIRFUART_TX_FIFO_RESET);
+	wr_regl(port, SIRFUART_TX_FIFO_OP, 0);
+	wr_regl(port, SIRFUART_RX_FIFO_OP, SIRFUART_RX_FIFO_RESET);
+	wr_regl(port, SIRFUART_RX_FIFO_OP, 0);
+	temp = port->line == 1 ? 16 : 64;
+	wr_regl(port, SIRFUART_TX_FIFO_CTRL, temp);
+	wr_regl(port, SIRFUART_RX_FIFO_CTRL, temp);
+}
+
+static int sirfsoc_uart_startup(struct uart_port *port)
+{
+	struct sirfsoc_uart_port *sirfport	= to_sirfport(port);
+	unsigned int index			= port->line;
+	int ret;
+	set_irq_flags(port->irq, IRQF_VALID | IRQF_NOAUTOEN);
+	ret = request_irq(port->irq,
+				sirfsoc_uart_isr,
+				0,
+				SIRFUART_PORT_NAME,
+				sirfport);
+	if (ret != 0) {
+		dev_err(port->dev, "UART%d request IRQ line (%d) failed.\n",
+							index, port->irq);
+		goto irq_err;
+	}
+	startup_uart_controller(port);
+	enable_irq(port->irq);
+irq_err:
+	return ret;
+}
+
+static void sirfsoc_uart_shutdown(struct uart_port *port)
+{
+	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
+	wr_regl(port, SIRFUART_INT_EN, 0);
+	free_irq(port->irq, sirfport);
+	if (sirfport->ms_enabled) {
+		sirfsoc_uart_disable_ms(port);
+		sirfport->ms_enabled = 0;
+	}
+}
+
+static const char *sirfsoc_uart_type(struct uart_port *port)
+{
+	return port->type == SIRFSOC_PORT_TYPE ? SIRFUART_PORT_NAME : NULL;
+}
+
+static int sirfsoc_uart_request_port(struct uart_port *port)
+{
+	void *ret;
+	ret = request_mem_region(port->mapbase,
+				SIRFUART_MAP_SIZE, SIRFUART_PORT_NAME);
+	return ret ? 0 : -EBUSY;
+}
+
+static void sirfsoc_uart_release_port(struct uart_port *port)
+{
+	release_mem_region(port->mapbase, SIRFUART_MAP_SIZE);
+}
+
+static void sirfsoc_uart_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE) {
+		port->type = SIRFSOC_PORT_TYPE;
+		sirfsoc_uart_request_port(port);
+	}
+}
+
+static struct uart_ops sirfsoc_uart_ops = {
+	.tx_empty	= sirfsoc_uart_tx_empty,
+	.get_mctrl	= sirfsoc_uart_get_mctrl,
+	.set_mctrl	= sirfsoc_uart_set_mctrl,
+	.stop_tx	= sirfsoc_uart_stop_tx,
+	.start_tx	= sirfsoc_uart_start_tx,
+	.stop_rx	= sirfsoc_uart_stop_rx,
+	.enable_ms	= sirfsoc_uart_enable_ms,
+	.break_ctl	= sirfsoc_uart_break_ctl,
+	.startup	= sirfsoc_uart_startup,
+	.shutdown	= sirfsoc_uart_shutdown,
+	.set_termios	= sirfsoc_uart_set_termios,
+	.type		= sirfsoc_uart_type,
+	.release_port	= sirfsoc_uart_release_port,
+	.request_port	= sirfsoc_uart_request_port,
+	.config_port	= sirfsoc_uart_config_port,
+};
+
+#ifdef CONFIG_SERIAL_SIRFSOC_CONSOLE
+static int __init sirfsoc_uart_console_setup(struct console *co, char *options)
+{
+	unsigned int baud = 115200;
+	unsigned int bits = 8;
+	unsigned int parity = 'n';
+	unsigned int flow = 'n';
+	struct uart_port *port = &sirfsoc_uart_ports[co->index].port;
+
+	if (co->index < 0 || co->index >= SIRFSOC_UART_NR)
+		return -EINVAL;
+
+	if (!port->mapbase)
+		return -ENODEV;
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+	port->cons = co;
+	return uart_set_options(port, co, baud, parity, bits, flow);
+}
+
+static void sirfsoc_uart_console_putchar(struct uart_port *port, int ch)
+{
+	while (rd_regl(port,
+		SIRFUART_TX_FIFO_STATUS) & SIRFUART_FIFOFULL_MASK(port))
+		cpu_relax();
+	wr_regb(port, SIRFUART_TX_FIFO_DATA, ch);
+}
+
+static void sirfsoc_uart_console_write(struct console *co, const char *s,
+							unsigned int count)
+{
+	struct uart_port *port = &sirfsoc_uart_ports[co->index].port;
+	uart_console_write(port, s, count, sirfsoc_uart_console_putchar);
+}
+
+static struct console sirfsoc_uart_console = {
+	.name		= SIRFSOC_UART_NAME,
+	.device		= uart_console_device,
+	.flags		= CON_PRINTBUFFER,
+	.index		= -1,
+	.write		= sirfsoc_uart_console_write,
+	.setup		= sirfsoc_uart_console_setup,
+	.data           = &sirfsoc_uart_drv,
+};
+
+static int __init sirfsoc_uart_console_init(void)
+{
+	register_console(&sirfsoc_uart_console);
+	return 0;
+}
+console_initcall(sirfsoc_uart_console_init);
+#endif
+
+static struct uart_driver sirfsoc_uart_drv = {
+	.owner		= THIS_MODULE,
+	.driver_name	= SIRFUART_PORT_NAME,
+	.nr		= SIRFSOC_UART_NR,
+	.dev_name	= SIRFSOC_UART_NAME,
+	.major		= SIRFSOC_UART_MAJOR,
+	.minor		= SIRFSOC_UART_MINOR,
+#ifdef CONFIG_SERIAL_SIRFSOC_CONSOLE
+	.cons			= &sirfsoc_uart_console,
+#else
+	.cons			= NULL,
+#endif
+};
+
+int sirfsoc_uart_probe(struct platform_device *pdev)
+{
+	struct sirfsoc_uart_port *sirfport;
+	struct uart_port *port;
+	struct resource *res;
+	int ret;
+
+	if (of_property_read_u32(pdev->dev.of_node, "cell-index", &pdev->id)) {
+		dev_err(&pdev->dev,
+			"Unable to find cell-index in uart node.\n");
+		ret = -EFAULT;
+		goto err;
+	}
+
+	sirfport = &sirfsoc_uart_ports[pdev->id];
+	port = &sirfport->port;
+	port->dev = &pdev->dev;
+	port->private_data = sirfport;
+
+	if (of_find_property(pdev->dev.of_node, "hw_flow_ctrl", NULL))
+		sirfport->hw_flow_ctrl = 1;
+
+	if (of_property_read_u32(pdev->dev.of_node,
+			"fifosize",
+			&port->fifosize)) {
+		dev_err(&pdev->dev,
+			"Unable to find fifosize in uart node.\n");
+		ret = -EFAULT;
+		goto err;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "Insufficient resources.\n");
+		ret = -EFAULT;
+		goto err;
+	}
+	port->mapbase = res->start;
+	port->membase = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!port->membase) {
+		dev_err(&pdev->dev, "Cannot remap resource.\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "Insufficient resources.\n");
+		ret = -EFAULT;
+		goto irq_err;
+	}
+	port->irq = res->start;
+
+	if (sirfport->hw_flow_ctrl) {
+		sirfport->pmx = pinmux_get(&pdev->dev, NULL);
+		ret = IS_ERR(sirfport->pmx);
+		if (ret)
+			goto pmx_err;
+
+		pinmux_enable(sirfport->pmx);
+	}
+
+	port->ops = &sirfsoc_uart_ops;
+	spin_lock_init(&port->lock);
+
+	platform_set_drvdata(pdev, sirfport);
+	ret = uart_add_one_port(&sirfsoc_uart_drv, port);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "Cannot add UART port(%d).\n", pdev->id);
+		goto port_err;
+	}
+
+	return 0;
+
+port_err:
+	platform_set_drvdata(pdev, NULL);
+	if (sirfport->hw_flow_ctrl) {
+		pinmux_disable(sirfport->pmx);
+		pinmux_put(sirfport->pmx);
+	}
+pmx_err:
+irq_err:
+	devm_iounmap(&pdev->dev, port->membase);
+err:
+	return ret;
+}
+
+static int sirfsoc_uart_remove(struct platform_device *pdev)
+{
+	struct sirfsoc_uart_port *sirfport = platform_get_drvdata(pdev);
+	struct uart_port *port = &sirfport->port;
+	platform_set_drvdata(pdev, NULL);
+	if (sirfport->hw_flow_ctrl) {
+		pinmux_disable(sirfport->pmx);
+		pinmux_put(sirfport->pmx);
+	}
+	devm_iounmap(&pdev->dev, port->membase);
+	uart_remove_one_port(&sirfsoc_uart_drv, port);
+	return 0;
+}
+
+static int
+sirfsoc_uart_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct sirfsoc_uart_port *sirfport = platform_get_drvdata(pdev);
+	struct uart_port *port = &sirfport->port;
+	uart_suspend_port(&sirfsoc_uart_drv, port);
+	return 0;
+}
+
+static int sirfsoc_uart_resume(struct platform_device *pdev)
+{
+	struct sirfsoc_uart_port *sirfport = platform_get_drvdata(pdev);
+	struct uart_port *port = &sirfport->port;
+	uart_resume_port(&sirfsoc_uart_drv, port);
+	return 0;
+}
+
+static struct of_device_id sirfsoc_uart_ids[] __devinitdata = {
+	{ .compatible = "sirf,prima2-uart", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sirfsoc_serial_of_match);
+
+static struct platform_driver sirfsoc_uart_driver = {
+	.probe		= sirfsoc_uart_probe,
+	.remove		= __devexit_p(sirfsoc_uart_remove),
+	.suspend	= sirfsoc_uart_suspend,
+	.resume		= sirfsoc_uart_resume,
+	.driver		= {
+		.name	= SIRFUART_PORT_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = sirfsoc_uart_ids,
+	},
+};
+
+static int __init sirfsoc_uart_init(void)
+{
+	int ret = 0;
+
+	ret = uart_register_driver(&sirfsoc_uart_drv);
+	if (ret)
+		goto out;
+
+	ret = platform_driver_register(&sirfsoc_uart_driver);
+	if (ret)
+		uart_unregister_driver(&sirfsoc_uart_drv);
+out:
+	return ret;
+}
+module_init(sirfsoc_uart_init);
+
+static void __exit sirfsoc_uart_exit(void)
+{
+	platform_driver_unregister(&sirfsoc_uart_driver);
+	uart_unregister_driver(&sirfsoc_uart_drv);
+}
+module_exit(sirfsoc_uart_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Bin Shi <Bin.Shi@csr.com>, Rong Wang<Rong.Wang@csr.com>");
+MODULE_DESCRIPTION("CSR SiRFprimaII Uart Driver");
diff --git a/drivers/tty/serial/sirfsoc_uart.h b/drivers/tty/serial/sirfsoc_uart.h
new file mode 100644
index 000000000000..fc64260fa93c
--- /dev/null
+++ b/drivers/tty/serial/sirfsoc_uart.h
@@ -0,0 +1,185 @@
+/*
+ * Drivers for CSR SiRFprimaII onboard UARTs.
+ *
+ * Copyright (c) 2011 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+#include <linux/bitops.h>
+
+/* UART Register Offset Define */
+#define SIRFUART_LINE_CTRL			0x0040
+#define SIRFUART_TX_RX_EN			0x004c
+#define SIRFUART_DIVISOR			0x0050
+#define SIRFUART_INT_EN				0x0054
+#define SIRFUART_INT_STATUS			0x0058
+#define SIRFUART_TX_DMA_IO_CTRL			0x0100
+#define SIRFUART_TX_DMA_IO_LEN			0x0104
+#define SIRFUART_TX_FIFO_CTRL			0x0108
+#define SIRFUART_TX_FIFO_LEVEL_CHK		0x010C
+#define SIRFUART_TX_FIFO_OP			0x0110
+#define SIRFUART_TX_FIFO_STATUS			0x0114
+#define SIRFUART_TX_FIFO_DATA			0x0118
+#define SIRFUART_RX_DMA_IO_CTRL			0x0120
+#define SIRFUART_RX_DMA_IO_LEN			0x0124
+#define SIRFUART_RX_FIFO_CTRL			0x0128
+#define SIRFUART_RX_FIFO_LEVEL_CHK		0x012C
+#define SIRFUART_RX_FIFO_OP			0x0130
+#define SIRFUART_RX_FIFO_STATUS			0x0134
+#define SIRFUART_RX_FIFO_DATA			0x0138
+#define SIRFUART_AFC_CTRL			0x0140
+#define SIRFUART_SWH_DMA_IO			0x0148
+
+/* UART Line Control Register */
+#define SIRFUART_DATA_BIT_LEN_MASK		0x3
+#define SIRFUART_DATA_BIT_LEN_5			BIT(0)
+#define SIRFUART_DATA_BIT_LEN_6			1
+#define SIRFUART_DATA_BIT_LEN_7			2
+#define SIRFUART_DATA_BIT_LEN_8			3
+#define SIRFUART_STOP_BIT_LEN_1			0
+#define SIRFUART_STOP_BIT_LEN_2			BIT(2)
+#define SIRFUART_PARITY_EN			BIT(3)
+#define SIRFUART_EVEN_BIT			BIT(4)
+#define SIRFUART_STICK_BIT_MASK			(7 << 3)
+#define SIRFUART_STICK_BIT_NONE			(0 << 3)
+#define SIRFUART_STICK_BIT_EVEN			BIT(3)
+#define SIRFUART_STICK_BIT_ODD			(3 << 3)
+#define SIRFUART_STICK_BIT_MARK			(5 << 3)
+#define SIRFUART_STICK_BIT_SPACE		(7 << 3)
+#define SIRFUART_SET_BREAK			BIT(6)
+#define SIRFUART_LOOP_BACK			BIT(7)
+#define SIRFUART_PARITY_MASK			(7 << 3)
+#define SIRFUART_DUMMY_READ			BIT(16)
+
+#define SIRFSOC_UART_RX_TIMEOUT(br, to)	(((br) * (((to) + 999) / 1000)) / 1000)
+#define SIRFUART_RECV_TIMEOUT_MASK	(0xFFFF << 16)
+#define SIRFUART_RECV_TIMEOUT(x)	(((x) & 0xFFFF) << 16)
+
+/* UART Auto Flow Control */
+#define SIRFUART_AFC_RX_THD_MASK		0x000000FF
+#define SIRFUART_AFC_RX_EN			BIT(8)
+#define SIRFUART_AFC_TX_EN			BIT(9)
+#define SIRFUART_CTS_CTRL			BIT(10)
+#define SIRFUART_RTS_CTRL			BIT(11)
+#define SIRFUART_CTS_IN_STATUS			BIT(12)
+#define SIRFUART_RTS_OUT_STATUS			BIT(13)
+
+/* UART Interrupt Enable Register */
+#define SIRFUART_RX_DONE_INT			BIT(0)
+#define SIRFUART_TX_DONE_INT			BIT(1)
+#define SIRFUART_RX_OFLOW_INT			BIT(2)
+#define SIRFUART_TX_ALLOUT_INT			BIT(3)
+#define SIRFUART_RX_IO_DMA_INT			BIT(4)
+#define SIRFUART_TX_IO_DMA_INT			BIT(5)
+#define SIRFUART_RXFIFO_FULL_INT		BIT(6)
+#define SIRFUART_TXFIFO_EMPTY_INT		BIT(7)
+#define SIRFUART_RXFIFO_THD_INT			BIT(8)
+#define SIRFUART_TXFIFO_THD_INT			BIT(9)
+#define SIRFUART_FRM_ERR_INT			BIT(10)
+#define SIRFUART_RXD_BREAK_INT			BIT(11)
+#define SIRFUART_RX_TIMEOUT_INT			BIT(12)
+#define SIRFUART_PARITY_ERR_INT			BIT(13)
+#define SIRFUART_CTS_INT_EN			BIT(14)
+#define SIRFUART_RTS_INT_EN			BIT(15)
+
+/* UART Interrupt Status Register */
+#define SIRFUART_RX_DONE			BIT(0)
+#define SIRFUART_TX_DONE			BIT(1)
+#define SIRFUART_RX_OFLOW			BIT(2)
+#define SIRFUART_TX_ALL_EMPTY			BIT(3)
+#define SIRFUART_DMA_IO_RX_DONE			BIT(4)
+#define SIRFUART_DMA_IO_TX_DONE			BIT(5)
+#define SIRFUART_RXFIFO_FULL			BIT(6)
+#define SIRFUART_TXFIFO_EMPTY			BIT(7)
+#define SIRFUART_RXFIFO_THD_REACH		BIT(8)
+#define SIRFUART_TXFIFO_THD_REACH		BIT(9)
+#define SIRFUART_FRM_ERR			BIT(10)
+#define SIRFUART_RXD_BREAK			BIT(11)
+#define SIRFUART_RX_TIMEOUT			BIT(12)
+#define SIRFUART_PARITY_ERR			BIT(13)
+#define SIRFUART_CTS_CHANGE			BIT(14)
+#define SIRFUART_RTS_CHANGE			BIT(15)
+#define SIRFUART_PLUG_IN			BIT(16)
+
+#define SIRFUART_ERR_INT_STAT					\
+				(SIRFUART_RX_OFLOW |		\
+				SIRFUART_FRM_ERR |		\
+				SIRFUART_RXD_BREAK |		\
+				SIRFUART_PARITY_ERR)
+#define SIRFUART_ERR_INT_EN					\
+				(SIRFUART_RX_OFLOW_INT |	\
+				SIRFUART_FRM_ERR_INT |		\
+				SIRFUART_RXD_BREAK_INT |	\
+				SIRFUART_PARITY_ERR_INT)
+#define SIRFUART_TX_INT_EN	SIRFUART_TXFIFO_EMPTY_INT
+#define SIRFUART_RX_IO_INT_EN					\
+				(SIRFUART_RX_TIMEOUT_INT |	\
+				SIRFUART_RXFIFO_THD_INT |	\
+				SIRFUART_RXFIFO_FULL_INT |	\
+				SIRFUART_ERR_INT_EN)
+
+/* UART FIFO Register */
+#define SIRFUART_TX_FIFO_STOP			0x0
+#define SIRFUART_TX_FIFO_RESET			0x1
+#define SIRFUART_TX_FIFO_START			0x2
+#define SIRFUART_RX_FIFO_STOP			0x0
+#define SIRFUART_RX_FIFO_RESET			0x1
+#define SIRFUART_RX_FIFO_START			0x2
+#define SIRFUART_TX_MODE_DMA			0
+#define SIRFUART_TX_MODE_IO			1
+#define SIRFUART_RX_MODE_DMA			0
+#define SIRFUART_RX_MODE_IO			1
+
+#define SIRFUART_RX_EN				0x1
+#define SIRFUART_TX_EN				0x2
+
+/* Generic Definitions */
+#define SIRFSOC_UART_NAME			"ttySiRF"
+#define SIRFSOC_UART_MAJOR			0
+#define SIRFSOC_UART_MINOR			0
+#define SIRFUART_PORT_NAME			"sirfsoc-uart"
+#define SIRFUART_MAP_SIZE			0x200
+#define SIRFSOC_UART_NR				3
+#define SIRFSOC_PORT_TYPE			0xa5
+
+/* Baud Rate Calculation */
+#define SIRF_MIN_SAMPLE_DIV			0xf
+#define SIRF_MAX_SAMPLE_DIV			0x3f
+#define SIRF_IOCLK_DIV_MAX			0xffff
+#define SIRF_SAMPLE_DIV_SHIFT			16
+#define SIRF_IOCLK_DIV_MASK			0xffff
+#define SIRF_SAMPLE_DIV_MASK			0x3f0000
+#define SIRF_BAUD_RATE_SUPPORT_NR		18
+
+/* For Fast Baud Rate Calculation */
+struct sirfsoc_baudrate_to_regv {
+	unsigned int baud_rate;
+	unsigned int reg_val;
+};
+
+struct sirfsoc_uart_port {
+	unsigned char			hw_flow_ctrl;
+	unsigned char			ms_enabled;
+
+	struct uart_port		port;
+	struct pinmux			*pmx;
+};
+
+/* Hardware Flow Control */
+#define SIRFUART_AFC_CTRL_RX_THD	0x70
+
+/* Register Access Control */
+#define portaddr(port, reg)		((port)->membase + (reg))
+#define rd_regb(port, reg)		(__raw_readb(portaddr(port, reg)))
+#define rd_regl(port, reg)		(__raw_readl(portaddr(port, reg)))
+#define wr_regb(port, reg, val)		__raw_writeb(val, portaddr(port, reg))
+#define wr_regl(port, reg, val)		__raw_writel(val, portaddr(port, reg))
+
+/* UART Port Mask */
+#define SIRFUART_FIFOLEVEL_MASK(port)	((port->line == 1) ? (0x1f) : (0x7f))
+#define SIRFUART_FIFOFULL_MASK(port)	((port->line == 1) ? (0x20) : (0x80))
+#define SIRFUART_FIFOEMPTY_MASK(port)	((port->line == 1) ? (0x40) : (0x100))
+
+/* I/O Mode */
+#define SIRFSOC_UART_IO_RX_MAX_CNT		256
+#define SIRFSOC_UART_IO_TX_REASONABLE_CNT	6
diff --git a/drivers/tty/serial/timbuart.c b/drivers/tty/serial/timbuart.c
index e76c8b747fb8..70f97491d8f2 100644
--- a/drivers/tty/serial/timbuart.c
+++ b/drivers/tty/serial/timbuart.c
@@ -513,20 +513,7 @@ static struct platform_driver timbuart_platform_driver = {
 	.remove		= __devexit_p(timbuart_remove),
 };
 
-/*--------------------------------------------------------------------------*/
-
-static int __init timbuart_init(void)
-{
-	return platform_driver_register(&timbuart_platform_driver);
-}
-
-static void __exit timbuart_exit(void)
-{
-	platform_driver_unregister(&timbuart_platform_driver);
-}
-
-module_init(timbuart_init);
-module_exit(timbuart_exit);
+module_platform_driver(timbuart_platform_driver);
 
 MODULE_DESCRIPTION("Timberdale UART driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/vr41xx_siu.c b/drivers/tty/serial/vr41xx_siu.c
index 3beb6ab4fa68..83148e79ca13 100644
--- a/drivers/tty/serial/vr41xx_siu.c
+++ b/drivers/tty/serial/vr41xx_siu.c
@@ -961,18 +961,7 @@ static struct platform_driver siu_device_driver = {
 	},
 };
 
-static int __init vr41xx_siu_init(void)
-{
-	return platform_driver_register(&siu_device_driver);
-}
-
-static void __exit vr41xx_siu_exit(void)
-{
-	platform_driver_unregister(&siu_device_driver);
-}
-
-module_init(vr41xx_siu_init);
-module_exit(vr41xx_siu_exit);
+module_platform_driver(siu_device_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:SIU");
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index 3fdebd306b94..e41b9bbc107d 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -790,19 +790,24 @@ static void session_clear_tty(struct pid *session)
 void disassociate_ctty(int on_exit)
 {
 	struct tty_struct *tty;
-	struct pid *tty_pgrp = NULL;
 
 	if (!current->signal->leader)
 		return;
 
 	tty = get_current_tty();
 	if (tty) {
-		tty_pgrp = get_pid(tty->pgrp);
+		struct pid *tty_pgrp = get_pid(tty->pgrp);
 		if (on_exit) {
 			if (tty->driver->type != TTY_DRIVER_TYPE_PTY)
 				tty_vhangup(tty);
 		}
 		tty_kref_put(tty);
+		if (tty_pgrp) {
+			kill_pgrp(tty_pgrp, SIGHUP, on_exit);
+			if (!on_exit)
+				kill_pgrp(tty_pgrp, SIGCONT, on_exit);
+			put_pid(tty_pgrp);
+		}
 	} else if (on_exit) {
 		struct pid *old_pgrp;
 		spin_lock_irq(&current->sighand->siglock);
@@ -816,12 +821,6 @@ void disassociate_ctty(int on_exit)
 		}
 		return;
 	}
-	if (tty_pgrp) {
-		kill_pgrp(tty_pgrp, SIGHUP, on_exit);
-		if (!on_exit)
-			kill_pgrp(tty_pgrp, SIGCONT, on_exit);
-		put_pid(tty_pgrp);
-	}
 
 	spin_lock_irq(&current->sighand->siglock);
 	put_pid(current->signal->tty_old_pgrp);
@@ -1558,6 +1557,59 @@ static void release_tty(struct tty_struct *tty, int idx)
 }
 
 /**
+ *	tty_release_checks - check a tty before real release
+ *	@tty: tty to check
+ *	@o_tty: link of @tty (if any)
+ *	@idx: index of the tty
+ *
+ *	Performs some paranoid checking before true release of the @tty.
+ *	This is a no-op unless TTY_PARANOIA_CHECK is defined.
+ */
+static int tty_release_checks(struct tty_struct *tty, struct tty_struct *o_tty,
+		int idx)
+{
+#ifdef TTY_PARANOIA_CHECK
+	if (idx < 0 || idx >= tty->driver->num) {
+		printk(KERN_DEBUG "%s: bad idx when trying to free (%s)\n",
+				__func__, tty->name);
+		return -1;
+	}
+
+	/* not much to check for devpts */
+	if (tty->driver->flags & TTY_DRIVER_DEVPTS_MEM)
+		return 0;
+
+	if (tty != tty->driver->ttys[idx]) {
+		printk(KERN_DEBUG "%s: driver.table[%d] not tty for (%s)\n",
+				__func__, idx, tty->name);
+		return -1;
+	}
+	if (tty->termios != tty->driver->termios[idx]) {
+		printk(KERN_DEBUG "%s: driver.termios[%d] not termios for (%s)\n",
+				__func__, idx, tty->name);
+		return -1;
+	}
+	if (tty->driver->other) {
+		if (o_tty != tty->driver->other->ttys[idx]) {
+			printk(KERN_DEBUG "%s: other->table[%d] not o_tty for (%s)\n",
+					__func__, idx, tty->name);
+			return -1;
+		}
+		if (o_tty->termios != tty->driver->other->termios[idx]) {
+			printk(KERN_DEBUG "%s: other->termios[%d] not o_termios for (%s)\n",
+					__func__, idx, tty->name);
+			return -1;
+		}
+		if (o_tty->link != tty) {
+			printk(KERN_DEBUG "%s: bad pty pointers\n", __func__);
+			return -1;
+		}
+	}
+#endif
+	return 0;
+}
+
+/**
  *	tty_release		-	vfs callback for close
  *	@inode: inode of tty
  *	@filp: file pointer for handle to tty
@@ -1585,11 +1637,11 @@ int tty_release(struct inode *inode, struct file *filp)
 	int	idx;
 	char	buf[64];
 
-	if (tty_paranoia_check(tty, inode, "tty_release_dev"))
+	if (tty_paranoia_check(tty, inode, __func__))
 		return 0;
 
 	tty_lock();
-	check_tty_count(tty, "tty_release_dev");
+	check_tty_count(tty, __func__);
 
 	__tty_fasync(-1, filp, 0);
 
@@ -1599,59 +1651,16 @@ int tty_release(struct inode *inode, struct file *filp)
 	devpts = (tty->driver->flags & TTY_DRIVER_DEVPTS_MEM) != 0;
 	o_tty = tty->link;
 
-#ifdef TTY_PARANOIA_CHECK
-	if (idx < 0 || idx >= tty->driver->num) {
-		printk(KERN_DEBUG "tty_release_dev: bad idx when trying to "
-				  "free (%s)\n", tty->name);
+	if (tty_release_checks(tty, o_tty, idx)) {
 		tty_unlock();
 		return 0;
 	}
-	if (!devpts) {
-		if (tty != tty->driver->ttys[idx]) {
-			tty_unlock();
-			printk(KERN_DEBUG "tty_release_dev: driver.table[%d] not tty "
-			       "for (%s)\n", idx, tty->name);
-			return 0;
-		}
-		if (tty->termios != tty->driver->termios[idx]) {
-			tty_unlock();
-			printk(KERN_DEBUG "tty_release_dev: driver.termios[%d] not termios "
-			       "for (%s)\n",
-			       idx, tty->name);
-			return 0;
-		}
-	}
-#endif
 
 #ifdef TTY_DEBUG_HANGUP
-	printk(KERN_DEBUG "tty_release_dev of %s (tty count=%d)...",
-	       tty_name(tty, buf), tty->count);
+	printk(KERN_DEBUG "%s: %s (tty count=%d)...\n", __func__,
+			tty_name(tty, buf), tty->count);
 #endif
 
-#ifdef TTY_PARANOIA_CHECK
-	if (tty->driver->other &&
-	     !(tty->driver->flags & TTY_DRIVER_DEVPTS_MEM)) {
-		if (o_tty != tty->driver->other->ttys[idx]) {
-			tty_unlock();
-			printk(KERN_DEBUG "tty_release_dev: other->table[%d] "
-					  "not o_tty for (%s)\n",
-			       idx, tty->name);
-			return 0 ;
-		}
-		if (o_tty->termios != tty->driver->other->termios[idx]) {
-			tty_unlock();
-			printk(KERN_DEBUG "tty_release_dev: other->termios[%d] "
-					  "not o_termios for (%s)\n",
-			       idx, tty->name);
-			return 0;
-		}
-		if (o_tty->link != tty) {
-			tty_unlock();
-			printk(KERN_DEBUG "tty_release_dev: bad pty pointers\n");
-			return 0;
-		}
-	}
-#endif
 	if (tty->ops->close)
 		tty->ops->close(tty, filp);
 
@@ -1707,8 +1716,8 @@ int tty_release(struct inode *inode, struct file *filp)
 		if (!do_sleep)
 			break;
 
-		printk(KERN_WARNING "tty_release_dev: %s: read/write wait queue "
-				    "active!\n", tty_name(tty, buf));
+		printk(KERN_WARNING "%s: %s: read/write wait queue active!\n",
+				__func__, tty_name(tty, buf));
 		tty_unlock();
 		mutex_unlock(&tty_mutex);
 		schedule();
@@ -1721,15 +1730,14 @@ int tty_release(struct inode *inode, struct file *filp)
 	 */
 	if (pty_master) {
 		if (--o_tty->count < 0) {
-			printk(KERN_WARNING "tty_release_dev: bad pty slave count "
-					    "(%d) for %s\n",
-			       o_tty->count, tty_name(o_tty, buf));
+			printk(KERN_WARNING "%s: bad pty slave count (%d) for %s\n",
+				__func__, o_tty->count, tty_name(o_tty, buf));
 			o_tty->count = 0;
 		}
 	}
 	if (--tty->count < 0) {
-		printk(KERN_WARNING "tty_release_dev: bad tty->count (%d) for %s\n",
-		       tty->count, tty_name(tty, buf));
+		printk(KERN_WARNING "%s: bad tty->count (%d) for %s\n",
+				__func__, tty->count, tty_name(tty, buf));
 		tty->count = 0;
 	}
 
@@ -1778,7 +1786,7 @@ int tty_release(struct inode *inode, struct file *filp)
 	}
 
 #ifdef TTY_DEBUG_HANGUP
-	printk(KERN_DEBUG "freeing tty structure...");
+	printk(KERN_DEBUG "%s: freeing tty structure...\n", __func__);
 #endif
 	/*
 	 * Ask the line discipline code to release its structures
@@ -1798,6 +1806,83 @@ int tty_release(struct inode *inode, struct file *filp)
 }
 
 /**
+ *	tty_open_current_tty - get tty of current task for open
+ *	@device: device number
+ *	@filp: file pointer to tty
+ *	@return: tty of the current task iff @device is /dev/tty
+ *
+ *	We cannot return driver and index like for the other nodes because
+ *	devpts will not work then. It expects inodes to be from devpts FS.
+ */
+static struct tty_struct *tty_open_current_tty(dev_t device, struct file *filp)
+{
+	struct tty_struct *tty;
+
+	if (device != MKDEV(TTYAUX_MAJOR, 0))
+		return NULL;
+
+	tty = get_current_tty();
+	if (!tty)
+		return ERR_PTR(-ENXIO);
+
+	filp->f_flags |= O_NONBLOCK; /* Don't let /dev/tty block */
+	/* noctty = 1; */
+	tty_kref_put(tty);
+	/* FIXME: we put a reference and return a TTY! */
+	return tty;
+}
+
+/**
+ *	tty_lookup_driver - lookup a tty driver for a given device file
+ *	@device: device number
+ *	@filp: file pointer to tty
+ *	@noctty: set if the device should not become a controlling tty
+ *	@index: index for the device in the @return driver
+ *	@return: driver for this inode (with increased refcount)
+ *
+ * 	If @return is not erroneous, the caller is responsible to decrement the
+ * 	refcount by tty_driver_kref_put.
+ *
+ *	Locking: tty_mutex protects get_tty_driver
+ */
+static struct tty_driver *tty_lookup_driver(dev_t device, struct file *filp,
+		int *noctty, int *index)
+{
+	struct tty_driver *driver;
+
+	switch (device) {
+#ifdef CONFIG_VT
+	case MKDEV(TTY_MAJOR, 0): {
+		extern struct tty_driver *console_driver;
+		driver = tty_driver_kref_get(console_driver);
+		*index = fg_console;
+		*noctty = 1;
+		break;
+	}
+#endif
+	case MKDEV(TTYAUX_MAJOR, 1): {
+		struct tty_driver *console_driver = console_device(index);
+		if (console_driver) {
+			driver = tty_driver_kref_get(console_driver);
+			if (driver) {
+				/* Don't let /dev/console block */
+				filp->f_flags |= O_NONBLOCK;
+				*noctty = 1;
+				break;
+			}
+		}
+		return ERR_PTR(-ENODEV);
+	}
+	default:
+		driver = get_tty_driver(device, index);
+		if (!driver)
+			return ERR_PTR(-ENODEV);
+		break;
+	}
+	return driver;
+}
+
+/**
  *	tty_open		-	open a tty device
  *	@inode: inode of device file
  *	@filp: file pointer to tty
@@ -1813,16 +1898,16 @@ int tty_release(struct inode *inode, struct file *filp)
  *	The termios state of a pty is reset on first open so that
  *	settings don't persist across reuse.
  *
- *	Locking: tty_mutex protects tty, get_tty_driver and tty_init_dev work.
+ *	Locking: tty_mutex protects tty, tty_lookup_driver and tty_init_dev.
  *		 tty->count should protect the rest.
  *		 ->siglock protects ->signal/->sighand
  */
 
 static int tty_open(struct inode *inode, struct file *filp)
 {
-	struct tty_struct *tty = NULL;
+	struct tty_struct *tty;
 	int noctty, retval;
-	struct tty_driver *driver;
+	struct tty_driver *driver = NULL;
 	int index;
 	dev_t device = inode->i_rdev;
 	unsigned saved_flags = filp->f_flags;
@@ -1841,66 +1926,22 @@ retry_open:
 	mutex_lock(&tty_mutex);
 	tty_lock();
 
-	if (device == MKDEV(TTYAUX_MAJOR, 0)) {
-		tty = get_current_tty();
-		if (!tty) {
-			tty_unlock();
-			mutex_unlock(&tty_mutex);
-			tty_free_file(filp);
-			return -ENXIO;
-		}
-		driver = tty_driver_kref_get(tty->driver);
-		index = tty->index;
-		filp->f_flags |= O_NONBLOCK; /* Don't let /dev/tty block */
-		/* noctty = 1; */
-		/* FIXME: Should we take a driver reference ? */
-		tty_kref_put(tty);
-		goto got_driver;
-	}
-#ifdef CONFIG_VT
-	if (device == MKDEV(TTY_MAJOR, 0)) {
-		extern struct tty_driver *console_driver;
-		driver = tty_driver_kref_get(console_driver);
-		index = fg_console;
-		noctty = 1;
-		goto got_driver;
-	}
-#endif
-	if (device == MKDEV(TTYAUX_MAJOR, 1)) {
-		struct tty_driver *console_driver = console_device(&index);
-		if (console_driver) {
-			driver = tty_driver_kref_get(console_driver);
-			if (driver) {
-				/* Don't let /dev/console block */
-				filp->f_flags |= O_NONBLOCK;
-				noctty = 1;
-				goto got_driver;
-			}
+	tty = tty_open_current_tty(device, filp);
+	if (IS_ERR(tty)) {
+		retval = PTR_ERR(tty);
+		goto err_unlock;
+	} else if (!tty) {
+		driver = tty_lookup_driver(device, filp, &noctty, &index);
+		if (IS_ERR(driver)) {
+			retval = PTR_ERR(driver);
+			goto err_unlock;
 		}
-		tty_unlock();
-		mutex_unlock(&tty_mutex);
-		tty_free_file(filp);
-		return -ENODEV;
-	}
 
-	driver = get_tty_driver(device, &index);
-	if (!driver) {
-		tty_unlock();
-		mutex_unlock(&tty_mutex);
-		tty_free_file(filp);
-		return -ENODEV;
-	}
-got_driver:
-	if (!tty) {
 		/* check whether we're reopening an existing tty */
 		tty = tty_driver_lookup_tty(driver, inode, index);
-
 		if (IS_ERR(tty)) {
-			tty_unlock();
-			mutex_unlock(&tty_mutex);
-			tty_driver_kref_put(driver);
-			tty_free_file(filp);
-			return PTR_ERR(tty);
+			retval = PTR_ERR(tty);
+			goto err_unlock;
 		}
 	}
 
@@ -1912,21 +1953,22 @@ got_driver:
 		tty = tty_init_dev(driver, index, 0);
 
 	mutex_unlock(&tty_mutex);
-	tty_driver_kref_put(driver);
+	if (driver)
+		tty_driver_kref_put(driver);
 	if (IS_ERR(tty)) {
 		tty_unlock();
-		tty_free_file(filp);
-		return PTR_ERR(tty);
+		retval = PTR_ERR(tty);
+		goto err_file;
 	}
 
 	tty_add_file(tty, filp);
 
-	check_tty_count(tty, "tty_open");
+	check_tty_count(tty, __func__);
 	if (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
 	    tty->driver->subtype == PTY_TYPE_MASTER)
 		noctty = 1;
 #ifdef TTY_DEBUG_HANGUP
-	printk(KERN_DEBUG "opening %s...", tty->name);
+	printk(KERN_DEBUG "%s: opening %s...\n", __func__, tty->name);
 #endif
 	if (tty->ops->open)
 		retval = tty->ops->open(tty, filp);
@@ -1940,8 +1982,8 @@ got_driver:
 
 	if (retval) {
 #ifdef TTY_DEBUG_HANGUP
-		printk(KERN_DEBUG "error %d in opening %s...", retval,
-		       tty->name);
+		printk(KERN_DEBUG "%s: error %d in opening %s...\n", __func__,
+				retval, tty->name);
 #endif
 		tty_unlock(); /* need to call tty_release without BTM */
 		tty_release(inode, filp);
@@ -1976,6 +2018,15 @@ got_driver:
 	tty_unlock();
 	mutex_unlock(&tty_mutex);
 	return 0;
+err_unlock:
+	tty_unlock();
+	mutex_unlock(&tty_mutex);
+	/* after locks to avoid deadlock */
+	if (!IS_ERR_OR_NULL(driver))
+		tty_driver_kref_put(driver);
+err_file:
+	tty_free_file(filp);
+	return retval;
 }
 
 
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index 8e0924f55446..24b95db75d84 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -1,19 +1,11 @@
 #include <linux/types.h>
-#include <linux/major.h>
 #include <linux/errno.h>
-#include <linux/signal.h>
-#include <linux/fcntl.h>
+#include <linux/kmod.h>
 #include <linux/sched.h>
 #include <linux/interrupt.h>
 #include <linux/tty.h>
 #include <linux/tty_driver.h>
-#include <linux/tty_flip.h>
-#include <linux/devpts_fs.h>
 #include <linux/file.h>
-#include <linux/console.h>
-#include <linux/timer.h>
-#include <linux/ctype.h>
-#include <linux/kd.h>
 #include <linux/mm.h>
 #include <linux/string.h>
 #include <linux/slab.h>
@@ -24,18 +16,8 @@
 #include <linux/device.h>
 #include <linux/wait.h>
 #include <linux/bitops.h>
-#include <linux/delay.h>
 #include <linux/seq_file.h>
-
 #include <linux/uaccess.h>
-#include <asm/system.h>
-
-#include <linux/kbd_kern.h>
-#include <linux/vt_kern.h>
-#include <linux/selection.h>
-
-#include <linux/kmod.h>
-#include <linux/nsproxy.h>
 #include <linux/ratelimit.h>
 
 /*
@@ -558,8 +540,6 @@ static int tty_ldisc_wait_idle(struct tty_struct *tty, long timeout)
 	long ret;
 	ret = wait_event_timeout(tty_ldisc_idle,
 			atomic_read(&tty->ldisc->users) == 1, timeout);
-	if (ret < 0)
-		return ret;
 	return ret > 0 ? 0 : -EBUSY;
 }
 
diff --git a/drivers/tty/vt/consolemap.c b/drivers/tty/vt/consolemap.c
index 45d3e80156d4..a0f3d6c4d39d 100644
--- a/drivers/tty/vt/consolemap.c
+++ b/drivers/tty/vt/consolemap.c
@@ -584,7 +584,7 @@ int con_set_default_unimap(struct vc_data *vc)
 			return 0;
 		dflt->refcount++;
 		*vc->vc_uni_pagedir_loc = (unsigned long)dflt;
-		if (p && --p->refcount) {
+		if (p && !--p->refcount) {
 			con_release_unimap(p);
 			kfree(p);
 		}
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index 791f11bed606..75823a1abeb6 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -48,6 +48,7 @@ config USB_ARCH_HAS_OHCI
 	default y if ARCH_DAVINCI_DA8XX
 	default y if ARCH_CNS3XXX
 	default y if PLAT_SPEAR
+	default y if ARCH_EXYNOS
 	# PPC:
 	default y if STB03xxx
 	default y if PPC_MPC52xx
diff --git a/drivers/usb/Makefile b/drivers/usb/Makefile
index 75eca7645227..53a7bc07dd8d 100644
--- a/drivers/usb/Makefile
+++ b/drivers/usb/Makefile
@@ -6,6 +6,8 @@
 
 obj-$(CONFIG_USB)		+= core/
 
+obj-$(CONFIG_USB_OTG_UTILS)	+= otg/
+
 obj-$(CONFIG_USB_DWC3)		+= dwc3/
 
 obj-$(CONFIG_USB_MON)		+= mon/
@@ -51,7 +53,6 @@ obj-$(CONFIG_USB_SPEEDTOUCH)	+= atm/
 
 obj-$(CONFIG_USB_MUSB_HDRC)	+= musb/
 obj-$(CONFIG_USB_RENESAS_USBHS)	+= renesas_usbhs/
-obj-$(CONFIG_USB_OTG_UTILS)	+= otg/
 obj-$(CONFIG_USB_GADGET)	+= gadget/
 
 obj-$(CONFIG_USB_COMMON)	+= usb-common.o
diff --git a/drivers/usb/c67x00/c67x00-drv.c b/drivers/usb/c67x00/c67x00-drv.c
index 57ae44cd0b88..6f3b6e267398 100644
--- a/drivers/usb/c67x00/c67x00-drv.c
+++ b/drivers/usb/c67x00/c67x00-drv.c
@@ -225,21 +225,10 @@ static struct platform_driver c67x00_driver = {
 		.name = "c67x00",
 	},
 };
-MODULE_ALIAS("platform:c67x00");
-
-static int __init c67x00_init(void)
-{
-	return platform_driver_register(&c67x00_driver);
-}
 
-static void __exit c67x00_exit(void)
-{
-	platform_driver_unregister(&c67x00_driver);
-}
-
-module_init(c67x00_init);
-module_exit(c67x00_exit);
+module_platform_driver(c67x00_driver);
 
 MODULE_AUTHOR("Peter Korsgaard, Jan Veldeman, Grant Likely");
 MODULE_DESCRIPTION("Cypress C67X00 USB Controller Driver");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:c67x00");
diff --git a/drivers/usb/c67x00/c67x00-hcd.c b/drivers/usb/c67x00/c67x00-hcd.c
index d3e1356d091e..75e47b860a53 100644
--- a/drivers/usb/c67x00/c67x00-hcd.c
+++ b/drivers/usb/c67x00/c67x00-hcd.c
@@ -271,7 +271,6 @@ static void c67x00_hcd_irq(struct c67x00_sie *sie, u16 int_status, u16 msg)
 	if (int_status & SOFEOP_FLG(sie->sie_num)) {
 		c67x00_ll_usb_clear_status(sie, SOF_EOP_IRQ_FLG);
 		c67x00_sched_kick(c67x00);
-		set_bit(HCD_FLAG_SAW_IRQ, &hcd->flags);
 	}
 }
 
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index a8078d0638fa..9543b19d410c 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -58,12 +58,62 @@ static struct usb_driver acm_driver;
 static struct tty_driver *acm_tty_driver;
 static struct acm *acm_table[ACM_TTY_MINORS];
 
-static DEFINE_MUTEX(open_mutex);
+static DEFINE_MUTEX(acm_table_lock);
 
-#define ACM_READY(acm)	(acm && acm->dev && acm->port.count)
+/*
+ * acm_table accessors
+ */
 
-static const struct tty_port_operations acm_port_ops = {
-};
+/*
+ * Look up an ACM structure by index. If found and not disconnected, increment
+ * its refcount and return it with its mutex held.
+ */
+static struct acm *acm_get_by_index(unsigned index)
+{
+	struct acm *acm;
+
+	mutex_lock(&acm_table_lock);
+	acm = acm_table[index];
+	if (acm) {
+		mutex_lock(&acm->mutex);
+		if (acm->disconnected) {
+			mutex_unlock(&acm->mutex);
+			acm = NULL;
+		} else {
+			tty_port_get(&acm->port);
+			mutex_unlock(&acm->mutex);
+		}
+	}
+	mutex_unlock(&acm_table_lock);
+	return acm;
+}
+
+/*
+ * Try to find an available minor number and if found, associate it with 'acm'.
+ */
+static int acm_alloc_minor(struct acm *acm)
+{
+	int minor;
+
+	mutex_lock(&acm_table_lock);
+	for (minor = 0; minor < ACM_TTY_MINORS; minor++) {
+		if (!acm_table[minor]) {
+			acm_table[minor] = acm;
+			break;
+		}
+	}
+	mutex_unlock(&acm_table_lock);
+
+	return minor;
+}
+
+/* Release the minor number associated with 'acm'.  */
+static void acm_release_minor(struct acm *acm)
+{
+	mutex_lock(&acm_table_lock);
+	acm_table[acm->minor] = NULL;
+	mutex_unlock(&acm_table_lock);
+}
 
 /*
  * Functions for ACM control messages.
@@ -267,9 +317,6 @@ static void acm_ctrl_irq(struct urb *urb)
 		goto exit;
 	}
 
-	if (!ACM_READY(acm))
-		goto exit;
-
 	usb_mark_last_busy(acm->dev);
 
 	data = (unsigned char *)(dr + 1);
@@ -429,8 +476,7 @@ static void acm_write_bulk(struct urb *urb)
 	spin_lock_irqsave(&acm->write_lock, flags);
 	acm_write_done(acm, wb);
 	spin_unlock_irqrestore(&acm->write_lock, flags);
-	if (ACM_READY(acm))
-		schedule_work(&acm->work);
+	schedule_work(&acm->work);
 }
 
 static void acm_softint(struct work_struct *work)
@@ -440,8 +486,6 @@ static void acm_softint(struct work_struct *work)
 
 	dev_vdbg(&acm->data->dev, "%s\n", __func__);
 
-	if (!ACM_READY(acm))
-		return;
 	tty = tty_port_tty_get(&acm->port);
 	if (!tty)
 		return;
@@ -453,93 +497,122 @@ static void acm_softint(struct work_struct *work)
  * TTY handlers
  */
 
-static int acm_tty_open(struct tty_struct *tty, struct file *filp)
+static int acm_tty_install(struct tty_driver *driver, struct tty_struct *tty)
 {
 	struct acm *acm;
-	int rv = -ENODEV;
-
-	mutex_lock(&open_mutex);
+	int retval;
 
-	acm = acm_table[tty->index];
-	if (!acm || !acm->dev)
-		goto out;
-	else
-		rv = 0;
+	dev_dbg(tty->dev, "%s\n", __func__);
 
-	dev_dbg(&acm->control->dev, "%s\n", __func__);
+	acm = acm_get_by_index(tty->index);
+	if (!acm)
+		return -ENODEV;
 
-	set_bit(TTY_NO_WRITE_SPLIT, &tty->flags);
+	retval = tty_init_termios(tty);
+	if (retval)
+		goto error_init_termios;
 
 	tty->driver_data = acm;
-	tty_port_tty_set(&acm->port, tty);
 
-	if (usb_autopm_get_interface(acm->control) < 0)
-		goto early_bail;
-	else
-		acm->control->needs_remote_wakeup = 1;
+	/* Final install (we use the default method) */
+	tty_driver_kref_get(driver);
+	tty->count++;
+	driver->ttys[tty->index] = tty;
+
+	return 0;
+
+error_init_termios:
+	tty_port_put(&acm->port);
+	return retval;
+}
+
+static int acm_tty_open(struct tty_struct *tty, struct file *filp)
+{
+	struct acm *acm = tty->driver_data;
+
+	dev_dbg(tty->dev, "%s\n", __func__);
+
+	return tty_port_open(&acm->port, tty, filp);
+}
+
+static int acm_port_activate(struct tty_port *port, struct tty_struct *tty)
+{
+	struct acm *acm = container_of(port, struct acm, port);
+	int retval = -ENODEV;
+
+	dev_dbg(&acm->control->dev, "%s\n", __func__);
 
 	mutex_lock(&acm->mutex);
-	if (acm->port.count++) {
-		mutex_unlock(&acm->mutex);
-		usb_autopm_put_interface(acm->control);
-		goto out;
-	}
+	if (acm->disconnected)
+		goto disconnected;
+
+	retval = usb_autopm_get_interface(acm->control);
+	if (retval)
+		goto error_get_interface;
+
+	/*
+	 * FIXME: Why do we need this? Allocating 64K of physically contiguous
+	 * memory is really nasty...
+	 */
+	set_bit(TTY_NO_WRITE_SPLIT, &tty->flags);
+	acm->control->needs_remote_wakeup = 1;
 
 	acm->ctrlurb->dev = acm->dev;
 	if (usb_submit_urb(acm->ctrlurb, GFP_KERNEL)) {
 		dev_err(&acm->control->dev,
 			"%s - usb_submit_urb(ctrl irq) failed\n", __func__);
-		goto bail_out;
+		goto error_submit_urb;
 	}
 
-	if (0 > acm_set_control(acm, acm->ctrlout = ACM_CTRL_DTR | ACM_CTRL_RTS) &&
+	acm->ctrlout = ACM_CTRL_DTR | ACM_CTRL_RTS;
+	if (acm_set_control(acm, acm->ctrlout) < 0 &&
 	    (acm->ctrl_caps & USB_CDC_CAP_LINE))
-		goto bail_out;
+		goto error_set_control;
 
 	usb_autopm_put_interface(acm->control);
 
 	if (acm_submit_read_urbs(acm, GFP_KERNEL))
-		goto bail_out;
-
-	set_bit(ASYNCB_INITIALIZED, &acm->port.flags);
-	rv = tty_port_block_til_ready(&acm->port, tty, filp);
+		goto error_submit_read_urbs;
 
 	mutex_unlock(&acm->mutex);
-out:
-	mutex_unlock(&open_mutex);
-	return rv;
 
-bail_out:
-	acm->port.count--;
-	mutex_unlock(&acm->mutex);
+	return 0;
+
+error_submit_read_urbs:
+	acm->ctrlout = 0;
+	acm_set_control(acm, acm->ctrlout);
+error_set_control:
+	usb_kill_urb(acm->ctrlurb);
+error_submit_urb:
 	usb_autopm_put_interface(acm->control);
-early_bail:
-	mutex_unlock(&open_mutex);
-	tty_port_tty_set(&acm->port, NULL);
-	return -EIO;
+error_get_interface:
+disconnected:
+	mutex_unlock(&acm->mutex);
+	return retval;
 }
 
-static void acm_tty_unregister(struct acm *acm)
+static void acm_port_destruct(struct tty_port *port)
 {
-	int i;
+	struct acm *acm = container_of(port, struct acm, port);
+
+	dev_dbg(&acm->control->dev, "%s\n", __func__);
 
 	tty_unregister_device(acm_tty_driver, acm->minor);
+	acm_release_minor(acm);
 	usb_put_intf(acm->control);
-	acm_table[acm->minor] = NULL;
-	usb_free_urb(acm->ctrlurb);
-	for (i = 0; i < ACM_NW; i++)
-		usb_free_urb(acm->wb[i].urb);
-	for (i = 0; i < acm->rx_buflimit; i++)
-		usb_free_urb(acm->read_urbs[i]);
 	kfree(acm->country_codes);
 	kfree(acm);
 }
 
-static void acm_port_down(struct acm *acm)
+static void acm_port_shutdown(struct tty_port *port)
 {
+	struct acm *acm = container_of(port, struct acm, port);
 	int i;
 
-	if (acm->dev) {
+	dev_dbg(&acm->control->dev, "%s\n", __func__);
+
+	mutex_lock(&acm->mutex);
+	if (!acm->disconnected) {
 		usb_autopm_get_interface(acm->control);
 		acm_set_control(acm, acm->ctrlout = 0);
 		usb_kill_urb(acm->ctrlurb);
@@ -550,40 +623,28 @@ static void acm_port_down(struct acm *acm)
 		acm->control->needs_remote_wakeup = 0;
 		usb_autopm_put_interface(acm->control);
 	}
+	mutex_unlock(&acm->mutex);
+}
+
+static void acm_tty_cleanup(struct tty_struct *tty)
+{
+	struct acm *acm = tty->driver_data;
+	dev_dbg(&acm->control->dev, "%s\n", __func__);
+	tty_port_put(&acm->port);
 }
 
 static void acm_tty_hangup(struct tty_struct *tty)
 {
 	struct acm *acm = tty->driver_data;
+	dev_dbg(&acm->control->dev, "%s\n", __func__);
 	tty_port_hangup(&acm->port);
-	mutex_lock(&open_mutex);
-	acm_port_down(acm);
-	mutex_unlock(&open_mutex);
 }
 
 static void acm_tty_close(struct tty_struct *tty, struct file *filp)
 {
 	struct acm *acm = tty->driver_data;
-
-	/* Perform the closing process and see if we need to do the hardware
-	   shutdown */
-	if (!acm)
-		return;
-
-	mutex_lock(&open_mutex);
-	if (tty_port_close_start(&acm->port, tty, filp) == 0) {
-		if (!acm->dev) {
-			tty_port_tty_set(&acm->port, NULL);
-			acm_tty_unregister(acm);
-			tty->driver_data = NULL;
-		}
-		mutex_unlock(&open_mutex);
-		return;
-	}
-	acm_port_down(acm);
-	tty_port_close_end(&acm->port, tty);
-	tty_port_tty_set(&acm->port, NULL);
-	mutex_unlock(&open_mutex);
+	dev_dbg(&acm->control->dev, "%s\n", __func__);
+	tty_port_close(&acm->port, tty, filp);
 }
 
 static int acm_tty_write(struct tty_struct *tty,
@@ -595,8 +656,6 @@ static int acm_tty_write(struct tty_struct *tty,
 	int wbn;
 	struct acm_wb *wb;
 
-	if (!ACM_READY(acm))
-		return -EINVAL;
 	if (!count)
 		return 0;
 
@@ -625,8 +684,6 @@ static int acm_tty_write(struct tty_struct *tty,
 static int acm_tty_write_room(struct tty_struct *tty)
 {
 	struct acm *acm = tty->driver_data;
-	if (!ACM_READY(acm))
-		return -EINVAL;
 	/*
 	 * Do not let the line discipline to know that we have a reserve,
 	 * or it might get too enthusiastic.
@@ -637,7 +694,11 @@ static int acm_tty_write_room(struct tty_struct *tty)
 static int acm_tty_chars_in_buffer(struct tty_struct *tty)
 {
 	struct acm *acm = tty->driver_data;
-	if (!ACM_READY(acm))
+	/*
+	 * if the device was unplugged then any remaining characters fell out
+	 * of the connector ;)
+	 */
+	if (acm->disconnected)
 		return 0;
 	/*
 	 * This is inaccurate (overcounts), but it works.
@@ -649,9 +710,6 @@ static void acm_tty_throttle(struct tty_struct *tty)
 {
 	struct acm *acm = tty->driver_data;
 
-	if (!ACM_READY(acm))
-		return;
-
 	spin_lock_irq(&acm->read_lock);
 	acm->throttle_req = 1;
 	spin_unlock_irq(&acm->read_lock);
@@ -662,9 +720,6 @@ static void acm_tty_unthrottle(struct tty_struct *tty)
 	struct acm *acm = tty->driver_data;
 	unsigned int was_throttled;
 
-	if (!ACM_READY(acm))
-		return;
-
 	spin_lock_irq(&acm->read_lock);
 	was_throttled = acm->throttled;
 	acm->throttled = 0;
@@ -679,8 +734,7 @@ static int acm_tty_break_ctl(struct tty_struct *tty, int state)
 {
 	struct acm *acm = tty->driver_data;
 	int retval;
-	if (!ACM_READY(acm))
-		return -EINVAL;
+
 	retval = acm_send_break(acm, state ? 0xffff : 0);
 	if (retval < 0)
 		dev_dbg(&acm->control->dev, "%s - send break failed\n",
@@ -692,9 +746,6 @@ static int acm_tty_tiocmget(struct tty_struct *tty)
 {
 	struct acm *acm = tty->driver_data;
 
-	if (!ACM_READY(acm))
-		return -EINVAL;
-
 	return (acm->ctrlout & ACM_CTRL_DTR ? TIOCM_DTR : 0) |
 	       (acm->ctrlout & ACM_CTRL_RTS ? TIOCM_RTS : 0) |
 	       (acm->ctrlin  & ACM_CTRL_DSR ? TIOCM_DSR : 0) |
@@ -709,9 +760,6 @@ static int acm_tty_tiocmset(struct tty_struct *tty,
 	struct acm *acm = tty->driver_data;
 	unsigned int newctrl;
 
-	if (!ACM_READY(acm))
-		return -EINVAL;
-
 	newctrl = acm->ctrlout;
 	set = (set & TIOCM_DTR ? ACM_CTRL_DTR : 0) |
 					(set & TIOCM_RTS ? ACM_CTRL_RTS : 0);
@@ -728,11 +776,6 @@ static int acm_tty_tiocmset(struct tty_struct *tty,
 static int acm_tty_ioctl(struct tty_struct *tty,
 					unsigned int cmd, unsigned long arg)
 {
-	struct acm *acm = tty->driver_data;
-
-	if (!ACM_READY(acm))
-		return -EINVAL;
-
 	return -ENOIOCTLCMD;
 }
 
@@ -756,9 +799,6 @@ static void acm_tty_set_termios(struct tty_struct *tty,
 	struct usb_cdc_line_coding newline;
 	int newctrl = acm->ctrlout;
 
-	if (!ACM_READY(acm))
-		return;
-
 	newline.dwDTERate = cpu_to_le32(tty_get_baud_rate(tty));
 	newline.bCharFormat = termios->c_cflag & CSTOPB ? 2 : 0;
 	newline.bParityType = termios->c_cflag & PARENB ?
@@ -788,6 +828,12 @@ static void acm_tty_set_termios(struct tty_struct *tty,
 	}
 }
 
+static const struct tty_port_operations acm_port_ops = {
+	.shutdown = acm_port_shutdown,
+	.activate = acm_port_activate,
+	.destruct = acm_port_destruct,
+};
+
 /*
  * USB probe and disconnect routines.
  */
@@ -1047,12 +1093,6 @@ skip_normal_probe:
 	}
 made_compressed_probe:
 	dev_dbg(&intf->dev, "interfaces are valid\n");
-	for (minor = 0; minor < ACM_TTY_MINORS && acm_table[minor]; minor++);
-
-	if (minor == ACM_TTY_MINORS) {
-		dev_err(&intf->dev, "no more free acm devices\n");
-		return -ENODEV;
-	}
 
 	acm = kzalloc(sizeof(struct acm), GFP_KERNEL);
 	if (acm == NULL) {
@@ -1060,6 +1100,13 @@ made_compressed_probe:
 		goto alloc_fail;
 	}
 
+	minor = acm_alloc_minor(acm);
+	if (minor == ACM_TTY_MINORS) {
+		dev_err(&intf->dev, "no more free acm devices\n");
+		kfree(acm);
+		return -ENODEV;
+	}
+
 	ctrlsize = usb_endpoint_maxp(epctrl);
 	readsize = usb_endpoint_maxp(epread) *
 				(quirks == SINGLE_RX_URB ? 1 : 2);
@@ -1183,6 +1230,8 @@ made_compressed_probe:
 		i = device_create_file(&intf->dev, &dev_attr_wCountryCodes);
 		if (i < 0) {
 			kfree(acm->country_codes);
+			acm->country_codes = NULL;
+			acm->country_code_size = 0;
 			goto skip_countries;
 		}
 
@@ -1191,6 +1240,8 @@ made_compressed_probe:
 		if (i < 0) {
 			device_remove_file(&intf->dev, &dev_attr_wCountryCodes);
 			kfree(acm->country_codes);
+			acm->country_codes = NULL;
+			acm->country_code_size = 0;
 			goto skip_countries;
 		}
 	}
@@ -1218,8 +1269,6 @@ skip_countries:
 	usb_get_intf(control_interface);
 	tty_register_device(acm_tty_driver, minor, &control_interface->dev);
 
-	acm_table[minor] = acm;
-
 	return 0;
 alloc_fail7:
 	for (i = 0; i < ACM_NW; i++)
@@ -1234,6 +1283,7 @@ alloc_fail5:
 alloc_fail4:
 	usb_free_coherent(usb_dev, ctrlsize, acm->ctrl_buffer, acm->ctrl_dma);
 alloc_fail2:
+	acm_release_minor(acm);
 	kfree(acm);
 alloc_fail:
 	return -ENOMEM;
@@ -1259,12 +1309,16 @@ static void acm_disconnect(struct usb_interface *intf)
 	struct acm *acm = usb_get_intfdata(intf);
 	struct usb_device *usb_dev = interface_to_usbdev(intf);
 	struct tty_struct *tty;
+	int i;
+
+	dev_dbg(&intf->dev, "%s\n", __func__);
 
 	/* sibling interface is already cleaning up */
 	if (!acm)
 		return;
 
-	mutex_lock(&open_mutex);
+	mutex_lock(&acm->mutex);
+	acm->disconnected = true;
 	if (acm->country_codes) {
 		device_remove_file(&acm->control->dev,
 				&dev_attr_wCountryCodes);
@@ -1272,33 +1326,32 @@ static void acm_disconnect(struct usb_interface *intf)
 				&dev_attr_iCountryCodeRelDate);
 	}
 	device_remove_file(&acm->control->dev, &dev_attr_bmCapabilities);
-	acm->dev = NULL;
 	usb_set_intfdata(acm->control, NULL);
 	usb_set_intfdata(acm->data, NULL);
+	mutex_unlock(&acm->mutex);
+
+	tty = tty_port_tty_get(&acm->port);
+	if (tty) {
+		tty_vhangup(tty);
+		tty_kref_put(tty);
+	}
 
 	stop_data_traffic(acm);
 
+	usb_free_urb(acm->ctrlurb);
+	for (i = 0; i < ACM_NW; i++)
+		usb_free_urb(acm->wb[i].urb);
+	for (i = 0; i < acm->rx_buflimit; i++)
+		usb_free_urb(acm->read_urbs[i]);
 	acm_write_buffers_free(acm);
-	usb_free_coherent(usb_dev, acm->ctrlsize, acm->ctrl_buffer,
-			  acm->ctrl_dma);
+	usb_free_coherent(usb_dev, acm->ctrlsize, acm->ctrl_buffer, acm->ctrl_dma);
 	acm_read_buffers_free(acm);
 
 	if (!acm->combined_interfaces)
 		usb_driver_release_interface(&acm_driver, intf == acm->control ?
 					acm->data : acm->control);
 
-	if (acm->port.count == 0) {
-		acm_tty_unregister(acm);
-		mutex_unlock(&open_mutex);
-		return;
-	}
-
-	mutex_unlock(&open_mutex);
-	tty = tty_port_tty_get(&acm->port);
-	if (tty) {
-		tty_hangup(tty);
-		tty_kref_put(tty);
-	}
+	tty_port_put(&acm->port);
 }
 
 #ifdef CONFIG_PM
@@ -1325,16 +1378,10 @@ static int acm_suspend(struct usb_interface *intf, pm_message_t message)
 
 	if (cnt)
 		return 0;
-	/*
-	we treat opened interfaces differently,
-	we must guard against open
-	*/
-	mutex_lock(&acm->mutex);
 
-	if (acm->port.count)
+	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags))
 		stop_data_traffic(acm);
 
-	mutex_unlock(&acm->mutex);
 	return 0;
 }
 
@@ -1353,8 +1400,7 @@ static int acm_resume(struct usb_interface *intf)
 	if (cnt)
 		return 0;
 
-	mutex_lock(&acm->mutex);
-	if (acm->port.count) {
+	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags)) {
 		rv = usb_submit_urb(acm->ctrlurb, GFP_NOIO);
 
 		spin_lock_irq(&acm->write_lock);
@@ -1378,7 +1424,6 @@ static int acm_resume(struct usb_interface *intf)
 	}
 
 err_out:
-	mutex_unlock(&acm->mutex);
 	return rv;
 }
 
@@ -1387,15 +1432,14 @@ static int acm_reset_resume(struct usb_interface *intf)
 	struct acm *acm = usb_get_intfdata(intf);
 	struct tty_struct *tty;
 
-	mutex_lock(&acm->mutex);
-	if (acm->port.count) {
+	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags)) {
 		tty = tty_port_tty_get(&acm->port);
 		if (tty) {
 			tty_hangup(tty);
 			tty_kref_put(tty);
 		}
 	}
-	mutex_unlock(&acm->mutex);
+
 	return acm_resume(intf);
 }
 
@@ -1604,8 +1648,10 @@ static struct usb_driver acm_driver = {
  */
 
 static const struct tty_operations acm_ops = {
+	.install =		acm_tty_install,
 	.open =			acm_tty_open,
 	.close =		acm_tty_close,
+	.cleanup =		acm_tty_cleanup,
 	.hangup =		acm_tty_hangup,
 	.write =		acm_tty_write,
 	.write_room =		acm_tty_write_room,
diff --git a/drivers/usb/class/cdc-acm.h b/drivers/usb/class/cdc-acm.h
index ca7937f26e27..35ef887b7417 100644
--- a/drivers/usb/class/cdc-acm.h
+++ b/drivers/usb/class/cdc-acm.h
@@ -101,6 +101,7 @@ struct acm {
 	int transmitting;
 	spinlock_t write_lock;
 	struct mutex mutex;
+	bool disconnected;
 	struct usb_cdc_line_coding line;		/* bits, stop, parity */
 	struct work_struct work;			/* work queue entry for line discipline waking up */
 	unsigned int ctrlin;				/* input control lines (DCD, DSR, RI, break, overruns) */
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index e3beaf229ee3..3af5e2dd1d82 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -86,6 +86,7 @@ struct async {
 	void __user *userbuffer;
 	void __user *userurb;
 	struct urb *urb;
+	unsigned int mem_usage;
 	int status;
 	u32 secid;
 	u8 bulk_addr;
@@ -108,8 +109,44 @@ enum snoop_when {
 
 #define USB_DEVICE_DEV		MKDEV(USB_DEVICE_MAJOR, 0)
 
-#define	MAX_USBFS_BUFFER_SIZE	16384
+/* Limit on the total amount of memory we can allocate for transfers */
+static unsigned usbfs_memory_mb = 16;
+module_param(usbfs_memory_mb, uint, 0644);
+MODULE_PARM_DESC(usbfs_memory_mb,
+		"maximum MB allowed for usbfs buffers (0 = no limit)");
 
+/* Hard limit, necessary to avoid aithmetic overflow */
+#define USBFS_XFER_MAX		(UINT_MAX / 2 - 1000000)
+
+static atomic_t usbfs_memory_usage;	/* Total memory currently allocated */
+
+/* Check whether it's okay to allocate more memory for a transfer */
+static int usbfs_increase_memory_usage(unsigned amount)
+{
+	unsigned lim;
+
+	/*
+	 * Convert usbfs_memory_mb to bytes, avoiding overflows.
+	 * 0 means use the hard limit (effectively unlimited).
+	 */
+	lim = ACCESS_ONCE(usbfs_memory_mb);
+	if (lim == 0 || lim > (USBFS_XFER_MAX >> 20))
+		lim = USBFS_XFER_MAX;
+	else
+		lim <<= 20;
+
+	atomic_add(amount, &usbfs_memory_usage);
+	if (atomic_read(&usbfs_memory_usage) <= lim)
+		return 0;
+	atomic_sub(amount, &usbfs_memory_usage);
+	return -ENOMEM;
+}
+
+/* Memory for a transfer is being deallocated */
+static void usbfs_decrease_memory_usage(unsigned amount)
+{
+	atomic_sub(amount, &usbfs_memory_usage);
+}
 
 static int connected(struct dev_state *ps)
 {
@@ -249,10 +286,12 @@ static struct async *alloc_async(unsigned int numisoframes)
 static void free_async(struct async *as)
 {
 	put_pid(as->pid);
-	put_cred(as->cred);
+	if (as->cred)
+		put_cred(as->cred);
 	kfree(as->urb->transfer_buffer);
 	kfree(as->urb->setup_packet);
 	usb_free_urb(as->urb);
+	usbfs_decrease_memory_usage(as->mem_usage);
 	kfree(as);
 }
 
@@ -792,9 +831,15 @@ static int proc_control(struct dev_state *ps, void __user *arg)
 	wLength = ctrl.wLength;		/* To suppress 64k PAGE_SIZE warning */
 	if (wLength > PAGE_SIZE)
 		return -EINVAL;
+	ret = usbfs_increase_memory_usage(PAGE_SIZE + sizeof(struct urb) +
+			sizeof(struct usb_ctrlrequest));
+	if (ret)
+		return ret;
 	tbuf = (unsigned char *)__get_free_page(GFP_KERNEL);
-	if (!tbuf)
-		return -ENOMEM;
+	if (!tbuf) {
+		ret = -ENOMEM;
+		goto done;
+	}
 	tmo = ctrl.timeout;
 	snoop(&dev->dev, "control urb: bRequestType=%02x "
 		"bRequest=%02x wValue=%04x "
@@ -806,8 +851,8 @@ static int proc_control(struct dev_state *ps, void __user *arg)
 	if (ctrl.bRequestType & 0x80) {
 		if (ctrl.wLength && !access_ok(VERIFY_WRITE, ctrl.data,
 					       ctrl.wLength)) {
-			free_page((unsigned long)tbuf);
-			return -EINVAL;
+			ret = -EINVAL;
+			goto done;
 		}
 		pipe = usb_rcvctrlpipe(dev, 0);
 		snoop_urb(dev, NULL, pipe, ctrl.wLength, tmo, SUBMIT, NULL, 0);
@@ -821,15 +866,15 @@ static int proc_control(struct dev_state *ps, void __user *arg)
 			  tbuf, max(i, 0));
 		if ((i > 0) && ctrl.wLength) {
 			if (copy_to_user(ctrl.data, tbuf, i)) {
-				free_page((unsigned long)tbuf);
-				return -EFAULT;
+				ret = -EFAULT;
+				goto done;
 			}
 		}
 	} else {
 		if (ctrl.wLength) {
 			if (copy_from_user(tbuf, ctrl.data, ctrl.wLength)) {
-				free_page((unsigned long)tbuf);
-				return -EFAULT;
+				ret = -EFAULT;
+				goto done;
 			}
 		}
 		pipe = usb_sndctrlpipe(dev, 0);
@@ -843,14 +888,18 @@ static int proc_control(struct dev_state *ps, void __user *arg)
 		usb_lock_device(dev);
 		snoop_urb(dev, NULL, pipe, max(i, 0), min(i, 0), COMPLETE, NULL, 0);
 	}
-	free_page((unsigned long)tbuf);
 	if (i < 0 && i != -EPIPE) {
 		dev_printk(KERN_DEBUG, &dev->dev, "usbfs: USBDEVFS_CONTROL "
 			   "failed cmd %s rqt %u rq %u len %u ret %d\n",
 			   current->comm, ctrl.bRequestType, ctrl.bRequest,
 			   ctrl.wLength, i);
 	}
-	return i;
+	ret = i;
+ done:
+	free_page((unsigned long) tbuf);
+	usbfs_decrease_memory_usage(PAGE_SIZE + sizeof(struct urb) +
+			sizeof(struct usb_ctrlrequest));
+	return ret;
 }
 
 static int proc_bulk(struct dev_state *ps, void __user *arg)
@@ -877,15 +926,20 @@ static int proc_bulk(struct dev_state *ps, void __user *arg)
 	if (!usb_maxpacket(dev, pipe, !(bulk.ep & USB_DIR_IN)))
 		return -EINVAL;
 	len1 = bulk.len;
-	if (len1 > MAX_USBFS_BUFFER_SIZE)
+	if (len1 >= USBFS_XFER_MAX)
 		return -EINVAL;
-	if (!(tbuf = kmalloc(len1, GFP_KERNEL)))
-		return -ENOMEM;
+	ret = usbfs_increase_memory_usage(len1 + sizeof(struct urb));
+	if (ret)
+		return ret;
+	if (!(tbuf = kmalloc(len1, GFP_KERNEL))) {
+		ret = -ENOMEM;
+		goto done;
+	}
 	tmo = bulk.timeout;
 	if (bulk.ep & 0x80) {
 		if (len1 && !access_ok(VERIFY_WRITE, bulk.data, len1)) {
-			kfree(tbuf);
-			return -EINVAL;
+			ret = -EINVAL;
+			goto done;
 		}
 		snoop_urb(dev, NULL, pipe, len1, tmo, SUBMIT, NULL, 0);
 
@@ -896,15 +950,15 @@ static int proc_bulk(struct dev_state *ps, void __user *arg)
 
 		if (!i && len2) {
 			if (copy_to_user(bulk.data, tbuf, len2)) {
-				kfree(tbuf);
-				return -EFAULT;
+				ret = -EFAULT;
+				goto done;
 			}
 		}
 	} else {
 		if (len1) {
 			if (copy_from_user(tbuf, bulk.data, len1)) {
-				kfree(tbuf);
-				return -EFAULT;
+				ret = -EFAULT;
+				goto done;
 			}
 		}
 		snoop_urb(dev, NULL, pipe, len1, tmo, SUBMIT, tbuf, len1);
@@ -914,10 +968,11 @@ static int proc_bulk(struct dev_state *ps, void __user *arg)
 		usb_lock_device(dev);
 		snoop_urb(dev, NULL, pipe, len2, i, COMPLETE, NULL, 0);
 	}
+	ret = (i < 0 ? i : len2);
+ done:
 	kfree(tbuf);
-	if (i < 0)
-		return i;
-	return len2;
+	usbfs_decrease_memory_usage(len1 + sizeof(struct urb));
+	return ret;
 }
 
 static int proc_resetep(struct dev_state *ps, void __user *arg)
@@ -1062,7 +1117,7 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 {
 	struct usbdevfs_iso_packet_desc *isopkt = NULL;
 	struct usb_host_endpoint *ep;
-	struct async *as;
+	struct async *as = NULL;
 	struct usb_ctrlrequest *dr = NULL;
 	unsigned int u, totlen, isofrmlen;
 	int ret, ifnum = -1;
@@ -1095,32 +1150,30 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 	}
 	if (!ep)
 		return -ENOENT;
+
+	u = 0;
 	switch(uurb->type) {
 	case USBDEVFS_URB_TYPE_CONTROL:
 		if (!usb_endpoint_xfer_control(&ep->desc))
 			return -EINVAL;
-		/* min 8 byte setup packet,
-		 * max 8 byte setup plus an arbitrary data stage */
-		if (uurb->buffer_length < 8 ||
-		    uurb->buffer_length > (8 + MAX_USBFS_BUFFER_SIZE))
+		/* min 8 byte setup packet */
+		if (uurb->buffer_length < 8)
 			return -EINVAL;
 		dr = kmalloc(sizeof(struct usb_ctrlrequest), GFP_KERNEL);
 		if (!dr)
 			return -ENOMEM;
 		if (copy_from_user(dr, uurb->buffer, 8)) {
-			kfree(dr);
-			return -EFAULT;
+			ret = -EFAULT;
+			goto error;
 		}
 		if (uurb->buffer_length < (le16_to_cpup(&dr->wLength) + 8)) {
-			kfree(dr);
-			return -EINVAL;
+			ret = -EINVAL;
+			goto error;
 		}
 		ret = check_ctrlrecip(ps, dr->bRequestType, dr->bRequest,
 				      le16_to_cpup(&dr->wIndex));
-		if (ret) {
-			kfree(dr);
-			return ret;
-		}
+		if (ret)
+			goto error;
 		uurb->number_of_packets = 0;
 		uurb->buffer_length = le16_to_cpup(&dr->wLength);
 		uurb->buffer += 8;
@@ -1138,6 +1191,7 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 			__le16_to_cpup(&dr->wValue),
 			__le16_to_cpup(&dr->wIndex),
 			__le16_to_cpup(&dr->wLength));
+		u = sizeof(struct usb_ctrlrequest);
 		break;
 
 	case USBDEVFS_URB_TYPE_BULK:
@@ -1151,8 +1205,6 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 			goto interrupt_urb;
 		}
 		uurb->number_of_packets = 0;
-		if (uurb->buffer_length > MAX_USBFS_BUFFER_SIZE)
-			return -EINVAL;
 		break;
 
 	case USBDEVFS_URB_TYPE_INTERRUPT:
@@ -1160,8 +1212,6 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 			return -EINVAL;
  interrupt_urb:
 		uurb->number_of_packets = 0;
-		if (uurb->buffer_length > MAX_USBFS_BUFFER_SIZE)
-			return -EINVAL;
 		break;
 
 	case USBDEVFS_URB_TYPE_ISO:
@@ -1176,50 +1226,53 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 		if (!(isopkt = kmalloc(isofrmlen, GFP_KERNEL)))
 			return -ENOMEM;
 		if (copy_from_user(isopkt, iso_frame_desc, isofrmlen)) {
-			kfree(isopkt);
-			return -EFAULT;
+			ret = -EFAULT;
+			goto error;
 		}
 		for (totlen = u = 0; u < uurb->number_of_packets; u++) {
 			/* arbitrary limit,
 			 * sufficient for USB 2.0 high-bandwidth iso */
 			if (isopkt[u].length > 8192) {
-				kfree(isopkt);
-				return -EINVAL;
+				ret = -EINVAL;
+				goto error;
 			}
 			totlen += isopkt[u].length;
 		}
-		/* 3072 * 64 microframes */
-		if (totlen > 196608) {
-			kfree(isopkt);
-			return -EINVAL;
-		}
+		u *= sizeof(struct usb_iso_packet_descriptor);
 		uurb->buffer_length = totlen;
 		break;
 
 	default:
 		return -EINVAL;
 	}
+
+	if (uurb->buffer_length >= USBFS_XFER_MAX) {
+		ret = -EINVAL;
+		goto error;
+	}
 	if (uurb->buffer_length > 0 &&
 			!access_ok(is_in ? VERIFY_WRITE : VERIFY_READ,
 				uurb->buffer, uurb->buffer_length)) {
-		kfree(isopkt);
-		kfree(dr);
-		return -EFAULT;
+		ret = -EFAULT;
+		goto error;
 	}
 	as = alloc_async(uurb->number_of_packets);
 	if (!as) {
-		kfree(isopkt);
-		kfree(dr);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto error;
 	}
+	u += sizeof(struct async) + sizeof(struct urb) + uurb->buffer_length;
+	ret = usbfs_increase_memory_usage(u);
+	if (ret)
+		goto error;
+	as->mem_usage = u;
+
 	if (uurb->buffer_length > 0) {
 		as->urb->transfer_buffer = kmalloc(uurb->buffer_length,
 				GFP_KERNEL);
 		if (!as->urb->transfer_buffer) {
-			kfree(isopkt);
-			kfree(dr);
-			free_async(as);
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto error;
 		}
 		/* Isochronous input data may end up being discontiguous
 		 * if some of the packets are short.  Clear the buffer so
@@ -1253,6 +1306,7 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 
 	as->urb->transfer_buffer_length = uurb->buffer_length;
 	as->urb->setup_packet = (unsigned char *)dr;
+	dr = NULL;
 	as->urb->start_frame = uurb->start_frame;
 	as->urb->number_of_packets = uurb->number_of_packets;
 	if (uurb->type == USBDEVFS_URB_TYPE_ISO ||
@@ -1268,6 +1322,7 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 		totlen += isopkt[u].length;
 	}
 	kfree(isopkt);
+	isopkt = NULL;
 	as->ps = ps;
 	as->userurb = arg;
 	if (is_in && uurb->buffer_length > 0)
@@ -1282,8 +1337,8 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 	if (!is_in && uurb->buffer_length > 0) {
 		if (copy_from_user(as->urb->transfer_buffer, uurb->buffer,
 				uurb->buffer_length)) {
-			free_async(as);
-			return -EFAULT;
+			ret = -EFAULT;
+			goto error;
 		}
 	}
 	snoop_urb(ps->dev, as->userurb, as->urb->pipe,
@@ -1329,10 +1384,16 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 		snoop_urb(ps->dev, as->userurb, as->urb->pipe,
 				0, ret, COMPLETE, NULL, 0);
 		async_removepending(as);
-		free_async(as);
-		return ret;
+		goto error;
 	}
 	return 0;
+
+ error:
+	kfree(isopkt);
+	kfree(dr);
+	if (as)
+		free_async(as);
+	return ret;
 }
 
 static int proc_submiturb(struct dev_state *ps, void __user *arg)
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index 45887a0ff873..d40ff9568813 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -45,10 +45,12 @@ ssize_t usb_store_new_id(struct usb_dynids *dynids,
 	struct usb_dynid *dynid;
 	u32 idVendor = 0;
 	u32 idProduct = 0;
+	unsigned int bInterfaceClass = 0;
 	int fields = 0;
 	int retval = 0;
 
-	fields = sscanf(buf, "%x %x", &idVendor, &idProduct);
+	fields = sscanf(buf, "%x %x %x", &idVendor, &idProduct,
+					&bInterfaceClass);
 	if (fields < 2)
 		return -EINVAL;
 
@@ -60,6 +62,10 @@ ssize_t usb_store_new_id(struct usb_dynids *dynids,
 	dynid->id.idVendor = idVendor;
 	dynid->id.idProduct = idProduct;
 	dynid->id.match_flags = USB_DEVICE_ID_MATCH_DEVICE;
+	if (fields == 3) {
+		dynid->id.bInterfaceClass = (u8)bInterfaceClass;
+		dynid->id.match_flags |= USB_DEVICE_ID_MATCH_INT_CLASS;
+	}
 
 	spin_lock(&dynids->lock);
 	list_add_tail(&dynid->node, &dynids->list);
@@ -1073,17 +1079,10 @@ static int usb_suspend_interface(struct usb_device *udev,
 		goto done;
 	driver = to_usb_driver(intf->dev.driver);
 
-	if (driver->suspend) {
-		status = driver->suspend(intf, msg);
-		if (status && !PMSG_IS_AUTO(msg))
-			dev_err(&intf->dev, "%s error %d\n",
-					"suspend", status);
-	} else {
-		/* Later we will unbind the driver and reprobe */
-		intf->needs_binding = 1;
-		dev_warn(&intf->dev, "no %s for driver %s?\n",
-				"suspend", driver->name);
-	}
+	/* at this time we know the driver supports suspend */
+	status = driver->suspend(intf, msg);
+	if (status && !PMSG_IS_AUTO(msg))
+		dev_err(&intf->dev, "suspend error %d\n", status);
 
  done:
 	dev_vdbg(&intf->dev, "%s: status %d\n", __func__, status);
@@ -1132,16 +1131,9 @@ static int usb_resume_interface(struct usb_device *udev,
 					"reset_resume", driver->name);
 		}
 	} else {
-		if (driver->resume) {
-			status = driver->resume(intf);
-			if (status)
-				dev_err(&intf->dev, "%s error %d\n",
-						"resume", status);
-		} else {
-			intf->needs_binding = 1;
-			dev_warn(&intf->dev, "no %s for driver %s?\n",
-					"resume", driver->name);
-		}
+		status = driver->resume(intf);
+		if (status)
+			dev_err(&intf->dev, "resume error %d\n", status);
 	}
 
 done:
diff --git a/drivers/usb/core/hcd-pci.c b/drivers/usb/core/hcd-pci.c
index a004db35f6d0..d136b8f4c8a7 100644
--- a/drivers/usb/core/hcd-pci.c
+++ b/drivers/usb/core/hcd-pci.c
@@ -453,10 +453,6 @@ static int resume_common(struct device *dev, int event)
 
 	pci_set_master(pci_dev);
 
-	clear_bit(HCD_FLAG_SAW_IRQ, &hcd->flags);
-	if (hcd->shared_hcd)
-		clear_bit(HCD_FLAG_SAW_IRQ, &hcd->shared_hcd->flags);
-
 	if (hcd->driver->pci_resume && !HCD_DEAD(hcd)) {
 		if (event != PM_EVENT_AUTO_RESUME)
 			wait_for_companions(pci_dev, hcd);
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 13222d352a61..eb19cba34ac9 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -658,7 +658,7 @@ error:
 				len > offsetof(struct usb_device_descriptor,
 						bDeviceProtocol))
 			((struct usb_device_descriptor *) ubuf)->
-					bDeviceProtocol = 1;
+				bDeviceProtocol = USB_HUB_PR_HS_SINGLE_TT;
 	}
 
 	/* any errors get returned through the urb completion */
@@ -1168,20 +1168,6 @@ int usb_hcd_check_unlink_urb(struct usb_hcd *hcd, struct urb *urb,
 	if (urb->unlinked)
 		return -EBUSY;
 	urb->unlinked = status;
-
-	/* IRQ setup can easily be broken so that USB controllers
-	 * never get completion IRQs ... maybe even the ones we need to
-	 * finish unlinking the initial failed usb_set_address()
-	 * or device descriptor fetch.
-	 */
-	if (!HCD_SAW_IRQ(hcd) && !is_root_hub(urb->dev)) {
-		dev_warn(hcd->self.controller, "Unlink after no-IRQ?  "
-			"Controller is probably using the wrong IRQ.\n");
-		set_bit(HCD_FLAG_SAW_IRQ, &hcd->flags);
-		if (hcd->shared_hcd)
-			set_bit(HCD_FLAG_SAW_IRQ, &hcd->shared_hcd->flags);
-	}
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(usb_hcd_check_unlink_urb);
@@ -1412,11 +1398,10 @@ int usb_hcd_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 					ret = -EAGAIN;
 				else
 					urb->transfer_flags |= URB_DMA_MAP_SG;
-				if (n != urb->num_sgs) {
-					urb->num_sgs = n;
+				urb->num_mapped_sgs = n;
+				if (n != urb->num_sgs)
 					urb->transfer_flags |=
 							URB_DMA_SG_COMBINED;
-				}
 			} else if (urb->sg) {
 				struct scatterlist *sg = urb->sg;
 				urb->transfer_dma = dma_map_page(
@@ -2148,16 +2133,12 @@ irqreturn_t usb_hcd_irq (int irq, void *__hcd)
 	 */
 	local_irq_save(flags);
 
-	if (unlikely(HCD_DEAD(hcd) || !HCD_HW_ACCESSIBLE(hcd))) {
+	if (unlikely(HCD_DEAD(hcd) || !HCD_HW_ACCESSIBLE(hcd)))
 		rc = IRQ_NONE;
-	} else if (hcd->driver->irq(hcd) == IRQ_NONE) {
+	else if (hcd->driver->irq(hcd) == IRQ_NONE)
 		rc = IRQ_NONE;
-	} else {
-		set_bit(HCD_FLAG_SAW_IRQ, &hcd->flags);
-		if (hcd->shared_hcd)
-			set_bit(HCD_FLAG_SAW_IRQ, &hcd->shared_hcd->flags);
+	else
 		rc = IRQ_HANDLED;
-	}
 
 	local_irq_restore(flags);
 	return rc;
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 79781461eec9..79d339e2e700 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -84,7 +84,7 @@ struct usb_hub {
 
 static inline int hub_is_superspeed(struct usb_device *hdev)
 {
-	return (hdev->descriptor.bDeviceProtocol == 3);
+	return (hdev->descriptor.bDeviceProtocol == USB_HUB_PR_SS);
 }
 
 /* Protect struct usb_device->state and ->children members
@@ -1041,58 +1041,58 @@ static int hub_configure(struct usb_hub *hub,
 		dev_dbg(hub_dev, "standalone hub\n");
 
 	switch (wHubCharacteristics & HUB_CHAR_LPSM) {
-		case 0x00:
-			dev_dbg(hub_dev, "ganged power switching\n");
-			break;
-		case 0x01:
-			dev_dbg(hub_dev, "individual port power switching\n");
-			break;
-		case 0x02:
-		case 0x03:
-			dev_dbg(hub_dev, "no power switching (usb 1.0)\n");
-			break;
+	case HUB_CHAR_COMMON_LPSM:
+		dev_dbg(hub_dev, "ganged power switching\n");
+		break;
+	case HUB_CHAR_INDV_PORT_LPSM:
+		dev_dbg(hub_dev, "individual port power switching\n");
+		break;
+	case HUB_CHAR_NO_LPSM:
+	case HUB_CHAR_LPSM:
+		dev_dbg(hub_dev, "no power switching (usb 1.0)\n");
+		break;
 	}
 
 	switch (wHubCharacteristics & HUB_CHAR_OCPM) {
-		case 0x00:
-			dev_dbg(hub_dev, "global over-current protection\n");
-			break;
-		case 0x08:
-			dev_dbg(hub_dev, "individual port over-current protection\n");
-			break;
-		case 0x10:
-		case 0x18:
-			dev_dbg(hub_dev, "no over-current protection\n");
-                        break;
+	case HUB_CHAR_COMMON_OCPM:
+		dev_dbg(hub_dev, "global over-current protection\n");
+		break;
+	case HUB_CHAR_INDV_PORT_OCPM:
+		dev_dbg(hub_dev, "individual port over-current protection\n");
+		break;
+	case HUB_CHAR_NO_OCPM:
+	case HUB_CHAR_OCPM:
+		dev_dbg(hub_dev, "no over-current protection\n");
+		break;
 	}
 
 	spin_lock_init (&hub->tt.lock);
 	INIT_LIST_HEAD (&hub->tt.clear_list);
 	INIT_WORK(&hub->tt.clear_work, hub_tt_work);
 	switch (hdev->descriptor.bDeviceProtocol) {
-		case 0:
-			break;
-		case 1:
-			dev_dbg(hub_dev, "Single TT\n");
-			hub->tt.hub = hdev;
-			break;
-		case 2:
-			ret = usb_set_interface(hdev, 0, 1);
-			if (ret == 0) {
-				dev_dbg(hub_dev, "TT per port\n");
-				hub->tt.multi = 1;
-			} else
-				dev_err(hub_dev, "Using single TT (err %d)\n",
-					ret);
-			hub->tt.hub = hdev;
-			break;
-		case 3:
-			/* USB 3.0 hubs don't have a TT */
-			break;
-		default:
-			dev_dbg(hub_dev, "Unrecognized hub protocol %d\n",
-				hdev->descriptor.bDeviceProtocol);
-			break;
+	case USB_HUB_PR_FS:
+		break;
+	case USB_HUB_PR_HS_SINGLE_TT:
+		dev_dbg(hub_dev, "Single TT\n");
+		hub->tt.hub = hdev;
+		break;
+	case USB_HUB_PR_HS_MULTI_TT:
+		ret = usb_set_interface(hdev, 0, 1);
+		if (ret == 0) {
+			dev_dbg(hub_dev, "TT per port\n");
+			hub->tt.multi = 1;
+		} else
+			dev_err(hub_dev, "Using single TT (err %d)\n",
+				ret);
+		hub->tt.hub = hdev;
+		break;
+	case USB_HUB_PR_SS:
+		/* USB 3.0 hubs don't have a TT */
+		break;
+	default:
+		dev_dbg(hub_dev, "Unrecognized hub protocol %d\n",
+			hdev->descriptor.bDeviceProtocol);
+		break;
 	}
 
 	/* Note 8 FS bit times == (8 bits / 12000000 bps) ~= 666ns */
@@ -1360,7 +1360,6 @@ descriptor_error:
 	return -ENODEV;
 }
 
-/* No BKL needed */
 static int
 hub_ioctl(struct usb_interface *intf, unsigned int code, void *user_data)
 {
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index ecf12e15a7ef..4c65eb6a867a 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -117,9 +117,12 @@ static const struct usb_device_id usb_quirk_list[] = {
 	{ USB_DEVICE(0x06a3, 0x0006), .driver_info =
 			USB_QUIRK_CONFIG_INTF_STRINGS },
 
-	/* Guillemot Webcam Hercules Dualpix Exchange*/
+	/* Guillemot Webcam Hercules Dualpix Exchange (2nd ID) */
 	{ USB_DEVICE(0x06f8, 0x0804), .driver_info = USB_QUIRK_RESET_RESUME },
 
+	/* Guillemot Webcam Hercules Dualpix Exchange*/
+	{ USB_DEVICE(0x06f8, 0x3005), .driver_info = USB_QUIRK_RESET_RESUME },
+
 	/* M-Systems Flash Disk Pioneers */
 	{ USB_DEVICE(0x08ec, 0x1000), .driver_info = USB_QUIRK_RESET_RESUME },
 
diff --git a/drivers/usb/core/usb.h b/drivers/usb/core/usb.h
index 3888778582c4..45e8479c377d 100644
--- a/drivers/usb/core/usb.h
+++ b/drivers/usb/core/usb.h
@@ -132,20 +132,6 @@ static inline int is_usb_device_driver(struct device_driver *drv)
 			for_devices;
 }
 
-/* translate USB error codes to codes user space understands */
-static inline int usb_translate_errors(int error_code)
-{
-	switch (error_code) {
-	case 0:
-	case -ENOMEM:
-	case -ENODEV:
-		return error_code;
-	default:
-		return -EIO;
-	}
-}
-
-
 /* for labeling diagnostics */
 extern const char *usbcore_name;
 
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index 3c1d67d324fd..d8f741f9e56e 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -1,7 +1,10 @@
 config USB_DWC3
 	tristate "DesignWare USB3 DRD Core Support"
-	depends on (USB || USB_GADGET)
+	depends on (USB && USB_GADGET)
 	select USB_OTG_UTILS
+	select USB_GADGET_DUALSPEED
+	select USB_GADGET_SUPERSPEED
+	select USB_XHCI_PLATFORM
 	help
 	  Say Y or M here if your system has a Dual Role SuperSpeed
 	  USB controller based on the DesignWare USB3 IP Core.
diff --git a/drivers/usb/dwc3/Makefile b/drivers/usb/dwc3/Makefile
index 593d1dbc465b..900ae74357f1 100644
--- a/drivers/usb/dwc3/Makefile
+++ b/drivers/usb/dwc3/Makefile
@@ -4,10 +4,8 @@ ccflags-$(CONFIG_USB_DWC3_VERBOSE)	+= -DVERBOSE_DEBUG
 obj-$(CONFIG_USB_DWC3)			+= dwc3.o
 
 dwc3-y					:= core.o
-
-ifneq ($(CONFIG_USB_GADGET_DWC3),)
-	dwc3-y				+= gadget.o ep0.o
-endif
+dwc3-y					+= host.o
+dwc3-y					+= gadget.o ep0.o
 
 ifneq ($(CONFIG_DEBUG_FS),)
 	dwc3-y				+= debugfs.o
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index 600d82348511..7c9df630dbe4 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -59,6 +59,60 @@
 
 #include "debug.h"
 
+static char *maximum_speed = "super";
+module_param(maximum_speed, charp, 0);
+MODULE_PARM_DESC(maximum_speed, "Maximum supported speed.");
+
+/* -------------------------------------------------------------------------- */
+
+#define DWC3_DEVS_POSSIBLE	32
+
+static DECLARE_BITMAP(dwc3_devs, DWC3_DEVS_POSSIBLE);
+
+int dwc3_get_device_id(void)
+{
+	int		id;
+
+again:
+	id = find_first_zero_bit(dwc3_devs, DWC3_DEVS_POSSIBLE);
+	if (id < DWC3_DEVS_POSSIBLE) {
+		int old;
+
+		old = test_and_set_bit(id, dwc3_devs);
+		if (old)
+			goto again;
+	} else {
+		pr_err("dwc3: no space for new device\n");
+		id = -ENOMEM;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(dwc3_get_device_id);
+
+void dwc3_put_device_id(int id)
+{
+	int			ret;
+
+	if (id < 0)
+		return;
+
+	ret = test_bit(id, dwc3_devs);
+	WARN(!ret, "dwc3: ID %d not in use\n", id);
+	clear_bit(id, dwc3_devs);
+}
+EXPORT_SYMBOL_GPL(dwc3_put_device_id);
+
+void dwc3_set_mode(struct dwc3 *dwc, u32 mode)
+{
+	u32 reg;
+
+	reg = dwc3_readl(dwc->regs, DWC3_GCTL);
+	reg &= ~(DWC3_GCTL_PRTCAPDIR(DWC3_GCTL_PRTCAP_OTG));
+	reg |= DWC3_GCTL_PRTCAPDIR(mode);
+	dwc3_writel(dwc->regs, DWC3_GCTL, reg);
+}
+
 /**
  * dwc3_core_soft_reset - Issues core soft reset and PHY reset
  * @dwc: pointer to our context structure
@@ -150,7 +204,7 @@ static void dwc3_free_event_buffers(struct dwc3 *dwc)
 	struct dwc3_event_buffer	*evt;
 	int i;
 
-	for (i = 0; i < DWC3_EVENT_BUFFERS_NUM; i++) {
+	for (i = 0; i < dwc->num_event_buffers; i++) {
 		evt = dwc->ev_buffs[i];
 		if (evt) {
 			dwc3_free_one_event_buffer(dwc, evt);
@@ -162,17 +216,25 @@ static void dwc3_free_event_buffers(struct dwc3 *dwc)
 /**
  * dwc3_alloc_event_buffers - Allocates @num event buffers of size @length
  * @dwc: Pointer to out controller context structure
- * @num: number of event buffers to allocate
  * @length: size of event buffer
  *
  * Returns 0 on success otherwise negative errno. In error the case, dwc
  * may contain some buffers allocated but not all which were requested.
  */
-static int __devinit dwc3_alloc_event_buffers(struct dwc3 *dwc, unsigned num,
-		unsigned length)
+static int __devinit dwc3_alloc_event_buffers(struct dwc3 *dwc, unsigned length)
 {
+	int			num;
 	int			i;
 
+	num = DWC3_NUM_INT(dwc->hwparams.hwparams1);
+	dwc->num_event_buffers = num;
+
+	dwc->ev_buffs = kzalloc(sizeof(*dwc->ev_buffs) * num, GFP_KERNEL);
+	if (!dwc->ev_buffs) {
+		dev_err(dwc->dev, "can't allocate event buffers array\n");
+		return -ENOMEM;
+	}
+
 	for (i = 0; i < num; i++) {
 		struct dwc3_event_buffer	*evt;
 
@@ -198,7 +260,7 @@ static int __devinit dwc3_event_buffers_setup(struct dwc3 *dwc)
 	struct dwc3_event_buffer	*evt;
 	int				n;
 
-	for (n = 0; n < DWC3_EVENT_BUFFERS_NUM; n++) {
+	for (n = 0; n < dwc->num_event_buffers; n++) {
 		evt = dwc->ev_buffs[n];
 		dev_dbg(dwc->dev, "Event buf %p dma %08llx length %d\n",
 				evt->buf, (unsigned long long) evt->dma,
@@ -221,7 +283,7 @@ static void dwc3_event_buffers_cleanup(struct dwc3 *dwc)
 	struct dwc3_event_buffer	*evt;
 	int				n;
 
-	for (n = 0; n < DWC3_EVENT_BUFFERS_NUM; n++) {
+	for (n = 0; n < dwc->num_event_buffers; n++) {
 		evt = dwc->ev_buffs[n];
 		dwc3_writel(dwc->regs, DWC3_GEVNTADRLO(n), 0);
 		dwc3_writel(dwc->regs, DWC3_GEVNTADRHI(n), 0);
@@ -285,8 +347,32 @@ static int __devinit dwc3_core_init(struct dwc3 *dwc)
 		cpu_relax();
 	} while (true);
 
-	ret = dwc3_alloc_event_buffers(dwc, DWC3_EVENT_BUFFERS_NUM,
-			DWC3_EVENT_BUFFERS_SIZE);
+	dwc3_cache_hwparams(dwc);
+
+	reg = dwc3_readl(dwc->regs, DWC3_GCTL);
+	reg &= ~DWC3_GCTL_SCALEDOWN(3);
+	reg &= ~DWC3_GCTL_DISSCRAMBLE;
+
+	switch (DWC3_GHWPARAMS1_EN_PWROPT(dwc->hwparams.hwparams1)) {
+	case DWC3_GHWPARAMS1_EN_PWROPT_CLK:
+		reg &= ~DWC3_GCTL_DSBLCLKGTNG;
+		break;
+	default:
+		dev_dbg(dwc->dev, "No power optimization available\n");
+	}
+
+	/*
+	 * WORKAROUND: DWC3 revisions <1.90a have a bug
+	 * when The device fails to connect at SuperSpeed
+	 * and falls back to high-speed mode which causes
+	 * the device to enter in a Connect/Disconnect loop
+	 */
+	if (dwc->revision < DWC3_REVISION_190A)
+		reg |= DWC3_GCTL_U2RSTECN;
+
+	dwc3_writel(dwc->regs, DWC3_GCTL, reg);
+
+	ret = dwc3_alloc_event_buffers(dwc, DWC3_EVENT_BUFFERS_SIZE);
 	if (ret) {
 		dev_err(dwc->dev, "failed to allocate event buffers\n");
 		ret = -ENOMEM;
@@ -299,8 +385,6 @@ static int __devinit dwc3_core_init(struct dwc3 *dwc)
 		goto err1;
 	}
 
-	dwc3_cache_hwparams(dwc);
-
 	return 0;
 
 err1:
@@ -320,15 +404,17 @@ static void dwc3_core_exit(struct dwc3 *dwc)
 
 static int __devinit dwc3_probe(struct platform_device *pdev)
 {
-	const struct platform_device_id *id = platform_get_device_id(pdev);
 	struct resource		*res;
 	struct dwc3		*dwc;
-	void __iomem		*regs;
-	unsigned int		features = id->driver_data;
+
 	int			ret = -ENOMEM;
 	int			irq;
+
+	void __iomem		*regs;
 	void			*mem;
 
+	u8			mode;
+
 	mem = kzalloc(sizeof(*dwc) + DWC3_ALIGN_MASK, GFP_KERNEL);
 	if (!mem) {
 		dev_err(&pdev->dev, "not enough memory\n");
@@ -343,6 +429,8 @@ static int __devinit dwc3_probe(struct platform_device *pdev)
 		goto err1;
 	}
 
+	dwc->res = res;
+
 	res = request_mem_region(res->start, resource_size(res),
 			dev_name(&pdev->dev));
 	if (!res) {
@@ -370,6 +458,17 @@ static int __devinit dwc3_probe(struct platform_device *pdev)
 	dwc->dev	= &pdev->dev;
 	dwc->irq	= irq;
 
+	if (!strncmp("super", maximum_speed, 5))
+		dwc->maximum_speed = DWC3_DCFG_SUPERSPEED;
+	else if (!strncmp("high", maximum_speed, 4))
+		dwc->maximum_speed = DWC3_DCFG_HIGHSPEED;
+	else if (!strncmp("full", maximum_speed, 4))
+		dwc->maximum_speed = DWC3_DCFG_FULLSPEED1;
+	else if (!strncmp("low", maximum_speed, 3))
+		dwc->maximum_speed = DWC3_DCFG_LOWSPEED;
+	else
+		dwc->maximum_speed = DWC3_DCFG_SUPERSPEED;
+
 	pm_runtime_enable(&pdev->dev);
 	pm_runtime_get_sync(&pdev->dev);
 	pm_runtime_forbid(&pdev->dev);
@@ -380,13 +479,44 @@ static int __devinit dwc3_probe(struct platform_device *pdev)
 		goto err3;
 	}
 
-	if (features & DWC3_HAS_PERIPHERAL) {
+	mode = DWC3_MODE(dwc->hwparams.hwparams0);
+
+	switch (mode) {
+	case DWC3_MODE_DEVICE:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_DEVICE);
 		ret = dwc3_gadget_init(dwc);
 		if (ret) {
-			dev_err(&pdev->dev, "failed to initialized gadget\n");
+			dev_err(&pdev->dev, "failed to initialize gadget\n");
 			goto err4;
 		}
+		break;
+	case DWC3_MODE_HOST:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_HOST);
+		ret = dwc3_host_init(dwc);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to initialize host\n");
+			goto err4;
+		}
+		break;
+	case DWC3_MODE_DRD:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_OTG);
+		ret = dwc3_host_init(dwc);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to initialize host\n");
+			goto err4;
+		}
+
+		ret = dwc3_gadget_init(dwc);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to initialize gadget\n");
+			goto err4;
+		}
+		break;
+	default:
+		dev_err(&pdev->dev, "Unsupported mode of operation %d\n", mode);
+		goto err4;
 	}
+	dwc->mode = mode;
 
 	ret = dwc3_debugfs_init(dwc);
 	if (ret) {
@@ -399,8 +529,21 @@ static int __devinit dwc3_probe(struct platform_device *pdev)
 	return 0;
 
 err5:
-	if (features & DWC3_HAS_PERIPHERAL)
+	switch (mode) {
+	case DWC3_MODE_DEVICE:
+		dwc3_gadget_exit(dwc);
+		break;
+	case DWC3_MODE_HOST:
+		dwc3_host_exit(dwc);
+		break;
+	case DWC3_MODE_DRD:
+		dwc3_host_exit(dwc);
 		dwc3_gadget_exit(dwc);
+		break;
+	default:
+		/* do nothing */
+		break;
+	}
 
 err4:
 	dwc3_core_exit(dwc);
@@ -420,10 +563,8 @@ err0:
 
 static int __devexit dwc3_remove(struct platform_device *pdev)
 {
-	const struct platform_device_id *id = platform_get_device_id(pdev);
 	struct dwc3	*dwc = platform_get_drvdata(pdev);
 	struct resource	*res;
-	unsigned int	features = id->driver_data;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
@@ -432,8 +573,21 @@ static int __devexit dwc3_remove(struct platform_device *pdev)
 
 	dwc3_debugfs_exit(dwc);
 
-	if (features & DWC3_HAS_PERIPHERAL)
+	switch (dwc->mode) {
+	case DWC3_MODE_DEVICE:
+		dwc3_gadget_exit(dwc);
+		break;
+	case DWC3_MODE_HOST:
+		dwc3_host_exit(dwc);
+		break;
+	case DWC3_MODE_DRD:
+		dwc3_host_exit(dwc);
 		dwc3_gadget_exit(dwc);
+		break;
+	default:
+		/* do nothing */
+		break;
+	}
 
 	dwc3_core_exit(dwc);
 	release_mem_region(res->start, resource_size(res));
@@ -443,30 +597,15 @@ static int __devexit dwc3_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct platform_device_id dwc3_id_table[] __devinitconst = {
-	{
-		.name	= "dwc3-omap",
-		.driver_data = (DWC3_HAS_PERIPHERAL
-			| DWC3_HAS_XHCI
-			| DWC3_HAS_OTG),
-	},
-	{
-		.name	= "dwc3-pci",
-		.driver_data = DWC3_HAS_PERIPHERAL,
-	},
-	{  },	/* Terminating Entry */
-};
-MODULE_DEVICE_TABLE(platform, dwc3_id_table);
-
 static struct platform_driver dwc3_driver = {
 	.probe		= dwc3_probe,
 	.remove		= __devexit_p(dwc3_remove),
 	.driver		= {
 		.name	= "dwc3",
 	},
-	.id_table	= dwc3_id_table,
 };
 
+MODULE_ALIAS("platform:dwc3");
 MODULE_AUTHOR("Felipe Balbi <balbi@ti.com>");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_DESCRIPTION("DesignWare USB3 DRD Controller Driver");
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 29a8e1679e12..9e57f8e9bf17 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -41,6 +41,7 @@
 
 #include <linux/device.h>
 #include <linux/spinlock.h>
+#include <linux/ioport.h>
 #include <linux/list.h>
 #include <linux/dma-mapping.h>
 #include <linux/mm.h>
@@ -52,7 +53,6 @@
 /* Global constants */
 #define DWC3_ENDPOINTS_NUM	32
 
-#define DWC3_EVENT_BUFFERS_NUM	2
 #define DWC3_EVENT_BUFFERS_SIZE	PAGE_SIZE
 #define DWC3_EVENT_TYPE_MASK	0xfe
 
@@ -153,6 +153,7 @@
 #define DWC3_GCTL_CLK_PIPEHALF	(2)
 #define DWC3_GCTL_CLK_MASK	(3)
 
+#define DWC3_GCTL_PRTCAP(n)	(((n) & (3 << 12)) >> 12)
 #define DWC3_GCTL_PRTCAPDIR(n)	(n << 12)
 #define DWC3_GCTL_PRTCAP_HOST	1
 #define DWC3_GCTL_PRTCAP_DEVICE	2
@@ -347,6 +348,7 @@ struct dwc3_ep {
 	u32			free_slot;
 	u32			busy_slot;
 	const struct usb_endpoint_descriptor *desc;
+	const struct usb_ss_ep_comp_descriptor *comp_desc;
 	struct dwc3		*dwc;
 
 	unsigned		flags;
@@ -536,6 +538,31 @@ struct dwc3_hwparams {
 	u32	hwparams8;
 };
 
+/* HWPARAMS0 */
+#define DWC3_MODE(n)		((n) & 0x7)
+
+#define DWC3_MODE_DEVICE	0
+#define DWC3_MODE_HOST		1
+#define DWC3_MODE_DRD		2
+#define DWC3_MODE_HUB		3
+
+/* HWPARAMS1 */
+#define DWC3_NUM_INT(n)	(((n) & (0x3f << 15)) >> 15)
+
+struct dwc3_request {
+	struct usb_request	request;
+	struct list_head	list;
+	struct dwc3_ep		*dep;
+
+	u8			epnum;
+	struct dwc3_trb_hw	*trb;
+	dma_addr_t		trb_dma;
+
+	unsigned		direction:1;
+	unsigned		mapped:1;
+	unsigned		queued:1;
+};
+
 /**
  * struct dwc3 - representation of our controller
  * @ctrl_req: usb control request which is used for ep0
@@ -549,19 +576,24 @@ struct dwc3_hwparams {
  * @ep0_bounce_addr: dma address of ep0_bounce
  * @lock: for synchronizing
  * @dev: pointer to our struct device
+ * @xhci: pointer to our xHCI child
  * @event_buffer_list: a list of event buffers
  * @gadget: device side representation of the peripheral controller
  * @gadget_driver: pointer to the gadget driver
  * @regs: base address for our registers
  * @regs_size: address space size
  * @irq: IRQ number
+ * @num_event_buffers: calculated number of event buffers
+ * @u1u2: only used on revisions <1.83a for workaround
+ * @maximum_speed: maximum speed requested (mainly for testing purposes)
  * @revision: revision register contents
+ * @mode: mode of operation
  * @is_selfpowered: true when we are selfpowered
  * @three_stage_setup: set if we perform a three phase setup
- * @ep0_status_pending: ep0 status response without a req is pending
  * @ep0_bounced: true when we used bounce buffer
  * @ep0_expect_in: true when we expect a DATA IN transfer
  * @start_config_issued: true when StartConfig command has been issued
+ * @setup_packet_pending: true when there's a Setup Packet in FIFO. Workaround
  * @ep0_next_event: hold the next expected event
  * @ep0state: state of endpoint zero
  * @link_state: link state
@@ -579,12 +611,15 @@ struct dwc3 {
 	dma_addr_t		ep0_trb_addr;
 	dma_addr_t		setup_buf_addr;
 	dma_addr_t		ep0_bounce_addr;
-	struct usb_request	ep0_usb_req;
+	struct dwc3_request	ep0_usb_req;
 	/* device lock */
 	spinlock_t		lock;
 	struct device		*dev;
 
-	struct dwc3_event_buffer *ev_buffs[DWC3_EVENT_BUFFERS_NUM];
+	struct platform_device	*xhci;
+	struct resource		*res;
+
+	struct dwc3_event_buffer **ev_buffs;
 	struct dwc3_ep		*eps[DWC3_ENDPOINTS_NUM];
 
 	struct usb_gadget	gadget;
@@ -595,7 +630,11 @@ struct dwc3 {
 
 	int			irq;
 
+	u32			num_event_buffers;
+	u32			u1u2;
+	u32			maximum_speed;
 	u32			revision;
+	u32			mode;
 
 #define DWC3_REVISION_173A	0x5533173a
 #define DWC3_REVISION_175A	0x5533175a
@@ -607,10 +646,11 @@ struct dwc3 {
 
 	unsigned		is_selfpowered:1;
 	unsigned		three_stage_setup:1;
-	unsigned		ep0_status_pending:1;
 	unsigned		ep0_bounced:1;
 	unsigned		ep0_expect_in:1;
 	unsigned		start_config_issued:1;
+	unsigned		setup_packet_pending:1;
+	unsigned		delayed_status:1;
 
 	enum dwc3_ep0_next	ep0_next_event;
 	enum dwc3_ep0_state	ep0state;
@@ -765,4 +805,16 @@ union dwc3_event {
 #define DWC3_HAS_XHCI			BIT(1)
 #define DWC3_HAS_OTG			BIT(3)
 
+/* prototypes */
+void dwc3_set_mode(struct dwc3 *dwc, u32 mode);
+
+int dwc3_host_init(struct dwc3 *dwc);
+void dwc3_host_exit(struct dwc3 *dwc);
+
+int dwc3_gadget_init(struct dwc3 *dwc);
+void dwc3_gadget_exit(struct dwc3 *dwc);
+
+extern int dwc3_get_device_id(void);
+extern void dwc3_put_device_id(int id);
+
 #endif /* __DRIVERS_USB_DWC3_CORE_H */
diff --git a/drivers/usb/dwc3/debugfs.c b/drivers/usb/dwc3/debugfs.c
index fcfa91517ea1..433c97c15fc5 100644
--- a/drivers/usb/dwc3/debugfs.c
+++ b/drivers/usb/dwc3/debugfs.c
@@ -44,12 +44,12 @@
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/delay.h>
-
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 #include "core.h"
 #include "gadget.h"
 #include "io.h"
+#include "debug.h"
 
 #define dump_register(nm)				\
 {							\
@@ -395,6 +395,75 @@ static const struct file_operations dwc3_regdump_fops = {
 	.release		= single_release,
 };
 
+static int dwc3_mode_show(struct seq_file *s, void *unused)
+{
+	struct dwc3		*dwc = s->private;
+	unsigned long		flags;
+	u32			reg;
+
+	spin_lock_irqsave(&dwc->lock, flags);
+	reg = dwc3_readl(dwc->regs, DWC3_GCTL);
+	spin_unlock_irqrestore(&dwc->lock, flags);
+
+	switch (DWC3_GCTL_PRTCAP(reg)) {
+	case DWC3_GCTL_PRTCAP_HOST:
+		seq_printf(s, "host\n");
+		break;
+	case DWC3_GCTL_PRTCAP_DEVICE:
+		seq_printf(s, "device\n");
+		break;
+	case DWC3_GCTL_PRTCAP_OTG:
+		seq_printf(s, "OTG\n");
+		break;
+	default:
+		seq_printf(s, "UNKNOWN %08x\n", DWC3_GCTL_PRTCAP(reg));
+	}
+
+	return 0;
+}
+
+static int dwc3_mode_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dwc3_mode_show, inode->i_private);
+}
+
+static ssize_t dwc3_mode_write(struct file *file,
+		const char __user *ubuf, size_t count, loff_t *ppos)
+{
+	struct seq_file		*s = file->private_data;
+	struct dwc3		*dwc = s->private;
+	unsigned long		flags;
+	u32			mode = 0;
+	char			buf[32];
+
+	if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+		return -EFAULT;
+
+	if (!strncmp(buf, "host", 4))
+		mode |= DWC3_GCTL_PRTCAP_HOST;
+
+	if (!strncmp(buf, "device", 6))
+		mode |= DWC3_GCTL_PRTCAP_DEVICE;
+
+	if (!strncmp(buf, "otg", 3))
+		mode |= DWC3_GCTL_PRTCAP_OTG;
+
+	if (mode) {
+		spin_lock_irqsave(&dwc->lock, flags);
+		dwc3_set_mode(dwc, mode);
+		spin_unlock_irqrestore(&dwc->lock, flags);
+	}
+	return count;
+}
+
+static const struct file_operations dwc3_mode_fops = {
+	.open			= dwc3_mode_open,
+	.write			= dwc3_mode_write,
+	.read			= seq_read,
+	.llseek			= seq_lseek,
+	.release		= single_release,
+};
+
 int __devinit dwc3_debugfs_init(struct dwc3 *dwc)
 {
 	struct dentry		*root;
@@ -402,7 +471,7 @@ int __devinit dwc3_debugfs_init(struct dwc3 *dwc)
 	int			ret;
 
 	root = debugfs_create_dir(dev_name(dwc->dev), NULL);
-	if (IS_ERR(root)){
+	if (IS_ERR(root)) {
 		ret = PTR_ERR(root);
 		goto err0;
 	}
@@ -415,6 +484,14 @@ int __devinit dwc3_debugfs_init(struct dwc3 *dwc)
 		ret = PTR_ERR(file);
 		goto err1;
 	}
+
+	file = debugfs_create_file("mode", S_IRUGO | S_IWUSR, root,
+			dwc, &dwc3_mode_fops);
+	if (IS_ERR(file)) {
+		ret = PTR_ERR(file);
+		goto err1;
+	}
+
 	return 0;
 
 err1:
diff --git a/drivers/usb/dwc3/dwc3-omap.c b/drivers/usb/dwc3/dwc3-omap.c
index 062552b5fc8a..3274ac8f1200 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -48,6 +48,7 @@
 #include <linux/io.h>
 #include <linux/module.h>
 
+#include "core.h"
 #include "io.h"
 
 /*
@@ -200,6 +201,7 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 	struct dwc3_omap	*omap;
 	struct resource		*res;
 
+	int			devid;
 	int			ret = -ENOMEM;
 	int			irq;
 
@@ -236,16 +238,20 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 		goto err1;
 	}
 
-	dwc3 = platform_device_alloc("dwc3-omap", -1);
+	devid = dwc3_get_device_id();
+	if (devid < 0)
+		goto err2;
+
+	dwc3 = platform_device_alloc("dwc3", devid);
 	if (!dwc3) {
 		dev_err(&pdev->dev, "couldn't allocate dwc3 device\n");
-		goto err2;
+		goto err3;
 	}
 
 	context = kzalloc(resource_size(res), GFP_KERNEL);
 	if (!context) {
 		dev_err(&pdev->dev, "couldn't allocate dwc3 context memory\n");
-		goto err3;
+		goto err4;
 	}
 
 	spin_lock_init(&omap->lock);
@@ -299,7 +305,7 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 	if (ret) {
 		dev_err(&pdev->dev, "failed to request IRQ #%d --> %d\n",
 				omap->irq, ret);
-		goto err4;
+		goto err5;
 	}
 
 	/* enable all IRQs */
@@ -322,26 +328,29 @@ static int __devinit dwc3_omap_probe(struct platform_device *pdev)
 			pdev->num_resources);
 	if (ret) {
 		dev_err(&pdev->dev, "couldn't add resources to dwc3 device\n");
-		goto err5;
+		goto err6;
 	}
 
 	ret = platform_device_add(dwc3);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register dwc3 device\n");
-		goto err5;
+		goto err6;
 	}
 
 	return 0;
 
-err5:
+err6:
 	free_irq(omap->irq, omap);
 
-err4:
+err5:
 	kfree(omap->context);
 
-err3:
+err4:
 	platform_device_put(dwc3);
 
+err3:
+	dwc3_put_device_id(devid);
+
 err2:
 	iounmap(base);
 
@@ -358,6 +367,7 @@ static int __devexit dwc3_omap_remove(struct platform_device *pdev)
 
 	platform_device_unregister(omap->dwc3);
 
+	dwc3_put_device_id(omap->dwc3->id);
 	free_irq(omap->irq, omap);
 	iounmap(omap->base);
 
@@ -384,18 +394,9 @@ static struct platform_driver dwc3_omap_driver = {
 	},
 };
 
+module_platform_driver(dwc3_omap_driver);
+
+MODULE_ALIAS("platform:omap-dwc3");
 MODULE_AUTHOR("Felipe Balbi <balbi@ti.com>");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_DESCRIPTION("DesignWare USB3 OMAP Glue Layer");
-
-static int __devinit dwc3_omap_init(void)
-{
-	return platform_driver_register(&dwc3_omap_driver);
-}
-module_init(dwc3_omap_init);
-
-static void __exit dwc3_omap_exit(void)
-{
-	platform_driver_unregister(&dwc3_omap_driver);
-}
-module_exit(dwc3_omap_exit);
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index f77c00042685..64e1f7c67b08 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -42,52 +42,17 @@
 #include <linux/pci.h>
 #include <linux/platform_device.h>
 
+#include "core.h"
+
 /* FIXME define these in <linux/pci_ids.h> */
 #define PCI_VENDOR_ID_SYNOPSYS		0x16c3
 #define PCI_DEVICE_ID_SYNOPSYS_HAPSUSB3	0xabcd
 
-#define DWC3_PCI_DEVS_POSSIBLE	32
-
 struct dwc3_pci {
 	struct device		*dev;
 	struct platform_device	*dwc3;
 };
 
-static DECLARE_BITMAP(dwc3_pci_devs, DWC3_PCI_DEVS_POSSIBLE);
-
-static int dwc3_pci_get_device_id(struct dwc3_pci *glue)
-{
-	int		id;
-
-again:
-	id = find_first_zero_bit(dwc3_pci_devs, DWC3_PCI_DEVS_POSSIBLE);
-	if (id < DWC3_PCI_DEVS_POSSIBLE) {
-		int old;
-
-		old = test_and_set_bit(id, dwc3_pci_devs);
-		if (old)
-			goto again;
-	} else {
-		dev_err(glue->dev, "no space for new device\n");
-		id = -ENOMEM;
-	}
-
-	return 0;
-}
-
-static void dwc3_pci_put_device_id(struct dwc3_pci *glue, int id)
-{
-	int			ret;
-
-	if (id < 0)
-		return;
-
-	ret = test_bit(id, dwc3_pci_devs);
-	WARN(!ret, "Device: %s\nID %d not in use\n",
-			dev_driver_string(glue->dev), id);
-	clear_bit(id, dwc3_pci_devs);
-}
-
 static int __devinit dwc3_pci_probe(struct pci_dev *pci,
 		const struct pci_device_id *id)
 {
@@ -114,11 +79,11 @@ static int __devinit dwc3_pci_probe(struct pci_dev *pci,
 	pci_set_power_state(pci, PCI_D0);
 	pci_set_master(pci);
 
-	devid = dwc3_pci_get_device_id(glue);
+	devid = dwc3_get_device_id();
 	if (devid < 0)
 		goto err2;
 
-	dwc3 = platform_device_alloc("dwc3-pci", devid);
+	dwc3 = platform_device_alloc("dwc3", devid);
 	if (!dwc3) {
 		dev_err(&pci->dev, "couldn't allocate dwc3 device\n");
 		goto err3;
@@ -163,13 +128,13 @@ err4:
 	platform_device_put(dwc3);
 
 err3:
-	dwc3_pci_put_device_id(glue, devid);
+	dwc3_put_device_id(devid);
 
 err2:
 	pci_disable_device(pci);
 
 err1:
-	kfree(pci);
+	kfree(glue);
 
 err0:
 	return ret;
@@ -179,7 +144,7 @@ static void __devexit dwc3_pci_remove(struct pci_dev *pci)
 {
 	struct dwc3_pci	*glue = pci_get_drvdata(pci);
 
-	dwc3_pci_put_device_id(glue, glue->dwc3->id);
+	dwc3_put_device_id(glue->dwc3->id);
 	platform_device_unregister(glue->dwc3);
 	pci_set_drvdata(pci, NULL);
 	pci_disable_device(pci);
@@ -196,7 +161,7 @@ static DEFINE_PCI_DEVICE_TABLE(dwc3_pci_id_table) = {
 MODULE_DEVICE_TABLE(pci, dwc3_pci_id_table);
 
 static struct pci_driver dwc3_pci_driver = {
-	.name		= "pci-dwc3",
+	.name		= "dwc3-pci",
 	.id_table	= dwc3_pci_id_table,
 	.probe		= dwc3_pci_probe,
 	.remove		= __devexit_p(dwc3_pci_remove),
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 69a4e43ddf59..2f51de57593a 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -48,13 +48,13 @@
 
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb/composite.h>
 
 #include "core.h"
 #include "gadget.h"
 #include "io.h"
 
-static void dwc3_ep0_inspect_setup(struct dwc3 *dwc,
-		const struct dwc3_event_depevt *event);
+static void dwc3_ep0_do_control_status(struct dwc3 *dwc, u32 epnum);
 
 static const char *dwc3_ep0_state_string(enum dwc3_ep0_state state)
 {
@@ -125,6 +125,8 @@ static int dwc3_ep0_start_trans(struct dwc3 *dwc, u8 epnum, dma_addr_t buf_dma,
 static int __dwc3_gadget_ep0_queue(struct dwc3_ep *dep,
 		struct dwc3_request *req)
 {
+	struct dwc3		*dwc = dep->dwc;
+	u32			type;
 	int			ret = 0;
 
 	req->request.actual	= 0;
@@ -143,9 +145,7 @@ static int __dwc3_gadget_ep0_queue(struct dwc3_ep *dep,
 	 * IRQ we were waiting for is long gone.
 	 */
 	if (dep->flags & DWC3_EP_PENDING_REQUEST) {
-		struct dwc3	*dwc = dep->dwc;
 		unsigned	direction;
-		u32		type;
 
 		direction = !!(dep->flags & DWC3_EP0_DIR_IN);
 
@@ -165,6 +165,13 @@ static int __dwc3_gadget_ep0_queue(struct dwc3_ep *dep,
 				req->request.dma, req->request.length, type);
 		dep->flags &= ~(DWC3_EP_PENDING_REQUEST |
 				DWC3_EP0_DIR_IN);
+	} else if (dwc->delayed_status) {
+		dwc->delayed_status = false;
+
+		if (dwc->ep0state == EP0_STATUS_PHASE)
+			dwc3_ep0_do_control_status(dwc, 1);
+		else
+			dev_dbg(dwc->dev, "too early for delayed status\n");
 	}
 
 	return ret;
@@ -190,9 +197,7 @@ int dwc3_gadget_ep0_queue(struct usb_ep *ep, struct usb_request *request,
 	}
 
 	/* we share one TRB for ep0/1 */
-	if (!list_empty(&dwc->eps[0]->request_list) ||
-			!list_empty(&dwc->eps[1]->request_list) ||
-			dwc->ep0_status_pending) {
+	if (!list_empty(&dep->request_list)) {
 		ret = -EBUSY;
 		goto out;
 	}
@@ -214,8 +219,9 @@ static void dwc3_ep0_stall_and_restart(struct dwc3 *dwc)
 	struct dwc3_ep		*dep = dwc->eps[0];
 
 	/* stall is always issued on EP0 */
-	__dwc3_gadget_ep_set_halt(dwc->eps[0], 1);
-	dwc->eps[0]->flags = DWC3_EP_ENABLED;
+	__dwc3_gadget_ep_set_halt(dep, 1);
+	dep->flags = DWC3_EP_ENABLED;
+	dwc->delayed_status = false;
 
 	if (!list_empty(&dep->request_list)) {
 		struct dwc3_request	*req;
@@ -254,17 +260,14 @@ static struct dwc3_ep *dwc3_wIndex_to_dep(struct dwc3 *dwc, __le16 wIndex_le)
 	return NULL;
 }
 
-static void dwc3_ep0_send_status_response(struct dwc3 *dwc)
+static void dwc3_ep0_status_cmpl(struct usb_ep *ep, struct usb_request *req)
 {
-	dwc3_ep0_start_trans(dwc, 1, dwc->setup_buf_addr,
-			dwc->ep0_usb_req.length,
-			DWC3_TRBCTL_CONTROL_DATA);
 }
-
 /*
  * ch 9.4.5
  */
-static int dwc3_ep0_handle_status(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
+static int dwc3_ep0_handle_status(struct dwc3 *dwc,
+		struct usb_ctrlrequest *ctrl)
 {
 	struct dwc3_ep		*dep;
 	u32			recip;
@@ -291,7 +294,7 @@ static int dwc3_ep0_handle_status(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl
 	case USB_RECIP_ENDPOINT:
 		dep = dwc3_wIndex_to_dep(dwc, ctrl->wIndex);
 		if (!dep)
-		       return -EINVAL;
+			return -EINVAL;
 
 		if (dep->flags & DWC3_EP_STALL)
 			usb_status = 1 << USB_ENDPOINT_HALT;
@@ -302,10 +305,14 @@ static int dwc3_ep0_handle_status(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl
 
 	response_pkt = (__le16 *) dwc->setup_buf;
 	*response_pkt = cpu_to_le16(usb_status);
-	dwc->ep0_usb_req.length = sizeof(*response_pkt);
-	dwc->ep0_status_pending = 1;
 
-	return 0;
+	dep = dwc->eps[0];
+	dwc->ep0_usb_req.dep = dep;
+	dwc->ep0_usb_req.request.length = sizeof(*response_pkt);
+	dwc->ep0_usb_req.request.dma = dwc->setup_buf_addr;
+	dwc->ep0_usb_req.request.complete = dwc3_ep0_status_cmpl;
+
+	return __dwc3_gadget_ep0_queue(dep, &dwc->ep0_usb_req);
 }
 
 static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
@@ -396,8 +403,7 @@ static int dwc3_ep0_handle_feature(struct dwc3 *dwc,
 	case USB_RECIP_ENDPOINT:
 		switch (wValue) {
 		case USB_ENDPOINT_HALT:
-
-			dep =  dwc3_wIndex_to_dep(dwc, ctrl->wIndex);
+			dep =  dwc3_wIndex_to_dep(dwc, wIndex);
 			if (!dep)
 				return -EINVAL;
 			ret = __dwc3_gadget_ep_set_halt(dep, set);
@@ -422,8 +428,15 @@ static int dwc3_ep0_set_address(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 	u32 reg;
 
 	addr = le16_to_cpu(ctrl->wValue);
-	if (addr > 127)
+	if (addr > 127) {
+		dev_dbg(dwc->dev, "invalid device address %d\n", addr);
 		return -EINVAL;
+	}
+
+	if (dwc->dev_state == DWC3_CONFIGURED_STATE) {
+		dev_dbg(dwc->dev, "trying to set address when configured\n");
+		return -EINVAL;
+	}
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
 	reg &= ~(DWC3_DCFG_DEVADDR_MASK);
@@ -473,8 +486,10 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 		if (!cfg)
 			dwc->dev_state = DWC3_ADDRESS_STATE;
 		break;
+	default:
+		ret = -EINVAL;
 	}
-	return 0;
+	return ret;
 }
 
 static int dwc3_ep0_std_request(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
@@ -537,6 +552,9 @@ static void dwc3_ep0_inspect_setup(struct dwc3 *dwc,
 	else
 		ret = dwc3_ep0_delegate_req(dwc, ctrl);
 
+	if (ret == USB_GADGET_DELAYED_STATUS)
+		dwc->delayed_status = true;
+
 	if (ret >= 0)
 		return;
 
@@ -550,27 +568,21 @@ static void dwc3_ep0_complete_data(struct dwc3 *dwc,
 	struct dwc3_request	*r = NULL;
 	struct usb_request	*ur;
 	struct dwc3_trb		trb;
-	struct dwc3_ep		*dep;
+	struct dwc3_ep		*ep0;
 	u32			transferred;
 	u8			epnum;
 
 	epnum = event->endpoint_number;
-	dep = dwc->eps[epnum];
+	ep0 = dwc->eps[0];
 
 	dwc->ep0_next_event = DWC3_EP0_NRDY_STATUS;
 
-	if (!dwc->ep0_status_pending) {
-		r = next_request(&dwc->eps[0]->request_list);
-		ur = &r->request;
-	} else {
-		ur = &dwc->ep0_usb_req;
-		dwc->ep0_status_pending = 0;
-	}
+	r = next_request(&ep0->request_list);
+	ur = &r->request;
 
 	dwc3_trb_to_nat(dwc->ep0_trb, &trb);
 
 	if (dwc->ep0_bounced) {
-		struct dwc3_ep	*ep0 = dwc->eps[0];
 
 		transferred = min_t(u32, ur->length,
 				ep0->endpoint.maxpacket - trb.length);
@@ -591,7 +603,7 @@ static void dwc3_ep0_complete_data(struct dwc3 *dwc,
 		 * seems to be case when req.length > maxpacket. Could it be?
 		 */
 		if (r)
-			dwc3_gadget_giveback(dep, r, 0);
+			dwc3_gadget_giveback(ep0, r, 0);
 	}
 }
 
@@ -619,6 +631,7 @@ static void dwc3_ep0_xfer_complete(struct dwc3 *dwc,
 	struct dwc3_ep		*dep = dwc->eps[event->endpoint_number];
 
 	dep->flags &= ~DWC3_EP_BUSY;
+	dwc->setup_packet_pending = false;
 
 	switch (dwc->ep0state) {
 	case EP0_SETUP_PHASE:
@@ -643,7 +656,6 @@ static void dwc3_ep0_xfer_complete(struct dwc3 *dwc,
 static void dwc3_ep0_do_control_setup(struct dwc3 *dwc,
 		const struct dwc3_event_depevt *event)
 {
-	dwc->ep0state = EP0_SETUP_PHASE;
 	dwc3_ep0_out_start(dwc);
 }
 
@@ -655,12 +667,6 @@ static void dwc3_ep0_do_control_data(struct dwc3 *dwc,
 	int			ret;
 
 	dep = dwc->eps[0];
-	dwc->ep0state = EP0_DATA_PHASE;
-
-	if (dwc->ep0_status_pending) {
-		dwc3_ep0_send_status_response(dwc);
-		return;
-	}
 
 	if (list_empty(&dep->request_list)) {
 		dev_vdbg(dwc->dev, "pending request for EP0 Data phase\n");
@@ -674,7 +680,6 @@ static void dwc3_ep0_do_control_data(struct dwc3 *dwc,
 	req = next_request(&dep->request_list);
 	req->direction = !!event->endpoint_number;
 
-	dwc->ep0state = EP0_DATA_PHASE;
 	if (req->request.length == 0) {
 		ret = dwc3_ep0_start_trans(dwc, event->endpoint_number,
 				dwc->ctrl_req_addr, 0,
@@ -706,35 +711,79 @@ static void dwc3_ep0_do_control_data(struct dwc3 *dwc,
 	WARN_ON(ret < 0);
 }
 
-static void dwc3_ep0_do_control_status(struct dwc3 *dwc,
-		const struct dwc3_event_depevt *event)
+static int dwc3_ep0_start_control_status(struct dwc3_ep *dep)
 {
+	struct dwc3		*dwc = dep->dwc;
 	u32			type;
-	int			ret;
-
-	dwc->ep0state = EP0_STATUS_PHASE;
 
 	type = dwc->three_stage_setup ? DWC3_TRBCTL_CONTROL_STATUS3
 		: DWC3_TRBCTL_CONTROL_STATUS2;
 
-	ret = dwc3_ep0_start_trans(dwc, event->endpoint_number,
+	return dwc3_ep0_start_trans(dwc, dep->number,
 			dwc->ctrl_req_addr, 0, type);
+}
 
-	WARN_ON(ret < 0);
+static void dwc3_ep0_do_control_status(struct dwc3 *dwc, u32 epnum)
+{
+	struct dwc3_ep		*dep = dwc->eps[epnum];
+
+	WARN_ON(dwc3_ep0_start_control_status(dep));
 }
 
 static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 		const struct dwc3_event_depevt *event)
 {
+	dwc->setup_packet_pending = true;
+
+	/*
+	 * This part is very tricky: If we has just handled
+	 * XferNotReady(Setup) and we're now expecting a
+	 * XferComplete but, instead, we receive another
+	 * XferNotReady(Setup), we should STALL and restart
+	 * the state machine.
+	 *
+	 * In all other cases, we just continue waiting
+	 * for the XferComplete event.
+	 *
+	 * We are a little bit unsafe here because we're
+	 * not trying to ensure that last event was, indeed,
+	 * XferNotReady(Setup).
+	 *
+	 * Still, we don't expect any condition where that
+	 * should happen and, even if it does, it would be
+	 * another error condition.
+	 */
+	if (dwc->ep0_next_event == DWC3_EP0_COMPLETE) {
+		switch (event->status) {
+		case DEPEVT_STATUS_CONTROL_SETUP:
+			dev_vdbg(dwc->dev, "Unexpected XferNotReady(Setup)\n");
+			dwc3_ep0_stall_and_restart(dwc);
+			break;
+		case DEPEVT_STATUS_CONTROL_DATA:
+			/* FALLTHROUGH */
+		case DEPEVT_STATUS_CONTROL_STATUS:
+			/* FALLTHROUGH */
+		default:
+			dev_vdbg(dwc->dev, "waiting for XferComplete\n");
+		}
+
+		return;
+	}
+
 	switch (event->status) {
 	case DEPEVT_STATUS_CONTROL_SETUP:
 		dev_vdbg(dwc->dev, "Control Setup\n");
+
+		dwc->ep0state = EP0_SETUP_PHASE;
+
 		dwc3_ep0_do_control_setup(dwc, event);
 		break;
 
 	case DEPEVT_STATUS_CONTROL_DATA:
 		dev_vdbg(dwc->dev, "Control Data\n");
 
+		dwc->ep0state = EP0_DATA_PHASE;
+
 		if (dwc->ep0_next_event != DWC3_EP0_NRDY_DATA) {
 			dev_vdbg(dwc->dev, "Expected %d got %d\n",
 					dwc->ep0_next_event,
@@ -764,6 +813,8 @@ static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 	case DEPEVT_STATUS_CONTROL_STATUS:
 		dev_vdbg(dwc->dev, "Control Status\n");
 
+		dwc->ep0state = EP0_STATUS_PHASE;
+
 		if (dwc->ep0_next_event != DWC3_EP0_NRDY_STATUS) {
 			dev_vdbg(dwc->dev, "Expected %d got %d\n",
 					dwc->ep0_next_event,
@@ -772,12 +823,19 @@ static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 			dwc3_ep0_stall_and_restart(dwc);
 			return;
 		}
-		dwc3_ep0_do_control_status(dwc, event);
+
+		if (dwc->delayed_status) {
+			WARN_ON_ONCE(event->endpoint_number != 1);
+			dev_vdbg(dwc->dev, "Mass Storage delayed status\n");
+			return;
+		}
+
+		dwc3_ep0_do_control_status(dwc, event->endpoint_number);
 	}
 }
 
 void dwc3_ep0_interrupt(struct dwc3 *dwc,
-		const const struct dwc3_event_depevt *event)
+		const struct dwc3_event_depevt *event)
 {
 	u8			epnum = event->endpoint_number;
 
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 25dbd8614e72..a696bde53222 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -65,6 +65,22 @@ void dwc3_map_buffer_to_dma(struct dwc3_request *req)
 		return;
 	}
 
+	if (req->request.num_sgs) {
+		int	mapped;
+
+		mapped = dma_map_sg(dwc->dev, req->request.sg,
+				req->request.num_sgs,
+				req->direction ? DMA_TO_DEVICE
+				: DMA_FROM_DEVICE);
+		if (mapped < 0) {
+			dev_err(dwc->dev, "failed to map SGs\n");
+			return;
+		}
+
+		req->request.num_mapped_sgs = mapped;
+		return;
+	}
+
 	if (req->request.dma == DMA_ADDR_INVALID) {
 		req->request.dma = dma_map_single(dwc->dev, req->request.buf,
 				req->request.length, req->direction
@@ -82,6 +98,17 @@ void dwc3_unmap_buffer_from_dma(struct dwc3_request *req)
 		return;
 	}
 
+	if (req->request.num_mapped_sgs) {
+		req->request.dma = DMA_ADDR_INVALID;
+		dma_unmap_sg(dwc->dev, req->request.sg,
+				req->request.num_sgs,
+				req->direction ? DMA_TO_DEVICE
+				: DMA_FROM_DEVICE);
+
+		req->request.num_mapped_sgs = 0;
+		return;
+	}
+
 	if (req->mapped) {
 		dma_unmap_single(dwc->dev, req->request.dma,
 				req->request.length, req->direction
@@ -97,7 +124,11 @@ void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
 	struct dwc3			*dwc = dep->dwc;
 
 	if (req->queued) {
-		dep->busy_slot++;
+		if (req->request.num_mapped_sgs)
+			dep->busy_slot += req->request.num_mapped_sgs;
+		else
+			dep->busy_slot++;
+
 		/*
 		 * Skip LINK TRB. We can't use req->trb and check for
 		 * DWC3_TRBCTL_LINK_TRB because it points the TRB we just
@@ -108,6 +139,7 @@ void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
 			dep->busy_slot++;
 	}
 	list_del(&req->list);
+	req->trb = NULL;
 
 	if (req->request.status == -EINPROGRESS)
 		req->request.status = status;
@@ -251,7 +283,8 @@ static int dwc3_gadget_start_config(struct dwc3 *dwc, struct dwc3_ep *dep)
 }
 
 static int dwc3_gadget_set_ep_config(struct dwc3 *dwc, struct dwc3_ep *dep,
-		const struct usb_endpoint_descriptor *desc)
+		const struct usb_endpoint_descriptor *desc,
+		const struct usb_ss_ep_comp_descriptor *comp_desc)
 {
 	struct dwc3_gadget_ep_cmd_params params;
 
@@ -264,7 +297,7 @@ static int dwc3_gadget_set_ep_config(struct dwc3 *dwc, struct dwc3_ep *dep,
 	params.param1 = DWC3_DEPCFG_XFER_COMPLETE_EN
 		| DWC3_DEPCFG_XFER_NOT_READY_EN;
 
-	if (usb_endpoint_xfer_bulk(desc) && dep->endpoint.max_streams) {
+	if (usb_ss_max_streams(comp_desc) && usb_endpoint_xfer_bulk(desc)) {
 		params.param1 |= DWC3_DEPCFG_STREAM_CAPABLE
 			| DWC3_DEPCFG_STREAM_EVENT_EN;
 		dep->stream_capable = true;
@@ -317,7 +350,8 @@ static int dwc3_gadget_set_xfer_resource(struct dwc3 *dwc, struct dwc3_ep *dep)
  * Caller should take care of locking
  */
 static int __dwc3_gadget_ep_enable(struct dwc3_ep *dep,
-		const struct usb_endpoint_descriptor *desc)
+		const struct usb_endpoint_descriptor *desc,
+		const struct usb_ss_ep_comp_descriptor *comp_desc)
 {
 	struct dwc3		*dwc = dep->dwc;
 	u32			reg;
@@ -329,7 +363,7 @@ static int __dwc3_gadget_ep_enable(struct dwc3_ep *dep,
 			return ret;
 	}
 
-	ret = dwc3_gadget_set_ep_config(dwc, dep, desc);
+	ret = dwc3_gadget_set_ep_config(dwc, dep, desc, comp_desc);
 	if (ret)
 		return ret;
 
@@ -343,6 +377,7 @@ static int __dwc3_gadget_ep_enable(struct dwc3_ep *dep,
 			return ret;
 
 		dep->desc = desc;
+		dep->comp_desc = comp_desc;
 		dep->type = usb_endpoint_type(desc);
 		dep->flags |= DWC3_EP_ENABLED;
 
@@ -405,6 +440,7 @@ static int __dwc3_gadget_ep_disable(struct dwc3_ep *dep)
 
 	dep->stream_capable = false;
 	dep->desc = NULL;
+	dep->comp_desc = NULL;
 	dep->type = 0;
 	dep->flags = 0;
 
@@ -473,7 +509,7 @@ static int dwc3_gadget_ep_enable(struct usb_ep *ep,
 	dev_vdbg(dwc->dev, "Enabling %s\n", dep->name);
 
 	spin_lock_irqsave(&dwc->lock, flags);
-	ret = __dwc3_gadget_ep_enable(dep, desc);
+	ret = __dwc3_gadget_ep_enable(dep, desc, ep->comp_desc);
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
 	return ret;
@@ -539,6 +575,85 @@ static void dwc3_gadget_ep_free_request(struct usb_ep *ep,
 	kfree(req);
 }
 
+/**
+ * dwc3_prepare_one_trb - setup one TRB from one request
+ * @dep: endpoint for which this request is prepared
+ * @req: dwc3_request pointer
+ */
+static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
+		struct dwc3_request *req, dma_addr_t dma,
+		unsigned length, unsigned last, unsigned chain)
+{
+	struct dwc3		*dwc = dep->dwc;
+	struct dwc3_trb_hw	*trb_hw;
+	struct dwc3_trb		trb;
+
+	unsigned int		cur_slot;
+
+	dev_vdbg(dwc->dev, "%s: req %p dma %08llx length %d%s%s\n",
+			dep->name, req, (unsigned long long) dma,
+			length, last ? " last" : "",
+			chain ? " chain" : "");
+
+	trb_hw = &dep->trb_pool[dep->free_slot & DWC3_TRB_MASK];
+	cur_slot = dep->free_slot;
+	dep->free_slot++;
+
+	/* Skip the LINK-TRB on ISOC */
+	if (((cur_slot & DWC3_TRB_MASK) == DWC3_TRB_NUM - 1) &&
+			usb_endpoint_xfer_isoc(dep->desc))
+		return;
+
+	memset(&trb, 0, sizeof(trb));
+	if (!req->trb) {
+		dwc3_gadget_move_request_queued(req);
+		req->trb = trb_hw;
+		req->trb_dma = dwc3_trb_dma_offset(dep, trb_hw);
+	}
+
+	if (usb_endpoint_xfer_isoc(dep->desc)) {
+		trb.isp_imi = true;
+		trb.csp = true;
+	} else {
+		trb.chn = chain;
+		trb.lst = last;
+	}
+
+	if (usb_endpoint_xfer_bulk(dep->desc) && dep->stream_capable)
+		trb.sid_sofn = req->request.stream_id;
+
+	switch (usb_endpoint_type(dep->desc)) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		trb.trbctl = DWC3_TRBCTL_CONTROL_SETUP;
+		break;
+
+	case USB_ENDPOINT_XFER_ISOC:
+		trb.trbctl = DWC3_TRBCTL_ISOCHRONOUS_FIRST;
+
+		/* IOC every DWC3_TRB_NUM / 4 so we can refill */
+		if (!(cur_slot % (DWC3_TRB_NUM / 4)))
+			trb.ioc = last;
+		break;
+
+	case USB_ENDPOINT_XFER_BULK:
+	case USB_ENDPOINT_XFER_INT:
+		trb.trbctl = DWC3_TRBCTL_NORMAL;
+		break;
+	default:
+		/*
+		 * This is only possible with faulty memory because we
+		 * checked it already :)
+		 */
+		BUG();
+	}
+
+	trb.length	= length;
+	trb.bplh	= dma;
+	trb.hwo		= true;
+
+	dwc3_trb_to_hw(&trb, trb_hw);
+}
+
 /*
  * dwc3_prepare_trbs - setup TRBs from requests
  * @dep: endpoint for which requests are being prepared
@@ -548,18 +663,17 @@ static void dwc3_gadget_ep_free_request(struct usb_ep *ep,
  * transfers. The functions returns once there are not more TRBs available or
  * it run out of requests.
  */
-static struct dwc3_request *dwc3_prepare_trbs(struct dwc3_ep *dep,
-		bool starting)
+static void dwc3_prepare_trbs(struct dwc3_ep *dep, bool starting)
 {
-	struct dwc3_request	*req, *n, *ret = NULL;
-	struct dwc3_trb_hw	*trb_hw;
-	struct dwc3_trb		trb;
+	struct dwc3_request	*req, *n;
 	u32			trbs_left;
+	unsigned int		last_one = 0;
 
 	BUILD_BUG_ON_NOT_POWER_OF_2(DWC3_TRB_NUM);
 
 	/* the first request must not be queued */
 	trbs_left = (dep->busy_slot - dep->free_slot) & DWC3_TRB_MASK;
+
 	/*
 	 * if busy & slot are equal than it is either full or empty. If we are
 	 * starting to proceed requests then we are empty. Otherwise we ar
@@ -567,7 +681,7 @@ static struct dwc3_request *dwc3_prepare_trbs(struct dwc3_ep *dep,
 	 */
 	if (!trbs_left) {
 		if (!starting)
-			return NULL;
+			return;
 		trbs_left = DWC3_TRB_NUM;
 		/*
 		 * In case we start from scratch, we queue the ISOC requests
@@ -591,94 +705,62 @@ static struct dwc3_request *dwc3_prepare_trbs(struct dwc3_ep *dep,
 
 	/* The last TRB is a link TRB, not used for xfer */
 	if ((trbs_left <= 1) && usb_endpoint_xfer_isoc(dep->desc))
-		return NULL;
+		return;
 
 	list_for_each_entry_safe(req, n, &dep->request_list, list) {
-		unsigned int last_one = 0;
-		unsigned int cur_slot;
+		unsigned	length;
+		dma_addr_t	dma;
 
-		trb_hw = &dep->trb_pool[dep->free_slot & DWC3_TRB_MASK];
-		cur_slot = dep->free_slot;
-		dep->free_slot++;
+		if (req->request.num_mapped_sgs > 0) {
+			struct usb_request *request = &req->request;
+			struct scatterlist *sg = request->sg;
+			struct scatterlist *s;
+			int		i;
 
-		/* Skip the LINK-TRB on ISOC */
-		if (((cur_slot & DWC3_TRB_MASK) == DWC3_TRB_NUM - 1) &&
-				usb_endpoint_xfer_isoc(dep->desc))
-			continue;
+			for_each_sg(sg, s, request->num_mapped_sgs, i) {
+				unsigned chain = true;
 
-		dwc3_gadget_move_request_queued(req);
-		memset(&trb, 0, sizeof(trb));
-		trbs_left--;
+				length = sg_dma_len(s);
+				dma = sg_dma_address(s);
 
-		/* Is our TRB pool empty? */
-		if (!trbs_left)
-			last_one = 1;
-		/* Is this the last request? */
-		if (list_empty(&dep->request_list))
-			last_one = 1;
+				if (i == (request->num_mapped_sgs - 1)
+						|| sg_is_last(s)) {
+					last_one = true;
+					chain = false;
+				}
 
-		/*
-		 * FIXME we shouldn't need to set LST bit always but we are
-		 * facing some weird problem with the Hardware where it doesn't
-		 * complete even though it has been previously started.
-		 *
-		 * While we're debugging the problem, as a workaround to
-		 * multiple TRBs handling, use only one TRB at a time.
-		 */
-		last_one = 1;
+				trbs_left--;
+				if (!trbs_left)
+					last_one = true;
 
-		req->trb = trb_hw;
-		if (!ret)
-			ret = req;
+				if (last_one)
+					chain = false;
 
-		trb.bplh = req->request.dma;
+				dwc3_prepare_one_trb(dep, req, dma, length,
+						last_one, chain);
 
-		if (usb_endpoint_xfer_isoc(dep->desc)) {
-			trb.isp_imi = true;
-			trb.csp = true;
+				if (last_one)
+					break;
+			}
 		} else {
-			trb.lst = last_one;
-		}
+			dma = req->request.dma;
+			length = req->request.length;
+			trbs_left--;
 
-		if (usb_endpoint_xfer_bulk(dep->desc) && dep->stream_capable)
-			trb.sid_sofn = req->request.stream_id;
-
-		switch (usb_endpoint_type(dep->desc)) {
-		case USB_ENDPOINT_XFER_CONTROL:
-			trb.trbctl = DWC3_TRBCTL_CONTROL_SETUP;
-			break;
+			if (!trbs_left)
+				last_one = 1;
 
-		case USB_ENDPOINT_XFER_ISOC:
-			trb.trbctl = DWC3_TRBCTL_ISOCHRONOUS_FIRST;
+			/* Is this the last request? */
+			if (list_is_last(&req->list, &dep->request_list))
+				last_one = 1;
 
-			/* IOC every DWC3_TRB_NUM / 4 so we can refill */
-			if (!(cur_slot % (DWC3_TRB_NUM / 4)))
-				trb.ioc = last_one;
-			break;
+			dwc3_prepare_one_trb(dep, req, dma, length,
+					last_one, false);
 
-		case USB_ENDPOINT_XFER_BULK:
-		case USB_ENDPOINT_XFER_INT:
-			trb.trbctl = DWC3_TRBCTL_NORMAL;
-			break;
-		default:
-			/*
-			 * This is only possible with faulty memory because we
-			 * checked it already :)
-			 */
-			BUG();
+			if (last_one)
+				break;
 		}
-
-		trb.length	= req->request.length;
-		trb.hwo = true;
-
-		dwc3_trb_to_hw(&trb, trb_hw);
-		req->trb_dma = dwc3_trb_dma_offset(dep, trb_hw);
-
-		if (last_one)
-			break;
 	}
-
-	return ret;
 }
 
 static int __dwc3_gadget_kick_transfer(struct dwc3_ep *dep, u16 cmd_param,
@@ -707,11 +789,13 @@ static int __dwc3_gadget_kick_transfer(struct dwc3_ep *dep, u16 cmd_param,
 		/* req points to the first request which will be sent */
 		req = next_request(&dep->req_queued);
 	} else {
+		dwc3_prepare_trbs(dep, start_new);
+
 		/*
 		 * req points to the first request where HWO changed
 		 * from 0 to 1
 		 */
-		req = dwc3_prepare_trbs(dep, start_new);
+		req = next_request(&dep->req_queued);
 	}
 	if (!req) {
 		dep->flags |= DWC3_EP_PENDING_REQUEST;
@@ -745,8 +829,9 @@ static int __dwc3_gadget_kick_transfer(struct dwc3_ep *dep, u16 cmd_param,
 	dep->flags |= DWC3_EP_BUSY;
 	dep->res_trans_idx = dwc3_gadget_ep_get_transfer_index(dwc,
 			dep->number);
-	if (!dep->res_trans_idx)
-		printk_once(KERN_ERR "%s() res_trans_idx is invalid\n", __func__);
+
+	WARN_ON_ONCE(!dep->res_trans_idx);
+
 	return 0;
 }
 
@@ -1155,35 +1240,9 @@ static int dwc3_gadget_start(struct usb_gadget *g,
 	dwc->gadget_driver	= driver;
 	dwc->gadget.dev.driver	= &driver->driver;
 
-	reg = dwc3_readl(dwc->regs, DWC3_GCTL);
-
-	reg &= ~DWC3_GCTL_SCALEDOWN(3);
-	reg &= ~DWC3_GCTL_PRTCAPDIR(DWC3_GCTL_PRTCAP_OTG);
-	reg &= ~DWC3_GCTL_DISSCRAMBLE;
-	reg |= DWC3_GCTL_PRTCAPDIR(DWC3_GCTL_PRTCAP_DEVICE);
-
-	switch (DWC3_GHWPARAMS1_EN_PWROPT(dwc->hwparams.hwparams0)) {
-	case DWC3_GHWPARAMS1_EN_PWROPT_CLK:
-		reg &= ~DWC3_GCTL_DSBLCLKGTNG;
-		break;
-	default:
-		dev_dbg(dwc->dev, "No power optimization available\n");
-	}
-
-	/*
-	 * WORKAROUND: DWC3 revisions <1.90a have a bug
-	 * when The device fails to connect at SuperSpeed
-	 * and falls back to high-speed mode which causes
-	 * the device to enter in a Connect/Disconnect loop
-	 */
-	if (dwc->revision < DWC3_REVISION_190A)
-		reg |= DWC3_GCTL_U2RSTECN;
-
-	dwc3_writel(dwc->regs, DWC3_GCTL, reg);
-
 	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
 	reg &= ~(DWC3_DCFG_SPEED_MASK);
-	reg |= DWC3_DCFG_SUPERSPEED;
+	reg |= dwc->maximum_speed;
 	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
 
 	dwc->start_config_issued = false;
@@ -1192,14 +1251,14 @@ static int dwc3_gadget_start(struct usb_gadget *g,
 	dwc3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
 
 	dep = dwc->eps[0];
-	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc);
+	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc, NULL);
 	if (ret) {
 		dev_err(dwc->dev, "failed to enable %s\n", dep->name);
 		goto err0;
 	}
 
 	dep = dwc->eps[1];
-	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc);
+	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc, NULL);
 	if (ret) {
 		dev_err(dwc->dev, "failed to enable %s\n", dep->name);
 		goto err1;
@@ -1290,11 +1349,10 @@ static int __devinit dwc3_gadget_init_endpoints(struct dwc3 *dwc)
 					&dwc->gadget.ep_list);
 
 			ret = dwc3_alloc_trb_pool(dep);
-			if (ret) {
-				dev_err(dwc->dev, "%s: failed to allocate TRB pool\n", dep->name);
+			if (ret)
 				return ret;
-			}
 		}
+
 		INIT_LIST_HEAD(&dep->request_list);
 		INIT_LIST_HEAD(&dep->req_queued);
 	}
@@ -1334,8 +1392,10 @@ static int dwc3_cleanup_done_reqs(struct dwc3 *dwc, struct dwc3_ep *dep,
 
 	do {
 		req = next_request(&dep->req_queued);
-		if (!req)
-			break;
+		if (!req) {
+			WARN_ON_ONCE(1);
+			return 1;
+		}
 
 		dwc3_trb_to_nat(req->trb, &trb);
 
@@ -1400,6 +1460,31 @@ static void dwc3_endpoint_transfer_complete(struct dwc3 *dwc,
 		dep->flags &= ~DWC3_EP_BUSY;
 		dep->res_trans_idx = 0;
 	}
+
+	/*
+	 * WORKAROUND: This is the 2nd half of U1/U2 -> U0 workaround.
+	 * See dwc3_gadget_linksts_change_interrupt() for 1st half.
+	 */
+	if (dwc->revision < DWC3_REVISION_183A) {
+		u32		reg;
+		int		i;
+
+		for (i = 0; i < DWC3_ENDPOINTS_NUM; i++) {
+			struct dwc3_ep	*dep = dwc->eps[i];
+
+			if (!(dep->flags & DWC3_EP_ENABLED))
+				continue;
+
+			if (!list_empty(&dep->req_queued))
+				return;
+		}
+
+		reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+		reg |= dwc->u1u2;
+		dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+
+		dwc->u1u2 = 0;
+	}
 }
 
 static void dwc3_gadget_start_isoc(struct dwc3 *dwc,
@@ -1639,6 +1724,7 @@ static void dwc3_gadget_disconnect_interrupt(struct dwc3 *dwc)
 	dwc->start_config_issued = false;
 
 	dwc->gadget.speed = USB_SPEED_UNKNOWN;
+	dwc->setup_packet_pending = false;
 }
 
 static void dwc3_gadget_usb3_phy_power(struct dwc3 *dwc, int on)
@@ -1675,6 +1761,40 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 
 	dev_vdbg(dwc->dev, "%s\n", __func__);
 
+	/*
+	 * WORKAROUND: DWC3 revisions <1.88a have an issue which
+	 * would cause a missing Disconnect Event if there's a
+	 * pending Setup Packet in the FIFO.
+	 *
+	 * There's no suggested workaround on the official Bug
+	 * report, which states that "unless the driver/application
+	 * is doing any special handling of a disconnect event,
+	 * there is no functional issue".
+	 *
+	 * Unfortunately, it turns out that we _do_ some special
+	 * handling of a disconnect event, namely complete all
+	 * pending transfers, notify gadget driver of the
+	 * disconnection, and so on.
+	 *
+	 * Our suggested workaround is to follow the Disconnect
+	 * Event steps here, instead, based on a setup_packet_pending
+	 * flag. Such flag gets set whenever we have a XferNotReady
+	 * event on EP0 and gets cleared on XferComplete for the
+	 * same endpoint.
+	 *
+	 * Refers to:
+	 *
+	 * STAR#9000466709: RTL: Device : Disconnect event not
+	 * generated if setup packet pending in FIFO
+	 */
+	if (dwc->revision < DWC3_REVISION_188A) {
+		if (dwc->setup_packet_pending)
+			dwc3_gadget_disconnect_interrupt(dwc);
+	}
+
+	/* after reset -> Default State */
+	dwc->dev_state = DWC3_DEFAULT_STATE;
+
 	/* Enable PHYs */
 	dwc3_gadget_usb2_phy_power(dwc, true);
 	dwc3_gadget_usb3_phy_power(dwc, true);
@@ -1755,6 +1875,22 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
 
 	switch (speed) {
 	case DWC3_DCFG_SUPERSPEED:
+		/*
+		 * WORKAROUND: DWC3 revisions <1.90a have an issue which
+		 * would cause a missing USB3 Reset event.
+		 *
+		 * In such situations, we should force a USB3 Reset
+		 * event by calling our dwc3_gadget_reset_interrupt()
+		 * routine.
+		 *
+		 * Refers to:
+		 *
+		 * STAR#9000483510: RTL: SS : USB3 reset event may
+		 * not be generated always when the link enters poll
+		 */
+		if (dwc->revision < DWC3_REVISION_190A)
+			dwc3_gadget_reset_interrupt(dwc);
+
 		dwc3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
 		dwc->gadget.ep0->maxpacket = 512;
 		dwc->gadget.speed = USB_SPEED_SUPER;
@@ -1781,14 +1917,14 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
 	dwc3_gadget_disable_phy(dwc, dwc->gadget.speed);
 
 	dep = dwc->eps[0];
-	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc);
+	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc, NULL);
 	if (ret) {
 		dev_err(dwc->dev, "failed to enable %s\n", dep->name);
 		return;
 	}
 
 	dep = dwc->eps[1];
-	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc);
+	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc, NULL);
 	if (ret) {
 		dev_err(dwc->dev, "failed to enable %s\n", dep->name);
 		return;
@@ -1818,8 +1954,55 @@ static void dwc3_gadget_wakeup_interrupt(struct dwc3 *dwc)
 static void dwc3_gadget_linksts_change_interrupt(struct dwc3 *dwc,
 		unsigned int evtinfo)
 {
-	/*  The fith bit says SuperSpeed yes or no. */
-	dwc->link_state = evtinfo & DWC3_LINK_STATE_MASK;
+	enum dwc3_link_state	next = evtinfo & DWC3_LINK_STATE_MASK;
+
+	/*
+	 * WORKAROUND: DWC3 Revisions <1.83a have an issue which, depending
+	 * on the link partner, the USB session might do multiple entry/exit
+	 * of low power states before a transfer takes place.
+	 *
+	 * Due to this problem, we might experience lower throughput. The
+	 * suggested workaround is to disable DCTL[12:9] bits if we're
+	 * transitioning from U1/U2 to U0 and enable those bits again
+	 * after a transfer completes and there are no pending transfers
+	 * on any of the enabled endpoints.
+	 *
+	 * This is the first half of that workaround.
+	 *
+	 * Refers to:
+	 *
+	 * STAR#9000446952: RTL: Device SS : if U1/U2 ->U0 takes >128us
+	 * core send LGO_Ux entering U0
+	 */
+	if (dwc->revision < DWC3_REVISION_183A) {
+		if (next == DWC3_LINK_STATE_U0) {
+			u32	u1u2;
+			u32	reg;
+
+			switch (dwc->link_state) {
+			case DWC3_LINK_STATE_U1:
+			case DWC3_LINK_STATE_U2:
+				reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+				u1u2 = reg & (DWC3_DCTL_INITU2ENA
+						| DWC3_DCTL_ACCEPTU2ENA
+						| DWC3_DCTL_INITU1ENA
+						| DWC3_DCTL_ACCEPTU1ENA);
+
+				if (!dwc->u1u2)
+					dwc->u1u2 = reg & u1u2;
+
+				reg &= ~u1u2;
+
+				dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+				break;
+			default:
+				/* do nothing */
+				break;
+			}
+		}
+	}
+
+	dwc->link_state = next;
 
 	dev_vdbg(dwc->dev, "%s link %d\n", __func__, dwc->link_state);
 }
@@ -1925,7 +2108,7 @@ static irqreturn_t dwc3_interrupt(int irq, void *_dwc)
 
 	spin_lock(&dwc->lock);
 
-	for (i = 0; i < DWC3_EVENT_BUFFERS_NUM; i++) {
+	for (i = 0; i < dwc->num_event_buffers; i++) {
 		irqreturn_t status;
 
 		status = dwc3_process_event_buf(dwc, i);
@@ -1986,9 +2169,10 @@ int __devinit dwc3_gadget_init(struct dwc3 *dwc)
 	dev_set_name(&dwc->gadget.dev, "gadget");
 
 	dwc->gadget.ops			= &dwc3_gadget_ops;
-	dwc->gadget.is_dualspeed	= true;
+	dwc->gadget.max_speed		= USB_SPEED_SUPER;
 	dwc->gadget.speed		= USB_SPEED_UNKNOWN;
 	dwc->gadget.dev.parent		= dwc->dev;
+	dwc->gadget.sg_supported	= true;
 
 	dma_set_coherent_mask(&dwc->gadget.dev, dwc->dev->coherent_dma_mask);
 
@@ -2076,7 +2260,6 @@ err0:
 void dwc3_gadget_exit(struct dwc3 *dwc)
 {
 	int			irq;
-	int			i;
 
 	usb_del_gadget_udc(&dwc->gadget);
 	irq = platform_get_irq(to_platform_device(dwc->dev), 0);
@@ -2084,9 +2267,6 @@ void dwc3_gadget_exit(struct dwc3 *dwc)
 	dwc3_writel(dwc->regs, DWC3_DEVTEN, 0x00);
 	free_irq(irq, dwc);
 
-	for (i = 0; i < ARRAY_SIZE(dwc->eps); i++)
-		__dwc3_gadget_ep_disable(dwc->eps[i]);
-
 	dwc3_gadget_free_endpoints(dwc);
 
 	dma_free_coherent(dwc->dev, 512, dwc->ep0_bounce,
diff --git a/drivers/usb/dwc3/gadget.h b/drivers/usb/dwc3/gadget.h
index 71145a449d99..d97f467d41cc 100644
--- a/drivers/usb/dwc3/gadget.h
+++ b/drivers/usb/dwc3/gadget.h
@@ -79,19 +79,6 @@ struct dwc3_gadget_ep_cmd_params {
 
 /* -------------------------------------------------------------------------- */
 
-struct dwc3_request {
-	struct usb_request	request;
-	struct list_head	list;
-	struct dwc3_ep		*dep;
-
-	u8			epnum;
-	struct dwc3_trb_hw	*trb;
-	dma_addr_t		trb_dma;
-
-	unsigned		direction:1;
-	unsigned		mapped:1;
-	unsigned		queued:1;
-};
 #define to_dwc3_request(r)	(container_of(r, struct dwc3_request, request))
 
 static inline struct dwc3_request *next_request(struct list_head *list)
@@ -110,23 +97,11 @@ static inline void dwc3_gadget_move_request_queued(struct dwc3_request *req)
 	list_move_tail(&req->list, &dep->req_queued);
 }
 
-#if defined(CONFIG_USB_GADGET_DWC3) || defined(CONFIG_USB_GADGET_DWC3_MODULE)
-int dwc3_gadget_init(struct dwc3 *dwc);
-void dwc3_gadget_exit(struct dwc3 *dwc);
-#else
-static inline int dwc3_gadget_init(struct dwc3 *dwc) { return 0; }
-static inline void dwc3_gadget_exit(struct dwc3 *dwc) { }
-static inline int dwc3_send_gadget_ep_cmd(struct dwc3 *dwc, unsigned ep,
-		unsigned cmd, struct dwc3_gadget_ep_cmd_params *params)
-{
-	return 0;
-}
-#endif
-
 void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
 		int status);
 
-void dwc3_ep0_interrupt(struct dwc3 *dwc, const struct dwc3_event_depevt *event);
+void dwc3_ep0_interrupt(struct dwc3 *dwc,
+		const struct dwc3_event_depevt *event);
 void dwc3_ep0_out_start(struct dwc3 *dwc);
 int dwc3_gadget_ep0_queue(struct usb_ep *ep, struct usb_request *request,
 		gfp_t gfp_flags);
diff --git a/drivers/usb/dwc3/host.c b/drivers/usb/dwc3/host.c
new file mode 100644
index 000000000000..7cfe211b6c37
--- /dev/null
+++ b/drivers/usb/dwc3/host.c
@@ -0,0 +1,102 @@
+/**
+ * host.c - DesignWare USB3 DRD Controller Host Glue
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Authors: Felipe Balbi <balbi@ti.com>,
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The names of the above-listed copyright holders may not be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2, as published by the Free
+ * Software Foundation.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+ * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <linux/platform_device.h>
+
+#include "core.h"
+
+static struct resource generic_resources[] = {
+	{
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+int dwc3_host_init(struct dwc3 *dwc)
+{
+	struct platform_device	*xhci;
+	int			ret;
+
+	xhci = platform_device_alloc("xhci", -1);
+	if (!xhci) {
+		dev_err(dwc->dev, "couldn't allocate xHCI device\n");
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	dma_set_coherent_mask(&xhci->dev, dwc->dev->coherent_dma_mask);
+
+	xhci->dev.parent	= dwc->dev;
+	xhci->dev.dma_mask	= dwc->dev->dma_mask;
+	xhci->dev.dma_parms	= dwc->dev->dma_parms;
+
+	dwc->xhci = xhci;
+
+	/* setup resources */
+	generic_resources[0].start = dwc->irq;
+
+	generic_resources[1].start = dwc->res->start;
+	generic_resources[1].end = dwc->res->start + 0x7fff;
+
+	ret = platform_device_add_resources(xhci, generic_resources,
+			ARRAY_SIZE(generic_resources));
+	if (ret) {
+		dev_err(dwc->dev, "couldn't add resources to xHCI device\n");
+		goto err1;
+	}
+
+	ret = platform_device_add(xhci);
+	if (ret) {
+		dev_err(dwc->dev, "failed to register xHCI device\n");
+		goto err1;
+	}
+
+	return 0;
+
+err1:
+	platform_device_put(xhci);
+
+err0:
+	return ret;
+}
+
+void dwc3_host_exit(struct dwc3 *dwc)
+{
+	platform_device_unregister(dwc->xhci);
+}
diff --git a/drivers/usb/dwc3/io.h b/drivers/usb/dwc3/io.h
index bc957db1ea4b..071d561f3e68 100644
--- a/drivers/usb/dwc3/io.h
+++ b/drivers/usb/dwc3/io.h
@@ -39,7 +39,7 @@
 #ifndef __DRIVERS_USB_DWC3_IO_H
 #define __DRIVERS_USB_DWC3_IO_H
 
-#include <asm/io.h>
+#include <linux/io.h>
 
 static inline u32 dwc3_readl(void __iomem *base, u32 offset)
 {
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 12a401a144b7..7ecb68a67411 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -15,6 +15,7 @@
 
 menuconfig USB_GADGET
 	tristate "USB Gadget Support"
+	select NLS
 	help
 	   USB is a master/slave protocol, organized with one master
 	   host (such as a PC) controlling up to 127 peripheral devices.
@@ -124,7 +125,6 @@ config USB_GADGET_STORAGE_NUM_BUFFERS
 #
 choice
 	prompt "USB Peripheral Controller"
-	depends on USB_GADGET
 	help
 	   A USB device uses a controller to talk to its host.
 	   Systems should have only one such upstream link.
@@ -234,7 +234,6 @@ config USB_R8A66597
 
 config USB_RENESAS_USBHS_UDC
 	tristate 'Renesas USBHS controller'
-	depends on SUPERH || ARCH_SHMOBILE
 	depends on USB_RENESAS_USBHS
 	select USB_GADGET_DUALSPEED
 	help
@@ -309,25 +308,13 @@ config USB_S3C_HSUDC
 
 	  This driver has been tested on S3C2416 and S3C2450 processors.
 
-config USB_PXA_U2O
-	tristate "PXA9xx Processor USB2.0 controller"
-	depends on ARCH_MMP
+config USB_MV_UDC
+	tristate "Marvell USB2.0 Device Controller"
 	select USB_GADGET_DUALSPEED
 	help
-	  PXA9xx Processor series include a high speed USB2.0 device
-	  controller, which support high speed and full speed USB peripheral.
-
-config USB_GADGET_DWC3
-	tristate "DesignWare USB3.0 (DRD) Controller"
-	depends on USB_DWC3
-	select USB_GADGET_DUALSPEED
-	select USB_GADGET_SUPERSPEED
-	help
-	  DesignWare USB3.0 controller is a SuperSpeed USB3.0 Controller
-	  which can be configured for peripheral-only, host-only, hub-only
-	  and Dual-Role operation. This Controller was first integrated into
-	  the OMAP5 series of processors. More information about the OMAP5
-	  version of this controller, refer to http://www.ti.com/omap5.
+	  Marvell Socs (including PXA and MMP series) include a high speed
+	  USB2.0 OTG controller, which can be configured as high speed or
+	  full speed USB peripheral.
 
 #
 # Controllers available in both integrated and discrete versions
@@ -543,12 +530,10 @@ endchoice
 # Selected by UDC drivers that support high-speed operation.
 config USB_GADGET_DUALSPEED
 	bool
-	depends on USB_GADGET
 
 # Selected by UDC drivers that support super-speed opperation
 config USB_GADGET_SUPERSPEED
 	bool
-	depends on USB_GADGET
 	depends on USB_GADGET_DUALSPEED
 
 #
@@ -556,7 +541,6 @@ config USB_GADGET_SUPERSPEED
 #
 choice
 	tristate "USB Gadget Drivers"
-	depends on USB_GADGET
 	default USB_ETH
 	help
 	  A Linux "Gadget Driver" talks to the USB Peripheral Controller
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index b54ac6190890..b7f6eefc3927 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -27,7 +27,7 @@ obj-$(CONFIG_USB_S3C_HSOTG)	+= s3c-hsotg.o
 obj-$(CONFIG_USB_S3C_HSUDC)	+= s3c-hsudc.o
 obj-$(CONFIG_USB_LANGWELL)	+= langwell_udc.o
 obj-$(CONFIG_USB_EG20T)		+= pch_udc.o
-obj-$(CONFIG_USB_PXA_U2O)	+= mv_udc.o
+obj-$(CONFIG_USB_MV_UDC)	+= mv_udc.o
 mv_udc-y			:= mv_udc_core.o
 obj-$(CONFIG_USB_CI13XXX_MSM)	+= ci13xxx_msm.o
 obj-$(CONFIG_USB_FUSB300)	+= fusb300_udc.o
diff --git a/drivers/usb/gadget/amd5536udc.c b/drivers/usb/gadget/amd5536udc.c
index 45f422ac103f..e9a2c5c44454 100644
--- a/drivers/usb/gadget/amd5536udc.c
+++ b/drivers/usb/gadget/amd5536udc.c
@@ -1959,7 +1959,7 @@ static int amd5536_start(struct usb_gadget_driver *driver,
 	u32 tmp;
 
 	if (!driver || !bind || !driver->setup
-			|| driver->speed < USB_SPEED_HIGH)
+			|| driver->max_speed < USB_SPEED_HIGH)
 		return -EINVAL;
 	if (!dev)
 		return -ENODEV;
@@ -3349,7 +3349,7 @@ static int udc_probe(struct udc *dev)
 	dev_set_name(&dev->gadget.dev, "gadget");
 	dev->gadget.dev.release = gadget_release;
 	dev->gadget.name = name;
-	dev->gadget.is_dualspeed = 1;
+	dev->gadget.max_speed = USB_SPEED_HIGH;
 
 	/* init registers, interrupts, ... */
 	startup_registers(dev);
diff --git a/drivers/usb/gadget/at91_udc.c b/drivers/usb/gadget/at91_udc.c
index 8efe0fa9228d..143a7256b598 100644
--- a/drivers/usb/gadget/at91_udc.c
+++ b/drivers/usb/gadget/at91_udc.c
@@ -1633,7 +1633,7 @@ static int at91_start(struct usb_gadget_driver *driver,
 	unsigned long	flags;
 
 	if (!driver
-			|| driver->speed < USB_SPEED_FULL
+			|| driver->max_speed < USB_SPEED_FULL
 			|| !bind
 			|| !driver->setup) {
 		DBG("bad parameter.\n");
@@ -1748,7 +1748,7 @@ static int __init at91udc_probe(struct platform_device *pdev)
 
 	/* rm9200 needs manual D+ pullup; off by default */
 	if (cpu_is_at91rm9200()) {
-		if (udc->board.pullup_pin <= 0) {
+		if (gpio_is_valid(udc->board.pullup_pin)) {
 			DBG("no D+ pullup?\n");
 			retval = -ENODEV;
 			goto fail0;
@@ -1815,7 +1815,7 @@ static int __init at91udc_probe(struct platform_device *pdev)
 		DBG("request irq %d failed\n", udc->udp_irq);
 		goto fail1;
 	}
-	if (udc->board.vbus_pin > 0) {
+	if (gpio_is_valid(udc->board.vbus_pin)) {
 		retval = gpio_request(udc->board.vbus_pin, "udc_vbus");
 		if (retval < 0) {
 			DBG("request vbus pin failed\n");
@@ -1859,10 +1859,10 @@ static int __init at91udc_probe(struct platform_device *pdev)
 	INFO("%s version %s\n", driver_name, DRIVER_VERSION);
 	return 0;
 fail4:
-	if (udc->board.vbus_pin > 0 && !udc->board.vbus_polled)
+	if (gpio_is_valid(udc->board.vbus_pin) && !udc->board.vbus_polled)
 		free_irq(udc->board.vbus_pin, udc);
 fail3:
-	if (udc->board.vbus_pin > 0)
+	if (gpio_is_valid(udc->board.vbus_pin))
 		gpio_free(udc->board.vbus_pin);
 fail2:
 	free_irq(udc->udp_irq, udc);
@@ -1897,7 +1897,7 @@ static int __exit at91udc_remove(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 0);
 	remove_debug_file(udc);
-	if (udc->board.vbus_pin > 0) {
+	if (gpio_is_valid(udc->board.vbus_pin)) {
 		free_irq(udc->board.vbus_pin, udc);
 		gpio_free(udc->board.vbus_pin);
 	}
@@ -1941,7 +1941,7 @@ static int at91udc_suspend(struct platform_device *pdev, pm_message_t mesg)
 		enable_irq_wake(udc->udp_irq);
 
 	udc->active_suspend = wake;
-	if (udc->board.vbus_pin > 0 && !udc->board.vbus_polled && wake)
+	if (gpio_is_valid(udc->board.vbus_pin) && !udc->board.vbus_polled && wake)
 		enable_irq_wake(udc->board.vbus_pin);
 	return 0;
 }
@@ -1951,7 +1951,7 @@ static int at91udc_resume(struct platform_device *pdev)
 	struct at91_udc *udc = platform_get_drvdata(pdev);
 	unsigned long	flags;
 
-	if (udc->board.vbus_pin > 0 && !udc->board.vbus_polled &&
+	if (gpio_is_valid(udc->board.vbus_pin) && !udc->board.vbus_polled &&
 	    udc->active_suspend)
 		disable_irq_wake(udc->board.vbus_pin);
 
diff --git a/drivers/usb/gadget/atmel_usba_udc.c b/drivers/usb/gadget/atmel_usba_udc.c
index 271a9d873608..e2fb6d583bd9 100644
--- a/drivers/usb/gadget/atmel_usba_udc.c
+++ b/drivers/usb/gadget/atmel_usba_udc.c
@@ -1038,7 +1038,7 @@ static struct usba_udc the_udc = {
 	.gadget	= {
 		.ops		= &usba_udc_ops,
 		.ep_list	= LIST_HEAD_INIT(the_udc.gadget.ep_list),
-		.is_dualspeed	= 1,
+		.max_speed	= USB_SPEED_HIGH,
 		.name		= "atmel_usba_udc",
 		.dev	= {
 			.init_name	= "gadget",
diff --git a/drivers/usb/gadget/ci13xxx_udc.c b/drivers/usb/gadget/ci13xxx_udc.c
index 9a0c3979ff43..27e313718422 100644
--- a/drivers/usb/gadget/ci13xxx_udc.c
+++ b/drivers/usb/gadget/ci13xxx_udc.c
@@ -182,6 +182,16 @@ static inline int hw_ep_bit(int num, int dir)
 	return num + (dir ? 16 : 0);
 }
 
+static int ep_to_bit(int n)
+{
+	int fill = 16 - hw_ep_max / 2;
+
+	if (n >= hw_ep_max / 2)
+		n += fill;
+
+	return n;
+}
+
 /**
  * hw_aread: reads from register bitfield
  * @addr: address relative to bus map
@@ -440,12 +450,13 @@ static int hw_ep_get_halt(int num, int dir)
 /**
  * hw_test_and_clear_setup_status: test & clear setup status (execute without
  *                                 interruption)
- * @n: bit number (endpoint)
+ * @n: endpoint number
  *
  * This function returns setup status
  */
 static int hw_test_and_clear_setup_status(int n)
 {
+	n = ep_to_bit(n);
 	return hw_ctest_and_clear(CAP_ENDPTSETUPSTAT, BIT(n));
 }
 
@@ -641,12 +652,13 @@ static int hw_register_write(u16 addr, u32 data)
 /**
  * hw_test_and_clear_complete: test & clear complete status (execute without
  *                             interruption)
- * @n: bit number (endpoint)
+ * @n: endpoint number
  *
  * This function returns complete status
  */
 static int hw_test_and_clear_complete(int n)
 {
+	n = ep_to_bit(n);
 	return hw_ctest_and_clear(CAP_ENDPTCOMPLETE, BIT(n));
 }
 
@@ -754,8 +766,11 @@ static ssize_t show_device(struct device *dev, struct device_attribute *attr,
 
 	n += scnprintf(buf + n, PAGE_SIZE - n, "speed             = %d\n",
 		       gadget->speed);
+	n += scnprintf(buf + n, PAGE_SIZE - n, "max_speed         = %d\n",
+		       gadget->max_speed);
+	/* TODO: Scheduled for removal in 3.8. */
 	n += scnprintf(buf + n, PAGE_SIZE - n, "is_dualspeed      = %d\n",
-		       gadget->is_dualspeed);
+		       gadget_is_dualspeed(gadget));
 	n += scnprintf(buf + n, PAGE_SIZE - n, "is_otg            = %d\n",
 		       gadget->is_otg);
 	n += scnprintf(buf + n, PAGE_SIZE - n, "is_a_peripheral   = %d\n",
@@ -798,7 +813,7 @@ static ssize_t show_driver(struct device *dev, struct device_attribute *attr,
 	n += scnprintf(buf + n, PAGE_SIZE - n, "function  = %s\n",
 		       (driver->function ? driver->function : ""));
 	n += scnprintf(buf + n, PAGE_SIZE - n, "max speed = %d\n",
-		       driver->speed);
+		       driver->max_speed);
 
 	return n;
 }
@@ -2563,9 +2578,7 @@ static int ci13xxx_start(struct usb_gadget_driver *driver,
 	if (driver             == NULL ||
 	    bind               == NULL ||
 	    driver->setup      == NULL ||
-	    driver->disconnect == NULL ||
-	    driver->suspend    == NULL ||
-	    driver->resume     == NULL)
+	    driver->disconnect == NULL)
 		return -EINVAL;
 	else if (udc         == NULL)
 		return -ENODEV;
@@ -2693,8 +2706,6 @@ static int ci13xxx_stop(struct usb_gadget_driver *driver)
 	    driver->unbind     == NULL ||
 	    driver->setup      == NULL ||
 	    driver->disconnect == NULL ||
-	    driver->suspend    == NULL ||
-	    driver->resume     == NULL ||
 	    driver             != udc->driver)
 		return -EINVAL;
 
@@ -2793,7 +2804,7 @@ static irqreturn_t udc_irq(void)
 			isr_statistics.pci++;
 			udc->gadget.speed = hw_port_is_high_speed() ?
 				USB_SPEED_HIGH : USB_SPEED_FULL;
-			if (udc->suspended) {
+			if (udc->suspended && udc->driver->resume) {
 				spin_unlock(udc->lock);
 				udc->driver->resume(&udc->gadget);
 				spin_lock(udc->lock);
@@ -2807,7 +2818,8 @@ static irqreturn_t udc_irq(void)
 			isr_tr_complete_handler(udc);
 		}
 		if (USBi_SLI & intr) {
-			if (udc->gadget.speed != USB_SPEED_UNKNOWN) {
+			if (udc->gadget.speed != USB_SPEED_UNKNOWN &&
+			    udc->driver->suspend) {
 				udc->suspended = 1;
 				spin_unlock(udc->lock);
 				udc->driver->suspend(&udc->gadget);
@@ -2871,7 +2883,7 @@ static int udc_probe(struct ci13xxx_udc_driver *driver, struct device *dev,
 
 	udc->gadget.ops          = &usb_gadget_ops;
 	udc->gadget.speed        = USB_SPEED_UNKNOWN;
-	udc->gadget.is_dualspeed = 1;
+	udc->gadget.max_speed    = USB_SPEED_HIGH;
 	udc->gadget.is_otg       = 0;
 	udc->gadget.name         = driver->name;
 
diff --git a/drivers/usb/gadget/ci13xxx_udc.h b/drivers/usb/gadget/ci13xxx_udc.h
index 23707775cb43..f4871e1fac59 100644
--- a/drivers/usb/gadget/ci13xxx_udc.h
+++ b/drivers/usb/gadget/ci13xxx_udc.h
@@ -127,7 +127,7 @@ struct ci13xxx {
 	struct ci13xxx_ep          ci13xxx_ep[ENDPT_MAX]; /* extended endpts */
 	u32                        ep0_dir;    /* ep0 direction */
 #define ep0out ci13xxx_ep[0]
-#define ep0in  ci13xxx_ep[16]
+#define ep0in  ci13xxx_ep[hw_ep_max / 2]
 	u8                         remote_wakeup; /* Is remote wakeup feature
 							enabled by the host? */
 	u8                         suspended;  /* suspended by the host */
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index f71b0787983f..a95de6a4a134 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -1535,9 +1535,9 @@ composite_resume(struct usb_gadget *gadget)
 
 static struct usb_gadget_driver composite_driver = {
 #ifdef CONFIG_USB_GADGET_SUPERSPEED
-	.speed		= USB_SPEED_SUPER,
+	.max_speed	= USB_SPEED_SUPER,
 #else
-	.speed		= USB_SPEED_HIGH,
+	.max_speed	= USB_SPEED_HIGH,
 #endif
 
 	.unbind		= composite_unbind,
@@ -1584,8 +1584,8 @@ int usb_composite_probe(struct usb_composite_driver *driver,
 		driver->iProduct = driver->name;
 	composite_driver.function =  (char *) driver->name;
 	composite_driver.driver.name = driver->name;
-	composite_driver.speed = min((u8)composite_driver.speed,
-				     (u8)driver->max_speed);
+	composite_driver.max_speed =
+		min_t(u8, composite_driver.max_speed, driver->max_speed);
 	composite = driver;
 	composite_gadget_bind = bind;
 
diff --git a/drivers/usb/gadget/dbgp.c b/drivers/usb/gadget/dbgp.c
index 6256420089f3..19d7bb0df75a 100644
--- a/drivers/usb/gadget/dbgp.c
+++ b/drivers/usb/gadget/dbgp.c
@@ -404,7 +404,7 @@ fail:
 
 static struct usb_gadget_driver dbgp_driver = {
 	.function = "dbgp",
-	.speed = USB_SPEED_HIGH,
+	.max_speed = USB_SPEED_HIGH,
 	.unbind = dbgp_unbind,
 	.setup = dbgp_setup,
 	.disconnect = dbgp_disconnect,
diff --git a/drivers/usb/gadget/dummy_hcd.c b/drivers/usb/gadget/dummy_hcd.c
index ab8f1b488d54..db815c2da7ed 100644
--- a/drivers/usb/gadget/dummy_hcd.c
+++ b/drivers/usb/gadget/dummy_hcd.c
@@ -823,19 +823,18 @@ static int dummy_pullup (struct usb_gadget *_gadget, int value)
 
 	if (value && dum->driver) {
 		if (mod_data.is_super_speed)
-			dum->gadget.speed = dum->driver->speed;
+			dum->gadget.speed = dum->driver->max_speed;
 		else if (mod_data.is_high_speed)
 			dum->gadget.speed = min_t(u8, USB_SPEED_HIGH,
-					dum->driver->speed);
+					dum->driver->max_speed);
 		else
 			dum->gadget.speed = USB_SPEED_FULL;
 		dummy_udc_udpate_ep0(dum);
 
-		if (dum->gadget.speed < dum->driver->speed)
+		if (dum->gadget.speed < dum->driver->max_speed)
 			dev_dbg(udc_dev(dum), "This device can perform faster"
-					" if you connect it to a %s port...\n",
-					(dum->driver->speed == USB_SPEED_SUPER ?
-					 "SuperSpeed" : "HighSpeed"));
+				" if you connect it to a %s port...\n",
+				usb_speed_string(dum->driver->max_speed));
 	}
 	dum_hcd = gadget_to_dummy_hcd(_gadget);
 
@@ -898,7 +897,7 @@ static int dummy_udc_start(struct usb_gadget *g,
 	struct dummy_hcd	*dum_hcd = gadget_to_dummy_hcd(g);
 	struct dummy		*dum = dum_hcd->dum;
 
-	if (driver->speed == USB_SPEED_UNKNOWN)
+	if (driver->max_speed == USB_SPEED_UNKNOWN)
 		return -EINVAL;
 
 	/*
@@ -977,7 +976,7 @@ static int dummy_udc_probe (struct platform_device *pdev)
 
 	dum->gadget.name = gadget_name;
 	dum->gadget.ops = &dummy_ops;
-	dum->gadget.is_dualspeed = 1;
+	dum->gadget.max_speed = USB_SPEED_SUPER;
 
 	dev_set_name(&dum->gadget.dev, "gadget");
 	dum->gadget.dev.parent = &pdev->dev;
diff --git a/drivers/usb/gadget/epautoconf.c b/drivers/usb/gadget/epautoconf.c
index 4dff83d2f265..753aa0683ac1 100644
--- a/drivers/usb/gadget/epautoconf.c
+++ b/drivers/usb/gadget/epautoconf.c
@@ -149,7 +149,7 @@ ep_matches (
 	switch (type) {
 	case USB_ENDPOINT_XFER_INT:
 		/* INT:  limit 64 bytes full speed, 1024 high/super speed */
-		if (!gadget->is_dualspeed && max > 64)
+		if (!gadget_is_dualspeed(gadget) && max > 64)
 			return 0;
 		/* FALLTHROUGH */
 
@@ -157,12 +157,12 @@ ep_matches (
 		/* ISO:  limit 1023 bytes full speed, 1024 high/super speed */
 		if (ep->maxpacket < max)
 			return 0;
-		if (!gadget->is_dualspeed && max > 1023)
+		if (!gadget_is_dualspeed(gadget) && max > 1023)
 			return 0;
 
 		/* BOTH:  "high bandwidth" works only at high speed */
 		if ((desc->wMaxPacketSize & cpu_to_le16(3<<11))) {
-			if (!gadget->is_dualspeed)
+			if (!gadget_is_dualspeed(gadget))
 				return 0;
 			/* configure your hardware with enough buffering!! */
 		}
diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index acb38004eec0..f63dc6c150d2 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -1037,7 +1037,6 @@ static int ffs_sb_fill(struct super_block *sb, void *_data, int silent)
 {
 	struct ffs_sb_fill_data *data = _data;
 	struct inode	*inode;
-	struct dentry	*d;
 	struct ffs_data	*ffs;
 
 	ENTER();
@@ -1045,7 +1044,7 @@ static int ffs_sb_fill(struct super_block *sb, void *_data, int silent)
 	/* Initialise data */
 	ffs = ffs_data_new();
 	if (unlikely(!ffs))
-		goto enomem0;
+		goto Enomem;
 
 	ffs->sb              = sb;
 	ffs->dev_name        = data->dev_name;
@@ -1065,26 +1064,21 @@ static int ffs_sb_fill(struct super_block *sb, void *_data, int silent)
 				  &simple_dir_inode_operations,
 				  &data->perms);
 	if (unlikely(!inode))
-		goto enomem1;
-	d = d_alloc_root(inode);
-	if (unlikely(!d))
-		goto enomem2;
-	sb->s_root = d;
+		goto Enomem;
+	sb->s_root = d_alloc_root(inode);
+	if (unlikely(!sb->s_root)) {
+		iput(inode);
+		goto Enomem;
+	}
 
 	/* EP0 file */
 	if (unlikely(!ffs_sb_create_file(sb, "ep0", ffs,
 					 &ffs_ep0_operations, NULL)))
-		goto enomem3;
+		goto Enomem;
 
 	return 0;
 
-enomem3:
-	dput(d);
-enomem2:
-	iput(inode);
-enomem1:
-	ffs_data_put(ffs);
-enomem0:
+Enomem:
 	return -ENOMEM;
 }
 
@@ -1196,14 +1190,11 @@ ffs_fs_mount(struct file_system_type *t, int flags,
 static void
 ffs_fs_kill_sb(struct super_block *sb)
 {
-	void *ptr;
-
 	ENTER();
 
 	kill_litter_super(sb);
-	ptr = xchg(&sb->s_fs_info, NULL);
-	if (ptr)
-		ffs_data_put(ptr);
+	if (sb->s_fs_info)
+		ffs_data_put(sb->s_fs_info);
 }
 
 static struct file_system_type ffs_fs_type = {
@@ -1408,7 +1399,7 @@ static int ffs_epfiles_create(struct ffs_data *ffs)
 	ENTER();
 
 	count = ffs->eps_count;
-	epfiles = kzalloc(count * sizeof *epfiles, GFP_KERNEL);
+	epfiles = kcalloc(count, sizeof(*epfiles), GFP_KERNEL);
 	if (!epfiles)
 		return -ENOMEM;
 
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index 1a6f415c0d02..6353eca1e852 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -1873,17 +1873,14 @@ static int check_command(struct fsg_common *common, int cmnd_size,
 		    common->lun, lun);
 
 	/* Check the LUN */
-	if (common->lun < common->nluns) {
-		curlun = &common->luns[common->lun];
-		common->curlun = curlun;
+	curlun = common->curlun;
+	if (curlun) {
 		if (common->cmnd[0] != REQUEST_SENSE) {
 			curlun->sense_data = SS_NO_SENSE;
 			curlun->sense_data_info = 0;
 			curlun->info_valid = 0;
 		}
 	} else {
-		common->curlun = NULL;
-		curlun = NULL;
 		common->bad_lun_okay = 0;
 
 		/*
@@ -1929,6 +1926,17 @@ static int check_command(struct fsg_common *common, int cmnd_size,
 	return 0;
 }
 
+/* wrapper of check_command for data size in blocks handling */
+static int check_command_size_in_blocks(struct fsg_common *common,
+		int cmnd_size, enum data_direction data_dir,
+		unsigned int mask, int needs_medium, const char *name)
+{
+	if (common->curlun)
+		common->data_size_from_cmnd <<= common->curlun->blkbits;
+	return check_command(common, cmnd_size, data_dir,
+			mask, needs_medium, name);
+}
+
 static int do_scsi_command(struct fsg_common *common)
 {
 	struct fsg_buffhd	*bh;
@@ -2011,9 +2019,9 @@ static int do_scsi_command(struct fsg_common *common)
 
 	case READ_6:
 		i = common->cmnd[4];
-		common->data_size_from_cmnd = (i == 0 ? 256 : i) <<
-				common->curlun->blkbits;
-		reply = check_command(common, 6, DATA_DIR_TO_HOST,
+		common->data_size_from_cmnd = (i == 0) ? 256 : i;
+		reply = check_command_size_in_blocks(common, 6,
+				      DATA_DIR_TO_HOST,
 				      (7<<1) | (1<<4), 1,
 				      "READ(6)");
 		if (reply == 0)
@@ -2022,9 +2030,9 @@ static int do_scsi_command(struct fsg_common *common)
 
 	case READ_10:
 		common->data_size_from_cmnd =
-				get_unaligned_be16(&common->cmnd[7]) <<
-						common->curlun->blkbits;
-		reply = check_command(common, 10, DATA_DIR_TO_HOST,
+				get_unaligned_be16(&common->cmnd[7]);
+		reply = check_command_size_in_blocks(common, 10,
+				      DATA_DIR_TO_HOST,
 				      (1<<1) | (0xf<<2) | (3<<7), 1,
 				      "READ(10)");
 		if (reply == 0)
@@ -2033,9 +2041,9 @@ static int do_scsi_command(struct fsg_common *common)
 
 	case READ_12:
 		common->data_size_from_cmnd =
-				get_unaligned_be32(&common->cmnd[6]) <<
-						common->curlun->blkbits;
-		reply = check_command(common, 12, DATA_DIR_TO_HOST,
+				get_unaligned_be32(&common->cmnd[6]);
+		reply = check_command_size_in_blocks(common, 12,
+				      DATA_DIR_TO_HOST,
 				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
 				      "READ(12)");
 		if (reply == 0)
@@ -2134,9 +2142,9 @@ static int do_scsi_command(struct fsg_common *common)
 
 	case WRITE_6:
 		i = common->cmnd[4];
-		common->data_size_from_cmnd = (i == 0 ? 256 : i) <<
-					common->curlun->blkbits;
-		reply = check_command(common, 6, DATA_DIR_FROM_HOST,
+		common->data_size_from_cmnd = (i == 0) ? 256 : i;
+		reply = check_command_size_in_blocks(common, 6,
+				      DATA_DIR_FROM_HOST,
 				      (7<<1) | (1<<4), 1,
 				      "WRITE(6)");
 		if (reply == 0)
@@ -2145,9 +2153,9 @@ static int do_scsi_command(struct fsg_common *common)
 
 	case WRITE_10:
 		common->data_size_from_cmnd =
-				get_unaligned_be16(&common->cmnd[7]) <<
-						common->curlun->blkbits;
-		reply = check_command(common, 10, DATA_DIR_FROM_HOST,
+				get_unaligned_be16(&common->cmnd[7]);
+		reply = check_command_size_in_blocks(common, 10,
+				      DATA_DIR_FROM_HOST,
 				      (1<<1) | (0xf<<2) | (3<<7), 1,
 				      "WRITE(10)");
 		if (reply == 0)
@@ -2156,9 +2164,9 @@ static int do_scsi_command(struct fsg_common *common)
 
 	case WRITE_12:
 		common->data_size_from_cmnd =
-				get_unaligned_be32(&common->cmnd[6]) <<
-						common->curlun->blkbits;
-		reply = check_command(common, 12, DATA_DIR_FROM_HOST,
+				get_unaligned_be32(&common->cmnd[6]);
+		reply = check_command_size_in_blocks(common, 12,
+				      DATA_DIR_FROM_HOST,
 				      (1<<1) | (0xf<<2) | (0xf<<6), 1,
 				      "WRITE(12)");
 		if (reply == 0)
@@ -2273,6 +2281,10 @@ static int received_cbw(struct fsg_dev *fsg, struct fsg_buffhd *bh)
 	if (common->data_size == 0)
 		common->data_dir = DATA_DIR_NONE;
 	common->lun = cbw->Lun;
+	if (common->lun >= 0 && common->lun < common->nluns)
+		common->curlun = &common->luns[common->lun];
+	else
+		common->curlun = NULL;
 	common->tag = cbw->Tag;
 	return 0;
 }
@@ -2763,7 +2775,7 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
 	 * Create the LUNs, open their backing files, and register the
 	 * LUN devices in sysfs.
 	 */
-	curlun = kzalloc(nluns * sizeof *curlun, GFP_KERNEL);
+	curlun = kcalloc(nluns, sizeof(*curlun), GFP_KERNEL);
 	if (unlikely(!curlun)) {
 		rc = -ENOMEM;
 		goto error_release;
diff --git a/drivers/usb/gadget/file_storage.c b/drivers/usb/gadget/file_storage.c
index 11b5196284ae..e0f30fc70e45 100644
--- a/drivers/usb/gadget/file_storage.c
+++ b/drivers/usb/gadget/file_storage.c
@@ -2297,19 +2297,17 @@ static int check_command(struct fsg_dev *fsg, int cmnd_size,
 			DBG(fsg, "using LUN %d from CBW, "
 					"not LUN %d from CDB\n",
 					fsg->lun, lun);
-	} else
-		fsg->lun = lun;		// Use LUN from the command
+	}
 
 	/* Check the LUN */
-	if (fsg->lun < fsg->nluns) {
-		fsg->curlun = curlun = &fsg->luns[fsg->lun];
+	curlun = fsg->curlun;
+	if (curlun) {
 		if (fsg->cmnd[0] != REQUEST_SENSE) {
 			curlun->sense_data = SS_NO_SENSE;
 			curlun->sense_data_info = 0;
 			curlun->info_valid = 0;
 		}
 	} else {
-		fsg->curlun = curlun = NULL;
 		fsg->bad_lun_okay = 0;
 
 		/* INQUIRY and REQUEST SENSE commands are explicitly allowed
@@ -2351,6 +2349,16 @@ static int check_command(struct fsg_dev *fsg, int cmnd_size,
 	return 0;
 }
 
+/* wrapper of check_command for data size in blocks handling */
+static int check_command_size_in_blocks(struct fsg_dev *fsg, int cmnd_size,
+		enum data_direction data_dir, unsigned int mask,
+		int needs_medium, const char *name)
+{
+	if (fsg->curlun)
+		fsg->data_size_from_cmnd <<= fsg->curlun->blkbits;
+	return check_command(fsg, cmnd_size, data_dir,
+			mask, needs_medium, name);
+}
 
 static int do_scsi_command(struct fsg_dev *fsg)
 {
@@ -2425,26 +2433,27 @@ static int do_scsi_command(struct fsg_dev *fsg)
 
 	case READ_6:
 		i = fsg->cmnd[4];
-		fsg->data_size_from_cmnd = (i == 0 ? 256 : i) << fsg->curlun->blkbits;
-		if ((reply = check_command(fsg, 6, DATA_DIR_TO_HOST,
+		fsg->data_size_from_cmnd = (i == 0) ? 256 : i;
+		if ((reply = check_command_size_in_blocks(fsg, 6,
+				DATA_DIR_TO_HOST,
 				(7<<1) | (1<<4), 1,
 				"READ(6)")) == 0)
 			reply = do_read(fsg);
 		break;
 
 	case READ_10:
-		fsg->data_size_from_cmnd =
-				get_unaligned_be16(&fsg->cmnd[7]) << fsg->curlun->blkbits;
-		if ((reply = check_command(fsg, 10, DATA_DIR_TO_HOST,
+		fsg->data_size_from_cmnd = get_unaligned_be16(&fsg->cmnd[7]);
+		if ((reply = check_command_size_in_blocks(fsg, 10,
+				DATA_DIR_TO_HOST,
 				(1<<1) | (0xf<<2) | (3<<7), 1,
 				"READ(10)")) == 0)
 			reply = do_read(fsg);
 		break;
 
 	case READ_12:
-		fsg->data_size_from_cmnd =
-				get_unaligned_be32(&fsg->cmnd[6]) << fsg->curlun->blkbits;
-		if ((reply = check_command(fsg, 12, DATA_DIR_TO_HOST,
+		fsg->data_size_from_cmnd = get_unaligned_be32(&fsg->cmnd[6]);
+		if ((reply = check_command_size_in_blocks(fsg, 12,
+				DATA_DIR_TO_HOST,
 				(1<<1) | (0xf<<2) | (0xf<<6), 1,
 				"READ(12)")) == 0)
 			reply = do_read(fsg);
@@ -2529,26 +2538,27 @@ static int do_scsi_command(struct fsg_dev *fsg)
 
 	case WRITE_6:
 		i = fsg->cmnd[4];
-		fsg->data_size_from_cmnd = (i == 0 ? 256 : i) << fsg->curlun->blkbits;
-		if ((reply = check_command(fsg, 6, DATA_DIR_FROM_HOST,
+		fsg->data_size_from_cmnd = (i == 0) ? 256 : i;
+		if ((reply = check_command_size_in_blocks(fsg, 6,
+				DATA_DIR_FROM_HOST,
 				(7<<1) | (1<<4), 1,
 				"WRITE(6)")) == 0)
 			reply = do_write(fsg);
 		break;
 
 	case WRITE_10:
-		fsg->data_size_from_cmnd =
-				get_unaligned_be16(&fsg->cmnd[7]) << fsg->curlun->blkbits;
-		if ((reply = check_command(fsg, 10, DATA_DIR_FROM_HOST,
+		fsg->data_size_from_cmnd = get_unaligned_be16(&fsg->cmnd[7]);
+		if ((reply = check_command_size_in_blocks(fsg, 10,
+				DATA_DIR_FROM_HOST,
 				(1<<1) | (0xf<<2) | (3<<7), 1,
 				"WRITE(10)")) == 0)
 			reply = do_write(fsg);
 		break;
 
 	case WRITE_12:
-		fsg->data_size_from_cmnd =
-				get_unaligned_be32(&fsg->cmnd[6]) << fsg->curlun->blkbits;
-		if ((reply = check_command(fsg, 12, DATA_DIR_FROM_HOST,
+		fsg->data_size_from_cmnd = get_unaligned_be32(&fsg->cmnd[6]);
+		if ((reply = check_command_size_in_blocks(fsg, 12,
+				DATA_DIR_FROM_HOST,
 				(1<<1) | (0xf<<2) | (0xf<<6), 1,
 				"WRITE(12)")) == 0)
 			reply = do_write(fsg);
@@ -2715,7 +2725,17 @@ static int get_next_command(struct fsg_dev *fsg)
 		memcpy(fsg->cmnd, fsg->cbbuf_cmnd, fsg->cmnd_size);
 		fsg->cbbuf_cmnd_size = 0;
 		spin_unlock_irq(&fsg->lock);
+
+		/* Use LUN from the command */
+		fsg->lun = fsg->cmnd[1] >> 5;
 	}
+
+	/* Update current lun */
+	if (fsg->lun >= 0 && fsg->lun < fsg->nluns)
+		fsg->curlun = &fsg->luns[fsg->lun];
+	else
+		fsg->curlun = NULL;
+
 	return rc;
 }
 
@@ -3584,7 +3604,7 @@ static void fsg_resume(struct usb_gadget *gadget)
 /*-------------------------------------------------------------------------*/
 
 static struct usb_gadget_driver		fsg_driver = {
-	.speed		= USB_SPEED_SUPER,
+	.max_speed	= USB_SPEED_SUPER,
 	.function	= (char *) fsg_string_product,
 	.unbind		= fsg_unbind,
 	.disconnect	= fsg_disconnect,
diff --git a/drivers/usb/gadget/fsl_qe_udc.c b/drivers/usb/gadget/fsl_qe_udc.c
index e00cf92409ce..b95697c03d07 100644
--- a/drivers/usb/gadget/fsl_qe_udc.c
+++ b/drivers/usb/gadget/fsl_qe_udc.c
@@ -2336,7 +2336,7 @@ static int fsl_qe_start(struct usb_gadget_driver *driver,
 	if (!udc_controller)
 		return -ENODEV;
 
-	if (!driver || driver->speed < USB_SPEED_FULL
+	if (!driver || driver->max_speed < USB_SPEED_FULL
 			|| !bind || !driver->disconnect || !driver->setup)
 		return -EINVAL;
 
@@ -2350,7 +2350,7 @@ static int fsl_qe_start(struct usb_gadget_driver *driver,
 	/* hook up the driver */
 	udc_controller->driver = driver;
 	udc_controller->gadget.dev.driver = &driver->driver;
-	udc_controller->gadget.speed = (enum usb_device_speed)(driver->speed);
+	udc_controller->gadget.speed = driver->max_speed;
 	spin_unlock_irqrestore(&udc_controller->lock, flags);
 
 	retval = bind(&udc_controller->gadget);
@@ -2814,20 +2814,7 @@ static struct platform_driver udc_driver = {
 #endif
 };
 
-static int __init qe_udc_init(void)
-{
-	printk(KERN_INFO "%s: %s, %s\n", driver_name, driver_desc,
-			DRIVER_VERSION);
-	return platform_driver_register(&udc_driver);
-}
-
-static void __exit qe_udc_exit(void)
-{
-	platform_driver_unregister(&udc_driver);
-}
-
-module_init(qe_udc_init);
-module_exit(qe_udc_exit);
+module_platform_driver(udc_driver);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/gadget/fsl_udc_core.c b/drivers/usb/gadget/fsl_udc_core.c
index dd28ef3def71..d7ea6c076ce9 100644
--- a/drivers/usb/gadget/fsl_udc_core.c
+++ b/drivers/usb/gadget/fsl_udc_core.c
@@ -1934,7 +1934,7 @@ static int fsl_start(struct usb_gadget_driver *driver,
 	if (!udc_controller)
 		return -ENODEV;
 
-	if (!driver || driver->speed < USB_SPEED_FULL
+	if (!driver || driver->max_speed < USB_SPEED_FULL
 			|| !bind || !driver->disconnect || !driver->setup)
 		return -EINVAL;
 
@@ -2525,7 +2525,7 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 
 	/* Setup gadget structure */
 	udc_controller->gadget.ops = &fsl_gadget_ops;
-	udc_controller->gadget.is_dualspeed = 1;
+	udc_controller->gadget.max_speed = USB_SPEED_HIGH;
 	udc_controller->gadget.ep0 = &udc_controller->eps[0].ep;
 	INIT_LIST_HEAD(&udc_controller->gadget.ep_list);
 	udc_controller->gadget.speed = USB_SPEED_UNKNOWN;
diff --git a/drivers/usb/gadget/fusb300_udc.c b/drivers/usb/gadget/fusb300_udc.c
index 74da206c8406..5831cb4a0b35 100644
--- a/drivers/usb/gadget/fusb300_udc.c
+++ b/drivers/usb/gadget/fusb300_udc.c
@@ -1317,7 +1317,7 @@ static int fusb300_udc_start(struct usb_gadget_driver *driver,
 	int retval;
 
 	if (!driver
-			|| driver->speed < USB_SPEED_FULL
+			|| driver->max_speed < USB_SPEED_FULL
 			|| !bind
 			|| !driver->setup)
 		return -EINVAL;
@@ -1463,7 +1463,7 @@ static int __init fusb300_probe(struct platform_device *pdev)
 
 	dev_set_name(&fusb300->gadget.dev, "gadget");
 
-	fusb300->gadget.is_dualspeed = 1;
+	fusb300->gadget.max_speed = USB_SPEED_HIGH;
 	fusb300->gadget.dev.parent = &pdev->dev;
 	fusb300->gadget.dev.dma_mask = pdev->dev.dma_mask;
 	fusb300->gadget.dev.release = pdev->dev.release;
diff --git a/drivers/usb/gadget/goku_udc.c b/drivers/usb/gadget/goku_udc.c
index 7f87805cddc4..5af70fcce139 100644
--- a/drivers/usb/gadget/goku_udc.c
+++ b/drivers/usb/gadget/goku_udc.c
@@ -1357,7 +1357,7 @@ static int goku_start(struct usb_gadget_driver *driver,
 	int			retval;
 
 	if (!driver
-			|| driver->speed < USB_SPEED_FULL
+			|| driver->max_speed < USB_SPEED_FULL
 			|| !bind
 			|| !driver->disconnect
 			|| !driver->setup)
@@ -1796,6 +1796,7 @@ static int goku_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	spin_lock_init(&dev->lock);
 	dev->pdev = pdev;
 	dev->gadget.ops = &goku_ops;
+	dev->gadget.max_speed = USB_SPEED_FULL;
 
 	/* the "gadget" abstracts/virtualizes the controller */
 	dev_set_name(&dev->gadget.dev, "gadget");
diff --git a/drivers/usb/gadget/imx_udc.c b/drivers/usb/gadget/imx_udc.c
index 2d978c0e7ced..8d1c75abd73d 100644
--- a/drivers/usb/gadget/imx_udc.c
+++ b/drivers/usb/gadget/imx_udc.c
@@ -1336,7 +1336,7 @@ static int imx_udc_start(struct usb_gadget_driver *driver,
 	int retval;
 
 	if (!driver
-		|| driver->speed < USB_SPEED_FULL
+		|| driver->max_speed < USB_SPEED_FULL
 		|| !bind
 		|| !driver->disconnect
 		|| !driver->setup)
diff --git a/drivers/usb/gadget/inode.c b/drivers/usb/gadget/inode.c
index 6ccae2707e59..ae04266dba1b 100644
--- a/drivers/usb/gadget/inode.c
+++ b/drivers/usb/gadget/inode.c
@@ -1766,9 +1766,9 @@ gadgetfs_suspend (struct usb_gadget *gadget)
 
 static struct usb_gadget_driver gadgetfs_driver = {
 #ifdef	CONFIG_USB_GADGET_DUALSPEED
-	.speed		= USB_SPEED_HIGH,
+	.max_speed	= USB_SPEED_HIGH,
 #else
-	.speed		= USB_SPEED_FULL,
+	.max_speed	= USB_SPEED_FULL,
 #endif
 	.function	= (char *) driver_desc,
 	.unbind		= gadgetfs_unbind,
@@ -1792,7 +1792,7 @@ static int gadgetfs_probe (struct usb_gadget *gadget)
 }
 
 static struct usb_gadget_driver probe_driver = {
-	.speed		= USB_SPEED_HIGH,
+	.max_speed	= USB_SPEED_HIGH,
 	.unbind		= gadgetfs_nop,
 	.setup		= (void *)gadgetfs_nop,
 	.disconnect	= gadgetfs_nop,
@@ -2035,7 +2035,6 @@ static int
 gadgetfs_fill_super (struct super_block *sb, void *opts, int silent)
 {
 	struct inode	*inode;
-	struct dentry	*d;
 	struct dev_data	*dev;
 
 	if (the_device)
@@ -2058,24 +2057,27 @@ gadgetfs_fill_super (struct super_block *sb, void *opts, int silent)
 			NULL, &simple_dir_operations,
 			S_IFDIR | S_IRUGO | S_IXUGO);
 	if (!inode)
-		goto enomem0;
+		goto Enomem;
 	inode->i_op = &simple_dir_inode_operations;
-	if (!(d = d_alloc_root (inode)))
-		goto enomem1;
-	sb->s_root = d;
+	if (!(sb->s_root = d_alloc_root (inode))) {
+		iput(inode);
+		goto Enomem;
+	}
 
 	/* the ep0 file is named after the controller we expect;
 	 * user mode code can use it for sanity checks, like we do.
 	 */
 	dev = dev_new ();
 	if (!dev)
-		goto enomem2;
+		goto Enomem;
 
 	dev->sb = sb;
 	if (!gadgetfs_create_file (sb, CHIP,
 				dev, &dev_init_operations,
-				&dev->dentry))
-		goto enomem3;
+				&dev->dentry)) {
+		put_dev(dev);
+		goto Enomem;
+	}
 
 	/* other endpoint files are available after hardware setup,
 	 * from binding to a controller.
@@ -2083,13 +2085,7 @@ gadgetfs_fill_super (struct super_block *sb, void *opts, int silent)
 	the_device = dev;
 	return 0;
 
-enomem3:
-	put_dev (dev);
-enomem2:
-	dput (d);
-enomem1:
-	iput (inode);
-enomem0:
+Enomem:
 	return -ENOMEM;
 }
 
diff --git a/drivers/usb/gadget/langwell_udc.c b/drivers/usb/gadget/langwell_udc.c
index c9fa3bf5b377..fa0fcc11263f 100644
--- a/drivers/usb/gadget/langwell_udc.c
+++ b/drivers/usb/gadget/langwell_udc.c
@@ -3267,7 +3267,7 @@ static int langwell_udc_probe(struct pci_dev *pdev,
 	dev->gadget.ep0 = &dev->ep[0].ep;	/* gadget ep0 */
 	INIT_LIST_HEAD(&dev->gadget.ep_list);	/* ep_list */
 	dev->gadget.speed = USB_SPEED_UNKNOWN;	/* speed */
-	dev->gadget.is_dualspeed = 1;		/* support dual speed */
+	dev->gadget.max_speed = USB_SPEED_HIGH;	/* support dual speed */
 #ifdef	OTG_TRANSCEIVER
 	dev->gadget.is_otg = 1;			/* support otg mode */
 #endif
diff --git a/drivers/usb/gadget/m66592-udc.c b/drivers/usb/gadget/m66592-udc.c
index 9aa1cbbee45b..3608b3bd5732 100644
--- a/drivers/usb/gadget/m66592-udc.c
+++ b/drivers/usb/gadget/m66592-udc.c
@@ -1472,7 +1472,7 @@ static int m66592_start(struct usb_gadget_driver *driver,
 	int retval;
 
 	if (!driver
-			|| driver->speed < USB_SPEED_HIGH
+			|| driver->max_speed < USB_SPEED_HIGH
 			|| !bind
 			|| !driver->setup)
 		return -EINVAL;
@@ -1653,7 +1653,7 @@ static int __init m66592_probe(struct platform_device *pdev)
 	m66592->gadget.ops = &m66592_gadget_ops;
 	device_initialize(&m66592->gadget.dev);
 	dev_set_name(&m66592->gadget.dev, "gadget");
-	m66592->gadget.is_dualspeed = 1;
+	m66592->gadget.max_speed = USB_SPEED_HIGH;
 	m66592->gadget.dev.parent = &pdev->dev;
 	m66592->gadget.dev.dma_mask = pdev->dev.dma_mask;
 	m66592->gadget.dev.release = pdev->dev.release;
diff --git a/drivers/usb/gadget/mv_udc.h b/drivers/usb/gadget/mv_udc.h
index daa75c12f336..34aadfae723d 100644
--- a/drivers/usb/gadget/mv_udc.h
+++ b/drivers/usb/gadget/mv_udc.h
@@ -180,7 +180,7 @@ struct mv_udc {
 
 	struct mv_cap_regs __iomem	*cap_regs;
 	struct mv_op_regs __iomem	*op_regs;
-	unsigned int			phy_regs;
+	void __iomem                    *phy_regs;
 	unsigned int			max_eps;
 	struct mv_dqh			*ep_dqh;
 	size_t				ep_dqh_size;
@@ -211,11 +211,14 @@ struct mv_udc {
 				softconnected:1,
 				force_fs:1,
 				clock_gating:1,
-				active:1;
+				active:1,
+				stopped:1;      /* stop bit is setted */
 
 	struct work_struct	vbus_work;
 	struct workqueue_struct *qwork;
 
+	struct otg_transceiver	*transceiver;
+
 	struct mv_usb_platform_data     *pdata;
 
 	/* some SOC has mutiple clock sources for USB*/
diff --git a/drivers/usb/gadget/mv_udc_core.c b/drivers/usb/gadget/mv_udc_core.c
index 892412103dd8..f97e737d26f7 100644
--- a/drivers/usb/gadget/mv_udc_core.c
+++ b/drivers/usb/gadget/mv_udc_core.c
@@ -276,11 +276,12 @@ static void done(struct mv_ep *ep, struct mv_req *req, int status)
 
 static int queue_dtd(struct mv_ep *ep, struct mv_req *req)
 {
-	u32 tmp, epstatus, bit_pos, direction;
 	struct mv_udc *udc;
 	struct mv_dqh *dqh;
+	u32 bit_pos, direction;
+	u32 usbcmd, epstatus;
 	unsigned int loops;
-	int readsafe, retval = 0;
+	int retval = 0;
 
 	udc = ep->udc;
 	direction = ep_dir(ep);
@@ -293,30 +294,18 @@ static int queue_dtd(struct mv_ep *ep, struct mv_req *req)
 		lastreq = list_entry(ep->queue.prev, struct mv_req, queue);
 		lastreq->tail->dtd_next =
 			req->head->td_dma & EP_QUEUE_HEAD_NEXT_POINTER_MASK;
-		if (readl(&udc->op_regs->epprime) & bit_pos) {
-			loops = LOOPS(PRIME_TIMEOUT);
-			while (readl(&udc->op_regs->epprime) & bit_pos) {
-				if (loops == 0) {
-					retval = -ETIME;
-					goto done;
-				}
-				udelay(LOOPS_USEC);
-				loops--;
-			}
-			if (readl(&udc->op_regs->epstatus) & bit_pos)
-				goto done;
-		}
-		readsafe = 0;
+
+		wmb();
+
+		if (readl(&udc->op_regs->epprime) & bit_pos)
+			goto done;
+
 		loops = LOOPS(READSAFE_TIMEOUT);
-		while (readsafe == 0) {
-			if (loops == 0) {
-				retval = -ETIME;
-				goto done;
-			}
+		while (1) {
 			/* start with setting the semaphores */
-			tmp = readl(&udc->op_regs->usbcmd);
-			tmp |= USBCMD_ATDTW_TRIPWIRE_SET;
-			writel(tmp, &udc->op_regs->usbcmd);
+			usbcmd = readl(&udc->op_regs->usbcmd);
+			usbcmd |= USBCMD_ATDTW_TRIPWIRE_SET;
+			writel(usbcmd, &udc->op_regs->usbcmd);
 
 			/* read the endpoint status */
 			epstatus = readl(&udc->op_regs->epstatus) & bit_pos;
@@ -329,98 +318,46 @@ static int queue_dtd(struct mv_ep *ep, struct mv_req *req)
 			 * primed.
 			 */
 			if (readl(&udc->op_regs->usbcmd)
-				& USBCMD_ATDTW_TRIPWIRE_SET) {
-				readsafe = 1;
-			}
+				& USBCMD_ATDTW_TRIPWIRE_SET)
+				break;
+
 			loops--;
+			if (loops == 0) {
+				dev_err(&udc->dev->dev,
+					"Timeout for ATDTW_TRIPWIRE...\n");
+				retval = -ETIME;
+				goto done;
+			}
 			udelay(LOOPS_USEC);
 		}
 
 		/* Clear the semaphore */
-		tmp = readl(&udc->op_regs->usbcmd);
-		tmp &= USBCMD_ATDTW_TRIPWIRE_CLEAR;
-		writel(tmp, &udc->op_regs->usbcmd);
-
-		/* If endpoint is not active, we activate it now. */
-		if (!epstatus) {
-			if (direction == EP_DIR_IN) {
-				struct mv_dtd *curr_dtd = dma_to_virt(
-					&udc->dev->dev, dqh->curr_dtd_ptr);
-
-				loops = LOOPS(DTD_TIMEOUT);
-				while (curr_dtd->size_ioc_sts
-					& DTD_STATUS_ACTIVE) {
-					if (loops == 0) {
-						retval = -ETIME;
-						goto done;
-					}
-					loops--;
-					udelay(LOOPS_USEC);
-				}
-			}
-			/* No other transfers on the queue */
+		usbcmd = readl(&udc->op_regs->usbcmd);
+		usbcmd &= USBCMD_ATDTW_TRIPWIRE_CLEAR;
+		writel(usbcmd, &udc->op_regs->usbcmd);
 
-			/* Write dQH next pointer and terminate bit to 0 */
-			dqh->next_dtd_ptr = req->head->td_dma
-				& EP_QUEUE_HEAD_NEXT_POINTER_MASK;
-			dqh->size_ioc_int_sts = 0;
+		if (epstatus)
+			goto done;
+	}
 
-			/*
-			 * Ensure that updates to the QH will
-			 * occur before priming.
-			 */
-			wmb();
+	/* Write dQH next pointer and terminate bit to 0 */
+	dqh->next_dtd_ptr = req->head->td_dma
+				& EP_QUEUE_HEAD_NEXT_POINTER_MASK;
 
-			/* Prime the Endpoint */
-			writel(bit_pos, &udc->op_regs->epprime);
-		}
-	} else {
-		/* Write dQH next pointer and terminate bit to 0 */
-		dqh->next_dtd_ptr = req->head->td_dma
-			& EP_QUEUE_HEAD_NEXT_POINTER_MASK;
-		dqh->size_ioc_int_sts = 0;
+	/* clear active and halt bit, in case set from a previous error */
+	dqh->size_ioc_int_sts &= ~(DTD_STATUS_ACTIVE | DTD_STATUS_HALTED);
 
-		/* Ensure that updates to the QH will occur before priming. */
-		wmb();
+	/* Ensure that updates to the QH will occure before priming. */
+	wmb();
 
-		/* Prime the Endpoint */
-		writel(bit_pos, &udc->op_regs->epprime);
+	/* Prime the Endpoint */
+	writel(bit_pos, &udc->op_regs->epprime);
 
-		if (direction == EP_DIR_IN) {
-			/* FIXME add status check after prime the IN ep */
-			int prime_again;
-			u32 curr_dtd_ptr = dqh->curr_dtd_ptr;
-
-			loops = LOOPS(DTD_TIMEOUT);
-			prime_again = 0;
-			while ((curr_dtd_ptr != req->head->td_dma)) {
-				curr_dtd_ptr = dqh->curr_dtd_ptr;
-				if (loops == 0) {
-					dev_err(&udc->dev->dev,
-						"failed to prime %s\n",
-						ep->name);
-					retval = -ETIME;
-					goto done;
-				}
-				loops--;
-				udelay(LOOPS_USEC);
-
-				if (loops == (LOOPS(DTD_TIMEOUT) >> 2)) {
-					if (prime_again)
-						goto done;
-					dev_info(&udc->dev->dev,
-						"prime again\n");
-					writel(bit_pos,
-						&udc->op_regs->epprime);
-					prime_again = 1;
-				}
-			}
-		}
-	}
 done:
 	return retval;
 }
 
+
 static struct mv_dtd *build_dtd(struct mv_req *req, unsigned *length,
 		dma_addr_t *dma, int *is_last)
 {
@@ -841,6 +778,27 @@ mv_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	return 0;
 }
 
+static void mv_prime_ep(struct mv_ep *ep, struct mv_req *req)
+{
+	struct mv_dqh *dqh = ep->dqh;
+	u32 bit_pos;
+
+	/* Write dQH next pointer and terminate bit to 0 */
+	dqh->next_dtd_ptr = req->head->td_dma
+		& EP_QUEUE_HEAD_NEXT_POINTER_MASK;
+
+	/* clear active and halt bit, in case set from a previous error */
+	dqh->size_ioc_int_sts &= ~(DTD_STATUS_ACTIVE | DTD_STATUS_HALTED);
+
+	/* Ensure that updates to the QH will occure before priming. */
+	wmb();
+
+	bit_pos = 1 << (((ep_dir(ep) == EP_DIR_OUT) ? 0 : 16) + ep->ep_num);
+
+	/* Prime the Endpoint */
+	writel(bit_pos, &ep->udc->op_regs->epprime);
+}
+
 /* dequeues (cancels, unlinks) an I/O request from an endpoint */
 static int mv_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 {
@@ -883,15 +841,13 @@ static int mv_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 
 		/* The request isn't the last request in this ep queue */
 		if (req->queue.next != &ep->queue) {
-			struct mv_dqh *qh;
 			struct mv_req *next_req;
 
-			qh = ep->dqh;
-			next_req = list_entry(req->queue.next, struct mv_req,
-					queue);
+			next_req = list_entry(req->queue.next,
+				struct mv_req, queue);
 
 			/* Point the QH to the first TD of next request */
-			writel((u32) next_req->head, &qh->curr_dtd_ptr);
+			mv_prime_ep(ep, next_req);
 		} else {
 			struct mv_dqh *qh;
 
@@ -1056,6 +1012,8 @@ static void udc_stop(struct mv_udc *udc)
 		USBINTR_PORT_CHANGE_DETECT_EN | USBINTR_RESET_EN);
 	writel(tmp, &udc->op_regs->usbintr);
 
+	udc->stopped = 1;
+
 	/* Reset the Run the bit in the command register to stop VUSB */
 	tmp = readl(&udc->op_regs->usbcmd);
 	tmp &= ~USBCMD_RUN_STOP;
@@ -1072,6 +1030,8 @@ static void udc_start(struct mv_udc *udc)
 	/* Enable interrupts */
 	writel(usbintr, &udc->op_regs->usbintr);
 
+	udc->stopped = 0;
+
 	/* Set the Run bit in the command register */
 	writel(USBCMD_RUN_STOP, &udc->op_regs->usbcmd);
 }
@@ -1134,11 +1094,11 @@ static int udc_reset(struct mv_udc *udc)
 	return 0;
 }
 
-static int mv_udc_enable(struct mv_udc *udc)
+static int mv_udc_enable_internal(struct mv_udc *udc)
 {
 	int retval;
 
-	if (udc->clock_gating == 0 || udc->active)
+	if (udc->active)
 		return 0;
 
 	dev_dbg(&udc->dev->dev, "enable udc\n");
@@ -1157,9 +1117,17 @@ static int mv_udc_enable(struct mv_udc *udc)
 	return 0;
 }
 
-static void mv_udc_disable(struct mv_udc *udc)
+static int mv_udc_enable(struct mv_udc *udc)
 {
-	if (udc->clock_gating && udc->active) {
+	if (udc->clock_gating)
+		return mv_udc_enable_internal(udc);
+
+	return 0;
+}
+
+static void mv_udc_disable_internal(struct mv_udc *udc)
+{
+	if (udc->active) {
 		dev_dbg(&udc->dev->dev, "disable udc\n");
 		if (udc->pdata->phy_deinit)
 			udc->pdata->phy_deinit(udc->phy_regs);
@@ -1168,6 +1136,12 @@ static void mv_udc_disable(struct mv_udc *udc)
 	}
 }
 
+static void mv_udc_disable(struct mv_udc *udc)
+{
+	if (udc->clock_gating)
+		mv_udc_disable_internal(udc);
+}
+
 static int mv_udc_get_frame(struct usb_gadget *gadget)
 {
 	struct mv_udc *udc;
@@ -1178,7 +1152,7 @@ static int mv_udc_get_frame(struct usb_gadget *gadget)
 
 	udc = container_of(gadget, struct mv_udc, gadget);
 
-	retval = readl(udc->op_regs->frindex) & USB_FRINDEX_MASKS;
+	retval = readl(&udc->op_regs->frindex) & USB_FRINDEX_MASKS;
 
 	return retval;
 }
@@ -1212,10 +1186,11 @@ static int mv_udc_vbus_session(struct usb_gadget *gadget, int is_active)
 	udc = container_of(gadget, struct mv_udc, gadget);
 	spin_lock_irqsave(&udc->lock, flags);
 
+	udc->vbus_active = (is_active != 0);
+
 	dev_dbg(&udc->dev->dev, "%s: softconnect %d, vbus_active %d\n",
 		__func__, udc->softconnect, udc->vbus_active);
 
-	udc->vbus_active = (is_active != 0);
 	if (udc->driver && udc->softconnect && udc->vbus_active) {
 		retval = mv_udc_enable(udc);
 		if (retval == 0) {
@@ -1244,10 +1219,11 @@ static int mv_udc_pullup(struct usb_gadget *gadget, int is_on)
 	udc = container_of(gadget, struct mv_udc, gadget);
 	spin_lock_irqsave(&udc->lock, flags);
 
+	udc->softconnect = (is_on != 0);
+
 	dev_dbg(&udc->dev->dev, "%s: softconnect %d, vbus_active %d\n",
 			__func__, udc->softconnect, udc->vbus_active);
 
-	udc->softconnect = (is_on != 0);
 	if (udc->driver && udc->softconnect && udc->vbus_active) {
 		retval = mv_udc_enable(udc);
 		if (retval == 0) {
@@ -1407,6 +1383,20 @@ static int mv_udc_start(struct usb_gadget_driver *driver,
 		return retval;
 	}
 
+	if (udc->transceiver) {
+		retval = otg_set_peripheral(udc->transceiver, &udc->gadget);
+		if (retval) {
+			dev_err(&udc->dev->dev,
+				"unable to register peripheral to otg\n");
+			if (driver->unbind) {
+				driver->unbind(&udc->gadget);
+				udc->gadget.dev.driver = NULL;
+				udc->driver = NULL;
+			}
+			return retval;
+		}
+	}
+
 	/* pullup is always on */
 	mv_udc_pullup(&udc->gadget, 1);
 
@@ -2026,6 +2016,10 @@ static irqreturn_t mv_udc_irq(int irq, void *dev)
 	struct mv_udc *udc = (struct mv_udc *)dev;
 	u32 status, intr;
 
+	/* Disable ISR when stopped bit is set */
+	if (udc->stopped)
+		return IRQ_NONE;
+
 	spin_lock(&udc->lock);
 
 	status = readl(&udc->op_regs->usbsts);
@@ -2109,7 +2103,12 @@ static int __devexit mv_udc_remove(struct platform_device *dev)
 		destroy_workqueue(udc->qwork);
 	}
 
-	if (udc->pdata && udc->pdata->vbus && udc->clock_gating)
+	/*
+	 * If we have transceiver inited,
+	 * then vbus irq will not be requested in udc driver.
+	 */
+	if (udc->pdata && udc->pdata->vbus
+		&& udc->clock_gating && udc->transceiver == NULL)
 		free_irq(udc->pdata->vbus->irq, &dev->dev);
 
 	/* free memory allocated in probe */
@@ -2129,11 +2128,9 @@ static int __devexit mv_udc_remove(struct platform_device *dev)
 
 	if (udc->cap_regs)
 		iounmap(udc->cap_regs);
-	udc->cap_regs = NULL;
 
 	if (udc->phy_regs)
-		iounmap((void *)udc->phy_regs);
-	udc->phy_regs = 0;
+		iounmap(udc->phy_regs);
 
 	if (udc->status_req) {
 		kfree(udc->status_req->req.buf);
@@ -2182,6 +2179,11 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 
 	udc->dev = dev;
 
+#ifdef CONFIG_USB_OTG_UTILS
+	if (pdata->mode == MV_USB_MODE_OTG)
+		udc->transceiver = otg_get_transceiver();
+#endif
+
 	udc->clknum = pdata->clknum;
 	for (clk_i = 0; clk_i < udc->clknum; clk_i++) {
 		udc->clk[clk_i] = clk_get(&dev->dev, pdata->clkname[clk_i]);
@@ -2213,24 +2215,20 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 		goto err_iounmap_capreg;
 	}
 
-	udc->phy_regs = (unsigned int)ioremap(r->start, resource_size(r));
-	if (udc->phy_regs == 0) {
+	udc->phy_regs = ioremap(r->start, resource_size(r));
+	if (udc->phy_regs == NULL) {
 		dev_err(&dev->dev, "failed to map phy I/O memory\n");
 		retval = -EBUSY;
 		goto err_iounmap_capreg;
 	}
 
 	/* we will acces controller register, so enable the clk */
-	udc_clock_enable(udc);
-	if (pdata->phy_init) {
-		retval = pdata->phy_init(udc->phy_regs);
-		if (retval) {
-			dev_err(&dev->dev, "phy init error %d\n", retval);
-			goto err_iounmap_phyreg;
-		}
-	}
+	retval = mv_udc_enable_internal(udc);
+	if (retval)
+		goto err_iounmap_phyreg;
 
-	udc->op_regs = (struct mv_op_regs __iomem *)((u32)udc->cap_regs
+	udc->op_regs =
+		(struct mv_op_regs __iomem *)((unsigned long)udc->cap_regs
 		+ (readl(&udc->cap_regs->caplength_hciversion)
 			& CAPLENGTH_MASK));
 	udc->max_eps = readl(&udc->cap_regs->dccparams) & DCCPARAMS_DEN_MASK;
@@ -2312,7 +2310,7 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 	udc->gadget.ep0 = &udc->eps[0].ep;	/* gadget ep0 */
 	INIT_LIST_HEAD(&udc->gadget.ep_list);	/* ep_list */
 	udc->gadget.speed = USB_SPEED_UNKNOWN;	/* speed */
-	udc->gadget.is_dualspeed = 1;		/* support dual speed */
+	udc->gadget.max_speed = USB_SPEED_HIGH;	/* support dual speed */
 
 	/* the "gadget" abstracts/virtualizes the controller */
 	dev_set_name(&udc->gadget.dev, "gadget");
@@ -2328,7 +2326,9 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 	eps_init(udc);
 
 	/* VBUS detect: we can disable/enable clock on demand.*/
-	if (pdata->vbus) {
+	if (udc->transceiver)
+		udc->clock_gating = 1;
+	else if (pdata->vbus) {
 		udc->clock_gating = 1;
 		retval = request_threaded_irq(pdata->vbus->irq, NULL,
 				mv_udc_vbus_irq, IRQF_ONESHOT, "vbus", udc);
@@ -2354,11 +2354,9 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 	 * If not, it means that VBUS detection is not supported, we
 	 * have to enable vbus active all the time to let controller work.
 	 */
-	if (udc->clock_gating) {
-		if (udc->pdata->phy_deinit)
-			udc->pdata->phy_deinit(udc->phy_regs);
-		udc_clock_disable(udc);
-	} else
+	if (udc->clock_gating)
+		mv_udc_disable_internal(udc);
+	else
 		udc->vbus_active = 1;
 
 	retval = usb_add_gadget_udc(&dev->dev, &udc->gadget);
@@ -2371,7 +2369,8 @@ static int __devinit mv_udc_probe(struct platform_device *dev)
 	return 0;
 
 err_unregister:
-	if (udc->pdata && udc->pdata->vbus && udc->clock_gating)
+	if (udc->pdata && udc->pdata->vbus
+		&& udc->clock_gating && udc->transceiver == NULL)
 		free_irq(pdata->vbus->irq, &dev->dev);
 	device_unregister(&udc->gadget.dev);
 err_free_irq:
@@ -2387,11 +2386,9 @@ err_free_dma:
 	dma_free_coherent(&dev->dev, udc->ep_dqh_size,
 			udc->ep_dqh, udc->ep_dqh_dma);
 err_disable_clock:
-	if (udc->pdata->phy_deinit)
-		udc->pdata->phy_deinit(udc->phy_regs);
-	udc_clock_disable(udc);
+	mv_udc_disable_internal(udc);
 err_iounmap_phyreg:
-	iounmap((void *)udc->phy_regs);
+	iounmap(udc->phy_regs);
 err_iounmap_capreg:
 	iounmap(udc->cap_regs);
 err_put_clk:
@@ -2407,7 +2404,30 @@ static int mv_udc_suspend(struct device *_dev)
 {
 	struct mv_udc *udc = the_controller;
 
-	udc_stop(udc);
+	/* if OTG is enabled, the following will be done in OTG driver*/
+	if (udc->transceiver)
+		return 0;
+
+	if (udc->pdata->vbus && udc->pdata->vbus->poll)
+		if (udc->pdata->vbus->poll() == VBUS_HIGH) {
+			dev_info(&udc->dev->dev, "USB cable is connected!\n");
+			return -EAGAIN;
+		}
+
+	/*
+	 * only cable is unplugged, udc can suspend.
+	 * So do not care about clock_gating == 1.
+	 */
+	if (!udc->clock_gating) {
+		udc_stop(udc);
+
+		spin_lock_irq(&udc->lock);
+		/* stop all usb activities */
+		stop_activity(udc, udc->driver);
+		spin_unlock_irq(&udc->lock);
+
+		mv_udc_disable_internal(udc);
+	}
 
 	return 0;
 }
@@ -2417,20 +2437,22 @@ static int mv_udc_resume(struct device *_dev)
 	struct mv_udc *udc = the_controller;
 	int retval;
 
-	if (udc->pdata->phy_init) {
-		retval = udc->pdata->phy_init(udc->phy_regs);
-		if (retval) {
-			dev_err(&udc->dev->dev,
-				"init phy error %d when resume back\n",
-				retval);
+	/* if OTG is enabled, the following will be done in OTG driver*/
+	if (udc->transceiver)
+		return 0;
+
+	if (!udc->clock_gating) {
+		retval = mv_udc_enable_internal(udc);
+		if (retval)
 			return retval;
+
+		if (udc->driver && udc->softconnect) {
+			udc_reset(udc);
+			ep0_reset(udc);
+			udc_start(udc);
 		}
 	}
 
-	udc_reset(udc);
-	ep0_reset(udc);
-	udc_start(udc);
-
 	return 0;
 }
 
@@ -2457,30 +2479,16 @@ static struct platform_driver udc_driver = {
 	.shutdown	= mv_udc_shutdown,
 	.driver		= {
 		.owner	= THIS_MODULE,
-		.name	= "pxa-u2o",
+		.name	= "mv-udc",
 #ifdef CONFIG_PM
 		.pm	= &mv_udc_pm_ops,
 #endif
 	},
 };
-MODULE_ALIAS("platform:pxa-u2o");
 
+module_platform_driver(udc_driver);
+MODULE_ALIAS("platform:mv-udc");
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR("Chao Xie <chao.xie@marvell.com>");
 MODULE_VERSION(DRIVER_VERSION);
 MODULE_LICENSE("GPL");
-
-
-static int __init init(void)
-{
-	return platform_driver_register(&udc_driver);
-}
-module_init(init);
-
-
-static void __exit cleanup(void)
-{
-	platform_driver_unregister(&udc_driver);
-}
-module_exit(cleanup);
-
diff --git a/drivers/usb/gadget/net2272.c b/drivers/usb/gadget/net2272.c
index d1b76368472f..4c81d540bc26 100644
--- a/drivers/usb/gadget/net2272.c
+++ b/drivers/usb/gadget/net2272.c
@@ -1459,7 +1459,7 @@ static int net2272_start(struct usb_gadget *_gadget,
 	unsigned i;
 
 	if (!driver || !driver->unbind || !driver->setup ||
-	    driver->speed != USB_SPEED_HIGH)
+	    driver->max_speed != USB_SPEED_HIGH)
 		return -EINVAL;
 
 	dev = container_of(_gadget, struct net2272, gadget);
@@ -2235,7 +2235,7 @@ net2272_probe_init(struct device *dev, unsigned int irq)
 	ret->irq = irq;
 	ret->dev = dev;
 	ret->gadget.ops = &net2272_ops;
-	ret->gadget.is_dualspeed = 1;
+	ret->gadget.max_speed = USB_SPEED_HIGH;
 
 	/* the "gadget" abstracts/virtualizes the controller */
 	dev_set_name(&ret->gadget.dev, "gadget");
diff --git a/drivers/usb/gadget/net2280.c b/drivers/usb/gadget/net2280.c
index da2b9d0be3ca..cf1f36454d08 100644
--- a/drivers/usb/gadget/net2280.c
+++ b/drivers/usb/gadget/net2280.c
@@ -1881,7 +1881,7 @@ static int net2280_start(struct usb_gadget *_gadget,
 	 * (dev->usb->xcvrdiag & FORCE_FULL_SPEED_MODE)
 	 * "must not be used in normal operation"
 	 */
-	if (!driver || driver->speed < USB_SPEED_HIGH
+	if (!driver || driver->max_speed < USB_SPEED_HIGH
 			|| !driver->setup)
 		return -EINVAL;
 
@@ -2698,7 +2698,7 @@ static int net2280_probe (struct pci_dev *pdev, const struct pci_device_id *id)
 	spin_lock_init (&dev->lock);
 	dev->pdev = pdev;
 	dev->gadget.ops = &net2280_ops;
-	dev->gadget.is_dualspeed = 1;
+	dev->gadget.max_speed = USB_SPEED_HIGH;
 
 	/* the "gadget" abstracts/virtualizes the controller */
 	dev_set_name(&dev->gadget.dev, "gadget");
diff --git a/drivers/usb/gadget/omap_udc.c b/drivers/usb/gadget/omap_udc.c
index 788989a10223..7db5bbe6251b 100644
--- a/drivers/usb/gadget/omap_udc.c
+++ b/drivers/usb/gadget/omap_udc.c
@@ -2110,7 +2110,7 @@ static int omap_udc_start(struct usb_gadget_driver *driver,
 		return -ENODEV;
 	if (!driver
 			// FIXME if otg, check:  driver->is_otg
-			|| driver->speed < USB_SPEED_FULL
+			|| driver->max_speed < USB_SPEED_FULL
 			|| !bind || !driver->setup)
 		return -EINVAL;
 
@@ -2676,6 +2676,7 @@ omap_udc_setup(struct platform_device *odev, struct otg_transceiver *xceiv)
 	INIT_LIST_HEAD(&udc->gadget.ep_list);
 	INIT_LIST_HEAD(&udc->iso);
 	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->gadget.max_speed = USB_SPEED_FULL;
 	udc->gadget.name = driver_name;
 
 	device_initialize(&udc->gadget.dev);
diff --git a/drivers/usb/gadget/pch_udc.c b/drivers/usb/gadget/pch_udc.c
index 5048a0c07640..dd2313cce1d3 100644
--- a/drivers/usb/gadget/pch_udc.c
+++ b/drivers/usb/gadget/pch_udc.c
@@ -2693,7 +2693,7 @@ static int pch_udc_start(struct usb_gadget_driver *driver,
 	struct pch_udc_dev	*dev = pch_udc;
 	int			retval;
 
-	if (!driver || (driver->speed == USB_SPEED_UNKNOWN) || !bind ||
+	if (!driver || (driver->max_speed == USB_SPEED_UNKNOWN) || !bind ||
 	    !driver->setup || !driver->unbind || !driver->disconnect) {
 		dev_err(&dev->pdev->dev,
 			"%s: invalid driver parameter\n", __func__);
@@ -2941,7 +2941,7 @@ static int pch_udc_probe(struct pci_dev *pdev,
 	dev->gadget.dev.dma_mask = pdev->dev.dma_mask;
 	dev->gadget.dev.release = gadget_release;
 	dev->gadget.name = KBUILD_MODNAME;
-	dev->gadget.is_dualspeed = 1;
+	dev->gadget.max_speed = USB_SPEED_HIGH;
 
 	retval = device_register(&dev->gadget.dev);
 	if (retval)
diff --git a/drivers/usb/gadget/printer.c b/drivers/usb/gadget/printer.c
index 65a8834f274b..d83134b0f78a 100644
--- a/drivers/usb/gadget/printer.c
+++ b/drivers/usb/gadget/printer.c
@@ -1141,7 +1141,7 @@ printer_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 				break;
 #ifdef CONFIG_USB_GADGET_DUALSPEED
 			case USB_DT_DEVICE_QUALIFIER:
-				if (!gadget->is_dualspeed)
+				if (!gadget_is_dualspeed(gadget))
 					break;
 				/*
 				 * assumes ep0 uses the same value for both
@@ -1155,7 +1155,7 @@ printer_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 				break;
 
 			case USB_DT_OTHER_SPEED_CONFIG:
-				if (!gadget->is_dualspeed)
+				if (!gadget_is_dualspeed(gadget))
 					break;
 				/* FALLTHROUGH */
 #endif /* CONFIG_USB_GADGET_DUALSPEED */
@@ -1535,7 +1535,7 @@ fail:
 /*-------------------------------------------------------------------------*/
 
 static struct usb_gadget_driver printer_driver = {
-	.speed		= DEVSPEED,
+	.max_speed	= DEVSPEED,
 
 	.function	= (char *) driver_desc,
 	.unbind		= printer_unbind,
diff --git a/drivers/usb/gadget/pxa25x_udc.c b/drivers/usb/gadget/pxa25x_udc.c
index c090a7e3ecf8..dd470635f4f7 100644
--- a/drivers/usb/gadget/pxa25x_udc.c
+++ b/drivers/usb/gadget/pxa25x_udc.c
@@ -1264,7 +1264,7 @@ static int pxa25x_start(struct usb_gadget_driver *driver,
 	int			retval;
 
 	if (!driver
-			|| driver->speed < USB_SPEED_FULL
+			|| driver->max_speed < USB_SPEED_FULL
 			|| !bind
 			|| !driver->disconnect
 			|| !driver->setup)
diff --git a/drivers/usb/gadget/pxa27x_udc.c b/drivers/usb/gadget/pxa27x_udc.c
index 18b6b091f2a6..f4c44eb806c3 100644
--- a/drivers/usb/gadget/pxa27x_udc.c
+++ b/drivers/usb/gadget/pxa27x_udc.c
@@ -1807,7 +1807,7 @@ static int pxa27x_udc_start(struct usb_gadget_driver *driver,
 	struct pxa_udc *udc = the_controller;
 	int retval;
 
-	if (!driver || driver->speed < USB_SPEED_FULL || !bind
+	if (!driver || driver->max_speed < USB_SPEED_FULL || !bind
 			|| !driver->disconnect || !driver->setup)
 		return -EINVAL;
 	if (!udc)
diff --git a/drivers/usb/gadget/r8a66597-udc.c b/drivers/usb/gadget/r8a66597-udc.c
index fc719a3f8557..f5b8d215e1d5 100644
--- a/drivers/usb/gadget/r8a66597-udc.c
+++ b/drivers/usb/gadget/r8a66597-udc.c
@@ -1746,7 +1746,7 @@ static int r8a66597_start(struct usb_gadget *gadget,
 	struct r8a66597 *r8a66597 = gadget_to_r8a66597(gadget);
 
 	if (!driver
-			|| driver->speed < USB_SPEED_HIGH
+			|| driver->max_speed < USB_SPEED_HIGH
 			|| !driver->setup)
 		return -EINVAL;
 	if (!r8a66597)
@@ -1911,7 +1911,7 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 
 	r8a66597->gadget.ops = &r8a66597_gadget_ops;
 	dev_set_name(&r8a66597->gadget.dev, "gadget");
-	r8a66597->gadget.is_dualspeed = 1;
+	r8a66597->gadget.max_speed = USB_SPEED_HIGH;
 	r8a66597->gadget.dev.parent = &pdev->dev;
 	r8a66597->gadget.dev.dma_mask = pdev->dev.dma_mask;
 	r8a66597->gadget.dev.release = pdev->dev.release;
diff --git a/drivers/usb/gadget/s3c-hsotg.c b/drivers/usb/gadget/s3c-hsotg.c
index b31448229f0b..69295ba9d99a 100644
--- a/drivers/usb/gadget/s3c-hsotg.c
+++ b/drivers/usb/gadget/s3c-hsotg.c
@@ -2586,7 +2586,7 @@ static int s3c_hsotg_start(struct usb_gadget_driver *driver,
 		return -EINVAL;
 	}
 
-	if (driver->speed < USB_SPEED_FULL)
+	if (driver->max_speed < USB_SPEED_FULL)
 		dev_err(hsotg->dev, "%s: bad speed\n", __func__);
 
 	if (!bind || !driver->setup) {
@@ -3362,7 +3362,7 @@ static int __devinit s3c_hsotg_probe(struct platform_device *pdev)
 
 	dev_set_name(&hsotg->gadget.dev, "gadget");
 
-	hsotg->gadget.is_dualspeed = 1;
+	hsotg->gadget.max_speed = USB_SPEED_HIGH;
 	hsotg->gadget.ops = &s3c_hsotg_gadget_ops;
 	hsotg->gadget.name = dev_name(dev);
 
@@ -3467,18 +3467,7 @@ static struct platform_driver s3c_hsotg_driver = {
 	.resume		= s3c_hsotg_resume,
 };
 
-static int __init s3c_hsotg_modinit(void)
-{
-	return platform_driver_register(&s3c_hsotg_driver);
-}
-
-static void __exit s3c_hsotg_modexit(void)
-{
-	platform_driver_unregister(&s3c_hsotg_driver);
-}
-
-module_init(s3c_hsotg_modinit);
-module_exit(s3c_hsotg_modexit);
+module_platform_driver(s3c_hsotg_driver);
 
 MODULE_DESCRIPTION("Samsung S3C USB High-speed/OtG device");
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
diff --git a/drivers/usb/gadget/s3c-hsudc.c b/drivers/usb/gadget/s3c-hsudc.c
index 20a553b46aed..df8661d266cb 100644
--- a/drivers/usb/gadget/s3c-hsudc.c
+++ b/drivers/usb/gadget/s3c-hsudc.c
@@ -28,9 +28,10 @@
 #include <linux/usb/gadget.h>
 #include <linux/usb/otg.h>
 #include <linux/prefetch.h>
+#include <linux/platform_data/s3c-hsudc.h>
+#include <linux/regulator/consumer.h>
 
 #include <mach/regs-s3c2443-clock.h>
-#include <plat/udc.h>
 
 #define S3C_HSUDC_REG(x)	(x)
 
@@ -87,6 +88,12 @@
 #define DATA_STATE_XMIT			(1)
 #define DATA_STATE_RECV			(2)
 
+static const char * const s3c_hsudc_supply_names[] = {
+	"vdda",		/* analog phy supply, 3.3V */
+	"vddi",		/* digital phy supply, 1.2V */
+	"vddosc",	/* oscillator supply, 1.8V - 3.3V */
+};
+
 /**
  * struct s3c_hsudc_ep - Endpoint representation used by driver.
  * @ep: USB gadget layer representation of device endpoint.
@@ -139,6 +146,7 @@ struct s3c_hsudc {
 	struct device *dev;
 	struct s3c24xx_hsudc_platdata *pd;
 	struct otg_transceiver *transceiver;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsudc_supply_names)];
 	spinlock_t lock;
 	void __iomem *regs;
 	struct resource *mem_rsrc;
@@ -153,7 +161,6 @@ struct s3c_hsudc {
 #define ep_index(_ep)		((_ep)->bEndpointAddress & \
 					USB_ENDPOINT_NUMBER_MASK)
 
-static struct s3c_hsudc *the_controller;
 static const char driver_name[] = "s3c-udc";
 static const char ep0name[] = "ep0-control";
 
@@ -282,8 +289,7 @@ static void s3c_hsudc_nuke_ep(struct s3c_hsudc_ep *hsep, int status)
  * All the endpoints are stopped and any pending transfer requests if any on
  * the endpoint are terminated.
  */
-static void s3c_hsudc_stop_activity(struct s3c_hsudc *hsudc,
-			  struct usb_gadget_driver *driver)
+static void s3c_hsudc_stop_activity(struct s3c_hsudc *hsudc)
 {
 	struct s3c_hsudc_ep *hsep;
 	int epnum;
@@ -295,10 +301,6 @@ static void s3c_hsudc_stop_activity(struct s3c_hsudc *hsudc,
 		hsep->stopped = 1;
 		s3c_hsudc_nuke_ep(hsep, -ESHUTDOWN);
 	}
-
-	spin_unlock(&hsudc->lock);
-	driver->disconnect(&hsudc->gadget);
-	spin_lock(&hsudc->lock);
 }
 
 /**
@@ -1135,16 +1137,15 @@ static irqreturn_t s3c_hsudc_irq(int irq, void *_dev)
 	return IRQ_HANDLED;
 }
 
-static int s3c_hsudc_start(struct usb_gadget_driver *driver,
-		int (*bind)(struct usb_gadget *))
+static int s3c_hsudc_start(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	struct s3c_hsudc *hsudc = the_controller;
+	struct s3c_hsudc *hsudc = to_hsudc(gadget);
 	int ret;
 
 	if (!driver
-		|| driver->speed < USB_SPEED_FULL
-		|| !bind
-		|| !driver->unbind || !driver->disconnect || !driver->setup)
+		|| driver->max_speed < USB_SPEED_FULL
+		|| !driver->setup)
 		return -EINVAL;
 
 	if (!hsudc)
@@ -1155,21 +1156,12 @@ static int s3c_hsudc_start(struct usb_gadget_driver *driver,
 
 	hsudc->driver = driver;
 	hsudc->gadget.dev.driver = &driver->driver;
-	hsudc->gadget.speed = USB_SPEED_UNKNOWN;
-	ret = device_add(&hsudc->gadget.dev);
-	if (ret) {
-		dev_err(hsudc->dev, "failed to probe gadget device");
-		return ret;
-	}
 
-	ret = bind(&hsudc->gadget);
-	if (ret) {
-		dev_err(hsudc->dev, "%s: bind failed\n", hsudc->gadget.name);
-		device_del(&hsudc->gadget.dev);
-
-		hsudc->driver = NULL;
-		hsudc->gadget.dev.driver = NULL;
-		return ret;
+	ret = regulator_bulk_enable(ARRAY_SIZE(hsudc->supplies),
+				    hsudc->supplies);
+	if (ret != 0) {
+		dev_err(hsudc->dev, "failed to enable supplies: %d\n", ret);
+		goto err_supplies;
 	}
 
 	/* connect to bus through transceiver */
@@ -1178,13 +1170,7 @@ static int s3c_hsudc_start(struct usb_gadget_driver *driver,
 		if (ret) {
 			dev_err(hsudc->dev, "%s: can't bind to transceiver\n",
 					hsudc->gadget.name);
-			driver->unbind(&hsudc->gadget);
-
-			device_del(&hsudc->gadget.dev);
-
-			hsudc->driver = NULL;
-			hsudc->gadget.dev.driver = NULL;
-			return ret;
+			goto err_otg;
 		}
 	}
 
@@ -1197,34 +1183,43 @@ static int s3c_hsudc_start(struct usb_gadget_driver *driver,
 		hsudc->pd->gpio_init();
 
 	return 0;
+err_otg:
+	regulator_bulk_disable(ARRAY_SIZE(hsudc->supplies), hsudc->supplies);
+err_supplies:
+	hsudc->driver = NULL;
+	hsudc->gadget.dev.driver = NULL;
+	return ret;
 }
 
-static int s3c_hsudc_stop(struct usb_gadget_driver *driver)
+static int s3c_hsudc_stop(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
 {
-	struct s3c_hsudc *hsudc = the_controller;
+	struct s3c_hsudc *hsudc = to_hsudc(gadget);
 	unsigned long flags;
 
 	if (!hsudc)
 		return -ENODEV;
 
-	if (!driver || driver != hsudc->driver || !driver->unbind)
+	if (!driver || driver != hsudc->driver)
 		return -EINVAL;
 
 	spin_lock_irqsave(&hsudc->lock, flags);
-	hsudc->driver = 0;
+	hsudc->driver = NULL;
+	hsudc->gadget.dev.driver = NULL;
+	hsudc->gadget.speed = USB_SPEED_UNKNOWN;
 	s3c_hsudc_uninit_phy();
 	if (hsudc->pd->gpio_uninit)
 		hsudc->pd->gpio_uninit();
-	s3c_hsudc_stop_activity(hsudc, driver);
+	s3c_hsudc_stop_activity(hsudc);
 	spin_unlock_irqrestore(&hsudc->lock, flags);
 
 	if (hsudc->transceiver)
 		(void) otg_set_peripheral(hsudc->transceiver, NULL);
 
-	driver->unbind(&hsudc->gadget);
-	device_del(&hsudc->gadget.dev);
 	disable_irq(hsudc->irq);
 
+	regulator_bulk_disable(ARRAY_SIZE(hsudc->supplies), hsudc->supplies);
+
 	dev_info(hsudc->dev, "unregistered gadget driver '%s'\n",
 			driver->driver.name);
 	return 0;
@@ -1242,7 +1237,7 @@ static int s3c_hsudc_gadget_getframe(struct usb_gadget *gadget)
 
 static int s3c_hsudc_vbus_draw(struct usb_gadget *gadget, unsigned mA)
 {
-	struct s3c_hsudc *hsudc = the_controller;
+	struct s3c_hsudc *hsudc = to_hsudc(gadget);
 
 	if (!hsudc)
 		return -ENODEV;
@@ -1255,18 +1250,18 @@ static int s3c_hsudc_vbus_draw(struct usb_gadget *gadget, unsigned mA)
 
 static struct usb_gadget_ops s3c_hsudc_gadget_ops = {
 	.get_frame	= s3c_hsudc_gadget_getframe,
-	.start		= s3c_hsudc_start,
-	.stop		= s3c_hsudc_stop,
+	.udc_start	= s3c_hsudc_start,
+	.udc_stop	= s3c_hsudc_stop,
 	.vbus_draw	= s3c_hsudc_vbus_draw,
 };
 
-static int s3c_hsudc_probe(struct platform_device *pdev)
+static int __devinit s3c_hsudc_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct resource *res;
 	struct s3c_hsudc *hsudc;
 	struct s3c24xx_hsudc_platdata *pd = pdev->dev.platform_data;
-	int ret;
+	int ret, i;
 
 	hsudc = kzalloc(sizeof(struct s3c_hsudc) +
 			sizeof(struct s3c_hsudc_ep) * pd->epnum,
@@ -1276,13 +1271,22 @@ static int s3c_hsudc_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	the_controller = hsudc;
 	platform_set_drvdata(pdev, dev);
 	hsudc->dev = dev;
 	hsudc->pd = pdev->dev.platform_data;
 
 	hsudc->transceiver = otg_get_transceiver();
 
+	for (i = 0; i < ARRAY_SIZE(hsudc->supplies); i++)
+		hsudc->supplies[i].supply = s3c_hsudc_supply_names[i];
+
+	ret = regulator_bulk_get(dev, ARRAY_SIZE(hsudc->supplies),
+				 hsudc->supplies);
+	if (ret != 0) {
+		dev_err(dev, "failed to request supplies: %d\n", ret);
+		goto err_supplies;
+	}
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(dev, "unable to obtain driver resource data\n");
@@ -1307,10 +1311,9 @@ static int s3c_hsudc_probe(struct platform_device *pdev)
 
 	spin_lock_init(&hsudc->lock);
 
-	device_initialize(&hsudc->gadget.dev);
 	dev_set_name(&hsudc->gadget.dev, "gadget");
 
-	hsudc->gadget.is_dualspeed = 1;
+	hsudc->gadget.max_speed = USB_SPEED_HIGH;
 	hsudc->gadget.ops = &s3c_hsudc_gadget_ops;
 	hsudc->gadget.name = dev_name(dev);
 	hsudc->gadget.dev.parent = dev;
@@ -1319,6 +1322,7 @@ static int s3c_hsudc_probe(struct platform_device *pdev)
 
 	hsudc->gadget.is_otg = 0;
 	hsudc->gadget.is_a_peripheral = 0;
+	hsudc->gadget.speed = USB_SPEED_UNKNOWN;
 
 	s3c_hsudc_setup_ep(hsudc);
 
@@ -1348,12 +1352,20 @@ static int s3c_hsudc_probe(struct platform_device *pdev)
 	disable_irq(hsudc->irq);
 	local_irq_enable();
 
+	ret = device_register(&hsudc->gadget.dev);
+	if (ret) {
+		put_device(&hsudc->gadget.dev);
+		goto err_add_device;
+	}
+
 	ret = usb_add_gadget_udc(&pdev->dev, &hsudc->gadget);
 	if (ret)
 		goto err_add_udc;
 
 	return 0;
 err_add_udc:
+	device_unregister(&hsudc->gadget.dev);
+err_add_device:
 	clk_disable(hsudc->uclk);
 	clk_put(hsudc->uclk);
 err_clk:
@@ -1362,10 +1374,13 @@ err_irq:
 	iounmap(hsudc->regs);
 
 err_remap:
-	release_resource(hsudc->mem_rsrc);
-	kfree(hsudc->mem_rsrc);
-
+	release_mem_region(res->start, resource_size(res));
 err_res:
+	if (hsudc->transceiver)
+		otg_put_transceiver(hsudc->transceiver);
+
+	regulator_bulk_free(ARRAY_SIZE(hsudc->supplies), hsudc->supplies);
+err_supplies:
 	kfree(hsudc);
 	return ret;
 }
@@ -1377,21 +1392,10 @@ static struct platform_driver s3c_hsudc_driver = {
 	},
 	.probe		= s3c_hsudc_probe,
 };
-MODULE_ALIAS("platform:s3c-hsudc");
-
-static int __init s3c_hsudc_modinit(void)
-{
-	return platform_driver_register(&s3c_hsudc_driver);
-}
 
-static void __exit s3c_hsudc_modexit(void)
-{
-	platform_driver_unregister(&s3c_hsudc_driver);
-}
-
-module_init(s3c_hsudc_modinit);
-module_exit(s3c_hsudc_modexit);
+module_platform_driver(s3c_hsudc_driver);
 
 MODULE_DESCRIPTION("Samsung S3C24XX USB high-speed controller driver");
 MODULE_AUTHOR("Thomas Abraham <thomas.ab@samsung.com>");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:s3c-hsudc");
diff --git a/drivers/usb/gadget/s3c2410_udc.c b/drivers/usb/gadget/s3c2410_udc.c
index fac4c650d4bb..3f87cb9344bb 100644
--- a/drivers/usb/gadget/s3c2410_udc.c
+++ b/drivers/usb/gadget/s3c2410_udc.c
@@ -1683,9 +1683,9 @@ static int s3c2410_udc_start(struct usb_gadget_driver *driver,
 	if (udc->driver)
 		return -EBUSY;
 
-	if (!bind || !driver->setup || driver->speed < USB_SPEED_FULL) {
+	if (!bind || !driver->setup || driver->max_speed < USB_SPEED_FULL) {
 		printk(KERN_ERR "Invalid driver: bind %p setup %p speed %d\n",
-			bind, driver->setup, driver->speed);
+			bind, driver->setup, driver->max_speed);
 		return -EINVAL;
 	}
 #if defined(MODULE)
diff --git a/drivers/usb/gadget/udc-core.c b/drivers/usb/gadget/udc-core.c
index 6939e17f4580..0b0d12ccc487 100644
--- a/drivers/usb/gadget/udc-core.c
+++ b/drivers/usb/gadget/udc-core.c
@@ -371,14 +371,28 @@ static ssize_t usb_udc_softconn_store(struct device *dev,
 }
 static DEVICE_ATTR(soft_connect, S_IWUSR, NULL, usb_udc_softconn_store);
 
-static ssize_t usb_udc_speed_show(struct device *dev,
+#define USB_UDC_SPEED_ATTR(name, param)					\
+ssize_t usb_udc_##param##_show(struct device *dev,			\
+		struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_udc *udc = container_of(dev, struct usb_udc, dev);	\
+	return snprintf(buf, PAGE_SIZE, "%s\n",				\
+			usb_speed_string(udc->gadget->param));		\
+}									\
+static DEVICE_ATTR(name, S_IRUSR, usb_udc_##param##_show, NULL)
+
+static USB_UDC_SPEED_ATTR(current_speed, speed);
+static USB_UDC_SPEED_ATTR(maximum_speed, max_speed);
+
+/* TODO: Scheduled for removal in 3.8. */
+static ssize_t usb_udc_is_dualspeed_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
 	struct usb_udc		*udc = container_of(dev, struct usb_udc, dev);
-	return snprintf(buf, PAGE_SIZE, "%s\n",
-			usb_speed_string(udc->gadget->speed));
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			gadget_is_dualspeed(udc->gadget));
 }
-static DEVICE_ATTR(speed, S_IRUGO, usb_udc_speed_show, NULL);
+static DEVICE_ATTR(is_dualspeed, S_IRUSR, usb_udc_is_dualspeed_show, NULL);
 
 #define USB_UDC_ATTR(name)					\
 ssize_t usb_udc_##name##_show(struct device *dev,		\
@@ -391,7 +405,6 @@ ssize_t usb_udc_##name##_show(struct device *dev,		\
 }								\
 static DEVICE_ATTR(name, S_IRUGO, usb_udc_##name##_show, NULL)
 
-static USB_UDC_ATTR(is_dualspeed);
 static USB_UDC_ATTR(is_otg);
 static USB_UDC_ATTR(is_a_peripheral);
 static USB_UDC_ATTR(b_hnp_enable);
@@ -401,7 +414,8 @@ static USB_UDC_ATTR(a_alt_hnp_support);
 static struct attribute *usb_udc_attrs[] = {
 	&dev_attr_srp.attr,
 	&dev_attr_soft_connect.attr,
-	&dev_attr_speed.attr,
+	&dev_attr_current_speed.attr,
+	&dev_attr_maximum_speed.attr,
 
 	&dev_attr_is_dualspeed.attr,
 	&dev_attr_is_otg.attr,
diff --git a/drivers/usb/gadget/usbstring.c b/drivers/usb/gadget/usbstring.c
index 58c4d37d312a..4d25b9009edf 100644
--- a/drivers/usb/gadget/usbstring.c
+++ b/drivers/usb/gadget/usbstring.c
@@ -13,82 +13,17 @@
 #include <linux/string.h>
 #include <linux/device.h>
 #include <linux/init.h>
+#include <linux/nls.h>
 
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 
-#include <asm/unaligned.h>
-
-
-static int utf8_to_utf16le(const char *s, __le16 *cp, unsigned len)
-{
-	int	count = 0;
-	u8	c;
-	u16	uchar;
-
-	/* this insists on correct encodings, though not minimal ones.
-	 * BUT it currently rejects legit 4-byte UTF-8 code points,
-	 * which need surrogate pairs.  (Unicode 3.1 can use them.)
-	 */
-	while (len != 0 && (c = (u8) *s++) != 0) {
-		if (unlikely(c & 0x80)) {
-			// 2-byte sequence:
-			// 00000yyyyyxxxxxx = 110yyyyy 10xxxxxx
-			if ((c & 0xe0) == 0xc0) {
-				uchar = (c & 0x1f) << 6;
-
-				c = (u8) *s++;
-				if ((c & 0xc0) != 0x80)
-					goto fail;
-				c &= 0x3f;
-				uchar |= c;
-
-			// 3-byte sequence (most CJKV characters):
-			// zzzzyyyyyyxxxxxx = 1110zzzz 10yyyyyy 10xxxxxx
-			} else if ((c & 0xf0) == 0xe0) {
-				uchar = (c & 0x0f) << 12;
-
-				c = (u8) *s++;
-				if ((c & 0xc0) != 0x80)
-					goto fail;
-				c &= 0x3f;
-				uchar |= c << 6;
-
-				c = (u8) *s++;
-				if ((c & 0xc0) != 0x80)
-					goto fail;
-				c &= 0x3f;
-				uchar |= c;
-
-				/* no bogus surrogates */
-				if (0xd800 <= uchar && uchar <= 0xdfff)
-					goto fail;
-
-			// 4-byte sequence (surrogate pairs, currently rare):
-			// 11101110wwwwzzzzyy + 110111yyyyxxxxxx
-			//     = 11110uuu 10uuzzzz 10yyyyyy 10xxxxxx
-			// (uuuuu = wwww + 1)
-			// FIXME accept the surrogate code points (only)
-
-			} else
-				goto fail;
-		} else
-			uchar = c;
-		put_unaligned_le16(uchar, cp++);
-		count++;
-		len--;
-	}
-	return count;
-fail:
-	return -1;
-}
-
 
 /**
  * usb_gadget_get_string - fill out a string descriptor 
  * @table: of c strings encoded using UTF-8
  * @id: string id, from low byte of wValue in get string descriptor
- * @buf: at least 256 bytes
+ * @buf: at least 256 bytes, must be 16-bit aligned
  *
  * Finds the UTF-8 string matching the ID, and converts it into a
  * string descriptor in utf16-le.
@@ -125,8 +60,8 @@ usb_gadget_get_string (struct usb_gadget_strings *table, int id, u8 *buf)
 
 	/* string descriptors have length, tag, then UTF16-LE text */
 	len = min ((size_t) 126, strlen (s->s));
-	memset (buf + 2, 0, 2 * len);	/* zero all the bytes */
-	len = utf8_to_utf16le(s->s, (__le16 *)&buf[2], len);
+	len = utf8s_to_utf16s(s->s, len, UTF16_LITTLE_ENDIAN,
+			(wchar_t *) &buf[2], 126);
 	if (len < 0)
 		return -EINVAL;
 	buf [0] = (len + 1) * 2;
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 060e0e2b1ae6..4c0c9734251d 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -194,6 +194,15 @@ config USB_EHCI_S5P
        help
 	 Enable support for the S5P SOC's on-chip EHCI controller.
 
+config USB_EHCI_MV
+	bool "EHCI support for Marvell on-chip controller"
+	depends on USB_EHCI_HCD
+	select USB_EHCI_ROOT_HUB_TT
+	---help---
+	  Enables support for Marvell (including PXA and MMP series) on-chip
+	  USB SPH and OTG controller. SPH is a single port host, and it can
+	  only be EHCI host. OTG is controller that can switch to host mode.
+
 config USB_W90X900_EHCI
 	bool "W90X900(W90P910) EHCI support"
 	depends on USB_EHCI_HCD && ARCH_W90X900
@@ -371,6 +380,12 @@ config USB_OHCI_SH
 	  Enables support for the on-chip OHCI controller on the SuperH.
 	  If you use the PCI OHCI controller, this option is not necessary.
 
+config USB_OHCI_EXYNOS
+	boolean "OHCI support for Samsung EXYNOS SoC Series"
+	depends on USB_OHCI_HCD && ARCH_EXYNOS
+	help
+	 Enable support for the Samsung Exynos SOC's on-chip OHCI controller.
+
 config USB_CNS3XXX_OHCI
 	bool "Cavium CNS3XXX OHCI Module"
 	depends on USB_OHCI_HCD && ARCH_CNS3XXX
diff --git a/drivers/usb/host/ehci-au1xxx.c b/drivers/usb/host/ehci-au1xxx.c
index 18bafa99fe57..bf7441afed16 100644
--- a/drivers/usb/host/ehci-au1xxx.c
+++ b/drivers/usb/host/ehci-au1xxx.c
@@ -23,6 +23,7 @@ static int au1xxx_ehci_setup(struct usb_hcd *hcd)
 	int ret = ehci_init(hcd);
 
 	ehci->need_io_watchdog = 0;
+	ehci_reset(ehci);
 	return ret;
 }
 
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 3ff9f82f7263..e311a511529b 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -48,6 +48,10 @@
 #include <asm/system.h>
 #include <asm/unaligned.h>
 
+#if defined(CONFIG_PPC_PS3)
+#include <asm/firmware.h>
+#endif
+
 /*-------------------------------------------------------------------------*/
 
 /*
@@ -230,12 +234,58 @@ static int ehci_halt (struct ehci_hcd *ehci)
 			  STS_HALT, STS_HALT, 16 * 125);
 }
 
+#if defined(CONFIG_USB_SUSPEND) && defined(CONFIG_PPC_PS3)
+
+/*
+ * The EHCI controller of the Cell Super Companion Chip used in the
+ * PS3 will stop the root hub after all root hub ports are suspended.
+ * When in this condition handshake will return -ETIMEDOUT.  The
+ * STS_HLT bit will not be set, so inspection of the frame index is
+ * used here to test for the condition.  If the condition is found
+ * return success to allow the USB suspend to complete.
+ */
+
+static int handshake_for_broken_root_hub(struct ehci_hcd *ehci,
+					 void __iomem *ptr, u32 mask, u32 done,
+					 int usec)
+{
+	unsigned int old_index;
+	int error;
+
+	if (!firmware_has_feature(FW_FEATURE_PS3_LV1))
+		return -ETIMEDOUT;
+
+	old_index = ehci_read_frame_index(ehci);
+
+	error = handshake(ehci, ptr, mask, done, usec);
+
+	if (error == -ETIMEDOUT && ehci_read_frame_index(ehci) == old_index)
+		return 0;
+
+	return error;
+}
+
+#else
+
+static int handshake_for_broken_root_hub(struct ehci_hcd *ehci,
+					 void __iomem *ptr, u32 mask, u32 done,
+					 int usec)
+{
+	return -ETIMEDOUT;
+}
+
+#endif
+
 static int handshake_on_error_set_halt(struct ehci_hcd *ehci, void __iomem *ptr,
 				       u32 mask, u32 done, int usec)
 {
 	int error;
 
 	error = handshake(ehci, ptr, mask, done, usec);
+	if (error == -ETIMEDOUT)
+		error = handshake_for_broken_root_hub(ehci, ptr, mask, done,
+						      usec);
+
 	if (error) {
 		ehci_halt(ehci);
 		ehci->rh_state = EHCI_RH_HALTED;
@@ -620,6 +670,7 @@ static int ehci_init(struct usb_hcd *hcd)
 	hw = ehci->async->hw;
 	hw->hw_next = QH_NEXT(ehci, ehci->async->qh_dma);
 	hw->hw_info1 = cpu_to_hc32(ehci, QH_HEAD);
+	hw->hw_info1 |= cpu_to_hc32(ehci, (1 << 7));	/* I = 1 */
 	hw->hw_token = cpu_to_hc32(ehci, QTD_STS_HALT);
 	hw->hw_qtd_next = EHCI_LIST_END(ehci);
 	ehci->async->qh_state = QH_STATE_LINKED;
@@ -677,22 +728,13 @@ static int ehci_init(struct usb_hcd *hcd)
 static int ehci_run (struct usb_hcd *hcd)
 {
 	struct ehci_hcd		*ehci = hcd_to_ehci (hcd);
-	int			retval;
 	u32			temp;
 	u32			hcc_params;
 
 	hcd->uses_new_polling = 1;
 
 	/* EHCI spec section 4.1 */
-	/*
-	 * TDI driver does the ehci_reset in their reset callback.
-	 * Don't reset here, because configuration settings will
-	 * vanish.
-	 */
-	if (!ehci_is_TDI(ehci) && (retval = ehci_reset(ehci)) != 0) {
-		ehci_mem_cleanup(ehci);
-		return retval;
-	}
+
 	ehci_writel(ehci, ehci->periodic_dma, &ehci->regs->frame_list);
 	ehci_writel(ehci, (u32)ehci->async->qh_dma, &ehci->regs->async_next);
 
@@ -1324,11 +1366,16 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER		ehci_pxa168_driver
 #endif
 
-#ifdef CONFIG_NLM_XLR
+#ifdef CONFIG_CPU_XLR
 #include "ehci-xls.c"
 #define PLATFORM_DRIVER		ehci_xls_driver
 #endif
 
+#ifdef CONFIG_USB_EHCI_MV
+#include "ehci-mv.c"
+#define        PLATFORM_DRIVER         ehci_mv_driver
+#endif
+
 #if !defined(PCI_DRIVER) && !defined(PLATFORM_DRIVER) && \
     !defined(PS3_SYSTEM_BUS_DRIVER) && !defined(OF_PLATFORM_DRIVER) && \
     !defined(XILINX_OF_PLATFORM_DRIVER)
diff --git a/drivers/usb/host/ehci-mv.c b/drivers/usb/host/ehci-mv.c
new file mode 100644
index 000000000000..52a604fb9321
--- /dev/null
+++ b/drivers/usb/host/ehci-mv.c
@@ -0,0 +1,391 @@
+/*
+ * Copyright (C) 2011 Marvell International Ltd. All rights reserved.
+ * Author: Chao Xie <chao.xie@marvell.com>
+ *        Neil Zhang <zhangwm@marvell.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/usb/otg.h>
+#include <linux/platform_data/mv_usb.h>
+
+#define CAPLENGTH_MASK         (0xff)
+
+struct ehci_hcd_mv {
+	struct usb_hcd *hcd;
+
+	/* Which mode does this ehci running OTG/Host ? */
+	int mode;
+
+	void __iomem *phy_regs;
+	void __iomem *cap_regs;
+	void __iomem *op_regs;
+
+	struct otg_transceiver *otg;
+
+	struct mv_usb_platform_data *pdata;
+
+	/* clock source and total clock number */
+	unsigned int clknum;
+	struct clk *clk[0];
+};
+
+static void ehci_clock_enable(struct ehci_hcd_mv *ehci_mv)
+{
+	unsigned int i;
+
+	for (i = 0; i < ehci_mv->clknum; i++)
+		clk_enable(ehci_mv->clk[i]);
+}
+
+static void ehci_clock_disable(struct ehci_hcd_mv *ehci_mv)
+{
+	unsigned int i;
+
+	for (i = 0; i < ehci_mv->clknum; i++)
+		clk_disable(ehci_mv->clk[i]);
+}
+
+static int mv_ehci_enable(struct ehci_hcd_mv *ehci_mv)
+{
+	int retval;
+
+	ehci_clock_enable(ehci_mv);
+	if (ehci_mv->pdata->phy_init) {
+		retval = ehci_mv->pdata->phy_init(ehci_mv->phy_regs);
+		if (retval)
+			return retval;
+	}
+
+	return 0;
+}
+
+static void mv_ehci_disable(struct ehci_hcd_mv *ehci_mv)
+{
+	if (ehci_mv->pdata->phy_deinit)
+		ehci_mv->pdata->phy_deinit(ehci_mv->phy_regs);
+	ehci_clock_disable(ehci_mv);
+}
+
+static int mv_ehci_reset(struct usb_hcd *hcd)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	struct device *dev = hcd->self.controller;
+	struct ehci_hcd_mv *ehci_mv = dev_get_drvdata(dev);
+	int retval;
+
+	if (ehci_mv == NULL) {
+		dev_err(dev, "Can not find private ehci data\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * data structure init
+	 */
+	retval = ehci_init(hcd);
+	if (retval) {
+		dev_err(dev, "ehci_init failed %d\n", retval);
+		return retval;
+	}
+
+	hcd->has_tt = 1;
+	ehci->sbrn = 0x20;
+
+	retval = ehci_reset(ehci);
+	if (retval) {
+		dev_err(dev, "ehci_reset failed %d\n", retval);
+		return retval;
+	}
+
+	return 0;
+}
+
+static const struct hc_driver mv_ehci_hc_driver = {
+	.description = hcd_name,
+	.product_desc = "Marvell EHCI",
+	.hcd_priv_size = sizeof(struct ehci_hcd),
+
+	/*
+	 * generic hardware linkage
+	 */
+	.irq = ehci_irq,
+	.flags = HCD_MEMORY | HCD_USB2,
+
+	/*
+	 * basic lifecycle operations
+	 */
+	.reset = mv_ehci_reset,
+	.start = ehci_run,
+	.stop = ehci_stop,
+	.shutdown = ehci_shutdown,
+
+	/*
+	 * managing i/o requests and associated device resources
+	 */
+	.urb_enqueue = ehci_urb_enqueue,
+	.urb_dequeue = ehci_urb_dequeue,
+	.endpoint_disable = ehci_endpoint_disable,
+	.endpoint_reset = ehci_endpoint_reset,
+	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
+
+	/*
+	 * scheduling support
+	 */
+	.get_frame_number = ehci_get_frame,
+
+	/*
+	 * root hub support
+	 */
+	.hub_status_data = ehci_hub_status_data,
+	.hub_control = ehci_hub_control,
+	.bus_suspend = ehci_bus_suspend,
+	.bus_resume = ehci_bus_resume,
+};
+
+static int mv_ehci_probe(struct platform_device *pdev)
+{
+	struct mv_usb_platform_data *pdata = pdev->dev.platform_data;
+	struct usb_hcd *hcd;
+	struct ehci_hcd *ehci;
+	struct ehci_hcd_mv *ehci_mv;
+	struct resource *r;
+	int clk_i, retval = -ENODEV;
+	u32 offset;
+	size_t size;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "missing platform_data\n");
+		return -ENODEV;
+	}
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	hcd = usb_create_hcd(&mv_ehci_hc_driver, &pdev->dev, "mv ehci");
+	if (!hcd)
+		return -ENOMEM;
+
+	size = sizeof(*ehci_mv) + sizeof(struct clk *) * pdata->clknum;
+	ehci_mv = kzalloc(size, GFP_KERNEL);
+	if (ehci_mv == NULL) {
+		dev_err(&pdev->dev, "cannot allocate ehci_hcd_mv\n");
+		retval = -ENOMEM;
+		goto err_put_hcd;
+	}
+
+	platform_set_drvdata(pdev, ehci_mv);
+	ehci_mv->pdata = pdata;
+	ehci_mv->hcd = hcd;
+
+	ehci_mv->clknum = pdata->clknum;
+	for (clk_i = 0; clk_i < ehci_mv->clknum; clk_i++) {
+		ehci_mv->clk[clk_i] =
+		    clk_get(&pdev->dev, pdata->clkname[clk_i]);
+		if (IS_ERR(ehci_mv->clk[clk_i])) {
+			dev_err(&pdev->dev, "error get clck \"%s\"\n",
+				pdata->clkname[clk_i]);
+			retval = PTR_ERR(ehci_mv->clk[clk_i]);
+			goto err_put_clk;
+		}
+	}
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phyregs");
+	if (r == NULL) {
+		dev_err(&pdev->dev, "no phy I/O memory resource defined\n");
+		retval = -ENODEV;
+		goto err_put_clk;
+	}
+
+	ehci_mv->phy_regs = ioremap(r->start, resource_size(r));
+	if (ehci_mv->phy_regs == 0) {
+		dev_err(&pdev->dev, "failed to map phy I/O memory\n");
+		retval = -EFAULT;
+		goto err_put_clk;
+	}
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "capregs");
+	if (!r) {
+		dev_err(&pdev->dev, "no I/O memory resource defined\n");
+		retval = -ENODEV;
+		goto err_iounmap_phyreg;
+	}
+
+	ehci_mv->cap_regs = ioremap(r->start, resource_size(r));
+	if (ehci_mv->cap_regs == NULL) {
+		dev_err(&pdev->dev, "failed to map I/O memory\n");
+		retval = -EFAULT;
+		goto err_iounmap_phyreg;
+	}
+
+	retval = mv_ehci_enable(ehci_mv);
+	if (retval) {
+		dev_err(&pdev->dev, "init phy error %d\n", retval);
+		goto err_iounmap_capreg;
+	}
+
+	offset = readl(ehci_mv->cap_regs) & CAPLENGTH_MASK;
+	ehci_mv->op_regs =
+		(void __iomem *) ((unsigned long) ehci_mv->cap_regs + offset);
+
+	hcd->rsrc_start = r->start;
+	hcd->rsrc_len = r->end - r->start + 1;
+	hcd->regs = ehci_mv->op_regs;
+
+	hcd->irq = platform_get_irq(pdev, 0);
+	if (!hcd->irq) {
+		dev_err(&pdev->dev, "Cannot get irq.");
+		retval = -ENODEV;
+		goto err_disable_clk;
+	}
+
+	ehci = hcd_to_ehci(hcd);
+	ehci->caps = (struct ehci_caps *) ehci_mv->cap_regs;
+	ehci->regs = (struct ehci_regs *) ehci_mv->op_regs;
+	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
+
+	ehci_mv->mode = pdata->mode;
+	if (ehci_mv->mode == MV_USB_MODE_OTG) {
+#ifdef CONFIG_USB_OTG_UTILS
+		ehci_mv->otg = otg_get_transceiver();
+		if (!ehci_mv->otg) {
+			dev_err(&pdev->dev,
+				"unable to find transceiver\n");
+			retval = -ENODEV;
+			goto err_disable_clk;
+		}
+
+		retval = otg_set_host(ehci_mv->otg, &hcd->self);
+		if (retval < 0) {
+			dev_err(&pdev->dev,
+				"unable to register with transceiver\n");
+			retval = -ENODEV;
+			goto err_put_transceiver;
+		}
+		/* otg will enable clock before use as host */
+		mv_ehci_disable(ehci_mv);
+#else
+		dev_info(&pdev->dev, "MV_USB_MODE_OTG "
+			 "must have CONFIG_USB_OTG_UTILS enabled\n");
+		goto err_disable_clk;
+#endif
+	} else {
+		if (pdata->set_vbus)
+			pdata->set_vbus(1);
+
+		retval = usb_add_hcd(hcd, hcd->irq, IRQF_SHARED);
+		if (retval) {
+			dev_err(&pdev->dev,
+				"failed to add hcd with err %d\n", retval);
+			goto err_set_vbus;
+		}
+	}
+
+	if (pdata->private_init)
+		pdata->private_init(ehci_mv->op_regs, ehci_mv->phy_regs);
+
+	dev_info(&pdev->dev,
+		 "successful find EHCI device with regs 0x%p irq %d"
+		 " working in %s mode\n", hcd->regs, hcd->irq,
+		 ehci_mv->mode == MV_USB_MODE_OTG ? "OTG" : "Host");
+
+	return 0;
+
+err_set_vbus:
+	if (pdata->set_vbus)
+		pdata->set_vbus(0);
+#ifdef CONFIG_USB_OTG_UTILS
+err_put_transceiver:
+	if (ehci_mv->otg)
+		otg_put_transceiver(ehci_mv->otg);
+#endif
+err_disable_clk:
+	mv_ehci_disable(ehci_mv);
+err_iounmap_capreg:
+	iounmap(ehci_mv->cap_regs);
+err_iounmap_phyreg:
+	iounmap(ehci_mv->phy_regs);
+err_put_clk:
+	for (clk_i--; clk_i >= 0; clk_i--)
+		clk_put(ehci_mv->clk[clk_i]);
+	platform_set_drvdata(pdev, NULL);
+	kfree(ehci_mv);
+err_put_hcd:
+	usb_put_hcd(hcd);
+
+	return retval;
+}
+
+static int mv_ehci_remove(struct platform_device *pdev)
+{
+	struct ehci_hcd_mv *ehci_mv = platform_get_drvdata(pdev);
+	struct usb_hcd *hcd = ehci_mv->hcd;
+	int clk_i;
+
+	if (hcd->rh_registered)
+		usb_remove_hcd(hcd);
+
+	if (ehci_mv->otg) {
+		otg_set_host(ehci_mv->otg, NULL);
+		otg_put_transceiver(ehci_mv->otg);
+	}
+
+	if (ehci_mv->mode == MV_USB_MODE_HOST) {
+		if (ehci_mv->pdata->set_vbus)
+			ehci_mv->pdata->set_vbus(0);
+
+		mv_ehci_disable(ehci_mv);
+	}
+
+	iounmap(ehci_mv->cap_regs);
+	iounmap(ehci_mv->phy_regs);
+
+	for (clk_i = 0; clk_i < ehci_mv->clknum; clk_i++)
+		clk_put(ehci_mv->clk[clk_i]);
+
+	platform_set_drvdata(pdev, NULL);
+
+	kfree(ehci_mv);
+	usb_put_hcd(hcd);
+
+	return 0;
+}
+
+MODULE_ALIAS("mv-ehci");
+
+static const struct platform_device_id ehci_id_table[] = {
+	{"pxa-u2oehci", PXA_U2OEHCI},
+	{"pxa-sph", PXA_SPH},
+	{"mmp3-hsic", MMP3_HSIC},
+	{"mmp3-fsic", MMP3_FSIC},
+	{},
+};
+
+static void mv_ehci_shutdown(struct platform_device *pdev)
+{
+	struct ehci_hcd_mv *ehci_mv = platform_get_drvdata(pdev);
+	struct usb_hcd *hcd = ehci_mv->hcd;
+
+	if (!hcd->rh_registered)
+		return;
+
+	if (hcd->driver->shutdown)
+		hcd->driver->shutdown(hcd);
+}
+
+static struct platform_driver ehci_mv_driver = {
+	.probe = mv_ehci_probe,
+	.remove = mv_ehci_remove,
+	.shutdown = mv_ehci_shutdown,
+	.driver = {
+		   .name = "mv-ehci",
+		   .bus = &platform_bus_type,
+		   },
+	.id_table = ehci_id_table,
+};
diff --git a/drivers/usb/host/ehci-octeon.c b/drivers/usb/host/ehci-octeon.c
index ba1f51361134..c0104882c72d 100644
--- a/drivers/usb/host/ehci-octeon.c
+++ b/drivers/usb/host/ehci-octeon.c
@@ -155,6 +155,8 @@ static int ehci_octeon_drv_probe(struct platform_device *pdev)
 	/* cache this readonly data; minimize chip reads */
 	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 
+	ehci_reset(ehci);
+
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret) {
 		dev_dbg(&pdev->dev, "failed to add hcd with err %d\n", ret);
diff --git a/drivers/usb/host/ehci-omap.c b/drivers/usb/host/ehci-omap.c
index e39b0297bad1..bba9850f32f0 100644
--- a/drivers/usb/host/ehci-omap.c
+++ b/drivers/usb/host/ehci-omap.c
@@ -41,6 +41,7 @@
 #include <linux/usb/ulpi.h>
 #include <plat/usb.h>
 #include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
 
 /* EHCI Register Set */
 #define EHCI_INSNREG04					(0xA0)
@@ -190,11 +191,8 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 		}
 	}
 
-	ret = omap_usbhs_enable(dev);
-	if (ret) {
-		dev_err(dev, "failed to start usbhs with err %d\n", ret);
-		goto err_enable;
-	}
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
 
 	/*
 	 * An undocumented "feature" in the OMAP3 EHCI controller,
@@ -228,6 +226,8 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 	/* cache this readonly data; minimize chip reads */
 	omap_ehci->hcs_params = readl(&omap_ehci->caps->hcs_params);
 
+	ehci_reset(omap_ehci);
+
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret) {
 		dev_err(dev, "failed to add hcd with err %d\n", ret);
@@ -240,11 +240,8 @@ static int ehci_hcd_omap_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_hcd:
-	omap_usbhs_disable(dev);
-
-err_enable:
 	disable_put_regulator(pdata);
-	usb_put_hcd(hcd);
+	pm_runtime_put_sync(dev);
 
 err_io:
 	iounmap(regs);
@@ -266,10 +263,12 @@ static int ehci_hcd_omap_remove(struct platform_device *pdev)
 	struct usb_hcd *hcd	= dev_get_drvdata(dev);
 
 	usb_remove_hcd(hcd);
-	omap_usbhs_disable(dev);
 	disable_put_regulator(dev->platform_data);
 	iounmap(hcd->regs);
 	usb_put_hcd(hcd);
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
+
 	return 0;
 }
 
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index a68a2a5c4b83..6c6a5a3b4ea7 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -172,7 +172,7 @@ static const struct hc_driver ehci_orion_hc_driver = {
 
 static void __init
 ehci_orion_conf_mbus_windows(struct usb_hcd *hcd,
-				struct mbus_dram_target_info *dram)
+			     const struct mbus_dram_target_info *dram)
 {
 	int i;
 
@@ -182,7 +182,7 @@ ehci_orion_conf_mbus_windows(struct usb_hcd *hcd,
 	}
 
 	for (i = 0; i < dram->num_cs; i++) {
-		struct mbus_dram_window *cs = dram->cs + i;
+		const struct mbus_dram_window *cs = dram->cs + i;
 
 		wrl(USB_WINDOW_CTRL(i), ((cs->size - 1) & 0xffff0000) |
 					(cs->mbus_attr << 8) |
@@ -194,6 +194,7 @@ ehci_orion_conf_mbus_windows(struct usb_hcd *hcd,
 static int __devinit ehci_orion_drv_probe(struct platform_device *pdev)
 {
 	struct orion_ehci_data *pd = pdev->dev.platform_data;
+	const struct mbus_dram_target_info *dram;
 	struct resource *res;
 	struct usb_hcd *hcd;
 	struct ehci_hcd *ehci;
@@ -259,8 +260,9 @@ static int __devinit ehci_orion_drv_probe(struct platform_device *pdev)
 	/*
 	 * (Re-)program MBUS remapping windows if we are asked to.
 	 */
-	if (pd != NULL && pd->dram != NULL)
-		ehci_orion_conf_mbus_windows(hcd, pd->dram);
+	dram = mv_mbus_dram_info();
+	if (dram)
+		ehci_orion_conf_mbus_windows(hcd, dram);
 
 	/*
 	 * setup Orion USB controller.
diff --git a/drivers/usb/host/ehci-ps3.c b/drivers/usb/host/ehci-ps3.c
index 2dc32da75cfc..a20e496eb479 100644
--- a/drivers/usb/host/ehci-ps3.c
+++ b/drivers/usb/host/ehci-ps3.c
@@ -21,6 +21,34 @@
 #include <asm/firmware.h>
 #include <asm/ps3.h>
 
+static void ps3_ehci_setup_insnreg(struct ehci_hcd *ehci)
+{
+	/* PS3 HC internal setup register offsets. */
+
+	enum ps3_ehci_hc_insnreg {
+		ps3_ehci_hc_insnreg01 = 0x084,
+		ps3_ehci_hc_insnreg02 = 0x088,
+		ps3_ehci_hc_insnreg03 = 0x08c,
+	};
+
+	/* PS3 EHCI HC errata fix 316 - The PS3 EHCI HC will reset its
+	 * internal INSNREGXX setup regs back to the chip default values
+	 * on Host Controller Reset (CMD_RESET) or Light Host Controller
+	 * Reset (CMD_LRESET).  The work-around for this is for the HC
+	 * driver to re-initialise these regs when ever the HC is reset.
+	 */
+
+	/* Set burst transfer counts to 256 out, 32 in. */
+
+	writel_be(0x01000020, (void __iomem *)ehci->regs +
+		ps3_ehci_hc_insnreg01);
+
+	/* Enable burst transfer counts. */
+
+	writel_be(0x00000001, (void __iomem *)ehci->regs +
+		ps3_ehci_hc_insnreg03);
+}
+
 static int ps3_ehci_hc_reset(struct usb_hcd *hcd)
 {
 	int result;
@@ -49,6 +77,8 @@ static int ps3_ehci_hc_reset(struct usb_hcd *hcd)
 
 	ehci_reset(ehci);
 
+	ps3_ehci_setup_insnreg(ehci);
+
 	return result;
 }
 
diff --git a/drivers/usb/host/ehci-pxa168.c b/drivers/usb/host/ehci-pxa168.c
index ac0c16e8f539..8d0e7a22e711 100644
--- a/drivers/usb/host/ehci-pxa168.c
+++ b/drivers/usb/host/ehci-pxa168.c
@@ -299,7 +299,7 @@ static int __devinit ehci_pxa168_drv_probe(struct platform_device *pdev)
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = hcd->regs + 0x100;
 	ehci->regs = hcd->regs + 0x100 +
-		HC_LENGTH(ehci_readl(ehci, &ehci->caps->hc_capbase));
+		HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
 	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
 	hcd->has_tt = 1;
 	ehci->sbrn = 0x20;
diff --git a/drivers/usb/host/ehci-q.c b/drivers/usb/host/ehci-q.c
index 4e4066c35a09..36ca5077cdf7 100644
--- a/drivers/usb/host/ehci-q.c
+++ b/drivers/usb/host/ehci-q.c
@@ -373,6 +373,17 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
  retry_xacterr:
 		if ((token & QTD_STS_ACTIVE) == 0) {
 
+			/* Report Data Buffer Error: non-fatal but useful */
+			if (token & QTD_STS_DBE)
+				ehci_dbg(ehci,
+					"detected DataBufferErr for urb %p ep%d%s len %d, qtd %p [qh %p]\n",
+					urb,
+					usb_endpoint_num(&urb->ep->desc),
+					usb_endpoint_dir_in(&urb->ep->desc) ? "in" : "out",
+					urb->transfer_buffer_length,
+					qtd,
+					qh);
+
 			/* on STALL, error, and short reads this urb must
 			 * complete and all its qtds must be recycled.
 			 */
@@ -647,7 +658,7 @@ qh_urb_transaction (
 	/*
 	 * data transfer stage:  buffer setup
 	 */
-	i = urb->num_sgs;
+	i = urb->num_mapped_sgs;
 	if (len > 0 && i > 0) {
 		sg = urb->sg;
 		buf = sg_dma_address(sg);
diff --git a/drivers/usb/host/ehci-s5p.c b/drivers/usb/host/ehci-s5p.c
index 024b65c4990d..293f7412992e 100644
--- a/drivers/usb/host/ehci-s5p.c
+++ b/drivers/usb/host/ehci-s5p.c
@@ -14,8 +14,6 @@
 
 #include <linux/clk.h>
 #include <linux/platform_device.h>
-#include <mach/regs-pmu.h>
-#include <plat/cpu.h>
 #include <plat/ehci.h>
 #include <plat/usb-phy.h>
 
@@ -136,6 +134,8 @@ static int __devinit s5p_ehci_probe(struct platform_device *pdev)
 	/* cache this readonly data; minimize chip reads */
 	ehci->hcs_params = readl(&ehci->caps->hcs_params);
 
+	ehci_reset(ehci);
+
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to add USB HCD\n");
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index db9d1b4bfbdc..dbc7fe8ca9e7 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -21,7 +21,12 @@
 #include <linux/platform_data/tegra_usb.h>
 #include <linux/irq.h>
 #include <linux/usb/otg.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
 #include <mach/usb_phy.h>
+#include <mach/iomap.h>
 
 #define TEGRA_USB_DMA_ALIGN 32
 
@@ -574,6 +579,35 @@ static const struct hc_driver tegra_ehci_hc_driver = {
 	.port_handed_over	= ehci_port_handed_over,
 };
 
+static int setup_vbus_gpio(struct platform_device *pdev)
+{
+	int err = 0;
+	int gpio;
+
+	if (!pdev->dev.of_node)
+		return 0;
+
+	gpio = of_get_named_gpio(pdev->dev.of_node, "nvidia,vbus-gpio", 0);
+	if (!gpio_is_valid(gpio))
+		return 0;
+
+	err = gpio_request(gpio, "vbus_gpio");
+	if (err) {
+		dev_err(&pdev->dev, "can't request vbus gpio %d", gpio);
+		return err;
+	}
+	err = gpio_direction_output(gpio, 1);
+	if (err) {
+		dev_err(&pdev->dev, "can't enable vbus\n");
+		return err;
+	}
+	gpio_set_value(gpio, 1);
+
+	return err;
+}
+
+static u64 tegra_ehci_dma_mask = DMA_BIT_MASK(32);
+
 static int tegra_ehci_probe(struct platform_device *pdev)
 {
 	struct resource *res;
@@ -590,6 +624,15 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
+	/* Right now device-tree probed devices don't get dma_mask set.
+	 * Since shared usb code relies on it, set it here for now.
+	 * Once we have dma capability bindings this can go away.
+	 */
+	if (!pdev->dev.dma_mask)
+		pdev->dev.dma_mask = &tegra_ehci_dma_mask;
+
+	setup_vbus_gpio(pdev);
+
 	tegra = kzalloc(sizeof(struct tegra_ehci_hcd), GFP_KERNEL);
 	if (!tegra)
 		return -ENOMEM;
@@ -640,6 +683,28 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 		goto fail_io;
 	}
 
+	/* This is pretty ugly and needs to be fixed when we do only
+	 * device-tree probing. Old code relies on the platform_device
+	 * numbering that we lack for device-tree-instantiated devices.
+	 */
+	if (instance < 0) {
+		switch (res->start) {
+		case TEGRA_USB_BASE:
+			instance = 0;
+			break;
+		case TEGRA_USB2_BASE:
+			instance = 1;
+			break;
+		case TEGRA_USB3_BASE:
+			instance = 2;
+			break;
+		default:
+			err = -ENODEV;
+			dev_err(&pdev->dev, "unknown usb instance\n");
+			goto fail_phy;
+		}
+	}
+
 	tegra->phy = tegra_usb_phy_open(instance, hcd->regs, pdata->phy_config,
 						TEGRA_USB_PHY_MODE_HOST);
 	if (IS_ERR(tegra->phy)) {
@@ -773,6 +838,11 @@ static void tegra_ehci_hcd_shutdown(struct platform_device *pdev)
 		hcd->driver->shutdown(hcd);
 }
 
+static struct of_device_id tegra_ehci_of_match[] __devinitdata = {
+	{ .compatible = "nvidia,tegra20-ehci", },
+	{ },
+};
+
 static struct platform_driver tegra_ehci_driver = {
 	.probe		= tegra_ehci_probe,
 	.remove		= tegra_ehci_remove,
@@ -783,5 +853,6 @@ static struct platform_driver tegra_ehci_driver = {
 	.shutdown	= tegra_ehci_hcd_shutdown,
 	.driver		= {
 		.name	= "tegra-ehci",
+		.of_match_table = tegra_ehci_of_match,
 	}
 };
diff --git a/drivers/usb/host/ehci-vt8500.c b/drivers/usb/host/ehci-vt8500.c
index 54d1ab8aec49..c1eda73916cd 100644
--- a/drivers/usb/host/ehci-vt8500.c
+++ b/drivers/usb/host/ehci-vt8500.c
@@ -132,6 +132,8 @@ static int vt8500_ehci_drv_probe(struct platform_device *pdev)
 
 	ehci_port_power(ehci, 1);
 
+	ehci_reset(ehci);
+
 	ret = usb_add_hcd(hcd, pdev->resource[1].start,
 			  IRQF_SHARED);
 	if (ret == 0) {
diff --git a/drivers/usb/host/ehci-w90x900.c b/drivers/usb/host/ehci-w90x900.c
index d661cf7de140..3d2e26cbb34c 100644
--- a/drivers/usb/host/ehci-w90x900.c
+++ b/drivers/usb/host/ehci-w90x900.c
@@ -78,6 +78,8 @@ static int __devinit usb_w90x900_probe(const struct hc_driver *driver,
 	if (irq < 0)
 		goto err4;
 
+	ehci_reset(ehci);
+
 	retval = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (retval != 0)
 		goto err4;
diff --git a/drivers/usb/host/ehci-xls.c b/drivers/usb/host/ehci-xls.c
index b4fb511d24bc..72f08196f8cd 100644
--- a/drivers/usb/host/ehci-xls.c
+++ b/drivers/usb/host/ehci-xls.c
@@ -69,7 +69,7 @@ int ehci_xls_probe_internal(const struct hc_driver *driver,
 	}
 
 	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = res->end - res->start + 1;
+	hcd->rsrc_len = resource_size(res);
 
 	if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len,
 				driver->description)) {
diff --git a/drivers/usb/host/fhci-hcd.c b/drivers/usb/host/fhci-hcd.c
index 4ed6d19f2a54..d2623747b489 100644
--- a/drivers/usb/host/fhci-hcd.c
+++ b/drivers/usb/host/fhci-hcd.c
@@ -824,17 +824,7 @@ static struct platform_driver of_fhci_driver = {
 	.remove		= __devexit_p(of_fhci_remove),
 };
 
-static int __init fhci_module_init(void)
-{
-	return platform_driver_register(&of_fhci_driver);
-}
-module_init(fhci_module_init);
-
-static void __exit fhci_module_exit(void)
-{
-	platform_driver_unregister(&of_fhci_driver);
-}
-module_exit(fhci_module_exit);
+module_platform_driver(of_fhci_driver);
 
 MODULE_DESCRIPTION("USB Freescale Host Controller Interface Driver");
 MODULE_AUTHOR("Shlomi Gridish <gridish@freescale.com>, "
diff --git a/drivers/usb/host/fsl-mph-dr-of.c b/drivers/usb/host/fsl-mph-dr-of.c
index 9037035ad1e4..7916e56a725e 100644
--- a/drivers/usb/host/fsl-mph-dr-of.c
+++ b/drivers/usb/host/fsl-mph-dr-of.c
@@ -297,17 +297,7 @@ static struct platform_driver fsl_usb2_mph_dr_driver = {
 	.remove	= __devexit_p(fsl_usb2_mph_dr_of_remove),
 };
 
-static int __init fsl_usb2_mph_dr_init(void)
-{
-	return platform_driver_register(&fsl_usb2_mph_dr_driver);
-}
-module_init(fsl_usb2_mph_dr_init);
-
-static void __exit fsl_usb2_mph_dr_exit(void)
-{
-	platform_driver_unregister(&fsl_usb2_mph_dr_driver);
-}
-module_exit(fsl_usb2_mph_dr_exit);
+module_platform_driver(fsl_usb2_mph_dr_driver);
 
 MODULE_DESCRIPTION("FSL MPH DR OF devices driver");
 MODULE_AUTHOR("Anatolij Gustschin <agust@denx.de>");
diff --git a/drivers/usb/host/hwa-hc.c b/drivers/usb/host/hwa-hc.c
index 43b3ca48d753..104730dabd2d 100644
--- a/drivers/usb/host/hwa-hc.c
+++ b/drivers/usb/host/hwa-hc.c
@@ -776,7 +776,6 @@ static int hwahc_probe(struct usb_interface *usb_iface,
 		goto error_alloc;
 	}
 	usb_hcd->wireless = 1;
-	set_bit(HCD_FLAG_SAW_IRQ, &usb_hcd->flags);
 	wusbhc = usb_hcd_to_wusbhc(usb_hcd);
 	hwahc = container_of(wusbhc, struct hwahc, wusbhc);
 	hwahc_init(hwahc);
diff --git a/drivers/usb/host/imx21-hcd.c b/drivers/usb/host/imx21-hcd.c
index dbf0f156ed9e..ff471c1c165e 100644
--- a/drivers/usb/host/imx21-hcd.c
+++ b/drivers/usb/host/imx21-hcd.c
@@ -1924,18 +1924,7 @@ static struct platform_driver imx21_hcd_driver = {
 	.resume = NULL,
 };
 
-static int __init imx21_hcd_init(void)
-{
-	return platform_driver_register(&imx21_hcd_driver);
-}
-
-static void __exit imx21_hcd_cleanup(void)
-{
-	platform_driver_unregister(&imx21_hcd_driver);
-}
-
-module_init(imx21_hcd_init);
-module_exit(imx21_hcd_cleanup);
+module_platform_driver(imx21_hcd_driver);
 
 MODULE_DESCRIPTION("i.MX21 USB Host controller");
 MODULE_AUTHOR("Martin Fuzzey");
diff --git a/drivers/usb/host/isp1760-hcd.c b/drivers/usb/host/isp1760-hcd.c
index 27dfab80ed8f..fc72d44bf787 100644
--- a/drivers/usb/host/isp1760-hcd.c
+++ b/drivers/usb/host/isp1760-hcd.c
@@ -32,6 +32,13 @@ static struct kmem_cache *qtd_cachep;
 static struct kmem_cache *qh_cachep;
 static struct kmem_cache *urb_listitem_cachep;
 
+enum queue_head_types {
+	QH_CONTROL,
+	QH_BULK,
+	QH_INTERRUPT,
+	QH_END
+};
+
 struct isp1760_hcd {
 	u32 hcs_params;
 	spinlock_t		lock;
@@ -40,7 +47,7 @@ struct isp1760_hcd {
 	struct slotinfo		int_slots[32];
 	int			int_done_map;
 	struct memory_chunk memory_pool[BLOCKS];
-	struct list_head	controlqhs, bulkqhs, interruptqhs;
+	struct list_head	qh_list[QH_END];
 
 	/* periodic schedule support */
 #define	DEFAULT_I_TDPS		1024
@@ -406,12 +413,12 @@ static int priv_init(struct usb_hcd *hcd)
 {
 	struct isp1760_hcd		*priv = hcd_to_priv(hcd);
 	u32			hcc_params;
+	int i;
 
 	spin_lock_init(&priv->lock);
 
-	INIT_LIST_HEAD(&priv->interruptqhs);
-	INIT_LIST_HEAD(&priv->controlqhs);
-	INIT_LIST_HEAD(&priv->bulkqhs);
+	for (i = 0; i < QH_END; i++)
+		INIT_LIST_HEAD(&priv->qh_list[i]);
 
 	/*
 	 * hw default: 1K periodic list heads, one per frame.
@@ -930,9 +937,9 @@ void schedule_ptds(struct usb_hcd *hcd)
 	struct isp1760_hcd *priv;
 	struct isp1760_qh *qh, *qh_next;
 	struct list_head *ep_queue;
-	struct usb_host_endpoint *ep;
 	LIST_HEAD(urb_list);
 	struct urb_listitem *urb_listitem, *urb_listitem_next;
+	int i;
 
 	if (!hcd) {
 		WARN_ON(1);
@@ -944,28 +951,13 @@ void schedule_ptds(struct usb_hcd *hcd)
 	/*
 	 * check finished/retired xfers, transfer payloads, call urb_done()
 	 */
-	ep_queue = &priv->interruptqhs;
-	while (ep_queue) {
+	for (i = 0; i < QH_END; i++) {
+		ep_queue = &priv->qh_list[i];
 		list_for_each_entry_safe(qh, qh_next, ep_queue, qh_list) {
-			ep = list_entry(qh->qtd_list.next, struct isp1760_qtd,
-							qtd_list)->urb->ep;
 			collect_qtds(hcd, qh, &urb_list);
-			if (list_empty(&qh->qtd_list)) {
+			if (list_empty(&qh->qtd_list))
 				list_del(&qh->qh_list);
-				if (ep->hcpriv == NULL) {
-					/* Endpoint has been disabled, so we
-					can free the associated queue head. */
-					qh_free(qh);
-				}
-			}
 		}
-
-		if (ep_queue == &priv->interruptqhs)
-			ep_queue = &priv->controlqhs;
-		else if (ep_queue == &priv->controlqhs)
-			ep_queue = &priv->bulkqhs;
-		else
-			ep_queue = NULL;
 	}
 
 	list_for_each_entry_safe(urb_listitem, urb_listitem_next, &urb_list,
@@ -998,17 +990,10 @@ void schedule_ptds(struct usb_hcd *hcd)
 	 *
 	 * I'm sure this scheme could be improved upon!
 	 */
-	ep_queue = &priv->controlqhs;
-	while (ep_queue) {
+	for (i = 0; i < QH_END; i++) {
+		ep_queue = &priv->qh_list[i];
 		list_for_each_entry_safe(qh, qh_next, ep_queue, qh_list)
 			enqueue_qtds(hcd, qh);
-
-		if (ep_queue == &priv->controlqhs)
-			ep_queue = &priv->interruptqhs;
-		else if (ep_queue == &priv->interruptqhs)
-			ep_queue = &priv->bulkqhs;
-		else
-			ep_queue = NULL;
 	}
 }
 
@@ -1543,16 +1528,16 @@ static int isp1760_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 
 	switch (usb_pipetype(urb->pipe)) {
 	case PIPE_CONTROL:
-		ep_queue = &priv->controlqhs;
+		ep_queue = &priv->qh_list[QH_CONTROL];
 		break;
 	case PIPE_BULK:
-		ep_queue = &priv->bulkqhs;
+		ep_queue = &priv->qh_list[QH_BULK];
 		break;
 	case PIPE_INTERRUPT:
 		if (urb->interval < 0)
 			return -EINVAL;
 		/* FIXME: Check bandwidth  */
-		ep_queue = &priv->interruptqhs;
+		ep_queue = &priv->qh_list[QH_INTERRUPT];
 		break;
 	case PIPE_ISOCHRONOUS:
 		dev_err(hcd->self.controller, "%s: isochronous USB packets "
@@ -1714,8 +1699,8 @@ static void isp1760_endpoint_disable(struct usb_hcd *hcd,
 {
 	struct isp1760_hcd *priv = hcd_to_priv(hcd);
 	unsigned long spinflags;
-	struct isp1760_qh *qh;
-	struct isp1760_qtd *qtd;
+	struct isp1760_qh *qh, *qh_iter;
+	int i;
 
 	spin_lock_irqsave(&priv->lock, spinflags);
 
@@ -1723,14 +1708,17 @@ static void isp1760_endpoint_disable(struct usb_hcd *hcd,
 	if (!qh)
 		goto out;
 
-	list_for_each_entry(qtd, &qh->qtd_list, qtd_list)
-		if (qtd->status != QTD_RETIRE) {
-			dequeue_urb_from_qtd(hcd, qh, qtd);
-			qtd->urb->status = -ECONNRESET;
-		}
+	WARN_ON(!list_empty(&qh->qtd_list));
 
+	for (i = 0; i < QH_END; i++)
+		list_for_each_entry(qh_iter, &priv->qh_list[i], qh_list)
+			if (qh_iter == qh) {
+				list_del(&qh_iter->qh_list);
+				i = QH_END;
+				break;
+			}
+	qh_free(qh);
 	ep->hcpriv = NULL;
-	/* Cannot free qh here since it will be parsed by schedule_ptds() */
 
 	schedule_ptds(hcd);
 
diff --git a/drivers/usb/host/isp1760-if.c b/drivers/usb/host/isp1760-if.c
index 2ac4ac2e4ef9..4592dc17a9f9 100644
--- a/drivers/usb/host/isp1760-if.c
+++ b/drivers/usb/host/isp1760-if.c
@@ -47,23 +47,27 @@ static int of_isp1760_probe(struct platform_device *dev)
 	int virq;
 	resource_size_t res_len;
 	int ret;
-	const unsigned int *prop;
 	unsigned int devflags = 0;
 	enum of_gpio_flags gpio_flags;
+	u32 bus_width = 0;
 
 	drvdata = kzalloc(sizeof(*drvdata), GFP_KERNEL);
 	if (!drvdata)
 		return -ENOMEM;
 
 	ret = of_address_to_resource(dp, 0, &memory);
-	if (ret)
-		return -ENXIO;
+	if (ret) {
+		ret = -ENXIO;
+		goto free_data;
+	}
 
 	res_len = resource_size(&memory);
 
 	res = request_mem_region(memory.start, res_len, dev_name(&dev->dev));
-	if (!res)
-		return -EBUSY;
+	if (!res) {
+		ret = -EBUSY;
+		goto free_data;
+	}
 
 	if (of_irq_map_one(dp, 0, &oirq)) {
 		ret = -ENODEV;
@@ -77,8 +81,8 @@ static int of_isp1760_probe(struct platform_device *dev)
 		devflags |= ISP1760_FLAG_ISP1761;
 
 	/* Some systems wire up only 16 of the 32 data lines */
-	prop = of_get_property(dp, "bus-width", NULL);
-	if (prop && *prop == 16)
+	of_property_read_u32(dp, "bus-width", &bus_width);
+	if (bus_width == 16)
 		devflags |= ISP1760_FLAG_BUS_WIDTH_16;
 
 	if (of_get_property(dp, "port1-otg", NULL) != NULL)
@@ -125,6 +129,7 @@ free_gpio:
 		gpio_free(drvdata->rst_gpio);
 release_reg:
 	release_mem_region(memory.start, res_len);
+free_data:
 	kfree(drvdata);
 	return ret;
 }
diff --git a/drivers/usb/host/ohci-at91.c b/drivers/usb/host/ohci-at91.c
index 95a9fec38e89..5df0b0e3392b 100644
--- a/drivers/usb/host/ohci-at91.c
+++ b/drivers/usb/host/ohci-at91.c
@@ -223,7 +223,7 @@ static void ohci_at91_usb_set_power(struct at91_usbh_data *pdata, int port, int
 	if (port < 0 || port >= 2)
 		return;
 
-	if (pdata->vbus_pin[port] <= 0)
+	if (!gpio_is_valid(pdata->vbus_pin[port]))
 		return;
 
 	gpio_set_value(pdata->vbus_pin[port], !pdata->vbus_pin_inverted ^ enable);
@@ -234,7 +234,7 @@ static int ohci_at91_usb_get_power(struct at91_usbh_data *pdata, int port)
 	if (port < 0 || port >= 2)
 		return -EINVAL;
 
-	if (pdata->vbus_pin[port] <= 0)
+	if (!gpio_is_valid(pdata->vbus_pin[port]))
 		return -EINVAL;
 
 	return gpio_get_value(pdata->vbus_pin[port]) ^ !pdata->vbus_pin_inverted;
@@ -465,7 +465,7 @@ static int ohci_hcd_at91_drv_probe(struct platform_device *pdev)
 
 	if (pdata) {
 		for (i = 0; i < ARRAY_SIZE(pdata->vbus_pin); i++) {
-			if (pdata->vbus_pin[i] <= 0)
+			if (!gpio_is_valid(pdata->vbus_pin[i]))
 				continue;
 			gpio_request(pdata->vbus_pin[i], "ohci_vbus");
 			ohci_at91_usb_set_power(pdata, i, 1);
@@ -474,7 +474,7 @@ static int ohci_hcd_at91_drv_probe(struct platform_device *pdev)
 		for (i = 0; i < ARRAY_SIZE(pdata->overcurrent_pin); i++) {
 			int ret;
 
-			if (pdata->overcurrent_pin[i] <= 0)
+			if (!gpio_is_valid(pdata->overcurrent_pin[i]))
 				continue;
 			gpio_request(pdata->overcurrent_pin[i], "ohci_overcurrent");
 
@@ -499,14 +499,14 @@ static int ohci_hcd_at91_drv_remove(struct platform_device *pdev)
 
 	if (pdata) {
 		for (i = 0; i < ARRAY_SIZE(pdata->vbus_pin); i++) {
-			if (pdata->vbus_pin[i] <= 0)
+			if (!gpio_is_valid(pdata->vbus_pin[i]))
 				continue;
 			ohci_at91_usb_set_power(pdata, i, 0);
 			gpio_free(pdata->vbus_pin[i]);
 		}
 
 		for (i = 0; i < ARRAY_SIZE(pdata->overcurrent_pin); i++) {
-			if (pdata->overcurrent_pin[i] <= 0)
+			if (!gpio_is_valid(pdata->overcurrent_pin[i]))
 				continue;
 			free_irq(gpio_to_irq(pdata->overcurrent_pin[i]), pdev);
 			gpio_free(pdata->overcurrent_pin[i]);
diff --git a/drivers/usb/host/ohci-au1xxx.c b/drivers/usb/host/ohci-au1xxx.c
index 9b66df8278f3..40d886adff53 100644
--- a/drivers/usb/host/ohci-au1xxx.c
+++ b/drivers/usb/host/ohci-au1xxx.c
@@ -173,12 +173,9 @@ static int ohci_hcd_au1xxx_drv_suspend(struct device *dev)
 	 * mark HW unaccessible, bail out if RH has been resumed. Use
 	 * the spinlock to properly synchronize with possible pending
 	 * RH suspend or resume activity.
-	 *
-	 * This is still racy as hcd->state is manipulated outside of
-	 * any locks =P But that will be a different fix.
 	 */
 	spin_lock_irqsave(&ohci->lock, flags);
-	if (hcd->state != HC_STATE_SUSPENDED) {
+	if (ohci->rh_state != OHCI_RH_SUSPENDED) {
 		rc = -EINVAL;
 		goto bail;
 	}
diff --git a/drivers/usb/host/ohci-dbg.c b/drivers/usb/host/ohci-dbg.c
index d7d34492934a..5179fcd73d8a 100644
--- a/drivers/usb/host/ohci-dbg.c
+++ b/drivers/usb/host/ohci-dbg.c
@@ -127,6 +127,19 @@ static char *hcfs2string (int state)
 	return "?";
 }
 
+static const char *rh_state_string(struct ohci_hcd *ohci)
+{
+	switch (ohci->rh_state) {
+	case OHCI_RH_HALTED:
+		return "halted";
+	case OHCI_RH_SUSPENDED:
+		return "suspended";
+	case OHCI_RH_RUNNING:
+		return "running";
+	}
+	return "?";
+}
+
 // dump control and status registers
 static void
 ohci_dump_status (struct ohci_hcd *controller, char **next, unsigned *size)
@@ -136,9 +149,10 @@ ohci_dump_status (struct ohci_hcd *controller, char **next, unsigned *size)
 
 	temp = ohci_readl (controller, &regs->revision) & 0xff;
 	ohci_dbg_sw (controller, next, size,
-		"OHCI %d.%d, %s legacy support registers\n",
+		"OHCI %d.%d, %s legacy support registers, rh state %s\n",
 		0x03 & (temp >> 4), (temp & 0x0f),
-		(temp & 0x0100) ? "with" : "NO");
+		(temp & 0x0100) ? "with" : "NO",
+		rh_state_string(controller));
 
 	temp = ohci_readl (controller, &regs->control);
 	ohci_dbg_sw (controller, next, size,
diff --git a/drivers/usb/host/ohci-ep93xx.c b/drivers/usb/host/ohci-ep93xx.c
index dc45d489d00e..3d63574d2c7e 100644
--- a/drivers/usb/host/ohci-ep93xx.c
+++ b/drivers/usb/host/ohci-ep93xx.c
@@ -179,8 +179,6 @@ static int ohci_hcd_ep93xx_drv_suspend(struct platform_device *pdev, pm_message_
 	ohci->next_statechange = jiffies;
 
 	ep93xx_stop_hc(&pdev->dev);
-	hcd->state = HC_STATE_SUSPENDED;
-
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
new file mode 100644
index 000000000000..55aa35aa3d7b
--- /dev/null
+++ b/drivers/usb/host/ohci-exynos.c
@@ -0,0 +1,274 @@
+/*
+ * SAMSUNG EXYNOS USB HOST OHCI Controller
+ *
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Author: Jingoo Han <jg1.han@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <mach/ohci.h>
+#include <plat/usb-phy.h>
+
+struct exynos_ohci_hcd {
+	struct device *dev;
+	struct usb_hcd *hcd;
+	struct clk *clk;
+};
+
+static int ohci_exynos_start(struct usb_hcd *hcd)
+{
+	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
+	int ret;
+
+	ohci_dbg(ohci, "ohci_exynos_start, ohci:%p", ohci);
+
+	ret = ohci_init(ohci);
+	if (ret < 0)
+		return ret;
+
+	ret = ohci_run(ohci);
+	if (ret < 0) {
+		err("can't start %s", hcd->self.bus_name);
+		ohci_stop(hcd);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct hc_driver exynos_ohci_hc_driver = {
+	.description		= hcd_name,
+	.product_desc		= "EXYNOS OHCI Host Controller",
+	.hcd_priv_size		= sizeof(struct ohci_hcd),
+
+	.irq			= ohci_irq,
+	.flags			= HCD_MEMORY|HCD_USB11,
+
+	.start			= ohci_exynos_start,
+	.stop			= ohci_stop,
+	.shutdown		= ohci_shutdown,
+
+	.get_frame_number	= ohci_get_frame,
+
+	.urb_enqueue		= ohci_urb_enqueue,
+	.urb_dequeue		= ohci_urb_dequeue,
+	.endpoint_disable	= ohci_endpoint_disable,
+
+	.hub_status_data	= ohci_hub_status_data,
+	.hub_control		= ohci_hub_control,
+#ifdef	CONFIG_PM
+	.bus_suspend		= ohci_bus_suspend,
+	.bus_resume		= ohci_bus_resume,
+#endif
+	.start_port_reset	= ohci_start_port_reset,
+};
+
+static int __devinit exynos_ohci_probe(struct platform_device *pdev)
+{
+	struct exynos4_ohci_platdata *pdata;
+	struct exynos_ohci_hcd *exynos_ohci;
+	struct usb_hcd *hcd;
+	struct ohci_hcd *ohci;
+	struct resource *res;
+	int irq;
+	int err;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform data defined\n");
+		return -EINVAL;
+	}
+
+	exynos_ohci = kzalloc(sizeof(struct exynos_ohci_hcd), GFP_KERNEL);
+	if (!exynos_ohci)
+		return -ENOMEM;
+
+	exynos_ohci->dev = &pdev->dev;
+
+	hcd = usb_create_hcd(&exynos_ohci_hc_driver, &pdev->dev,
+					dev_name(&pdev->dev));
+	if (!hcd) {
+		dev_err(&pdev->dev, "Unable to create HCD\n");
+		err = -ENOMEM;
+		goto fail_hcd;
+	}
+
+	exynos_ohci->hcd = hcd;
+	exynos_ohci->clk = clk_get(&pdev->dev, "usbhost");
+
+	if (IS_ERR(exynos_ohci->clk)) {
+		dev_err(&pdev->dev, "Failed to get usbhost clock\n");
+		err = PTR_ERR(exynos_ohci->clk);
+		goto fail_clk;
+	}
+
+	err = clk_enable(exynos_ohci->clk);
+	if (err)
+		goto fail_clken;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get I/O memory\n");
+		err = -ENXIO;
+		goto fail_io;
+	}
+
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+	hcd->regs = ioremap(res->start, resource_size(res));
+	if (!hcd->regs) {
+		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
+		err = -ENOMEM;
+		goto fail_io;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (!irq) {
+		dev_err(&pdev->dev, "Failed to get IRQ\n");
+		err = -ENODEV;
+		goto fail;
+	}
+
+	if (pdata->phy_init)
+		pdata->phy_init(pdev, S5P_USB_PHY_HOST);
+
+	ohci = hcd_to_ohci(hcd);
+	ohci_hcd_init(ohci);
+
+	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to add USB HCD\n");
+		goto fail;
+	}
+
+	platform_set_drvdata(pdev, exynos_ohci);
+
+	return 0;
+
+fail:
+	iounmap(hcd->regs);
+fail_io:
+	clk_disable(exynos_ohci->clk);
+fail_clken:
+	clk_put(exynos_ohci->clk);
+fail_clk:
+	usb_put_hcd(hcd);
+fail_hcd:
+	kfree(exynos_ohci);
+	return err;
+}
+
+static int __devexit exynos_ohci_remove(struct platform_device *pdev)
+{
+	struct exynos4_ohci_platdata *pdata = pdev->dev.platform_data;
+	struct exynos_ohci_hcd *exynos_ohci = platform_get_drvdata(pdev);
+	struct usb_hcd *hcd = exynos_ohci->hcd;
+
+	usb_remove_hcd(hcd);
+
+	if (pdata && pdata->phy_exit)
+		pdata->phy_exit(pdev, S5P_USB_PHY_HOST);
+
+	iounmap(hcd->regs);
+
+	clk_disable(exynos_ohci->clk);
+	clk_put(exynos_ohci->clk);
+
+	usb_put_hcd(hcd);
+	kfree(exynos_ohci);
+
+	return 0;
+}
+
+static void exynos_ohci_shutdown(struct platform_device *pdev)
+{
+	struct exynos_ohci_hcd *exynos_ohci = platform_get_drvdata(pdev);
+	struct usb_hcd *hcd = exynos_ohci->hcd;
+
+	if (hcd->driver->shutdown)
+		hcd->driver->shutdown(hcd);
+}
+
+#ifdef CONFIG_PM
+static int exynos_ohci_suspend(struct device *dev)
+{
+	struct exynos_ohci_hcd *exynos_ohci = dev_get_drvdata(dev);
+	struct usb_hcd *hcd = exynos_ohci->hcd;
+	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
+	struct platform_device *pdev = to_platform_device(dev);
+	struct exynos4_ohci_platdata *pdata = pdev->dev.platform_data;
+	unsigned long flags;
+	int rc = 0;
+
+	/*
+	 * Root hub was already suspended. Disable irq emission and
+	 * mark HW unaccessible, bail out if RH has been resumed. Use
+	 * the spinlock to properly synchronize with possible pending
+	 * RH suspend or resume activity.
+	 *
+	 * This is still racy as hcd->state is manipulated outside of
+	 * any locks =P But that will be a different fix.
+	 */
+	spin_lock_irqsave(&ohci->lock, flags);
+	if (hcd->state != HC_STATE_SUSPENDED && hcd->state != HC_STATE_HALT) {
+		rc = -EINVAL;
+		goto fail;
+	}
+
+	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+
+	if (pdata && pdata->phy_exit)
+		pdata->phy_exit(pdev, S5P_USB_PHY_HOST);
+fail:
+	spin_unlock_irqrestore(&ohci->lock, flags);
+
+	return rc;
+}
+
+static int exynos_ohci_resume(struct device *dev)
+{
+	struct exynos_ohci_hcd *exynos_ohci = dev_get_drvdata(dev);
+	struct usb_hcd *hcd = exynos_ohci->hcd;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct exynos4_ohci_platdata *pdata = pdev->dev.platform_data;
+
+	if (pdata && pdata->phy_init)
+		pdata->phy_init(pdev, S5P_USB_PHY_HOST);
+
+	/* Mark hardware accessible again as we are out of D3 state by now */
+	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+
+	ohci_finish_controller_resume(hcd);
+
+	return 0;
+}
+#else
+#define exynos_ohci_suspend	NULL
+#define exynos_ohci_resume	NULL
+#endif
+
+static const struct dev_pm_ops exynos_ohci_pm_ops = {
+	.suspend	= exynos_ohci_suspend,
+	.resume		= exynos_ohci_resume,
+};
+
+static struct platform_driver exynos_ohci_driver = {
+	.probe		= exynos_ohci_probe,
+	.remove		= __devexit_p(exynos_ohci_remove),
+	.shutdown	= exynos_ohci_shutdown,
+	.driver = {
+		.name	= "exynos-ohci",
+		.owner	= THIS_MODULE,
+		.pm	= &exynos_ohci_pm_ops,
+	}
+};
+
+MODULE_ALIAS("platform:exynos-ohci");
+MODULE_AUTHOR("Jingoo Han <jg1.han@samsung.com>");
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index b2639191549e..5f5a63241436 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -209,7 +209,7 @@ static int ohci_urb_enqueue (
 		retval = -ENODEV;
 		goto fail;
 	}
-	if (!HC_IS_RUNNING(hcd->state)) {
+	if (ohci->rh_state != OHCI_RH_RUNNING) {
 		retval = -ENODEV;
 		goto fail;
 	}
@@ -274,7 +274,7 @@ static int ohci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 	rc = usb_hcd_check_unlink_urb(hcd, urb, status);
 	if (rc) {
 		;	/* Do nothing */
-	} else if (HC_IS_RUNNING(hcd->state)) {
+	} else if (ohci->rh_state == OHCI_RH_RUNNING) {
 		urb_priv_t  *urb_priv;
 
 		/* Unless an IRQ completed the unlink while it was being
@@ -321,7 +321,7 @@ ohci_endpoint_disable (struct usb_hcd *hcd, struct usb_host_endpoint *ep)
 rescan:
 	spin_lock_irqsave (&ohci->lock, flags);
 
-	if (!HC_IS_RUNNING (hcd->state)) {
+	if (ohci->rh_state != OHCI_RH_RUNNING) {
 sanitize:
 		ed->state = ED_IDLE;
 		if (quirk_zfmicro(ohci) && ed->type == PIPE_INTERRUPT)
@@ -377,6 +377,7 @@ static void ohci_usb_reset (struct ohci_hcd *ohci)
 	ohci->hc_control = ohci_readl (ohci, &ohci->regs->control);
 	ohci->hc_control &= OHCI_CTRL_RWC;
 	ohci_writel (ohci, ohci->hc_control, &ohci->regs->control);
+	ohci->rh_state = OHCI_RH_HALTED;
 }
 
 /* ohci_shutdown forcibly disables IRQs and DMA, helping kexec and
@@ -500,7 +501,7 @@ static int ohci_init (struct ohci_hcd *ohci)
 	if (distrust_firmware)
 		ohci->flags |= OHCI_QUIRK_HUB_POWER;
 
-	disable (ohci);
+	ohci->rh_state = OHCI_RH_HALTED;
 	ohci->regs = hcd->regs;
 
 	/* REVISIT this BIOS handshake is now moved into PCI "quirks", and
@@ -575,7 +576,7 @@ static int ohci_run (struct ohci_hcd *ohci)
 	int			first = ohci->fminterval == 0;
 	struct usb_hcd		*hcd = ohci_to_hcd(ohci);
 
-	disable (ohci);
+	ohci->rh_state = OHCI_RH_HALTED;
 
 	/* boot firmware should have set this up (5.1.1.3.1) */
 	if (first) {
@@ -688,7 +689,7 @@ retry:
 	ohci->hc_control &= OHCI_CTRL_RWC;
 	ohci->hc_control |= OHCI_CONTROL_INIT | OHCI_USB_OPER;
 	ohci_writel (ohci, ohci->hc_control, &ohci->regs->control);
-	hcd->state = HC_STATE_RUNNING;
+	ohci->rh_state = OHCI_RH_RUNNING;
 
 	/* wake on ConnectStatusChange, matching external hubs */
 	ohci_writel (ohci, RH_HS_DRWE, &ohci->regs->roothub.status);
@@ -725,7 +726,6 @@ retry:
 
 	// POTPGT delay is bits 24-31, in 2 ms units.
 	mdelay ((val >> 23) & 0x1fe);
-	hcd->state = HC_STATE_RUNNING;
 
 	if (quirk_zfmicro(ohci)) {
 		/* Create timer to watch for bad queue state on ZF Micro */
@@ -761,7 +761,7 @@ static irqreturn_t ohci_irq (struct usb_hcd *hcd)
 	 * of dead, unclocked, or unplugged (CardBus...) devices
 	 */
 	if (ints == ~(u32)0) {
-		disable (ohci);
+		ohci->rh_state = OHCI_RH_HALTED;
 		ohci_dbg (ohci, "device removed!\n");
 		usb_hc_died(hcd);
 		return IRQ_HANDLED;
@@ -771,7 +771,7 @@ static irqreturn_t ohci_irq (struct usb_hcd *hcd)
 	ints &= ohci_readl(ohci, &regs->intrenable);
 
 	/* interrupt for some other device? */
-	if (ints == 0 || unlikely(hcd->state == HC_STATE_HALT))
+	if (ints == 0 || unlikely(ohci->rh_state == OHCI_RH_HALTED))
 		return IRQ_NOTMINE;
 
 	if (ints & OHCI_INTR_UE) {
@@ -786,8 +786,8 @@ static irqreturn_t ohci_irq (struct usb_hcd *hcd)
 
 			schedule_work (&ohci->nec_work);
 		} else {
-			disable (ohci);
 			ohci_err (ohci, "OHCI Unrecoverable Error, disabled\n");
+			ohci->rh_state = OHCI_RH_HALTED;
 			usb_hc_died(hcd);
 		}
 
@@ -871,11 +871,11 @@ static irqreturn_t ohci_irq (struct usb_hcd *hcd)
 	if ((ints & OHCI_INTR_SF) != 0
 			&& !ohci->ed_rm_list
 			&& !ohci->ed_to_check
-			&& HC_IS_RUNNING(hcd->state))
+			&& ohci->rh_state == OHCI_RH_RUNNING)
 		ohci_writel (ohci, OHCI_INTR_SF, &regs->intrdisable);
 	spin_unlock (&ohci->lock);
 
-	if (HC_IS_RUNNING(hcd->state)) {
+	if (ohci->rh_state == OHCI_RH_RUNNING) {
 		ohci_writel (ohci, ints, &regs->intrstatus);
 		ohci_writel (ohci, OHCI_INTR_MIE, &regs->intrenable);
 		// flush those writes
@@ -929,7 +929,7 @@ static int ohci_restart (struct ohci_hcd *ohci)
 	struct urb_priv *priv;
 
 	spin_lock_irq(&ohci->lock);
-	disable (ohci);
+	ohci->rh_state = OHCI_RH_HALTED;
 
 	/* Recycle any "live" eds/tds (and urbs). */
 	if (!list_empty (&ohci->pending))
@@ -1005,6 +1005,11 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER		ohci_hcd_s3c2410_driver
 #endif
 
+#ifdef CONFIG_USB_OHCI_EXYNOS
+#include "ohci-exynos.c"
+#define PLATFORM_DRIVER		exynos_ohci_driver
+#endif
+
 #ifdef CONFIG_USB_OHCI_HCD_OMAP1
 #include "ohci-omap.c"
 #define OMAP1_PLATFORM_DRIVER	ohci_hcd_omap_driver
@@ -1111,7 +1116,7 @@ MODULE_LICENSE ("GPL");
 #define PLATFORM_DRIVER		ohci_hcd_ath79_driver
 #endif
 
-#ifdef CONFIG_NLM_XLR
+#ifdef CONFIG_CPU_XLR
 #include "ohci-xls.c"
 #define PLATFORM_DRIVER		ohci_xls_driver
 #endif
diff --git a/drivers/usb/host/ohci-hub.c b/drivers/usb/host/ohci-hub.c
index 2f00040fc408..836772dfabd3 100644
--- a/drivers/usb/host/ohci-hub.c
+++ b/drivers/usb/host/ohci-hub.c
@@ -111,6 +111,7 @@ __acquires(ohci->lock)
 	if (!autostop) {
 		ohci->next_statechange = jiffies + msecs_to_jiffies (5);
 		ohci->autostop = 0;
+		ohci->rh_state = OHCI_RH_SUSPENDED;
 	}
 
 done:
@@ -140,7 +141,7 @@ __acquires(ohci->lock)
 
 	if (ohci->hc_control & (OHCI_CTRL_IR | OHCI_SCHED_ENABLES)) {
 		/* this can happen after resuming a swsusp snapshot */
-		if (hcd->state == HC_STATE_RESUMING) {
+		if (ohci->rh_state != OHCI_RH_RUNNING) {
 			ohci_dbg (ohci, "BIOS/SMM active, control %03x\n",
 					ohci->hc_control);
 			status = -EBUSY;
@@ -274,6 +275,7 @@ skip_resume:
 		(void) ohci_readl (ohci, &ohci->regs->control);
 	}
 
+	ohci->rh_state = OHCI_RH_RUNNING;
 	return 0;
 }
 
@@ -336,11 +338,8 @@ static void ohci_finish_controller_resume(struct usb_hcd *hcd)
 	/* If needed, reinitialize and suspend the root hub */
 	if (need_reinit) {
 		spin_lock_irq(&ohci->lock);
-		hcd->state = HC_STATE_RESUMING;
 		ohci_rh_resume(ohci);
-		hcd->state = HC_STATE_QUIESCING;
 		ohci_rh_suspend(ohci, 0);
-		hcd->state = HC_STATE_SUSPENDED;
 		spin_unlock_irq(&ohci->lock);
 	}
 
diff --git a/drivers/usb/host/ohci-omap.c b/drivers/usb/host/ohci-omap.c
index e4b8782cc6e2..db3968656d21 100644
--- a/drivers/usb/host/ohci-omap.c
+++ b/drivers/usb/host/ohci-omap.c
@@ -516,7 +516,6 @@ static int ohci_omap_suspend(struct platform_device *dev, pm_message_t message)
 	ohci->next_statechange = jiffies;
 
 	omap_ohci_clock_power(0);
-	ohci_to_hcd(ohci)->state = HC_STATE_SUSPENDED;
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-omap3.c b/drivers/usb/host/ohci-omap3.c
index 516ebc4d6cc2..1b8133b6e451 100644
--- a/drivers/usb/host/ohci-omap3.c
+++ b/drivers/usb/host/ohci-omap3.c
@@ -31,6 +31,7 @@
 
 #include <linux/platform_device.h>
 #include <plat/usb.h>
+#include <linux/pm_runtime.h>
 
 /*-------------------------------------------------------------------------*/
 
@@ -134,7 +135,7 @@ static int __devinit ohci_hcd_omap3_probe(struct platform_device *pdev)
 	int			irq;
 
 	if (usb_disabled())
-		goto err_end;
+		return -ENODEV;
 
 	if (!dev->parent) {
 		dev_err(dev, "Missing parent device\n");
@@ -172,11 +173,8 @@ static int __devinit ohci_hcd_omap3_probe(struct platform_device *pdev)
 	hcd->rsrc_len = resource_size(res);
 	hcd->regs =  regs;
 
-	ret = omap_usbhs_enable(dev);
-	if (ret) {
-		dev_dbg(dev, "failed to start ohci\n");
-		goto err_end;
-	}
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
 
 	ohci_hcd_init(hcd_to_ohci(hcd));
 
@@ -189,9 +187,7 @@ static int __devinit ohci_hcd_omap3_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_hcd:
-	omap_usbhs_disable(dev);
-
-err_end:
+	pm_runtime_put_sync(dev);
 	usb_put_hcd(hcd);
 
 err_io:
@@ -220,9 +216,9 @@ static int __devexit ohci_hcd_omap3_remove(struct platform_device *pdev)
 
 	iounmap(hcd->regs);
 	usb_remove_hcd(hcd);
-	omap_usbhs_disable(dev);
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
 	usb_put_hcd(hcd);
-
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-pci.c b/drivers/usb/host/ohci-pci.c
index bc01b064585a..6109810cc2d3 100644
--- a/drivers/usb/host/ohci-pci.c
+++ b/drivers/usb/host/ohci-pci.c
@@ -308,12 +308,9 @@ static int ohci_pci_suspend(struct usb_hcd *hcd, bool do_wakeup)
 	 * mark HW unaccessible, bail out if RH has been resumed. Use
 	 * the spinlock to properly synchronize with possible pending
 	 * RH suspend or resume activity.
-	 *
-	 * This is still racy as hcd->state is manipulated outside of
-	 * any locks =P But that will be a different fix.
 	 */
 	spin_lock_irqsave (&ohci->lock, flags);
-	if (hcd->state != HC_STATE_SUSPENDED) {
+	if (ohci->rh_state != OHCI_RH_SUSPENDED) {
 		rc = -EINVAL;
 		goto bail;
 	}
diff --git a/drivers/usb/host/ohci-pxa27x.c b/drivers/usb/host/ohci-pxa27x.c
index 29dfefe1c726..6313e4439f37 100644
--- a/drivers/usb/host/ohci-pxa27x.c
+++ b/drivers/usb/host/ohci-pxa27x.c
@@ -502,8 +502,6 @@ static int ohci_hcd_pxa27x_drv_suspend(struct device *dev)
 	ohci->ohci.next_statechange = jiffies;
 
 	pxa27x_stop_hc(ohci, dev);
-	hcd->state = HC_STATE_SUSPENDED;
-
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-q.c b/drivers/usb/host/ohci-q.c
index 15dc51ded61a..c5a1ea9145fa 100644
--- a/drivers/usb/host/ohci-q.c
+++ b/drivers/usb/host/ohci-q.c
@@ -912,7 +912,7 @@ rescan_all:
 		/* only take off EDs that the HC isn't using, accounting for
 		 * frame counter wraps and EDs with partially retired TDs
 		 */
-		if (likely (HC_IS_RUNNING(ohci_to_hcd(ohci)->state))) {
+		if (likely(ohci->rh_state == OHCI_RH_RUNNING)) {
 			if (tick_before (tick, ed->tick)) {
 skip_ed:
 				last = &ed->ed_next;
@@ -1012,7 +1012,7 @@ rescan_this:
 
 		/* but if there's work queued, reschedule */
 		if (!list_empty (&ed->td_list)) {
-			if (HC_IS_RUNNING(ohci_to_hcd(ohci)->state))
+			if (ohci->rh_state == OHCI_RH_RUNNING)
 				ed_schedule (ohci, ed);
 		}
 
@@ -1021,9 +1021,7 @@ rescan_this:
 	}
 
 	/* maybe reenable control and bulk lists */
-	if (HC_IS_RUNNING(ohci_to_hcd(ohci)->state)
-			&& ohci_to_hcd(ohci)->state != HC_STATE_QUIESCING
-			&& !ohci->ed_rm_list) {
+	if (ohci->rh_state == OHCI_RH_RUNNING && !ohci->ed_rm_list) {
 		u32	command = 0, control = 0;
 
 		if (ohci->ed_controltail) {
diff --git a/drivers/usb/host/ohci-s3c2410.c b/drivers/usb/host/ohci-s3c2410.c
index a1877c47601e..56dcf069246d 100644
--- a/drivers/usb/host/ohci-s3c2410.c
+++ b/drivers/usb/host/ohci-s3c2410.c
@@ -486,15 +486,66 @@ static int __devexit ohci_hcd_s3c2410_drv_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int ohci_hcd_s3c2410_drv_suspend(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
+	struct platform_device *pdev = to_platform_device(dev);
+	unsigned long flags;
+	int rc = 0;
+
+	/*
+	 * Root hub was already suspended. Disable irq emission and
+	 * mark HW unaccessible, bail out if RH has been resumed. Use
+	 * the spinlock to properly synchronize with possible pending
+	 * RH suspend or resume activity.
+	 */
+	spin_lock_irqsave(&ohci->lock, flags);
+	if (ohci->rh_state != OHCI_RH_SUSPENDED) {
+		rc = -EINVAL;
+		goto bail;
+	}
+
+	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+
+	s3c2410_stop_hc(pdev);
+bail:
+	spin_unlock_irqrestore(&ohci->lock, flags);
+
+	return rc;
+}
+
+static int ohci_hcd_s3c2410_drv_resume(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct platform_device *pdev = to_platform_device(dev);
+
+	s3c2410_start_hc(pdev, hcd);
+
+	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+	ohci_finish_controller_resume(hcd);
+
+	return 0;
+}
+#else
+#define ohci_hcd_s3c2410_drv_suspend	NULL
+#define ohci_hcd_s3c2410_drv_resume	NULL
+#endif
+
+static const struct dev_pm_ops ohci_hcd_s3c2410_pm_ops = {
+	.suspend	= ohci_hcd_s3c2410_drv_suspend,
+	.resume		= ohci_hcd_s3c2410_drv_resume,
+};
+
 static struct platform_driver ohci_hcd_s3c2410_driver = {
 	.probe		= ohci_hcd_s3c2410_drv_probe,
 	.remove		= __devexit_p(ohci_hcd_s3c2410_drv_remove),
 	.shutdown	= usb_hcd_platform_shutdown,
-	/*.suspend	= ohci_hcd_s3c2410_drv_suspend, */
-	/*.resume	= ohci_hcd_s3c2410_drv_resume, */
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "s3c2410-ohci",
+		.pm	= &ohci_hcd_s3c2410_pm_ops,
 	},
 };
 
diff --git a/drivers/usb/host/ohci-sh.c b/drivers/usb/host/ohci-sh.c
index afc4eb6bb9d0..84686d90805b 100644
--- a/drivers/usb/host/ohci-sh.c
+++ b/drivers/usb/host/ohci-sh.c
@@ -29,7 +29,6 @@ static int ohci_sh_start(struct usb_hcd *hcd)
 	ohci_hcd_init(ohci);
 	ohci_init(ohci);
 	ohci_run(ohci);
-	hcd->state = HC_STATE_RUNNING;
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-sm501.c b/drivers/usb/host/ohci-sm501.c
index 968cea2b6d4e..5596ac2ba1ca 100644
--- a/drivers/usb/host/ohci-sm501.c
+++ b/drivers/usb/host/ohci-sm501.c
@@ -224,7 +224,6 @@ static int ohci_sm501_suspend(struct platform_device *pdev, pm_message_t msg)
 	ohci->next_statechange = jiffies;
 
 	sm501_unit_power(dev->parent, SM501_GATE_USB_HOST, 0);
-	ohci_to_hcd(ohci)->state = HC_STATE_SUSPENDED;
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-spear.c b/drivers/usb/host/ohci-spear.c
index 69874654f3b5..95c16489e883 100644
--- a/drivers/usb/host/ohci-spear.c
+++ b/drivers/usb/host/ohci-spear.c
@@ -203,7 +203,6 @@ static int spear_ohci_hcd_drv_suspend(struct platform_device *dev,
 	ohci->next_statechange = jiffies;
 
 	spear_stop_ohci(ohci_p);
-	ohci_to_hcd(ohci)->state = HC_STATE_SUSPENDED;
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-tmio.c b/drivers/usb/host/ohci-tmio.c
index 06331d931171..120bfe6ede38 100644
--- a/drivers/usb/host/ohci-tmio.c
+++ b/drivers/usb/host/ohci-tmio.c
@@ -318,9 +318,6 @@ static int ohci_hcd_tmio_drv_suspend(struct platform_device *dev, pm_message_t s
 		if (ret)
 			return ret;
 	}
-
-	hcd->state = HC_STATE_SUSPENDED;
-
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-xls.c b/drivers/usb/host/ohci-xls.c
index a3a9c6f45b91..a2247867af86 100644
--- a/drivers/usb/host/ohci-xls.c
+++ b/drivers/usb/host/ohci-xls.c
@@ -40,7 +40,7 @@ static int ohci_xls_probe_internal(const struct hc_driver *driver,
 		goto err1;
 	}
 	hcd->rsrc_start = res->start;
-	hcd->rsrc_len = res->end - res->start + 1;
+	hcd->rsrc_len = resource_size(res);
 
 	if (!request_mem_region(hcd->rsrc_start, hcd->rsrc_len,
 			driver->description)) {
diff --git a/drivers/usb/host/ohci.h b/drivers/usb/host/ohci.h
index 0795b934d00c..8ff6f7ea96fd 100644
--- a/drivers/usb/host/ohci.h
+++ b/drivers/usb/host/ohci.h
@@ -344,6 +344,12 @@ typedef struct urb_priv {
  * a subset of what the full implementation needs. (Linus)
  */
 
+enum ohci_rh_state {
+	OHCI_RH_HALTED,
+	OHCI_RH_SUSPENDED,
+	OHCI_RH_RUNNING
+};
+
 struct ohci_hcd {
 	spinlock_t		lock;
 
@@ -384,6 +390,7 @@ struct ohci_hcd {
 	/*
 	 * driver state
 	 */
+	enum ohci_rh_state	rh_state;
 	int			num_ports;
 	int			load [NUM_INTS];
 	u32			hc_control;	/* copy of hc control reg */
@@ -679,11 +686,6 @@ static inline u16 ohci_hwPSW(const struct ohci_hcd *ohci,
 
 /*-------------------------------------------------------------------------*/
 
-static inline void disable (struct ohci_hcd *ohci)
-{
-	ohci_to_hcd(ohci)->state = HC_STATE_HALT;
-}
-
 #define	FI			0x2edf		/* 12000 bits per frame (-1) */
 #define	FSMP(fi)		(0x7fff & ((6 * ((fi) - 210)) / 7))
 #define	FIT			(1 << 31)
@@ -707,7 +709,7 @@ static inline void periodic_reinit (struct ohci_hcd *ohci)
 #define read_roothub(hc, register, mask) ({ \
 	u32 temp = ohci_readl (hc, &hc->regs->roothub.register); \
 	if (temp == -1) \
-		disable (hc); \
+		hc->rh_state = OHCI_RH_HALTED; \
 	else if (hc->flags & OHCI_QUIRK_AMD756) \
 		while (temp & mask) \
 			temp = ohci_readl (hc, &hc->regs->roothub.register); \
diff --git a/drivers/usb/host/oxu210hp-hcd.c b/drivers/usb/host/oxu210hp-hcd.c
index dcd889803f0f..6f62de5c6e35 100644
--- a/drivers/usb/host/oxu210hp-hcd.c
+++ b/drivers/usb/host/oxu210hp-hcd.c
@@ -3951,24 +3951,7 @@ static struct platform_driver oxu_driver = {
 	}
 };
 
-static int __init oxu_module_init(void)
-{
-	int retval = 0;
-
-	retval = platform_driver_register(&oxu_driver);
-	if (retval < 0)
-		return retval;
-
-	return retval;
-}
-
-static void __exit oxu_module_cleanup(void)
-{
-	platform_driver_unregister(&oxu_driver);
-}
-
-module_init(oxu_module_init);
-module_exit(oxu_module_cleanup);
+module_platform_driver(oxu_driver);
 
 MODULE_DESCRIPTION("Oxford OXU210HP HCD driver - ver. " DRIVER_VERSION);
 MODULE_AUTHOR("Rodolfo Giometti <giometti@linux.it>");
diff --git a/drivers/usb/host/uhci-q.c b/drivers/usb/host/uhci-q.c
index f6ca80ee4cec..d2c6f5ac4626 100644
--- a/drivers/usb/host/uhci-q.c
+++ b/drivers/usb/host/uhci-q.c
@@ -943,7 +943,7 @@ static int uhci_submit_common(struct uhci_hcd *uhci, struct urb *urb,
 	if (usb_pipein(urb->pipe))
 		status |= TD_CTRL_SPD;
 
-	i = urb->num_sgs;
+	i = urb->num_mapped_sgs;
 	if (len > 0 && i > 0) {
 		sg = urb->sg;
 		data = sg_dma_address(sg);
diff --git a/drivers/usb/host/whci/qset.c b/drivers/usb/host/whci/qset.c
index a403b53e86b9..76083ae92138 100644
--- a/drivers/usb/host/whci/qset.c
+++ b/drivers/usb/host/whci/qset.c
@@ -443,7 +443,7 @@ static int qset_add_urb_sg(struct whc *whc, struct whc_qset *qset, struct urb *u
 
 	remaining = urb->transfer_buffer_length;
 
-	for_each_sg(urb->sg, sg, urb->num_sgs, i) {
+	for_each_sg(urb->sg, sg, urb->num_mapped_sgs, i) {
 		dma_addr_t dma_addr;
 		size_t dma_remaining;
 		dma_addr_t sp, ep;
@@ -561,7 +561,7 @@ static int qset_add_urb_sg_linearize(struct whc *whc, struct whc_qset *qset,
 
 	remaining = urb->transfer_buffer_length;
 
-	for_each_sg(urb->sg, sg, urb->num_sgs, i) {
+	for_each_sg(urb->sg, sg, urb->num_mapped_sgs, i) {
 		size_t len;
 		size_t sg_remaining;
 		void *orig;
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 430e88fd3f6c..35e257f79c7b 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -57,17 +57,15 @@ static void xhci_common_hub_descriptor(struct xhci_hcd *xhci,
 	desc->bHubContrCurrent = 0;
 
 	desc->bNbrPorts = ports;
-	/* Ugh, these should be #defines, FIXME */
-	/* Using table 11-13 in USB 2.0 spec. */
 	temp = 0;
-	/* Bits 1:0 - support port power switching, or power always on */
+	/* Bits 1:0 - support per-port power switching, or power always on */
 	if (HCC_PPC(xhci->hcc_params))
-		temp |= 0x0001;
+		temp |= HUB_CHAR_INDV_PORT_LPSM;
 	else
-		temp |= 0x0002;
+		temp |= HUB_CHAR_NO_LPSM;
 	/* Bit  2 - root hubs are not part of a compound device */
 	/* Bits 4:3 - individual port over current protection */
-	temp |= 0x0008;
+	temp |= HUB_CHAR_INDV_PORT_OCPM;
 	/* Bits 6:5 - no TTs in root ports */
 	/* Bit  7 - no port indicators */
 	desc->wHubCharacteristics = cpu_to_le16(temp);
@@ -86,9 +84,9 @@ static void xhci_usb2_hub_descriptor(struct usb_hcd *hcd, struct xhci_hcd *xhci,
 	ports = xhci->num_usb2_ports;
 
 	xhci_common_hub_descriptor(xhci, desc, ports);
-	desc->bDescriptorType = 0x29;
+	desc->bDescriptorType = USB_DT_HUB;
 	temp = 1 + (ports / 8);
-	desc->bDescLength = 7 + 2 * temp;
+	desc->bDescLength = USB_DT_HUB_NONVAR_SIZE + 2 * temp;
 
 	/* The Device Removable bits are reported on a byte granularity.
 	 * If the port doesn't exist within that byte, the bit is set to 0.
@@ -137,8 +135,8 @@ static void xhci_usb3_hub_descriptor(struct usb_hcd *hcd, struct xhci_hcd *xhci,
 
 	ports = xhci->num_usb3_ports;
 	xhci_common_hub_descriptor(xhci, desc, ports);
-	desc->bDescriptorType = 0x2a;
-	desc->bDescLength = 12;
+	desc->bDescriptorType = USB_DT_SS_HUB;
+	desc->bDescLength = USB_DT_SS_HUB_SIZE;
 
 	/* header decode latency should be zero for roothubs,
 	 * see section 4.23.5.2.
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 0e4b25fa3bcd..36cbe2226a44 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -42,15 +42,12 @@ static struct xhci_segment *xhci_segment_alloc(struct xhci_hcd *xhci, gfp_t flag
 	seg = kzalloc(sizeof *seg, flags);
 	if (!seg)
 		return NULL;
-	xhci_dbg(xhci, "Allocating priv segment structure at %p\n", seg);
 
 	seg->trbs = dma_pool_alloc(xhci->segment_pool, flags, &dma);
 	if (!seg->trbs) {
 		kfree(seg);
 		return NULL;
 	}
-	xhci_dbg(xhci, "// Allocating segment at %p (virtual) 0x%llx (DMA)\n",
-			seg->trbs, (unsigned long long)dma);
 
 	memset(seg->trbs, 0, SEGMENT_SIZE);
 	seg->dma = dma;
@@ -62,12 +59,9 @@ static struct xhci_segment *xhci_segment_alloc(struct xhci_hcd *xhci, gfp_t flag
 static void xhci_segment_free(struct xhci_hcd *xhci, struct xhci_segment *seg)
 {
 	if (seg->trbs) {
-		xhci_dbg(xhci, "Freeing DMA segment at %p (virtual) 0x%llx (DMA)\n",
-				seg->trbs, (unsigned long long)seg->dma);
 		dma_pool_free(xhci->segment_pool, seg->trbs, seg->dma);
 		seg->trbs = NULL;
 	}
-	xhci_dbg(xhci, "Freeing priv segment structure at %p\n", seg);
 	kfree(seg);
 }
 
@@ -101,9 +95,6 @@ static void xhci_link_segments(struct xhci_hcd *xhci, struct xhci_segment *prev,
 			val |= TRB_CHAIN;
 		prev->trbs[TRBS_PER_SEGMENT-1].link.control = cpu_to_le32(val);
 	}
-	xhci_dbg(xhci, "Linking segment 0x%llx to segment 0x%llx (DMA)\n",
-			(unsigned long long)prev->dma,
-			(unsigned long long)next->dma);
 }
 
 /* XXX: Do we need the hcd structure in all these functions? */
@@ -117,7 +108,6 @@ void xhci_ring_free(struct xhci_hcd *xhci, struct xhci_ring *ring)
 	if (ring->first_seg) {
 		first_seg = ring->first_seg;
 		seg = first_seg->next;
-		xhci_dbg(xhci, "Freeing ring at %p\n", ring);
 		while (seg != first_seg) {
 			struct xhci_segment *next = seg->next;
 			xhci_segment_free(xhci, seg);
@@ -160,7 +150,6 @@ static struct xhci_ring *xhci_ring_alloc(struct xhci_hcd *xhci,
 	struct xhci_segment	*prev;
 
 	ring = kzalloc(sizeof *(ring), flags);
-	xhci_dbg(xhci, "Allocating ring at %p\n", ring);
 	if (!ring)
 		return NULL;
 
@@ -191,9 +180,6 @@ static struct xhci_ring *xhci_ring_alloc(struct xhci_hcd *xhci,
 		/* See section 4.9.2.1 and 6.4.4.1 */
 		prev->trbs[TRBS_PER_SEGMENT-1].link.control |=
 			cpu_to_le32(LINK_TOGGLE);
-		xhci_dbg(xhci, "Wrote link toggle flag to"
-				" segment %p (virtual), 0x%llx (DMA)\n",
-				prev, (unsigned long long)prev->dma);
 	}
 	xhci_initialize_ring_info(ring);
 	return ring;
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 9f1d4b15d818..b90e1386418b 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -155,10 +155,6 @@ static void inc_deq(struct xhci_hcd *xhci, struct xhci_ring *ring, bool consumer
 	while (last_trb(xhci, ring, ring->deq_seg, next)) {
 		if (consumer && last_trb_on_last_seg(xhci, ring, ring->deq_seg, next)) {
 			ring->cycle_state = (ring->cycle_state ? 0 : 1);
-			if (!in_interrupt())
-				xhci_dbg(xhci, "Toggle cycle state for ring %p = %i\n",
-						ring,
-						(unsigned int) ring->cycle_state);
 		}
 		ring->deq_seg = ring->deq_seg->next;
 		ring->dequeue = ring->deq_seg->trbs;
@@ -231,10 +227,6 @@ static void inc_enq(struct xhci_hcd *xhci, struct xhci_ring *ring,
 			/* Toggle the cycle bit after the last ring segment. */
 			if (last_trb_on_last_seg(xhci, ring, ring->enq_seg, next)) {
 				ring->cycle_state = (ring->cycle_state ? 0 : 1);
-				if (!in_interrupt())
-					xhci_dbg(xhci, "Toggle cycle state for ring %p = %i\n",
-							ring,
-							(unsigned int) ring->cycle_state);
 			}
 		}
 		ring->enq_seg = ring->enq_seg->next;
@@ -560,12 +552,9 @@ static void td_to_noop(struct xhci_hcd *xhci, struct xhci_ring *ep_ring,
 					cpu_to_le32(TRB_CYCLE);
 			cur_trb->generic.field[3] |= cpu_to_le32(
 				TRB_TYPE(TRB_TR_NOOP));
-			xhci_dbg(xhci, "Cancel TRB %p (0x%llx dma) "
-					"in seg %p (0x%llx dma)\n",
-					cur_trb,
-					(unsigned long long)xhci_trb_virt_to_dma(cur_seg, cur_trb),
-					cur_seg,
-					(unsigned long long)cur_seg->dma);
+			xhci_dbg(xhci, "TRB to noop at offset 0x%llx\n",
+					(unsigned long long)
+					xhci_trb_virt_to_dma(cur_seg, cur_trb));
 		}
 		if (cur_trb == cur_td->last_trb)
 			break;
@@ -705,9 +694,9 @@ static void handle_stopped_endpoint(struct xhci_hcd *xhci,
 	 */
 	list_for_each(entry, &ep->cancelled_td_list) {
 		cur_td = list_entry(entry, struct xhci_td, cancelled_td_list);
-		xhci_dbg(xhci, "Cancelling TD starting at %p, 0x%llx (dma).\n",
-				cur_td->first_trb,
-				(unsigned long long)xhci_trb_virt_to_dma(cur_td->start_seg, cur_td->first_trb));
+		xhci_dbg(xhci, "Removing canceled TD starting at 0x%llx (dma).\n",
+				(unsigned long long)xhci_trb_virt_to_dma(
+					cur_td->start_seg, cur_td->first_trb));
 		ep_ring = xhci_urb_to_transfer_ring(xhci, cur_td->urb);
 		if (!ep_ring) {
 			/* This shouldn't happen unless a driver is mucking
@@ -1627,7 +1616,6 @@ static int process_ctrl_td(struct xhci_hcd *xhci, struct xhci_td *td,
 	ep_ctx = xhci_get_ep_ctx(xhci, xdev->out_ctx, ep_index);
 	trb_comp_code = GET_COMP_CODE(le32_to_cpu(event->transfer_len));
 
-	xhci_debug_trb(xhci, xhci->event_ring->dequeue);
 	switch (trb_comp_code) {
 	case COMP_SUCCESS:
 		if (event_trb == ep_ring->dequeue) {
@@ -1643,7 +1631,6 @@ static int process_ctrl_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		}
 		break;
 	case COMP_SHORT_TX:
-		xhci_warn(xhci, "WARN: short transfer on control ep\n");
 		if (td->urb->transfer_flags & URB_SHORT_NOT_OK)
 			*status = -EREMOTEIO;
 		else
@@ -1946,6 +1933,16 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 	xdev = xhci->devs[slot_id];
 	if (!xdev) {
 		xhci_err(xhci, "ERROR Transfer event pointed to bad slot\n");
+		xhci_err(xhci, "@%016llx %08x %08x %08x %08x\n",
+			 (unsigned long long) xhci_trb_virt_to_dma(
+				 xhci->event_ring->deq_seg,
+				 xhci->event_ring->dequeue),
+			 lower_32_bits(le64_to_cpu(event->buffer)),
+			 upper_32_bits(le64_to_cpu(event->buffer)),
+			 le32_to_cpu(event->transfer_len),
+			 le32_to_cpu(event->flags));
+		xhci_dbg(xhci, "Event ring:\n");
+		xhci_debug_segment(xhci, xhci->event_ring->deq_seg);
 		return -ENODEV;
 	}
 
@@ -1959,6 +1956,16 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 	    EP_STATE_DISABLED) {
 		xhci_err(xhci, "ERROR Transfer event for disabled endpoint "
 				"or incorrect stream ring\n");
+		xhci_err(xhci, "@%016llx %08x %08x %08x %08x\n",
+			 (unsigned long long) xhci_trb_virt_to_dma(
+				 xhci->event_ring->deq_seg,
+				 xhci->event_ring->dequeue),
+			 lower_32_bits(le64_to_cpu(event->buffer)),
+			 upper_32_bits(le64_to_cpu(event->buffer)),
+			 le32_to_cpu(event->transfer_len),
+			 le32_to_cpu(event->flags));
+		xhci_dbg(xhci, "Event ring:\n");
+		xhci_debug_segment(xhci, xhci->event_ring->deq_seg);
 		return -ENODEV;
 	}
 
@@ -1985,7 +1992,7 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 		xhci_dbg(xhci, "Stopped on No-op or Link TRB\n");
 		break;
 	case COMP_STALL:
-		xhci_warn(xhci, "WARN: Stalled endpoint\n");
+		xhci_dbg(xhci, "Stalled endpoint\n");
 		ep->ep_state |= EP_HALTED;
 		status = -EPIPE;
 		break;
@@ -1995,11 +2002,11 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 		break;
 	case COMP_SPLIT_ERR:
 	case COMP_TX_ERR:
-		xhci_warn(xhci, "WARN: transfer error on endpoint\n");
+		xhci_dbg(xhci, "Transfer error on endpoint\n");
 		status = -EPROTO;
 		break;
 	case COMP_BABBLE:
-		xhci_warn(xhci, "WARN: babble error on endpoint\n");
+		xhci_dbg(xhci, "Babble error on endpoint\n");
 		status = -EOVERFLOW;
 		break;
 	case COMP_DB_ERR:
@@ -2390,17 +2397,7 @@ hw_died:
 
 irqreturn_t xhci_msi_irq(int irq, struct usb_hcd *hcd)
 {
-	irqreturn_t ret;
-	struct xhci_hcd *xhci;
-
-	xhci = hcd_to_xhci(hcd);
-	set_bit(HCD_FLAG_SAW_IRQ, &hcd->flags);
-	if (xhci->shared_hcd)
-		set_bit(HCD_FLAG_SAW_IRQ, &xhci->shared_hcd->flags);
-
-	ret = xhci_irq(hcd);
-
-	return ret;
+	return xhci_irq(hcd);
 }
 
 /****		Endpoint Ring Operations	****/
@@ -2488,11 +2485,6 @@ static int prepare_ring(struct xhci_hcd *xhci, struct xhci_ring *ep_ring,
 			/* Toggle the cycle bit after the last ring segment. */
 			if (last_trb_on_last_seg(xhci, ring, ring->enq_seg, next)) {
 				ring->cycle_state = (ring->cycle_state ? 0 : 1);
-				if (!in_interrupt()) {
-					xhci_dbg(xhci, "queue_trb: Toggle cycle "
-						"state for ring %p = %i\n",
-						ring, (unsigned int)ring->cycle_state);
-				}
 			}
 			ring->enq_seg = ring->enq_seg->next;
 			ring->enqueue = ring->enq_seg->trbs;
@@ -2561,13 +2553,11 @@ static unsigned int count_sg_trbs_needed(struct xhci_hcd *xhci, struct urb *urb)
 	struct scatterlist *sg;
 
 	sg = NULL;
-	num_sgs = urb->num_sgs;
+	num_sgs = urb->num_mapped_sgs;
 	temp = urb->transfer_buffer_length;
 
-	xhci_dbg(xhci, "count sg list trbs: \n");
 	num_trbs = 0;
 	for_each_sg(urb->sg, sg, num_sgs, i) {
-		unsigned int previous_total_trbs = num_trbs;
 		unsigned int len = sg_dma_len(sg);
 
 		/* Scatter gather list entries may cross 64KB boundaries */
@@ -2582,22 +2572,11 @@ static unsigned int count_sg_trbs_needed(struct xhci_hcd *xhci, struct urb *urb)
 			num_trbs++;
 			running_total += TRB_MAX_BUFF_SIZE;
 		}
-		xhci_dbg(xhci, " sg #%d: dma = %#llx, len = %#x (%d), num_trbs = %d\n",
-				i, (unsigned long long)sg_dma_address(sg),
-				len, len, num_trbs - previous_total_trbs);
-
 		len = min_t(int, len, temp);
 		temp -= len;
 		if (temp == 0)
 			break;
 	}
-	xhci_dbg(xhci, "\n");
-	if (!in_interrupt())
-		xhci_dbg(xhci, "ep %#x - urb len = %d, sglist used, "
-				"num_trbs = %d\n",
-				urb->ep->desc.bEndpointAddress,
-				urb->transfer_buffer_length,
-				num_trbs);
 	return num_trbs;
 }
 
@@ -2745,7 +2724,7 @@ static int queue_bulk_sg_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		return -EINVAL;
 
 	num_trbs = count_sg_trbs_needed(xhci, urb);
-	num_sgs = urb->num_sgs;
+	num_sgs = urb->num_mapped_sgs;
 	total_packet_count = roundup(urb->transfer_buffer_length,
 			usb_endpoint_maxp(&urb->ep->desc));
 
@@ -2783,8 +2762,6 @@ static int queue_bulk_sg_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 	trb_buff_len = min_t(int, trb_buff_len, this_sg_len);
 	if (trb_buff_len > urb->transfer_buffer_length)
 		trb_buff_len = urb->transfer_buffer_length;
-	xhci_dbg(xhci, "First length to xfer from 1st sglist entry = %u\n",
-			trb_buff_len);
 
 	first_trb = true;
 	/* Queue the first TRB, even if it's zero-length */
@@ -2816,11 +2793,6 @@ static int queue_bulk_sg_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		if (usb_urb_dir_in(urb))
 			field |= TRB_ISP;
 
-		xhci_dbg(xhci, " sg entry: dma = %#x, len = %#x (%d), "
-				"64KB boundary at %#x, end dma = %#x\n",
-				(unsigned int) addr, trb_buff_len, trb_buff_len,
-				(unsigned int) (addr + TRB_MAX_BUFF_SIZE) & ~(TRB_MAX_BUFF_SIZE - 1),
-				(unsigned int) addr + trb_buff_len);
 		if (TRB_MAX_BUFF_SIZE -
 				(addr & (TRB_MAX_BUFF_SIZE - 1)) < trb_buff_len) {
 			xhci_warn(xhci, "WARN: sg dma xfer crosses 64KB boundaries!\n");
@@ -2926,15 +2898,6 @@ int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 	}
 	/* FIXME: this doesn't deal with URB_ZERO_PACKET - need one more */
 
-	if (!in_interrupt())
-		xhci_dbg(xhci, "ep %#x - urb len = %#x (%d), "
-				"addr = %#llx, num_trbs = %d\n",
-				urb->ep->desc.bEndpointAddress,
-				urb->transfer_buffer_length,
-				urb->transfer_buffer_length,
-				(unsigned long long)urb->transfer_dma,
-				num_trbs);
-
 	ret = prepare_transfer(xhci, xhci->devs[slot_id],
 			ep_index, urb->stream_id,
 			num_trbs, urb, 0, false, mem_flags);
@@ -3055,9 +3018,6 @@ int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 	if (!urb->setup_packet)
 		return -EINVAL;
 
-	if (!in_interrupt())
-		xhci_dbg(xhci, "Queueing ctrl tx for slot id %d, ep %d\n",
-				slot_id, ep_index);
 	/* 1 TRB for setup, 1 for status */
 	num_trbs = 2;
 	/*
@@ -3249,15 +3209,6 @@ static int xhci_queue_isoc_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		return -EINVAL;
 	}
 
-	if (!in_interrupt())
-		xhci_dbg(xhci, "ep %#x - urb len = %#x (%d),"
-				" addr = %#llx, num_tds = %d\n",
-				urb->ep->desc.bEndpointAddress,
-				urb->transfer_buffer_length,
-				urb->transfer_buffer_length,
-				(unsigned long long)urb->transfer_dma,
-				num_tds);
-
 	start_addr = (u64) urb->transfer_dma;
 	start_trb = &ep_ring->enqueue->generic;
 	start_cycle = ep_ring->cycle_state;
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index a1afb7c39f7e..6bbe3c3a7111 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -200,14 +200,14 @@ static int xhci_setup_msi(struct xhci_hcd *xhci)
 
 	ret = pci_enable_msi(pdev);
 	if (ret) {
-		xhci_err(xhci, "failed to allocate MSI entry\n");
+		xhci_dbg(xhci, "failed to allocate MSI entry\n");
 		return ret;
 	}
 
 	ret = request_irq(pdev->irq, (irq_handler_t)xhci_msi_irq,
 				0, "xhci_hcd", xhci_to_hcd(xhci));
 	if (ret) {
-		xhci_err(xhci, "disable MSI interrupt\n");
+		xhci_dbg(xhci, "disable MSI interrupt\n");
 		pci_disable_msi(pdev);
 	}
 
@@ -270,7 +270,7 @@ static int xhci_setup_msix(struct xhci_hcd *xhci)
 
 	ret = pci_enable_msix(pdev, xhci->msix_entries, xhci->msix_count);
 	if (ret) {
-		xhci_err(xhci, "Failed to enable MSI-X\n");
+		xhci_dbg(xhci, "Failed to enable MSI-X\n");
 		goto free_entries;
 	}
 
@@ -286,7 +286,7 @@ static int xhci_setup_msix(struct xhci_hcd *xhci)
 	return ret;
 
 disable_msix:
-	xhci_err(xhci, "disable MSI-X interrupt\n");
+	xhci_dbg(xhci, "disable MSI-X interrupt\n");
 	xhci_free_irq(xhci);
 	pci_disable_msix(pdev);
 free_entries:
@@ -1333,9 +1333,6 @@ int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 		goto done;
 	}
 
-	xhci_dbg(xhci, "Cancel URB %p\n", urb);
-	xhci_dbg(xhci, "Event ring:\n");
-	xhci_debug_ring(xhci, xhci->event_ring);
 	ep_index = xhci_get_endpoint_index(&urb->ep->desc);
 	ep = &xhci->devs[urb->dev->slot_id]->eps[ep_index];
 	ep_ring = xhci_urb_to_transfer_ring(xhci, urb);
@@ -1344,12 +1341,18 @@ int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 		goto done;
 	}
 
-	xhci_dbg(xhci, "Endpoint ring:\n");
-	xhci_debug_ring(xhci, ep_ring);
-
 	urb_priv = urb->hcpriv;
-
-	for (i = urb_priv->td_cnt; i < urb_priv->length; i++) {
+	i = urb_priv->td_cnt;
+	if (i < urb_priv->length)
+		xhci_dbg(xhci, "Cancel URB %p, dev %s, ep 0x%x, "
+				"starting at offset 0x%llx\n",
+				urb, urb->dev->devpath,
+				urb->ep->desc.bEndpointAddress,
+				(unsigned long long) xhci_trb_virt_to_dma(
+					urb_priv->td[i]->start_seg,
+					urb_priv->td[i]->first_trb));
+
+	for (; i < urb_priv->length; i++) {
 		td = urb_priv->td[i];
 		list_add_tail(&td->cancelled_td_list, &ep->cancelled_td_list);
 	}
@@ -1620,6 +1623,7 @@ static int xhci_configure_endpoint_result(struct xhci_hcd *xhci,
 		/* FIXME: can we allocate more resources for the HC? */
 		break;
 	case COMP_BW_ERR:
+	case COMP_2ND_BW_ERR:
 		dev_warn(&udev->dev, "Not enough bandwidth "
 				"for new device state.\n");
 		ret = -ENOSPC;
@@ -2796,8 +2800,7 @@ static int xhci_calculate_streams_and_bitmask(struct xhci_hcd *xhci,
 		if (ret < 0)
 			return ret;
 
-		max_streams = USB_SS_MAX_STREAMS(
-				eps[i]->ss_ep_comp.bmAttributes);
+		max_streams = usb_ss_max_streams(&eps[i]->ss_ep_comp);
 		if (max_streams < (*num_streams - 1)) {
 			xhci_dbg(xhci, "Ep 0x%x only supports %u stream IDs.\n",
 					eps[i]->desc.bEndpointAddress,
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 3c8fbd2772ea..fb99c8379142 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -1033,7 +1033,6 @@ struct xhci_transfer_event {
 /* Invalid Stream ID Error */
 #define COMP_STRID_ERR	34
 /* Secondary Bandwidth Error - may be returned by a Configure Endpoint cmd */
-/* FIXME - check for this */
 #define COMP_2ND_BW_ERR	35
 /* Split Transaction Error */
 #define	COMP_SPLIT_ERR	36
@@ -1356,7 +1355,7 @@ static inline unsigned int hcd_index(struct usb_hcd *hcd)
 		return 1;
 }
 
-/* There is one ehci_hci structure per controller */
+/* There is one xhci_hcd structure per controller */
 struct xhci_hcd {
 	struct usb_hcd *main_hcd;
 	struct usb_hcd *shared_hcd;
diff --git a/drivers/usb/misc/isight_firmware.c b/drivers/usb/misc/isight_firmware.c
index 1dc7e9581cc6..1c61830e96f9 100644
--- a/drivers/usb/misc/isight_firmware.c
+++ b/drivers/usb/misc/isight_firmware.c
@@ -55,8 +55,9 @@ static int isight_firmware_load(struct usb_interface *intf,
 
 	ptr = firmware->data;
 
+	buf[0] = 0x01;
 	if (usb_control_msg
-	    (dev, usb_sndctrlpipe(dev, 0), 0xa0, 0x40, 0xe600, 0, "\1", 1,
+	    (dev, usb_sndctrlpipe(dev, 0), 0xa0, 0x40, 0xe600, 0, buf, 1,
 	     300) != 1) {
 		printk(KERN_ERR
 		       "Failed to initialise isight firmware loader\n");
@@ -100,8 +101,9 @@ static int isight_firmware_load(struct usb_interface *intf,
 		}
 	}
 
+	buf[0] = 0x00;
 	if (usb_control_msg
-	    (dev, usb_sndctrlpipe(dev, 0), 0xa0, 0x40, 0xe600, 0, "\0", 1,
+	    (dev, usb_sndctrlpipe(dev, 0), 0xa0, 0x40, 0xe600, 0, buf, 1,
 	     300) != 1) {
 		printk(KERN_ERR "isight firmware loading completion failed\n");
 		ret = -ENODEV;
diff --git a/drivers/usb/misc/usbled.c b/drivers/usb/misc/usbled.c
index 4af56fbc3c06..12d03e7ad636 100644
--- a/drivers/usb/misc/usbled.c
+++ b/drivers/usb/misc/usbled.c
@@ -31,6 +31,8 @@ static const struct usb_device_id id_table[] = {
 			.driver_info = DELCOM_VISUAL_SIGNAL_INDICATOR },
 	{ USB_DEVICE(0x1d34, 0x0004),
 			.driver_info = DREAM_CHEEKY_WEBMAIL_NOTIFIER },
+	{ USB_DEVICE(0x1d34, 0x000a),
+			.driver_info = DREAM_CHEEKY_WEBMAIL_NOTIFIER },
 	{ },
 };
 MODULE_DEVICE_TABLE(usb, id_table);
diff --git a/drivers/usb/misc/usbtest.c b/drivers/usb/misc/usbtest.c
index bd6d00802eab..959145baf3cf 100644
--- a/drivers/usb/misc/usbtest.c
+++ b/drivers/usb/misc/usbtest.c
@@ -1765,7 +1765,6 @@ static int test_unaligned_bulk(
  * off just killing the userspace task and waiting for it to exit.
  */
 
-/* No BKL needed */
 static int
 usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
 {
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index 07a03460a598..f70cab3beeec 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -5,14 +5,13 @@
 
 # (M)HDRC = (Multipoint) Highspeed Dual-Role Controller
 config USB_MUSB_HDRC
+	tristate 'Inventra Highspeed Dual Role Controller (TI, ADI, ...)'
 	depends on USB && USB_GADGET
-	depends on (ARM || (BF54x && !BF544) || (BF52x && !BF522 && !BF523))
 	select NOP_USB_XCEIV if (ARCH_DAVINCI || MACH_OMAP3EVM || BLACKFIN)
 	select TWL4030_USB if MACH_OMAP_3430SDP
 	select TWL6030_USB if MACH_OMAP_4430SDP || MACH_OMAP4_PANDA
 	select USB_OTG_UTILS
 	select USB_GADGET_DUALSPEED
-	tristate 'Inventra Highspeed Dual Role Controller (TI, ADI, ...)'
 	help
 	  Say Y here if your system has a dual role high speed USB
 	  controller based on the Mentor Graphics silicon IP.  Then
@@ -31,9 +30,10 @@ config USB_MUSB_HDRC
 	  To compile this driver as a module, choose M here; the
 	  module will be called "musb-hdrc".
 
+if USB_MUSB_HDRC
+
 choice
 	prompt "Platform Glue Layer"
-	depends on USB_MUSB_HDRC
 
 config USB_MUSB_DAVINCI
 	tristate "DaVinci"
@@ -45,7 +45,6 @@ config USB_MUSB_DA8XX
 
 config USB_MUSB_TUSB6010
 	tristate "TUSB6010"
-	depends on ARCH_OMAP
 
 config USB_MUSB_OMAP2PLUS
 	tristate "OMAP2430 and onwards"
@@ -65,46 +64,54 @@ config USB_MUSB_UX500
 
 endchoice
 
-config MUSB_PIO_ONLY
-	bool 'Disable DMA (always use PIO)'
-	depends on USB_MUSB_HDRC
-	default USB_MUSB_TUSB6010 || USB_MUSB_DA8XX || USB_MUSB_AM35X
+choice
+	prompt 'MUSB DMA mode'
+	default USB_UX500_DMA if USB_MUSB_UX500
+	default USB_INVENTRA_DMA if USB_MUSB_OMAP2PLUS || USB_MUSB_BLACKFIN
+	default USB_TI_CPPI_DMA if USB_MUSB_DAVINCI
+	default USB_TUSB_OMAP_DMA if USB_MUSB_TUSB6010
+	default MUSB_PIO_ONLY if USB_MUSB_TUSB6010 || USB_MUSB_DA8XX || USB_MUSB_AM35X
 	help
-	  All data is copied between memory and FIFO by the CPU.
-	  DMA controllers are ignored.
-
-	  Do not select 'n' here unless DMA support for your SOC or board
-	  is unavailable (or unstable).  When DMA is enabled at compile time,
-	  you can still disable it at run time using the "use_dma=n" module
-	  parameter.
+	  Unfortunately, only one option can be enabled here. Ideally one
+	  should be able to build all these drivers into one kernel to
+	  allow using DMA on multiplatform kernels.
 
 config USB_UX500_DMA
-	bool
-	depends on USB_MUSB_HDRC && !MUSB_PIO_ONLY
-	default USB_MUSB_UX500
+	bool 'ST Ericsson U8500 and U5500'
+	depends on USB_MUSB_UX500
 	help
 	  Enable DMA transfers on UX500 platforms.
 
 config USB_INVENTRA_DMA
-	bool
-	depends on USB_MUSB_HDRC && !MUSB_PIO_ONLY
-	default USB_MUSB_OMAP2PLUS || USB_MUSB_BLACKFIN
+	bool 'Inventra'
+	depends on USB_MUSB_OMAP2PLUS || USB_MUSB_BLACKFIN
 	help
 	  Enable DMA transfers using Mentor's engine.
 
 config USB_TI_CPPI_DMA
-	bool
-	depends on USB_MUSB_HDRC && !MUSB_PIO_ONLY
-	default USB_MUSB_DAVINCI
+	bool 'TI CPPI (Davinci)'
+	depends on USB_MUSB_DAVINCI
 	help
 	  Enable DMA transfers when TI CPPI DMA is available.
 
 config USB_TUSB_OMAP_DMA
-	bool
-	depends on USB_MUSB_HDRC && !MUSB_PIO_ONLY
+	bool 'TUSB 6010'
 	depends on USB_MUSB_TUSB6010
 	depends on ARCH_OMAP
-	default y
 	help
 	  Enable DMA transfers on TUSB 6010 when OMAP DMA is available.
 
+config MUSB_PIO_ONLY
+	bool 'Disable DMA (always use PIO)'
+	help
+	  All data is copied between memory and FIFO by the CPU.
+	  DMA controllers are ignored.
+
+	  Do not choose this unless DMA support for your SOC or board
+	  is unavailable (or unstable).  When DMA is enabled at compile time,
+	  you can still disable it at run time using the "use_dma=n" module
+	  parameter.
+
+endchoice
+
+endif # USB_MUSB_HDRC
diff --git a/drivers/usb/musb/Makefile b/drivers/usb/musb/Makefile
index d8fd9d092dec..88bfb9dee4bf 100644
--- a/drivers/usb/musb/Makefile
+++ b/drivers/usb/musb/Makefile
@@ -24,25 +24,7 @@ obj-$(CONFIG_USB_MUSB_UX500)			+= ux500.o
 # PIO only, or DMA (several potential schemes).
 # though PIO is always there to back up DMA, and for ep0
 
-ifneq ($(CONFIG_MUSB_PIO_ONLY),y)
-
-  ifeq ($(CONFIG_USB_INVENTRA_DMA),y)
-    musb_hdrc-y			+= musbhsdma.o
-
-  else
-    ifeq ($(CONFIG_USB_TI_CPPI_DMA),y)
-      musb_hdrc-y		+= cppi_dma.o
-
-    else
-      ifeq ($(CONFIG_USB_TUSB_OMAP_DMA),y)
-	musb_hdrc-y		+= tusb6010_omap.o
-
-      else
-        ifeq ($(CONFIG_USB_UX500_DMA),y)
-	  musb_hdrc-y		+= ux500_dma.o
-
-        endif
-      endif
-    endif
-  endif
-endif
+musb_hdrc-$(CONFIG_USB_INVENTRA_DMA)		+= musbhsdma.o
+musb_hdrc-$(CONFIG_USB_TI_CPPI_DMA)		+= cppi_dma.o
+musb_hdrc-$(CONFIG_USB_TUSB_OMAP_DMA)		+= tusb6010_omap.o
+musb_hdrc-$(CONFIG_USB_UX500_DMA)		+= ux500_dma.o
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index b63ab1570103..f6ff7923048b 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -661,7 +661,6 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
 
 		handled = IRQ_HANDLED;
 		musb->is_active = 1;
-		set_bit(HCD_FLAG_SAW_IRQ, &hcd->flags);
 
 		musb->ep0_stage = MUSB_EP0_START;
 
@@ -1432,7 +1431,7 @@ static int __init musb_core_init(u16 musb_type, struct musb *musb)
 		struct musb_hw_ep	*hw_ep = musb->endpoints + i;
 
 		hw_ep->fifo = MUSB_FIFO_OFFSET(i) + mbase;
-#ifdef CONFIG_USB_MUSB_TUSB6010
+#if defined(CONFIG_USB_MUSB_TUSB6010) || defined (CONFIG_USB_MUSB_TUSB6010_MODULE)
 		hw_ep->fifo_async = musb->async + 0x400 + MUSB_FIFO_OFFSET(i);
 		hw_ep->fifo_sync = musb->sync + 0x400 + MUSB_FIFO_OFFSET(i);
 		hw_ep->fifo_sync_va =
@@ -1631,6 +1630,7 @@ void musb_dma_completion(struct musb *musb, u8 epnum, u8 transmit)
 		}
 	}
 }
+EXPORT_SYMBOL_GPL(musb_dma_completion);
 
 #else
 #define use_dma			0
@@ -2012,8 +2012,6 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	if (status < 0)
 		goto fail3;
 
-	pm_runtime_put(musb->controller);
-
 	status = musb_init_debugfs(musb);
 	if (status < 0)
 		goto fail4;
@@ -2158,6 +2156,7 @@ static void musb_save_context(struct musb *musb)
 		if (!epio)
 			continue;
 
+		musb_writeb(musb_base, MUSB_INDEX, i);
 		musb->context.index_regs[i].txmaxp =
 			musb_readw(epio, MUSB_TXMAXP);
 		musb->context.index_regs[i].txcsr =
@@ -2233,6 +2232,7 @@ static void musb_restore_context(struct musb *musb)
 		if (!epio)
 			continue;
 
+		musb_writeb(musb_base, MUSB_INDEX, i);
 		musb_writew(epio, MUSB_TXMAXP,
 			musb->context.index_regs[i].txmaxp);
 		musb_writew(epio, MUSB_TXCSR,
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index b3c065ab9dbc..3d28fb8a2dc9 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -40,7 +40,6 @@
 #include <linux/interrupt.h>
 #include <linux/errno.h>
 #include <linux/timer.h>
-#include <linux/clk.h>
 #include <linux/device.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
@@ -311,6 +310,7 @@ struct musb_context_registers {
 	u8 index, testmode;
 
 	u8 devctl, busctl, misc;
+	u32 otg_interfsel;
 
 	struct musb_csr_regs index_regs[MUSB_C_NUM_EPS];
 };
@@ -327,6 +327,7 @@ struct musb {
 
 	irqreturn_t		(*isr)(int, void *);
 	struct work_struct	irq_work;
+	struct work_struct	otg_notifier_work;
 	u16			hwvers;
 
 /* this hub status bit is reserved by USB 2.0 and not seen by usbcore */
@@ -372,6 +373,7 @@ struct musb {
 	u16			int_tx;
 
 	struct otg_transceiver	*xceiv;
+	u8			xceiv_event;
 
 	int nIrq;
 	unsigned		irq_wake:1;
diff --git a/drivers/usb/musb/musb_debug.h b/drivers/usb/musb/musb_debug.h
index 742eada5002e..27ba8f799462 100644
--- a/drivers/usb/musb/musb_debug.h
+++ b/drivers/usb/musb/musb_debug.h
@@ -43,8 +43,8 @@
 #define ERR(fmt, args...) yprintk(KERN_ERR, fmt, ## args)
 
 #ifdef CONFIG_DEBUG_FS
-extern int musb_init_debugfs(struct musb *musb);
-extern void musb_exit_debugfs(struct musb *musb);
+int musb_init_debugfs(struct musb *musb);
+void musb_exit_debugfs(struct musb *musb);
 #else
 static inline int musb_init_debugfs(struct musb *musb)
 {
diff --git a/drivers/usb/musb/musb_debugfs.c b/drivers/usb/musb/musb_debugfs.c
index 61f4ee466df7..13d9af9bf920 100644
--- a/drivers/usb/musb/musb_debugfs.c
+++ b/drivers/usb/musb/musb_debugfs.c
@@ -33,11 +33,7 @@
 
 #include <linux/module.h>
 #include <linux/kernel.h>
-#include <linux/sched.h>
 #include <linux/init.h>
-#include <linux/list.h>
-#include <linux/platform_device.h>
-#include <linux/io.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 
@@ -46,10 +42,6 @@
 #include "musb_core.h"
 #include "musb_debug.h"
 
-#ifdef CONFIG_ARCH_DAVINCI
-#include "davinci.h"
-#endif
-
 struct musb_register_map {
 	char			*name;
 	unsigned		offset;
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 922148ff8d29..ac3d2eec20fe 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -40,8 +40,6 @@
 #include <linux/smp.h>
 #include <linux/spinlock.h>
 #include <linux/delay.h>
-#include <linux/moduleparam.h>
-#include <linux/stat.h>
 #include <linux/dma-mapping.h>
 #include <linux/slab.h>
 
@@ -1844,7 +1842,7 @@ int __init musb_gadget_setup(struct musb *musb)
 	 */
 
 	musb->g.ops = &musb_gadget_operations;
-	musb->g.is_dualspeed = 1;
+	musb->g.max_speed = USB_SPEED_HIGH;
 	musb->g.speed = USB_SPEED_UNKNOWN;
 
 	/* this "gadget" abstracts/virtualizes the controller */
@@ -1903,7 +1901,7 @@ static int musb_gadget_start(struct usb_gadget *g,
 	unsigned long		flags;
 	int			retval = -EINVAL;
 
-	if (driver->speed < USB_SPEED_HIGH)
+	if (driver->max_speed < USB_SPEED_HIGH)
 		goto err0;
 
 	pm_runtime_get_sync(musb->controller);
diff --git a/drivers/usb/musb/musb_gadget_ep0.c b/drivers/usb/musb/musb_gadget_ep0.c
index 6a0d0467ec74..e40d7647caf1 100644
--- a/drivers/usb/musb/musb_gadget_ep0.c
+++ b/drivers/usb/musb/musb_gadget_ep0.c
@@ -37,7 +37,6 @@
 #include <linux/list.h>
 #include <linux/timer.h>
 #include <linux/spinlock.h>
-#include <linux/init.h>
 #include <linux/device.h>
 #include <linux/interrupt.h>
 
diff --git a/drivers/usb/musb/musb_io.h b/drivers/usb/musb/musb_io.h
index 03c6ccdbb3be..e61aa95f2d2a 100644
--- a/drivers/usb/musb/musb_io.h
+++ b/drivers/usb/musb/musb_io.h
@@ -74,7 +74,7 @@ static inline void musb_writel(void __iomem *addr, unsigned offset, u32 data)
 	{ __raw_writel(data, addr + offset); }
 
 
-#ifdef CONFIG_USB_MUSB_TUSB6010
+#if defined(CONFIG_USB_MUSB_TUSB6010) || defined (CONFIG_USB_MUSB_TUSB6010_MODULE)
 
 /*
  * TUSB6010 doesn't allow 8-bit access; 16-bit access is the minimum.
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index ba85f273e487..c27bbbf32b52 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -29,7 +29,6 @@
 #include <linux/sched.h>
 #include <linux/init.h>
 #include <linux/list.h>
-#include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
@@ -228,21 +227,25 @@ static int musb_otg_notifications(struct notifier_block *nb,
 		unsigned long event, void *unused)
 {
 	struct musb	*musb = container_of(nb, struct musb, nb);
+
+	musb->xceiv_event = event;
+	schedule_work(&musb->otg_notifier_work);
+
+	return 0;
+}
+
+static void musb_otg_notifier_work(struct work_struct *data_notifier_work)
+{
+	struct musb *musb = container_of(data_notifier_work, struct musb, otg_notifier_work);
 	struct device *dev = musb->controller;
 	struct musb_hdrc_platform_data *pdata = dev->platform_data;
 	struct omap_musb_board_data *data = pdata->board_data;
 
-	switch (event) {
+	switch (musb->xceiv_event) {
 	case USB_EVENT_ID:
 		dev_dbg(musb->controller, "ID GND\n");
 
-		if (is_otg_enabled(musb)) {
-			if (musb->gadget_driver) {
-				pm_runtime_get_sync(musb->controller);
-				otg_init(musb->xceiv);
-				omap2430_musb_set_vbus(musb, 1);
-			}
-		} else {
+		if (!is_otg_enabled(musb) || musb->gadget_driver) {
 			pm_runtime_get_sync(musb->controller);
 			otg_init(musb->xceiv);
 			omap2430_musb_set_vbus(musb, 1);
@@ -274,10 +277,7 @@ static int musb_otg_notifications(struct notifier_block *nb,
 		break;
 	default:
 		dev_dbg(musb->controller, "ID float\n");
-		return NOTIFY_DONE;
 	}
-
-	return NOTIFY_OK;
 }
 
 static int omap2430_musb_init(struct musb *musb)
@@ -297,6 +297,8 @@ static int omap2430_musb_init(struct musb *musb)
 		return -ENODEV;
 	}
 
+	INIT_WORK(&musb->otg_notifier_work, musb_otg_notifier_work);
+
 	status = pm_runtime_get_sync(dev);
 	if (status < 0) {
 		dev_err(dev, "pm_runtime_get_sync FAILED");
@@ -334,7 +336,6 @@ static int omap2430_musb_init(struct musb *musb)
 	return 0;
 
 err1:
-	pm_runtime_disable(dev);
 	return status;
 }
 
@@ -350,20 +351,19 @@ static void omap2430_musb_enable(struct musb *musb)
 
 	case USB_EVENT_ID:
 		otg_init(musb->xceiv);
-		if (data->interface_type == MUSB_INTERFACE_UTMI) {
-			devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
-			/* start the session */
-			devctl |= MUSB_DEVCTL_SESSION;
-			musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
-			while (musb_readb(musb->mregs, MUSB_DEVCTL) &
-						MUSB_DEVCTL_BDEVICE) {
-				cpu_relax();
-
-				if (time_after(jiffies, timeout)) {
-					dev_err(musb->controller,
-					"configured as A device timeout");
-					break;
-				}
+		if (data->interface_type != MUSB_INTERFACE_UTMI)
+			break;
+		devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
+		/* start the session */
+		devctl |= MUSB_DEVCTL_SESSION;
+		musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
+		while (musb_readb(musb->mregs, MUSB_DEVCTL) &
+				MUSB_DEVCTL_BDEVICE) {
+			cpu_relax();
+
+			if (time_after(jiffies, timeout)) {
+				dev_err(dev, "configured as A device timeout");
+				break;
 			}
 		}
 		break;
@@ -478,7 +478,6 @@ static int __exit omap2430_remove(struct platform_device *pdev)
 	platform_device_del(glue->musb);
 	platform_device_put(glue->musb);
 	pm_runtime_put(&pdev->dev);
-	pm_runtime_disable(&pdev->dev);
 	kfree(glue);
 
 	return 0;
@@ -491,6 +490,9 @@ static int omap2430_runtime_suspend(struct device *dev)
 	struct omap2430_glue		*glue = dev_get_drvdata(dev);
 	struct musb			*musb = glue_to_musb(glue);
 
+	musb->context.otg_interfsel = musb_readl(musb->mregs,
+						OTG_INTERFSEL);
+
 	omap2430_low_level_exit(musb);
 	otg_set_suspend(musb->xceiv, 1);
 
@@ -503,6 +505,9 @@ static int omap2430_runtime_resume(struct device *dev)
 	struct musb			*musb = glue_to_musb(glue);
 
 	omap2430_low_level_init(musb);
+	musb_writel(musb->mregs, OTG_INTERFSEL,
+					musb->context.otg_interfsel);
+
 	otg_set_suspend(musb->xceiv, 0);
 
 	return 0;
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index ec1480191f78..1f405616e6cd 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -56,6 +56,7 @@ u8 tusb_get_revision(struct musb *musb)
 
 	return rev;
 }
+EXPORT_SYMBOL_GPL(tusb_get_revision);
 
 static int tusb_print_revision(struct musb *musb)
 {
diff --git a/drivers/usb/musb/ux500_dma.c b/drivers/usb/musb/ux500_dma.c
index ef4333f4bbe0..a163632877af 100644
--- a/drivers/usb/musb/ux500_dma.c
+++ b/drivers/usb/musb/ux500_dma.c
@@ -37,7 +37,6 @@ struct ux500_dma_channel {
 	struct dma_channel channel;
 	struct ux500_dma_controller *controller;
 	struct musb_hw_ep *hw_ep;
-	struct work_struct channel_work;
 	struct dma_chan *dma_chan;
 	unsigned int cur_len;
 	dma_cookie_t cookie;
@@ -56,31 +55,11 @@ struct ux500_dma_controller {
 	dma_addr_t phy_base;
 };
 
-/* Work function invoked from DMA callback to handle tx transfers. */
-static void ux500_tx_work(struct work_struct *data)
-{
-	struct ux500_dma_channel *ux500_channel = container_of(data,
-		struct ux500_dma_channel, channel_work);
-	struct musb_hw_ep       *hw_ep = ux500_channel->hw_ep;
-	struct musb *musb = hw_ep->musb;
-	unsigned long flags;
-
-	dev_dbg(musb->controller, "DMA tx transfer done on hw_ep=%d\n",
-		hw_ep->epnum);
-
-	spin_lock_irqsave(&musb->lock, flags);
-	ux500_channel->channel.actual_len = ux500_channel->cur_len;
-	ux500_channel->channel.status = MUSB_DMA_STATUS_FREE;
-	musb_dma_completion(musb, hw_ep->epnum,
-				ux500_channel->is_tx);
-	spin_unlock_irqrestore(&musb->lock, flags);
-}
-
 /* Work function invoked from DMA callback to handle rx transfers. */
-static void ux500_rx_work(struct work_struct *data)
+void ux500_dma_callback(void *private_data)
 {
-	struct ux500_dma_channel *ux500_channel = container_of(data,
-		struct ux500_dma_channel, channel_work);
+	struct dma_channel *channel = private_data;
+	struct ux500_dma_channel *ux500_channel = channel->private_data;
 	struct musb_hw_ep       *hw_ep = ux500_channel->hw_ep;
 	struct musb *musb = hw_ep->musb;
 	unsigned long flags;
@@ -94,14 +73,7 @@ static void ux500_rx_work(struct work_struct *data)
 	musb_dma_completion(musb, hw_ep->epnum,
 		ux500_channel->is_tx);
 	spin_unlock_irqrestore(&musb->lock, flags);
-}
-
-void ux500_dma_callback(void *private_data)
-{
-	struct dma_channel *channel = (struct dma_channel *)private_data;
-	struct ux500_dma_channel *ux500_channel = channel->private_data;
 
-	schedule_work(&ux500_channel->channel_work);
 }
 
 static bool ux500_configure_channel(struct dma_channel *channel,
@@ -330,7 +302,6 @@ static int ux500_dma_controller_start(struct dma_controller *c)
 	void **param_array;
 	struct ux500_dma_channel *channel_array;
 	u32 ch_count;
-	void (*musb_channel_work)(struct work_struct *);
 	dma_cap_mask_t mask;
 
 	if ((data->num_rx_channels > UX500_MUSB_DMA_NUM_RX_CHANNELS) ||
@@ -347,7 +318,6 @@ static int ux500_dma_controller_start(struct dma_controller *c)
 	channel_array = controller->rx_channel;
 	ch_count = data->num_rx_channels;
 	param_array = data->dma_rx_param_array;
-	musb_channel_work = ux500_rx_work;
 
 	for (dir = 0; dir < 2; dir++) {
 		for (ch_num = 0; ch_num < ch_count; ch_num++) {
@@ -374,15 +344,12 @@ static int ux500_dma_controller_start(struct dma_controller *c)
 				return -EBUSY;
 			}
 
-			INIT_WORK(&ux500_channel->channel_work,
-				musb_channel_work);
 		}
 
 		/* Prepare the loop for TX channels */
 		channel_array = controller->tx_channel;
 		ch_count = data->num_tx_channels;
 		param_array = data->dma_tx_param_array;
-		musb_channel_work = ux500_tx_work;
 		is_tx = 1;
 	}
 
diff --git a/drivers/usb/otg/Kconfig b/drivers/usb/otg/Kconfig
index c66481ad98d7..2a25955881fc 100644
--- a/drivers/usb/otg/Kconfig
+++ b/drivers/usb/otg/Kconfig
@@ -82,9 +82,9 @@ config NOP_USB_XCEIV
 	tristate "NOP USB Transceiver Driver"
 	select USB_OTG_UTILS
 	help
-	 this driver is to be used by all the usb transceiver which are either
-	 built-in with usb ip or which are autonomous and doesn't require any
-	 phy programming such as ISP1x04 etc.
+	  This driver is to be used by all the usb transceiver which are either
+	  built-in with usb ip or which are autonomous and doesn't require any
+	  phy programming such as ISP1x04 etc.
 
 config USB_LANGWELL_OTG
 	tristate "Intel Langwell USB OTG dual-role support"
@@ -114,13 +114,13 @@ config USB_MSM_OTG
 	  has an external PHY.
 
 config AB8500_USB
-        tristate "AB8500 USB Transceiver Driver"
-        depends on AB8500_CORE
-        select USB_OTG_UTILS
-        help
-          Enable this to support the USB OTG transceiver in AB8500 chip.
-          This transceiver supports high and full speed devices plus,
-          in host mode, low speed.
+	tristate "AB8500 USB Transceiver Driver"
+	depends on AB8500_CORE
+	select USB_OTG_UTILS
+	help
+	  Enable this to support the USB OTG transceiver in AB8500 chip.
+	  This transceiver supports high and full speed devices plus,
+	  in host mode, low speed.
 
 config FSL_USB2_OTG
 	bool "Freescale USB OTG Transceiver Driver"
@@ -130,4 +130,16 @@ config FSL_USB2_OTG
 	help
 	  Enable this to support Freescale USB OTG transceiver.
 
+config USB_MV_OTG
+	tristate "Marvell USB OTG support"
+	depends on USB_MV_UDC
+	select USB_OTG
+	select USB_OTG_UTILS
+	help
+	  Say Y here if you want to build Marvell USB OTG transciever
+	  driver in kernel (including PXA and MMP series). This driver
+	  implements role switch between EHCI host driver and gadget driver.
+
+	  To compile this driver as a module, choose M here.
+
 endif # USB || OTG
diff --git a/drivers/usb/otg/Makefile b/drivers/usb/otg/Makefile
index 566655c53331..b2c5a9598637 100644
--- a/drivers/usb/otg/Makefile
+++ b/drivers/usb/otg/Makefile
@@ -21,3 +21,4 @@ obj-$(CONFIG_USB_MSM_OTG)	+= msm_otg.o
 obj-$(CONFIG_AB8500_USB)	+= ab8500-usb.o
 fsl_usb2_otg-objs		:= fsl_otg.o otg_fsm.o
 obj-$(CONFIG_FSL_USB2_OTG)	+= fsl_usb2_otg.o
+obj-$(CONFIG_USB_MV_OTG)	+= mv_otg.o
diff --git a/drivers/usb/otg/fsl_otg.c b/drivers/usb/otg/fsl_otg.c
index 2d9cc445fc73..a190850d2d3b 100644
--- a/drivers/usb/otg/fsl_otg.c
+++ b/drivers/usb/otg/fsl_otg.c
@@ -1151,18 +1151,7 @@ struct platform_driver fsl_otg_driver = {
 	},
 };
 
-static int __init fsl_usb_otg_init(void)
-{
-	pr_info(DRIVER_INFO "\n");
-	return platform_driver_register(&fsl_otg_driver);
-}
-module_init(fsl_usb_otg_init);
-
-static void __exit fsl_usb_otg_exit(void)
-{
-	platform_driver_unregister(&fsl_otg_driver);
-}
-module_exit(fsl_usb_otg_exit);
+module_platform_driver(fsl_otg_driver);
 
 MODULE_DESCRIPTION(DRIVER_INFO);
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/otg/mv_otg.c b/drivers/usb/otg/mv_otg.c
new file mode 100644
index 000000000000..db0d4fcdc8e2
--- /dev/null
+++ b/drivers/usb/otg/mv_otg.c
@@ -0,0 +1,957 @@
+/*
+ * Copyright (C) 2011 Marvell International Ltd. All rights reserved.
+ * Author: Chao Xie <chao.xie@marvell.com>
+ *	   Neil Zhang <zhangwm@marvell.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/device.h>
+#include <linux/proc_fs.h>
+#include <linux/clk.h>
+#include <linux/workqueue.h>
+#include <linux/platform_device.h>
+
+#include <linux/usb.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/otg.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/hcd.h>
+#include <linux/platform_data/mv_usb.h>
+
+#include "mv_otg.h"
+
+#define	DRIVER_DESC	"Marvell USB OTG transceiver driver"
+#define	DRIVER_VERSION	"Jan 20, 2010"
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL");
+
+static const char driver_name[] = "mv-otg";
+
+static char *state_string[] = {
+	"undefined",
+	"b_idle",
+	"b_srp_init",
+	"b_peripheral",
+	"b_wait_acon",
+	"b_host",
+	"a_idle",
+	"a_wait_vrise",
+	"a_wait_bcon",
+	"a_host",
+	"a_suspend",
+	"a_peripheral",
+	"a_wait_vfall",
+	"a_vbus_err"
+};
+
+static int mv_otg_set_vbus(struct otg_transceiver *otg, bool on)
+{
+	struct mv_otg *mvotg = container_of(otg, struct mv_otg, otg);
+	if (mvotg->pdata->set_vbus == NULL)
+		return -ENODEV;
+
+	return mvotg->pdata->set_vbus(on);
+}
+
+static int mv_otg_set_host(struct otg_transceiver *otg,
+			   struct usb_bus *host)
+{
+	otg->host = host;
+
+	return 0;
+}
+
+static int mv_otg_set_peripheral(struct otg_transceiver *otg,
+				 struct usb_gadget *gadget)
+{
+	otg->gadget = gadget;
+
+	return 0;
+}
+
+static void mv_otg_run_state_machine(struct mv_otg *mvotg,
+				     unsigned long delay)
+{
+	dev_dbg(&mvotg->pdev->dev, "transceiver is updated\n");
+	if (!mvotg->qwork)
+		return;
+
+	queue_delayed_work(mvotg->qwork, &mvotg->work, delay);
+}
+
+static void mv_otg_timer_await_bcon(unsigned long data)
+{
+	struct mv_otg *mvotg = (struct mv_otg *) data;
+
+	mvotg->otg_ctrl.a_wait_bcon_timeout = 1;
+
+	dev_info(&mvotg->pdev->dev, "B Device No Response!\n");
+
+	if (spin_trylock(&mvotg->wq_lock)) {
+		mv_otg_run_state_machine(mvotg, 0);
+		spin_unlock(&mvotg->wq_lock);
+	}
+}
+
+static int mv_otg_cancel_timer(struct mv_otg *mvotg, unsigned int id)
+{
+	struct timer_list *timer;
+
+	if (id >= OTG_TIMER_NUM)
+		return -EINVAL;
+
+	timer = &mvotg->otg_ctrl.timer[id];
+
+	if (timer_pending(timer))
+		del_timer(timer);
+
+	return 0;
+}
+
+static int mv_otg_set_timer(struct mv_otg *mvotg, unsigned int id,
+			    unsigned long interval,
+			    void (*callback) (unsigned long))
+{
+	struct timer_list *timer;
+
+	if (id >= OTG_TIMER_NUM)
+		return -EINVAL;
+
+	timer = &mvotg->otg_ctrl.timer[id];
+	if (timer_pending(timer)) {
+		dev_err(&mvotg->pdev->dev, "Timer%d is already running\n", id);
+		return -EBUSY;
+	}
+
+	init_timer(timer);
+	timer->data = (unsigned long) mvotg;
+	timer->function = callback;
+	timer->expires = jiffies + interval;
+	add_timer(timer);
+
+	return 0;
+}
+
+static int mv_otg_reset(struct mv_otg *mvotg)
+{
+	unsigned int loops;
+	u32 tmp;
+
+	/* Stop the controller */
+	tmp = readl(&mvotg->op_regs->usbcmd);
+	tmp &= ~USBCMD_RUN_STOP;
+	writel(tmp, &mvotg->op_regs->usbcmd);
+
+	/* Reset the controller to get default values */
+	writel(USBCMD_CTRL_RESET, &mvotg->op_regs->usbcmd);
+
+	loops = 500;
+	while (readl(&mvotg->op_regs->usbcmd) & USBCMD_CTRL_RESET) {
+		if (loops == 0) {
+			dev_err(&mvotg->pdev->dev,
+				"Wait for RESET completed TIMEOUT\n");
+			return -ETIMEDOUT;
+		}
+		loops--;
+		udelay(20);
+	}
+
+	writel(0x0, &mvotg->op_regs->usbintr);
+	tmp = readl(&mvotg->op_regs->usbsts);
+	writel(tmp, &mvotg->op_regs->usbsts);
+
+	return 0;
+}
+
+static void mv_otg_init_irq(struct mv_otg *mvotg)
+{
+	u32 otgsc;
+
+	mvotg->irq_en = OTGSC_INTR_A_SESSION_VALID
+	    | OTGSC_INTR_A_VBUS_VALID;
+	mvotg->irq_status = OTGSC_INTSTS_A_SESSION_VALID
+	    | OTGSC_INTSTS_A_VBUS_VALID;
+
+	if (mvotg->pdata->vbus == NULL) {
+		mvotg->irq_en |= OTGSC_INTR_B_SESSION_VALID
+		    | OTGSC_INTR_B_SESSION_END;
+		mvotg->irq_status |= OTGSC_INTSTS_B_SESSION_VALID
+		    | OTGSC_INTSTS_B_SESSION_END;
+	}
+
+	if (mvotg->pdata->id == NULL) {
+		mvotg->irq_en |= OTGSC_INTR_USB_ID;
+		mvotg->irq_status |= OTGSC_INTSTS_USB_ID;
+	}
+
+	otgsc = readl(&mvotg->op_regs->otgsc);
+	otgsc |= mvotg->irq_en;
+	writel(otgsc, &mvotg->op_regs->otgsc);
+}
+
+static void mv_otg_start_host(struct mv_otg *mvotg, int on)
+{
+	struct otg_transceiver *otg = &mvotg->otg;
+	struct usb_hcd *hcd;
+
+	if (!otg->host)
+		return;
+
+	dev_info(&mvotg->pdev->dev, "%s host\n", on ? "start" : "stop");
+
+	hcd = bus_to_hcd(otg->host);
+
+	if (on)
+		usb_add_hcd(hcd, hcd->irq, IRQF_SHARED);
+	else
+		usb_remove_hcd(hcd);
+}
+
+static void mv_otg_start_periphrals(struct mv_otg *mvotg, int on)
+{
+	struct otg_transceiver *otg = &mvotg->otg;
+
+	if (!otg->gadget)
+		return;
+
+	dev_info(otg->dev, "gadget %s\n", on ? "on" : "off");
+
+	if (on)
+		usb_gadget_vbus_connect(otg->gadget);
+	else
+		usb_gadget_vbus_disconnect(otg->gadget);
+}
+
+static void otg_clock_enable(struct mv_otg *mvotg)
+{
+	unsigned int i;
+
+	for (i = 0; i < mvotg->clknum; i++)
+		clk_enable(mvotg->clk[i]);
+}
+
+static void otg_clock_disable(struct mv_otg *mvotg)
+{
+	unsigned int i;
+
+	for (i = 0; i < mvotg->clknum; i++)
+		clk_disable(mvotg->clk[i]);
+}
+
+static int mv_otg_enable_internal(struct mv_otg *mvotg)
+{
+	int retval = 0;
+
+	if (mvotg->active)
+		return 0;
+
+	dev_dbg(&mvotg->pdev->dev, "otg enabled\n");
+
+	otg_clock_enable(mvotg);
+	if (mvotg->pdata->phy_init) {
+		retval = mvotg->pdata->phy_init(mvotg->phy_regs);
+		if (retval) {
+			dev_err(&mvotg->pdev->dev,
+				"init phy error %d\n", retval);
+			otg_clock_disable(mvotg);
+			return retval;
+		}
+	}
+	mvotg->active = 1;
+
+	return 0;
+
+}
+
+static int mv_otg_enable(struct mv_otg *mvotg)
+{
+	if (mvotg->clock_gating)
+		return mv_otg_enable_internal(mvotg);
+
+	return 0;
+}
+
+static void mv_otg_disable_internal(struct mv_otg *mvotg)
+{
+	if (mvotg->active) {
+		dev_dbg(&mvotg->pdev->dev, "otg disabled\n");
+		if (mvotg->pdata->phy_deinit)
+			mvotg->pdata->phy_deinit(mvotg->phy_regs);
+		otg_clock_disable(mvotg);
+		mvotg->active = 0;
+	}
+}
+
+static void mv_otg_disable(struct mv_otg *mvotg)
+{
+	if (mvotg->clock_gating)
+		mv_otg_disable_internal(mvotg);
+}
+
+static void mv_otg_update_inputs(struct mv_otg *mvotg)
+{
+	struct mv_otg_ctrl *otg_ctrl = &mvotg->otg_ctrl;
+	u32 otgsc;
+
+	otgsc = readl(&mvotg->op_regs->otgsc);
+
+	if (mvotg->pdata->vbus) {
+		if (mvotg->pdata->vbus->poll() == VBUS_HIGH) {
+			otg_ctrl->b_sess_vld = 1;
+			otg_ctrl->b_sess_end = 0;
+		} else {
+			otg_ctrl->b_sess_vld = 0;
+			otg_ctrl->b_sess_end = 1;
+		}
+	} else {
+		otg_ctrl->b_sess_vld = !!(otgsc & OTGSC_STS_B_SESSION_VALID);
+		otg_ctrl->b_sess_end = !!(otgsc & OTGSC_STS_B_SESSION_END);
+	}
+
+	if (mvotg->pdata->id)
+		otg_ctrl->id = !!mvotg->pdata->id->poll();
+	else
+		otg_ctrl->id = !!(otgsc & OTGSC_STS_USB_ID);
+
+	if (mvotg->pdata->otg_force_a_bus_req && !otg_ctrl->id)
+		otg_ctrl->a_bus_req = 1;
+
+	otg_ctrl->a_sess_vld = !!(otgsc & OTGSC_STS_A_SESSION_VALID);
+	otg_ctrl->a_vbus_vld = !!(otgsc & OTGSC_STS_A_VBUS_VALID);
+
+	dev_dbg(&mvotg->pdev->dev, "%s: ", __func__);
+	dev_dbg(&mvotg->pdev->dev, "id %d\n", otg_ctrl->id);
+	dev_dbg(&mvotg->pdev->dev, "b_sess_vld %d\n", otg_ctrl->b_sess_vld);
+	dev_dbg(&mvotg->pdev->dev, "b_sess_end %d\n", otg_ctrl->b_sess_end);
+	dev_dbg(&mvotg->pdev->dev, "a_vbus_vld %d\n", otg_ctrl->a_vbus_vld);
+	dev_dbg(&mvotg->pdev->dev, "a_sess_vld %d\n", otg_ctrl->a_sess_vld);
+}
+
+static void mv_otg_update_state(struct mv_otg *mvotg)
+{
+	struct mv_otg_ctrl *otg_ctrl = &mvotg->otg_ctrl;
+	struct otg_transceiver *otg = &mvotg->otg;
+	int old_state = otg->state;
+
+	switch (old_state) {
+	case OTG_STATE_UNDEFINED:
+		otg->state = OTG_STATE_B_IDLE;
+		/* FALL THROUGH */
+	case OTG_STATE_B_IDLE:
+		if (otg_ctrl->id == 0)
+			otg->state = OTG_STATE_A_IDLE;
+		else if (otg_ctrl->b_sess_vld)
+			otg->state = OTG_STATE_B_PERIPHERAL;
+		break;
+	case OTG_STATE_B_PERIPHERAL:
+		if (!otg_ctrl->b_sess_vld || otg_ctrl->id == 0)
+			otg->state = OTG_STATE_B_IDLE;
+		break;
+	case OTG_STATE_A_IDLE:
+		if (otg_ctrl->id)
+			otg->state = OTG_STATE_B_IDLE;
+		else if (!(otg_ctrl->a_bus_drop) &&
+			 (otg_ctrl->a_bus_req || otg_ctrl->a_srp_det))
+			otg->state = OTG_STATE_A_WAIT_VRISE;
+		break;
+	case OTG_STATE_A_WAIT_VRISE:
+		if (otg_ctrl->a_vbus_vld)
+			otg->state = OTG_STATE_A_WAIT_BCON;
+		break;
+	case OTG_STATE_A_WAIT_BCON:
+		if (otg_ctrl->id || otg_ctrl->a_bus_drop
+		    || otg_ctrl->a_wait_bcon_timeout) {
+			mv_otg_cancel_timer(mvotg, A_WAIT_BCON_TIMER);
+			mvotg->otg_ctrl.a_wait_bcon_timeout = 0;
+			otg->state = OTG_STATE_A_WAIT_VFALL;
+			otg_ctrl->a_bus_req = 0;
+		} else if (!otg_ctrl->a_vbus_vld) {
+			mv_otg_cancel_timer(mvotg, A_WAIT_BCON_TIMER);
+			mvotg->otg_ctrl.a_wait_bcon_timeout = 0;
+			otg->state = OTG_STATE_A_VBUS_ERR;
+		} else if (otg_ctrl->b_conn) {
+			mv_otg_cancel_timer(mvotg, A_WAIT_BCON_TIMER);
+			mvotg->otg_ctrl.a_wait_bcon_timeout = 0;
+			otg->state = OTG_STATE_A_HOST;
+		}
+		break;
+	case OTG_STATE_A_HOST:
+		if (otg_ctrl->id || !otg_ctrl->b_conn
+		    || otg_ctrl->a_bus_drop)
+			otg->state = OTG_STATE_A_WAIT_BCON;
+		else if (!otg_ctrl->a_vbus_vld)
+			otg->state = OTG_STATE_A_VBUS_ERR;
+		break;
+	case OTG_STATE_A_WAIT_VFALL:
+		if (otg_ctrl->id
+		    || (!otg_ctrl->b_conn && otg_ctrl->a_sess_vld)
+		    || otg_ctrl->a_bus_req)
+			otg->state = OTG_STATE_A_IDLE;
+		break;
+	case OTG_STATE_A_VBUS_ERR:
+		if (otg_ctrl->id || otg_ctrl->a_clr_err
+		    || otg_ctrl->a_bus_drop) {
+			otg_ctrl->a_clr_err = 0;
+			otg->state = OTG_STATE_A_WAIT_VFALL;
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static void mv_otg_work(struct work_struct *work)
+{
+	struct mv_otg *mvotg;
+	struct otg_transceiver *otg;
+	int old_state;
+
+	mvotg = container_of((struct delayed_work *)work, struct mv_otg, work);
+
+run:
+	/* work queue is single thread, or we need spin_lock to protect */
+	otg = &mvotg->otg;
+	old_state = otg->state;
+
+	if (!mvotg->active)
+		return;
+
+	mv_otg_update_inputs(mvotg);
+	mv_otg_update_state(mvotg);
+
+	if (old_state != otg->state) {
+		dev_info(&mvotg->pdev->dev, "change from state %s to %s\n",
+			 state_string[old_state],
+			 state_string[otg->state]);
+
+		switch (otg->state) {
+		case OTG_STATE_B_IDLE:
+			mvotg->otg.default_a = 0;
+			if (old_state == OTG_STATE_B_PERIPHERAL)
+				mv_otg_start_periphrals(mvotg, 0);
+			mv_otg_reset(mvotg);
+			mv_otg_disable(mvotg);
+			break;
+		case OTG_STATE_B_PERIPHERAL:
+			mv_otg_enable(mvotg);
+			mv_otg_start_periphrals(mvotg, 1);
+			break;
+		case OTG_STATE_A_IDLE:
+			mvotg->otg.default_a = 1;
+			mv_otg_enable(mvotg);
+			if (old_state == OTG_STATE_A_WAIT_VFALL)
+				mv_otg_start_host(mvotg, 0);
+			mv_otg_reset(mvotg);
+			break;
+		case OTG_STATE_A_WAIT_VRISE:
+			mv_otg_set_vbus(&mvotg->otg, 1);
+			break;
+		case OTG_STATE_A_WAIT_BCON:
+			if (old_state != OTG_STATE_A_HOST)
+				mv_otg_start_host(mvotg, 1);
+			mv_otg_set_timer(mvotg, A_WAIT_BCON_TIMER,
+					 T_A_WAIT_BCON,
+					 mv_otg_timer_await_bcon);
+			/*
+			 * Now, we directly enter A_HOST. So set b_conn = 1
+			 * here. In fact, it need host driver to notify us.
+			 */
+			mvotg->otg_ctrl.b_conn = 1;
+			break;
+		case OTG_STATE_A_HOST:
+			break;
+		case OTG_STATE_A_WAIT_VFALL:
+			/*
+			 * Now, we has exited A_HOST. So set b_conn = 0
+			 * here. In fact, it need host driver to notify us.
+			 */
+			mvotg->otg_ctrl.b_conn = 0;
+			mv_otg_set_vbus(&mvotg->otg, 0);
+			break;
+		case OTG_STATE_A_VBUS_ERR:
+			break;
+		default:
+			break;
+		}
+		goto run;
+	}
+}
+
+static irqreturn_t mv_otg_irq(int irq, void *dev)
+{
+	struct mv_otg *mvotg = dev;
+	u32 otgsc;
+
+	otgsc = readl(&mvotg->op_regs->otgsc);
+	writel(otgsc, &mvotg->op_regs->otgsc);
+
+	/*
+	 * if we have vbus, then the vbus detection for B-device
+	 * will be done by mv_otg_inputs_irq().
+	 */
+	if (mvotg->pdata->vbus)
+		if ((otgsc & OTGSC_STS_USB_ID) &&
+		    !(otgsc & OTGSC_INTSTS_USB_ID))
+			return IRQ_NONE;
+
+	if ((otgsc & mvotg->irq_status) == 0)
+		return IRQ_NONE;
+
+	mv_otg_run_state_machine(mvotg, 0);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mv_otg_inputs_irq(int irq, void *dev)
+{
+	struct mv_otg *mvotg = dev;
+
+	/* The clock may disabled at this time */
+	if (!mvotg->active) {
+		mv_otg_enable(mvotg);
+		mv_otg_init_irq(mvotg);
+	}
+
+	mv_otg_run_state_machine(mvotg, 0);
+
+	return IRQ_HANDLED;
+}
+
+static ssize_t
+get_a_bus_req(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct mv_otg *mvotg = dev_get_drvdata(dev);
+	return scnprintf(buf, PAGE_SIZE, "%d\n",
+			 mvotg->otg_ctrl.a_bus_req);
+}
+
+static ssize_t
+set_a_bus_req(struct device *dev, struct device_attribute *attr,
+	      const char *buf, size_t count)
+{
+	struct mv_otg *mvotg = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	/* We will use this interface to change to A device */
+	if (mvotg->otg.state != OTG_STATE_B_IDLE
+	    && mvotg->otg.state != OTG_STATE_A_IDLE)
+		return -1;
+
+	/* The clock may disabled and we need to set irq for ID detected */
+	mv_otg_enable(mvotg);
+	mv_otg_init_irq(mvotg);
+
+	if (buf[0] == '1') {
+		mvotg->otg_ctrl.a_bus_req = 1;
+		mvotg->otg_ctrl.a_bus_drop = 0;
+		dev_dbg(&mvotg->pdev->dev,
+			"User request: a_bus_req = 1\n");
+
+		if (spin_trylock(&mvotg->wq_lock)) {
+			mv_otg_run_state_machine(mvotg, 0);
+			spin_unlock(&mvotg->wq_lock);
+		}
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(a_bus_req, S_IRUGO | S_IWUSR, get_a_bus_req,
+		   set_a_bus_req);
+
+static ssize_t
+set_a_clr_err(struct device *dev, struct device_attribute *attr,
+	      const char *buf, size_t count)
+{
+	struct mv_otg *mvotg = dev_get_drvdata(dev);
+	if (!mvotg->otg.default_a)
+		return -1;
+
+	if (count > 2)
+		return -1;
+
+	if (buf[0] == '1') {
+		mvotg->otg_ctrl.a_clr_err = 1;
+		dev_dbg(&mvotg->pdev->dev,
+			"User request: a_clr_err = 1\n");
+	}
+
+	if (spin_trylock(&mvotg->wq_lock)) {
+		mv_otg_run_state_machine(mvotg, 0);
+		spin_unlock(&mvotg->wq_lock);
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(a_clr_err, S_IWUSR, NULL, set_a_clr_err);
+
+static ssize_t
+get_a_bus_drop(struct device *dev, struct device_attribute *attr,
+	       char *buf)
+{
+	struct mv_otg *mvotg = dev_get_drvdata(dev);
+	return scnprintf(buf, PAGE_SIZE, "%d\n",
+			 mvotg->otg_ctrl.a_bus_drop);
+}
+
+static ssize_t
+set_a_bus_drop(struct device *dev, struct device_attribute *attr,
+	       const char *buf, size_t count)
+{
+	struct mv_otg *mvotg = dev_get_drvdata(dev);
+	if (!mvotg->otg.default_a)
+		return -1;
+
+	if (count > 2)
+		return -1;
+
+	if (buf[0] == '0') {
+		mvotg->otg_ctrl.a_bus_drop = 0;
+		dev_dbg(&mvotg->pdev->dev,
+			"User request: a_bus_drop = 0\n");
+	} else if (buf[0] == '1') {
+		mvotg->otg_ctrl.a_bus_drop = 1;
+		mvotg->otg_ctrl.a_bus_req = 0;
+		dev_dbg(&mvotg->pdev->dev,
+			"User request: a_bus_drop = 1\n");
+		dev_dbg(&mvotg->pdev->dev,
+			"User request: and a_bus_req = 0\n");
+	}
+
+	if (spin_trylock(&mvotg->wq_lock)) {
+		mv_otg_run_state_machine(mvotg, 0);
+		spin_unlock(&mvotg->wq_lock);
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(a_bus_drop, S_IRUGO | S_IWUSR,
+		   get_a_bus_drop, set_a_bus_drop);
+
+static struct attribute *inputs_attrs[] = {
+	&dev_attr_a_bus_req.attr,
+	&dev_attr_a_clr_err.attr,
+	&dev_attr_a_bus_drop.attr,
+	NULL,
+};
+
+static struct attribute_group inputs_attr_group = {
+	.name = "inputs",
+	.attrs = inputs_attrs,
+};
+
+int mv_otg_remove(struct platform_device *pdev)
+{
+	struct mv_otg *mvotg = platform_get_drvdata(pdev);
+	int clk_i;
+
+	sysfs_remove_group(&mvotg->pdev->dev.kobj, &inputs_attr_group);
+
+	if (mvotg->irq)
+		free_irq(mvotg->irq, mvotg);
+
+	if (mvotg->pdata->vbus)
+		free_irq(mvotg->pdata->vbus->irq, mvotg);
+	if (mvotg->pdata->id)
+		free_irq(mvotg->pdata->id->irq, mvotg);
+
+	if (mvotg->qwork) {
+		flush_workqueue(mvotg->qwork);
+		destroy_workqueue(mvotg->qwork);
+	}
+
+	mv_otg_disable(mvotg);
+
+	if (mvotg->cap_regs)
+		iounmap(mvotg->cap_regs);
+
+	if (mvotg->phy_regs)
+		iounmap(mvotg->phy_regs);
+
+	for (clk_i = 0; clk_i <= mvotg->clknum; clk_i++)
+		clk_put(mvotg->clk[clk_i]);
+
+	otg_set_transceiver(NULL);
+	platform_set_drvdata(pdev, NULL);
+
+	kfree(mvotg);
+
+	return 0;
+}
+
+static int mv_otg_probe(struct platform_device *pdev)
+{
+	struct mv_usb_platform_data *pdata = pdev->dev.platform_data;
+	struct mv_otg *mvotg;
+	struct resource *r;
+	int retval = 0, clk_i, i;
+	size_t size;
+
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "failed to get platform data\n");
+		return -ENODEV;
+	}
+
+	size = sizeof(*mvotg) + sizeof(struct clk *) * pdata->clknum;
+	mvotg = kzalloc(size, GFP_KERNEL);
+	if (!mvotg) {
+		dev_err(&pdev->dev, "failed to allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, mvotg);
+
+	mvotg->pdev = pdev;
+	mvotg->pdata = pdata;
+
+	mvotg->clknum = pdata->clknum;
+	for (clk_i = 0; clk_i < mvotg->clknum; clk_i++) {
+		mvotg->clk[clk_i] = clk_get(&pdev->dev, pdata->clkname[clk_i]);
+		if (IS_ERR(mvotg->clk[clk_i])) {
+			retval = PTR_ERR(mvotg->clk[clk_i]);
+			goto err_put_clk;
+		}
+	}
+
+	mvotg->qwork = create_singlethread_workqueue("mv_otg_queue");
+	if (!mvotg->qwork) {
+		dev_dbg(&pdev->dev, "cannot create workqueue for OTG\n");
+		retval = -ENOMEM;
+		goto err_put_clk;
+	}
+
+	INIT_DELAYED_WORK(&mvotg->work, mv_otg_work);
+
+	/* OTG common part */
+	mvotg->pdev = pdev;
+	mvotg->otg.dev = &pdev->dev;
+	mvotg->otg.label = driver_name;
+	mvotg->otg.set_host = mv_otg_set_host;
+	mvotg->otg.set_peripheral = mv_otg_set_peripheral;
+	mvotg->otg.set_vbus = mv_otg_set_vbus;
+	mvotg->otg.state = OTG_STATE_UNDEFINED;
+
+	for (i = 0; i < OTG_TIMER_NUM; i++)
+		init_timer(&mvotg->otg_ctrl.timer[i]);
+
+	r = platform_get_resource_byname(mvotg->pdev,
+					 IORESOURCE_MEM, "phyregs");
+	if (r == NULL) {
+		dev_err(&pdev->dev, "no phy I/O memory resource defined\n");
+		retval = -ENODEV;
+		goto err_destroy_workqueue;
+	}
+
+	mvotg->phy_regs = ioremap(r->start, resource_size(r));
+	if (mvotg->phy_regs == NULL) {
+		dev_err(&pdev->dev, "failed to map phy I/O memory\n");
+		retval = -EFAULT;
+		goto err_destroy_workqueue;
+	}
+
+	r = platform_get_resource_byname(mvotg->pdev,
+					 IORESOURCE_MEM, "capregs");
+	if (r == NULL) {
+		dev_err(&pdev->dev, "no I/O memory resource defined\n");
+		retval = -ENODEV;
+		goto err_unmap_phyreg;
+	}
+
+	mvotg->cap_regs = ioremap(r->start, resource_size(r));
+	if (mvotg->cap_regs == NULL) {
+		dev_err(&pdev->dev, "failed to map I/O memory\n");
+		retval = -EFAULT;
+		goto err_unmap_phyreg;
+	}
+
+	/* we will acces controller register, so enable the udc controller */
+	retval = mv_otg_enable_internal(mvotg);
+	if (retval) {
+		dev_err(&pdev->dev, "mv otg enable error %d\n", retval);
+		goto err_unmap_capreg;
+	}
+
+	mvotg->op_regs =
+		(struct mv_otg_regs __iomem *) ((unsigned long) mvotg->cap_regs
+			+ (readl(mvotg->cap_regs) & CAPLENGTH_MASK));
+
+	if (pdata->id) {
+		retval = request_threaded_irq(pdata->id->irq, NULL,
+					      mv_otg_inputs_irq,
+					      IRQF_ONESHOT, "id", mvotg);
+		if (retval) {
+			dev_info(&pdev->dev,
+				 "Failed to request irq for ID\n");
+			pdata->id = NULL;
+		}
+	}
+
+	if (pdata->vbus) {
+		mvotg->clock_gating = 1;
+		retval = request_threaded_irq(pdata->vbus->irq, NULL,
+					      mv_otg_inputs_irq,
+					      IRQF_ONESHOT, "vbus", mvotg);
+		if (retval) {
+			dev_info(&pdev->dev,
+				 "Failed to request irq for VBUS, "
+				 "disable clock gating\n");
+			mvotg->clock_gating = 0;
+			pdata->vbus = NULL;
+		}
+	}
+
+	if (pdata->disable_otg_clock_gating)
+		mvotg->clock_gating = 0;
+
+	mv_otg_reset(mvotg);
+	mv_otg_init_irq(mvotg);
+
+	r = platform_get_resource(mvotg->pdev, IORESOURCE_IRQ, 0);
+	if (r == NULL) {
+		dev_err(&pdev->dev, "no IRQ resource defined\n");
+		retval = -ENODEV;
+		goto err_disable_clk;
+	}
+
+	mvotg->irq = r->start;
+	if (request_irq(mvotg->irq, mv_otg_irq, IRQF_SHARED,
+			driver_name, mvotg)) {
+		dev_err(&pdev->dev, "Request irq %d for OTG failed\n",
+			mvotg->irq);
+		mvotg->irq = 0;
+		retval = -ENODEV;
+		goto err_disable_clk;
+	}
+
+	retval = otg_set_transceiver(&mvotg->otg);
+	if (retval < 0) {
+		dev_err(&pdev->dev, "can't register transceiver, %d\n",
+			retval);
+		goto err_free_irq;
+	}
+
+	retval = sysfs_create_group(&pdev->dev.kobj, &inputs_attr_group);
+	if (retval < 0) {
+		dev_dbg(&pdev->dev,
+			"Can't register sysfs attr group: %d\n", retval);
+		goto err_set_transceiver;
+	}
+
+	spin_lock_init(&mvotg->wq_lock);
+	if (spin_trylock(&mvotg->wq_lock)) {
+		mv_otg_run_state_machine(mvotg, 2 * HZ);
+		spin_unlock(&mvotg->wq_lock);
+	}
+
+	dev_info(&pdev->dev,
+		 "successful probe OTG device %s clock gating.\n",
+		 mvotg->clock_gating ? "with" : "without");
+
+	return 0;
+
+err_set_transceiver:
+	otg_set_transceiver(NULL);
+err_free_irq:
+	free_irq(mvotg->irq, mvotg);
+err_disable_clk:
+	if (pdata->vbus)
+		free_irq(pdata->vbus->irq, mvotg);
+	if (pdata->id)
+		free_irq(pdata->id->irq, mvotg);
+	mv_otg_disable_internal(mvotg);
+err_unmap_capreg:
+	iounmap(mvotg->cap_regs);
+err_unmap_phyreg:
+	iounmap(mvotg->phy_regs);
+err_destroy_workqueue:
+	flush_workqueue(mvotg->qwork);
+	destroy_workqueue(mvotg->qwork);
+err_put_clk:
+	for (clk_i--; clk_i >= 0; clk_i--)
+		clk_put(mvotg->clk[clk_i]);
+
+	platform_set_drvdata(pdev, NULL);
+	kfree(mvotg);
+
+	return retval;
+}
+
+#ifdef CONFIG_PM
+static int mv_otg_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mv_otg *mvotg = platform_get_drvdata(pdev);
+
+	if (mvotg->otg.state != OTG_STATE_B_IDLE) {
+		dev_info(&pdev->dev,
+			 "OTG state is not B_IDLE, it is %d!\n",
+			 mvotg->otg.state);
+		return -EAGAIN;
+	}
+
+	if (!mvotg->clock_gating)
+		mv_otg_disable_internal(mvotg);
+
+	return 0;
+}
+
+static int mv_otg_resume(struct platform_device *pdev)
+{
+	struct mv_otg *mvotg = platform_get_drvdata(pdev);
+	u32 otgsc;
+
+	if (!mvotg->clock_gating) {
+		mv_otg_enable_internal(mvotg);
+
+		otgsc = readl(&mvotg->op_regs->otgsc);
+		otgsc |= mvotg->irq_en;
+		writel(otgsc, &mvotg->op_regs->otgsc);
+
+		if (spin_trylock(&mvotg->wq_lock)) {
+			mv_otg_run_state_machine(mvotg, 0);
+			spin_unlock(&mvotg->wq_lock);
+		}
+	}
+	return 0;
+}
+#endif
+
+static struct platform_driver mv_otg_driver = {
+	.probe = mv_otg_probe,
+	.remove = __exit_p(mv_otg_remove),
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = driver_name,
+		   },
+#ifdef CONFIG_PM
+	.suspend = mv_otg_suspend,
+	.resume = mv_otg_resume,
+#endif
+};
+
+static int __init mv_otg_init(void)
+{
+	return platform_driver_register(&mv_otg_driver);
+}
+
+static void __exit mv_otg_exit(void)
+{
+	platform_driver_unregister(&mv_otg_driver);
+}
+
+module_init(mv_otg_init);
+module_exit(mv_otg_exit);
diff --git a/drivers/usb/otg/mv_otg.h b/drivers/usb/otg/mv_otg.h
new file mode 100644
index 000000000000..be6ca1437645
--- /dev/null
+++ b/drivers/usb/otg/mv_otg.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright (C) 2011 Marvell International Ltd. All rights reserved.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#ifndef	__MV_USB_OTG_CONTROLLER__
+#define	__MV_USB_OTG_CONTROLLER__
+
+#include <linux/types.h>
+
+/* Command Register Bit Masks */
+#define USBCMD_RUN_STOP			(0x00000001)
+#define USBCMD_CTRL_RESET		(0x00000002)
+
+/* otgsc Register Bit Masks */
+#define OTGSC_CTRL_VUSB_DISCHARGE		0x00000001
+#define OTGSC_CTRL_VUSB_CHARGE			0x00000002
+#define OTGSC_CTRL_OTG_TERM			0x00000008
+#define OTGSC_CTRL_DATA_PULSING			0x00000010
+#define OTGSC_STS_USB_ID			0x00000100
+#define OTGSC_STS_A_VBUS_VALID			0x00000200
+#define OTGSC_STS_A_SESSION_VALID		0x00000400
+#define OTGSC_STS_B_SESSION_VALID		0x00000800
+#define OTGSC_STS_B_SESSION_END			0x00001000
+#define OTGSC_STS_1MS_TOGGLE			0x00002000
+#define OTGSC_STS_DATA_PULSING			0x00004000
+#define OTGSC_INTSTS_USB_ID			0x00010000
+#define OTGSC_INTSTS_A_VBUS_VALID		0x00020000
+#define OTGSC_INTSTS_A_SESSION_VALID		0x00040000
+#define OTGSC_INTSTS_B_SESSION_VALID		0x00080000
+#define OTGSC_INTSTS_B_SESSION_END		0x00100000
+#define OTGSC_INTSTS_1MS			0x00200000
+#define OTGSC_INTSTS_DATA_PULSING		0x00400000
+#define OTGSC_INTR_USB_ID			0x01000000
+#define OTGSC_INTR_A_VBUS_VALID			0x02000000
+#define OTGSC_INTR_A_SESSION_VALID		0x04000000
+#define OTGSC_INTR_B_SESSION_VALID		0x08000000
+#define OTGSC_INTR_B_SESSION_END		0x10000000
+#define OTGSC_INTR_1MS_TIMER			0x20000000
+#define OTGSC_INTR_DATA_PULSING			0x40000000
+
+#define CAPLENGTH_MASK		(0xff)
+
+/* Timer's interval, unit 10ms */
+#define T_A_WAIT_VRISE		100
+#define T_A_WAIT_BCON		2000
+#define T_A_AIDL_BDIS		100
+#define T_A_BIDL_ADIS		20
+#define T_B_ASE0_BRST		400
+#define T_B_SE0_SRP		300
+#define T_B_SRP_FAIL		2000
+#define T_B_DATA_PLS		10
+#define T_B_SRP_INIT		100
+#define T_A_SRP_RSPNS		10
+#define T_A_DRV_RSM		5
+
+enum otg_function {
+	OTG_B_DEVICE = 0,
+	OTG_A_DEVICE
+};
+
+enum mv_otg_timer {
+	A_WAIT_BCON_TIMER = 0,
+	OTG_TIMER_NUM
+};
+
+/* PXA OTG state machine */
+struct mv_otg_ctrl {
+	/* internal variables */
+	u8 a_set_b_hnp_en;	/* A-Device set b_hnp_en */
+	u8 b_srp_done;
+	u8 b_hnp_en;
+
+	/* OTG inputs */
+	u8 a_bus_drop;
+	u8 a_bus_req;
+	u8 a_clr_err;
+	u8 a_bus_resume;
+	u8 a_bus_suspend;
+	u8 a_conn;
+	u8 a_sess_vld;
+	u8 a_srp_det;
+	u8 a_vbus_vld;
+	u8 b_bus_req;		/* B-Device Require Bus */
+	u8 b_bus_resume;
+	u8 b_bus_suspend;
+	u8 b_conn;
+	u8 b_se0_srp;
+	u8 b_sess_end;
+	u8 b_sess_vld;
+	u8 id;
+	u8 a_suspend_req;
+
+	/*Timer event */
+	u8 a_aidl_bdis_timeout;
+	u8 b_ase0_brst_timeout;
+	u8 a_bidl_adis_timeout;
+	u8 a_wait_bcon_timeout;
+
+	struct timer_list timer[OTG_TIMER_NUM];
+};
+
+#define VUSBHS_MAX_PORTS	8
+
+struct mv_otg_regs {
+	u32 usbcmd;		/* Command register */
+	u32 usbsts;		/* Status register */
+	u32 usbintr;		/* Interrupt enable */
+	u32 frindex;		/* Frame index */
+	u32 reserved1[1];
+	u32 deviceaddr;		/* Device Address */
+	u32 eplistaddr;		/* Endpoint List Address */
+	u32 ttctrl;		/* HOST TT status and control */
+	u32 burstsize;		/* Programmable Burst Size */
+	u32 txfilltuning;	/* Host Transmit Pre-Buffer Packet Tuning */
+	u32 reserved[4];
+	u32 epnak;		/* Endpoint NAK */
+	u32 epnaken;		/* Endpoint NAK Enable */
+	u32 configflag;		/* Configured Flag register */
+	u32 portsc[VUSBHS_MAX_PORTS];	/* Port Status/Control x, x = 1..8 */
+	u32 otgsc;
+	u32 usbmode;		/* USB Host/Device mode */
+	u32 epsetupstat;	/* Endpoint Setup Status */
+	u32 epprime;		/* Endpoint Initialize */
+	u32 epflush;		/* Endpoint De-initialize */
+	u32 epstatus;		/* Endpoint Status */
+	u32 epcomplete;		/* Endpoint Interrupt On Complete */
+	u32 epctrlx[16];	/* Endpoint Control, where x = 0.. 15 */
+	u32 mcr;		/* Mux Control */
+	u32 isr;		/* Interrupt Status */
+	u32 ier;		/* Interrupt Enable */
+};
+
+struct mv_otg {
+	struct otg_transceiver otg;
+	struct mv_otg_ctrl otg_ctrl;
+
+	/* base address */
+	void __iomem *phy_regs;
+	void __iomem *cap_regs;
+	struct mv_otg_regs __iomem *op_regs;
+
+	struct platform_device *pdev;
+	int irq;
+	u32 irq_status;
+	u32 irq_en;
+
+	struct delayed_work work;
+	struct workqueue_struct *qwork;
+
+	spinlock_t wq_lock;
+
+	struct mv_usb_platform_data *pdata;
+
+	unsigned int active;
+	unsigned int clock_gating;
+	unsigned int clknum;
+	struct clk *clk[0];
+};
+
+#endif
diff --git a/drivers/usb/renesas_usbhs/common.c b/drivers/usb/renesas_usbhs/common.c
index 08c679c0dde5..e9a5b1d2615e 100644
--- a/drivers/usb/renesas_usbhs/common.c
+++ b/drivers/usb/renesas_usbhs/common.c
@@ -95,25 +95,15 @@ struct usbhs_priv *usbhs_pdev_to_priv(struct platform_device *pdev)
 /*
  *		syscfg functions
  */
-void usbhs_sys_clock_ctrl(struct usbhs_priv *priv, int enable)
+static void usbhs_sys_clock_ctrl(struct usbhs_priv *priv, int enable)
 {
 	usbhs_bset(priv, SYSCFG, SCKE, enable ? SCKE : 0);
 }
 
-void usbhs_sys_hispeed_ctrl(struct usbhs_priv *priv, int enable)
-{
-	usbhs_bset(priv, SYSCFG, HSE, enable ? HSE : 0);
-}
-
-void usbhs_sys_usb_ctrl(struct usbhs_priv *priv, int enable)
-{
-	usbhs_bset(priv, SYSCFG, USBE, enable ? USBE : 0);
-}
-
 void usbhs_sys_host_ctrl(struct usbhs_priv *priv, int enable)
 {
-	u16 mask = DCFM | DRPD | DPRPU;
-	u16 val  = DCFM | DRPD;
+	u16 mask = DCFM | DRPD | DPRPU | HSE | USBE;
+	u16 val  = DCFM | DRPD | HSE | USBE;
 	int has_otg = usbhs_get_dparam(priv, has_otg);
 
 	if (has_otg)
@@ -130,8 +120,8 @@ void usbhs_sys_host_ctrl(struct usbhs_priv *priv, int enable)
 
 void usbhs_sys_function_ctrl(struct usbhs_priv *priv, int enable)
 {
-	u16 mask = DCFM | DRPD | DPRPU;
-	u16 val  = DPRPU;
+	u16 mask = DCFM | DRPD | DPRPU | HSE | USBE;
+	u16 val  = DPRPU | HSE | USBE;
 
 	/*
 	 * if enable
@@ -142,6 +132,11 @@ void usbhs_sys_function_ctrl(struct usbhs_priv *priv, int enable)
 	usbhs_bset(priv, SYSCFG, mask, enable ? val : 0);
 }
 
+void usbhs_sys_set_test_mode(struct usbhs_priv *priv, u16 mode)
+{
+	usbhs_write(priv, TESTMODE, mode);
+}
+
 /*
  *		frame functions
  */
@@ -229,7 +224,7 @@ static void usbhsc_bus_init(struct usbhs_priv *priv)
 /*
  *		device configuration
  */
-int usbhs_set_device_speed(struct usbhs_priv *priv, int devnum,
+int usbhs_set_device_config(struct usbhs_priv *priv, int devnum,
 			   u16 upphub, u16 hubport, u16 speed)
 {
 	struct device *dev = usbhs_priv_to_dev(priv);
@@ -301,18 +296,25 @@ static u32 usbhsc_default_pipe_type[] = {
  */
 static void usbhsc_power_ctrl(struct usbhs_priv *priv, int enable)
 {
+	struct platform_device *pdev = usbhs_priv_to_pdev(priv);
 	struct device *dev = usbhs_priv_to_dev(priv);
 
 	if (enable) {
 		/* enable PM */
 		pm_runtime_get_sync(dev);
 
+		/* enable platform power */
+		usbhs_platform_call(priv, power_ctrl, pdev, priv->base, enable);
+
 		/* USB on */
 		usbhs_sys_clock_ctrl(priv, enable);
 	} else {
 		/* USB off */
 		usbhs_sys_clock_ctrl(priv, enable);
 
+		/* disable platform power */
+		usbhs_platform_call(priv, power_ctrl, pdev, priv->base, enable);
+
 		/* disable PM */
 		pm_runtime_put_sync(dev);
 	}
@@ -388,7 +390,7 @@ static void usbhsc_notify_hotplug(struct work_struct *work)
 	usbhsc_hotplug(priv);
 }
 
-int usbhsc_drvcllbck_notify_hotplug(struct platform_device *pdev)
+static int usbhsc_drvcllbck_notify_hotplug(struct platform_device *pdev)
 {
 	struct usbhs_priv *priv = usbhs_pdev_to_priv(pdev);
 	int delay = usbhs_get_dparam(priv, detection_delay);
@@ -398,7 +400,8 @@ int usbhsc_drvcllbck_notify_hotplug(struct platform_device *pdev)
 	 * To make sure safety context,
 	 * use workqueue for usbhs_notify_hotplug
 	 */
-	schedule_delayed_work(&priv->notify_hotplug_work, delay);
+	schedule_delayed_work(&priv->notify_hotplug_work,
+			      msecs_to_jiffies(delay));
 	return 0;
 }
 
@@ -637,18 +640,7 @@ static struct platform_driver renesas_usbhs_driver = {
 	.remove		= __devexit_p(usbhs_remove),
 };
 
-static int __init usbhs_init(void)
-{
-	return platform_driver_register(&renesas_usbhs_driver);
-}
-
-static void __exit usbhs_exit(void)
-{
-	platform_driver_unregister(&renesas_usbhs_driver);
-}
-
-module_init(usbhs_init);
-module_exit(usbhs_exit);
+module_platform_driver(renesas_usbhs_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Renesas USB driver");
diff --git a/drivers/usb/renesas_usbhs/common.h b/drivers/usb/renesas_usbhs/common.h
index 8729da5c3be6..d79b3e27db95 100644
--- a/drivers/usb/renesas_usbhs/common.h
+++ b/drivers/usb/renesas_usbhs/common.h
@@ -33,6 +33,7 @@ struct usbhs_priv;
 #define SYSCFG		0x0000
 #define BUSWAIT		0x0002
 #define DVSTCTR		0x0008
+#define TESTMODE	0x000C
 #define CFIFO		0x0014
 #define CFIFOSEL	0x0020
 #define CFIFOCTR	0x0022
@@ -275,19 +276,15 @@ u16 usbhs_read(struct usbhs_priv *priv, u32 reg);
 void usbhs_write(struct usbhs_priv *priv, u32 reg, u16 data);
 void usbhs_bset(struct usbhs_priv *priv, u32 reg, u16 mask, u16 data);
 
-int usbhsc_drvcllbck_notify_hotplug(struct platform_device *pdev);
-
 #define usbhs_lock(p, f) spin_lock_irqsave(usbhs_priv_to_lock(p), f)
 #define usbhs_unlock(p, f) spin_unlock_irqrestore(usbhs_priv_to_lock(p), f)
 
 /*
  * sysconfig
  */
-void usbhs_sys_clock_ctrl(struct usbhs_priv *priv, int enable);
-void usbhs_sys_hispeed_ctrl(struct usbhs_priv *priv, int enable);
-void usbhs_sys_usb_ctrl(struct usbhs_priv *priv, int enable);
 void usbhs_sys_host_ctrl(struct usbhs_priv *priv, int enable);
 void usbhs_sys_function_ctrl(struct usbhs_priv *priv, int enable);
+void usbhs_sys_set_test_mode(struct usbhs_priv *priv, u16 mode);
 
 /*
  * usb request
@@ -311,7 +308,7 @@ int usbhs_frame_get_num(struct usbhs_priv *priv);
 /*
  * device config
  */
-int usbhs_set_device_speed(struct usbhs_priv *priv, int devnum, u16 upphub,
+int usbhs_set_device_config(struct usbhs_priv *priv, int devnum, u16 upphub,
 			   u16 hubport, u16 speed);
 
 /*
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index ffdf5d15085e..b51fcd80d244 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -56,7 +56,7 @@ static struct usbhs_pkt_handle usbhsf_null_handler = {
 void usbhs_pkt_push(struct usbhs_pipe *pipe, struct usbhs_pkt *pkt,
 		    void (*done)(struct usbhs_priv *priv,
 				 struct usbhs_pkt *pkt),
-		    void *buf, int len, int zero)
+		    void *buf, int len, int zero, int sequence)
 {
 	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
 	struct device *dev = usbhs_priv_to_dev(priv);
@@ -90,6 +90,7 @@ void usbhs_pkt_push(struct usbhs_pipe *pipe, struct usbhs_pkt *pkt,
 	pkt->zero	= zero;
 	pkt->actual	= 0;
 	pkt->done	= done;
+	pkt->sequence	= sequence;
 
 	usbhs_unlock(priv, flags);
 	/********************  spin unlock ******************/
@@ -481,6 +482,9 @@ static int usbhsf_pio_try_push(struct usbhs_pkt *pkt, int *is_done)
 	int i, ret, len;
 	int is_short;
 
+	usbhs_pipe_data_sequence(pipe, pkt->sequence);
+	pkt->sequence = -1; /* -1 sequence will be ignored */
+
 	ret = usbhsf_fifo_select(pipe, fifo, 1);
 	if (ret < 0)
 		return 0;
@@ -584,6 +588,8 @@ static int usbhsf_prepare_pop(struct usbhs_pkt *pkt, int *is_done)
 	/*
 	 * pipe enable to prepare packet receive
 	 */
+	usbhs_pipe_data_sequence(pipe, pkt->sequence);
+	pkt->sequence = -1; /* -1 sequence will be ignored */
 
 	usbhs_pipe_enable(pipe);
 	usbhsf_rx_irq_ctrl(pipe, 1);
@@ -641,6 +647,7 @@ static int usbhsf_pio_try_pop(struct usbhs_pkt *pkt, int *is_done)
 	 * "Operation" - "FIFO Buffer Memory" - "FIFO Port Function"
 	 */
 	if (0 == rcv_len) {
+		pkt->zero = 1;
 		usbhsf_fifo_clear(pipe, fifo);
 		goto usbhs_fifo_read_end;
 	}
diff --git a/drivers/usb/renesas_usbhs/fifo.h b/drivers/usb/renesas_usbhs/fifo.h
index 32a7b246b28d..f68609c0f489 100644
--- a/drivers/usb/renesas_usbhs/fifo.h
+++ b/drivers/usb/renesas_usbhs/fifo.h
@@ -59,6 +59,7 @@ struct usbhs_pkt {
 	int trans;
 	int actual;
 	int zero;
+	int sequence;
 };
 
 struct usbhs_pkt_handle {
@@ -95,7 +96,7 @@ void usbhs_pkt_init(struct usbhs_pkt *pkt);
 void usbhs_pkt_push(struct usbhs_pipe *pipe, struct usbhs_pkt *pkt,
 		    void (*done)(struct usbhs_priv *priv,
 				 struct usbhs_pkt *pkt),
-		    void *buf, int len, int zero);
+		    void *buf, int len, int zero, int sequence);
 struct usbhs_pkt *usbhs_pkt_pop(struct usbhs_pipe *pipe, struct usbhs_pkt *pkt);
 void usbhs_pkt_start(struct usbhs_pipe *pipe);
 
diff --git a/drivers/usb/renesas_usbhs/mod.c b/drivers/usb/renesas_usbhs/mod.c
index ad96a3896729..1b97fb12694b 100644
--- a/drivers/usb/renesas_usbhs/mod.c
+++ b/drivers/usb/renesas_usbhs/mod.c
@@ -50,7 +50,9 @@ static int usbhsm_autonomy_irq_vbus(struct usbhs_priv *priv,
 {
 	struct platform_device *pdev = usbhs_priv_to_pdev(priv);
 
-	return usbhsc_drvcllbck_notify_hotplug(pdev);
+	renesas_usbhs_call_notify_hotplug(pdev);
+
+	return 0;
 }
 
 void usbhs_mod_autonomy_mode(struct usbhs_priv *priv)
diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index 7f4e80338570..528691d5f3e2 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -14,6 +14,7 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  *
  */
+#include <linux/delay.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
 #include <linux/module.h>
@@ -44,7 +45,6 @@ struct usbhsg_uep {
 struct usbhsg_gpriv {
 	struct usb_gadget	 gadget;
 	struct usbhs_mod	 mod;
-	struct list_head	 link;
 
 	struct usbhsg_uep	*uep;
 	int			 uep_size;
@@ -114,16 +114,6 @@ struct usbhsg_recip_handle {
 #define usbhsg_status_clr(gp, b) (gp->status &= ~b)
 #define usbhsg_status_has(gp, b) (gp->status &   b)
 
-/* controller */
-LIST_HEAD(the_controller_link);
-
-#define usbhsg_for_each_controller(gpriv)\
-	list_for_each_entry(gpriv, &the_controller_link, link)
-#define usbhsg_controller_register(gpriv)\
-	list_add_tail(&(gpriv)->link, &the_controller_link)
-#define usbhsg_controller_unregister(gpriv)\
-	list_del_init(&(gpriv)->link)
-
 /*
  *		queue push/pop
  */
@@ -164,7 +154,7 @@ static void usbhsg_queue_push(struct usbhsg_uep *uep,
 	req->actual = 0;
 	req->status = -EINPROGRESS;
 	usbhs_pkt_push(pipe, pkt, usbhsg_queue_done,
-		       req->buf, req->length, req->zero);
+		       req->buf, req->length, req->zero, -1);
 	usbhs_pkt_start(pipe);
 
 	dev_dbg(dev, "pipe %d : queue push (%d)\n",
@@ -195,7 +185,7 @@ static int usbhsg_dma_map(struct device *dev,
 	}
 
 	if (dma_mapping_error(dev, pkt->dma)) {
-		dev_err(dev, "dma mapping error %x\n", pkt->dma);
+		dev_err(dev, "dma mapping error %llx\n", (u64)pkt->dma);
 		return -EIO;
 	}
 
@@ -271,6 +261,8 @@ static int usbhsg_recip_handler_std_clear_endpoint(struct usbhs_priv *priv,
 
 	usbhsg_recip_handler_std_control_done(priv, uep, ctrl);
 
+	usbhs_pkt_start(pipe);
+
 	return 0;
 }
 
@@ -282,6 +274,145 @@ struct usbhsg_recip_handle req_clear_feature = {
 };
 
 /*
+ *		USB_TYPE_STANDARD / set feature functions
+ */
+static int usbhsg_recip_handler_std_set_device(struct usbhs_priv *priv,
+						 struct usbhsg_uep *uep,
+						 struct usb_ctrlrequest *ctrl)
+{
+	switch (le16_to_cpu(ctrl->wValue)) {
+	case USB_DEVICE_TEST_MODE:
+		usbhsg_recip_handler_std_control_done(priv, uep, ctrl);
+		udelay(100);
+		usbhs_sys_set_test_mode(priv, le16_to_cpu(ctrl->wIndex >> 8));
+		break;
+	default:
+		usbhsg_recip_handler_std_control_done(priv, uep, ctrl);
+		break;
+	}
+
+	return 0;
+}
+
+static int usbhsg_recip_handler_std_set_endpoint(struct usbhs_priv *priv,
+						 struct usbhsg_uep *uep,
+						 struct usb_ctrlrequest *ctrl)
+{
+	struct usbhs_pipe *pipe = usbhsg_uep_to_pipe(uep);
+
+	usbhs_pipe_stall(pipe);
+
+	usbhsg_recip_handler_std_control_done(priv, uep, ctrl);
+
+	return 0;
+}
+
+struct usbhsg_recip_handle req_set_feature = {
+	.name		= "set feature",
+	.device		= usbhsg_recip_handler_std_set_device,
+	.interface	= usbhsg_recip_handler_std_control_done,
+	.endpoint	= usbhsg_recip_handler_std_set_endpoint,
+};
+
+/*
+ *		USB_TYPE_STANDARD / get status functions
+ */
+static void __usbhsg_recip_send_complete(struct usb_ep *ep,
+					 struct usb_request *req)
+{
+	struct usbhsg_request *ureq = usbhsg_req_to_ureq(req);
+
+	/* free allocated recip-buffer/usb_request */
+	kfree(ureq->pkt.buf);
+	usb_ep_free_request(ep, req);
+}
+
+static void __usbhsg_recip_send_status(struct usbhsg_gpriv *gpriv,
+				       unsigned short status)
+{
+	struct usbhsg_uep *dcp = usbhsg_gpriv_to_dcp(gpriv);
+	struct usbhs_pipe *pipe = usbhsg_uep_to_pipe(dcp);
+	struct device *dev = usbhsg_gpriv_to_dev(gpriv);
+	struct usb_request *req;
+	unsigned short *buf;
+
+	/* alloc new usb_request for recip */
+	req = usb_ep_alloc_request(&dcp->ep, GFP_ATOMIC);
+	if (!req) {
+		dev_err(dev, "recip request allocation fail\n");
+		return;
+	}
+
+	/* alloc recip data buffer */
+	buf = kmalloc(sizeof(*buf), GFP_ATOMIC);
+	if (!buf) {
+		usb_ep_free_request(&dcp->ep, req);
+		dev_err(dev, "recip data allocation fail\n");
+		return;
+	}
+
+	/* recip data is status */
+	*buf = cpu_to_le16(status);
+
+	/* allocated usb_request/buffer will be freed */
+	req->complete	= __usbhsg_recip_send_complete;
+	req->buf	= buf;
+	req->length	= sizeof(*buf);
+	req->zero	= 0;
+
+	/* push packet */
+	pipe->handler = &usbhs_fifo_pio_push_handler;
+	usbhsg_queue_push(dcp, usbhsg_req_to_ureq(req));
+}
+
+static int usbhsg_recip_handler_std_get_device(struct usbhs_priv *priv,
+					       struct usbhsg_uep *uep,
+					       struct usb_ctrlrequest *ctrl)
+{
+	struct usbhsg_gpriv *gpriv = usbhsg_uep_to_gpriv(uep);
+	unsigned short status = 1 << USB_DEVICE_SELF_POWERED;
+
+	__usbhsg_recip_send_status(gpriv, status);
+
+	return 0;
+}
+
+static int usbhsg_recip_handler_std_get_interface(struct usbhs_priv *priv,
+						  struct usbhsg_uep *uep,
+						  struct usb_ctrlrequest *ctrl)
+{
+	struct usbhsg_gpriv *gpriv = usbhsg_uep_to_gpriv(uep);
+	unsigned short status = 0;
+
+	__usbhsg_recip_send_status(gpriv, status);
+
+	return 0;
+}
+
+static int usbhsg_recip_handler_std_get_endpoint(struct usbhs_priv *priv,
+						 struct usbhsg_uep *uep,
+						 struct usb_ctrlrequest *ctrl)
+{
+	struct usbhsg_gpriv *gpriv = usbhsg_uep_to_gpriv(uep);
+	struct usbhs_pipe *pipe = usbhsg_uep_to_pipe(uep);
+	unsigned short status = 0;
+
+	if (usbhs_pipe_is_stall(pipe))
+		status = 1 << USB_ENDPOINT_HALT;
+
+	__usbhsg_recip_send_status(gpriv, status);
+
+	return 0;
+}
+
+struct usbhsg_recip_handle req_get_status = {
+	.name		= "get status",
+	.device		= usbhsg_recip_handler_std_get_device,
+	.interface	= usbhsg_recip_handler_std_get_interface,
+	.endpoint	= usbhsg_recip_handler_std_get_endpoint,
+};
+
+/*
  *		USB_TYPE handler
  */
 static int usbhsg_recip_run_handle(struct usbhs_priv *priv,
@@ -303,8 +434,7 @@ static int usbhsg_recip_run_handle(struct usbhs_priv *priv,
 	pipe = usbhsg_uep_to_pipe(uep);
 	if (!pipe) {
 		dev_err(dev, "wrong recip request\n");
-		ret = -EINVAL;
-		goto usbhsg_recip_run_handle_end;
+		return -EINVAL;
 	}
 
 	switch (recip) {
@@ -327,20 +457,10 @@ static int usbhsg_recip_run_handle(struct usbhs_priv *priv,
 	}
 
 	if (func) {
-		unsigned long flags;
-
 		dev_dbg(dev, "%s (pipe %d :%s)\n", handler->name, nth, msg);
-
-		/********************  spin lock ********************/
-		usbhs_lock(priv, flags);
 		ret = func(priv, uep, ctrl);
-		usbhs_unlock(priv, flags);
-		/********************  spin unlock ******************/
 	}
 
-usbhsg_recip_run_handle_end:
-	usbhs_pkt_start(pipe);
-
 	return ret;
 }
 
@@ -412,6 +532,12 @@ static int usbhsg_irq_ctrl_stage(struct usbhs_priv *priv,
 		case USB_REQ_CLEAR_FEATURE:
 			recip_handler = &req_clear_feature;
 			break;
+		case USB_REQ_SET_FEATURE:
+			recip_handler = &req_set_feature;
+			break;
+		case USB_REQ_GET_STATUS:
+			recip_handler = &req_get_status;
+			break;
 		}
 	}
 
@@ -439,14 +565,16 @@ static int usbhsg_pipe_disable(struct usbhsg_uep *uep)
 	struct usbhs_pipe *pipe = usbhsg_uep_to_pipe(uep);
 	struct usbhs_pkt *pkt;
 
-	usbhs_pipe_disable(pipe);
-
 	while (1) {
 		pkt = usbhs_pkt_pop(pipe, NULL);
 		if (!pkt)
 			break;
+
+		usbhsg_queue_pop(uep, usbhsg_pkt_to_ureq(pkt), -ECONNRESET);
 	}
 
+	usbhs_pipe_disable(pipe);
+
 	return 0;
 }
 
@@ -681,9 +809,7 @@ static int usbhsg_try_start(struct usbhs_priv *priv, u32 status)
 	 * - function
 	 * - usb module
 	 */
-	usbhs_sys_hispeed_ctrl(priv, 1);
 	usbhs_sys_function_ctrl(priv, 1);
-	usbhs_sys_usb_ctrl(priv, 1);
 
 	/*
 	 * enable irq callback
@@ -731,9 +857,8 @@ static int usbhsg_try_stop(struct usbhs_priv *priv, u32 status)
 	gpriv->gadget.speed = USB_SPEED_UNKNOWN;
 
 	/* disable sys */
-	usbhs_sys_hispeed_ctrl(priv, 0);
+	usbhs_sys_set_test_mode(priv, 0);
 	usbhs_sys_function_ctrl(priv, 0);
-	usbhs_sys_usb_ctrl(priv, 0);
 
 	usbhsg_pipe_disable(dcp);
 
@@ -755,7 +880,7 @@ static int usbhsg_gadget_start(struct usb_gadget *gadget,
 
 	if (!driver		||
 	    !driver->setup	||
-	    driver->speed < USB_SPEED_FULL)
+	    driver->max_speed < USB_SPEED_FULL)
 		return -EINVAL;
 
 	/* first hook up the driver ... */
@@ -866,7 +991,7 @@ int usbhs_mod_gadget_probe(struct usbhs_priv *priv)
 	gpriv->gadget.dev.parent	= dev;
 	gpriv->gadget.name		= "renesas_usbhs_udc";
 	gpriv->gadget.ops		= &usbhsg_gadget_ops;
-	gpriv->gadget.is_dualspeed	= 1;
+	gpriv->gadget.max_speed		= USB_SPEED_HIGH;
 	ret = device_register(&gpriv->gadget.dev);
 	if (ret < 0)
 		goto err_add_udc;
@@ -896,8 +1021,6 @@ int usbhs_mod_gadget_probe(struct usbhs_priv *priv)
 		}
 	}
 
-	usbhsg_controller_register(gpriv);
-
 	ret = usb_add_gadget_udc(dev, &gpriv->gadget);
 	if (ret)
 		goto err_register;
@@ -926,8 +1049,6 @@ void usbhs_mod_gadget_remove(struct usbhs_priv *priv)
 
 	device_unregister(&gpriv->gadget.dev);
 
-	usbhsg_controller_unregister(gpriv);
-
 	kfree(gpriv->uep);
 	kfree(gpriv);
 }
diff --git a/drivers/usb/renesas_usbhs/mod_host.c b/drivers/usb/renesas_usbhs/mod_host.c
index 7955de589951..1834cf50888c 100644
--- a/drivers/usb/renesas_usbhs/mod_host.c
+++ b/drivers/usb/renesas_usbhs/mod_host.c
@@ -45,36 +45,34 @@
  *
  * +--------+					pipes are reused for each uep.
  * | udev 1 |-+- [uep 0 (dcp) ] --+		pipe will be switched when
- * +--------+ |			  |		target device was changed
+ * +--------+ |			  |		other device requested
  *	      +- [uep 1 (bulk)]	--|---+		   +--------------+
  *	      |			  +--------------> | pipe0 (dcp)  |
- *	      +- [uep 2 (bulk)]	--|---|---+	   +--------------+
- *				  |   |	  |	   | pipe1 (isoc) |
- * +--------+			  |   |	  |	   +--------------+
- * | udev 2 |-+- [uep 0 (dcp) ]	--+   +-- |------> | pipe2 (bulk) |
- * +--------+ |			  |   |	  |	   +--------------+
- *	      +- [uep 1 (int) ]	--|-+ |	  +------> | pipe3 (bulk) |
- *				  | | |	  |	   +--------------+
- * +--------+			  | +-|---|------> | pipe4 (int)  |
- * | udev 3 |-+- [uep 0 (dcp) ]	--+   |	  |	   +--------------+
- * +--------+ |			      |	  |	   | ....	  |
- *	      +- [uep 1 (bulk)]	------+	  |	   | ....	  |
+ *	      +- [uep 2 (bulk)]	-@    |		   +--------------+
+ *				      |		   | pipe1 (isoc) |
+ * +--------+			      |		   +--------------+
+ * | udev 2 |-+- [uep 0 (dcp) ]	-@    +----------> | pipe2 (bulk) |
+ * +--------+ |					   +--------------+
+ *	      +- [uep 1 (int) ]	----+	  +------> | pipe3 (bulk) |
+ *				    |	  |	   +--------------+
+ * +--------+			    +-----|------> | pipe4 (int)  |
+ * | udev 3 |-+- [uep 0 (dcp) ]	-@	  |	   +--------------+
+ * +--------+ |				  |	   | ....	  |
+ *	      +- [uep 1 (bulk)]	-@	  |	   | ....	  |
  *	      |				  |
  *	      +- [uep 2 (bulk)]-----------+
+ *
+ * @ :	uep requested free pipe, but all have been used.
+ *	now it is waiting for free pipe
  */
 
 
 /*
  *		struct
  */
-struct usbhsh_pipe_info {
-	unsigned int		usr_cnt; /* see usbhsh_endpoint_alloc() */
-};
-
 struct usbhsh_request {
 	struct urb		*urb;
 	struct usbhs_pkt	pkt;
-	struct list_head	ureq_link; /* see hpriv :: ureq_link_xxx */
 };
 
 struct usbhsh_device {
@@ -83,11 +81,10 @@ struct usbhsh_device {
 };
 
 struct usbhsh_ep {
-	struct usbhs_pipe	*pipe;
+	struct usbhs_pipe	*pipe;   /* attached pipe */
 	struct usbhsh_device	*udev;   /* attached udev */
+	struct usb_host_endpoint *ep;
 	struct list_head	ep_list; /* list to usbhsh_device */
-
-	int maxp;
 };
 
 #define USBHSH_DEVICE_MAX	10 /* see DEVADDn / DCPMAXP / PIPEMAXP */
@@ -98,16 +95,9 @@ struct usbhsh_hpriv {
 
 	struct usbhsh_device	udev[USBHSH_DEVICE_MAX];
 
-	struct usbhsh_pipe_info	*pipe_info;
-	int			 pipe_size;
-
 	u32	port_stat;	/* USB_PORT_STAT_xxx */
 
 	struct completion	setup_ack_done;
-
-	/* see usbhsh_req_alloc/free */
-	struct list_head	ureq_link_active;
-	struct list_head	ureq_link_free;
 };
 
 
@@ -119,17 +109,6 @@ static const char usbhsh_hcd_name[] = "renesas_usbhs host";
 #define usbhsh_priv_to_hpriv(priv) \
 	container_of(usbhs_mod_get(priv, USBHS_HOST), struct usbhsh_hpriv, mod)
 
-#define __usbhsh_for_each_hpipe(start, pos, h, i)	\
-	for (i = start, pos = (h)->hpipe + i;		\
-	     i < (h)->hpipe_size;			\
-	     i++, pos = (h)->hpipe + i)
-
-#define usbhsh_for_each_hpipe(pos, hpriv, i)	\
-	__usbhsh_for_each_hpipe(1, pos, hpriv, i)
-
-#define usbhsh_for_each_hpipe_with_dcp(pos, hpriv, i)	\
-	__usbhsh_for_each_hpipe(0, pos, hpriv, i)
-
 #define __usbhsh_for_each_udev(start, pos, h, i)	\
 	for (i = start, pos = (h)->udev + i;		\
 	     i < USBHSH_DEVICE_MAX;			\
@@ -152,15 +131,20 @@ static const char usbhsh_hcd_name[] = "renesas_usbhs host";
 #define usbhsh_ep_to_uep(u)	((u)->hcpriv)
 #define usbhsh_uep_to_pipe(u)	((u)->pipe)
 #define usbhsh_uep_to_udev(u)	((u)->udev)
+#define usbhsh_uep_to_ep(u)	((u)->ep)
+
 #define usbhsh_urb_to_ureq(u)	((u)->hcpriv)
 #define usbhsh_urb_to_usbv(u)	((u)->dev)
 
 #define usbhsh_usbv_to_udev(d)	dev_get_drvdata(&(d)->dev)
 
 #define usbhsh_udev_to_usbv(h)	((h)->usbv)
+#define usbhsh_udev_is_used(h)	usbhsh_udev_to_usbv(h)
 
-#define usbhsh_pipe_info(p)	((p)->mod_private)
+#define usbhsh_pipe_to_uep(p)	((p)->mod_private)
 
+#define usbhsh_device_parent(d)		(usbhsh_usbv_to_udev((d)->usbv->parent))
+#define usbhsh_device_hubport(d)	((d)->usbv->portnum)
 #define usbhsh_device_number(h, d)	((int)((d) - (h)->udev))
 #define usbhsh_device_nth(h, d)		((h)->udev + d)
 #define usbhsh_device0(h)		usbhsh_device_nth(h, 0)
@@ -170,38 +154,13 @@ static const char usbhsh_hcd_name[] = "renesas_usbhs host";
 #define usbhsh_port_stat_clear(h, s)	((h)->port_stat &= ~(s))
 #define usbhsh_port_stat_get(h)		((h)->port_stat)
 
-#define usbhsh_pkt_to_req(p)	\
+#define usbhsh_pkt_to_ureq(p)	\
 	container_of((void *)p, struct usbhsh_request, pkt)
 
 /*
  *		req alloc/free
  */
-static void usbhsh_req_list_init(struct usbhsh_hpriv *hpriv)
-{
-	INIT_LIST_HEAD(&hpriv->ureq_link_active);
-	INIT_LIST_HEAD(&hpriv->ureq_link_free);
-}
-
-static void usbhsh_req_list_quit(struct usbhsh_hpriv *hpriv)
-{
-	struct usb_hcd *hcd = usbhsh_hpriv_to_hcd(hpriv);
-	struct device *dev = usbhsh_hcd_to_dev(hcd);
-	struct usbhsh_request *ureq, *next;
-
-	/* kfree all active ureq */
-	list_for_each_entry_safe(ureq, next,
-				 &hpriv->ureq_link_active,
-				 ureq_link) {
-		dev_err(dev, "active ureq (%p) is force freed\n", ureq);
-		kfree(ureq);
-	}
-
-	/* kfree all free ureq */
-	list_for_each_entry_safe(ureq, next, &hpriv->ureq_link_free, ureq_link)
-		kfree(ureq);
-}
-
-static struct usbhsh_request *usbhsh_req_alloc(struct usbhsh_hpriv *hpriv,
+static struct usbhsh_request *usbhsh_ureq_alloc(struct usbhsh_hpriv *hpriv,
 					       struct urb *urb,
 					       gfp_t mem_flags)
 {
@@ -209,270 +168,460 @@ static struct usbhsh_request *usbhsh_req_alloc(struct usbhsh_hpriv *hpriv,
 	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
 	struct device *dev = usbhs_priv_to_dev(priv);
 
-	if (list_empty(&hpriv->ureq_link_free)) {
-		/*
-		 * create new one if there is no free ureq
-		 */
-		ureq = kzalloc(sizeof(struct usbhsh_request), mem_flags);
-		if (ureq)
-			INIT_LIST_HEAD(&ureq->ureq_link);
-	} else {
-		/*
-		 * reuse "free" ureq if exist
-		 */
-		ureq = list_entry(hpriv->ureq_link_free.next,
-				  struct usbhsh_request,
-				  ureq_link);
-		if (ureq)
-			list_del_init(&ureq->ureq_link);
-	}
-
+	ureq = kzalloc(sizeof(struct usbhsh_request), mem_flags);
 	if (!ureq) {
 		dev_err(dev, "ureq alloc fail\n");
 		return NULL;
 	}
 
 	usbhs_pkt_init(&ureq->pkt);
-
-	/*
-	 * push it to "active" list
-	 */
-	list_add_tail(&ureq->ureq_link, &hpriv->ureq_link_active);
 	ureq->urb = urb;
+	usbhsh_urb_to_ureq(urb) = ureq;
 
 	return ureq;
 }
 
-static void usbhsh_req_free(struct usbhsh_hpriv *hpriv,
+static void usbhsh_ureq_free(struct usbhsh_hpriv *hpriv,
 			    struct usbhsh_request *ureq)
 {
-	struct usbhs_pkt *pkt = &ureq->pkt;
+	usbhsh_urb_to_ureq(ureq->urb) = NULL;
+	ureq->urb = NULL;
 
-	usbhs_pkt_init(pkt);
+	kfree(ureq);
+}
 
+/*
+ *		status
+ */
+static int usbhsh_is_running(struct usbhsh_hpriv *hpriv)
+{
 	/*
-	 * removed from "active" list,
-	 * and push it to "free" list
+	 * we can decide some device is attached or not
+	 * by checking mod.irq_attch
+	 * see
+	 *	usbhsh_irq_attch()
+	 *	usbhsh_irq_dtch()
 	 */
-	ureq->urb = NULL;
-	list_del_init(&ureq->ureq_link);
-	list_add_tail(&ureq->ureq_link, &hpriv->ureq_link_free);
+	return (hpriv->mod.irq_attch == NULL);
 }
 
 /*
- *		device control
+ *		pipe control
  */
-
-static int usbhsh_device_has_endpoint(struct usbhsh_device *udev)
+static void usbhsh_endpoint_sequence_save(struct usbhsh_hpriv *hpriv,
+					  struct urb *urb,
+					  struct usbhs_pkt *pkt)
 {
-	return !list_empty(&udev->ep_list_head);
-}
+	int len = urb->actual_length;
+	int maxp = usb_endpoint_maxp(&urb->ep->desc);
+	int t = 0;
 
-static struct usbhsh_device *usbhsh_device_alloc(struct usbhsh_hpriv *hpriv,
-						 struct urb *urb)
-{
-	struct usbhsh_device *udev = NULL;
-	struct usb_hcd *hcd = usbhsh_hpriv_to_hcd(hpriv);
-	struct device *dev = usbhsh_hcd_to_dev(hcd);
-	struct usb_device *usbv = usbhsh_urb_to_usbv(urb);
-	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
-	int i;
+	/* DCP is out of sequence control */
+	if (usb_pipecontrol(urb->pipe))
+		return;
 
 	/*
-	 * device 0
+	 * renesas_usbhs pipe has a limitation in a number.
+	 * So, driver should re-use the limited pipe for each device/endpoint.
+	 * DATA0/1 sequence should be saved for it.
+	 * see [image of mod_host]
+	 *     [HARDWARE LIMITATION]
 	 */
-	if (0 == usb_pipedevice(urb->pipe)) {
-		udev = usbhsh_device0(hpriv);
-		goto usbhsh_device_find;
-	}
 
 	/*
-	 * find unused device
+	 * next sequence depends on actual_length
+	 *
+	 * ex) actual_length = 1147, maxp = 512
+	 * data0 : 512
+	 * data1 : 512
+	 * data0 : 123
+	 * data1 is the next sequence
 	 */
-	usbhsh_for_each_udev(udev, hpriv, i) {
-		if (usbhsh_udev_to_usbv(udev))
-			continue;
-		goto usbhsh_device_find;
+	t = len / maxp;
+	if (len % maxp)
+		t++;
+	if (pkt->zero)
+		t++;
+	t %= 2;
+
+	if (t)
+		usb_dotoggle(urb->dev,
+			     usb_pipeendpoint(urb->pipe),
+			     usb_pipeout(urb->pipe));
+}
+
+static struct usbhsh_device *usbhsh_device_get(struct usbhsh_hpriv *hpriv,
+					       struct urb *urb);
+
+static int usbhsh_pipe_attach(struct usbhsh_hpriv *hpriv,
+			      struct urb *urb)
+{
+	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
+	struct usbhsh_ep *uep = usbhsh_ep_to_uep(urb->ep);
+	struct usbhsh_device *udev = usbhsh_device_get(hpriv, urb);
+	struct usbhs_pipe *pipe;
+	struct usb_endpoint_descriptor *desc = &urb->ep->desc;
+	struct device *dev = usbhs_priv_to_dev(priv);
+	unsigned long flags;
+	int dir_in_req = !!usb_pipein(urb->pipe);
+	int is_dcp = usb_endpoint_xfer_control(desc);
+	int i, dir_in;
+	int ret = -EBUSY;
+
+	/********************  spin lock ********************/
+	usbhs_lock(priv, flags);
+
+	if (unlikely(usbhsh_uep_to_pipe(uep))) {
+		dev_err(dev, "uep already has pipe\n");
+		goto usbhsh_pipe_attach_done;
 	}
 
-	dev_err(dev, "no free usbhsh_device\n");
+	usbhs_for_each_pipe_with_dcp(pipe, priv, i) {
 
-	return NULL;
+		/* check pipe type */
+		if (!usbhs_pipe_type_is(pipe, usb_endpoint_type(desc)))
+			continue;
 
-usbhsh_device_find:
-	if (usbhsh_device_has_endpoint(udev))
-		dev_warn(dev, "udev have old endpoint\n");
+		/* check pipe direction if normal pipe */
+		if (!is_dcp) {
+			dir_in = !!usbhs_pipe_is_dir_in(pipe);
+			if (0 != (dir_in - dir_in_req))
+				continue;
+		}
 
-	/* uep will be attached */
-	INIT_LIST_HEAD(&udev->ep_list_head);
+		/* check pipe is free */
+		if (usbhsh_pipe_to_uep(pipe))
+			continue;
 
-	/*
-	 * usbhsh_usbv_to_udev()
-	 * usbhsh_udev_to_usbv()
-	 * will be enable
-	 */
-	dev_set_drvdata(&usbv->dev, udev);
-	udev->usbv = usbv;
+		/*
+		 * attach pipe to uep
+		 *
+		 * usbhs_pipe_config_update() should be called after
+		 * usbhs_set_device_config()
+		 * see
+		 *  DCPMAXP/PIPEMAXP
+		 */
+		usbhsh_uep_to_pipe(uep)		= pipe;
+		usbhsh_pipe_to_uep(pipe)	= uep;
 
-	/* set device config */
-	usbhs_set_device_speed(priv,
-			       usbhsh_device_number(hpriv, udev),
-			       usbhsh_device_number(hpriv, udev),
-			       0, /* FIXME no parent */
-			       usbv->speed);
+		usbhs_pipe_config_update(pipe,
+					 usbhsh_device_number(hpriv, udev),
+					 usb_endpoint_num(desc),
+					 usb_endpoint_maxp(desc));
 
-	dev_dbg(dev, "%s [%d](%p)\n", __func__,
-		usbhsh_device_number(hpriv, udev), udev);
+		dev_dbg(dev, "%s [%d-%d(%s:%s)]\n", __func__,
+			usbhsh_device_number(hpriv, udev),
+			usb_endpoint_num(desc),
+			usbhs_pipe_name(pipe),
+			dir_in_req ? "in" : "out");
 
-	return udev;
+		ret = 0;
+		break;
+	}
+
+usbhsh_pipe_attach_done:
+	usbhs_unlock(priv, flags);
+	/********************  spin unlock ******************/
+
+	return ret;
 }
 
-static void usbhsh_device_free(struct usbhsh_hpriv *hpriv,
-			       struct usbhsh_device *udev)
+static void usbhsh_pipe_detach(struct usbhsh_hpriv *hpriv,
+			       struct usbhsh_ep *uep)
 {
-	struct usb_hcd *hcd = usbhsh_hpriv_to_hcd(hpriv);
-	struct device *dev = usbhsh_hcd_to_dev(hcd);
-	struct usb_device *usbv = usbhsh_udev_to_usbv(udev);
+	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
+	struct usbhs_pipe *pipe;
+	struct device *dev = usbhs_priv_to_dev(priv);
+	unsigned long flags;
 
-	dev_dbg(dev, "%s [%d](%p)\n", __func__,
-		usbhsh_device_number(hpriv, udev), udev);
+	/********************  spin lock ********************/
+	usbhs_lock(priv, flags);
 
-	if (usbhsh_device_has_endpoint(udev))
-		dev_warn(dev, "udev still have endpoint\n");
+	pipe = usbhsh_uep_to_pipe(uep);
 
-	/*
-	 * usbhsh_usbv_to_udev()
-	 * usbhsh_udev_to_usbv()
-	 * will be disable
-	 */
-	dev_set_drvdata(&usbv->dev, NULL);
-	udev->usbv = NULL;
+	if (unlikely(!pipe)) {
+		dev_err(dev, "uep doens't have pipe\n");
+	} else {
+		struct usb_host_endpoint *ep = usbhsh_uep_to_ep(uep);
+		struct usbhsh_device *udev = usbhsh_uep_to_udev(uep);
+
+		/* detach pipe from uep */
+		usbhsh_uep_to_pipe(uep)		= NULL;
+		usbhsh_pipe_to_uep(pipe)	= NULL;
+
+		dev_dbg(dev, "%s [%d-%d(%s)]\n", __func__,
+			usbhsh_device_number(hpriv, udev),
+			usb_endpoint_num(&ep->desc),
+			usbhs_pipe_name(pipe));
+	}
+
+	usbhs_unlock(priv, flags);
+	/********************  spin unlock ******************/
 }
 
 /*
- *		end-point control
+ *		endpoint control
  */
-struct usbhsh_ep *usbhsh_endpoint_alloc(struct usbhsh_hpriv *hpriv,
-					struct usbhsh_device *udev,
-					struct usb_host_endpoint *ep,
-					int dir_in_req,
-					gfp_t mem_flags)
+static int usbhsh_endpoint_attach(struct usbhsh_hpriv *hpriv,
+				  struct urb *urb,
+				  gfp_t mem_flags)
 {
 	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
-	struct usb_hcd *hcd = usbhsh_hpriv_to_hcd(hpriv);
+	struct usbhsh_device *udev = usbhsh_device_get(hpriv, urb);
+	struct usb_host_endpoint *ep = urb->ep;
 	struct usbhsh_ep *uep;
-	struct usbhsh_pipe_info *info;
-	struct usbhs_pipe *pipe, *best_pipe;
-	struct device *dev = usbhsh_hcd_to_dev(hcd);
+	struct device *dev = usbhs_priv_to_dev(priv);
 	struct usb_endpoint_descriptor *desc = &ep->desc;
-	int type, i, dir_in;
-	unsigned int min_usr;
-
-	dir_in_req = !!dir_in_req;
+	unsigned long flags;
 
 	uep = kzalloc(sizeof(struct usbhsh_ep), mem_flags);
 	if (!uep) {
 		dev_err(dev, "usbhsh_ep alloc fail\n");
-		return NULL;
+		return -ENOMEM;
 	}
 
-	if (usb_endpoint_xfer_control(desc)) {
-		best_pipe = usbhsh_hpriv_to_dcp(hpriv);
-		goto usbhsh_endpoint_alloc_find_pipe;
+	/********************  spin lock ********************/
+	usbhs_lock(priv, flags);
+
+	/*
+	 * init endpoint
+	 */
+	INIT_LIST_HEAD(&uep->ep_list);
+	list_add_tail(&uep->ep_list, &udev->ep_list_head);
+
+	usbhsh_uep_to_udev(uep)	= udev;
+	usbhsh_uep_to_ep(uep)	= ep;
+	usbhsh_ep_to_uep(ep)	= uep;
+
+	usbhs_unlock(priv, flags);
+	/********************  spin unlock ******************/
+
+	dev_dbg(dev, "%s [%d-%d]\n", __func__,
+		usbhsh_device_number(hpriv, udev),
+		usb_endpoint_num(desc));
+
+	return 0;
+}
+
+static void usbhsh_endpoint_detach(struct usbhsh_hpriv *hpriv,
+				   struct usb_host_endpoint *ep)
+{
+	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
+	struct device *dev = usbhs_priv_to_dev(priv);
+	struct usbhsh_ep *uep = usbhsh_ep_to_uep(ep);
+	unsigned long flags;
+
+	if (!uep)
+		return;
+
+	dev_dbg(dev, "%s [%d-%d]\n", __func__,
+		usbhsh_device_number(hpriv, usbhsh_uep_to_udev(uep)),
+		usb_endpoint_num(&ep->desc));
+
+	if (usbhsh_uep_to_pipe(uep))
+		usbhsh_pipe_detach(hpriv, uep);
+
+	/********************  spin lock ********************/
+	usbhs_lock(priv, flags);
+
+	/* remove this endpoint from udev */
+	list_del_init(&uep->ep_list);
+
+	usbhsh_uep_to_udev(uep)	= NULL;
+	usbhsh_uep_to_ep(uep)	= NULL;
+	usbhsh_ep_to_uep(ep)	= NULL;
+
+	usbhs_unlock(priv, flags);
+	/********************  spin unlock ******************/
+
+	kfree(uep);
+}
+
+static void usbhsh_endpoint_detach_all(struct usbhsh_hpriv *hpriv,
+				       struct usbhsh_device *udev)
+{
+	struct usbhsh_ep *uep, *next;
+
+	list_for_each_entry_safe(uep, next, &udev->ep_list_head, ep_list)
+		usbhsh_endpoint_detach(hpriv, usbhsh_uep_to_ep(uep));
+}
+
+/*
+ *		device control
+ */
+static int usbhsh_connected_to_rhdev(struct usb_hcd *hcd,
+				     struct usbhsh_device *udev)
+{
+	struct usb_device *usbv = usbhsh_udev_to_usbv(udev);
+
+	return hcd->self.root_hub == usbv->parent;
+}
+
+static int usbhsh_device_has_endpoint(struct usbhsh_device *udev)
+{
+	return !list_empty(&udev->ep_list_head);
+}
+
+static struct usbhsh_device *usbhsh_device_get(struct usbhsh_hpriv *hpriv,
+					       struct urb *urb)
+{
+	struct usb_device *usbv = usbhsh_urb_to_usbv(urb);
+	struct usbhsh_device *udev = usbhsh_usbv_to_udev(usbv);
+
+	/* usbhsh_device_attach() is still not called */
+	if (!udev)
+		return NULL;
+
+	/* if it is device0, return it */
+	if (0 == usb_pipedevice(urb->pipe))
+		return usbhsh_device0(hpriv);
+
+	/* return attached device */
+	return udev;
+}
+
+static struct usbhsh_device *usbhsh_device_attach(struct usbhsh_hpriv *hpriv,
+						 struct urb *urb)
+{
+	struct usbhsh_device *udev = NULL;
+	struct usbhsh_device *udev0 = usbhsh_device0(hpriv);
+	struct usbhsh_device *pos;
+	struct usb_hcd *hcd = usbhsh_hpriv_to_hcd(hpriv);
+	struct device *dev = usbhsh_hcd_to_dev(hcd);
+	struct usb_device *usbv = usbhsh_urb_to_usbv(urb);
+	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
+	unsigned long flags;
+	u16 upphub, hubport;
+	int i;
+
+	/*
+	 * This function should be called only while urb is pointing to device0.
+	 * It will attach unused usbhsh_device to urb (usbv),
+	 * and initialize device0.
+	 * You can use usbhsh_device_get() to get "current" udev,
+	 * and usbhsh_usbv_to_udev() is for "attached" udev.
+	 */
+	if (0 != usb_pipedevice(urb->pipe)) {
+		dev_err(dev, "%s fail: urb isn't pointing device0\n", __func__);
+		return NULL;
 	}
 
+	/********************  spin lock ********************/
+	usbhs_lock(priv, flags);
+
 	/*
-	 * find best pipe for endpoint
-	 * see
-	 *	HARDWARE LIMITATION
+	 * find unused device
 	 */
-	type = usb_endpoint_type(desc);
-	min_usr = ~0;
-	best_pipe = NULL;
-	usbhs_for_each_pipe(pipe, priv, i) {
-		if (!usbhs_pipe_type_is(pipe, type))
+	usbhsh_for_each_udev(pos, hpriv, i) {
+		if (usbhsh_udev_is_used(pos))
 			continue;
+		udev = pos;
+		break;
+	}
 
-		dir_in = !!usbhs_pipe_is_dir_in(pipe);
-		if (0 != (dir_in - dir_in_req))
-			continue;
+	if (udev) {
+		/*
+		 * usbhsh_usbv_to_udev()
+		 * usbhsh_udev_to_usbv()
+		 * will be enable
+		 */
+		dev_set_drvdata(&usbv->dev, udev);
+		udev->usbv = usbv;
+	}
 
-		info = usbhsh_pipe_info(pipe);
+	usbhs_unlock(priv, flags);
+	/********************  spin unlock ******************/
 
-		if (min_usr > info->usr_cnt) {
-			min_usr		= info->usr_cnt;
-			best_pipe	= pipe;
-		}
+	if (!udev) {
+		dev_err(dev, "no free usbhsh_device\n");
+		return NULL;
 	}
 
-	if (unlikely(!best_pipe)) {
-		dev_err(dev, "couldn't find best pipe\n");
-		kfree(uep);
-		return NULL;
+	if (usbhsh_device_has_endpoint(udev)) {
+		dev_warn(dev, "udev have old endpoint\n");
+		usbhsh_endpoint_detach_all(hpriv, udev);
+	}
+
+	if (usbhsh_device_has_endpoint(udev0)) {
+		dev_warn(dev, "udev0 have old endpoint\n");
+		usbhsh_endpoint_detach_all(hpriv, udev0);
 	}
-usbhsh_endpoint_alloc_find_pipe:
+
+	/* uep will be attached */
+	INIT_LIST_HEAD(&udev0->ep_list_head);
+	INIT_LIST_HEAD(&udev->ep_list_head);
+
 	/*
-	 * init uep
+	 * set device0 config
 	 */
-	uep->pipe	= best_pipe;
-	uep->maxp	= usb_endpoint_maxp(desc);
-	usbhsh_uep_to_udev(uep)	= udev;
-	usbhsh_ep_to_uep(ep)	= uep;
+	usbhs_set_device_config(priv,
+				0, 0, 0, usbv->speed);
 
 	/*
-	 * update pipe user count
+	 * set new device config
 	 */
-	info = usbhsh_pipe_info(best_pipe);
-	info->usr_cnt++;
+	upphub	= 0;
+	hubport	= 0;
+	if (!usbhsh_connected_to_rhdev(hcd, udev)) {
+		/* if udev is not connected to rhdev, it means parent is Hub */
+		struct usbhsh_device *parent = usbhsh_device_parent(udev);
 
-	/* init this endpoint, and attach it to udev */
-	INIT_LIST_HEAD(&uep->ep_list);
-	list_add_tail(&uep->ep_list, &udev->ep_list_head);
+		upphub	= usbhsh_device_number(hpriv, parent);
+		hubport	= usbhsh_device_hubport(udev);
 
-	/*
-	 * usbhs_pipe_config_update() should be called after
-	 * usbhs_device_config()
-	 * see
-	 *  DCPMAXP/PIPEMAXP
-	 */
-	usbhs_pipe_sequence_data0(uep->pipe);
-	usbhs_pipe_config_update(uep->pipe,
-				 usbhsh_device_number(hpriv, udev),
-				 usb_endpoint_num(desc),
-				 uep->maxp);
+		dev_dbg(dev, "%s connecte to Hub [%d:%d](%p)\n", __func__,
+			upphub, hubport, parent);
+	}
 
-	dev_dbg(dev, "%s [%d-%s](%p)\n", __func__,
-		usbhsh_device_number(hpriv, udev),
-		usbhs_pipe_name(uep->pipe), uep);
+	usbhs_set_device_config(priv,
+			       usbhsh_device_number(hpriv, udev),
+			       upphub, hubport, usbv->speed);
 
-	return uep;
+	dev_dbg(dev, "%s [%d](%p)\n", __func__,
+		usbhsh_device_number(hpriv, udev), udev);
+
+	return udev;
 }
 
-void usbhsh_endpoint_free(struct usbhsh_hpriv *hpriv,
-			  struct usb_host_endpoint *ep)
+static void usbhsh_device_detach(struct usbhsh_hpriv *hpriv,
+			       struct usbhsh_device *udev)
 {
+	struct usb_hcd *hcd = usbhsh_hpriv_to_hcd(hpriv);
 	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
-	struct device *dev = usbhs_priv_to_dev(priv);
-	struct usbhsh_ep *uep = usbhsh_ep_to_uep(ep);
-	struct usbhsh_pipe_info *info;
+	struct device *dev = usbhsh_hcd_to_dev(hcd);
+	struct usb_device *usbv = usbhsh_udev_to_usbv(udev);
+	unsigned long flags;
 
-	if (!uep)
-		return;
+	dev_dbg(dev, "%s [%d](%p)\n", __func__,
+		usbhsh_device_number(hpriv, udev), udev);
 
-	dev_dbg(dev, "%s [%d-%s](%p)\n", __func__,
-		usbhsh_device_number(hpriv, usbhsh_uep_to_udev(uep)),
-		usbhs_pipe_name(uep->pipe), uep);
+	if (usbhsh_device_has_endpoint(udev)) {
+		dev_warn(dev, "udev still have endpoint\n");
+		usbhsh_endpoint_detach_all(hpriv, udev);
+	}
 
-	info = usbhsh_pipe_info(uep->pipe);
-	info->usr_cnt--;
+	/*
+	 * There is nothing to do if it is device0.
+	 * see
+	 *  usbhsh_device_attach()
+	 *  usbhsh_device_get()
+	 */
+	if (0 == usbhsh_device_number(hpriv, udev))
+		return;
 
-	/* remove this endpoint from udev */
-	list_del_init(&uep->ep_list);
+	/********************  spin lock ********************/
+	usbhs_lock(priv, flags);
 
-	usbhsh_uep_to_udev(uep) = NULL;
-	usbhsh_ep_to_uep(ep) = NULL;
+	/*
+	 * usbhsh_usbv_to_udev()
+	 * usbhsh_udev_to_usbv()
+	 * will be disable
+	 */
+	dev_set_drvdata(&usbv->dev, NULL);
+	udev->usbv = NULL;
 
-	kfree(uep);
+	usbhs_unlock(priv, flags);
+	/********************  spin unlock ******************/
 }
 
 /*
@@ -480,11 +629,12 @@ void usbhsh_endpoint_free(struct usbhsh_hpriv *hpriv,
  */
 static void usbhsh_queue_done(struct usbhs_priv *priv, struct usbhs_pkt *pkt)
 {
-	struct usbhsh_request *ureq = usbhsh_pkt_to_req(pkt);
+	struct usbhsh_request *ureq = usbhsh_pkt_to_ureq(pkt);
 	struct usbhsh_hpriv *hpriv = usbhsh_priv_to_hpriv(priv);
 	struct usb_hcd *hcd = usbhsh_hpriv_to_hcd(hpriv);
 	struct urb *urb = ureq->urb;
 	struct device *dev = usbhs_priv_to_dev(priv);
+	int status = 0;
 
 	dev_dbg(dev, "%s\n", __func__);
 
@@ -493,29 +643,43 @@ static void usbhsh_queue_done(struct usbhs_priv *priv, struct usbhs_pkt *pkt)
 		return;
 	}
 
+	if (!usbhsh_is_running(hpriv))
+		status = -ESHUTDOWN;
+
 	urb->actual_length = pkt->actual;
-	usbhsh_req_free(hpriv, ureq);
-	usbhsh_urb_to_ureq(urb) = NULL;
+	usbhsh_ureq_free(hpriv, ureq);
+
+	usbhsh_endpoint_sequence_save(hpriv, urb, pkt);
+	usbhsh_pipe_detach(hpriv, usbhsh_ep_to_uep(urb->ep));
 
 	usb_hcd_unlink_urb_from_ep(hcd, urb);
-	usb_hcd_giveback_urb(hcd, urb, 0);
+	usb_hcd_giveback_urb(hcd, urb, status);
 }
 
 static int usbhsh_queue_push(struct usb_hcd *hcd,
-			     struct usbhs_pipe *pipe,
-			     struct urb *urb)
+			     struct urb *urb,
+			     gfp_t mem_flags)
 {
-	struct usbhsh_request *ureq = usbhsh_urb_to_ureq(urb);
-	struct usbhs_pkt *pkt = &ureq->pkt;
+	struct usbhsh_hpriv *hpriv = usbhsh_hcd_to_hpriv(hcd);
+	struct usbhsh_ep *uep = usbhsh_ep_to_uep(urb->ep);
+	struct usbhs_pipe *pipe = usbhsh_uep_to_pipe(uep);
 	struct device *dev = usbhsh_hcd_to_dev(hcd);
+	struct usbhsh_request *ureq;
 	void *buf;
-	int len;
+	int len, sequence;
 
 	if (usb_pipeisoc(urb->pipe)) {
 		dev_err(dev, "pipe iso is not supported now\n");
 		return -EIO;
 	}
 
+	/* this ureq will be freed on usbhsh_queue_done() */
+	ureq = usbhsh_ureq_alloc(hpriv, urb, mem_flags);
+	if (unlikely(!ureq)) {
+		dev_err(dev, "ureq alloc fail\n");
+		return -ENOMEM;
+	}
+
 	if (usb_pipein(urb->pipe))
 		pipe->handler = &usbhs_fifo_pio_pop_handler;
 	else
@@ -524,25 +688,59 @@ static int usbhsh_queue_push(struct usb_hcd *hcd,
 	buf = (void *)(urb->transfer_buffer + urb->actual_length);
 	len = urb->transfer_buffer_length - urb->actual_length;
 
+	sequence = usb_gettoggle(urb->dev,
+				 usb_pipeendpoint(urb->pipe),
+				 usb_pipeout(urb->pipe));
+
 	dev_dbg(dev, "%s\n", __func__);
-	usbhs_pkt_push(pipe, pkt, usbhsh_queue_done,
-		       buf, len, (urb->transfer_flags & URB_ZERO_PACKET));
+	usbhs_pkt_push(pipe, &ureq->pkt, usbhsh_queue_done,
+		       buf, len, (urb->transfer_flags & URB_ZERO_PACKET),
+		       sequence);
+
 	usbhs_pkt_start(pipe);
 
 	return 0;
 }
 
+static void usbhsh_queue_force_pop(struct usbhs_priv *priv,
+				   struct usbhs_pipe *pipe)
+{
+	struct usbhs_pkt *pkt;
+
+	while (1) {
+		pkt = usbhs_pkt_pop(pipe, NULL);
+		if (!pkt)
+			break;
+
+		/*
+		 * if all packet are gone, usbhsh_endpoint_disable()
+		 * will be called.
+		 * then, attached device/endpoint/pipe will be detached
+		 */
+		usbhsh_queue_done(priv, pkt);
+	}
+}
+
+static void usbhsh_queue_force_pop_all(struct usbhs_priv *priv)
+{
+	struct usbhs_pipe *pos;
+	int i;
+
+	usbhs_for_each_pipe_with_dcp(pos, priv, i)
+		usbhsh_queue_force_pop(priv, pos);
+}
+
 /*
  *		DCP setup stage
  */
 static int usbhsh_is_request_address(struct urb *urb)
 {
-	struct usb_ctrlrequest *cmd;
+	struct usb_ctrlrequest *req;
 
-	cmd = (struct usb_ctrlrequest *)urb->setup_packet;
+	req = (struct usb_ctrlrequest *)urb->setup_packet;
 
-	if ((DeviceOutRequest    == cmd->bRequestType << 8) &&
-	    (USB_REQ_SET_ADDRESS == cmd->bRequest))
+	if ((DeviceOutRequest    == req->bRequestType << 8) &&
+	    (USB_REQ_SET_ADDRESS == req->bRequest))
 		return 1;
 	else
 		return 0;
@@ -570,11 +768,15 @@ static void usbhsh_setup_stage_packet_push(struct usbhsh_hpriv *hpriv,
 	/*
 	 * renesas_usbhs can not use original usb address.
 	 * see HARDWARE LIMITATION.
-	 * modify usb address here.
+	 * modify usb address here to use attached device.
+	 * see usbhsh_device_attach()
 	 */
 	if (usbhsh_is_request_address(urb)) {
-		/* FIXME */
-		req.wValue = 1;
+		struct usb_device *usbv = usbhsh_urb_to_usbv(urb);
+		struct usbhsh_device *udev = usbhsh_usbv_to_udev(usbv);
+
+		/* udev is a attached device */
+		req.wValue = usbhsh_device_number(hpriv, udev);
 		dev_dbg(dev, "create new address - %d\n", req.wValue);
 	}
 
@@ -595,82 +797,80 @@ static void usbhsh_setup_stage_packet_push(struct usbhsh_hpriv *hpriv,
 static void usbhsh_data_stage_packet_done(struct usbhs_priv *priv,
 					  struct usbhs_pkt *pkt)
 {
-	struct usbhsh_request *ureq = usbhsh_pkt_to_req(pkt);
+	struct usbhsh_request *ureq = usbhsh_pkt_to_ureq(pkt);
 	struct usbhsh_hpriv *hpriv = usbhsh_priv_to_hpriv(priv);
-	struct urb *urb = ureq->urb;
 
 	/* this ureq was connected to urb when usbhsh_urb_enqueue()  */
 
-	usbhsh_req_free(hpriv, ureq);
-	usbhsh_urb_to_ureq(urb) = NULL;
+	usbhsh_ureq_free(hpriv, ureq);
 }
 
-static void usbhsh_data_stage_packet_push(struct usbhsh_hpriv *hpriv,
-					  struct urb *urb,
-					  struct usbhs_pipe *pipe)
+static int usbhsh_data_stage_packet_push(struct usbhsh_hpriv *hpriv,
+					 struct urb *urb,
+					 struct usbhs_pipe *pipe,
+					 gfp_t mem_flags)
+
 {
 	struct usbhsh_request *ureq;
-	struct usbhs_pkt *pkt;
 
-	/*
-	 * FIXME
-	 *
-	 * data stage uses ureq which is connected to urb
-	 * see usbhsh_urb_enqueue() :: alloc new request.
-	 * it will be freed in usbhsh_data_stage_packet_done()
-	 */
-	ureq	= usbhsh_urb_to_ureq(urb);
-	pkt	= &ureq->pkt;
+	/* this ureq will be freed on usbhsh_data_stage_packet_done() */
+	ureq = usbhsh_ureq_alloc(hpriv, urb, mem_flags);
+	if (unlikely(!ureq))
+		return -ENOMEM;
 
 	if (usb_pipein(urb->pipe))
 		pipe->handler = &usbhs_dcp_data_stage_in_handler;
 	else
 		pipe->handler = &usbhs_dcp_data_stage_out_handler;
 
-	usbhs_pkt_push(pipe, pkt,
+	usbhs_pkt_push(pipe, &ureq->pkt,
 		       usbhsh_data_stage_packet_done,
 		       urb->transfer_buffer,
 		       urb->transfer_buffer_length,
-		       (urb->transfer_flags & URB_ZERO_PACKET));
+		       (urb->transfer_flags & URB_ZERO_PACKET),
+		       -1);
+
+	return 0;
 }
 
 /*
  *		DCP status stage
  */
-static void usbhsh_status_stage_packet_push(struct usbhsh_hpriv *hpriv,
+static int usbhsh_status_stage_packet_push(struct usbhsh_hpriv *hpriv,
 					    struct urb *urb,
-					    struct usbhs_pipe *pipe)
+					    struct usbhs_pipe *pipe,
+					    gfp_t mem_flags)
 {
 	struct usbhsh_request *ureq;
-	struct usbhs_pkt *pkt;
 
-	/*
-	 * FIXME
-	 *
-	 * status stage uses allocated ureq.
-	 * it will be freed on usbhsh_queue_done()
-	 */
-	ureq	= usbhsh_req_alloc(hpriv, urb, GFP_KERNEL);
-	pkt	= &ureq->pkt;
+	/* This ureq will be freed on usbhsh_queue_done() */
+	ureq = usbhsh_ureq_alloc(hpriv, urb, mem_flags);
+	if (unlikely(!ureq))
+		return -ENOMEM;
 
 	if (usb_pipein(urb->pipe))
 		pipe->handler = &usbhs_dcp_status_stage_in_handler;
 	else
 		pipe->handler = &usbhs_dcp_status_stage_out_handler;
 
-	usbhs_pkt_push(pipe, pkt,
+	usbhs_pkt_push(pipe, &ureq->pkt,
 		       usbhsh_queue_done,
 		       NULL,
 		       urb->transfer_buffer_length,
-		       0);
+		       0, -1);
+
+	return 0;
 }
 
 static int usbhsh_dcp_queue_push(struct usb_hcd *hcd,
-				 struct usbhsh_hpriv *hpriv,
-				 struct usbhs_pipe *pipe,
-				 struct urb *urb)
+				 struct urb *urb,
+				 gfp_t mflags)
 {
+	struct usbhsh_hpriv *hpriv = usbhsh_hcd_to_hpriv(hcd);
+	struct usbhsh_ep *uep = usbhsh_ep_to_uep(urb->ep);
+	struct usbhs_pipe *pipe = usbhsh_uep_to_pipe(uep);
 	struct device *dev = usbhsh_hcd_to_dev(hcd);
+	int ret;
 
 	dev_dbg(dev, "%s\n", __func__);
 
@@ -686,13 +886,22 @@ static int usbhsh_dcp_queue_push(struct usb_hcd *hcd,
 	 *
 	 * It is pushed only when urb has buffer.
 	 */
-	if (urb->transfer_buffer_length)
-		usbhsh_data_stage_packet_push(hpriv, urb, pipe);
+	if (urb->transfer_buffer_length) {
+		ret = usbhsh_data_stage_packet_push(hpriv, urb, pipe, mflags);
+		if (ret < 0) {
+			dev_err(dev, "data stage failed\n");
+			return ret;
+		}
+	}
 
 	/*
 	 * status stage
 	 */
-	usbhsh_status_stage_packet_push(hpriv, urb, pipe);
+	ret = usbhsh_status_stage_packet_push(hpriv, urb, pipe, mflags);
+	if (ret < 0) {
+		dev_err(dev, "status stage failed\n");
+		return ret;
+	}
 
 	/*
 	 * start pushed packets
@@ -729,71 +938,82 @@ static int usbhsh_urb_enqueue(struct usb_hcd *hcd,
 	struct usbhsh_hpriv *hpriv = usbhsh_hcd_to_hpriv(hcd);
 	struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
 	struct device *dev = usbhs_priv_to_dev(priv);
-	struct usb_device *usbv = usbhsh_urb_to_usbv(urb);
 	struct usb_host_endpoint *ep = urb->ep;
-	struct usbhsh_request *ureq;
-	struct usbhsh_device *udev, *new_udev = NULL;
-	struct usbhs_pipe *pipe;
-	struct usbhsh_ep *uep;
+	struct usbhsh_device *new_udev = NULL;
 	int is_dir_in = usb_pipein(urb->pipe);
-
+	int i;
 	int ret;
 
 	dev_dbg(dev, "%s (%s)\n", __func__, is_dir_in ? "in" : "out");
 
+	if (!usbhsh_is_running(hpriv)) {
+		ret = -EIO;
+		dev_err(dev, "host is not running\n");
+		goto usbhsh_urb_enqueue_error_not_linked;
+	}
+
 	ret = usb_hcd_link_urb_to_ep(hcd, urb);
-	if (ret)
+	if (ret) {
+		dev_err(dev, "urb link failed\n");
 		goto usbhsh_urb_enqueue_error_not_linked;
+	}
 
 	/*
-	 * get udev
+	 * attach udev if needed
+	 * see [image of mod_host]
 	 */
-	udev = usbhsh_usbv_to_udev(usbv);
-	if (!udev) {
-		new_udev = usbhsh_device_alloc(hpriv, urb);
-		if (!new_udev)
+	if (!usbhsh_device_get(hpriv, urb)) {
+		new_udev = usbhsh_device_attach(hpriv, urb);
+		if (!new_udev) {
+			ret = -EIO;
+			dev_err(dev, "device attach failed\n");
 			goto usbhsh_urb_enqueue_error_not_linked;
-
-		udev = new_udev;
+		}
 	}
 
 	/*
-	 * get uep
+	 * attach endpoint if needed
+	 * see [image of mod_host]
 	 */
-	uep = usbhsh_ep_to_uep(ep);
-	if (!uep) {
-		uep = usbhsh_endpoint_alloc(hpriv, udev, ep,
-					    is_dir_in, mem_flags);
-		if (!uep)
+	if (!usbhsh_ep_to_uep(ep)) {
+		ret = usbhsh_endpoint_attach(hpriv, urb, mem_flags);
+		if (ret < 0) {
+			dev_err(dev, "endpoint attach failed\n");
 			goto usbhsh_urb_enqueue_error_free_device;
+		}
 	}
-	pipe = usbhsh_uep_to_pipe(uep);
 
 	/*
-	 * alloc new request
+	 * attach pipe to endpoint
+	 * see [image of mod_host]
 	 */
-	ureq = usbhsh_req_alloc(hpriv, urb, mem_flags);
-	if (unlikely(!ureq)) {
-		ret = -ENOMEM;
+	for (i = 0; i < 1024; i++) {
+		ret = usbhsh_pipe_attach(hpriv, urb);
+		if (ret < 0)
+			msleep(100);
+		else
+			break;
+	}
+	if (ret < 0) {
+		dev_err(dev, "pipe attach failed\n");
 		goto usbhsh_urb_enqueue_error_free_endpoint;
 	}
-	usbhsh_urb_to_ureq(urb) = ureq;
 
 	/*
 	 * push packet
 	 */
 	if (usb_pipecontrol(urb->pipe))
-		usbhsh_dcp_queue_push(hcd, hpriv, pipe, urb);
+		ret = usbhsh_dcp_queue_push(hcd, urb, mem_flags);
 	else
-		usbhsh_queue_push(hcd, pipe, urb);
+		ret = usbhsh_queue_push(hcd, urb, mem_flags);
 
-	return 0;
+	return ret;
 
 usbhsh_urb_enqueue_error_free_endpoint:
-	usbhsh_endpoint_free(hpriv, ep);
+	usbhsh_endpoint_detach(hpriv, ep);
 usbhsh_urb_enqueue_error_free_device:
 	if (new_udev)
-		usbhsh_device_free(hpriv, new_udev);
+		usbhsh_device_detach(hpriv, new_udev);
 usbhsh_urb_enqueue_error_not_linked:
 
 	dev_dbg(dev, "%s error\n", __func__);
@@ -807,8 +1027,11 @@ static int usbhsh_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 	struct usbhsh_request *ureq = usbhsh_urb_to_ureq(urb);
 
 	if (ureq) {
-		usbhsh_req_free(hpriv, ureq);
-		usbhsh_urb_to_ureq(urb) = NULL;
+		struct usbhs_priv *priv = usbhsh_hpriv_to_priv(hpriv);
+		struct usbhs_pkt *pkt = &ureq->pkt;
+
+		usbhs_pkt_pop(pkt->pipe, pkt);
+		usbhsh_queue_done(priv, pkt);
 	}
 
 	return 0;
@@ -823,7 +1046,7 @@ static void usbhsh_endpoint_disable(struct usb_hcd *hcd,
 
 	/*
 	 * this function might be called manytimes by same hcd/ep
-	 * in-endpoitn == out-endpoint if ep == dcp.
+	 * in-endpoint == out-endpoint if ep == dcp.
 	 */
 	if (!uep)
 		return;
@@ -831,15 +1054,14 @@ static void usbhsh_endpoint_disable(struct usb_hcd *hcd,
 	udev	= usbhsh_uep_to_udev(uep);
 	hpriv	= usbhsh_hcd_to_hpriv(hcd);
 
-	usbhsh_endpoint_free(hpriv, ep);
-	ep->hcpriv = NULL;
+	usbhsh_endpoint_detach(hpriv, ep);
 
 	/*
 	 * if there is no endpoint,
 	 * free device
 	 */
 	if (!usbhsh_device_has_endpoint(udev))
-		usbhsh_device_free(hpriv, udev);
+		usbhsh_device_detach(hpriv, udev);
 }
 
 static int usbhsh_hub_status_data(struct usb_hcd *hcd, char *buf)
@@ -919,6 +1141,8 @@ static int __usbhsh_hub_port_feature(struct usbhsh_hpriv *hpriv,
 				       USB_PORT_STAT_HIGH_SPEED |
 				       USB_PORT_STAT_LOW_SPEED);
 
+		usbhsh_queue_force_pop_all(priv);
+
 		usbhs_bus_send_reset(priv);
 		msleep(20);
 		usbhs_bus_send_sof_enable(priv);
@@ -1082,6 +1306,20 @@ static int usbhsh_irq_attch(struct usbhs_priv *priv,
 	usbhsh_port_stat_set(hpriv, USB_PORT_STAT_CONNECTION);
 	usbhsh_port_stat_set(hpriv, USB_PORT_STAT_C_CONNECTION << 16);
 
+	/*
+	 * attch interrupt might happen infinitely on some device
+	 * (on self power USB hub ?)
+	 * disable it here.
+	 *
+	 * usbhsh_is_running() becomes effective
+	 * according to this process.
+	 * see
+	 *	usbhsh_is_running()
+	 *	usbhsh_urb_enqueue()
+	 */
+	hpriv->mod.irq_attch = NULL;
+	usbhs_irq_callback_update(priv, &hpriv->mod);
+
 	return 0;
 }
 
@@ -1096,6 +1334,24 @@ static int usbhsh_irq_dtch(struct usbhs_priv *priv,
 	usbhsh_port_stat_clear(hpriv, USB_PORT_STAT_CONNECTION);
 	usbhsh_port_stat_set(hpriv, USB_PORT_STAT_C_CONNECTION << 16);
 
+	/*
+	 * enable attch interrupt again
+	 *
+	 * usbhsh_is_running() becomes invalid
+	 * according to this process.
+	 * see
+	 *	usbhsh_is_running()
+	 *	usbhsh_urb_enqueue()
+	 */
+	hpriv->mod.irq_attch = usbhsh_irq_attch;
+	usbhs_irq_callback_update(priv, &hpriv->mod);
+
+	/*
+	 * usbhsh_queue_force_pop_all() should be called
+	 * after usbhsh_is_running() becomes invalid.
+	 */
+	usbhsh_queue_force_pop_all(priv);
+
 	return 0;
 }
 
@@ -1131,7 +1387,6 @@ static int usbhsh_irq_setup_err(struct usbhs_priv *priv,
 static void usbhsh_pipe_init_for_host(struct usbhs_priv *priv)
 {
 	struct usbhsh_hpriv *hpriv = usbhsh_priv_to_hpriv(priv);
-	struct usbhsh_pipe_info *pipe_info = hpriv->pipe_info;
 	struct usbhs_pipe *pipe;
 	u32 *pipe_type = usbhs_get_dparam(priv, pipe_type);
 	int pipe_size = usbhs_get_dparam(priv, pipe_size);
@@ -1140,7 +1395,6 @@ static void usbhsh_pipe_init_for_host(struct usbhs_priv *priv)
 	/* init all pipe */
 	old_type = USB_ENDPOINT_XFER_CONTROL;
 	for (i = 0; i < pipe_size; i++) {
-		pipe_info[i].usr_cnt	= 0;
 
 		/*
 		 * data "output" will be finished as soon as possible,
@@ -1174,7 +1428,7 @@ static void usbhsh_pipe_init_for_host(struct usbhs_priv *priv)
 						 dir_in);
 		}
 
-		pipe->mod_private = pipe_info + i;
+		pipe->mod_private = NULL;
 	}
 }
 
@@ -1205,9 +1459,7 @@ static int usbhsh_start(struct usbhs_priv *priv)
 	 * - host
 	 * - usb module
 	 */
-	usbhs_sys_hispeed_ctrl(priv, 1);
 	usbhs_sys_host_ctrl(priv, 1);
-	usbhs_sys_usb_ctrl(priv, 1);
 
 	/*
 	 * enable irq callback
@@ -1242,9 +1494,7 @@ static int usbhsh_stop(struct usbhs_priv *priv)
 	usb_remove_hcd(hcd);
 
 	/* disable sys */
-	usbhs_sys_hispeed_ctrl(priv, 0);
 	usbhs_sys_host_ctrl(priv, 0);
-	usbhs_sys_usb_ctrl(priv, 0);
 
 	dev_dbg(dev, "quit host\n");
 
@@ -1255,10 +1505,8 @@ int usbhs_mod_host_probe(struct usbhs_priv *priv)
 {
 	struct usbhsh_hpriv *hpriv;
 	struct usb_hcd *hcd;
-	struct usbhsh_pipe_info *pipe_info;
 	struct usbhsh_device *udev;
 	struct device *dev = usbhs_priv_to_dev(priv);
-	int pipe_size = usbhs_get_dparam(priv, pipe_size);
 	int i;
 
 	/* initialize hcd */
@@ -1269,12 +1517,6 @@ int usbhs_mod_host_probe(struct usbhs_priv *priv)
 	}
 	hcd->has_tt = 1; /* for low/full speed */
 
-	pipe_info = kzalloc(sizeof(*pipe_info) * pipe_size, GFP_KERNEL);
-	if (!pipe_info) {
-		dev_err(dev, "Could not allocate pipe_info\n");
-		goto usbhs_mod_host_probe_err;
-	}
-
 	/*
 	 * CAUTION
 	 *
@@ -1294,9 +1536,6 @@ int usbhs_mod_host_probe(struct usbhs_priv *priv)
 	hpriv->mod.name		= "host";
 	hpriv->mod.start	= usbhsh_start;
 	hpriv->mod.stop		= usbhsh_stop;
-	hpriv->pipe_info	= pipe_info;
-	hpriv->pipe_size	= pipe_size;
-	usbhsh_req_list_init(hpriv);
 	usbhsh_port_stat_init(hpriv);
 
 	/* init all device */
@@ -1308,11 +1547,6 @@ int usbhs_mod_host_probe(struct usbhs_priv *priv)
 	dev_info(dev, "host probed\n");
 
 	return 0;
-
-usbhs_mod_host_probe_err:
-	usb_put_hcd(hcd);
-
-	return -ENOMEM;
 }
 
 int usbhs_mod_host_remove(struct usbhs_priv *priv)
@@ -1320,8 +1554,6 @@ int usbhs_mod_host_remove(struct usbhs_priv *priv)
 	struct usbhsh_hpriv *hpriv = usbhsh_priv_to_hpriv(priv);
 	struct usb_hcd *hcd = usbhsh_hpriv_to_hcd(hpriv);
 
-	usbhsh_req_list_quit(hpriv);
-
 	usb_put_hcd(hcd);
 
 	return 0;
diff --git a/drivers/usb/renesas_usbhs/pipe.c b/drivers/usb/renesas_usbhs/pipe.c
index c74389ce2177..feb06d6d2814 100644
--- a/drivers/usb/renesas_usbhs/pipe.c
+++ b/drivers/usb/renesas_usbhs/pipe.c
@@ -257,6 +257,13 @@ void usbhs_pipe_stall(struct usbhs_pipe *pipe)
 	}
 }
 
+int usbhs_pipe_is_stall(struct usbhs_pipe *pipe)
+{
+	u16 pid = usbhsp_pipectrl_get(pipe) & PID_MASK;
+
+	return (int)(pid == PID_STALL10 || pid == PID_STALL11);
+}
+
 /*
  *		pipe setup
  */
@@ -323,8 +330,7 @@ static u16 usbhsp_setup_pipecfg(struct usbhs_pipe *pipe,
 	if (dir_in)
 		usbhsp_flags_set(pipe, IS_DIR_HOST);
 
-	if ((is_host  && !dir_in) ||
-	    (!is_host && dir_in))
+	if (!!is_host ^ !!dir_in)
 		dir |= DIR_OUT;
 
 	if (!dir)
@@ -471,10 +477,27 @@ int usbhs_pipe_is_dir_host(struct usbhs_pipe *pipe)
 	return usbhsp_flags_has(pipe, IS_DIR_HOST);
 }
 
-void usbhs_pipe_data_sequence(struct usbhs_pipe *pipe, int data)
+void usbhs_pipe_data_sequence(struct usbhs_pipe *pipe, int sequence)
 {
 	u16 mask = (SQCLR | SQSET);
-	u16 val = (data) ? SQSET : SQCLR;
+	u16 val;
+
+	/*
+	 * sequence
+	 *  0  : data0
+	 *  1  : data1
+	 *  -1 : no change
+	 */
+	switch (sequence) {
+	case 0:
+		val = SQCLR;
+		break;
+	case 1:
+		val = SQSET;
+		break;
+	default:
+		return;
+	}
 
 	usbhsp_pipectrl_set(pipe, mask, val);
 }
diff --git a/drivers/usb/renesas_usbhs/pipe.h b/drivers/usb/renesas_usbhs/pipe.h
index 6334fc644cc0..fa18b7dc2b2a 100644
--- a/drivers/usb/renesas_usbhs/pipe.h
+++ b/drivers/usb/renesas_usbhs/pipe.h
@@ -87,6 +87,7 @@ int usbhs_pipe_is_accessible(struct usbhs_pipe *pipe);
 void usbhs_pipe_enable(struct usbhs_pipe *pipe);
 void usbhs_pipe_disable(struct usbhs_pipe *pipe);
 void usbhs_pipe_stall(struct usbhs_pipe *pipe);
+int usbhs_pipe_is_stall(struct usbhs_pipe *pipe);
 void usbhs_pipe_select_fifo(struct usbhs_pipe *pipe, struct usbhs_fifo *fifo);
 void usbhs_pipe_config_update(struct usbhs_pipe *pipe, u16 devsel,
 			      u16 epnum, u16 maxp);
diff --git a/drivers/usb/serial/ChangeLog.history b/drivers/usb/serial/ChangeLog.history
deleted file mode 100644
index f13fd488ebec..000000000000
--- a/drivers/usb/serial/ChangeLog.history
+++ /dev/null
@@ -1,730 +0,0 @@
-This is the contents of some of the drivers/usb/serial/ files that had  old
-changelog comments.  They were quite old, and out of date, and we don't keep
-them anymore, so I've put them here, away from the source files, in case
-people still care to see them.
-
-- Greg Kroah-Hartman <greg@kroah.com> October 20, 2005
-
------------------------------------------------------------------------
-usb-serial.h Change Log comments:
-
- (03/26/2002) gkh
-	removed the port->tty check from port_paranoia_check() due to serial
-	consoles not having a tty device assigned to them.
-
- (12/03/2001) gkh
-	removed active from the port structure.
-	added documentation to the usb_serial_device_type structure
-
- (10/10/2001) gkh
-	added vendor and product to serial structure.  Needed to determine device
-	owner when the device is disconnected.
-
- (05/30/2001) gkh
-	added sem to port structure and removed port_lock
-
- (10/05/2000) gkh
-	Added interrupt_in_endpointAddress and bulk_in_endpointAddress to help
-	fix bug with urb->dev not being set properly, now that the usb core
-	needs it.
-
- (09/11/2000) gkh
-	Added usb_serial_debug_data function to help get rid of #DEBUG in the
-	drivers.
-
- (08/28/2000) gkh
-	Added port_lock to port structure.
-
- (08/08/2000) gkh
-	Added open_count to port structure.
-
- (07/23/2000) gkh
-	Added bulk_out_endpointAddress to port structure.
-
- (07/19/2000) gkh, pberger, and borchers
-	Modifications to allow usb-serial drivers to be modules.
-
------------------------------------------------------------------------
-usb-serial.c Change Log comments:
-
- (12/10/2002) gkh
-	Split the ports off into their own struct device, and added a
-	usb-serial bus driver.
-
- (11/19/2002) gkh
-	removed a few #ifdefs for the generic code and cleaned up the failure
-	logic in initialization.
-
- (10/02/2002) gkh
-	moved the console code to console.c and out of this file.
-
- (06/05/2002) gkh
-	moved location of startup() call in serial_probe() until after all
-	of the port information and endpoints are initialized.  This makes
-	things easier for some drivers.
-
- (04/10/2002) gkh
-	added serial_read_proc function which creates a
-	/proc/tty/driver/usb-serial file.
-
- (03/27/2002) gkh
-	Got USB serial console code working properly and merged into the main
-	version of the tree.  Thanks to Randy Dunlap for the initial version
-	of this code, and for pushing me to finish it up.
-	The USB serial console works with any usb serial driver device.
-
- (03/21/2002) gkh
-	Moved all manipulation of port->open_count into the core.  Now the
-	individual driver's open and close functions are called only when the
-	first open() and last close() is called.  Making the drivers a bit
-	smaller and simpler.
-	Fixed a bug if a driver didn't have the owner field set.
-
- (02/26/2002) gkh
-	Moved all locking into the main serial_* functions, instead of having
-	the individual drivers have to grab the port semaphore.  This should
-	reduce races.
-	Reworked the MOD_INC logic a bit to always increment and decrement, even
-	if the generic driver is being used.
-
- (10/10/2001) gkh
-	usb_serial_disconnect() now sets the serial->dev pointer is to NULL to
-	help prevent child drivers from accessing the device since it is now
-	gone.
-
- (09/13/2001) gkh
-	Moved generic driver initialize after we have registered with the USB
-	core.  Thanks to Randy Dunlap for pointing this problem out.
-
- (07/03/2001) gkh
-	Fixed module paramater size.  Thanks to John Brockmeyer for the pointer.
-	Fixed vendor and product getting defined through the MODULE_PARM macro
-	if the Generic driver wasn't compiled in.
-	Fixed problem with generic_shutdown() not being called for drivers that
-	don't have a shutdown() function.
-
- (06/06/2001) gkh
-	added evil hack that is needed for the prolific pl2303 device due to the
-	crazy way its endpoints are set up.
-
- (05/30/2001) gkh
-	switched from using spinlock to a semaphore, which fixes lots of problems.
-
- (04/08/2001) gb
-	Identify version on module load.
-
- 2001_02_05 gkh
-	Fixed buffer overflows bug with the generic serial driver.  Thanks to
-	Todd Squires <squirest@ct0.com> for fixing this.
-
- (01/10/2001) gkh
-	Fixed bug where the generic serial adaptor grabbed _any_ device that was
-	offered to it.
-
- (12/12/2000) gkh
-	Removed MOD_INC and MOD_DEC from poll and disconnect functions, and
-	moved them to the serial_open and serial_close functions.
-	Also fixed bug with there not being a MOD_DEC for the generic driver
-	(thanks to Gary Brubaker for finding this.)
-
- (11/29/2000) gkh
-	Small NULL pointer initialization cleanup which saves a bit of disk image
-
- (11/01/2000) Adam J. Richter
-	instead of using idVendor/idProduct pairs, usb serial drivers
-	now identify their hardware interest with usb_device_id tables,
-	which they usually have anyhow for use with MODULE_DEVICE_TABLE.
-
- (10/05/2000) gkh
-	Fixed bug with urb->dev not being set properly, now that the usb
-	core needs it.
-
- (09/11/2000) gkh
-	Removed DEBUG #ifdefs with call to usb_serial_debug_data
-
- (08/28/2000) gkh
-	Added port_lock to port structure.
-	Added locks for SMP safeness to generic driver
-	Fixed the ability to open a generic device's port more than once.
-
- (07/23/2000) gkh
-	Added bulk_out_endpointAddress to port structure.
-
- (07/19/2000) gkh, pberger, and borchers
-	Modifications to allow usb-serial drivers to be modules.
-
- (07/03/2000) gkh
-	Added more debugging to serial_ioctl call
-
- (06/25/2000) gkh
-	Changed generic_write_bulk_callback to not call wake_up_interruptible
-	directly, but to have port_softint do it at a safer time.
-
- (06/23/2000) gkh
-	Cleaned up debugging statements in a quest to find UHCI timeout bug.
-
- (05/22/2000) gkh
-	Changed the makefile, enabling the big CONFIG_USB_SERIAL_SOMTHING to be
-	removed from the individual device source files.
-
- (05/03/2000) gkh
-	Added the Digi Acceleport driver from Al Borchers and Peter Berger.
-
- (05/02/2000) gkh
-	Changed devfs and tty register code to work properly now. This was based on
-	the ACM driver changes by Vojtech Pavlik.
-
- (04/27/2000) Ryan VanderBijl
- 	Put calls to *_paranoia_checks into one function.
-
- (04/23/2000) gkh
-	Fixed bug that Randy Dunlap found for Generic devices with no bulk out ports.
-	Moved when the startup code printed out the devices that are supported.
-
- (04/19/2000) gkh
-	Added driver for ZyXEL omni.net lcd plus ISDN TA
-	Made startup info message specify which drivers were compiled in.
-
- (04/03/2000) gkh
-	Changed the probe process to remove the module unload races.
-	Changed where the tty layer gets initialized to have devfs work nicer.
-	Added initial devfs support.
-
- (03/26/2000) gkh
-	Split driver up into device specific pieces.
-
- (03/19/2000) gkh
-	Fixed oops that could happen when device was removed while a program
-	was talking to the device.
-	Removed the static urbs and now all urbs are created and destroyed
-	dynamically.
-	Reworked the internal interface. Now everything is based on the
-	usb_serial_port structure instead of the larger usb_serial structure.
-	This fixes the bug that a multiport device could not have more than
-	one port open at one time.
-
- (03/17/2000) gkh
-	Added config option for debugging messages.
-	Added patch for keyspan pda from Brian Warner.
-
- (03/06/2000) gkh
-	Added the keyspan pda code from Brian Warner <warner@lothar.com>
-	Moved a bunch of the port specific stuff into its own structure. This
-	is in anticipation of the true multiport devices (there's a bug if you
-	try to access more than one port of any multiport device right now)
-
- (02/21/2000) gkh
-	Made it so that any serial devices only have to specify which functions
-	they want to overload from the generic function calls (great,
-	inheritance in C, in a driver, just what I wanted...)
-	Added support for set_termios and ioctl function calls. No drivers take
-	advantage of this yet.
-	Removed the #ifdef MODULE, now there is no module specific code.
-	Cleaned up a few comments in usb-serial.h that were wrong (thanks again
-	to Miles Lott).
-	Small fix to get_free_serial.
-
- (02/14/2000) gkh
-	Removed the Belkin and Peracom functionality from the driver due to
-	the lack of support from the vendor, and me not wanting people to
-	accidenatly buy the device, expecting it to work with Linux.
-	Added read_bulk_callback and write_bulk_callback to the type structure
-	for the needs of the FTDI and WhiteHEAT driver.
-	Changed all reverences to FTDI to FTDI_SIO at the request of Bill
-	Ryder.
-	Changed the output urb size back to the max endpoint size to make
-	the ftdi_sio driver have it easier, and due to the fact that it didn't
-	really increase the speed any.
-
- (02/11/2000) gkh
-	Added VISOR_FUNCTION_CONSOLE to the visor startup function. This was a
-	patch from Miles Lott (milos@insync.net).
-	Fixed bug with not restoring the minor range that a device grabs, if
-	the startup function fails (thanks Miles for finding this).
-
- (02/05/2000) gkh
-	Added initial framework for the Keyspan PDA serial converter so that
-	Brian Warner has a place to put his code.
-	Made the ezusb specific functions generic enough that different
-	devices can use them (whiteheat and keyspan_pda both need them).
-	Split out a whole bunch of structure and other stuff to a separate
-	usb-serial.h file.
-	Made the Visor connection messages a little more understandable, now
-	that Miles Lott (milos@insync.net) has gotten the Generic channel to
-	work. Also made them always show up in the log file.
-
- (01/25/2000) gkh
-	Added initial framework for FTDI serial converter so that Bill Ryder
-	has a place to put his code.
-	Added the vendor specific info from Handspring. Now we can print out
-	informational debug messages as well as understand what is happening.
-
- (01/23/2000) gkh
-	Fixed problem of crash when trying to open a port that didn't have a
-	device assigned to it. Made the minor node finding a little smarter,
-	now it looks to find a continuous space for the new device.
-
- (01/21/2000) gkh
-	Fixed bug in visor_startup with patch from Miles Lott (milos@insync.net)
-	Fixed get_serial_by_minor which was all messed up for multi port
-	devices. Fixed multi port problem for generic devices. Now the number
-	of ports is determined by the number of bulk out endpoints for the
-	generic device.
-
- (01/19/2000) gkh
-	Removed lots of cruft that was around from the old (pre urb) driver
-	interface.
-	Made the serial_table dynamic. This should save lots of memory when
-	the number of minor nodes goes up to 256.
-	Added initial support for devices that have more than one port.
-	Added more debugging comments for the Visor, and added a needed
-	set_configuration call.
-
- (01/17/2000) gkh
-	Fixed the WhiteHEAT firmware (my processing tool had a bug)
-	and added new debug loader firmware for it.
-	Removed the put_char function as it isn't really needed.
-	Added visor startup commands as found by the Win98 dump.
-
- (01/13/2000) gkh
-	Fixed the vendor id for the generic driver to the one I meant it to be.
-
- (01/12/2000) gkh
-	Forget the version numbering...that's pretty useless...
-	Made the driver able to be compiled so that the user can select which
-	converter they want to use. This allows people who only want the Visor
-	support to not pay the memory size price of the WhiteHEAT.
-	Fixed bug where the generic driver (idVendor=0000 and idProduct=0000)
-	grabbed the root hub. Not good.
-
- version 0.4.0 (01/10/2000) gkh
-	Added whiteheat.h containing the firmware for the ConnectTech WhiteHEAT
-	device. Added startup function to allow firmware to be downloaded to
-	a device if it needs to be.
-	Added firmware download logic to the WhiteHEAT device.
-	Started to add #defines to split up the different drivers for potential
-	configuration option.
-
- version 0.3.1 (12/30/99) gkh
-      Fixed problems with urb for bulk out.
-      Added initial support for multiple sets of endpoints. This enables
-      the Handspring Visor to be attached successfully. Only the first
-      bulk in / bulk out endpoint pair is being used right now.
-
- version 0.3.0 (12/27/99) gkh
-	Added initial support for the Handspring Visor based on a patch from
-	Miles Lott (milos@sneety.insync.net)
-	Cleaned up the code a bunch and converted over to using urbs only.
-
- version 0.2.3 (12/21/99) gkh
-	Added initial support for the Connect Tech WhiteHEAT converter.
-	Incremented the number of ports in expectation of getting the
-	WhiteHEAT to work properly (4 ports per connection).
-	Added notification on insertion and removal of what port the
-	device is/was connected to (and what kind of device it was).
-
- version 0.2.2 (12/16/99) gkh
-	Changed major number to the new allocated number. We're legal now!
-
- version 0.2.1 (12/14/99) gkh
-	Fixed bug that happens when device node is opened when there isn't a
-	device attached to it. Thanks to marek@webdesign.no for noticing this.
-
- version 0.2.0 (11/10/99) gkh
-	Split up internals to make it easier to add different types of serial
-	converters to the code.
-	Added a "generic" driver that gets it's vendor and product id
-	from when the module is loaded. Thanks to David E. Nelson (dnelson@jump.net)
-	for the idea and sample code (from the usb scanner driver.)
-	Cleared up any licensing questions by releasing it under the GNU GPL.
-
- version 0.1.2 (10/25/99) gkh
- 	Fixed bug in detecting device.
-
- version 0.1.1 (10/05/99) gkh
- 	Changed the major number to not conflict with anything else.
-
- version 0.1 (09/28/99) gkh
- 	Can recognize the two different devices and start up a read from
-	device when asked to. Writes also work. No control signals yet, this
-	all is vendor specific data (i.e. no spec), also no control for
-	different baud rates or other bit settings.
-	Currently we are using the same devid as the acm driver. This needs
-	to change.
-
------------------------------------------------------------------------
-visor.c Change Log comments:
-
- (06/03/2003) Judd Montgomery <judd at jpilot.org>
-     Added support for module parameter options for untested/unknown
-     devices.
-
- (03/09/2003) gkh
-	Added support for the Sony Clie NZ90V device.  Thanks to Martin Brachtl
-	<brachtl@redgrep.cz> for the information.
-
- (03/05/2003) gkh
-	Think Treo support is now working.
-
- (04/03/2002) gkh
-	Added support for the Sony OS 4.1 devices.  Thanks to Hiroyuki ARAKI
-	<hiro@zob.ne.jp> for the information.
-
- (03/27/2002) gkh
-	Removed assumptions that port->tty was always valid (is not true
-	for usb serial console devices.)
-
- (03/23/2002) gkh
-	Added support for the Palm i705 device, thanks to Thomas Riemer
-	<tom@netmech.com> for the information.
-
- (03/21/2002) gkh
-	Added support for the Palm m130 device, thanks to Udo Eisenbarth
-	<udo.eisenbarth@web.de> for the information.
-
- (02/27/2002) gkh
-	Reworked the urb handling logic.  We have no more pool, but dynamically
-	allocate the urb and the transfer buffer on the fly.  In testing this
-	does not incure any measurable overhead.  This also relies on the fact
-	that we have proper reference counting logic for urbs.
-
- (02/21/2002) SilaS
-  Added initial support for the Palm m515 devices.
-
- (02/14/2002) gkh
-	Added support for the Clie S-360 device.
-
- (12/18/2001) gkh
-	Added better Clie support for 3.5 devices.  Thanks to Geoffrey Levand
-	for the patch.
-
- (11/11/2001) gkh
-	Added support for the m125 devices, and added check to prevent oopses
-	for Clié devices that lie about the number of ports they have.
-
- (08/30/2001) gkh
-	Added support for the Clie devices, both the 3.5 and 4.0 os versions.
-	Many thanks to Daniel Burke, and Bryan Payne for helping with this.
-
- (08/23/2001) gkh
-	fixed a few potential bugs pointed out by Oliver Neukum.
-
- (05/30/2001) gkh
-	switched from using spinlock to a semaphore, which fixes lots of problems.
-
- (05/28/2000) gkh
-	Added initial support for the Palm m500 and Palm m505 devices.
-
- (04/08/2001) gb
-	Identify version on module load.
-
- (01/21/2000) gkh
-	Added write_room and chars_in_buffer, as they were previously using the
-	generic driver versions which is all wrong now that we are using an urb
-	pool.  Thanks to Wolfgang Grandegger for pointing this out to me.
-	Removed count assignment in the write function, which was not needed anymore
-	either.  Thanks to Al Borchers for pointing this out.
-
- (12/12/2000) gkh
-	Moved MOD_DEC to end of visor_close to be nicer, as the final write
-	message can sleep.
-
- (11/12/2000) gkh
-	Fixed bug with data being dropped on the floor by forcing tty->low_latency
-	to be on.  Hopefully this fixes the OHCI issue!
-
- (11/01/2000) Adam J. Richter
-	usb_device_id table support
-
- (10/05/2000) gkh
-	Fixed bug with urb->dev not being set properly, now that the usb
-	core needs it.
-
- (09/11/2000) gkh
-	Got rid of always calling kmalloc for every urb we wrote out to the
-	device.
-	Added visor_read_callback so we can keep track of bytes in and out for
-	those people who like to know the speed of their device.
-	Removed DEBUG #ifdefs with call to usb_serial_debug_data
-
- (09/06/2000) gkh
-	Fixed oops in visor_exit.  Need to uncomment usb_unlink_urb call _after_
-	the host controller drivers set urb->dev = NULL when the urb is finished.
-
- (08/28/2000) gkh
-	Added locks for SMP safeness.
-
- (08/08/2000) gkh
-	Fixed endian problem in visor_startup.
-	Fixed MOD_INC and MOD_DEC logic and the ability to open a port more
-	than once.
-
- (07/23/2000) gkh
-	Added pool of write urbs to speed up transfers to the visor.
-
- (07/19/2000) gkh
-	Added module_init and module_exit functions to handle the fact that this
-	driver is a loadable module now.
-
- (07/03/2000) gkh
-	Added visor_set_ioctl and visor_set_termios functions (they don't do much
-	of anything, but are good for debugging.)
-
- (06/25/2000) gkh
-	Fixed bug in visor_unthrottle that should help with the disconnect in PPP
-	bug that people have been reporting.
-
- (06/23/2000) gkh
-	Cleaned up debugging statements in a quest to find UHCI timeout bug.
-
- (04/27/2000) Ryan VanderBijl
- 	Fixed memory leak in visor_close
-
- (03/26/2000) gkh
-	Split driver up into device specific pieces.
-
------------------------------------------------------------------------
-pl2303.c Change Log comments:
-
- 2002_Mar_26 gkh
-	allowed driver to work properly if there is no tty assigned to a port
-	(this happens for serial console devices.)
-
- 2001_Oct_06 gkh
-	Added RTS and DTR line control.  Thanks to joe@bndlg.de for parts of it.
-
- 2001_Sep_19 gkh
-	Added break support.
-
- 2001_Aug_30 gkh
-	fixed oops in write_bulk_callback.
-
- 2001_Aug_28 gkh
-	reworked buffer logic to be like other usb-serial drivers.  Hopefully
-	removing some reported problems.
-
- 2001_Jun_06 gkh
-	finished porting to 2.4 format.
-
-
------------------------------------------------------------------------
-io_edgeport.c Change Log comments:
-
- 2003_04_03 al borchers
-  - fixed a bug (that shows up with dosemu) where the tty struct is
-    used in a callback after it has been freed
-
- 2.3 2002_03_08 greg kroah-hartman
-	- fixed bug when multiple devices were attached at the same time.
-
- 2.2 2001_11_14 greg kroah-hartman
-	- fixed bug in edge_close that kept the port from being used more
-	  than once.
-	- fixed memory leak on device removal.
-	- fixed potential double free of memory when command urb submitting
-	  failed.
-	- other small cleanups when the device is removed
-
- 2.1 2001_07_09 greg kroah-hartman
-	- added support for TIOCMBIS and TIOCMBIC.
-
-     (04/08/2001) gb
-	- Identify version on module load.
-
- 2.0 2001_03_05 greg kroah-hartman
-	- reworked entire driver to fit properly in with the other usb-serial
-	  drivers.  Occasional oopses still happen, but it's a good start.
-
- 1.2.3 (02/23/2001) greg kroah-hartman
-	- changed device table to work properly for 2.4.x final format.
-	- fixed problem with dropping data at high data rates.
-
- 1.2.2 (11/27/2000) greg kroah-hartman
-	- cleaned up more NTisms.
-	- Added device table for 2.4.0-test11
-
- 1.2.1 (11/08/2000) greg kroah-hartman
-	- Started to clean up NTisms.
-	- Fixed problem with dev field of urb for kernels >= 2.4.0-test9
-
- 1.2 (10/17/2000) David Iacovelli
- 	Remove all EPIC code and GPL source
-  Fix RELEVANT_IFLAG macro to include flow control
-  changes port configuration changes.
-  Fix redefinition of SERIAL_MAGIC
-  Change all timeout values to 5 seconds
-  Tried to fix the UHCI multiple urb submission, but failed miserably.
-  it seems to work fine with OHCI.
-  ( Greg take a look at the #if 0 at end of WriteCmdUsb() we must
-    find a way to work arount this UHCI bug )
-
- 1.1 (10/11/2000) David Iacovelli
-  Fix XON/XOFF flow control to support both IXON and IXOFF
-
- 0.9.27 (06/30/2000) David Iacovelli
-  Added transmit queue and now allocate urb for command writes.
-
- 0.9.26 (06/29/2000) David Iacovelli
-  Add support for 80251 based edgeport
-
- 0.9.25 (06/27/2000) David Iacovelli
-  Do not close the port if it has multiple opens.
-
- 0.9.24 (05/26/2000) David Iacovelli
-  Add IOCTLs to support RXTX and JAVA POS
-  and first cut at running BlackBox Demo
-
- 0.9.23 (05/24/2000) David Iacovelli
-  Add IOCTLs to support RXTX and JAVA POS
-
- 0.9.22 (05/23/2000) David Iacovelli
-  fixed bug in enumeration.  If epconfig turns on mapping by
-  path after a device is already plugged in, we now update
-  the mapping correctly
-
- 0.9.21 (05/16/2000) David Iacovelli
-  Added BlockUntilChaseResp() to also wait for txcredits
-  Updated the way we allocate and handle write URBs
-	Add debug code to dump buffers
-
- 0.9.20 (05/01/2000) David Iacovelli
-	change driver to use usb/tts/
-
- 0.9.19 (05/01/2000) David Iacovelli
-  Update code to compile if DEBUG is off
-
- 0.9.18 (04/28/2000) David Iacovelli
-  cleanup and test tty_register with devfs
-
- 0.9.17 (04/27/2000) greg kroah-hartman
- 	changed tty_register around to be like the way it
- 	was before, but now it works properly with devfs.
-
- 0.9.16 (04/26/2000) david iacovelli
-  Fixed bug in GetProductInfo()
-
- 0.9.15 (04/25/2000) david iacovelli
-	Updated enumeration
-
- 0.9.14 (04/24/2000) david iacovelli
-  Removed all config/status IOCTLS and
-  converted to using /proc/edgeport
-  still playing with devfs
-
- 0.9.13 (04/24/2000) david iacovelli
-  Removed configuration based on ttyUSB0
-  Added support for configuration using /prod/edgeport
-  first attempt at using devfs (not working yet!)
-  Added IOCTL to GetProductInfo()
-  Added support for custom baud rates
-	Add support for random port numbers
-
- 0.9.12 (04/18/2000) david iacovelli
-	added additional configuration IOCTLs
-  use ttyUSB0 for configuration
-
- 0.9.11 (04/17/2000) greg kroah-hartman
-	fixed module initialization race conditions.
-	made all urbs dynamically allocated.
-	made driver devfs compatible. now it only registers the tty device
-	when the device is actually plugged in.
-
- 0.9.10 (04/13/2000) greg kroah-hartman
-	added proc interface framework.
-
- 0.9.9 (04/13/2000) david iacovelli
-	added enumeration code and ioctls to configure the device
-
- 0.9.8 (04/12/2000) david iacovelli
-  Change interrupt read start when device is plugged in
-  and stop when device is removed
-	process interrupt reads when all ports are closed
-  (keep value of rxBytesAvail consistent with the edgeport)
-  set the USB_BULK_QUEUE flag so that we can shove a bunch
-  of urbs at once down the pipe
-
- 0.9.7 (04/10/2000) david iacovelli
- 	start to add enumeration code.
-  generate serial number for epic devices
-  add support for kdb
-
- 0.9.6 (03/30/2000) david iacovelli
-  add IOCTL to get string, manufacture, and boot descriptors
-
- 0.9.5 (03/14/2000) greg kroah-hartman
-	more error checking added to SerialOpen to try to fix UHCI open problem
-
- 0.9.4 (03/09/2000) greg kroah-hartman
-	added more error checking to handle oops when data is hanging
-	around and tty is abruptly closed.
-
- 0.9.3 (03/09/2000) david iacovelli
-	Add epic support for xon/xoff chars
-	play with performance
-
- 0.9.2 (03/08/2000) greg kroah-hartman
-	changed most "info" calls to "dbg"
-	implemented flow control properly in the termios call
-
- 0.9.1 (03/08/2000) david iacovelli
-	added EPIC support
-	enabled bootloader update
-
- 0.9 (03/08/2000) greg kroah-hartman
-	Release to IO networks.
-	Integrated changes that David made
-  made getting urbs for writing SMP safe
-
- 0.8 (03/07/2000) greg kroah-hartman
-	Release to IO networks.
-	Fixed problems that were seen in code by David.
-  Now both Edgeport/4 and Edgeport/2 works properly.
-  Changed most of the functions to use port instead of serial.
-
- 0.7 (02/27/2000) greg kroah-hartman
-	Milestone 3 release.
-	Release to IO Networks
-	ioctl for waiting on line change implemented.
-	ioctl for getting statistics implemented.
-	multiport support working.
-	lsr and msr registers are now handled properly.
-	change break now hooked up and working.
-	support for all known Edgeport devices.
-
- 0.6 (02/22/2000) greg kroah-hartman
-	Release to IO networks.
-	CHASE is implemented correctly when port is closed.
-	SerialOpen now blocks correctly until port is fully opened.
-
- 0.5 (02/20/2000) greg kroah-hartman
-	Release to IO networks.
-	Known problems:
-		modem status register changes are not sent on to the user
-		CHASE is not implemented when the port is closed.
-
- 0.4 (02/16/2000) greg kroah-hartman
-	Second cut at the CeBit demo.
-	Doesn't leak memory on every write to the port
-	Still small leaks on startup.
-	Added support for Edgeport/2 and Edgeport/8
-
- 0.3 (02/15/2000) greg kroah-hartman
-	CeBit demo release.
-	Force the line settings to 4800, 8, 1, e for the demo.
-	Warning! This version leaks memory like crazy!
-
- 0.2 (01/30/2000) greg kroah-hartman
-	Milestone 1 release.
-	Device is found by USB subsystem, enumerated, firmware is downloaded
-	and the descriptors are printed to the debug log, config is set, and
-	green light starts to blink. Open port works, and data can be sent
-	and received at the default settings of the UART. Loopback connector
-	and debug log confirms this.
-
- 0.1 (01/23/2000) greg kroah-hartman
-	Initial release to help IO Networks try to set up their test system.
-	Edgeport4 is recognized, firmware is downloaded, config is set so
-	device blinks green light every 3 sec. Port is bound, but opening,
-	closing, and sending data do not work properly.
-
-
diff --git a/drivers/usb/serial/belkin_sa.c b/drivers/usb/serial/belkin_sa.c
index d6921fa1403c..f9f29b289f2f 100644
--- a/drivers/usb/serial/belkin_sa.c
+++ b/drivers/usb/serial/belkin_sa.c
@@ -20,50 +20,7 @@
  * TODO:
  * -- Add true modem contol line query capability.  Currently we track the
  *    states reported by the interrupt and the states we request.
- * -- Add error reporting back to application for UART error conditions.
- *    Just point me at how to implement this and I'll do it. I've put the
- *    framework in, but haven't analyzed the "tty_flip" interface yet.
  * -- Add support for flush commands
- * -- Add everything that is missing :)
- *
- * 27-Nov-2001 gkh
- * 	compressed all the differnent device entries into 1.
- *
- * 30-May-2001 gkh
- *	switched from using spinlock to a semaphore, which fixes lots of
- *	problems.
- *
- * 08-Apr-2001 gb
- *	- Identify version on module load.
- *
- * 12-Mar-2001 gkh
- *	- Added support for the GoHubs GO-COM232 device which is the same as the
- *	  Peracom device.
- *
- * 06-Nov-2000 gkh
- *	- Added support for the old Belkin and Peracom devices.
- *	- Made the port able to be opened multiple times.
- *	- Added some defaults incase the line settings are things these devices
- *	  can't support.
- *
- * 18-Oct-2000 William Greathouse
- *    Released into the wild (linux-usb-devel)
- *
- * 17-Oct-2000 William Greathouse
- *    Add code to recognize firmware version and set hardware flow control
- *    appropriately.  Belkin states that firmware prior to 3.05 does not
- *    operate correctly in hardware handshake mode.  I have verified this
- *    on firmware 2.05 -- for both RTS and DTR input flow control, the control
- *    line is not reset.  The test performed by the Belkin Win* driver is
- *    to enable hardware flow control for firmware 2.06 or greater and
- *    for 1.00 or prior.  I am only enabling for 2.06 or greater.
- *
- * 12-Oct-2000 William Greathouse
- *    First cut at supporting Belkin USB Serial Adapter F5U103
- *    I did not have a copy of the original work to support this
- *    adapter, so pardon any stupid mistakes.  All of the information
- *    I am using to write this driver was acquired by using a modified
- *    UsbSnoop on Windows2000 and from examining the other USB drivers.
  */
 
 #include <linux/kernel.h>
diff --git a/drivers/usb/serial/ch341.c b/drivers/usb/serial/ch341.c
index 6ae1c0688b5e..0e77511060c0 100644
--- a/drivers/usb/serial/ch341.c
+++ b/drivers/usb/serial/ch341.c
@@ -335,13 +335,12 @@ static int ch341_open(struct tty_struct *tty, struct usb_serial_port *port)
 		goto out;
 
 	dbg("%s - submitting interrupt urb", __func__);
-	port->interrupt_in_urb->dev = serial->dev;
 	r = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (r) {
 		dev_err(&port->dev, "%s - failed submitting interrupt urb,"
 			" error %d\n", __func__, r);
 		ch341_close(port);
-		return -EPROTO;
+		goto out;
 	}
 
 	r = usb_serial_generic_open(tty, port);
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index fd67cc53545b..adfe660ed008 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -92,6 +92,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x10C4, 0x818B) }, /* AVIT Research USB to TTL */
 	{ USB_DEVICE(0x10C4, 0x819F) }, /* MJS USB Toslink Switcher */
 	{ USB_DEVICE(0x10C4, 0x81A6) }, /* ThinkOptics WavIt */
+	{ USB_DEVICE(0x10C4, 0x81A9) }, /* Multiplex RC Interface */
 	{ USB_DEVICE(0x10C4, 0x81AC) }, /* MSD Dash Hawk */
 	{ USB_DEVICE(0x10C4, 0x81AD) }, /* INSYS USB Modem */
 	{ USB_DEVICE(0x10C4, 0x81C8) }, /* Lipowsky Industrie Elektronik GmbH, Baby-JTAG */
@@ -280,7 +281,10 @@ static int cp210x_get_config(struct usb_serial_port *port, u8 request,
 		dbg("%s - Unable to send config request, "
 				"request=0x%x size=%d result=%d\n",
 				__func__, request, size, result);
-		return -EPROTO;
+		if (result > 0)
+			result = -EPROTO;
+
+		return result;
 	}
 
 	return 0;
@@ -331,7 +335,10 @@ static int cp210x_set_config(struct usb_serial_port *port, u8 request,
 		dbg("%s - Unable to send request, "
 				"request=0x%x size=%d result=%d\n",
 				__func__, request, size, result);
-		return -EPROTO;
+		if (result > 0)
+			result = -EPROTO;
+
+		return result;
 	}
 
 	return 0;
@@ -395,10 +402,11 @@ static int cp210x_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	dbg("%s - port %d", __func__, port->number);
 
-	if (cp210x_set_config_single(port, CP210X_IFC_ENABLE, UART_ENABLE)) {
-		dev_err(&port->dev, "%s - Unable to enable UART\n",
-				__func__);
-		return -EPROTO;
+	result = cp210x_set_config_single(port, CP210X_IFC_ENABLE,
+								UART_ENABLE);
+	if (result) {
+		dev_err(&port->dev, "%s - Unable to enable UART\n", __func__);
+		return result;
 	}
 
 	result = usb_serial_generic_open(tty, port);
@@ -520,18 +528,13 @@ static void cp210x_get_termios_port(struct usb_serial_port *port,
 		cflag |= PARENB;
 		break;
 	case BITS_PARITY_MARK:
-		dbg("%s - parity = MARK (not supported, disabling parity)",
-				__func__);
-		cflag &= ~PARENB;
-		bits &= ~BITS_PARITY_MASK;
-		cp210x_set_config(port, CP210X_SET_LINE_CTL, &bits, 2);
+		dbg("%s - parity = MARK", __func__);
+		cflag |= (PARENB|PARODD|CMSPAR);
 		break;
 	case BITS_PARITY_SPACE:
-		dbg("%s - parity = SPACE (not supported, disabling parity)",
-				__func__);
-		cflag &= ~PARENB;
-		bits &= ~BITS_PARITY_MASK;
-		cp210x_set_config(port, CP210X_SET_LINE_CTL, &bits, 2);
+		dbg("%s - parity = SPACE", __func__);
+		cflag &= ~PARODD;
+		cflag |= (PARENB|CMSPAR);
 		break;
 	default:
 		dbg("%s - Unknown parity mode, disabling parity", __func__);
@@ -588,7 +591,6 @@ static void cp210x_set_termios(struct tty_struct *tty,
 	if (!tty)
 		return;
 
-	tty->termios->c_cflag &= ~CMSPAR;
 	cflag = tty->termios->c_cflag;
 	old_cflag = old_termios->c_cflag;
 	baud = cp210x_quantise_baudrate(tty_get_baud_rate(tty));
@@ -643,16 +645,27 @@ static void cp210x_set_termios(struct tty_struct *tty,
 					"not supported by device\n");
 	}
 
-	if ((cflag & (PARENB|PARODD)) != (old_cflag & (PARENB|PARODD))) {
+	if ((cflag     & (PARENB|PARODD|CMSPAR)) !=
+	    (old_cflag & (PARENB|PARODD|CMSPAR))) {
 		cp210x_get_config(port, CP210X_GET_LINE_CTL, &bits, 2);
 		bits &= ~BITS_PARITY_MASK;
 		if (cflag & PARENB) {
-			if (cflag & PARODD) {
-				bits |= BITS_PARITY_ODD;
-				dbg("%s - parity = ODD", __func__);
+			if (cflag & CMSPAR) {
+			    if (cflag & PARODD) {
+				    bits |= BITS_PARITY_MARK;
+				    dbg("%s - parity = MARK", __func__);
+			    } else {
+				    bits |= BITS_PARITY_SPACE;
+				    dbg("%s - parity = SPACE", __func__);
+			    }
 			} else {
-				bits |= BITS_PARITY_EVEN;
-				dbg("%s - parity = EVEN", __func__);
+			    if (cflag & PARODD) {
+				    bits |= BITS_PARITY_ODD;
+				    dbg("%s - parity = ODD", __func__);
+			    } else {
+				    bits |= BITS_PARITY_EVEN;
+				    dbg("%s - parity = EVEN", __func__);
+			    }
 			}
 		}
 		if (cp210x_set_config(port, CP210X_SET_LINE_CTL, &bits, 2))
diff --git a/drivers/usb/serial/cyberjack.c b/drivers/usb/serial/cyberjack.c
index f744ab7a3b19..98bf83349838 100644
--- a/drivers/usb/serial/cyberjack.c
+++ b/drivers/usb/serial/cyberjack.c
@@ -138,7 +138,6 @@ static int cyberjack_startup(struct usb_serial *serial)
 
 	for (i = 0; i < serial->num_ports; ++i) {
 		int result;
-		serial->port[i]->interrupt_in_urb->dev = serial->dev;
 		result = usb_submit_urb(serial->port[i]->interrupt_in_urb,
 					GFP_KERNEL);
 		if (result)
@@ -208,7 +207,6 @@ static void cyberjack_close(struct usb_serial_port *port)
 static int cyberjack_write(struct tty_struct *tty,
 	struct usb_serial_port *port, const unsigned char *buf, int count)
 {
-	struct usb_serial *serial = port->serial;
 	struct cyberjack_private *priv = usb_get_serial_port_data(port);
 	unsigned long flags;
 	int result;
@@ -221,22 +219,18 @@ static int cyberjack_write(struct tty_struct *tty,
 		return 0;
 	}
 
-	spin_lock_bh(&port->lock);
-	if (port->write_urb_busy) {
-		spin_unlock_bh(&port->lock);
+	if (!test_and_clear_bit(0, &port->write_urbs_free)) {
 		dbg("%s - already writing", __func__);
 		return 0;
 	}
-	port->write_urb_busy = 1;
-	spin_unlock_bh(&port->lock);
 
 	spin_lock_irqsave(&priv->lock, flags);
 
 	if (count+priv->wrfilled > sizeof(priv->wrbuf)) {
 		/* To much data for buffer. Reset buffer. */
 		priv->wrfilled = 0;
-		port->write_urb_busy = 0;
 		spin_unlock_irqrestore(&priv->lock, flags);
+		set_bit(0, &port->write_urbs_free);
 		return 0;
 	}
 
@@ -265,13 +259,7 @@ static int cyberjack_write(struct tty_struct *tty,
 		priv->wrsent = length;
 
 		/* set up our urb */
-		usb_fill_bulk_urb(port->write_urb, serial->dev,
-			      usb_sndbulkpipe(serial->dev, port->bulk_out_endpointAddress),
-			      port->write_urb->transfer_buffer, length,
-			      ((serial->type->write_bulk_callback) ?
-			       serial->type->write_bulk_callback :
-			       cyberjack_write_bulk_callback),
-			      port);
+		port->write_urb->transfer_buffer_length = length;
 
 		/* send the data out the bulk port */
 		result = usb_submit_urb(port->write_urb, GFP_ATOMIC);
@@ -283,7 +271,7 @@ static int cyberjack_write(struct tty_struct *tty,
 			priv->wrfilled = 0;
 			priv->wrsent = 0;
 			spin_unlock_irqrestore(&priv->lock, flags);
-			port->write_urb_busy = 0;
+			set_bit(0, &port->write_urbs_free);
 			return 0;
 		}
 
@@ -351,7 +339,6 @@ static void cyberjack_read_int_callback(struct urb *urb)
 		spin_unlock(&priv->lock);
 
 		if (!old_rdtodo) {
-			port->read_urb->dev = port->serial->dev;
 			result = usb_submit_urb(port->read_urb, GFP_ATOMIC);
 			if (result)
 				dev_err(&port->dev, "%s - failed resubmitting "
@@ -362,7 +349,6 @@ static void cyberjack_read_int_callback(struct urb *urb)
 	}
 
 resubmit:
-	port->interrupt_in_urb->dev = port->serial->dev;
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_ATOMIC);
 	if (result)
 		dev_err(&port->dev, "usb_submit_urb(read int) failed\n");
@@ -415,7 +401,6 @@ static void cyberjack_read_bulk_callback(struct urb *urb)
 
 	/* Continue to read if we have still urbs to do. */
 	if (todo /* || (urb->actual_length==port->bulk_in_endpointAddress)*/) {
-		port->read_urb->dev = port->serial->dev;
 		result = usb_submit_urb(port->read_urb, GFP_ATOMIC);
 		if (result)
 			dev_err(&port->dev, "%s - failed resubmitting read "
@@ -432,7 +417,7 @@ static void cyberjack_write_bulk_callback(struct urb *urb)
 
 	dbg("%s - port %d", __func__, port->number);
 
-	port->write_urb_busy = 0;
+	set_bit(0, &port->write_urbs_free);
 	if (status) {
 		dbg("%s - nonzero write bulk status received: %d",
 		    __func__, status);
@@ -455,13 +440,7 @@ static void cyberjack_write_bulk_callback(struct urb *urb)
 		priv->wrsent += length;
 
 		/* set up our urb */
-		usb_fill_bulk_urb(port->write_urb, port->serial->dev,
-			      usb_sndbulkpipe(port->serial->dev, port->bulk_out_endpointAddress),
-			      port->write_urb->transfer_buffer, length,
-			      ((port->serial->type->write_bulk_callback) ?
-			       port->serial->type->write_bulk_callback :
-			       cyberjack_write_bulk_callback),
-			      port);
+		port->write_urb->transfer_buffer_length = length;
 
 		/* send the data out the bulk port */
 		result = usb_submit_urb(port->write_urb, GFP_ATOMIC);
diff --git a/drivers/usb/serial/cypress_m8.c b/drivers/usb/serial/cypress_m8.c
index d9906eb9d16a..07680d6b792b 100644
--- a/drivers/usb/serial/cypress_m8.c
+++ b/drivers/usb/serial/cypress_m8.c
@@ -16,32 +16,6 @@
  *
  * See http://geocities.com/i0xox0i for information on this driver and the
  * earthmate usb device.
- *
- *  Lonnie Mendez <dignome@gmail.com>
- *  4-29-2005
- *	Fixed problem where setting or retreiving the serial config would fail
- *	with EPIPE.  Removed CRTS toggling so the driver behaves more like
- *	other usbserial adapters.  Issued new interval of 1ms instead of the
- *	default 10ms.  As a result, transfer speed has been substantially
- *	increased from avg. 850bps to avg. 3300bps.  initial termios has also
- *	been modified.  Cleaned up code and formatting issues so it is more
- *	readable.  Replaced the C++ style comments.
- *
- *  Lonnie Mendez <dignome@gmail.com>
- *  12-15-2004
- *	Incorporated write buffering from pl2303 driver.  Fixed bug with line
- *	handling so both lines are raised in cypress_open. (was dropping rts)
- *      Various code cleanups made as well along with other misc bug fixes.
- *
- *  Lonnie Mendez <dignome@gmail.com>
- *  04-10-2004
- *	Driver modified to support dynamic line settings.  Various improvements
- *      and features.
- *
- *  Neil Whelchel
- *  10-2003
- *	Driver first released.
- *
  */
 
 /* Thanks to Neil Whelchel for writing the first cypress m8 implementation
@@ -1162,8 +1136,6 @@ static void cypress_unthrottle(struct tty_struct *tty)
 		return;
 
 	if (actually_throttled) {
-		port->interrupt_in_urb->dev = port->serial->dev;
-
 		result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 		if (result) {
 			dev_err(&port->dev, "%s - failed submitting read urb, "
@@ -1352,7 +1324,6 @@ static void cypress_write_int_callback(struct urb *urb)
 		dbg("%s - nonzero write bulk status received: %d",
 			__func__, status);
 		port->interrupt_out_urb->transfer_buffer_length = 1;
-		port->interrupt_out_urb->dev = port->serial->dev;
 		result = usb_submit_urb(port->interrupt_out_urb, GFP_ATOMIC);
 		if (!result)
 			return;
diff --git a/drivers/usb/serial/digi_acceleport.c b/drivers/usb/serial/digi_acceleport.c
index e92cbefc0f88..6d26a77d0f2a 100644
--- a/drivers/usb/serial/digi_acceleport.c
+++ b/drivers/usb/serial/digi_acceleport.c
@@ -13,222 +13,6 @@
 *
 *  Peter Berger (pberger@brimson.com)
 *  Al Borchers (borchers@steinerpoint.com)
-* 
-* (12/03/2001) gkh
-*	switched to using port->port.count instead of private version.
-*	Removed port->active
-*
-* (04/08/2001) gb
-*	Identify version on module load.
-*
-* (11/01/2000) Adam J. Richter
-*	usb_device_id table support
-* 
-* (11/01/2000) pberger and borchers
-*    -- Turned off the USB_DISABLE_SPD flag for write bulk urbs--it caused
-*       USB 4 ports to hang on startup.
-*    -- Serialized access to write urbs by adding the dp_write_urb_in_use
-*       flag; otherwise, the driver caused SMP system hangs.  Watching the
-*       urb status is not sufficient.
-*
-* (10/05/2000) gkh
-*    -- Fixed bug with urb->dev not being set properly, now that the usb
-*	core needs it.
-* 
-*  (8/8/2000) pberger and borchers
-*    -- Fixed close so that 
-*       - it can timeout while waiting for transmit idle, if needed;
-*       - it ignores interrupts when flushing the port, turning
-*         of modem signalling, and so on;
-*       - it waits for the flush to really complete before returning.
-*    -- Read_bulk_callback and write_bulk_callback check for a closed
-*       port before using the tty struct or writing to the port.
-*    -- The two changes above fix the oops caused by interrupted closes.
-*    -- Added interruptible args to write_oob_command and set_modem_signals
-*       and added a timeout arg to transmit_idle; needed for fixes to
-*       close.
-*    -- Added code for rx_throttle and rx_unthrottle so that input flow
-*       control works.
-*    -- Added code to set overrun, parity, framing, and break errors
-*       (untested).
-*    -- Set USB_DISABLE_SPD flag for write bulk urbs, so no 0 length
-*       bulk writes are done.  These hung the Digi USB device.  The
-*       0 length bulk writes were a new feature of usb-uhci added in
-*       the 2.4.0-test6 kernels.
-*    -- Fixed mod inc race in open; do mod inc before sleeping to wait
-*       for a close to finish.
-*
-*  (7/31/2000) pberger
-*    -- Fixed bugs with hardware handshaking:
-*       - Added code to set/clear tty->hw_stopped in digi_read_oob_callback()
-*         and digi_set_termios()
-*    -- Added code in digi_set_termios() to
-*       - add conditional in code handling transition from B0 to only
-*         set RTS if RTS/CTS flow control is either not in use or if
-*         the port is not currently throttled.
-*       - handle turning off CRTSCTS.
-*
-*  (7/30/2000) borchers
-*    -- Added support for more than one Digi USB device by moving
-*       globals to a private structure in the pointed to from the
-*       usb_serial structure.
-*    -- Moved the modem change and transmit idle wait queues into
-*       the port private structure, so each port has its own queue
-*       rather than sharing global queues.
-*    -- Added support for break signals.
-*
-*  (7/25/2000) pberger
-*    -- Added USB-2 support.  Note: the USB-2 supports 3 devices: two
-*       serial and a parallel port.  The parallel port is implemented
-*       as a serial-to-parallel converter.  That is, the driver actually
-*       presents all three USB-2 interfaces as serial ports, but the third
-*       one physically connects to a parallel device.  Thus, for example,
-*       one could plug a parallel printer into the USB-2's third port,
-*       but from the kernel's (and userland's) point of view what's
-*       actually out there is a serial device.
-*
-*  (7/15/2000) borchers
-*    -- Fixed race in open when a close is in progress.
-*    -- Keep count of opens and dec the module use count for each
-*       outstanding open when shutdown is called (on disconnect).
-*    -- Fixed sanity checks in read_bulk_callback and write_bulk_callback
-*       so pointers are checked before use.
-*    -- Split read bulk callback into in band and out of band
-*       callbacks, and no longer restart read chains if there is
-*       a status error or a sanity error.  This fixed the seg
-*       faults and other errors we used to get on disconnect.
-*    -- Port->active is once again a flag as usb-serial intended it
-*       to be, not a count.  Since it was only a char it would
-*       have been limited to 256 simultaneous opens.  Now the open
-*       count is kept in the port private structure in dp_open_count.
-*    -- Added code for modularization of the digi_acceleport driver.
-*
-*  (6/27/2000) pberger and borchers
-*    -- Zeroed out sync field in the wakeup_task before first use;
-*       otherwise the uninitialized value might prevent the task from
-*       being scheduled.
-*    -- Initialized ret value to 0 in write_bulk_callback, otherwise
-*       the uninitialized value could cause a spurious debugging message.
-*
-*  (6/22/2000) pberger and borchers
-*    -- Made cond_wait_... inline--apparently on SPARC the flags arg
-*       to spin_lock_irqsave cannot be passed to another function
-*       to call spin_unlock_irqrestore.  Thanks to Pauline Middelink.
-*    -- In digi_set_modem_signals the inner nested spin locks use just
-*       spin_lock() rather than spin_lock_irqsave().  The old code
-*       mistakenly left interrupts off.  Thanks to Pauline Middelink.
-*    -- copy_from_user (which can sleep) is no longer called while a
-*       spinlock is held.  We copy to a local buffer before getting
-*       the spinlock--don't like the extra copy but the code is simpler.
-*    -- Printk and dbg are no longer called while a spin lock is held.
-*
-*  (6/4/2000) pberger and borchers
-*    -- Replaced separate calls to spin_unlock_irqrestore and
-*       interruptible_sleep_on_timeout with a new function
-*       cond_wait_interruptible_timeout_irqrestore.  This eliminates
-*       the race condition where the wake up could happen after
-*       the unlock and before the sleep.
-*    -- Close now waits for output to drain.
-*    -- Open waits until any close in progress is finished.
-*    -- All out of band responses are now processed, not just the
-*       first in a USB packet.
-*    -- Fixed a bug that prevented the driver from working when the
-*       first Digi port was not the first USB serial port--the driver
-*       was mistakenly using the external USB serial port number to
-*       try to index into its internal ports.
-*    -- Fixed an SMP bug -- write_bulk_callback is called directly from
-*       an interrupt, so spin_lock_irqsave/spin_unlock_irqrestore are
-*       needed for locks outside write_bulk_callback that are also
-*       acquired by write_bulk_callback to prevent deadlocks.
-*    -- Fixed support for select() by making digi_chars_in_buffer()
-*       return 256 when -EINPROGRESS is set, as the line discipline
-*       code in n_tty.c expects.
-*    -- Fixed an include file ordering problem that prevented debugging
-*       messages from working.
-*    -- Fixed an intermittent timeout problem that caused writes to
-*       sometimes get stuck on some machines on some kernels.  It turns
-*       out in these circumstances write_chan() (in n_tty.c) was
-*       asleep waiting for our wakeup call.  Even though we call
-*       wake_up_interruptible() in digi_write_bulk_callback(), there is
-*       a race condition that could cause the wakeup to fail: if our
-*       wake_up_interruptible() call occurs between the time that our
-*       driver write routine finishes and write_chan() sets current->state
-*       to TASK_INTERRUPTIBLE, the effect of our wakeup setting the state
-*       to TASK_RUNNING will be lost and write_chan's subsequent call to
-*       schedule() will never return (unless it catches a signal).
-*       This race condition occurs because write_bulk_callback() (and thus
-*       the wakeup) are called asynchronously from an interrupt, rather than
-*       from the scheduler.  We can avoid the race by calling the wakeup
-*       from the scheduler queue and that's our fix:  Now, at the end of
-*       write_bulk_callback() we queue up a wakeup call on the scheduler
-*       task queue.  We still also invoke the wakeup directly since that
-*       squeezes a bit more performance out of the driver, and any lost
-*       race conditions will get cleaned up at the next scheduler run.
-*
-*       NOTE:  The problem also goes away if you comment out
-*       the two code lines in write_chan() where current->state
-*       is set to TASK_RUNNING just before calling driver.write() and to
-*       TASK_INTERRUPTIBLE immediately afterwards.  This is why the
-*       problem did not show up with the 2.2 kernels -- they do not
-*       include that code.
-*
-*  (5/16/2000) pberger and borchers
-*    -- Added timeouts to sleeps, to defend against lost wake ups.
-*    -- Handle transition to/from B0 baud rate in digi_set_termios.
-*
-*  (5/13/2000) pberger and borchers
-*    -- All commands now sent on out of band port, using
-*       digi_write_oob_command.
-*    -- Get modem control signals whenever they change, support TIOCMGET/
-*       SET/BIS/BIC ioctls.
-*    -- digi_set_termios now supports parity, word size, stop bits, and
-*       receive enable.
-*    -- Cleaned up open and close, use digi_set_termios and
-*       digi_write_oob_command to set port parameters.
-*    -- Added digi_startup_device to start read chains on all ports.
-*    -- Write buffer is only used when count==1, to be sure put_char can
-*       write a char (unless the buffer is full).
-*
-*  (5/10/2000) pberger and borchers
-*    -- Added MOD_INC_USE_COUNT/MOD_DEC_USE_COUNT calls on open/close.
-*    -- Fixed problem where the first incoming character is lost on
-*       port opens after the first close on that port.  Now we keep
-*       the read_urb chain open until shutdown.
-*    -- Added more port conditioning calls in digi_open and digi_close.
-*    -- Convert port->active to a use count so that we can deal with multiple
-*       opens and closes properly.
-*    -- Fixed some problems with the locking code.
-*
-*  (5/3/2000) pberger and borchers
-*    -- First alpha version of the driver--many known limitations and bugs.
-*
-*
-*  Locking and SMP
-*
-*  - Each port, including the out-of-band port, has a lock used to
-*    serialize all access to the port's private structure.
-*  - The port lock is also used to serialize all writes and access to
-*    the port's URB.
-*  - The port lock is also used for the port write_wait condition
-*    variable.  Holding the port lock will prevent a wake up on the
-*    port's write_wait; this can be used with cond_wait_... to be sure
-*    the wake up is not lost in a race when dropping the lock and
-*    sleeping waiting for the wakeup.
-*  - digi_write() does not sleep, since it is sometimes called on
-*    interrupt time.
-*  - digi_write_bulk_callback() and digi_read_bulk_callback() are
-*    called directly from interrupts.  Hence spin_lock_irqsave()
-*    and spin_unlock_irqrestore() are used in the rest of the code
-*    for any locks they acquire.
-*  - digi_write_bulk_callback() gets the port lock before waking up
-*    processes sleeping on the port write_wait.  It also schedules
-*    wake ups so they happen from the scheduler, because the tty
-*    system can miss wake ups from interrupts.
-*  - All sleeps use a timeout of DIGI_RETRY_TIMEOUT before looping to
-*    recheck the condition they are sleeping on.  This is defensive,
-*    in case a wake up is lost.
-*  - Following Documentation/DocBook/kernel-locking.tmpl no spin locks
-*    are held when calling copy_to/from_user or printk.
 */
 
 #include <linux/kernel.h>
@@ -654,7 +438,6 @@ static int digi_write_oob_command(struct usb_serial_port *port,
 			len &= ~3;
 		memcpy(oob_port->write_urb->transfer_buffer, buf, len);
 		oob_port->write_urb->transfer_buffer_length = len;
-		oob_port->write_urb->dev = port->serial->dev;
 		ret = usb_submit_urb(oob_port->write_urb, GFP_ATOMIC);
 		if (ret == 0) {
 			oob_priv->dp_write_urb_in_use = 1;
@@ -732,7 +515,6 @@ static int digi_write_inb_command(struct usb_serial_port *port,
 			memcpy(data, buf, len);
 			port->write_urb->transfer_buffer_length = len;
 		}
-		port->write_urb->dev = port->serial->dev;
 
 		ret = usb_submit_urb(port->write_urb, GFP_ATOMIC);
 		if (ret == 0) {
@@ -803,7 +585,6 @@ static int digi_set_modem_signals(struct usb_serial_port *port,
 	data[7] = 0;
 
 	oob_port->write_urb->transfer_buffer_length = 8;
-	oob_port->write_urb->dev = port->serial->dev;
 
 	ret = usb_submit_urb(oob_port->write_urb, GFP_ATOMIC);
 	if (ret == 0) {
@@ -899,10 +680,8 @@ static void digi_rx_unthrottle(struct tty_struct *tty)
 	spin_lock_irqsave(&priv->dp_port_lock, flags);
 
 	/* restart read chain */
-	if (priv->dp_throttle_restart) {
-		port->read_urb->dev = port->serial->dev;
+	if (priv->dp_throttle_restart)
 		ret = usb_submit_urb(port->read_urb, GFP_ATOMIC);
-	}
 
 	/* turn throttle off */
 	priv->dp_throttled = 0;
@@ -1195,7 +974,6 @@ static int digi_write(struct tty_struct *tty, struct usb_serial_port *port,
 	}
 
 	port->write_urb->transfer_buffer_length = data_len+2;
-	port->write_urb->dev = port->serial->dev;
 
 	*data++ = DIGI_CMD_SEND_DATA;
 	*data++ = data_len;
@@ -1271,7 +1049,6 @@ static void digi_write_bulk_callback(struct urb *urb)
 			= (unsigned char)priv->dp_out_buf_len;
 		port->write_urb->transfer_buffer_length =
 						priv->dp_out_buf_len + 2;
-		port->write_urb->dev = serial->dev;
 		memcpy(port->write_urb->transfer_buffer + 2, priv->dp_out_buf,
 			priv->dp_out_buf_len);
 		ret = usb_submit_urb(port->write_urb, GFP_ATOMIC);
@@ -1473,7 +1250,6 @@ static int digi_startup_device(struct usb_serial *serial)
 	/* set USB_DISABLE_SPD flag for write bulk urbs */
 	for (i = 0; i < serial->type->num_ports + 1; i++) {
 		port = serial->port[i];
-		port->write_urb->dev = port->serial->dev;
 		ret = usb_submit_urb(port->read_urb, GFP_KERNEL);
 		if (ret != 0) {
 			dev_err(&port->dev,
@@ -1616,7 +1392,6 @@ static void digi_read_bulk_callback(struct urb *urb)
 	}
 
 	/* continue read */
-	urb->dev = port->serial->dev;
 	ret = usb_submit_urb(urb, GFP_ATOMIC);
 	if (ret != 0 && ret != -EPERM) {
 		dev_err(&port->dev,
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index ff3db5d056a5..c290df97108e 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -2105,6 +2105,9 @@ static void ftdi_set_termios(struct tty_struct *tty,
 
 	cflag = termios->c_cflag;
 
+	if (old_termios == 0)
+		goto no_skip;
+
 	if (old_termios->c_cflag == termios->c_cflag
 	    && old_termios->c_ispeed == termios->c_ispeed
 	    && old_termios->c_ospeed == termios->c_ospeed)
@@ -2118,6 +2121,7 @@ static void ftdi_set_termios(struct tty_struct *tty,
 	    (termios->c_cflag & (CSIZE|PARODD|PARENB|CMSPAR|CSTOPB)))
 		goto no_data_parity_stop_changes;
 
+no_skip:
 	/* Set number of data bits, parity, stop bits */
 
 	urb_value = 0;
diff --git a/drivers/usb/serial/garmin_gps.c b/drivers/usb/serial/garmin_gps.c
index 1a49ca9c8ea5..bf12565f8e87 100644
--- a/drivers/usb/serial/garmin_gps.c
+++ b/drivers/usb/serial/garmin_gps.c
@@ -901,7 +901,6 @@ static int garmin_init_session(struct usb_serial_port *port)
 		usb_kill_urb(port->interrupt_in_urb);
 
 		dbg("%s - adding interrupt input", __func__);
-		port->interrupt_in_urb->dev = serial->dev;
 		status = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 		if (status)
 			dev_err(&serial->dev->dev,
@@ -1277,7 +1276,6 @@ static void garmin_read_int_callback(struct urb *urb)
 	unsigned long flags;
 	int retval;
 	struct usb_serial_port *port = urb->context;
-	struct usb_serial *serial = port->serial;
 	struct garmin_data *garmin_data_p = usb_get_serial_port_data(port);
 	unsigned char *data = urb->transfer_buffer;
 	int status = urb->status;
@@ -1311,12 +1309,6 @@ static void garmin_read_int_callback(struct urb *urb)
 		if (0 == (garmin_data_p->flags & FLAGS_BULK_IN_ACTIVE)) {
 
 			/* bulk data available */
-			usb_fill_bulk_urb(port->read_urb, serial->dev,
-					usb_rcvbulkpipe(serial->dev,
-						port->bulk_in_endpointAddress),
-					port->read_urb->transfer_buffer,
-					port->read_urb->transfer_buffer_length,
-					garmin_read_bulk_callback, port);
 			retval = usb_submit_urb(port->read_urb, GFP_ATOMIC);
 			if (retval) {
 				dev_err(&port->dev,
@@ -1353,7 +1345,6 @@ static void garmin_read_int_callback(struct urb *urb)
 
 	garmin_read_process(garmin_data_p, data, urb->actual_length, 0);
 
-	port->interrupt_in_urb->dev = port->serial->dev;
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
 	if (retval)
 		dev_err(&urb->dev->dev,
diff --git a/drivers/usb/serial/generic.c b/drivers/usb/serial/generic.c
index e4db5ad2bc55..f7403576f99f 100644
--- a/drivers/usb/serial/generic.c
+++ b/drivers/usb/serial/generic.c
@@ -1,7 +1,7 @@
 /*
  * USB Serial Converter Generic functions
  *
- * Copyright (C) 2010 Johan Hovold (jhovold@gmail.com)
+ * Copyright (C) 2010 - 2011 Johan Hovold (jhovold@gmail.com)
  * Copyright (C) 1999 - 2002 Greg Kroah-Hartman (greg@kroah.com)
  *
  *	This program is free software; you can redistribute it and/or
@@ -132,7 +132,7 @@ int usb_serial_generic_open(struct tty_struct *tty, struct usb_serial_port *port
 
 	/* if we have a bulk endpoint, start reading from it */
 	if (port->bulk_in_size)
-		result = usb_serial_generic_submit_read_urb(port, GFP_KERNEL);
+		result = usb_serial_generic_submit_read_urbs(port, GFP_KERNEL);
 
 	return result;
 }
@@ -157,8 +157,10 @@ static void generic_cleanup(struct usb_serial_port *port)
 			kfifo_reset_out(&port->write_fifo);
 			spin_unlock_irqrestore(&port->lock, flags);
 		}
-		if (port->bulk_in_size)
-			usb_kill_urb(port->read_urb);
+		if (port->bulk_in_size) {
+			for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i)
+				usb_kill_urb(port->read_urbs[i]);
+		}
 	}
 }
 
@@ -308,19 +310,52 @@ int usb_serial_generic_chars_in_buffer(struct tty_struct *tty)
 	return chars;
 }
 
-int usb_serial_generic_submit_read_urb(struct usb_serial_port *port,
+static int usb_serial_generic_submit_read_urb(struct usb_serial_port *port,
+						int index, gfp_t mem_flags)
+{
+	int res;
+
+	if (!test_and_clear_bit(index, &port->read_urbs_free))
+		return 0;
+
+	dbg("%s - port %d, urb %d\n", __func__, port->number, index);
+
+	res = usb_submit_urb(port->read_urbs[index], mem_flags);
+	if (res) {
+		if (res != -EPERM) {
+			dev_err(&port->dev,
+					"%s - usb_submit_urb failed: %d\n",
+					__func__, res);
+		}
+		set_bit(index, &port->read_urbs_free);
+		return res;
+	}
+
+	return 0;
+}
+
+int usb_serial_generic_submit_read_urbs(struct usb_serial_port *port,
 					gfp_t mem_flags)
 {
-	int result;
+	int res;
+	int i;
 
-	result = usb_submit_urb(port->read_urb, mem_flags);
-	if (result && result != -EPERM) {
-		dev_err(&port->dev, "%s - error submitting urb: %d\n",
-							__func__, result);
+	dbg("%s - port %d", __func__, port->number);
+
+	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i) {
+		res = usb_serial_generic_submit_read_urb(port, i, mem_flags);
+		if (res)
+			goto err;
 	}
-	return result;
+
+	return 0;
+err:
+	for (; i >= 0; --i)
+		usb_kill_urb(port->read_urbs[i]);
+
+	return res;
 }
-EXPORT_SYMBOL_GPL(usb_serial_generic_submit_read_urb);
+EXPORT_SYMBOL_GPL(usb_serial_generic_submit_read_urbs);
 
 void usb_serial_generic_process_read_urb(struct urb *urb)
 {
@@ -356,14 +391,19 @@ void usb_serial_generic_read_bulk_callback(struct urb *urb)
 {
 	struct usb_serial_port *port = urb->context;
 	unsigned char *data = urb->transfer_buffer;
-	int status = urb->status;
 	unsigned long flags;
+	int i;
 
-	dbg("%s - port %d", __func__, port->number);
+	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i) {
+		if (urb == port->read_urbs[i])
+			break;
+	}
+	set_bit(i, &port->read_urbs_free);
 
-	if (unlikely(status != 0)) {
-		dbg("%s - nonzero read bulk status received: %d",
-		    __func__, status);
+	dbg("%s - port %d, urb %d, len %d\n", __func__, port->number, i,
+							urb->actual_length);
+	if (urb->status) {
+		dbg("%s - non-zero urb status: %d\n", __func__, urb->status);
 		return;
 	}
 
@@ -376,7 +416,7 @@ void usb_serial_generic_read_bulk_callback(struct urb *urb)
 	port->throttled = port->throttle_req;
 	if (!port->throttled) {
 		spin_unlock_irqrestore(&port->lock, flags);
-		usb_serial_generic_submit_read_urb(port, GFP_ATOMIC);
+		usb_serial_generic_submit_read_urb(port, i, GFP_ATOMIC);
 	} else
 		spin_unlock_irqrestore(&port->lock, flags);
 }
@@ -443,7 +483,7 @@ void usb_serial_generic_unthrottle(struct tty_struct *tty)
 	spin_unlock_irq(&port->lock);
 
 	if (was_throttled)
-		usb_serial_generic_submit_read_urb(port, GFP_KERNEL);
+		usb_serial_generic_submit_read_urbs(port, GFP_KERNEL);
 }
 EXPORT_SYMBOL_GPL(usb_serial_generic_unthrottle);
 
@@ -509,8 +549,9 @@ int usb_serial_generic_resume(struct usb_serial *serial)
 		if (!test_bit(ASYNCB_INITIALIZED, &port->port.flags))
 			continue;
 
-		if (port->read_urb) {
-			r = usb_submit_urb(port->read_urb, GFP_NOIO);
+		if (port->bulk_in_size) {
+			r = usb_serial_generic_submit_read_urbs(port,
+								GFP_NOIO);
 			if (r < 0)
 				c++;
 		}
diff --git a/drivers/usb/serial/io_edgeport.c b/drivers/usb/serial/io_edgeport.c
index 2ee807523f53..abd2ee2b2f99 100644
--- a/drivers/usb/serial/io_edgeport.c
+++ b/drivers/usb/serial/io_edgeport.c
@@ -610,7 +610,6 @@ static void edge_interrupt_callback(struct urb *urb)
 
 					/* we have pending bytes on the
 					   bulk in pipe, send a request */
-					edge_serial->read_urb->dev = edge_serial->serial->dev;
 					result = usb_submit_urb(edge_serial->read_urb, GFP_ATOMIC);
 					if (result) {
 						dev_err(&edge_serial->serial->dev->dev, "%s - usb_submit_urb(read bulk) failed with result = %d\n", __func__, result);
@@ -711,7 +710,6 @@ static void edge_bulk_in_callback(struct urb *urb)
 	/* check to see if there's any more data for us to read */
 	if (edge_serial->rxBytesAvail > 0) {
 		dbg("%s - posting a read", __func__);
-		edge_serial->read_urb->dev = edge_serial->serial->dev;
 		retval = usb_submit_urb(edge_serial->read_urb, GFP_ATOMIC);
 		if (retval) {
 			dev_err(&urb->dev->dev,
@@ -1330,7 +1328,6 @@ static void send_more_port_data(struct edgeport_serial *edge_serial,
 	edge_port->txCredits -= count;
 	edge_port->icount.tx += count;
 
-	urb->dev = edge_serial->serial->dev;
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (status) {
 		/* something went wrong */
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index 0aac00afb5c8..e44d375edaad 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -15,13 +15,6 @@
  * For questions or problems with this driver, contact Inside Out
  * Networks technical support, or Peter Berger <pberger@brimson.com>,
  * or Al Borchers <alborchers@steinerpoint.com>.
- *
- * Version history:
- *
- *	July 11, 2002 	Removed 4 port device structure since all TI UMP
- *			chips have only 2 ports
- *			David Iacovelli (davidi@ionetworks.com)
- *
  */
 
 #include <linux/kernel.h>
@@ -1777,12 +1770,11 @@ static void edge_bulk_in_callback(struct urb *urb)
 exit:
 	/* continue read unless stopped */
 	spin_lock(&edge_port->ep_lock);
-	if (edge_port->ep_read_urb_state == EDGE_READ_URB_RUNNING) {
-		urb->dev = edge_port->port->serial->dev;
+	if (edge_port->ep_read_urb_state == EDGE_READ_URB_RUNNING)
 		retval = usb_submit_urb(urb, GFP_ATOMIC);
-	} else if (edge_port->ep_read_urb_state == EDGE_READ_URB_STOPPING) {
+	else if (edge_port->ep_read_urb_state == EDGE_READ_URB_STOPPING)
 		edge_port->ep_read_urb_state = EDGE_READ_URB_STOPPED;
-	}
+
 	spin_unlock(&edge_port->ep_lock);
 	if (retval)
 		dev_err(&urb->dev->dev,
@@ -1959,9 +1951,7 @@ static int edge_open(struct tty_struct *tty, struct usb_serial_port *port)
 			status = -EINVAL;
 			goto release_es_lock;
 		}
-		urb->complete = edge_interrupt_callback;
 		urb->context = edge_serial;
-		urb->dev = dev;
 		status = usb_submit_urb(urb, GFP_KERNEL);
 		if (status) {
 			dev_err(&port->dev,
@@ -1987,9 +1977,7 @@ static int edge_open(struct tty_struct *tty, struct usb_serial_port *port)
 		goto unlink_int_urb;
 	}
 	edge_port->ep_read_urb_state = EDGE_READ_URB_RUNNING;
-	urb->complete = edge_bulk_in_callback;
 	urb->context = edge_port;
-	urb->dev = dev;
 	status = usb_submit_urb(urb, GFP_KERNEL);
 	if (status) {
 		dev_err(&port->dev,
@@ -2118,12 +2106,7 @@ static void edge_send(struct tty_struct *tty)
 				port->write_urb->transfer_buffer);
 
 	/* set up our urb */
-	usb_fill_bulk_urb(port->write_urb, port->serial->dev,
-			   usb_sndbulkpipe(port->serial->dev,
-					    port->bulk_out_endpointAddress),
-			   port->write_urb->transfer_buffer, count,
-			   edge_bulk_out_callback,
-			   port);
+	port->write_urb->transfer_buffer_length = count;
 
 	/* send the data out the bulk port */
 	result = usb_submit_urb(port->write_urb, GFP_ATOMIC);
@@ -2267,9 +2250,6 @@ static int restart_read(struct edgeport_port *edge_port)
 
 	if (edge_port->ep_read_urb_state == EDGE_READ_URB_STOPPED) {
 		urb = edge_port->port->read_urb;
-		urb->complete = edge_bulk_in_callback;
-		urb->context = edge_port;
-		urb->dev = edge_port->port->serial->dev;
 		status = usb_submit_urb(urb, GFP_ATOMIC);
 	}
 	edge_port->ep_read_urb_state = EDGE_READ_URB_RUNNING;
diff --git a/drivers/usb/serial/ipaq.c b/drivers/usb/serial/ipaq.c
index 4735931b4c7b..36f5cbe90485 100644
--- a/drivers/usb/serial/ipaq.c
+++ b/drivers/usb/serial/ipaq.c
@@ -8,40 +8,6 @@
  *	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.
- *
- * (12/12/2002) ganesh
- * 	Added support for practically all devices supported by ActiveSync
- * 	on Windows. Thanks to Wes Cilldhaire <billybobjoehenrybob@hotmail.com>.
- *
- * (26/11/2002) ganesh
- * 	Added insmod options to specify product and vendor id.
- * 	Use modprobe ipaq vendor=0xfoo product=0xbar
- *
- * (26/7/2002) ganesh
- * 	Fixed up broken error handling in ipaq_open. Retry the "kickstart"
- * 	packet much harder - this drastically reduces connection failures.
- *
- * (30/4/2002) ganesh
- * 	Added support for the Casio EM500. Completely untested. Thanks
- * 	to info from Nathan <wfilardo@fuse.net>
- *
- * (19/3/2002) ganesh
- *	Don't submit urbs while holding spinlocks. Not strictly necessary
- *	in 2.5.x.
- *
- * (8/3/2002) ganesh
- * 	The ipaq sometimes emits a '\0' before the CLIENT string. At this
- * 	point of time, the ppp ldisc is not yet attached to the tty, so
- * 	n_tty echoes "^ " to the ipaq, which messes up the chat. In 2.5.6-pre2
- * 	this causes a panic because echo_char() tries to sleep in interrupt
- * 	context.
- * 	The fix is to tell the upper layers that this is a raw device so that
- * 	echoing is suppressed. Thanks to Lyle Lindholm for a detailed bug
- * 	report.
- *
- * (25/2/2002) ganesh
- * 	Added support for the HP Jornada 548 and 568. Completely untested.
- * 	Thanks to info from Heath Robinson and Arieh Davidoff.
  */
 
 #include <linux/kernel.h>
diff --git a/drivers/usb/serial/ir-usb.c b/drivers/usb/serial/ir-usb.c
index ccbce4066d04..0c537da0d3cd 100644
--- a/drivers/usb/serial/ir-usb.c
+++ b/drivers/usb/serial/ir-usb.c
@@ -22,38 +22,6 @@
  *
  * See Documentation/usb/usb-serial.txt for more information on using this
  * driver
- *
- * 2008_Jun_02  Felipe Balbi <me@felipebalbi.com>
- *	Introduced common header to be used also in USB Gadget Framework.
- *	Still needs some other style fixes.
- *
- * 2007_Jun_21  Alan Cox <alan@lxorguk.ukuu.org.uk>
- *	Minimal cleanups for some of the driver problens and tty layer abuse.
- *	Still needs fixing to allow multiple dongles.
- *
- * 2002_Mar_07	greg kh
- *	moved some needed structures and #define values from the
- *	net/irda/irda-usb.h file into our file, as we don't want to depend on
- *	that codebase compiling correctly :)
- *
- * 2002_Jan_14  gb
- *	Added module parameter to force specific number of XBOFs.
- *	Added ir_xbof_change().
- *	Reorganized read_bulk_callback error handling.
- *	Switched from FILL_BULK_URB() to usb_fill_bulk_urb().
- *
- * 2001_Nov_08  greg kh
- *	Changed the irda_usb_find_class_desc() function based on comments and
- *	code from Martin Diehl.
- *
- * 2001_Nov_01	greg kh
- *	Added support for more IrDA USB devices.
- *	Added support for zero packet.  Added buffer override paramater, so
- *	users can transfer larger packets at once if they wish.  Both patches
- *	came from Dag Brattli <dag@obexcode.com>.
- *
- * 2001_Oct_07	greg kh
- *	initial version released.
  */
 
 #include <linux/kernel.h>
diff --git a/drivers/usb/serial/iuu_phoenix.c b/drivers/usb/serial/iuu_phoenix.c
index 6aca631a407a..64d0ffd4440b 100644
--- a/drivers/usb/serial/iuu_phoenix.c
+++ b/drivers/usb/serial/iuu_phoenix.c
@@ -1168,15 +1168,14 @@ static int iuu_open(struct tty_struct *tty, struct usb_serial_port *port)
 			  port->write_urb->transfer_buffer, 1,
 			  read_rxcmd_callback, port);
 	result = usb_submit_urb(port->write_urb, GFP_KERNEL);
-
 	if (result) {
 		dev_err(&port->dev, "%s - failed submitting read urb,"
 			" error %d\n", __func__, result);
 		iuu_close(port);
-		return -EPROTO;
 	} else {
 		dbg("%s - rxcmd OK", __func__);
 	}
+
 	return result;
 }
 
diff --git a/drivers/usb/serial/keyspan.c b/drivers/usb/serial/keyspan.c
index a442352d7b61..bc8dc203e818 100644
--- a/drivers/usb/serial/keyspan.c
+++ b/drivers/usb/serial/keyspan.c
@@ -25,73 +25,6 @@
 
   Tip 'o the hat to IBM (and previously Linuxcare :) for supporting
   staff in their work on open source projects.
-
-  Change History
-
-    2003sep04	LPM (Keyspan) add support for new single port product USA19HS.
-				Improve setup message handling for all devices.
-
-    Wed Feb 19 22:00:00 PST 2003 (Jeffrey S. Laing <keyspan@jsl.com>)
-      Merged the current (1/31/03) Keyspan code with the current (2.4.21-pre4)
-      Linux source tree.  The Linux tree lacked support for the 49WLC and
-      others.  The Keyspan patches didn't work with the current kernel.
-
-    2003jan30	LPM	add support for the 49WLC and MPR
-
-    Wed Apr 25 12:00:00 PST 2002 (Keyspan)
-      Started with Hugh Blemings' code dated Jan 17, 2002.  All adapters
-      now supported (including QI and QW).  Modified port open, port
-      close, and send setup() logic to fix various data and endpoint
-      synchronization bugs and device LED status bugs.  Changed keyspan_
-      write_room() to accurately return transmit buffer availability.
-      Changed forwardingLength from 1 to 16 for all adapters.
-
-    Fri Oct 12 16:45:00 EST 2001
-      Preliminary USA-19QI and USA-28 support (both test OK for me, YMMV)
-
-    Wed Apr 25 12:00:00 PST 2002 (Keyspan)
-      Started with Hugh Blemings' code dated Jan 17, 2002.  All adapters
-      now supported (including QI and QW).  Modified port open, port
-      close, and send setup() logic to fix various data and endpoint
-      synchronization bugs and device LED status bugs.  Changed keyspan_
-      write_room() to accurately return transmit buffer availability.
-      Changed forwardingLength from 1 to 16 for all adapters.
-
-    Fri Oct 12 16:45:00 EST 2001
-      Preliminary USA-19QI and USA-28 support (both test OK for me, YMMV)
-
-    Mon Oct  8 14:29:00 EST 2001 hugh
-      Fixed bug that prevented mulitport devices operating correctly
-      if they weren't the first unit attached.
-
-    Sat Oct  6 12:31:21 EST 2001 hugh
-      Added support for USA-28XA and -28XB, misc cleanups, break support
-      for usa26 based models thanks to David Gibson.
-
-    Thu May 31 11:56:42 PDT 2001 gkh
-      switched from using spinlock to a semaphore
-
-    (04/08/2001) gb
-	Identify version on module load.
-
-    (11/01/2000) Adam J. Richter
-	usb_device_id table support.
-
-    Tue Oct 10 23:15:33 EST 2000 Hugh
-      Merged Paul's changes with my USA-49W mods.  Work in progress
-      still...
-
-    Wed Jul 19 14:00:42 EST 2000 gkh
-      Added module_init and module_exit functions to handle the fact that
-      this driver is a loadable module now.
-
-    Tue Jul 18 16:14:52 EST 2000 Hugh
-      Basic character input/output for USA-19 now mostly works,
-      fixed at 9600 baud for the moment.
-
-    Sat Jul  8 11:11:48 EST 2000 Hugh
-      First public release - nothing works except the firmware upload.
-      Tested on PPC and x86 architectures, seems to behave...
 */
 
 
@@ -397,7 +330,6 @@ static int keyspan_write(struct tty_struct *tty,
 		/* send the data out the bulk port */
 		this_urb->transfer_buffer_length = todo + dataOffset;
 
-		this_urb->dev = port->serial->dev;
 		err = usb_submit_urb(this_urb, GFP_ATOMIC);
 		if (err != 0)
 			dbg("usb_submit_urb(write bulk) failed (%d)", err);
@@ -463,7 +395,6 @@ static void	usa26_indat_callback(struct urb *urb)
 	tty_kref_put(tty);
 
 	/* Resubmit urb so we continue receiving */
-	urb->dev = port->serial->dev;
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -559,7 +490,6 @@ static void	usa26_instat_callback(struct urb *urb)
 	}
 
 	/* Resubmit urb so we continue receiving */
-	urb->dev = serial->dev;
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -609,7 +539,6 @@ static void usa28_indat_callback(struct urb *urb)
 		tty_kref_put(tty);
 
 		/* Resubmit urb so we continue receiving */
-		urb->dev = port->serial->dev;
 		err = usb_submit_urb(urb, GFP_ATOMIC);
 		if (err != 0)
 			dbg("%s - resubmit read urb failed. (%d)",
@@ -694,7 +623,6 @@ static void	usa28_instat_callback(struct urb *urb)
 	}
 
 		/* Resubmit urb so we continue receiving */
-	urb->dev = serial->dev;
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -789,8 +717,6 @@ static void	usa49_instat_callback(struct urb *urb)
 	}
 
 	/* Resubmit urb so we continue receiving */
-	urb->dev = serial->dev;
-
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -848,7 +774,6 @@ static void	usa49_indat_callback(struct urb *urb)
 	tty_kref_put(tty);
 
 	/* Resubmit urb so we continue receiving */
-	urb->dev = port->serial->dev;
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -919,8 +844,6 @@ static void usa49wg_indat_callback(struct urb *urb)
 	}
 
 	/* Resubmit urb so we continue receiving */
-	urb->dev = serial->dev;
-
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -996,7 +919,6 @@ static void usa90_indat_callback(struct urb *urb)
 	}
 
 	/* Resubmit urb so we continue receiving */
-	urb->dev = port->serial->dev;
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -1047,7 +969,6 @@ static void	usa90_instat_callback(struct urb *urb)
 	}
 
 	/* Resubmit urb so we continue receiving */
-	urb->dev = serial->dev;
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -1123,7 +1044,6 @@ static void	usa67_instat_callback(struct urb *urb)
 	}
 
 	/* Resubmit urb so we continue receiving */
-	urb->dev = serial->dev;
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - resubmit read urb failed. (%d)", __func__, err);
@@ -1223,7 +1143,6 @@ static int keyspan_open(struct tty_struct *tty, struct usb_serial_port *port)
 		urb = p_priv->in_urbs[i];
 		if (urb == NULL)
 			continue;
-		urb->dev = serial->dev;
 
 		/* make sure endpoint data toggle is synchronized
 		   with the device */
@@ -1239,7 +1158,6 @@ static int keyspan_open(struct tty_struct *tty, struct usb_serial_port *port)
 		urb = p_priv->out_urbs[i];
 		if (urb == NULL)
 			continue;
-		urb->dev = serial->dev;
 		/* usb_settoggle(urb->dev, usb_pipeendpoint(urb->pipe),
 						usb_pipeout(urb->pipe), 0); */
 	}
@@ -1956,7 +1874,6 @@ static int keyspan_usa26_send_setup(struct usb_serial *serial,
 	/* send the data out the device on control endpoint */
 	this_urb->transfer_buffer_length = sizeof(msg);
 
-	this_urb->dev = serial->dev;
 	err = usb_submit_urb(this_urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - usb_submit_urb(setup) failed (%d)", __func__, err);
@@ -2084,7 +2001,6 @@ static int keyspan_usa28_send_setup(struct usb_serial *serial,
 	/* send the data out the device on control endpoint */
 	this_urb->transfer_buffer_length = sizeof(msg);
 
-	this_urb->dev = serial->dev;
 	err = usb_submit_urb(this_urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - usb_submit_urb(setup) failed", __func__);
@@ -2271,8 +2187,6 @@ static int keyspan_usa49_send_setup(struct usb_serial *serial,
 
 		/* send the data out the device on control endpoint */
 		this_urb->transfer_buffer_length = sizeof(msg);
-
-		this_urb->dev = serial->dev;
 	}
 	err = usb_submit_urb(this_urb, GFP_ATOMIC);
 	if (err != 0)
@@ -2415,7 +2329,6 @@ static int keyspan_usa90_send_setup(struct usb_serial *serial,
 	/* send the data out the device on control endpoint */
 	this_urb->transfer_buffer_length = sizeof(msg);
 
-	this_urb->dev = serial->dev;
 	err = usb_submit_urb(this_urb, GFP_ATOMIC);
 	if (err != 0)
 		dbg("%s - usb_submit_urb(setup) failed (%d)", __func__, err);
@@ -2561,7 +2474,6 @@ static int keyspan_usa67_send_setup(struct usb_serial *serial,
 
 	/* send the data out the device on control endpoint */
 	this_urb->transfer_buffer_length = sizeof(msg);
-	this_urb->dev = serial->dev;
 
 	err = usb_submit_urb(this_urb, GFP_ATOMIC);
 	if (err != 0)
@@ -2650,14 +2562,12 @@ static int keyspan_startup(struct usb_serial *serial)
 	keyspan_setup_urbs(serial);
 
 	if (s_priv->instat_urb != NULL) {
-		s_priv->instat_urb->dev = serial->dev;
 		err = usb_submit_urb(s_priv->instat_urb, GFP_KERNEL);
 		if (err != 0)
 			dbg("%s - submit instat urb failed %d", __func__,
 				err);
 	}
 	if (s_priv->indat_urb != NULL) {
-		s_priv->indat_urb->dev = serial->dev;
 		err = usb_submit_urb(s_priv->indat_urb, GFP_KERNEL);
 		if (err != 0)
 			dbg("%s - submit indat urb failed %d", __func__,
diff --git a/drivers/usb/serial/keyspan_pda.c b/drivers/usb/serial/keyspan_pda.c
index d5c0c6ab4966..a40615674a68 100644
--- a/drivers/usb/serial/keyspan_pda.c
+++ b/drivers/usb/serial/keyspan_pda.c
@@ -12,59 +12,6 @@
  *
  * See Documentation/usb/usb-serial.txt for more information on using this
  * driver
- *
- * (09/07/2001) gkh
- *	cleaned up the Xircom support.  Added ids for Entregra device which is
- *	the same as the Xircom device.  Enabled the code to be compiled for
- *	either Xircom or Keyspan devices.
- *
- * (08/11/2001) Cristian M. Craciunescu
- *	support for Xircom PGSDB9
- *
- * (05/31/2001) gkh
- *	switched from using spinlock to a semaphore, which fixes lots of
- *	problems.
- *
- * (04/08/2001) gb
- *	Identify version on module load.
- *
- * (11/01/2000) Adam J. Richter
- *	usb_device_id table support
- *
- * (10/05/2000) gkh
- *	Fixed bug with urb->dev not being set properly, now that the usb
- *	core needs it.
- *
- * (08/28/2000) gkh
- *	Added locks for SMP safeness.
- *	Fixed MOD_INC and MOD_DEC logic and the ability to open a port more
- *	than once.
- *
- * (07/20/2000) borchers
- *	- keyspan_pda_write no longer sleeps if it is called on interrupt time;
- *	  PPP and the line discipline with stty echo on can call write on
- *	  interrupt time and this would cause an oops if write slept
- *	- if keyspan_pda_write is in an interrupt, it will not call
- *	  usb_control_msg (which sleeps) to query the room in the device
- *	  buffer, it simply uses the current room value it has
- *	- if the urb is busy or if it is throttled keyspan_pda_write just
- *	  returns 0, rather than sleeping to wait for this to change; the
- *	  write_chan code in n_tty.c will sleep if needed before calling
- *	  keyspan_pda_write again
- *	- if the device needs to be unthrottled, write now queues up the
- *	  call to usb_control_msg (which sleeps) to unthrottle the device
- *	- the wakeups from keyspan_pda_write_bulk_callback are queued rather
- *	  than done directly from the callback to avoid the race in write_chan
- *	- keyspan_pda_chars_in_buffer also indicates its buffer is full if the
- *	  urb status is -EINPROGRESS, meaning it cannot write at the moment
- *
- * (07/19/2000) gkh
- *	Added module_init and module_exit functions to handle the fact that this
- *	driver is a loadable module now.
- *
- * (03/26/2000) gkh
- *	Split driver up into device specific pieces.
- *
  */
 
 
@@ -290,7 +237,6 @@ static void keyspan_pda_rx_unthrottle(struct tty_struct *tty)
 	struct usb_serial_port *port = tty->driver_data;
 	/* just restart the receive interrupt URB */
 	dbg("keyspan_pda_rx_unthrottle port %d", port->number);
-	port->interrupt_in_urb->dev = port->serial->dev;
 	if (usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL))
 		dbg(" usb_submit_urb(read urb) failed");
 }
@@ -532,11 +478,11 @@ static int keyspan_pda_write(struct tty_struct *tty,
 	   the device is full (wait until it says there is room)
 	*/
 	spin_lock_bh(&port->lock);
-	if (port->write_urb_busy || priv->tx_throttled) {
+	if (!test_bit(0, &port->write_urbs_free) || priv->tx_throttled) {
 		spin_unlock_bh(&port->lock);
 		return 0;
 	}
-	port->write_urb_busy = 1;
+	clear_bit(0, &port->write_urbs_free);
 	spin_unlock_bh(&port->lock);
 
 	/* At this point the URB is in our control, nobody else can submit it
@@ -598,7 +544,6 @@ static int keyspan_pda_write(struct tty_struct *tty,
 
 		priv->tx_room -= count;
 
-		port->write_urb->dev = port->serial->dev;
 		rc = usb_submit_urb(port->write_urb, GFP_ATOMIC);
 		if (rc) {
 			dbg(" usb_submit_urb(write bulk) failed");
@@ -618,7 +563,7 @@ static int keyspan_pda_write(struct tty_struct *tty,
 	rc = count;
 exit:
 	if (rc < 0)
-		port->write_urb_busy = 0;
+		set_bit(0, &port->write_urbs_free);
 	return rc;
 }
 
@@ -628,7 +573,7 @@ static void keyspan_pda_write_bulk_callback(struct urb *urb)
 	struct usb_serial_port *port = urb->context;
 	struct keyspan_pda_private *priv;
 
-	port->write_urb_busy = 0;
+	set_bit(0, &port->write_urbs_free);
 	priv = usb_get_serial_port_data(port);
 
 	/* queue up a wakeup at scheduler time */
@@ -661,7 +606,7 @@ static int keyspan_pda_chars_in_buffer(struct tty_struct *tty)
 	   n_tty.c:normal_poll() ) that we're not writeable. */
 
 	spin_lock_irqsave(&port->lock, flags);
-	if (port->write_urb_busy || priv->tx_throttled)
+	if (!test_bit(0, &port->write_urbs_free) || priv->tx_throttled)
 		ret = 256;
 	spin_unlock_irqrestore(&port->lock, flags);
 	return ret;
@@ -717,7 +662,6 @@ static int keyspan_pda_open(struct tty_struct *tty,
 	priv->tx_throttled = *room ? 0 : 1;
 
 	/*Start reading from the device*/
-	port->interrupt_in_urb->dev = serial->dev;
 	rc = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (rc) {
 		dbg("%s - usb_submit_urb(read int) failed", __func__);
diff --git a/drivers/usb/serial/kobil_sct.c b/drivers/usb/serial/kobil_sct.c
index ddd146300ddb..5d3beeeb5fd9 100644
--- a/drivers/usb/serial/kobil_sct.c
+++ b/drivers/usb/serial/kobil_sct.c
@@ -20,18 +20,6 @@
  *
  * Supported readers: USB TWIN, KAAN Standard Plus and SecOVID Reader Plus
  * (Adapter K), B1 Professional and KAAN Professional (Adapter B)
- *
- * (21/05/2004) tw
- *      Fix bug with P'n'P readers
- *
- * (28/05/2003) tw
- *      Add support for KAAN SIM
- *
- * (12/09/2002) tw
- *      Adapted to 2.5.
- *
- * (11/08/2002) tw
- *      Initial version.
  */
 
 
@@ -231,9 +219,6 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 	dbg("%s - port %d", __func__, port->number);
 	priv = usb_get_serial_port_data(port);
 
-	/* someone sets the dev to 0 if the close method has been called */
-	port->interrupt_in_urb->dev = port->serial->dev;
-
 	/* allocate memory for transfer buffer */
 	transfer_buffer = kzalloc(transfer_buffer_length, GFP_KERNEL);
 	if (!transfer_buffer)
@@ -393,8 +378,6 @@ static void kobil_read_int_callback(struct urb *urb)
 		tty_flip_buffer_push(tty);
 	}
 	tty_kref_put(tty);
-	/* someone sets the dev to 0 if the close method has been called */
-	port->interrupt_in_urb->dev = port->serial->dev;
 
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_ATOMIC);
 	dbg("%s - port %d Send read URB returns: %i",
@@ -475,17 +458,9 @@ static int kobil_write(struct tty_struct *tty, struct usb_serial_port *port,
 		priv->filled = 0;
 		priv->cur_pos = 0;
 
-		/* someone sets the dev to 0 if the close method
-		   has been called */
-		port->interrupt_in_urb->dev = port->serial->dev;
-
 		/* start reading (except TWIN and KAAN SIM) */
 		if (priv->device_type == KOBIL_ADAPTER_B_PRODUCT_ID ||
 			priv->device_type == KOBIL_ADAPTER_K_PRODUCT_ID) {
-			/* someone sets the dev to 0 if the close method has
-			   been called */
-			port->interrupt_in_urb->dev = port->serial->dev;
-
 			result = usb_submit_urb(port->interrupt_in_urb,
 								GFP_NOIO);
 			dbg("%s - port %d Send read URB returns: %i",
diff --git a/drivers/usb/serial/mct_u232.c b/drivers/usb/serial/mct_u232.c
index ba0d28727ccb..a975bb80303f 100644
--- a/drivers/usb/serial/mct_u232.c
+++ b/drivers/usb/serial/mct_u232.c
@@ -19,50 +19,6 @@
  *   DTR/RTS signal handling may be incomplete or incorrect. I have mainly
  *   implemented what I have seen with SniffUSB or found in belkin_sa.c.
  *   For further TODOs check also belkin_sa.c.
- *
- * TEST STATUS:
- *   Basic tests have been performed with minicom/zmodem transfers and
- *   modem dialing under Linux 2.4.0-test10 (for me it works fine).
- *
- * 04-Nov-2003 Bill Marr <marr at flex dot com>
- *   - Mimic Windows driver by sending 2 USB 'device request' messages
- *     following normal 'baud rate change' message.  This allows data to be
- *     transmitted to RS-232 devices which don't assert the 'CTS' signal.
- *
- * 10-Nov-2001 Wolfgang Grandegger
- *   - Fixed an endianess problem with the baudrate selection for PowerPC.
- *
- * 06-Dec-2001 Martin Hamilton <martinh@gnu.org>
- *   - Added support for the Belkin F5U109 DB9 adaptor
- *
- * 30-May-2001 Greg Kroah-Hartman
- *   - switched from using spinlock to a semaphore, which fixes lots of
- *     problems.
- *
- * 04-May-2001 Stelian Pop
- *   - Set the maximum bulk output size for Sitecom U232-P25 model to 16 bytes
- *     instead of the device reported 32 (using 32 bytes causes many data
- *     loss, Windows driver uses 16 too).
- *
- * 02-May-2001 Stelian Pop
- *   - Fixed the baud calculation for Sitecom U232-P25 model
- *
- * 08-Apr-2001 gb
- *   - Identify version on module load.
- *
- * 06-Jan-2001 Cornel Ciocirlan
- *   - Added support for Sitecom U232-P25 model (Product Id 0x0230)
- *   - Added support for D-Link DU-H3SP USB BAY (Product Id 0x0200)
- *
- * 29-Nov-2000 Greg Kroah-Hartman
- *   - Added device id table to fit with 2.4.0-test11 structure.
- *   - took out DEAL_WITH_TWO_INT_IN_ENDPOINTS #define as it's not needed
- *     (lots of things will change if/when the usb-serial core changes to
- *     handle these issues.
- *
- * 27-Nov-2000 Wolfgang Grandegge
- *   A version for kernel 2.4.0-test10 released to the Linux community
- *   (via linux-usb-devel).
  */
 
 #include <linux/kernel.h>
@@ -526,7 +482,6 @@ static int  mct_u232_open(struct tty_struct *tty, struct usb_serial_port *port)
 	mct_u232_msr_to_state(&priv->control_state, priv->last_msr);
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	port->read_urb->dev = port->serial->dev;
 	retval = usb_submit_urb(port->read_urb, GFP_KERNEL);
 	if (retval) {
 		dev_err(&port->dev,
@@ -535,7 +490,6 @@ static int  mct_u232_open(struct tty_struct *tty, struct usb_serial_port *port)
 		goto error;
 	}
 
-	port->interrupt_in_urb->dev = port->serial->dev;
 	retval = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (retval) {
 		usb_kill_urb(port->read_urb);
diff --git a/drivers/usb/serial/mos7720.c b/drivers/usb/serial/mos7720.c
index 3524a105d042..19d112f51b97 100644
--- a/drivers/usb/serial/mos7720.c
+++ b/drivers/usb/serial/mos7720.c
@@ -939,14 +939,7 @@ static void mos7720_bulk_in_callback(struct urb *urb)
 	}
 	tty_kref_put(tty);
 
-	if (!port->read_urb) {
-		dbg("URB KILLED !!!");
-		return;
-	}
-
 	if (port->read_urb->status != -EINPROGRESS) {
-		port->read_urb->dev = port->serial->dev;
-
 		retval = usb_submit_urb(port->read_urb, GFP_ATOMIC);
 		if (retval)
 			dbg("usb_submit_urb(read bulk) failed, retval = %d",
@@ -1014,7 +1007,6 @@ static int mos77xx_calc_num_ports(struct usb_serial *serial)
 static int mos7720_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	struct usb_serial *serial;
-	struct usb_serial_port *port0;
 	struct urb *urb;
 	struct moschip_port *mos7720_port;
 	int response;
@@ -1029,8 +1021,6 @@ static int mos7720_open(struct tty_struct *tty, struct usb_serial_port *port)
 	if (mos7720_port == NULL)
 		return -ENODEV;
 
-	port0 = serial->port[0];
-
 	usb_clear_halt(serial->dev, port->write_urb->pipe);
 	usb_clear_halt(serial->dev, port->read_urb->pipe);
 
@@ -1735,8 +1725,6 @@ static void change_port_settings(struct tty_struct *tty,
 	write_mos_reg(serial, port_number, IER, 0x0c);
 
 	if (port->read_urb->status != -EINPROGRESS) {
-		port->read_urb->dev = serial->dev;
-
 		status = usb_submit_urb(port->read_urb, GFP_ATOMIC);
 		if (status)
 			dbg("usb_submit_urb(read bulk) failed, status = %d",
@@ -1786,13 +1774,7 @@ static void mos7720_set_termios(struct tty_struct *tty,
 	/* change the port settings to the new ones specified */
 	change_port_settings(tty, mos7720_port, old_termios);
 
-	if (!port->read_urb) {
-		dbg("%s", "URB KILLED !!!!!");
-		return;
-	}
-
 	if (port->read_urb->status != -EINPROGRESS) {
-		port->read_urb->dev = serial->dev;
 		status = usb_submit_urb(port->read_urb, GFP_ATOMIC);
 		if (status)
 			dbg("usb_submit_urb(read bulk) failed, status = %d",
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index c72abd524983..55cfd6265b98 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -792,8 +792,6 @@ static void mos7840_bulk_in_callback(struct urb *urb)
 	}
 
 
-	mos7840_port->read_urb->dev = serial->dev;
-
 	mos7840_port->read_urb_busy = true;
 	retval = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
 
@@ -2058,7 +2056,6 @@ static void mos7840_change_port_settings(struct tty_struct *tty,
 	mos7840_set_uart_reg(port, INTERRUPT_ENABLE_REGISTER, Data);
 
 	if (mos7840_port->read_urb_busy == false) {
-		mos7840_port->read_urb->dev = serial->dev;
 		mos7840_port->read_urb_busy = true;
 		status = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
 		if (status) {
@@ -2130,7 +2127,6 @@ static void mos7840_set_termios(struct tty_struct *tty,
 	}
 
 	if (mos7840_port->read_urb_busy == false) {
-		mos7840_port->read_urb->dev = serial->dev;
 		mos7840_port->read_urb_busy = true;
 		status = usb_submit_urb(mos7840_port->read_urb, GFP_ATOMIC);
 		if (status) {
diff --git a/drivers/usb/serial/omninet.c b/drivers/usb/serial/omninet.c
index 60f38d5e64fc..45a8c55881d3 100644
--- a/drivers/usb/serial/omninet.c
+++ b/drivers/usb/serial/omninet.c
@@ -9,31 +9,6 @@
  * driver
  *
  * Please report both successes and troubles to the author at omninet@kroah.com
- *
- * (05/30/2001) gkh
- *	switched from using spinlock to a semaphore, which fixes lots of
- *	problems.
- *
- * (04/08/2001) gb
- *	Identify version on module load.
- *
- * (11/01/2000) Adam J. Richter
- *	usb_device_id table support
- *
- * (10/05/2000) gkh
- *	Fixed bug with urb->dev not being set properly, now that the usb
- *	core needs it.
- *
- * (08/28/2000) gkh
- *	Added locks for SMP safeness.
- *	Fixed MOD_INC and MOD_DEC logic and the ability to open a port more
- *	than once.
- *	Fixed potential race in omninet_write_bulk_callback
- *
- * (07/19/2000) gkh
- *	Added module_init and module_exit functions to handle the fact that this
- *	driver is a loadable module now.
- *
  */
 
 #include <linux/kernel.h>
@@ -44,7 +19,6 @@
 #include <linux/tty_driver.h>
 #include <linux/tty_flip.h>
 #include <linux/module.h>
-#include <linux/spinlock.h>
 #include <linux/uaccess.h>
 #include <linux/usb.h>
 #include <linux/usb/serial.h>
@@ -174,12 +148,6 @@ static int omninet_open(struct tty_struct *tty, struct usb_serial_port *port)
 	tty_port_tty_set(&wport->port, tty);
 
 	/* Start reading from the device */
-	usb_fill_bulk_urb(port->read_urb, serial->dev,
-			usb_rcvbulkpipe(serial->dev,
-				port->bulk_in_endpointAddress),
-			port->read_urb->transfer_buffer,
-			port->read_urb->transfer_buffer_length,
-			omninet_read_bulk_callback, port);
 	result = usb_submit_urb(port->read_urb, GFP_KERNEL);
 	if (result)
 		dev_err(&port->dev,
@@ -236,11 +204,6 @@ static void omninet_read_bulk_callback(struct urb *urb)
 	}
 
 	/* Continue trying to always read  */
-	usb_fill_bulk_urb(urb, port->serial->dev,
-			usb_rcvbulkpipe(port->serial->dev,
-					port->bulk_in_endpointAddress),
-			urb->transfer_buffer, urb->transfer_buffer_length,
-			omninet_read_bulk_callback, port);
 	result = usb_submit_urb(urb, GFP_ATOMIC);
 	if (result)
 		dev_err(&port->dev,
@@ -267,14 +230,10 @@ static int omninet_write(struct tty_struct *tty, struct usb_serial_port *port,
 		return 0;
 	}
 
-	spin_lock_bh(&wport->lock);
-	if (wport->write_urb_busy) {
-		spin_unlock_bh(&wport->lock);
+	if (!test_and_clear_bit(0, &port->write_urbs_free)) {
 		dbg("%s - already writing", __func__);
 		return 0;
 	}
-	wport->write_urb_busy = 1;
-	spin_unlock_bh(&wport->lock);
 
 	count = (count > OMNINET_BULKOUTSIZE) ? OMNINET_BULKOUTSIZE : count;
 
@@ -292,10 +251,9 @@ static int omninet_write(struct tty_struct *tty, struct usb_serial_port *port,
 	/* send the data out the bulk port, always 64 bytes */
 	wport->write_urb->transfer_buffer_length = 64;
 
-	wport->write_urb->dev = serial->dev;
 	result = usb_submit_urb(wport->write_urb, GFP_ATOMIC);
 	if (result) {
-		wport->write_urb_busy = 0;
+		set_bit(0, &wport->write_urbs_free);
 		dev_err(&port->dev,
 			"%s - failed submitting write urb, error %d\n",
 			__func__, result);
@@ -314,8 +272,7 @@ static int omninet_write_room(struct tty_struct *tty)
 
 	int room = 0; /* Default: no room */
 
-	/* FIXME: no consistent locking for write_urb_busy */
-	if (wport->write_urb_busy)
+	if (test_bit(0, &wport->write_urbs_free))
 		room = wport->bulk_out_size - OMNINET_HEADERLEN;
 
 	dbg("%s - returns %d", __func__, room);
@@ -332,7 +289,7 @@ static void omninet_write_bulk_callback(struct urb *urb)
 
 	dbg("%s - port %0x", __func__, port->number);
 
-	port->write_urb_busy = 0;
+	set_bit(0, &port->write_urbs_free);
 	if (status) {
 		dbg("%s - nonzero write bulk status received: %d",
 		    __func__, status);
diff --git a/drivers/usb/serial/opticon.c b/drivers/usb/serial/opticon.c
index c248a9147439..691f57a9d712 100644
--- a/drivers/usb/serial/opticon.c
+++ b/drivers/usb/serial/opticon.c
@@ -384,7 +384,6 @@ static void opticon_unthrottle(struct tty_struct *tty)
 	priv->actually_throttled = false;
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	priv->bulk_read_urb->dev = port->serial->dev;
 	if (was_throttled) {
 		result = usb_submit_urb(priv->bulk_read_urb, GFP_ATOMIC);
 		if (result)
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index 6dd64534fad0..c96b6b6509fb 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -476,6 +476,10 @@ static void option_instat_callback(struct urb *urb);
 #define VIETTEL_VENDOR_ID			0x2262
 #define VIETTEL_PRODUCT_VT1000			0x0002
 
+/* ZD Incorporated */
+#define ZD_VENDOR_ID				0x0685
+#define ZD_PRODUCT_7000				0x7000
+
 /* some devices interfaces need special handling due to a number of reasons */
 enum option_blacklist_reason {
 		OPTION_BLACKLIST_NONE = 0,
@@ -1178,6 +1182,7 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU528) },
 	{ USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU526) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(VIETTEL_VENDOR_ID, VIETTEL_PRODUCT_VT1000, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZD_VENDOR_ID, ZD_PRODUCT_7000, 0xff, 0xff, 0xff) },
 	{ } /* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, option_ids);
diff --git a/drivers/usb/serial/oti6858.c b/drivers/usb/serial/oti6858.c
index 4c29e6c2bda7..2161d1c3c089 100644
--- a/drivers/usb/serial/oti6858.c
+++ b/drivers/usb/serial/oti6858.c
@@ -264,7 +264,6 @@ static void setup_line(struct work_struct *work)
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	dbg("%s(): submitting interrupt urb", __func__);
-	port->interrupt_in_urb->dev = port->serial->dev;
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (result != 0) {
 		dev_err(&port->dev, "%s(): usb_submit_urb() failed"
@@ -321,7 +320,6 @@ static void send_data(struct work_struct *work)
 		priv->flags.write_urb_in_use = 0;
 
 		dbg("%s(): submitting interrupt urb", __func__);
-		port->interrupt_in_urb->dev = port->serial->dev;
 		result = usb_submit_urb(port->interrupt_in_urb, GFP_NOIO);
 		if (result != 0) {
 			dev_err(&port->dev, "%s(): usb_submit_urb() failed"
@@ -334,7 +332,6 @@ static void send_data(struct work_struct *work)
 					port->write_urb->transfer_buffer,
 					count, &port->lock);
 	port->write_urb->transfer_buffer_length = count;
-	port->write_urb->dev = port->serial->dev;
 	result = usb_submit_urb(port->write_urb, GFP_NOIO);
 	if (result != 0) {
 		dev_err(&port->dev, "%s(): usb_submit_urb() failed"
@@ -583,13 +580,12 @@ static int oti6858_open(struct tty_struct *tty, struct usb_serial_port *port)
 	kfree(buf);
 
 	dbg("%s(): submitting interrupt urb", __func__);
-	port->interrupt_in_urb->dev = serial->dev;
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (result != 0) {
 		dev_err(&port->dev, "%s(): usb_submit_urb() failed"
 			       " with error %d\n", __func__, result);
 		oti6858_close(port);
-		return -EPROTO;
+		return result;
 	}
 
 	/* setup termios */
@@ -837,7 +833,6 @@ static void oti6858_read_int_callback(struct urb *urb)
 	if (can_recv) {
 		int result;
 
-		port->read_urb->dev = port->serial->dev;
 		result = usb_submit_urb(port->read_urb, GFP_ATOMIC);
 		if (result != 0) {
 			priv->flags.read_urb_in_use = 0;
@@ -866,7 +861,6 @@ static void oti6858_read_int_callback(struct urb *urb)
 		int result;
 
 /*		dbg("%s(): submitting interrupt urb", __func__); */
-		urb->dev = port->serial->dev;
 		result = usb_submit_urb(urb, GFP_ATOMIC);
 		if (result != 0) {
 			dev_err(&urb->dev->dev,
@@ -894,18 +888,6 @@ static void oti6858_read_bulk_callback(struct urb *urb)
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	if (status != 0) {
-		/*
-		if (status == -EPROTO) {
-			* PL2303 mysteriously fails with -EPROTO reschedule
-			   the read *
-			dbg("%s - caught -EPROTO, resubmitting the urb",
-								__func__);
-			result = usb_submit_urb(urb, GFP_ATOMIC);
-			if (result)
-				dev_err(&urb->dev->dev, "%s - failed resubmitting read urb, error %d\n", __func__, result);
-			return;
-		}
-		*/
 		dbg("%s(): unable to handle the error, exiting", __func__);
 		return;
 	}
@@ -918,7 +900,6 @@ static void oti6858_read_bulk_callback(struct urb *urb)
 	tty_kref_put(tty);
 
 	/* schedule the interrupt urb */
-	port->interrupt_in_urb->dev = port->serial->dev;
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_ATOMIC);
 	if (result != 0 && result != -EPERM) {
 		dev_err(&port->dev, "%s(): usb_submit_urb() failed,"
@@ -955,7 +936,6 @@ static void oti6858_write_bulk_callback(struct urb *urb)
 		dbg("%s(): overflow in write", __func__);
 
 		port->write_urb->transfer_buffer_length = 1;
-		port->write_urb->dev = port->serial->dev;
 		result = usb_submit_urb(port->write_urb, GFP_ATOMIC);
 		if (result) {
 			dev_err(&port->dev, "%s(): usb_submit_urb() failed,"
@@ -968,7 +948,6 @@ static void oti6858_write_bulk_callback(struct urb *urb)
 	priv->flags.write_urb_in_use = 0;
 
 	/* schedule the interrupt urb if we are still open */
-	port->interrupt_in_urb->dev = port->serial->dev;
 	dbg("%s(): submitting interrupt urb", __func__);
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_ATOMIC);
 	if (result != 0) {
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index fc2d66f7f4eb..329295615d06 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -502,21 +502,20 @@ static int pl2303_open(struct tty_struct *tty, struct usb_serial_port *port)
 	if (tty)
 		pl2303_set_termios(tty, port, &tmp_termios);
 
-	dbg("%s - submitting read urb", __func__);
-	result = usb_serial_generic_submit_read_urb(port, GFP_KERNEL);
-	if (result) {
-		pl2303_close(port);
-		return -EPROTO;
-	}
-
 	dbg("%s - submitting interrupt urb", __func__);
 	result = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
 	if (result) {
 		dev_err(&port->dev, "%s - failed submitting interrupt urb,"
 			" error %d\n", __func__, result);
-		pl2303_close(port);
-		return -EPROTO;
+		return result;
 	}
+
+	result = usb_serial_generic_open(tty, port);
+	if (result) {
+		usb_kill_urb(port->interrupt_in_urb);
+		return result;
+	}
+
 	port->port.drain_delay = 256;
 	return 0;
 }
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index b18179bda0d8..f2485429172f 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -681,7 +681,6 @@ static void sierra_instat_callback(struct urb *urb)
 	/* Resubmit urb so we continue receiving IRQ data */
 	if (status != -ESHUTDOWN && status != -ENOENT) {
 		usb_mark_last_busy(serial->dev);
-		urb->dev = serial->dev;
 		err = usb_submit_urb(urb, GFP_ATOMIC);
 		if (err && err != -EPERM)
 			dev_err(&port->dev, "%s: resubmit intr urb "
diff --git a/drivers/usb/serial/symbolserial.c b/drivers/usb/serial/symbolserial.c
index 7096f799b071..c70cc012d03f 100644
--- a/drivers/usb/serial/symbolserial.c
+++ b/drivers/usb/serial/symbolserial.c
@@ -182,7 +182,6 @@ static void symbol_unthrottle(struct tty_struct *tty)
 	priv->actually_throttled = false;
 	spin_unlock_irq(&priv->lock);
 
-	priv->int_urb->dev = port->serial->dev;
 	if (was_throttled) {
 		result = usb_submit_urb(priv->int_urb, GFP_KERNEL);
 		if (result)
diff --git a/drivers/usb/serial/ti_usb_3410_5052.c b/drivers/usb/serial/ti_usb_3410_5052.c
index ea8445689c85..4af21f46096e 100644
--- a/drivers/usb/serial/ti_usb_3410_5052.c
+++ b/drivers/usb/serial/ti_usb_3410_5052.c
@@ -535,9 +535,7 @@ static int ti_open(struct tty_struct *tty, struct usb_serial_port *port)
 			status = -EINVAL;
 			goto release_lock;
 		}
-		urb->complete = ti_interrupt_callback;
 		urb->context = tdev;
-		urb->dev = dev;
 		status = usb_submit_urb(urb, GFP_KERNEL);
 		if (status) {
 			dev_err(&port->dev,
@@ -619,9 +617,7 @@ static int ti_open(struct tty_struct *tty, struct usb_serial_port *port)
 		goto unlink_int_urb;
 	}
 	tport->tp_read_urb_state = TI_READ_URB_RUNNING;
-	urb->complete = ti_bulk_in_callback;
 	urb->context = tport;
-	urb->dev = dev;
 	status = usb_submit_urb(urb, GFP_KERNEL);
 	if (status) {
 		dev_err(&port->dev, "%s - submit read urb failed, %d\n",
@@ -1236,12 +1232,11 @@ static void ti_bulk_in_callback(struct urb *urb)
 exit:
 	/* continue to read unless stopping */
 	spin_lock(&tport->tp_lock);
-	if (tport->tp_read_urb_state == TI_READ_URB_RUNNING) {
-		urb->dev = port->serial->dev;
+	if (tport->tp_read_urb_state == TI_READ_URB_RUNNING)
 		retval = usb_submit_urb(urb, GFP_ATOMIC);
-	} else if (tport->tp_read_urb_state == TI_READ_URB_STOPPING) {
+	else if (tport->tp_read_urb_state == TI_READ_URB_STOPPING)
 		tport->tp_read_urb_state = TI_READ_URB_STOPPED;
-	}
+
 	spin_unlock(&tport->tp_lock);
 	if (retval)
 		dev_err(dev, "%s - resubmit read urb failed, %d\n",
@@ -1574,9 +1569,7 @@ static int ti_restart_read(struct ti_port *tport, struct tty_struct *tty)
 		tport->tp_read_urb_state = TI_READ_URB_RUNNING;
 		urb = tport->tp_port->read_urb;
 		spin_unlock_irqrestore(&tport->tp_lock, flags);
-		urb->complete = ti_bulk_in_callback;
 		urb->context = tport;
-		urb->dev = tport->tp_port->serial->dev;
 		status = usb_submit_urb(urb, GFP_KERNEL);
 	} else  {
 		tport->tp_read_urb_state = TI_READ_URB_RUNNING;
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index cc274fdf2627..ce6c1a65a544 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -50,7 +50,7 @@ static struct usb_driver usb_serial_driver = {
 	.disconnect =	usb_serial_disconnect,
 	.suspend =	usb_serial_suspend,
 	.resume =	usb_serial_resume,
-	.no_dynamic_id = 	1,
+	.no_dynamic_id =	1,
 	.supports_autosuspend =	1,
 };
 
@@ -260,6 +260,10 @@ static int serial_activate(struct tty_port *tport, struct tty_struct *tty)
 	else
 		retval = port->serial->type->open(tty, port);
 	mutex_unlock(&serial->disc_mutex);
+
+	if (retval < 0)
+		retval = usb_translate_errors(retval);
+
 	return retval;
 }
 
@@ -360,7 +364,8 @@ static int serial_write(struct tty_struct *tty, const unsigned char *buf,
 
 	/* pass on to the driver specific version of this function */
 	retval = port->serial->type->write(tty, port, buf, count);
-
+	if (retval < 0)
+		retval = usb_translate_errors(retval);
 exit:
 	return retval;
 }
@@ -562,8 +567,8 @@ static void kill_traffic(struct usb_serial_port *port)
 {
 	int i;
 
-	usb_kill_urb(port->read_urb);
-	usb_kill_urb(port->write_urb);
+	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i)
+		usb_kill_urb(port->read_urbs[i]);
 	for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i)
 		usb_kill_urb(port->write_urbs[i]);
 	/*
@@ -595,17 +600,17 @@ static void port_release(struct device *dev)
 	kill_traffic(port);
 	cancel_work_sync(&port->work);
 
-	usb_free_urb(port->read_urb);
-	usb_free_urb(port->write_urb);
 	usb_free_urb(port->interrupt_in_urb);
 	usb_free_urb(port->interrupt_out_urb);
+	for (i = 0; i < ARRAY_SIZE(port->read_urbs); ++i) {
+		usb_free_urb(port->read_urbs[i]);
+		kfree(port->bulk_in_buffers[i]);
+	}
 	for (i = 0; i < ARRAY_SIZE(port->write_urbs); ++i) {
 		usb_free_urb(port->write_urbs[i]);
 		kfree(port->bulk_out_buffers[i]);
 	}
 	kfifo_free(&port->write_fifo);
-	kfree(port->bulk_in_buffer);
-	kfree(port->bulk_out_buffer);
 	kfree(port->interrupt_in_buffer);
 	kfree(port->interrupt_out_buffer);
 	kfree(port);
@@ -686,16 +691,18 @@ static int serial_carrier_raised(struct tty_port *port)
 {
 	struct usb_serial_port *p = container_of(port, struct usb_serial_port, port);
 	struct usb_serial_driver *drv = p->serial->type;
+
 	if (drv->carrier_raised)
 		return drv->carrier_raised(p);
 	/* No carrier control - don't block */
-	return 1;	
+	return 1;
 }
 
 static void serial_dtr_rts(struct tty_port *port, int on)
 {
 	struct usb_serial_port *p = container_of(port, struct usb_serial_port, port);
 	struct usb_serial_driver *drv = p->serial->type;
+
 	if (drv->dtr_rts)
 		drv->dtr_rts(p, on);
 }
@@ -724,6 +731,7 @@ int usb_serial_probe(struct usb_interface *interface,
 	unsigned int minor;
 	int buffer_size;
 	int i;
+	int j;
 	int num_interrupt_in = 0;
 	int num_interrupt_out = 0;
 	int num_bulk_in = 0;
@@ -906,38 +914,41 @@ int usb_serial_probe(struct usb_interface *interface,
 	for (i = 0; i < num_bulk_in; ++i) {
 		endpoint = bulk_in_endpoint[i];
 		port = serial->port[i];
-		port->read_urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!port->read_urb) {
-			dev_err(&interface->dev, "No free urbs available\n");
-			goto probe_error;
-		}
 		buffer_size = max_t(int, serial->type->bulk_in_size,
 				usb_endpoint_maxp(endpoint));
 		port->bulk_in_size = buffer_size;
 		port->bulk_in_endpointAddress = endpoint->bEndpointAddress;
-		port->bulk_in_buffer = kmalloc(buffer_size, GFP_KERNEL);
-		if (!port->bulk_in_buffer) {
-			dev_err(&interface->dev,
+
+		for (j = 0; j < ARRAY_SIZE(port->read_urbs); ++j) {
+			set_bit(j, &port->read_urbs_free);
+			port->read_urbs[j] = usb_alloc_urb(0, GFP_KERNEL);
+			if (!port->read_urbs[j]) {
+				dev_err(&interface->dev,
+						"No free urbs available\n");
+				goto probe_error;
+			}
+			port->bulk_in_buffers[j] = kmalloc(buffer_size,
+								GFP_KERNEL);
+			if (!port->bulk_in_buffers[j]) {
+				dev_err(&interface->dev,
 					"Couldn't allocate bulk_in_buffer\n");
-			goto probe_error;
-		}
-		usb_fill_bulk_urb(port->read_urb, dev,
-				usb_rcvbulkpipe(dev,
+				goto probe_error;
+			}
+			usb_fill_bulk_urb(port->read_urbs[j], dev,
+					usb_rcvbulkpipe(dev,
 						endpoint->bEndpointAddress),
-				port->bulk_in_buffer, buffer_size,
-				serial->type->read_bulk_callback, port);
+					port->bulk_in_buffers[j], buffer_size,
+					serial->type->read_bulk_callback,
+					port);
+		}
+
+		port->read_urb = port->read_urbs[0];
+		port->bulk_in_buffer = port->bulk_in_buffers[0];
 	}
 
 	for (i = 0; i < num_bulk_out; ++i) {
-		int j;
-
 		endpoint = bulk_out_endpoint[i];
 		port = serial->port[i];
-		port->write_urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!port->write_urb) {
-			dev_err(&interface->dev, "No free urbs available\n");
-			goto probe_error;
-		}
 		if (kfifo_alloc(&port->write_fifo, PAGE_SIZE, GFP_KERNEL))
 			goto probe_error;
 		buffer_size = serial->type->bulk_out_size;
@@ -945,17 +956,7 @@ int usb_serial_probe(struct usb_interface *interface,
 			buffer_size = usb_endpoint_maxp(endpoint);
 		port->bulk_out_size = buffer_size;
 		port->bulk_out_endpointAddress = endpoint->bEndpointAddress;
-		port->bulk_out_buffer = kmalloc(buffer_size, GFP_KERNEL);
-		if (!port->bulk_out_buffer) {
-			dev_err(&interface->dev,
-					"Couldn't allocate bulk_out_buffer\n");
-			goto probe_error;
-		}
-		usb_fill_bulk_urb(port->write_urb, dev,
-				usb_sndbulkpipe(dev,
-					endpoint->bEndpointAddress),
-				port->bulk_out_buffer, buffer_size,
-				serial->type->write_bulk_callback, port);
+
 		for (j = 0; j < ARRAY_SIZE(port->write_urbs); ++j) {
 			set_bit(j, &port->write_urbs_free);
 			port->write_urbs[j] = usb_alloc_urb(0, GFP_KERNEL);
@@ -978,6 +979,9 @@ int usb_serial_probe(struct usb_interface *interface,
 					serial->type->write_bulk_callback,
 					port);
 		}
+
+		port->write_urb = port->write_urbs[0];
+		port->bulk_out_buffer = port->bulk_out_buffers[0];
 	}
 
 	if (serial->type->read_int_callback) {
@@ -1196,7 +1200,7 @@ static const struct tty_operations serial_ops = {
 	.open =			serial_open,
 	.close =		serial_close,
 	.write =		serial_write,
-	.hangup = 		serial_hangup,
+	.hangup =		serial_hangup,
 	.write_room =		serial_write_room,
 	.ioctl =		serial_ioctl,
 	.set_termios =		serial_set_termios,
@@ -1206,9 +1210,9 @@ static const struct tty_operations serial_ops = {
 	.chars_in_buffer =	serial_chars_in_buffer,
 	.tiocmget =		serial_tiocmget,
 	.tiocmset =		serial_tiocmset,
-	.get_icount = 		serial_get_icount,
-	.cleanup = 		serial_cleanup,
-	.install = 		serial_install,
+	.get_icount =		serial_get_icount,
+	.cleanup =		serial_cleanup,
+	.install =		serial_install,
 	.proc_fops =		&serial_proc_fops,
 };
 
@@ -1237,7 +1241,7 @@ static int __init usb_serial_init(void)
 
 	usb_serial_tty_driver->owner = THIS_MODULE;
 	usb_serial_tty_driver->driver_name = "usbserial";
-	usb_serial_tty_driver->name = 	"ttyUSB";
+	usb_serial_tty_driver->name = "ttyUSB";
 	usb_serial_tty_driver->major = SERIAL_TTY_MAJOR;
 	usb_serial_tty_driver->minor_start = 0;
 	usb_serial_tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
@@ -1336,7 +1340,6 @@ static void fixup_generic(struct usb_serial_driver *device)
 
 int usb_serial_register(struct usb_serial_driver *driver)
 {
-	/* must be called with BKL held */
 	int retval;
 
 	if (usb_disabled())
@@ -1374,7 +1377,6 @@ EXPORT_SYMBOL_GPL(usb_serial_register);
 
 void usb_serial_deregister(struct usb_serial_driver *device)
 {
-	/* must be called with BKL held */
 	printk(KERN_INFO "USB Serial deregistering driver %s\n",
 	       device->description);
 	mutex_lock(&table_lock);
diff --git a/drivers/usb/serial/usb_debug.c b/drivers/usb/serial/usb_debug.c
index 95a82148ee81..9b632e753210 100644
--- a/drivers/usb/serial/usb_debug.c
+++ b/drivers/usb/serial/usb_debug.c
@@ -40,7 +40,7 @@ static struct usb_driver debug_driver = {
 	.probe =	usb_serial_probe,
 	.disconnect =	usb_serial_disconnect,
 	.id_table =	id_table,
-	.no_dynamic_id = 	1,
+	.no_dynamic_id =	1,
 };
 
 /* This HW really does not support a serial break, so one will be
@@ -54,19 +54,18 @@ static void usb_debug_break_ctl(struct tty_struct *tty, int break_state)
 	usb_serial_generic_write(tty, port, USB_DEBUG_BRK, USB_DEBUG_BRK_SIZE);
 }
 
-static void usb_debug_read_bulk_callback(struct urb *urb)
+static void usb_debug_process_read_urb(struct urb *urb)
 {
 	struct usb_serial_port *port = urb->context;
 
 	if (urb->actual_length == USB_DEBUG_BRK_SIZE &&
-	    memcmp(urb->transfer_buffer, USB_DEBUG_BRK,
-		   USB_DEBUG_BRK_SIZE) == 0) {
+		memcmp(urb->transfer_buffer, USB_DEBUG_BRK,
+						USB_DEBUG_BRK_SIZE) == 0) {
 		usb_serial_handle_break(port);
-		usb_serial_generic_submit_read_urb(port, GFP_ATOMIC);
 		return;
 	}
 
-	usb_serial_generic_read_bulk_callback(urb);
+	usb_serial_generic_process_read_urb(urb);
 }
 
 static struct usb_serial_driver debug_device = {
@@ -79,7 +78,7 @@ static struct usb_serial_driver debug_device = {
 	.num_ports =		1,
 	.bulk_out_size =	USB_DEBUG_MAX_PACKET_SIZE,
 	.break_ctl =		usb_debug_break_ctl,
-	.read_bulk_callback =	usb_debug_read_bulk_callback,
+	.process_read_urb =	usb_debug_process_read_urb,
 };
 
 static int __init debug_init(void)
diff --git a/drivers/usb/serial/whiteheat.c b/drivers/usb/serial/whiteheat.c
index 5b073bcc807b..11af903cb09f 100644
--- a/drivers/usb/serial/whiteheat.c
+++ b/drivers/usb/serial/whiteheat.c
@@ -14,57 +14,6 @@
  *
  * See Documentation/usb/usb-serial.txt for more information on using this
  * driver
- *
- * (10/09/2002) Stuart MacDonald (stuartm@connecttech.com)
- *	Upgrade to full working driver
- *
- * (05/30/2001) gkh
- *	switched from using spinlock to a semaphore, which fixes lots of
- *	problems.
- *
- * (04/08/2001) gb
- *	Identify version on module load.
- *
- * 2001_Mar_19 gkh
- *	Fixed MOD_INC and MOD_DEC logic, the ability to open a port more
- *	than once, and the got the proper usb_device_id table entries so
- *	the driver works again.
- *
- * (11/01/2000) Adam J. Richter
- *	usb_device_id table support
- *
- * (10/05/2000) gkh
- *	Fixed bug with urb->dev not being set properly, now that the usb
- *	core needs it.
- *
- * (10/03/2000) smd
- *	firmware is improved to guard against crap sent to device
- *	firmware now replies CMD_FAILURE on bad things
- *	read_callback fix you provided for private info struct
- *	command_finished now indicates success or fail
- *	setup_port struct now packed to avoid gcc padding
- *	firmware uses 1 based port numbering, driver now handles that
- *
- * (09/11/2000) gkh
- *	Removed DEBUG #ifdefs with call to usb_serial_debug_data
- *
- * (07/19/2000) gkh
- *	Added module_init and module_exit functions to handle the fact that this
- *	driver is a loadable module now.
- *	Fixed bug with port->minor that was found by Al Borchers
- *
- * (07/04/2000) gkh
- *	Added support for port settings. Baud rate can now be changed. Line
- *	signals are not transferred to and from the tty layer yet, but things
- *	seem to be working well now.
- *
- * (05/04/2000) gkh
- *	First cut at open and close commands. Data can flow through the ports at
- *	default speeds now.
- *
- * (03/26/2000) gkh
- *	Split driver up into device specific pieces.
- *
  */
 
 #include <linux/kernel.h>
@@ -753,7 +702,6 @@ static void whiteheat_close(struct usb_serial_port *port)
 static int whiteheat_write(struct tty_struct *tty,
 	struct usb_serial_port *port, const unsigned char *buf, int count)
 {
-	struct usb_serial *serial = port->serial;
 	struct whiteheat_private *info = usb_get_serial_port_data(port);
 	struct whiteheat_urb_wrap *wrap;
 	struct urb *urb;
@@ -789,7 +737,6 @@ static int whiteheat_write(struct tty_struct *tty,
 		usb_serial_debug_data(debug, &port->dev,
 				__func__, bytes, urb->transfer_buffer);
 
-		urb->dev = serial->dev;
 		urb->transfer_buffer_length = bytes;
 		result = usb_submit_urb(urb, GFP_ATOMIC);
 		if (result) {
@@ -1035,7 +982,6 @@ static void command_port_read_callback(struct urb *urb)
 		dbg("%s - bad reply from firmware", __func__);
 
 	/* Continue trying to always read */
-	command_port->read_urb->dev = command_port->serial->dev;
 	result = usb_submit_urb(command_port->read_urb, GFP_ATOMIC);
 	if (result)
 		dbg("%s - failed resubmitting read urb, error %d",
@@ -1141,7 +1087,6 @@ static int firm_send_command(struct usb_serial_port *port, __u8 command,
 	transfer_buffer[0] = command;
 	memcpy(&transfer_buffer[1], data, datasize);
 	command_port->write_urb->transfer_buffer_length = datasize + 1;
-	command_port->write_urb->dev = port->serial->dev;
 	retval = usb_submit_urb(command_port->write_urb, GFP_NOIO);
 	if (retval) {
 		dbg("%s - submit urb failed", __func__);
@@ -1362,7 +1307,6 @@ static int start_command_port(struct usb_serial *serial)
 		/* Work around HCD bugs */
 		usb_clear_halt(serial->dev, command_port->read_urb->pipe);
 
-		command_port->read_urb->dev = serial->dev;
 		retval = usb_submit_urb(command_port->read_urb, GFP_KERNEL);
 		if (retval) {
 			dev_err(&serial->dev->dev,
@@ -1410,7 +1354,6 @@ static int start_port_read(struct usb_serial_port *port)
 		list_del(tmp);
 		wrap = list_entry(tmp, struct whiteheat_urb_wrap, list);
 		urb = wrap->urb;
-		urb->dev = port->serial->dev;
 		spin_unlock_irqrestore(&info->lock, flags);
 		retval = usb_submit_urb(urb, GFP_KERNEL);
 		if (retval) {
@@ -1490,7 +1433,6 @@ static void rx_data_softint(struct work_struct *work)
 			sent += tty_insert_flip_string(tty,
 				urb->transfer_buffer, urb->actual_length);
 
-		urb->dev = port->serial->dev;
 		result = usb_submit_urb(urb, GFP_ATOMIC);
 		if (result) {
 			dev_err(&port->dev,
diff --git a/drivers/usb/storage/alauda.c b/drivers/usb/storage/alauda.c
index 3ca87a823342..51af2fee2efd 100644
--- a/drivers/usb/storage/alauda.c
+++ b/drivers/usb/storage/alauda.c
@@ -139,7 +139,7 @@ static int init_alauda(struct us_data *us);
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id alauda_usb_ids[] = {
+static struct usb_device_id alauda_usb_ids[] = {
 #	include "unusual_alauda.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/cypress_atacb.c b/drivers/usb/storage/cypress_atacb.c
index c7909dfa2434..387cbd47acc9 100644
--- a/drivers/usb/storage/cypress_atacb.c
+++ b/drivers/usb/storage/cypress_atacb.c
@@ -43,7 +43,7 @@ MODULE_LICENSE("GPL");
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id cypress_usb_ids[] = {
+static struct usb_device_id cypress_usb_ids[] = {
 #	include "unusual_cypress.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/datafab.c b/drivers/usb/storage/datafab.c
index a99be857b794..15d41f2b3d6f 100644
--- a/drivers/usb/storage/datafab.c
+++ b/drivers/usb/storage/datafab.c
@@ -88,7 +88,7 @@ static int datafab_determine_lun(struct us_data *us,
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id datafab_usb_ids[] = {
+static struct usb_device_id datafab_usb_ids[] = {
 #	include "unusual_datafab.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/ene_ub6250.c b/drivers/usb/storage/ene_ub6250.c
index b0a1687ca942..a6ade4071a9a 100644
--- a/drivers/usb/storage/ene_ub6250.c
+++ b/drivers/usb/storage/ene_ub6250.c
@@ -42,7 +42,7 @@ MODULE_LICENSE("GPL");
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
 	.driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id ene_ub6250_usb_ids[] = {
+static struct usb_device_id ene_ub6250_usb_ids[] = {
 #	include "unusual_ene_ub6250.h"
 	{ }		/* Terminating entry */
 };
@@ -607,8 +607,8 @@ static int sd_scsi_mode_sense(struct us_data *us, struct scsi_cmnd *srb)
 
 static int sd_scsi_read_capacity(struct us_data *us, struct scsi_cmnd *srb)
 {
-	u32   bl_num;
-	u16    bl_len;
+	u32	bl_num;
+	u32	bl_len;
 	unsigned int offset = 0;
 	unsigned char    buf[8];
 	struct scatterlist *sg = NULL;
@@ -622,7 +622,7 @@ static int sd_scsi_read_capacity(struct us_data *us, struct scsi_cmnd *srb)
 		else
 			bl_num = (info->HC_C_SIZE + 1) * 1024 - 1;
 	} else {
-		bl_len = 1<<(info->SD_READ_BL_LEN);
+		bl_len = 1 << (info->SD_READ_BL_LEN);
 		bl_num = info->SD_Block_Mult * (info->SD_C_SIZE + 1)
 				* (1 << (info->SD_C_SIZE_MULT + 2)) - 1;
 	}
@@ -777,7 +777,7 @@ static int ms_lib_free_logicalmap(struct us_data *us)
 	return 0;
 }
 
-int ms_lib_alloc_logicalmap(struct us_data *us)
+static int ms_lib_alloc_logicalmap(struct us_data *us)
 {
 	u32  i;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
@@ -2248,7 +2248,7 @@ static int sd_scsi_irp(struct us_data *us, struct scsi_cmnd *srb)
 /*
  * ms_scsi_irp()
  */
-int ms_scsi_irp(struct us_data *us, struct scsi_cmnd *srb)
+static int ms_scsi_irp(struct us_data *us, struct scsi_cmnd *srb)
 {
 	int result;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *)us->extra;
diff --git a/drivers/usb/storage/freecom.c b/drivers/usb/storage/freecom.c
index 03d4a873748c..fa1615748475 100644
--- a/drivers/usb/storage/freecom.c
+++ b/drivers/usb/storage/freecom.c
@@ -119,7 +119,7 @@ static int init_freecom(struct us_data *us);
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id freecom_usb_ids[] = {
+static struct usb_device_id freecom_usb_ids[] = {
 #	include "unusual_freecom.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/isd200.c b/drivers/usb/storage/isd200.c
index 93d359f4d6a7..bd5502700831 100644
--- a/drivers/usb/storage/isd200.c
+++ b/drivers/usb/storage/isd200.c
@@ -76,7 +76,7 @@ static int isd200_Initialization(struct us_data *us);
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id isd200_usb_ids[] = {
+static struct usb_device_id isd200_usb_ids[] = {
 #	include "unusual_isd200.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/jumpshot.c b/drivers/usb/storage/jumpshot.c
index 54b71650b69c..a19211b5c265 100644
--- a/drivers/usb/storage/jumpshot.c
+++ b/drivers/usb/storage/jumpshot.c
@@ -71,7 +71,7 @@ MODULE_LICENSE("GPL");
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id jumpshot_usb_ids[] = {
+static struct usb_device_id jumpshot_usb_ids[] = {
 #	include "unusual_jumpshot.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/karma.c b/drivers/usb/storage/karma.c
index 35181e29124d..e720f8ebdf9f 100644
--- a/drivers/usb/storage/karma.c
+++ b/drivers/usb/storage/karma.c
@@ -59,7 +59,7 @@ static int rio_karma_init(struct us_data *us);
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id karma_usb_ids[] = {
+static struct usb_device_id karma_usb_ids[] = {
 #	include "unusual_karma.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/onetouch.c b/drivers/usb/storage/onetouch.c
index 721c8c587305..d75155c38200 100644
--- a/drivers/usb/storage/onetouch.c
+++ b/drivers/usb/storage/onetouch.c
@@ -69,7 +69,7 @@ struct usb_onetouch {
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id onetouch_usb_ids[] = {
+static struct usb_device_id onetouch_usb_ids[] = {
 #	include "unusual_onetouch.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/realtek_cr.c b/drivers/usb/storage/realtek_cr.c
index c41cd30d2c01..1f62723ef1a8 100644
--- a/drivers/usb/storage/realtek_cr.c
+++ b/drivers/usb/storage/realtek_cr.c
@@ -398,10 +398,9 @@ static int rts51x_write_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 	u8 cmnd[12] = { 0 };
 	u8 *buf;
 
-	buf = kmalloc(len, GFP_NOIO);
+	buf = kmemdup(data, len, GFP_NOIO);
 	if (buf == NULL)
 		return USB_STOR_TRANSPORT_ERROR;
-	memcpy(buf, data, len);
 
 	US_DEBUGP("%s, addr = 0x%x, len = %d\n", __func__, addr, len);
 
@@ -507,15 +506,14 @@ static int enable_oscillator(struct us_data *us)
 static int __do_config_autodelink(struct us_data *us, u8 *data, u16 len)
 {
 	int retval;
-	u16 addr = 0xFE47;
 	u8 cmnd[12] = {0};
 
-	US_DEBUGP("%s, addr = 0x%x, len = %d\n", __FUNCTION__, addr, len);
+	US_DEBUGP("%s, addr = 0xfe47, len = %d\n", __FUNCTION__, len);
 
 	cmnd[0] = 0xF0;
 	cmnd[1] = 0x0E;
-	cmnd[2] = (u8)(addr >> 8);
-	cmnd[3] = (u8)addr;
+	cmnd[2] = 0xfe;
+	cmnd[3] = 0x47;
 	cmnd[4] = (u8)(len >> 8);
 	cmnd[5] = (u8)len;
 
@@ -818,7 +816,7 @@ static inline int working_scsi(struct scsi_cmnd *srb)
 	return 1;
 }
 
-void rts51x_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
+static void rts51x_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 {
 	struct rts51x_chip *chip = (struct rts51x_chip *)(us->extra);
 	static int card_first_show = 1;
@@ -977,7 +975,7 @@ static void realtek_cr_destructor(void *extra)
 }
 
 #ifdef CONFIG_PM
-int realtek_cr_suspend(struct usb_interface *iface, pm_message_t message)
+static int realtek_cr_suspend(struct usb_interface *iface, pm_message_t message)
 {
 	struct us_data *us = usb_get_intfdata(iface);
 
diff --git a/drivers/usb/storage/sddr09.c b/drivers/usb/storage/sddr09.c
index 83ee49e737bd..425df7df2e56 100644
--- a/drivers/usb/storage/sddr09.c
+++ b/drivers/usb/storage/sddr09.c
@@ -71,7 +71,7 @@ static int usb_stor_sddr09_init(struct us_data *us);
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id sddr09_usb_ids[] = {
+static struct usb_device_id sddr09_usb_ids[] = {
 #	include "unusual_sddr09.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/sddr55.c b/drivers/usb/storage/sddr55.c
index 8983ec2ffb5a..e4ca5fcb7cc3 100644
--- a/drivers/usb/storage/sddr55.c
+++ b/drivers/usb/storage/sddr55.c
@@ -48,7 +48,7 @@ MODULE_LICENSE("GPL");
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id sddr55_usb_ids[] = {
+static struct usb_device_id sddr55_usb_ids[] = {
 #	include "unusual_sddr55.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/shuttle_usbat.c b/drivers/usb/storage/shuttle_usbat.c
index a4c02751af4e..1369d2590616 100644
--- a/drivers/usb/storage/shuttle_usbat.c
+++ b/drivers/usb/storage/shuttle_usbat.c
@@ -170,7 +170,7 @@ static int init_usbat_flash(struct us_data *us);
 { USB_DEVICE_VER(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax), \
   .driver_info = (flags)|(USB_US_TYPE_STOR<<24) }
 
-struct usb_device_id usbat_usb_ids[] = {
+static struct usb_device_id usbat_usb_ids[] = {
 #	include "unusual_usbat.h"
 	{ }		/* Terminating entry */
 };
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index aa84b3d77274..3dd7da9fd504 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -1074,6 +1074,7 @@ static struct usb_driver usb_storage_driver = {
 	.id_table =	usb_storage_usb_ids,
 	.supports_autosuspend = 1,
 	.soft_unbind =	1,
+	.no_dynamic_id = 1,
 };
 
 static int __init usb_stor_init(void)
diff --git a/drivers/usb/usb-skeleton.c b/drivers/usb/usb-skeleton.c
index 5c6c1bdbd455..8efeae24764f 100644
--- a/drivers/usb/usb-skeleton.c
+++ b/drivers/usb/usb-skeleton.c
@@ -27,6 +27,8 @@
 #define USB_SKEL_VENDOR_ID	0xfff0
 #define USB_SKEL_PRODUCT_ID	0xfff0
 
+static DEFINE_MUTEX(skel_mutex);
+
 /* table of devices that work with this driver */
 static const struct usb_device_id skel_table[] = {
 	{ USB_DEVICE(USB_SKEL_VENDOR_ID, USB_SKEL_PRODUCT_ID) },
@@ -60,7 +62,6 @@ struct usb_skel {
 	__u8			bulk_in_endpointAddr;	/* the address of the bulk in endpoint */
 	__u8			bulk_out_endpointAddr;	/* the address of the bulk out endpoint */
 	int			errors;			/* the last request tanked */
-	int			open_count;		/* count the number of openers */
 	bool			ongoing_read;		/* a read is going on */
 	bool			processed_urb;		/* indicates we haven't processed the urb */
 	spinlock_t		err_lock;		/* lock for errors */
@@ -100,39 +101,37 @@ static int skel_open(struct inode *inode, struct file *file)
 		goto exit;
 	}
 
+	mutex_lock(&skel_mutex);
 	dev = usb_get_intfdata(interface);
 	if (!dev) {
+		mutex_unlock(&skel_mutex);
 		retval = -ENODEV;
 		goto exit;
 	}
 
 	/* increment our usage count for the device */
 	kref_get(&dev->kref);
+	mutex_unlock(&skel_mutex);
 
 	/* lock the device to allow correctly handling errors
 	 * in resumption */
 	mutex_lock(&dev->io_mutex);
+	if (!dev->interface) {
+		retval = -ENODEV;
+		goto out_err;
+	}
 
-	if (!dev->open_count++) {
-		retval = usb_autopm_get_interface(interface);
-			if (retval) {
-				dev->open_count--;
-				mutex_unlock(&dev->io_mutex);
-				kref_put(&dev->kref, skel_delete);
-				goto exit;
-			}
-	} /* else { //uncomment this block if you want exclusive open
-		retval = -EBUSY;
-		dev->open_count--;
-		mutex_unlock(&dev->io_mutex);
-		kref_put(&dev->kref, skel_delete);
-		goto exit;
-	} */
-	/* prevent the device from being autosuspended */
+	retval = usb_autopm_get_interface(interface);
+	if (retval)
+		goto out_err;
 
 	/* save our object in the file's private structure */
 	file->private_data = dev;
+
+out_err:
 	mutex_unlock(&dev->io_mutex);
+	if (retval)
+		kref_put(&dev->kref, skel_delete);
 
 exit:
 	return retval;
@@ -148,7 +147,7 @@ static int skel_release(struct inode *inode, struct file *file)
 
 	/* allow the device to be autosuspended */
 	mutex_lock(&dev->io_mutex);
-	if (!--dev->open_count && dev->interface)
+	if (dev->interface)
 		usb_autopm_put_interface(dev->interface);
 	mutex_unlock(&dev->io_mutex);
 
@@ -612,7 +611,6 @@ static void skel_disconnect(struct usb_interface *interface)
 	int minor = interface->minor;
 
 	dev = usb_get_intfdata(interface);
-	usb_set_intfdata(interface, NULL);
 
 	/* give back our minor */
 	usb_deregister_dev(interface, &skel_class);
@@ -624,8 +622,12 @@ static void skel_disconnect(struct usb_interface *interface)
 
 	usb_kill_anchored_urbs(&dev->submitted);
 
+	mutex_lock(&skel_mutex);
+	usb_set_intfdata(interface, NULL);
+
 	/* decrement our usage count */
 	kref_put(&dev->kref, skel_delete);
+	mutex_unlock(&skel_mutex);
 
 	dev_info(&interface->dev, "USB Skeleton #%d now disconnected", minor);
 }
diff --git a/drivers/usb/wusbcore/Kconfig b/drivers/usb/wusbcore/Kconfig
index eb09a0a14a80..0ead8826ec79 100644
--- a/drivers/usb/wusbcore/Kconfig
+++ b/drivers/usb/wusbcore/Kconfig
@@ -5,6 +5,7 @@ config USB_WUSB
 	tristate "Enable Wireless USB extensions (EXPERIMENTAL)"
 	depends on EXPERIMENTAL
 	depends on USB
+	depends on PCI
         select UWB
         select CRYPTO
         select CRYPTO_BLKCIPHER
diff --git a/drivers/usb/wusbcore/security.c b/drivers/usb/wusbcore/security.c
index 371f61733f05..fa810a83e830 100644
--- a/drivers/usb/wusbcore/security.c
+++ b/drivers/usb/wusbcore/security.c
@@ -354,7 +354,7 @@ int wusb_dev_4way_handshake(struct wusbhc *wusbhc, struct wusb_dev *wusb_dev,
 	struct wusb_keydvt_in keydvt_in;
 	struct wusb_keydvt_out keydvt_out;
 
-	hs = kzalloc(3*sizeof(hs[0]), GFP_KERNEL);
+	hs = kcalloc(3, sizeof(hs[0]), GFP_KERNEL);
 	if (hs == NULL) {
 		dev_err(dev, "can't allocate handshake data\n");
 		goto error_kzalloc;
diff --git a/drivers/uwb/est.c b/drivers/uwb/est.c
index de81ebf51784..86ed7e61e597 100644
--- a/drivers/uwb/est.c
+++ b/drivers/uwb/est.c
@@ -184,7 +184,7 @@ int uwb_est_create(void)
 
 	uwb_est_size = 2;
 	uwb_est_used = 0;
-	uwb_est = kzalloc(uwb_est_size * sizeof(uwb_est[0]), GFP_KERNEL);
+	uwb_est = kcalloc(uwb_est_size, sizeof(uwb_est[0]), GFP_KERNEL);
 	if (uwb_est == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/video/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index 1105fa1ed7f4..a1376dc73d71 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -270,17 +270,7 @@ static struct platform_driver pm860x_backlight_driver = {
 	.remove		= pm860x_backlight_remove,
 };
 
-static int __init pm860x_backlight_init(void)
-{
-	return platform_driver_register(&pm860x_backlight_driver);
-}
-module_init(pm860x_backlight_init);
-
-static void __exit pm860x_backlight_exit(void)
-{
-	platform_driver_unregister(&pm860x_backlight_driver);
-}
-module_exit(pm860x_backlight_exit);
+module_platform_driver(pm860x_backlight_driver);
 
 MODULE_DESCRIPTION("Backlight Driver for Marvell Semiconductor 88PM8606");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index 278aeaa92505..681b36929fe4 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -280,14 +280,6 @@ config BACKLIGHT_WM831X
 	  If you have a backlight driven by the ISINK and DCDC of a
 	  WM831x PMIC say y to enable the backlight driver for it.
 
-config BACKLIGHT_ADX
-	tristate "Avionic Design Xanthos Backlight Driver"
-	depends on ARCH_PXA_ADX
-	default y
-	help
-	  Say Y to enable the backlight driver on Avionic Design Xanthos-based
-	  boards.
-
 config BACKLIGHT_ADP5520
 	tristate "Backlight Driver for ADP5520/ADP5501 using WLED"
 	depends on PMIC_ADP5520
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index fdd1fc4b2770..af5cf654ec7c 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -32,7 +32,6 @@ obj-$(CONFIG_BACKLIGHT_APPLE)	+= apple_bl.o
 obj-$(CONFIG_BACKLIGHT_TOSA)	+= tosa_bl.o
 obj-$(CONFIG_BACKLIGHT_SAHARA)	+= kb3886_bl.o
 obj-$(CONFIG_BACKLIGHT_WM831X)	+= wm831x_bl.o
-obj-$(CONFIG_BACKLIGHT_ADX)    += adx_bl.o
 obj-$(CONFIG_BACKLIGHT_ADP5520)	+= adp5520_bl.o
 obj-$(CONFIG_BACKLIGHT_ADP8860)	+= adp8860_bl.o
 obj-$(CONFIG_BACKLIGHT_ADP8870)	+= adp8870_bl.o
diff --git a/drivers/video/backlight/adp5520_bl.c b/drivers/video/backlight/adp5520_bl.c
index dfb763e9147f..2e630bf1164c 100644
--- a/drivers/video/backlight/adp5520_bl.c
+++ b/drivers/video/backlight/adp5520_bl.c
@@ -384,17 +384,7 @@ static struct platform_driver adp5520_bl_driver = {
 	.resume		= adp5520_bl_resume,
 };
 
-static int __init adp5520_bl_init(void)
-{
-	return platform_driver_register(&adp5520_bl_driver);
-}
-module_init(adp5520_bl_init);
-
-static void __exit adp5520_bl_exit(void)
-{
-	platform_driver_unregister(&adp5520_bl_driver);
-}
-module_exit(adp5520_bl_exit);
+module_platform_driver(adp5520_bl_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("ADP5520(01) Backlight Driver");
diff --git a/drivers/video/backlight/adx_bl.c b/drivers/video/backlight/adx_bl.c
deleted file mode 100644
index c861c41af442..000000000000
--- a/drivers/video/backlight/adx_bl.c
+++ /dev/null
@@ -1,182 +0,0 @@
-/*
- * linux/drivers/video/backlight/adx.c
- *
- * Copyright (C) 2009 Avionic Design GmbH
- *
- * 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.
- *
- * Written by Thierry Reding <thierry.reding@avionic-design.de>
- */
-
-#include <linux/backlight.h>
-#include <linux/fb.h>
-#include <linux/gfp.h>
-#include <linux/io.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-
-/* register definitions */
-#define ADX_BACKLIGHT_CONTROL		0x00
-#define ADX_BACKLIGHT_CONTROL_ENABLE	(1 << 0)
-#define ADX_BACKLIGHT_BRIGHTNESS	0x08
-#define ADX_BACKLIGHT_STATUS		0x10
-#define ADX_BACKLIGHT_ERROR		0x18
-
-struct adxbl {
-	void __iomem *base;
-};
-
-static int adx_backlight_update_status(struct backlight_device *bldev)
-{
-	struct adxbl *bl = bl_get_data(bldev);
-	u32 value;
-
-	value = bldev->props.brightness;
-	writel(value, bl->base + ADX_BACKLIGHT_BRIGHTNESS);
-
-	value = readl(bl->base + ADX_BACKLIGHT_CONTROL);
-
-	if (bldev->props.state & BL_CORE_FBBLANK)
-		value &= ~ADX_BACKLIGHT_CONTROL_ENABLE;
-	else
-		value |= ADX_BACKLIGHT_CONTROL_ENABLE;
-
-	writel(value, bl->base + ADX_BACKLIGHT_CONTROL);
-
-	return 0;
-}
-
-static int adx_backlight_get_brightness(struct backlight_device *bldev)
-{
-	struct adxbl *bl = bl_get_data(bldev);
-	u32 brightness;
-
-	brightness = readl(bl->base + ADX_BACKLIGHT_BRIGHTNESS);
-	return brightness & 0xff;
-}
-
-static int adx_backlight_check_fb(struct backlight_device *bldev, struct fb_info *fb)
-{
-	return 1;
-}
-
-static const struct backlight_ops adx_backlight_ops = {
-	.options = 0,
-	.update_status = adx_backlight_update_status,
-	.get_brightness = adx_backlight_get_brightness,
-	.check_fb = adx_backlight_check_fb,
-};
-
-static int __devinit adx_backlight_probe(struct platform_device *pdev)
-{
-	struct backlight_properties props;
-	struct backlight_device *bldev;
-	struct resource *res;
-	struct adxbl *bl;
-	int ret = 0;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		ret = -ENXIO;
-		goto out;
-	}
-
-	res = devm_request_mem_region(&pdev->dev, res->start,
-			resource_size(res), res->name);
-	if (!res) {
-		ret = -ENXIO;
-		goto out;
-	}
-
-	bl = devm_kzalloc(&pdev->dev, sizeof(*bl), GFP_KERNEL);
-	if (!bl) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	bl->base = devm_ioremap_nocache(&pdev->dev, res->start,
-			resource_size(res));
-	if (!bl->base) {
-		ret = -ENXIO;
-		goto out;
-	}
-
-	memset(&props, 0, sizeof(struct backlight_properties));
-	props.type = BACKLIGHT_RAW;
-	props.max_brightness = 0xff;
-	bldev = backlight_device_register(dev_name(&pdev->dev), &pdev->dev,
-					  bl, &adx_backlight_ops, &props);
-	if (IS_ERR(bldev)) {
-		ret = PTR_ERR(bldev);
-		goto out;
-	}
-
-	bldev->props.brightness = 0xff;
-	bldev->props.power = FB_BLANK_UNBLANK;
-
-	platform_set_drvdata(pdev, bldev);
-
-out:
-	return ret;
-}
-
-static int __devexit adx_backlight_remove(struct platform_device *pdev)
-{
-	struct backlight_device *bldev;
-	int ret = 0;
-
-	bldev = platform_get_drvdata(pdev);
-	bldev->props.power = FB_BLANK_UNBLANK;
-	bldev->props.brightness = 0xff;
-	backlight_update_status(bldev);
-	backlight_device_unregister(bldev);
-	platform_set_drvdata(pdev, NULL);
-
-	return ret;
-}
-
-#ifdef CONFIG_PM
-static int adx_backlight_suspend(struct platform_device *pdev,
-		pm_message_t state)
-{
-	return 0;
-}
-
-static int adx_backlight_resume(struct platform_device *pdev)
-{
-	return 0;
-}
-#else
-#define adx_backlight_suspend NULL
-#define adx_backlight_resume NULL
-#endif
-
-static struct platform_driver adx_backlight_driver = {
-	.probe = adx_backlight_probe,
-	.remove = __devexit_p(adx_backlight_remove),
-	.suspend = adx_backlight_suspend,
-	.resume = adx_backlight_resume,
-	.driver = {
-		.name = "adx-backlight",
-		.owner = THIS_MODULE,
-	},
-};
-
-static int __init adx_backlight_init(void)
-{
-	return platform_driver_register(&adx_backlight_driver);
-}
-
-static void __exit adx_backlight_exit(void)
-{
-	platform_driver_unregister(&adx_backlight_driver);
-}
-
-module_init(adx_backlight_init);
-module_exit(adx_backlight_exit);
-
-MODULE_AUTHOR("Thierry Reding <thierry.reding@avionic-design.de>");
-MODULE_DESCRIPTION("Avionic Design Xanthos Backlight Driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/backlight/backlight.c b/drivers/video/backlight/backlight.c
index 7363c1b169e8..bf5b1ece7160 100644
--- a/drivers/video/backlight/backlight.c
+++ b/drivers/video/backlight/backlight.c
@@ -102,7 +102,7 @@ static void backlight_generate_event(struct backlight_device *bd,
 }
 
 static ssize_t backlight_show_power(struct device *dev,
-		struct device_attribute *attr,char *buf)
+		struct device_attribute *attr, char *buf)
 {
 	struct backlight_device *bd = to_backlight_device(dev);
 
@@ -116,7 +116,7 @@ static ssize_t backlight_store_power(struct device *dev,
 	struct backlight_device *bd = to_backlight_device(dev);
 	unsigned long power;
 
-	rc = strict_strtoul(buf, 0, &power);
+	rc = kstrtoul(buf, 0, &power);
 	if (rc)
 		return rc;
 
@@ -150,7 +150,7 @@ static ssize_t backlight_store_brightness(struct device *dev,
 	struct backlight_device *bd = to_backlight_device(dev);
 	unsigned long brightness;
 
-	rc = strict_strtoul(buf, 0, &brightness);
+	rc = kstrtoul(buf, 0, &brightness);
 	if (rc)
 		return rc;
 
diff --git a/drivers/video/backlight/da903x_bl.c b/drivers/video/backlight/da903x_bl.c
index d68f14bbb687..abb4a06268f1 100644
--- a/drivers/video/backlight/da903x_bl.c
+++ b/drivers/video/backlight/da903x_bl.c
@@ -199,17 +199,7 @@ static struct platform_driver da903x_backlight_driver = {
 	.remove		= da903x_backlight_remove,
 };
 
-static int __init da903x_backlight_init(void)
-{
-	return platform_driver_register(&da903x_backlight_driver);
-}
-module_init(da903x_backlight_init);
-
-static void __exit da903x_backlight_exit(void)
-{
-	platform_driver_unregister(&da903x_backlight_driver);
-}
-module_exit(da903x_backlight_exit);
+module_platform_driver(da903x_backlight_driver);
 
 MODULE_DESCRIPTION("Backlight Driver for Dialog Semiconductor DA9030/DA9034");
 MODULE_AUTHOR("Eric Miao <eric.miao@marvell.com>"
diff --git a/drivers/video/backlight/ep93xx_bl.c b/drivers/video/backlight/ep93xx_bl.c
index c74a6f4baa12..b62b8b9063b5 100644
--- a/drivers/video/backlight/ep93xx_bl.c
+++ b/drivers/video/backlight/ep93xx_bl.c
@@ -13,7 +13,6 @@
 
 #include <linux/module.h>
 #include <linux/platform_device.h>
-#include <linux/module.h>
 #include <linux/io.h>
 #include <linux/fb.h>
 #include <linux/backlight.h>
@@ -144,17 +143,7 @@ static struct platform_driver ep93xxbl_driver = {
 	.resume		= ep93xxbl_resume,
 };
 
-static int __init ep93xxbl_init(void)
-{
-	return platform_driver_register(&ep93xxbl_driver);
-}
-module_init(ep93xxbl_init);
-
-static void __exit ep93xxbl_exit(void)
-{
-	platform_driver_unregister(&ep93xxbl_driver);
-}
-module_exit(ep93xxbl_exit);
+module_platform_driver(ep93xxbl_driver);
 
 MODULE_DESCRIPTION("EP93xx Backlight Driver");
 MODULE_AUTHOR("H Hartley Sweeten <hsweeten@visionengravers.com>");
diff --git a/drivers/video/backlight/generic_bl.c b/drivers/video/backlight/generic_bl.c
index adb191466d64..9ce6170c1860 100644
--- a/drivers/video/backlight/generic_bl.c
+++ b/drivers/video/backlight/generic_bl.c
@@ -132,18 +132,7 @@ static struct platform_driver genericbl_driver = {
 	},
 };
 
-static int __init genericbl_init(void)
-{
-	return platform_driver_register(&genericbl_driver);
-}
-
-static void __exit genericbl_exit(void)
-{
-	platform_driver_unregister(&genericbl_driver);
-}
-
-module_init(genericbl_init);
-module_exit(genericbl_exit);
+module_platform_driver(genericbl_driver);
 
 MODULE_AUTHOR("Richard Purdie <rpurdie@rpsys.net>");
 MODULE_DESCRIPTION("Generic Backlight Driver");
diff --git a/drivers/video/backlight/jornada720_bl.c b/drivers/video/backlight/jornada720_bl.c
index de65d80159be..2f8af5d786ab 100644
--- a/drivers/video/backlight/jornada720_bl.c
+++ b/drivers/video/backlight/jornada720_bl.c
@@ -147,19 +147,8 @@ static struct platform_driver jornada_bl_driver = {
 	},
 };
 
-static int __init jornada_bl_init(void)
-{
-	return platform_driver_register(&jornada_bl_driver);
-}
-
-static void __exit jornada_bl_exit(void)
-{
-	platform_driver_unregister(&jornada_bl_driver);
-}
+module_platform_driver(jornada_bl_driver);
 
 MODULE_AUTHOR("Kristoffer Ericson <kristoffer.ericson>");
 MODULE_DESCRIPTION("HP Jornada 710/720/728 Backlight driver");
 MODULE_LICENSE("GPL");
-
-module_init(jornada_bl_init);
-module_exit(jornada_bl_exit);
diff --git a/drivers/video/backlight/jornada720_lcd.c b/drivers/video/backlight/jornada720_lcd.c
index d2ff658b4144..22d231a17e3c 100644
--- a/drivers/video/backlight/jornada720_lcd.c
+++ b/drivers/video/backlight/jornada720_lcd.c
@@ -135,19 +135,8 @@ static struct platform_driver jornada_lcd_driver = {
 	},
 };
 
-static int __init jornada_lcd_init(void)
-{
-	return platform_driver_register(&jornada_lcd_driver);
-}
-
-static void __exit jornada_lcd_exit(void)
-{
-	platform_driver_unregister(&jornada_lcd_driver);
-}
+module_platform_driver(jornada_lcd_driver);
 
 MODULE_AUTHOR("Kristoffer Ericson <kristoffer.ericson@gmail.com>");
 MODULE_DESCRIPTION("HP Jornada 710/720/728 LCD driver");
 MODULE_LICENSE("GPL");
-
-module_init(jornada_lcd_init);
-module_exit(jornada_lcd_exit);
diff --git a/drivers/video/backlight/lcd.c b/drivers/video/backlight/lcd.c
index 71a11cadffc4..79c1b0d609a8 100644
--- a/drivers/video/backlight/lcd.c
+++ b/drivers/video/backlight/lcd.c
@@ -97,19 +97,16 @@ static ssize_t lcd_store_power(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	int rc = -ENXIO;
-	char *endp;
 	struct lcd_device *ld = to_lcd_device(dev);
-	int power = simple_strtoul(buf, &endp, 0);
-	size_t size = endp - buf;
+	unsigned long power;
 
-	if (isspace(*endp))
-		size++;
-	if (size != count)
-		return -EINVAL;
+	rc = kstrtoul(buf, 0, &power);
+	if (rc)
+		return rc;
 
 	mutex_lock(&ld->ops_lock);
 	if (ld->ops && ld->ops->set_power) {
-		pr_debug("lcd: set power to %d\n", power);
+		pr_debug("lcd: set power to %lu\n", power);
 		ld->ops->set_power(ld, power);
 		rc = count;
 	}
@@ -136,19 +133,16 @@ static ssize_t lcd_store_contrast(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
 {
 	int rc = -ENXIO;
-	char *endp;
 	struct lcd_device *ld = to_lcd_device(dev);
-	int contrast = simple_strtoul(buf, &endp, 0);
-	size_t size = endp - buf;
+	unsigned long contrast;
 
-	if (isspace(*endp))
-		size++;
-	if (size != count)
-		return -EINVAL;
+	rc = kstrtoul(buf, 0, &contrast);
+	if (rc)
+		return rc;
 
 	mutex_lock(&ld->ops_lock);
 	if (ld->ops && ld->ops->set_contrast) {
-		pr_debug("lcd: set contrast to %d\n", contrast);
+		pr_debug("lcd: set contrast to %lu\n", contrast);
 		ld->ops->set_contrast(ld, contrast);
 		rc = count;
 	}
diff --git a/drivers/video/backlight/ld9040.c b/drivers/video/backlight/ld9040.c
index da9a5ce0ccb8..78dafc0c8fc5 100644
--- a/drivers/video/backlight/ld9040.c
+++ b/drivers/video/backlight/ld9040.c
@@ -31,6 +31,7 @@
 #include <linux/lcd.h>
 #include <linux/backlight.h>
 #include <linux/module.h>
+#include <linux/regulator/consumer.h>
 
 #include "ld9040_gamma.h"
 
@@ -53,8 +54,51 @@ struct ld9040 {
 	struct lcd_device		*ld;
 	struct backlight_device		*bd;
 	struct lcd_platform_data	*lcd_pd;
+
+	struct mutex			lock;
+	bool  enabled;
+};
+
+static struct regulator_bulk_data supplies[] = {
+	{ .supply = "vdd3", },
+	{ .supply = "vci", },
 };
 
+static void ld9040_regulator_enable(struct ld9040 *lcd)
+{
+	int ret = 0;
+	struct lcd_platform_data *pd = NULL;
+
+	pd = lcd->lcd_pd;
+	mutex_lock(&lcd->lock);
+	if (!lcd->enabled) {
+		ret = regulator_bulk_enable(ARRAY_SIZE(supplies), supplies);
+		if (ret)
+			goto out;
+
+		lcd->enabled = true;
+	}
+	mdelay(pd->power_on_delay);
+out:
+	mutex_unlock(&lcd->lock);
+}
+
+static void ld9040_regulator_disable(struct ld9040 *lcd)
+{
+	int ret = 0;
+
+	mutex_lock(&lcd->lock);
+	if (lcd->enabled) {
+		ret = regulator_bulk_disable(ARRAY_SIZE(supplies), supplies);
+		if (ret)
+			goto out;
+
+		lcd->enabled = false;
+	}
+out:
+	mutex_unlock(&lcd->lock);
+}
+
 static const unsigned short seq_swreset[] = {
 	0x01, COMMAND_ONLY,
 	ENDDEF, 0x00
@@ -532,13 +576,8 @@ static int ld9040_power_on(struct ld9040 *lcd)
 		return -EFAULT;
 	}
 
-	if (!pd->power_on) {
-		dev_err(lcd->dev, "power_on is NULL.\n");
-		return -EFAULT;
-	} else {
-		pd->power_on(lcd->ld, 1);
-		mdelay(pd->power_on_delay);
-	}
+	/* lcd power on */
+	ld9040_regulator_enable(lcd);
 
 	if (!pd->reset) {
 		dev_err(lcd->dev, "reset is NULL.\n");
@@ -582,11 +621,8 @@ static int ld9040_power_off(struct ld9040 *lcd)
 
 	mdelay(pd->power_off_delay);
 
-	if (!pd->power_on) {
-		dev_err(lcd->dev, "power_on is NULL.\n");
-		return -EFAULT;
-	} else
-		pd->power_on(lcd->ld, 0);
+	/* lcd power off */
+	ld9040_regulator_disable(lcd);
 
 	return 0;
 }
@@ -693,6 +729,14 @@ static int ld9040_probe(struct spi_device *spi)
 		goto out_free_lcd;
 	}
 
+	mutex_init(&lcd->lock);
+
+	ret = regulator_bulk_get(lcd->dev, ARRAY_SIZE(supplies), supplies);
+	if (ret) {
+		dev_err(lcd->dev, "Failed to get regulators: %d\n", ret);
+		goto out_free_lcd;
+	}
+
 	ld = lcd_device_register("ld9040", &spi->dev, lcd, &ld9040_lcd_ops);
 	if (IS_ERR(ld)) {
 		ret = PTR_ERR(ld);
@@ -739,6 +783,8 @@ static int ld9040_probe(struct spi_device *spi)
 out_unregister_lcd:
 	lcd_device_unregister(lcd->ld);
 out_free_lcd:
+	regulator_bulk_free(ARRAY_SIZE(supplies), supplies);
+
 	kfree(lcd);
 	return ret;
 }
@@ -750,6 +796,7 @@ static int __devexit ld9040_remove(struct spi_device *spi)
 	ld9040_power(lcd, FB_BLANK_POWERDOWN);
 	backlight_device_unregister(lcd->bd);
 	lcd_device_unregister(lcd->ld);
+	regulator_bulk_free(ARRAY_SIZE(supplies), supplies);
 	kfree(lcd);
 
 	return 0;
diff --git a/drivers/video/backlight/max8925_bl.c b/drivers/video/backlight/max8925_bl.c
index 7bbc802560ea..c915e3b53886 100644
--- a/drivers/video/backlight/max8925_bl.c
+++ b/drivers/video/backlight/max8925_bl.c
@@ -188,17 +188,7 @@ static struct platform_driver max8925_backlight_driver = {
 	.remove		= __devexit_p(max8925_backlight_remove),
 };
 
-static int __init max8925_backlight_init(void)
-{
-	return platform_driver_register(&max8925_backlight_driver);
-}
-module_init(max8925_backlight_init);
-
-static void __exit max8925_backlight_exit(void)
-{
-	platform_driver_unregister(&max8925_backlight_driver);
-};
-module_exit(max8925_backlight_exit);
+module_platform_driver(max8925_backlight_driver);
 
 MODULE_DESCRIPTION("Backlight Driver for Maxim MAX8925");
 MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
diff --git a/drivers/video/backlight/omap1_bl.c b/drivers/video/backlight/omap1_bl.c
index 08d26a72394c..d8cde277ec83 100644
--- a/drivers/video/backlight/omap1_bl.c
+++ b/drivers/video/backlight/omap1_bl.c
@@ -195,18 +195,7 @@ static struct platform_driver omapbl_driver = {
 	},
 };
 
-static int __init omapbl_init(void)
-{
-	return platform_driver_register(&omapbl_driver);
-}
-
-static void __exit omapbl_exit(void)
-{
-	platform_driver_unregister(&omapbl_driver);
-}
-
-module_init(omapbl_init);
-module_exit(omapbl_exit);
+module_platform_driver(omapbl_driver);
 
 MODULE_AUTHOR("Andrzej Zaborowski <balrog@zabor.org>");
 MODULE_DESCRIPTION("OMAP LCD Backlight driver");
diff --git a/drivers/video/backlight/pcf50633-backlight.c b/drivers/video/backlight/pcf50633-backlight.c
index ef5628d60563..13e88b71daec 100644
--- a/drivers/video/backlight/pcf50633-backlight.c
+++ b/drivers/video/backlight/pcf50633-backlight.c
@@ -173,17 +173,7 @@ static struct platform_driver pcf50633_bl_driver = {
 	},
 };
 
-static int __init pcf50633_bl_init(void)
-{
-	return platform_driver_register(&pcf50633_bl_driver);
-}
-module_init(pcf50633_bl_init);
-
-static void __exit pcf50633_bl_exit(void)
-{
-	platform_driver_unregister(&pcf50633_bl_driver);
-}
-module_exit(pcf50633_bl_exit);
+module_platform_driver(pcf50633_bl_driver);
 
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
 MODULE_DESCRIPTION("PCF50633 backlight driver");
diff --git a/drivers/video/backlight/platform_lcd.c b/drivers/video/backlight/platform_lcd.c
index 302330acf628..f0bf491ed087 100644
--- a/drivers/video/backlight/platform_lcd.c
+++ b/drivers/video/backlight/platform_lcd.c
@@ -85,7 +85,8 @@ static int __devinit platform_lcd_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	plcd = kzalloc(sizeof(struct platform_lcd), GFP_KERNEL);
+	plcd = devm_kzalloc(&pdev->dev, sizeof(struct platform_lcd),
+			    GFP_KERNEL);
 	if (!plcd) {
 		dev_err(dev, "no memory for state\n");
 		return -ENOMEM;
@@ -98,7 +99,7 @@ static int __devinit platform_lcd_probe(struct platform_device *pdev)
 	if (IS_ERR(plcd->lcd)) {
 		dev_err(dev, "cannot register lcd device\n");
 		err = PTR_ERR(plcd->lcd);
-		goto err_mem;
+		goto err;
 	}
 
 	platform_set_drvdata(pdev, plcd);
@@ -106,8 +107,7 @@ static int __devinit platform_lcd_probe(struct platform_device *pdev)
 
 	return 0;
 
- err_mem:
-	kfree(plcd);
+ err:
 	return err;
 }
 
@@ -116,7 +116,6 @@ 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;
 }
@@ -157,18 +156,7 @@ static struct platform_driver platform_lcd_driver = {
 	.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_platform_driver(platform_lcd_driver);
 
 MODULE_AUTHOR("Ben Dooks <ben-linux@fluff.org>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index 8b5b2a4124c7..7496d04e1d3c 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -169,10 +169,9 @@ static int pwm_backlight_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_PM
-static int pwm_backlight_suspend(struct platform_device *pdev,
-				 pm_message_t state)
+static int pwm_backlight_suspend(struct device *dev)
 {
-	struct backlight_device *bl = platform_get_drvdata(pdev);
+	struct backlight_device *bl = dev_get_drvdata(dev);
 	struct pwm_bl_data *pb = dev_get_drvdata(&bl->dev);
 
 	if (pb->notify)
@@ -184,40 +183,32 @@ static int pwm_backlight_suspend(struct platform_device *pdev,
 	return 0;
 }
 
-static int pwm_backlight_resume(struct platform_device *pdev)
+static int pwm_backlight_resume(struct device *dev)
 {
-	struct backlight_device *bl = platform_get_drvdata(pdev);
+	struct backlight_device *bl = dev_get_drvdata(dev);
 
 	backlight_update_status(bl);
 	return 0;
 }
-#else
-#define pwm_backlight_suspend	NULL
-#define pwm_backlight_resume	NULL
+
+static SIMPLE_DEV_PM_OPS(pwm_backlight_pm_ops, pwm_backlight_suspend,
+			 pwm_backlight_resume);
+
 #endif
 
 static struct platform_driver pwm_backlight_driver = {
 	.driver		= {
 		.name	= "pwm-backlight",
 		.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm	= &pwm_backlight_pm_ops,
+#endif
 	},
 	.probe		= pwm_backlight_probe,
 	.remove		= pwm_backlight_remove,
-	.suspend	= pwm_backlight_suspend,
-	.resume		= pwm_backlight_resume,
 };
 
-static int __init pwm_backlight_init(void)
-{
-	return platform_driver_register(&pwm_backlight_driver);
-}
-module_init(pwm_backlight_init);
-
-static void __exit pwm_backlight_exit(void)
-{
-	platform_driver_unregister(&pwm_backlight_driver);
-}
-module_exit(pwm_backlight_exit);
+module_platform_driver(pwm_backlight_driver);
 
 MODULE_DESCRIPTION("PWM based Backlight Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/backlight/wm831x_bl.c b/drivers/video/backlight/wm831x_bl.c
index fbe9e9316f3b..4e915f5eca99 100644
--- a/drivers/video/backlight/wm831x_bl.c
+++ b/drivers/video/backlight/wm831x_bl.c
@@ -236,17 +236,7 @@ static struct platform_driver wm831x_backlight_driver = {
 	.remove		= wm831x_backlight_remove,
 };
 
-static int __init wm831x_backlight_init(void)
-{
-	return platform_driver_register(&wm831x_backlight_driver);
-}
-module_init(wm831x_backlight_init);
-
-static void __exit wm831x_backlight_exit(void)
-{
-	platform_driver_unregister(&wm831x_backlight_driver);
-}
-module_exit(wm831x_backlight_exit);
+module_platform_driver(wm831x_backlight_driver);
 
 MODULE_DESCRIPTION("Backlight Driver for WM831x PMICs");
 MODULE_AUTHOR("Mark Brown <broonie@opensource.wolfsonmicro.com");
diff --git a/drivers/video/mxsfb.c b/drivers/video/mxsfb.c
index d837d63c456f..eb3c5eea1a0f 100644
--- a/drivers/video/mxsfb.c
+++ b/drivers/video/mxsfb.c
@@ -328,7 +328,7 @@ static void mxsfb_enable_controller(struct fb_info *fb_info)
 
 	dev_dbg(&host->pdev->dev, "%s\n", __func__);
 
-	clk_enable(host->clk);
+	clk_prepare_enable(host->clk);
 	clk_set_rate(host->clk, PICOS2KHZ(fb_info->var.pixclock) * 1000U);
 
 	/* if it was disabled, re-enable the mode again */
@@ -368,7 +368,7 @@ static void mxsfb_disable_controller(struct fb_info *fb_info)
 
 	writel(VDCTRL4_SYNC_SIGNALS_ON, host->base + LCDC_VDCTRL4 + REG_CLR);
 
-	clk_disable(host->clk);
+	clk_disable_unprepare(host->clk);
 
 	host->enabled = 0;
 }
@@ -668,7 +668,7 @@ static int __devinit mxsfb_restore_mode(struct mxsfb_info *host)
 	line_count = fb_info->fix.smem_len / fb_info->fix.line_length;
 	fb_info->fix.ypanstep = 1;
 
-	clk_enable(host->clk);
+	clk_prepare_enable(host->clk);
 	host->enabled = 1;
 
 	return 0;
@@ -841,7 +841,7 @@ static int __devinit mxsfb_probe(struct platform_device *pdev)
 
 error_register:
 	if (host->enabled)
-		clk_disable(host->clk);
+		clk_disable_unprepare(host->clk);
 	fb_destroy_modelist(&fb_info->modelist);
 error_init_fb:
 	kfree(fb_info->pseudo_palette);
diff --git a/drivers/video/omap2/omapfb/Kconfig b/drivers/video/omap2/omapfb/Kconfig
index 83d3fe7ec9ae..4ea17dc3258c 100644
--- a/drivers/video/omap2/omapfb/Kconfig
+++ b/drivers/video/omap2/omapfb/Kconfig
@@ -1,6 +1,6 @@
 menuconfig FB_OMAP2
         tristate "OMAP2+ frame buffer support"
-        depends on FB && OMAP2_DSS
+        depends on FB && OMAP2_DSS && !DRM_OMAP
 
 	select OMAP2_VRAM
 	select OMAP2_VRFB if ARCH_OMAP2 || ARCH_OMAP3
diff --git a/drivers/video/xen-fbfront.c b/drivers/video/xen-fbfront.c
index beac52fc1c0e..cb4529c40d74 100644
--- a/drivers/video/xen-fbfront.c
+++ b/drivers/video/xen-fbfront.c
@@ -671,20 +671,17 @@ InitWait:
 	}
 }
 
-static struct xenbus_device_id xenfb_ids[] = {
+static const struct xenbus_device_id xenfb_ids[] = {
 	{ "vfb" },
 	{ "" }
 };
 
-static struct xenbus_driver xenfb_driver = {
-	.name = "vfb",
-	.owner = THIS_MODULE,
-	.ids = xenfb_ids,
+static DEFINE_XENBUS_DRIVER(xenfb, ,
 	.probe = xenfb_probe,
 	.remove = xenfb_remove,
 	.resume = xenfb_resume,
 	.otherend_changed = xenfb_backend_changed,
-};
+);
 
 static int __init xenfb_init(void)
 {
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 79fd606b7cd5..877b107f77a7 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -772,6 +772,19 @@ config SMSC37B787_WDT
 
 	  Most people will say N.
 
+config VIA_WDT
+	tristate "VIA Watchdog Timer"
+	depends on X86
+	select WATCHDOG_CORE
+	---help---
+	This is the driver for the hardware watchdog timer on VIA
+	southbridge chipset CX700, VX800/VX820 or VX855/VX875.
+
+	To compile this driver as a module, choose M here; the module
+	will be called via_wdt.
+
+	Most people will say N.
+
 config W83627HF_WDT
 	tristate "W83627HF/W83627DHG Watchdog Timer"
 	depends on X86
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index fe893e91935b..e8f479a16402 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -99,6 +99,7 @@ obj-$(CONFIG_SBC7240_WDT) += sbc7240_wdt.o
 obj-$(CONFIG_CPU5_WDT) += cpu5wdt.o
 obj-$(CONFIG_SMSC_SCH311X_WDT) += sch311x_wdt.o
 obj-$(CONFIG_SMSC37B787_WDT) += smsc37b787_wdt.o
+obj-$(CONFIG_VIA_WDT) += via_wdt.o
 obj-$(CONFIG_W83627HF_WDT) += w83627hf_wdt.o
 obj-$(CONFIG_W83697HF_WDT) += w83697hf_wdt.o
 obj-$(CONFIG_W83697UG_WDT) += w83697ug_wdt.o
diff --git a/drivers/watchdog/ar7_wdt.c b/drivers/watchdog/ar7_wdt.c
index b29221783598..502773ad5acd 100644
--- a/drivers/watchdog/ar7_wdt.c
+++ b/drivers/watchdog/ar7_wdt.c
@@ -70,8 +70,8 @@ struct ar7_wdt {
 };
 
 static unsigned long wdt_is_open;
-static spinlock_t wdt_lock;
 static unsigned expect_close;
+static DEFINE_SPINLOCK(wdt_lock);
 
 /* XXX currently fixed, allows max margin ~68.72 secs */
 #define prescale_value 0xffff
@@ -280,8 +280,6 @@ static int __devinit ar7_wdt_probe(struct platform_device *pdev)
 {
 	int rc;
 
-	spin_lock_init(&wdt_lock);
-
 	ar7_regs_wdt =
 		platform_get_resource_byname(pdev, IORESOURCE_MEM, "regs");
 	if (!ar7_regs_wdt) {
@@ -355,15 +353,4 @@ static struct platform_driver ar7_wdt_driver = {
 	},
 };
 
-static int __init ar7_wdt_init(void)
-{
-	return platform_driver_register(&ar7_wdt_driver);
-}
-
-static void __exit ar7_wdt_cleanup(void)
-{
-	platform_driver_unregister(&ar7_wdt_driver);
-}
-
-module_init(ar7_wdt_init);
-module_exit(ar7_wdt_cleanup);
+module_platform_driver(ar7_wdt_driver);
diff --git a/drivers/watchdog/at91sam9_wdt.c b/drivers/watchdog/at91sam9_wdt.c
index 87445b2d72a7..00562566ef5f 100644
--- a/drivers/watchdog/at91sam9_wdt.c
+++ b/drivers/watchdog/at91sam9_wdt.c
@@ -35,6 +35,11 @@
 
 #define DRV_NAME "AT91SAM9 Watchdog"
 
+#define wdt_read(field) \
+	__raw_readl(at91wdt_private.base + field)
+#define wdt_write(field, val) \
+	__raw_writel((val), at91wdt_private.base + field)
+
 /* AT91SAM9 watchdog runs a 12bit counter @ 256Hz,
  * use this to convert a watchdog
  * value from/to milliseconds.
@@ -63,6 +68,7 @@ MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started "
 static void at91_ping(unsigned long data);
 
 static struct {
+	void __iomem *base;
 	unsigned long next_heartbeat;	/* the next_heartbeat for the timer */
 	unsigned long open;
 	char expect_close;
@@ -77,7 +83,7 @@ static struct {
  */
 static inline void at91_wdt_reset(void)
 {
-	at91_sys_write(AT91_WDT_CR, AT91_WDT_KEY | AT91_WDT_WDRSTT);
+	wdt_write(AT91_WDT_CR, AT91_WDT_KEY | AT91_WDT_WDRSTT);
 }
 
 /*
@@ -132,7 +138,7 @@ static int at91_wdt_settimeout(unsigned int timeout)
 	unsigned int mr;
 
 	/* Check if disabled */
-	mr = at91_sys_read(AT91_WDT_MR);
+	mr = wdt_read(AT91_WDT_MR);
 	if (mr & AT91_WDT_WDDIS) {
 		printk(KERN_ERR DRV_NAME": sorry, watchdog is disabled\n");
 		return -EIO;
@@ -149,7 +155,7 @@ static int at91_wdt_settimeout(unsigned int timeout)
 		| AT91_WDT_WDDBGHLT	/* disabled in debug mode */
 		| AT91_WDT_WDD		/* restart at any time */
 		| (timeout & AT91_WDT_WDV);  /* timer value */
-	at91_sys_write(AT91_WDT_MR, reg);
+	wdt_write(AT91_WDT_MR, reg);
 
 	return 0;
 }
@@ -248,12 +254,22 @@ static struct miscdevice at91wdt_miscdev = {
 
 static int __init at91wdt_probe(struct platform_device *pdev)
 {
+	struct resource	*r;
 	int res;
 
 	if (at91wdt_miscdev.parent)
 		return -EBUSY;
 	at91wdt_miscdev.parent = &pdev->dev;
 
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r)
+		return -ENODEV;
+	at91wdt_private.base = ioremap(r->start, resource_size(r));
+	if (!at91wdt_private.base) {
+		dev_err(&pdev->dev, "failed to map registers, aborting.\n");
+		return -ENOMEM;
+	}
+
 	/* Set watchdog */
 	res = at91_wdt_settimeout(ms_to_ticks(WDT_HW_TIMEOUT * 1000));
 	if (res)
diff --git a/drivers/watchdog/at91sam9_wdt.h b/drivers/watchdog/at91sam9_wdt.h
index 757f9cab5c82..c6fbb2e6c41b 100644
--- a/drivers/watchdog/at91sam9_wdt.h
+++ b/drivers/watchdog/at91sam9_wdt.h
@@ -16,11 +16,11 @@
 #ifndef AT91_WDT_H
 #define AT91_WDT_H
 
-#define AT91_WDT_CR		(AT91_WDT + 0x00)	/* Watchdog Control Register */
+#define AT91_WDT_CR		0x00			/* Watchdog Control Register */
 #define		AT91_WDT_WDRSTT		(1    << 0)		/* Restart */
 #define		AT91_WDT_KEY		(0xa5 << 24)		/* KEY Password */
 
-#define AT91_WDT_MR		(AT91_WDT + 0x04)	/* Watchdog Mode Register */
+#define AT91_WDT_MR		0x04			/* Watchdog Mode Register */
 #define		AT91_WDT_WDV		(0xfff << 0)		/* Counter Value */
 #define		AT91_WDT_WDFIEN		(1     << 12)		/* Fault Interrupt Enable */
 #define		AT91_WDT_WDRSTEN	(1     << 13)		/* Reset Processor */
@@ -30,7 +30,7 @@
 #define		AT91_WDT_WDDBGHLT	(1     << 28)		/* Debug Halt */
 #define		AT91_WDT_WDIDLEHLT	(1     << 29)		/* Idle Halt */
 
-#define AT91_WDT_SR		(AT91_WDT + 0x08)	/* Watchdog Status Register */
+#define AT91_WDT_SR		0x08			/* Watchdog Status Register */
 #define		AT91_WDT_WDUNF		(1 << 0)		/* Watchdog Underflow */
 #define		AT91_WDT_WDERR		(1 << 1)		/* Watchdog Error */
 
diff --git a/drivers/watchdog/ath79_wdt.c b/drivers/watchdog/ath79_wdt.c
index 725c84bfdd76..9db808349f8b 100644
--- a/drivers/watchdog/ath79_wdt.c
+++ b/drivers/watchdog/ath79_wdt.c
@@ -68,17 +68,23 @@ static int max_timeout;
 static inline void ath79_wdt_keepalive(void)
 {
 	ath79_reset_wr(AR71XX_RESET_REG_WDOG, wdt_freq * timeout);
+	/* flush write */
+	ath79_reset_rr(AR71XX_RESET_REG_WDOG);
 }
 
 static inline void ath79_wdt_enable(void)
 {
 	ath79_wdt_keepalive();
 	ath79_reset_wr(AR71XX_RESET_REG_WDOG_CTRL, WDOG_CTRL_ACTION_FCR);
+	/* flush write */
+	ath79_reset_rr(AR71XX_RESET_REG_WDOG_CTRL);
 }
 
 static inline void ath79_wdt_disable(void)
 {
 	ath79_reset_wr(AR71XX_RESET_REG_WDOG_CTRL, WDOG_CTRL_ACTION_NONE);
+	/* flush write */
+	ath79_reset_rr(AR71XX_RESET_REG_WDOG_CTRL);
 }
 
 static int ath79_wdt_set_timeout(int val)
diff --git a/drivers/watchdog/bcm63xx_wdt.c b/drivers/watchdog/bcm63xx_wdt.c
index 5064e8317521..8dc7de641e26 100644
--- a/drivers/watchdog/bcm63xx_wdt.c
+++ b/drivers/watchdog/bcm63xx_wdt.c
@@ -311,18 +311,7 @@ static struct platform_driver bcm63xx_wdt = {
 	}
 };
 
-static int __init bcm63xx_wdt_init(void)
-{
-	return platform_driver_register(&bcm63xx_wdt);
-}
-
-static void __exit bcm63xx_wdt_exit(void)
-{
-	platform_driver_unregister(&bcm63xx_wdt);
-}
-
-module_init(bcm63xx_wdt_init);
-module_exit(bcm63xx_wdt_exit);
+module_platform_driver(bcm63xx_wdt);
 
 MODULE_AUTHOR("Miguel Gaio <miguel.gaio@efixo.com>");
 MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
diff --git a/drivers/watchdog/cpu5wdt.c b/drivers/watchdog/cpu5wdt.c
index edd3475f41db..251c863d71dd 100644
--- a/drivers/watchdog/cpu5wdt.c
+++ b/drivers/watchdog/cpu5wdt.c
@@ -39,7 +39,7 @@
 static int verbose;
 static int port = 0x91;
 static int ticks = 10000;
-static spinlock_t cpu5wdt_lock;
+static DEFINE_SPINLOCK(cpu5wdt_lock);
 
 #define PFX			"cpu5wdt: "
 
@@ -223,7 +223,6 @@ static int __devinit cpu5wdt_init(void)
 				"port=0x%x, verbose=%i\n", port, verbose);
 
 	init_completion(&cpu5wdt_device.stop);
-	spin_lock_init(&cpu5wdt_lock);
 	cpu5wdt_device.queue = 0;
 	setup_timer(&cpu5wdt_device.timer, cpu5wdt_trigger, 0);
 	cpu5wdt_device.default_ticks = ticks;
diff --git a/drivers/watchdog/cpwd.c b/drivers/watchdog/cpwd.c
index 1e013e8457b7..1b793dfd868f 100644
--- a/drivers/watchdog/cpwd.c
+++ b/drivers/watchdog/cpwd.c
@@ -687,15 +687,4 @@ static struct platform_driver cpwd_driver = {
 	.remove		= __devexit_p(cpwd_remove),
 };
 
-static int __init cpwd_init(void)
-{
-	return platform_driver_register(&cpwd_driver);
-}
-
-static void __exit cpwd_exit(void)
-{
-	platform_driver_unregister(&cpwd_driver);
-}
-
-module_init(cpwd_init);
-module_exit(cpwd_exit);
+module_platform_driver(cpwd_driver);
diff --git a/drivers/watchdog/davinci_wdt.c b/drivers/watchdog/davinci_wdt.c
index 51b5551b4e3f..c8c5c8032bcb 100644
--- a/drivers/watchdog/davinci_wdt.c
+++ b/drivers/watchdog/davinci_wdt.c
@@ -271,18 +271,7 @@ static struct platform_driver platform_wdt_driver = {
 	.remove = __devexit_p(davinci_wdt_remove),
 };
 
-static int __init davinci_wdt_init(void)
-{
-	return platform_driver_register(&platform_wdt_driver);
-}
-
-static void __exit davinci_wdt_exit(void)
-{
-	platform_driver_unregister(&platform_wdt_driver);
-}
-
-module_init(davinci_wdt_init);
-module_exit(davinci_wdt_exit);
+module_platform_driver(platform_wdt_driver);
 
 MODULE_AUTHOR("Texas Instruments");
 MODULE_DESCRIPTION("DaVinci Watchdog Driver");
diff --git a/drivers/watchdog/dw_wdt.c b/drivers/watchdog/dw_wdt.c
index f10f8c0abba4..1b0e3dd81c1a 100644
--- a/drivers/watchdog/dw_wdt.c
+++ b/drivers/watchdog/dw_wdt.c
@@ -358,17 +358,7 @@ static struct platform_driver dw_wdt_driver = {
 	},
 };
 
-static int __init dw_wdt_watchdog_init(void)
-{
-	return platform_driver_register(&dw_wdt_driver);
-}
-module_init(dw_wdt_watchdog_init);
-
-static void __exit dw_wdt_watchdog_exit(void)
-{
-	platform_driver_unregister(&dw_wdt_driver);
-}
-module_exit(dw_wdt_watchdog_exit);
+module_platform_driver(dw_wdt_driver);
 
 MODULE_AUTHOR("Jamie Iles");
 MODULE_DESCRIPTION("Synopsys DesignWare Watchdog Driver");
diff --git a/drivers/watchdog/eurotechwdt.c b/drivers/watchdog/eurotechwdt.c
index 41018d429abb..3946c51099c0 100644
--- a/drivers/watchdog/eurotechwdt.c
+++ b/drivers/watchdog/eurotechwdt.c
@@ -64,7 +64,7 @@
 static unsigned long eurwdt_is_open;
 static int eurwdt_timeout;
 static char eur_expect_close;
-static spinlock_t eurwdt_lock;
+static DEFINE_SPINLOCK(eurwdt_lock);
 
 /*
  * You must set these - there is no sane way to probe for this board.
@@ -446,8 +446,6 @@ static int __init eurwdt_init(void)
 		goto outreg;
 	}
 
-	spin_lock_init(&eurwdt_lock);
-
 	ret = misc_register(&eurwdt_miscdev);
 	if (ret) {
 		printk(KERN_ERR "eurwdt: can't misc_register on minor=%d\n",
diff --git a/drivers/watchdog/ibmasr.c b/drivers/watchdog/ibmasr.c
index 195e0f798e76..c7481ad51629 100644
--- a/drivers/watchdog/ibmasr.c
+++ b/drivers/watchdog/ibmasr.c
@@ -68,7 +68,7 @@ static char asr_expect_close;
 static unsigned int asr_type, asr_base, asr_length;
 static unsigned int asr_read_addr, asr_write_addr;
 static unsigned char asr_toggle_mask, asr_disable_mask;
-static spinlock_t asr_lock;
+static DEFINE_SPINLOCK(asr_lock);
 
 static void __asr_toggle(void)
 {
@@ -386,8 +386,6 @@ static int __init ibmasr_init(void)
 	if (!asr_type)
 		return -ENODEV;
 
-	spin_lock_init(&asr_lock);
-
 	rc = asr_get_base_address();
 	if (rc)
 		return rc;
diff --git a/drivers/watchdog/indydog.c b/drivers/watchdog/indydog.c
index 1cc5609666d1..1475e09f9af2 100644
--- a/drivers/watchdog/indydog.c
+++ b/drivers/watchdog/indydog.c
@@ -28,7 +28,7 @@
 
 #define PFX "indydog: "
 static unsigned long indydog_alive;
-static spinlock_t indydog_lock;
+static DEFINE_SPINLOCK(indydog_lock);
 
 #define WATCHDOG_TIMEOUT 30		/* 30 sec default timeout */
 
@@ -185,8 +185,6 @@ static int __init watchdog_init(void)
 {
 	int ret;
 
-	spin_lock_init(&indydog_lock);
-
 	ret = register_reboot_notifier(&indydog_notifier);
 	if (ret) {
 		printk(KERN_ERR PFX
diff --git a/drivers/watchdog/iop_wdt.c b/drivers/watchdog/iop_wdt.c
index aef94789019f..82fa7a92a8d2 100644
--- a/drivers/watchdog/iop_wdt.c
+++ b/drivers/watchdog/iop_wdt.c
@@ -37,7 +37,7 @@
 static int nowayout = WATCHDOG_NOWAYOUT;
 static unsigned long wdt_status;
 static unsigned long boot_status;
-static spinlock_t wdt_lock;
+static DEFINE_SPINLOCK(wdt_lock);
 
 #define WDT_IN_USE		0
 #define WDT_OK_TO_CLOSE		1
@@ -226,9 +226,6 @@ static int __init iop_wdt_init(void)
 {
 	int ret;
 
-	spin_lock_init(&wdt_lock);
-
-
 	/* check if the reset was caused by the watchdog timer */
 	boot_status = (read_rcsr() & IOP_RCSR_WDT) ? WDIOF_CARDRESET : 0;
 
diff --git a/drivers/watchdog/ixp2000_wdt.c b/drivers/watchdog/ixp2000_wdt.c
index e86952a7168c..084f71aa855a 100644
--- a/drivers/watchdog/ixp2000_wdt.c
+++ b/drivers/watchdog/ixp2000_wdt.c
@@ -32,7 +32,7 @@
 static int nowayout = WATCHDOG_NOWAYOUT;
 static unsigned int heartbeat = 60;	/* (secs) Default is 1 minute */
 static unsigned long wdt_status;
-static spinlock_t wdt_lock;
+static DEFINE_SPINLOCK(wdt_lock);
 
 #define	WDT_IN_USE		0
 #define	WDT_OK_TO_CLOSE		1
@@ -189,7 +189,6 @@ static int __init ixp2000_wdt_init(void)
 		return -EIO;
 	}
 	wdt_tick_rate = (*IXP2000_T1_CLD * HZ) / 256;
-	spin_lock_init(&wdt_lock);
 	return misc_register(&ixp2000_wdt_miscdev);
 }
 
diff --git a/drivers/watchdog/ixp4xx_wdt.c b/drivers/watchdog/ixp4xx_wdt.c
index e02c0ecda26b..4fc2e9ac26f7 100644
--- a/drivers/watchdog/ixp4xx_wdt.c
+++ b/drivers/watchdog/ixp4xx_wdt.c
@@ -181,7 +181,6 @@ static int __init ixp4xx_wdt_init(void)
 
 		return -ENODEV;
 	}
-	spin_lock_init(&wdt_lock);
 	boot_status = (*IXP4XX_OSST & IXP4XX_OSST_TIMER_WARM_RESET) ?
 			WDIOF_CARDRESET : 0;
 	ret = misc_register(&ixp4xx_wdt_miscdev);
diff --git a/drivers/watchdog/jz4740_wdt.c b/drivers/watchdog/jz4740_wdt.c
index 684ba01fb540..17ef300bccc5 100644
--- a/drivers/watchdog/jz4740_wdt.c
+++ b/drivers/watchdog/jz4740_wdt.c
@@ -295,18 +295,7 @@ static struct platform_driver jz4740_wdt_driver = {
 	},
 };
 
-
-static int __init jz4740_wdt_init(void)
-{
-	return platform_driver_register(&jz4740_wdt_driver);
-}
-module_init(jz4740_wdt_init);
-
-static void __exit jz4740_wdt_exit(void)
-{
-	platform_driver_unregister(&jz4740_wdt_driver);
-}
-module_exit(jz4740_wdt_exit);
+module_platform_driver(jz4740_wdt_driver);
 
 MODULE_AUTHOR("Paul Cercueil <paul@crapouillou.net>");
 MODULE_DESCRIPTION("jz4740 Watchdog Driver");
diff --git a/drivers/watchdog/ks8695_wdt.c b/drivers/watchdog/ks8695_wdt.c
index 811471903e8a..51757a520e8f 100644
--- a/drivers/watchdog/ks8695_wdt.c
+++ b/drivers/watchdog/ks8695_wdt.c
@@ -42,7 +42,7 @@ MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
 
 
 static unsigned long ks8695wdt_busy;
-static spinlock_t ks8695_lock;
+static DEFINE_SPINLOCK(ks8695_lock);
 
 /* ......................................................................... */
 
@@ -288,7 +288,6 @@ static struct platform_driver ks8695wdt_driver = {
 
 static int __init ks8695_wdt_init(void)
 {
-	spin_lock_init(&ks8695_lock);
 	/* Check that the heartbeat value is within range;
 	   if not reset to the default */
 	if (ks8695_wdt_settimeout(wdt_time)) {
diff --git a/drivers/watchdog/lantiq_wdt.c b/drivers/watchdog/lantiq_wdt.c
index 102aed0efbf1..d3a63be2e28d 100644
--- a/drivers/watchdog/lantiq_wdt.c
+++ b/drivers/watchdog/lantiq_wdt.c
@@ -222,9 +222,6 @@ ltq_wdt_remove(struct platform_device *pdev)
 {
 	misc_deregister(&ltq_wdt_miscdev);
 
-	if (ltq_wdt_membase)
-		iounmap(ltq_wdt_membase);
-
 	return 0;
 }
 
diff --git a/drivers/watchdog/max63xx_wdt.c b/drivers/watchdog/max63xx_wdt.c
index 73ba2fd8e591..af63ecfbfa6f 100644
--- a/drivers/watchdog/max63xx_wdt.c
+++ b/drivers/watchdog/max63xx_wdt.c
@@ -364,18 +364,7 @@ static struct platform_driver max63xx_wdt_driver = {
 	},
 };
 
-static int __init max63xx_wdt_init(void)
-{
-	return platform_driver_register(&max63xx_wdt_driver);
-}
-
-static void __exit max63xx_wdt_exit(void)
-{
-	platform_driver_unregister(&max63xx_wdt_driver);
-}
-
-module_init(max63xx_wdt_init);
-module_exit(max63xx_wdt_exit);
+module_platform_driver(max63xx_wdt_driver);
 
 MODULE_AUTHOR("Marc Zyngier <maz@misterjones.org>");
 MODULE_DESCRIPTION("max63xx Watchdog Driver");
diff --git a/drivers/watchdog/mtx-1_wdt.c b/drivers/watchdog/mtx-1_wdt.c
index ac37bb82392c..c29e31d99fe8 100644
--- a/drivers/watchdog/mtx-1_wdt.c
+++ b/drivers/watchdog/mtx-1_wdt.c
@@ -253,18 +253,7 @@ static struct platform_driver mtx1_wdt_driver = {
 	.driver.owner = THIS_MODULE,
 };
 
-static int __init mtx1_wdt_init(void)
-{
-	return platform_driver_register(&mtx1_wdt_driver);
-}
-
-static void __exit mtx1_wdt_exit(void)
-{
-	platform_driver_unregister(&mtx1_wdt_driver);
-}
-
-module_init(mtx1_wdt_init);
-module_exit(mtx1_wdt_exit);
+module_platform_driver(mtx1_wdt_driver);
 
 MODULE_AUTHOR("Michael Stickel, Florian Fainelli");
 MODULE_DESCRIPTION("Driver for the MTX-1 watchdog");
diff --git a/drivers/watchdog/nuc900_wdt.c b/drivers/watchdog/nuc900_wdt.c
index 6cee33d4b161..50359bad9177 100644
--- a/drivers/watchdog/nuc900_wdt.c
+++ b/drivers/watchdog/nuc900_wdt.c
@@ -334,18 +334,7 @@ static struct platform_driver nuc900wdt_driver = {
 	},
 };
 
-static int __init nuc900_wdt_init(void)
-{
-	return platform_driver_register(&nuc900wdt_driver);
-}
-
-static void __exit nuc900_wdt_exit(void)
-{
-	platform_driver_unregister(&nuc900wdt_driver);
-}
-
-module_init(nuc900_wdt_init);
-module_exit(nuc900_wdt_exit);
+module_platform_driver(nuc900wdt_driver);
 
 MODULE_AUTHOR("Wan ZongShun <mcuos.com@gmail.com>");
 MODULE_DESCRIPTION("Watchdog driver for NUC900");
diff --git a/drivers/watchdog/of_xilinx_wdt.c b/drivers/watchdog/of_xilinx_wdt.c
index 4ec741ac952c..f359ab85c3bc 100644
--- a/drivers/watchdog/of_xilinx_wdt.c
+++ b/drivers/watchdog/of_xilinx_wdt.c
@@ -414,18 +414,7 @@ static struct platform_driver xwdt_driver = {
 	},
 };
 
-static int __init xwdt_init(void)
-{
-	return platform_driver_register(&xwdt_driver);
-}
-
-static void __exit xwdt_exit(void)
-{
-	platform_driver_unregister(&xwdt_driver);
-}
-
-module_init(xwdt_init);
-module_exit(xwdt_exit);
+module_platform_driver(xwdt_driver);
 
 MODULE_AUTHOR("Alejandro Cabrera <aldaya@gmail.com>");
 MODULE_DESCRIPTION("Xilinx Watchdog driver");
diff --git a/drivers/watchdog/omap_wdt.c b/drivers/watchdog/omap_wdt.c
index 2b4acb86c191..4b33e3fd726b 100644
--- a/drivers/watchdog/omap_wdt.c
+++ b/drivers/watchdog/omap_wdt.c
@@ -55,7 +55,7 @@ module_param(timer_margin, uint, 0);
 MODULE_PARM_DESC(timer_margin, "initial watchdog timeout (in seconds)");
 
 static unsigned int wdt_trgr_pattern = 0x1234;
-static spinlock_t wdt_lock;
+static DEFINE_SPINLOCK(wdt_lock);
 
 struct omap_wdt_dev {
 	void __iomem    *base;          /* physical */
@@ -232,6 +232,7 @@ static long omap_wdt_ioctl(struct file *file, unsigned int cmd,
 		if (cpu_is_omap24xx())
 			return put_user(omap_prcm_get_reset_sources(),
 					(int __user *)arg);
+		return put_user(0, (int __user *)arg);
 	case WDIOC_KEEPALIVE:
 		pm_runtime_get_sync(wdev->dev);
 		spin_lock(&wdt_lock);
@@ -437,19 +438,7 @@ static struct platform_driver omap_wdt_driver = {
 	},
 };
 
-static int __init omap_wdt_init(void)
-{
-	spin_lock_init(&wdt_lock);
-	return platform_driver_register(&omap_wdt_driver);
-}
-
-static void __exit omap_wdt_exit(void)
-{
-	platform_driver_unregister(&omap_wdt_driver);
-}
-
-module_init(omap_wdt_init);
-module_exit(omap_wdt_exit);
+module_platform_driver(omap_wdt_driver);
 
 MODULE_AUTHOR("George G. Davis");
 MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/orion_wdt.c b/drivers/watchdog/orion_wdt.c
index 2d9fb96a9ee9..4ad78f868515 100644
--- a/drivers/watchdog/orion_wdt.c
+++ b/drivers/watchdog/orion_wdt.c
@@ -41,7 +41,7 @@ static int heartbeat = -1;		/* module parameter (seconds) */
 static unsigned int wdt_max_duration;	/* (seconds) */
 static unsigned int wdt_tclk;
 static unsigned long wdt_status;
-static spinlock_t wdt_lock;
+static DEFINE_SPINLOCK(wdt_lock);
 
 static void orion_wdt_ping(void)
 {
@@ -294,19 +294,7 @@ static struct platform_driver orion_wdt_driver = {
 	},
 };
 
-static int __init orion_wdt_init(void)
-{
-	spin_lock_init(&wdt_lock);
-	return platform_driver_register(&orion_wdt_driver);
-}
-
-static void __exit orion_wdt_exit(void)
-{
-	platform_driver_unregister(&orion_wdt_driver);
-}
-
-module_init(orion_wdt_init);
-module_exit(orion_wdt_exit);
+module_platform_driver(orion_wdt_driver);
 
 MODULE_AUTHOR("Sylver Bruneau <sylver.bruneau@googlemail.com>");
 MODULE_DESCRIPTION("Orion Processor Watchdog");
diff --git a/drivers/watchdog/pnx4008_wdt.c b/drivers/watchdog/pnx4008_wdt.c
index 614933225560..bd143c9dd3e6 100644
--- a/drivers/watchdog/pnx4008_wdt.c
+++ b/drivers/watchdog/pnx4008_wdt.c
@@ -334,18 +334,7 @@ static struct platform_driver platform_wdt_driver = {
 	.remove = __devexit_p(pnx4008_wdt_remove),
 };
 
-static int __init pnx4008_wdt_init(void)
-{
-	return platform_driver_register(&platform_wdt_driver);
-}
-
-static void __exit pnx4008_wdt_exit(void)
-{
-	platform_driver_unregister(&platform_wdt_driver);
-}
-
-module_init(pnx4008_wdt_init);
-module_exit(pnx4008_wdt_exit);
+module_platform_driver(platform_wdt_driver);
 
 MODULE_AUTHOR("MontaVista Software, Inc. <source@mvista.com>");
 MODULE_DESCRIPTION("PNX4008 Watchdog Driver");
diff --git a/drivers/watchdog/rc32434_wdt.c b/drivers/watchdog/rc32434_wdt.c
index d4c29b5311a4..bf7bc8aa1c01 100644
--- a/drivers/watchdog/rc32434_wdt.c
+++ b/drivers/watchdog/rc32434_wdt.c
@@ -332,18 +332,7 @@ static struct platform_driver rc32434_wdt_driver = {
 	}
 };
 
-static int __init rc32434_wdt_init(void)
-{
-	return platform_driver_register(&rc32434_wdt_driver);
-}
-
-static void __exit rc32434_wdt_exit(void)
-{
-	platform_driver_unregister(&rc32434_wdt_driver);
-}
-
-module_init(rc32434_wdt_init);
-module_exit(rc32434_wdt_exit);
+module_platform_driver(rc32434_wdt_driver);
 
 MODULE_AUTHOR("Ondrej Zajicek <santiago@crfreenet.org>,"
 		"Florian Fainelli <florian@openwrt.org>");
diff --git a/drivers/watchdog/rdc321x_wdt.c b/drivers/watchdog/rdc321x_wdt.c
index 428f8a1583e8..042ccc56ae26 100644
--- a/drivers/watchdog/rdc321x_wdt.c
+++ b/drivers/watchdog/rdc321x_wdt.c
@@ -293,18 +293,7 @@ static struct platform_driver rdc321x_wdt_driver = {
 	},
 };
 
-static int __init rdc321x_wdt_init(void)
-{
-	return platform_driver_register(&rdc321x_wdt_driver);
-}
-
-static void __exit rdc321x_wdt_exit(void)
-{
-	platform_driver_unregister(&rdc321x_wdt_driver);
-}
-
-module_init(rdc321x_wdt_init);
-module_exit(rdc321x_wdt_exit);
+module_platform_driver(rdc321x_wdt_driver);
 
 MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
 MODULE_DESCRIPTION("RDC321x watchdog driver");
diff --git a/drivers/watchdog/riowd.c b/drivers/watchdog/riowd.c
index 109b533896b7..c7e17ceafa6a 100644
--- a/drivers/watchdog/riowd.c
+++ b/drivers/watchdog/riowd.c
@@ -247,15 +247,4 @@ static struct platform_driver riowd_driver = {
 	.remove		= __devexit_p(riowd_remove),
 };
 
-static int __init riowd_init(void)
-{
-	return platform_driver_register(&riowd_driver);
-}
-
-static void __exit riowd_exit(void)
-{
-	platform_driver_unregister(&riowd_driver);
-}
-
-module_init(riowd_init);
-module_exit(riowd_exit);
+module_platform_driver(riowd_driver);
diff --git a/drivers/watchdog/s3c2410_wdt.c b/drivers/watchdog/s3c2410_wdt.c
index a79e3840782a..4bc3744e14e4 100644
--- a/drivers/watchdog/s3c2410_wdt.c
+++ b/drivers/watchdog/s3c2410_wdt.c
@@ -378,6 +378,8 @@ static int __devinit s3c2410wdt_probe(struct platform_device *pdev)
 							"cannot start\n");
 	}
 
+	watchdog_set_nowayout(&s3c2410_wdd, nowayout);
+
 	ret = watchdog_register_device(&s3c2410_wdd);
 	if (ret) {
 		dev_err(dev, "cannot register watchdog (%d)\n", ret);
diff --git a/drivers/watchdog/stmp3xxx_wdt.c b/drivers/watchdog/stmp3xxx_wdt.c
index ac2346a452e5..4c2a4e8698f9 100644
--- a/drivers/watchdog/stmp3xxx_wdt.c
+++ b/drivers/watchdog/stmp3xxx_wdt.c
@@ -272,18 +272,7 @@ static struct platform_driver platform_wdt_driver = {
 	.resume = stmp3xxx_wdt_resume,
 };
 
-static int __init stmp3xxx_wdt_init(void)
-{
-	return platform_driver_register(&platform_wdt_driver);
-}
-
-static void __exit stmp3xxx_wdt_exit(void)
-{
-	return platform_driver_unregister(&platform_wdt_driver);
-}
-
-module_init(stmp3xxx_wdt_init);
-module_exit(stmp3xxx_wdt_exit);
+module_platform_driver(platform_wdt_driver);
 
 MODULE_DESCRIPTION("STMP3XXX Watchdog Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/ts72xx_wdt.c b/drivers/watchdog/ts72xx_wdt.c
index 5a90a4a871dd..1490293dc7da 100644
--- a/drivers/watchdog/ts72xx_wdt.c
+++ b/drivers/watchdog/ts72xx_wdt.c
@@ -506,17 +506,7 @@ static struct platform_driver ts72xx_wdt_driver = {
 	},
 };
 
-static __init int ts72xx_wdt_init(void)
-{
-	return platform_driver_register(&ts72xx_wdt_driver);
-}
-module_init(ts72xx_wdt_init);
-
-static __exit void ts72xx_wdt_exit(void)
-{
-	platform_driver_unregister(&ts72xx_wdt_driver);
-}
-module_exit(ts72xx_wdt_exit);
+module_platform_driver(ts72xx_wdt_driver);
 
 MODULE_AUTHOR("Mika Westerberg <mika.westerberg@iki.fi>");
 MODULE_DESCRIPTION("TS-72xx SBC Watchdog");
diff --git a/drivers/watchdog/twl4030_wdt.c b/drivers/watchdog/twl4030_wdt.c
index b5045ca7e61c..0764c6239b98 100644
--- a/drivers/watchdog/twl4030_wdt.c
+++ b/drivers/watchdog/twl4030_wdt.c
@@ -256,17 +256,7 @@ static struct platform_driver twl4030_wdt_driver = {
 	},
 };
 
-static int __devinit twl4030_wdt_init(void)
-{
-	return platform_driver_register(&twl4030_wdt_driver);
-}
-module_init(twl4030_wdt_init);
-
-static void __devexit twl4030_wdt_exit(void)
-{
-	platform_driver_unregister(&twl4030_wdt_driver);
-}
-module_exit(twl4030_wdt_exit);
+module_platform_driver(twl4030_wdt_driver);
 
 MODULE_AUTHOR("Nokia Corporation");
 MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/via_wdt.c b/drivers/watchdog/via_wdt.c
new file mode 100644
index 000000000000..026b4bbfa0aa
--- /dev/null
+++ b/drivers/watchdog/via_wdt.c
@@ -0,0 +1,267 @@
+/*
+ * VIA Chipset Watchdog Driver
+ *
+ * Copyright (C) 2011 Sigfox
+ * License terms: GNU General Public License (GPL) version 2
+ * Author: Marc Vertes <marc.vertes@sigfox.com>
+ * Based on a preliminary version from Harald Welte <HaraldWelte@viatech.com>
+ * Timer code by Wim Van Sebroeck <wim@iguana.be>
+ *
+ * Caveat: PnP must be enabled in BIOS to allow full access to watchdog
+ * control registers. If not, the watchdog must be configured in BIOS manually.
+ */
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/timer.h>
+#include <linux/watchdog.h>
+
+/* Configuration registers relative to the pci device */
+#define VIA_WDT_MMIO_BASE	0xe8	/* MMIO region base address */
+#define VIA_WDT_CONF		0xec	/* watchdog enable state */
+
+/* Relevant bits for the VIA_WDT_CONF register */
+#define VIA_WDT_CONF_ENABLE	0x01	/* 1: enable watchdog */
+#define VIA_WDT_CONF_MMIO	0x02	/* 1: enable watchdog MMIO */
+
+/*
+ * The MMIO region contains the watchog control register and the
+ * hardware timer counter.
+ */
+#define VIA_WDT_MMIO_LEN	8	/* MMIO region length in bytes */
+#define VIA_WDT_CTL		0	/* MMIO addr+0: state/control reg. */
+#define VIA_WDT_COUNT		4	/* MMIO addr+4: timer counter reg. */
+
+/* Bits for the VIA_WDT_CTL register */
+#define VIA_WDT_RUNNING		0x01	/* 0: stop, 1: running */
+#define VIA_WDT_FIRED		0x02	/* 1: restarted by expired watchdog */
+#define VIA_WDT_PWROFF		0x04	/* 0: reset, 1: poweroff */
+#define VIA_WDT_DISABLED	0x08	/* 1: timer is disabled */
+#define VIA_WDT_TRIGGER		0x80	/* 1: start a new countdown */
+
+/* Hardware heartbeat in seconds */
+#define WDT_HW_HEARTBEAT 1
+
+/* Timer heartbeat (500ms) */
+#define WDT_HEARTBEAT	(HZ/2)	/* should be <= ((WDT_HW_HEARTBEAT*HZ)/2) */
+
+/* User space timeout in seconds */
+#define WDT_TIMEOUT_MAX	1023	/* approx. 17 min. */
+#define WDT_TIMEOUT	60
+static int timeout = WDT_TIMEOUT;
+module_param(timeout, int, 0);
+MODULE_PARM_DESC(timeout, "Watchdog timeout in seconds, between 1 and 1023 "
+	"(default = " __MODULE_STRING(WDT_TIMEOUT) ")");
+
+static int nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, int, 0);
+MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started "
+	"(default = " __MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+static struct watchdog_device wdt_dev;
+static struct resource wdt_res;
+static void __iomem *wdt_mem;
+static unsigned int mmio;
+static void wdt_timer_tick(unsigned long data);
+static DEFINE_TIMER(timer, wdt_timer_tick, 0, 0);
+					/* The timer that pings the watchdog */
+static unsigned long next_heartbeat;	/* the next_heartbeat for the timer */
+
+static inline void wdt_reset(void)
+{
+	unsigned int ctl = readl(wdt_mem);
+
+	writel(ctl | VIA_WDT_TRIGGER, wdt_mem);
+}
+
+/*
+ * Timer tick: the timer will make sure that the watchdog timer hardware
+ * is being reset in time. The conditions to do this are:
+ *  1) the watchog timer has been started and /dev/watchdog is open
+ *     and there is still time left before userspace should send the
+ *     next heartbeat/ping. (note: the internal heartbeat is much smaller
+ *     then the external/userspace heartbeat).
+ *  2) the watchdog timer has been stopped by userspace.
+ */
+static void wdt_timer_tick(unsigned long data)
+{
+	if (time_before(jiffies, next_heartbeat) ||
+	   (!test_bit(WDOG_ACTIVE, &wdt_dev.status))) {
+		wdt_reset();
+		mod_timer(&timer, jiffies + WDT_HEARTBEAT);
+	} else
+		pr_crit("I will reboot your machine !\n");
+}
+
+static int wdt_ping(struct watchdog_device *wdd)
+{
+	/* calculate when the next userspace timeout will be */
+	next_heartbeat = jiffies + timeout * HZ;
+	return 0;
+}
+
+static int wdt_start(struct watchdog_device *wdd)
+{
+	unsigned int ctl = readl(wdt_mem);
+
+	writel(timeout, wdt_mem + VIA_WDT_COUNT);
+	writel(ctl | VIA_WDT_RUNNING | VIA_WDT_TRIGGER, wdt_mem);
+	wdt_ping(wdd);
+	mod_timer(&timer, jiffies + WDT_HEARTBEAT);
+	return 0;
+}
+
+static int wdt_stop(struct watchdog_device *wdd)
+{
+	unsigned int ctl = readl(wdt_mem);
+
+	writel(ctl & ~VIA_WDT_RUNNING, wdt_mem);
+	return 0;
+}
+
+static int wdt_set_timeout(struct watchdog_device *wdd,
+			   unsigned int new_timeout)
+{
+	if (new_timeout < 1 || new_timeout > WDT_TIMEOUT_MAX)
+		return -EINVAL;
+	writel(new_timeout, wdt_mem + VIA_WDT_COUNT);
+	timeout = new_timeout;
+	return 0;
+}
+
+static const struct watchdog_info wdt_info = {
+	.identity =	"VIA watchdog",
+	.options =	WDIOF_CARDRESET |
+			WDIOF_SETTIMEOUT |
+			WDIOF_MAGICCLOSE |
+			WDIOF_KEEPALIVEPING,
+};
+
+static const struct watchdog_ops wdt_ops = {
+	.owner =	THIS_MODULE,
+	.start =	wdt_start,
+	.stop =		wdt_stop,
+	.ping =		wdt_ping,
+	.set_timeout =	wdt_set_timeout,
+};
+
+static struct watchdog_device wdt_dev = {
+	.info =		&wdt_info,
+	.ops =		&wdt_ops,
+};
+
+static int __devinit wdt_probe(struct pci_dev *pdev,
+			       const struct pci_device_id *ent)
+{
+	unsigned char conf;
+	int ret = -ENODEV;
+
+	if (pci_enable_device(pdev)) {
+		dev_err(&pdev->dev, "cannot enable PCI device\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Allocate a MMIO region which contains watchdog control register
+	 * and counter, then configure the watchdog to use this region.
+	 * This is possible only if PnP is properly enabled in BIOS.
+	 * If not, the watchdog must be configured in BIOS manually.
+	 */
+	if (allocate_resource(&iomem_resource, &wdt_res, VIA_WDT_MMIO_LEN,
+			      0xf0000000, 0xffffff00, 0xff, NULL, NULL)) {
+		dev_err(&pdev->dev, "MMIO allocation failed\n");
+		goto err_out_disable_device;
+	}
+
+	pci_write_config_dword(pdev, VIA_WDT_MMIO_BASE, wdt_res.start);
+	pci_read_config_byte(pdev, VIA_WDT_CONF, &conf);
+	conf |= VIA_WDT_CONF_ENABLE | VIA_WDT_CONF_MMIO;
+	pci_write_config_byte(pdev, VIA_WDT_CONF, conf);
+
+	pci_read_config_dword(pdev, VIA_WDT_MMIO_BASE, &mmio);
+	if (mmio) {
+		dev_info(&pdev->dev, "VIA Chipset watchdog MMIO: %x\n", mmio);
+	} else {
+		dev_err(&pdev->dev, "MMIO setting failed. Check BIOS.\n");
+		goto err_out_resource;
+	}
+
+	if (!request_mem_region(mmio, VIA_WDT_MMIO_LEN, "via_wdt")) {
+		dev_err(&pdev->dev, "MMIO region busy\n");
+		goto err_out_resource;
+	}
+
+	wdt_mem = ioremap(mmio, VIA_WDT_MMIO_LEN);
+	if (wdt_mem == NULL) {
+		dev_err(&pdev->dev, "cannot remap VIA wdt MMIO registers\n");
+		goto err_out_release;
+	}
+
+	wdt_dev.timeout = timeout;
+	watchdog_set_nowayout(&wdt_dev, nowayout);
+	if (readl(wdt_mem) & VIA_WDT_FIRED)
+		wdt_dev.bootstatus |= WDIOF_CARDRESET;
+
+	ret = watchdog_register_device(&wdt_dev);
+	if (ret)
+		goto err_out_iounmap;
+
+	/* start triggering, in case of watchdog already enabled by BIOS */
+	mod_timer(&timer, jiffies + WDT_HEARTBEAT);
+	return 0;
+
+err_out_iounmap:
+	iounmap(wdt_mem);
+err_out_release:
+	release_mem_region(mmio, VIA_WDT_MMIO_LEN);
+err_out_resource:
+	release_resource(&wdt_res);
+err_out_disable_device:
+	pci_disable_device(pdev);
+	return ret;
+}
+
+static void __devexit wdt_remove(struct pci_dev *pdev)
+{
+	watchdog_unregister_device(&wdt_dev);
+	del_timer(&timer);
+	iounmap(wdt_mem);
+	release_mem_region(mmio, VIA_WDT_MMIO_LEN);
+	release_resource(&wdt_res);
+	pci_disable_device(pdev);
+}
+
+DEFINE_PCI_DEVICE_TABLE(wdt_pci_table) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_CX700) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VX800) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VX855) },
+	{ 0 }
+};
+
+static struct pci_driver wdt_driver = {
+	.name		= "via_wdt",
+	.id_table	= wdt_pci_table,
+	.probe		= wdt_probe,
+	.remove		= __devexit_p(wdt_remove),
+};
+
+static int __init wdt_init(void)
+{
+	if (timeout < 1 || timeout > WDT_TIMEOUT_MAX)
+		timeout = WDT_TIMEOUT;
+	return pci_register_driver(&wdt_driver);
+}
+
+static void __exit wdt_exit(void)
+{
+	pci_unregister_driver(&wdt_driver);
+}
+
+module_init(wdt_init);
+module_exit(wdt_exit);
+
+MODULE_AUTHOR("Marc Vertes");
+MODULE_DESCRIPTION("Driver for watchdog timer on VIA chipset");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/wm831x_wdt.c b/drivers/watchdog/wm831x_wdt.c
index e789a47db41f..263c883f0806 100644
--- a/drivers/watchdog/wm831x_wdt.c
+++ b/drivers/watchdog/wm831x_wdt.c
@@ -199,7 +199,8 @@ static int __devinit wm831x_wdt_probe(struct platform_device *pdev)
 	if (reg & WM831X_WDOG_DEBUG)
 		dev_warn(wm831x->dev, "Watchdog is paused\n");
 
-	driver_data = kzalloc(sizeof(*driver_data), GFP_KERNEL);
+	driver_data = devm_kzalloc(&pdev->dev, sizeof(*driver_data),
+				   GFP_KERNEL);
 	if (!driver_data) {
 		dev_err(wm831x->dev, "Unable to alloacate watchdog device\n");
 		ret = -ENOMEM;
@@ -213,11 +214,9 @@ static int __devinit wm831x_wdt_probe(struct platform_device *pdev)
 
 	wm831x_wdt->info = &wm831x_wdt_info;
 	wm831x_wdt->ops = &wm831x_wdt_ops;
+	watchdog_set_nowayout(wm831x_wdt, nowayout);
 	watchdog_set_drvdata(wm831x_wdt, driver_data);
 
-	if (nowayout)
-		wm831x_wdt->status |= WDOG_NO_WAY_OUT;
-
 	reg = wm831x_reg_read(wm831x, WM831X_WATCHDOG);
 	reg &= WM831X_WDOG_TO_MASK;
 	for (i = 0; i < ARRAY_SIZE(wm831x_wdt_cfgs); i++)
@@ -252,7 +251,7 @@ static int __devinit wm831x_wdt_probe(struct platform_device *pdev)
 				dev_err(wm831x->dev,
 					"Failed to request update GPIO: %d\n",
 					ret);
-				goto err_alloc;
+				goto err;
 			}
 
 			ret = gpio_direction_output(pdata->update_gpio, 0);
@@ -294,8 +293,6 @@ static int __devinit wm831x_wdt_probe(struct platform_device *pdev)
 err_gpio:
 	if (driver_data->update_gpio)
 		gpio_free(driver_data->update_gpio);
-err_alloc:
-	kfree(driver_data);
 err:
 	return ret;
 }
@@ -320,17 +317,7 @@ static struct platform_driver wm831x_wdt_driver = {
 	},
 };
 
-static int __init wm831x_wdt_init(void)
-{
-	return platform_driver_register(&wm831x_wdt_driver);
-}
-module_init(wm831x_wdt_init);
-
-static void __exit wm831x_wdt_exit(void)
-{
-	platform_driver_unregister(&wm831x_wdt_driver);
-}
-module_exit(wm831x_wdt_exit);
+module_platform_driver(wm831x_wdt_driver);
 
 MODULE_AUTHOR("Mark Brown");
 MODULE_DESCRIPTION("WM831x Watchdog");
diff --git a/drivers/watchdog/wm8350_wdt.c b/drivers/watchdog/wm8350_wdt.c
index b68d928c8f90..909c78650d3e 100644
--- a/drivers/watchdog/wm8350_wdt.c
+++ b/drivers/watchdog/wm8350_wdt.c
@@ -311,17 +311,7 @@ static struct platform_driver wm8350_wdt_driver = {
 	},
 };
 
-static int __init wm8350_wdt_init(void)
-{
-	return platform_driver_register(&wm8350_wdt_driver);
-}
-module_init(wm8350_wdt_init);
-
-static void __exit wm8350_wdt_exit(void)
-{
-	platform_driver_unregister(&wm8350_wdt_driver);
-}
-module_exit(wm8350_wdt_exit);
+module_platform_driver(wm8350_wdt_driver);
 
 MODULE_AUTHOR("Mark Brown");
 MODULE_DESCRIPTION("WM8350 Watchdog");
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index 8795480c2350..a1ced521cf74 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -86,6 +86,7 @@ config XEN_BACKEND
 
 config XENFS
 	tristate "Xen filesystem"
+	select XEN_PRIVCMD
 	default y
 	help
 	  The xen filesystem provides a way for domains to share
@@ -171,4 +172,10 @@ config XEN_PCIDEV_BACKEND
 	  xen-pciback.hide=(03:00.0)(04:00.0)
 
 	  If in doubt, say m.
+
+config XEN_PRIVCMD
+	tristate
+	depends on XEN
+	default m
+
 endmenu
diff --git a/drivers/xen/Makefile b/drivers/xen/Makefile
index 974fffdf22b2..aa31337192cc 100644
--- a/drivers/xen/Makefile
+++ b/drivers/xen/Makefile
@@ -19,7 +19,9 @@ obj-$(CONFIG_XEN_TMEM)			+= tmem.o
 obj-$(CONFIG_SWIOTLB_XEN)		+= swiotlb-xen.o
 obj-$(CONFIG_XEN_DOM0)			+= pci.o
 obj-$(CONFIG_XEN_PCIDEV_BACKEND)	+= xen-pciback/
+obj-$(CONFIG_XEN_PRIVCMD)		+= xen-privcmd.o
 
 xen-evtchn-y				:= evtchn.o
 xen-gntdev-y				:= gntdev.o
 xen-gntalloc-y				:= gntalloc.o
+xen-privcmd-y				:= privcmd.o
diff --git a/drivers/xen/events.c b/drivers/xen/events.c
index 6e075cdd0c6b..e5e5812a1014 100644
--- a/drivers/xen/events.c
+++ b/drivers/xen/events.c
@@ -87,6 +87,7 @@ enum xen_irq_type {
  */
 struct irq_info {
 	struct list_head list;
+	int refcnt;
 	enum xen_irq_type type;	/* type */
 	unsigned irq;
 	unsigned short evtchn;	/* event channel */
@@ -406,6 +407,7 @@ static void xen_irq_init(unsigned irq)
 		panic("Unable to allocate metadata for IRQ%d\n", irq);
 
 	info->type = IRQT_UNBOUND;
+	info->refcnt = -1;
 
 	irq_set_handler_data(irq, info);
 
@@ -469,6 +471,8 @@ static void xen_free_irq(unsigned irq)
 
 	irq_set_handler_data(irq, NULL);
 
+	WARN_ON(info->refcnt > 0);
+
 	kfree(info);
 
 	/* Legacy IRQ descriptors are managed by the arch. */
@@ -637,7 +641,7 @@ int xen_bind_pirq_gsi_to_irq(unsigned gsi,
 	if (irq != -1) {
 		printk(KERN_INFO "xen_map_pirq_gsi: returning irq %d for gsi %u\n",
 		       irq, gsi);
-		goto out;	/* XXX need refcount? */
+		goto out;
 	}
 
 	irq = xen_allocate_irq_gsi(gsi);
@@ -939,9 +943,16 @@ static void unbind_from_irq(unsigned int irq)
 {
 	struct evtchn_close close;
 	int evtchn = evtchn_from_irq(irq);
+	struct irq_info *info = irq_get_handler_data(irq);
 
 	mutex_lock(&irq_mapping_update_lock);
 
+	if (info->refcnt > 0) {
+		info->refcnt--;
+		if (info->refcnt != 0)
+			goto done;
+	}
+
 	if (VALID_EVTCHN(evtchn)) {
 		close.port = evtchn;
 		if (HYPERVISOR_event_channel_op(EVTCHNOP_close, &close) != 0)
@@ -970,6 +981,7 @@ static void unbind_from_irq(unsigned int irq)
 
 	xen_free_irq(irq);
 
+ done:
 	mutex_unlock(&irq_mapping_update_lock);
 }
 
@@ -1065,6 +1077,69 @@ void unbind_from_irqhandler(unsigned int irq, void *dev_id)
 }
 EXPORT_SYMBOL_GPL(unbind_from_irqhandler);
 
+int evtchn_make_refcounted(unsigned int evtchn)
+{
+	int irq = evtchn_to_irq[evtchn];
+	struct irq_info *info;
+
+	if (irq == -1)
+		return -ENOENT;
+
+	info = irq_get_handler_data(irq);
+
+	if (!info)
+		return -ENOENT;
+
+	WARN_ON(info->refcnt != -1);
+
+	info->refcnt = 1;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(evtchn_make_refcounted);
+
+int evtchn_get(unsigned int evtchn)
+{
+	int irq;
+	struct irq_info *info;
+	int err = -ENOENT;
+
+	if (evtchn >= NR_EVENT_CHANNELS)
+		return -EINVAL;
+
+	mutex_lock(&irq_mapping_update_lock);
+
+	irq = evtchn_to_irq[evtchn];
+	if (irq == -1)
+		goto done;
+
+	info = irq_get_handler_data(irq);
+
+	if (!info)
+		goto done;
+
+	err = -EINVAL;
+	if (info->refcnt <= 0)
+		goto done;
+
+	info->refcnt++;
+	err = 0;
+ done:
+	mutex_unlock(&irq_mapping_update_lock);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(evtchn_get);
+
+void evtchn_put(unsigned int evtchn)
+{
+	int irq = evtchn_to_irq[evtchn];
+	if (WARN_ON(irq == -1))
+		return;
+	unbind_from_irq(irq);
+}
+EXPORT_SYMBOL_GPL(evtchn_put);
+
 void xen_send_IPI_one(unsigned int cpu, enum ipi_vector vector)
 {
 	int irq = per_cpu(ipi_to_irq, cpu)[vector];
diff --git a/drivers/xen/evtchn.c b/drivers/xen/evtchn.c
index dbc13e94b612..b1f60a0c0bea 100644
--- a/drivers/xen/evtchn.c
+++ b/drivers/xen/evtchn.c
@@ -268,7 +268,7 @@ static int evtchn_bind_to_user(struct per_user_data *u, int port)
 	rc = bind_evtchn_to_irqhandler(port, evtchn_interrupt, IRQF_DISABLED,
 				       u->name, (void *)(unsigned long)port);
 	if (rc >= 0)
-		rc = 0;
+		rc = evtchn_make_refcounted(port);
 
 	return rc;
 }
diff --git a/drivers/xen/gntalloc.c b/drivers/xen/gntalloc.c
index e1c4c6e5b469..934985d14c24 100644
--- a/drivers/xen/gntalloc.c
+++ b/drivers/xen/gntalloc.c
@@ -74,7 +74,7 @@ MODULE_PARM_DESC(limit, "Maximum number of grants that may be allocated by "
 		"the gntalloc device");
 
 static LIST_HEAD(gref_list);
-static DEFINE_SPINLOCK(gref_lock);
+static DEFINE_MUTEX(gref_mutex);
 static int gref_size;
 
 struct notify_info {
@@ -99,6 +99,12 @@ struct gntalloc_file_private_data {
 	uint64_t index;
 };
 
+struct gntalloc_vma_private_data {
+	struct gntalloc_gref *gref;
+	int users;
+	int count;
+};
+
 static void __del_gref(struct gntalloc_gref *gref);
 
 static void do_cleanup(void)
@@ -143,15 +149,15 @@ static int add_grefs(struct ioctl_gntalloc_alloc_gref *op,
 	}
 
 	/* Add to gref lists. */
-	spin_lock(&gref_lock);
+	mutex_lock(&gref_mutex);
 	list_splice_tail(&queue_gref, &gref_list);
 	list_splice_tail(&queue_file, &priv->list);
-	spin_unlock(&gref_lock);
+	mutex_unlock(&gref_mutex);
 
 	return 0;
 
 undo:
-	spin_lock(&gref_lock);
+	mutex_lock(&gref_mutex);
 	gref_size -= (op->count - i);
 
 	list_for_each_entry(gref, &queue_file, next_file) {
@@ -167,7 +173,7 @@ undo:
 	 */
 	if (unlikely(!list_empty(&queue_gref)))
 		list_splice_tail(&queue_gref, &gref_list);
-	spin_unlock(&gref_lock);
+	mutex_unlock(&gref_mutex);
 	return rc;
 }
 
@@ -178,8 +184,10 @@ static void __del_gref(struct gntalloc_gref *gref)
 		tmp[gref->notify.pgoff] = 0;
 		kunmap(gref->page);
 	}
-	if (gref->notify.flags & UNMAP_NOTIFY_SEND_EVENT)
+	if (gref->notify.flags & UNMAP_NOTIFY_SEND_EVENT) {
 		notify_remote_via_evtchn(gref->notify.event);
+		evtchn_put(gref->notify.event);
+	}
 
 	gref->notify.flags = 0;
 
@@ -189,6 +197,8 @@ static void __del_gref(struct gntalloc_gref *gref)
 
 		if (!gnttab_end_foreign_access_ref(gref->gref_id, 0))
 			return;
+
+		gnttab_free_grant_reference(gref->gref_id);
 	}
 
 	gref_size--;
@@ -251,7 +261,7 @@ static int gntalloc_release(struct inode *inode, struct file *filp)
 
 	pr_debug("%s: priv %p\n", __func__, priv);
 
-	spin_lock(&gref_lock);
+	mutex_lock(&gref_mutex);
 	while (!list_empty(&priv->list)) {
 		gref = list_entry(priv->list.next,
 			struct gntalloc_gref, next_file);
@@ -261,7 +271,7 @@ static int gntalloc_release(struct inode *inode, struct file *filp)
 			__del_gref(gref);
 	}
 	kfree(priv);
-	spin_unlock(&gref_lock);
+	mutex_unlock(&gref_mutex);
 
 	return 0;
 }
@@ -286,21 +296,21 @@ static long gntalloc_ioctl_alloc(struct gntalloc_file_private_data *priv,
 		goto out;
 	}
 
-	spin_lock(&gref_lock);
+	mutex_lock(&gref_mutex);
 	/* Clean up pages that were at zero (local) users but were still mapped
 	 * by remote domains. Since those pages count towards the limit that we
 	 * are about to enforce, removing them here is a good idea.
 	 */
 	do_cleanup();
 	if (gref_size + op.count > limit) {
-		spin_unlock(&gref_lock);
+		mutex_unlock(&gref_mutex);
 		rc = -ENOSPC;
 		goto out_free;
 	}
 	gref_size += op.count;
 	op.index = priv->index;
 	priv->index += op.count * PAGE_SIZE;
-	spin_unlock(&gref_lock);
+	mutex_unlock(&gref_mutex);
 
 	rc = add_grefs(&op, gref_ids, priv);
 	if (rc < 0)
@@ -343,7 +353,7 @@ static long gntalloc_ioctl_dealloc(struct gntalloc_file_private_data *priv,
 		goto dealloc_grant_out;
 	}
 
-	spin_lock(&gref_lock);
+	mutex_lock(&gref_mutex);
 	gref = find_grefs(priv, op.index, op.count);
 	if (gref) {
 		/* Remove from the file list only, and decrease reference count.
@@ -363,7 +373,7 @@ static long gntalloc_ioctl_dealloc(struct gntalloc_file_private_data *priv,
 
 	do_cleanup();
 
-	spin_unlock(&gref_lock);
+	mutex_unlock(&gref_mutex);
 dealloc_grant_out:
 	return rc;
 }
@@ -383,7 +393,7 @@ static long gntalloc_ioctl_unmap_notify(struct gntalloc_file_private_data *priv,
 	index = op.index & ~(PAGE_SIZE - 1);
 	pgoff = op.index & (PAGE_SIZE - 1);
 
-	spin_lock(&gref_lock);
+	mutex_lock(&gref_mutex);
 
 	gref = find_grefs(priv, index, 1);
 	if (!gref) {
@@ -396,12 +406,30 @@ static long gntalloc_ioctl_unmap_notify(struct gntalloc_file_private_data *priv,
 		goto unlock_out;
 	}
 
+	/* We need to grab a reference to the event channel we are going to use
+	 * to send the notify before releasing the reference we may already have
+	 * (if someone has called this ioctl twice). This is required so that
+	 * it is possible to change the clear_byte part of the notification
+	 * without disturbing the event channel part, which may now be the last
+	 * reference to that event channel.
+	 */
+	if (op.action & UNMAP_NOTIFY_SEND_EVENT) {
+		if (evtchn_get(op.event_channel_port)) {
+			rc = -EINVAL;
+			goto unlock_out;
+		}
+	}
+
+	if (gref->notify.flags & UNMAP_NOTIFY_SEND_EVENT)
+		evtchn_put(gref->notify.event);
+
 	gref->notify.flags = op.action;
 	gref->notify.pgoff = pgoff;
 	gref->notify.event = op.event_channel_port;
 	rc = 0;
+
  unlock_out:
-	spin_unlock(&gref_lock);
+	mutex_unlock(&gref_mutex);
 	return rc;
 }
 
@@ -429,26 +457,40 @@ static long gntalloc_ioctl(struct file *filp, unsigned int cmd,
 
 static void gntalloc_vma_open(struct vm_area_struct *vma)
 {
-	struct gntalloc_gref *gref = vma->vm_private_data;
-	if (!gref)
+	struct gntalloc_vma_private_data *priv = vma->vm_private_data;
+
+	if (!priv)
 		return;
 
-	spin_lock(&gref_lock);
-	gref->users++;
-	spin_unlock(&gref_lock);
+	mutex_lock(&gref_mutex);
+	priv->users++;
+	mutex_unlock(&gref_mutex);
 }
 
 static void gntalloc_vma_close(struct vm_area_struct *vma)
 {
-	struct gntalloc_gref *gref = vma->vm_private_data;
-	if (!gref)
+	struct gntalloc_vma_private_data *priv = vma->vm_private_data;
+	struct gntalloc_gref *gref, *next;
+	int i;
+
+	if (!priv)
 		return;
 
-	spin_lock(&gref_lock);
-	gref->users--;
-	if (gref->users == 0)
-		__del_gref(gref);
-	spin_unlock(&gref_lock);
+	mutex_lock(&gref_mutex);
+	priv->users--;
+	if (priv->users == 0) {
+		gref = priv->gref;
+		for (i = 0; i < priv->count; i++) {
+			gref->users--;
+			next = list_entry(gref->next_gref.next,
+					  struct gntalloc_gref, next_gref);
+			if (gref->users == 0)
+				__del_gref(gref);
+			gref = next;
+		}
+		kfree(priv);
+	}
+	mutex_unlock(&gref_mutex);
 }
 
 static struct vm_operations_struct gntalloc_vmops = {
@@ -459,30 +501,41 @@ static struct vm_operations_struct gntalloc_vmops = {
 static int gntalloc_mmap(struct file *filp, struct vm_area_struct *vma)
 {
 	struct gntalloc_file_private_data *priv = filp->private_data;
+	struct gntalloc_vma_private_data *vm_priv;
 	struct gntalloc_gref *gref;
 	int count = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
 	int rv, i;
 
-	pr_debug("%s: priv %p, page %lu+%d\n", __func__,
-		       priv, vma->vm_pgoff, count);
-
 	if (!(vma->vm_flags & VM_SHARED)) {
 		printk(KERN_ERR "%s: Mapping must be shared.\n", __func__);
 		return -EINVAL;
 	}
 
-	spin_lock(&gref_lock);
+	vm_priv = kmalloc(sizeof(*vm_priv), GFP_KERNEL);
+	if (!vm_priv)
+		return -ENOMEM;
+
+	mutex_lock(&gref_mutex);
+
+	pr_debug("%s: priv %p,%p, page %lu+%d\n", __func__,
+		       priv, vm_priv, vma->vm_pgoff, count);
+
 	gref = find_grefs(priv, vma->vm_pgoff << PAGE_SHIFT, count);
 	if (gref == NULL) {
 		rv = -ENOENT;
 		pr_debug("%s: Could not find grant reference",
 				__func__);
+		kfree(vm_priv);
 		goto out_unlock;
 	}
 
-	vma->vm_private_data = gref;
+	vm_priv->gref = gref;
+	vm_priv->users = 1;
+	vm_priv->count = count;
+
+	vma->vm_private_data = vm_priv;
 
-	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_RESERVED | VM_DONTEXPAND;
 
 	vma->vm_ops = &gntalloc_vmops;
 
@@ -499,7 +552,7 @@ static int gntalloc_mmap(struct file *filp, struct vm_area_struct *vma)
 	rv = 0;
 
 out_unlock:
-	spin_unlock(&gref_lock);
+	mutex_unlock(&gref_mutex);
 	return rv;
 }
 
diff --git a/drivers/xen/gntdev.c b/drivers/xen/gntdev.c
index afca14d9042e..99d8151c824a 100644
--- a/drivers/xen/gntdev.c
+++ b/drivers/xen/gntdev.c
@@ -193,8 +193,10 @@ static void gntdev_put_map(struct grant_map *map)
 
 	atomic_sub(map->count, &pages_mapped);
 
-	if (map->notify.flags & UNMAP_NOTIFY_SEND_EVENT)
+	if (map->notify.flags & UNMAP_NOTIFY_SEND_EVENT) {
 		notify_remote_via_evtchn(map->notify.event);
+		evtchn_put(map->notify.event);
+	}
 
 	if (map->pages) {
 		if (!use_ptemod)
@@ -312,7 +314,8 @@ static int __unmap_grant_pages(struct grant_map *map, int offset, int pages)
 		}
 	}
 
-	err = gnttab_unmap_refs(map->unmap_ops + offset, map->pages + offset, pages);
+	err = gnttab_unmap_refs(map->unmap_ops + offset, map->pages + offset,
+				pages, true);
 	if (err)
 		return err;
 
@@ -599,6 +602,8 @@ static long gntdev_ioctl_notify(struct gntdev_priv *priv, void __user *u)
 	struct ioctl_gntdev_unmap_notify op;
 	struct grant_map *map;
 	int rc;
+	int out_flags;
+	unsigned int out_event;
 
 	if (copy_from_user(&op, u, sizeof(op)))
 		return -EFAULT;
@@ -606,6 +611,21 @@ static long gntdev_ioctl_notify(struct gntdev_priv *priv, void __user *u)
 	if (op.action & ~(UNMAP_NOTIFY_CLEAR_BYTE|UNMAP_NOTIFY_SEND_EVENT))
 		return -EINVAL;
 
+	/* We need to grab a reference to the event channel we are going to use
+	 * to send the notify before releasing the reference we may already have
+	 * (if someone has called this ioctl twice). This is required so that
+	 * it is possible to change the clear_byte part of the notification
+	 * without disturbing the event channel part, which may now be the last
+	 * reference to that event channel.
+	 */
+	if (op.action & UNMAP_NOTIFY_SEND_EVENT) {
+		if (evtchn_get(op.event_channel_port))
+			return -EINVAL;
+	}
+
+	out_flags = op.action;
+	out_event = op.event_channel_port;
+
 	spin_lock(&priv->lock);
 
 	list_for_each_entry(map, &priv->maps, next) {
@@ -624,12 +644,22 @@ static long gntdev_ioctl_notify(struct gntdev_priv *priv, void __user *u)
 		goto unlock_out;
 	}
 
+	out_flags = map->notify.flags;
+	out_event = map->notify.event;
+
 	map->notify.flags = op.action;
 	map->notify.addr = op.index - (map->index << PAGE_SHIFT);
 	map->notify.event = op.event_channel_port;
+
 	rc = 0;
+
  unlock_out:
 	spin_unlock(&priv->lock);
+
+	/* Drop the reference to the event channel we did not save in the map */
+	if (out_flags & UNMAP_NOTIFY_SEND_EVENT)
+		evtchn_put(out_event);
+
 	return rc;
 }
 
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index bf1c094f4ebf..1cd94daa71db 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -44,16 +44,19 @@
 #include <xen/page.h>
 #include <xen/grant_table.h>
 #include <xen/interface/memory.h>
+#include <xen/hvc-console.h>
 #include <asm/xen/hypercall.h>
 
 #include <asm/pgtable.h>
 #include <asm/sync_bitops.h>
 
-
 /* External tools reserve first few grant table entries. */
 #define NR_RESERVED_ENTRIES 8
 #define GNTTAB_LIST_END 0xffffffff
-#define GREFS_PER_GRANT_FRAME (PAGE_SIZE / sizeof(struct grant_entry))
+#define GREFS_PER_GRANT_FRAME \
+(grant_table_version == 1 ?                      \
+(PAGE_SIZE / sizeof(struct grant_entry_v1)) :   \
+(PAGE_SIZE / sizeof(union grant_entry_v2)))
 
 static grant_ref_t **gnttab_list;
 static unsigned int nr_grant_frames;
@@ -64,13 +67,97 @@ static DEFINE_SPINLOCK(gnttab_list_lock);
 unsigned long xen_hvm_resume_frames;
 EXPORT_SYMBOL_GPL(xen_hvm_resume_frames);
 
-static struct grant_entry *shared;
+static union {
+	struct grant_entry_v1 *v1;
+	union grant_entry_v2 *v2;
+	void *addr;
+} gnttab_shared;
+
+/*This is a structure of function pointers for grant table*/
+struct gnttab_ops {
+	/*
+	 * Mapping a list of frames for storing grant entries. Frames parameter
+	 * is used to store grant table address when grant table being setup,
+	 * nr_gframes is the number of frames to map grant table. Returning
+	 * GNTST_okay means success and negative value means failure.
+	 */
+	int (*map_frames)(unsigned long *frames, unsigned int nr_gframes);
+	/*
+	 * Release a list of frames which are mapped in map_frames for grant
+	 * entry status.
+	 */
+	void (*unmap_frames)(void);
+	/*
+	 * Introducing a valid entry into the grant table, granting the frame of
+	 * this grant entry to domain for accessing or transfering. Ref
+	 * parameter is reference of this introduced grant entry, domid is id of
+	 * granted domain, frame is the page frame to be granted, and flags is
+	 * status of the grant entry to be updated.
+	 */
+	void (*update_entry)(grant_ref_t ref, domid_t domid,
+			     unsigned long frame, unsigned flags);
+	/*
+	 * Stop granting a grant entry to domain for accessing. Ref parameter is
+	 * reference of a grant entry whose grant access will be stopped,
+	 * readonly is not in use in this function. If the grant entry is
+	 * currently mapped for reading or writing, just return failure(==0)
+	 * directly and don't tear down the grant access. Otherwise, stop grant
+	 * access for this entry and return success(==1).
+	 */
+	int (*end_foreign_access_ref)(grant_ref_t ref, int readonly);
+	/*
+	 * Stop granting a grant entry to domain for transfer. Ref parameter is
+	 * reference of a grant entry whose grant transfer will be stopped. If
+	 * tranfer has not started, just reclaim the grant entry and return
+	 * failure(==0). Otherwise, wait for the transfer to complete and then
+	 * return the frame.
+	 */
+	unsigned long (*end_foreign_transfer_ref)(grant_ref_t ref);
+	/*
+	 * Query the status of a grant entry. Ref parameter is reference of
+	 * queried grant entry, return value is the status of queried entry.
+	 * Detailed status(writing/reading) can be gotten from the return value
+	 * by bit operations.
+	 */
+	int (*query_foreign_access)(grant_ref_t ref);
+	/*
+	 * Grant a domain to access a range of bytes within the page referred by
+	 * an available grant entry. Ref parameter is reference of a grant entry
+	 * which will be sub-page accessed, domid is id of grantee domain, frame
+	 * is frame address of subpage grant, flags is grant type and flag
+	 * information, page_off is offset of the range of bytes, and length is
+	 * length of bytes to be accessed.
+	 */
+	void (*update_subpage_entry)(grant_ref_t ref, domid_t domid,
+				     unsigned long frame, int flags,
+				     unsigned page_off, unsigned length);
+	/*
+	 * Redirect an available grant entry on domain A to another grant
+	 * reference of domain B, then allow domain C to use grant reference
+	 * of domain B transitively. Ref parameter is an available grant entry
+	 * reference on domain A, domid is id of domain C which accesses grant
+	 * entry transitively, flags is grant type and flag information,
+	 * trans_domid is id of domain B whose grant entry is finally accessed
+	 * transitively, trans_gref is grant entry transitive reference of
+	 * domain B.
+	 */
+	void (*update_trans_entry)(grant_ref_t ref, domid_t domid, int flags,
+				   domid_t trans_domid, grant_ref_t trans_gref);
+};
+
+static struct gnttab_ops *gnttab_interface;
+
+/*This reflects status of grant entries, so act as a global value*/
+static grant_status_t *grstatus;
+
+static int grant_table_version;
 
 static struct gnttab_free_callback *gnttab_free_callback_list;
 
 static int gnttab_expand(unsigned int req_entries);
 
 #define RPP (PAGE_SIZE / sizeof(grant_ref_t))
+#define SPP (PAGE_SIZE / sizeof(grant_status_t))
 
 static inline grant_ref_t *__gnttab_entry(grant_ref_t entry)
 {
@@ -142,23 +229,33 @@ static void put_free_entry(grant_ref_t ref)
 	spin_unlock_irqrestore(&gnttab_list_lock, flags);
 }
 
-static void update_grant_entry(grant_ref_t ref, domid_t domid,
-			       unsigned long frame, unsigned flags)
+/*
+ * Following applies to gnttab_update_entry_v1 and gnttab_update_entry_v2.
+ * Introducing a valid entry into the grant table:
+ *  1. Write ent->domid.
+ *  2. Write ent->frame:
+ *      GTF_permit_access:   Frame to which access is permitted.
+ *      GTF_accept_transfer: Pseudo-phys frame slot being filled by new
+ *                           frame, or zero if none.
+ *  3. Write memory barrier (WMB).
+ *  4. Write ent->flags, inc. valid type.
+ */
+static void gnttab_update_entry_v1(grant_ref_t ref, domid_t domid,
+				   unsigned long frame, unsigned flags)
+{
+	gnttab_shared.v1[ref].domid = domid;
+	gnttab_shared.v1[ref].frame = frame;
+	wmb();
+	gnttab_shared.v1[ref].flags = flags;
+}
+
+static void gnttab_update_entry_v2(grant_ref_t ref, domid_t domid,
+				   unsigned long frame, unsigned flags)
 {
-	/*
-	 * Introducing a valid entry into the grant table:
-	 *  1. Write ent->domid.
-	 *  2. Write ent->frame:
-	 *      GTF_permit_access:   Frame to which access is permitted.
-	 *      GTF_accept_transfer: Pseudo-phys frame slot being filled by new
-	 *                           frame, or zero if none.
-	 *  3. Write memory barrier (WMB).
-	 *  4. Write ent->flags, inc. valid type.
-	 */
-	shared[ref].frame = frame;
-	shared[ref].domid = domid;
+	gnttab_shared.v2[ref].hdr.domid = domid;
+	gnttab_shared.v2[ref].full_page.frame = frame;
 	wmb();
-	shared[ref].flags = flags;
+	gnttab_shared.v2[ref].hdr.flags = GTF_permit_access | flags;
 }
 
 /*
@@ -167,7 +264,7 @@ static void update_grant_entry(grant_ref_t ref, domid_t domid,
 void gnttab_grant_foreign_access_ref(grant_ref_t ref, domid_t domid,
 				     unsigned long frame, int readonly)
 {
-	update_grant_entry(ref, domid, frame,
+	gnttab_interface->update_entry(ref, domid, frame,
 			   GTF_permit_access | (readonly ? GTF_readonly : 0));
 }
 EXPORT_SYMBOL_GPL(gnttab_grant_foreign_access_ref);
@@ -187,31 +284,184 @@ int gnttab_grant_foreign_access(domid_t domid, unsigned long frame,
 }
 EXPORT_SYMBOL_GPL(gnttab_grant_foreign_access);
 
-int gnttab_query_foreign_access(grant_ref_t ref)
+void gnttab_update_subpage_entry_v2(grant_ref_t ref, domid_t domid,
+				    unsigned long frame, int flags,
+				    unsigned page_off,
+				    unsigned length)
+{
+	gnttab_shared.v2[ref].sub_page.frame = frame;
+	gnttab_shared.v2[ref].sub_page.page_off = page_off;
+	gnttab_shared.v2[ref].sub_page.length = length;
+	gnttab_shared.v2[ref].hdr.domid = domid;
+	wmb();
+	gnttab_shared.v2[ref].hdr.flags =
+				GTF_permit_access | GTF_sub_page | flags;
+}
+
+int gnttab_grant_foreign_access_subpage_ref(grant_ref_t ref, domid_t domid,
+					    unsigned long frame, int flags,
+					    unsigned page_off,
+					    unsigned length)
 {
-	u16 nflags;
+	if (flags & (GTF_accept_transfer | GTF_reading |
+		     GTF_writing | GTF_transitive))
+		return -EPERM;
 
-	nflags = shared[ref].flags;
+	if (gnttab_interface->update_subpage_entry == NULL)
+		return -ENOSYS;
 
-	return nflags & (GTF_reading|GTF_writing);
+	gnttab_interface->update_subpage_entry(ref, domid, frame, flags,
+					       page_off, length);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(gnttab_grant_foreign_access_subpage_ref);
+
+int gnttab_grant_foreign_access_subpage(domid_t domid, unsigned long frame,
+					int flags, unsigned page_off,
+					unsigned length)
+{
+	int ref, rc;
+
+	ref = get_free_entries(1);
+	if (unlikely(ref < 0))
+		return -ENOSPC;
+
+	rc = gnttab_grant_foreign_access_subpage_ref(ref, domid, frame, flags,
+						     page_off, length);
+	if (rc < 0) {
+		put_free_entry(ref);
+		return rc;
+	}
+
+	return ref;
+}
+EXPORT_SYMBOL_GPL(gnttab_grant_foreign_access_subpage);
+
+bool gnttab_subpage_grants_available(void)
+{
+	return gnttab_interface->update_subpage_entry != NULL;
+}
+EXPORT_SYMBOL_GPL(gnttab_subpage_grants_available);
+
+void gnttab_update_trans_entry_v2(grant_ref_t ref, domid_t domid,
+				  int flags, domid_t trans_domid,
+				  grant_ref_t trans_gref)
+{
+	gnttab_shared.v2[ref].transitive.trans_domid = trans_domid;
+	gnttab_shared.v2[ref].transitive.gref = trans_gref;
+	gnttab_shared.v2[ref].hdr.domid = domid;
+	wmb();
+	gnttab_shared.v2[ref].hdr.flags =
+				GTF_permit_access | GTF_transitive | flags;
+}
+
+int gnttab_grant_foreign_access_trans_ref(grant_ref_t ref, domid_t domid,
+					  int flags, domid_t trans_domid,
+					  grant_ref_t trans_gref)
+{
+	if (flags & (GTF_accept_transfer | GTF_reading |
+		     GTF_writing | GTF_sub_page))
+		return -EPERM;
+
+	if (gnttab_interface->update_trans_entry == NULL)
+		return -ENOSYS;
+
+	gnttab_interface->update_trans_entry(ref, domid, flags, trans_domid,
+					     trans_gref);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(gnttab_grant_foreign_access_trans_ref);
+
+int gnttab_grant_foreign_access_trans(domid_t domid, int flags,
+				      domid_t trans_domid,
+				      grant_ref_t trans_gref)
+{
+	int ref, rc;
+
+	ref = get_free_entries(1);
+	if (unlikely(ref < 0))
+		return -ENOSPC;
+
+	rc = gnttab_grant_foreign_access_trans_ref(ref, domid, flags,
+						   trans_domid, trans_gref);
+	if (rc < 0) {
+		put_free_entry(ref);
+		return rc;
+	}
+
+	return ref;
+}
+EXPORT_SYMBOL_GPL(gnttab_grant_foreign_access_trans);
+
+bool gnttab_trans_grants_available(void)
+{
+	return gnttab_interface->update_trans_entry != NULL;
+}
+EXPORT_SYMBOL_GPL(gnttab_trans_grants_available);
+
+static int gnttab_query_foreign_access_v1(grant_ref_t ref)
+{
+	return gnttab_shared.v1[ref].flags & (GTF_reading|GTF_writing);
+}
+
+static int gnttab_query_foreign_access_v2(grant_ref_t ref)
+{
+	return grstatus[ref] & (GTF_reading|GTF_writing);
+}
+
+int gnttab_query_foreign_access(grant_ref_t ref)
+{
+	return gnttab_interface->query_foreign_access(ref);
 }
 EXPORT_SYMBOL_GPL(gnttab_query_foreign_access);
 
-int gnttab_end_foreign_access_ref(grant_ref_t ref, int readonly)
+static int gnttab_end_foreign_access_ref_v1(grant_ref_t ref, int readonly)
 {
 	u16 flags, nflags;
+	u16 *pflags;
 
-	nflags = shared[ref].flags;
+	pflags = &gnttab_shared.v1[ref].flags;
+	nflags = *pflags;
 	do {
 		flags = nflags;
 		if (flags & (GTF_reading|GTF_writing)) {
 			printk(KERN_ALERT "WARNING: g.e. still in use!\n");
 			return 0;
 		}
-	} while ((nflags = sync_cmpxchg(&shared[ref].flags, flags, 0)) != flags);
+	} while ((nflags = sync_cmpxchg(pflags, flags, 0)) != flags);
+
+	return 1;
+}
+
+static int gnttab_end_foreign_access_ref_v2(grant_ref_t ref, int readonly)
+{
+	gnttab_shared.v2[ref].hdr.flags = 0;
+	mb();
+	if (grstatus[ref] & (GTF_reading|GTF_writing)) {
+		return 0;
+	} else {
+		/* The read of grstatus needs to have acquire
+		semantics.  On x86, reads already have
+		that, and we just need to protect against
+		compiler reorderings.  On other
+		architectures we may need a full
+		barrier. */
+#ifdef CONFIG_X86
+		barrier();
+#else
+		mb();
+#endif
+	}
 
 	return 1;
 }
+
+int gnttab_end_foreign_access_ref(grant_ref_t ref, int readonly)
+{
+	return gnttab_interface->end_foreign_access_ref(ref, readonly);
+}
 EXPORT_SYMBOL_GPL(gnttab_end_foreign_access_ref);
 
 void gnttab_end_foreign_access(grant_ref_t ref, int readonly,
@@ -246,37 +496,76 @@ EXPORT_SYMBOL_GPL(gnttab_grant_foreign_transfer);
 void gnttab_grant_foreign_transfer_ref(grant_ref_t ref, domid_t domid,
 				       unsigned long pfn)
 {
-	update_grant_entry(ref, domid, pfn, GTF_accept_transfer);
+	gnttab_interface->update_entry(ref, domid, pfn, GTF_accept_transfer);
 }
 EXPORT_SYMBOL_GPL(gnttab_grant_foreign_transfer_ref);
 
-unsigned long gnttab_end_foreign_transfer_ref(grant_ref_t ref)
+static unsigned long gnttab_end_foreign_transfer_ref_v1(grant_ref_t ref)
 {
 	unsigned long frame;
 	u16           flags;
+	u16          *pflags;
+
+	pflags = &gnttab_shared.v1[ref].flags;
 
 	/*
 	 * If a transfer is not even yet started, try to reclaim the grant
 	 * reference and return failure (== 0).
 	 */
-	while (!((flags = shared[ref].flags) & GTF_transfer_committed)) {
-		if (sync_cmpxchg(&shared[ref].flags, flags, 0) == flags)
+	while (!((flags = *pflags) & GTF_transfer_committed)) {
+		if (sync_cmpxchg(pflags, flags, 0) == flags)
 			return 0;
 		cpu_relax();
 	}
 
 	/* If a transfer is in progress then wait until it is completed. */
 	while (!(flags & GTF_transfer_completed)) {
-		flags = shared[ref].flags;
+		flags = *pflags;
 		cpu_relax();
 	}
 
 	rmb();	/* Read the frame number /after/ reading completion status. */
-	frame = shared[ref].frame;
+	frame = gnttab_shared.v1[ref].frame;
+	BUG_ON(frame == 0);
+
+	return frame;
+}
+
+static unsigned long gnttab_end_foreign_transfer_ref_v2(grant_ref_t ref)
+{
+	unsigned long frame;
+	u16           flags;
+	u16          *pflags;
+
+	pflags = &gnttab_shared.v2[ref].hdr.flags;
+
+	/*
+	 * If a transfer is not even yet started, try to reclaim the grant
+	 * reference and return failure (== 0).
+	 */
+	while (!((flags = *pflags) & GTF_transfer_committed)) {
+		if (sync_cmpxchg(pflags, flags, 0) == flags)
+			return 0;
+		cpu_relax();
+	}
+
+	/* If a transfer is in progress then wait until it is completed. */
+	while (!(flags & GTF_transfer_completed)) {
+		flags = *pflags;
+		cpu_relax();
+	}
+
+	rmb();  /* Read the frame number /after/ reading completion status. */
+	frame = gnttab_shared.v2[ref].full_page.frame;
 	BUG_ON(frame == 0);
 
 	return frame;
 }
+
+unsigned long gnttab_end_foreign_transfer_ref(grant_ref_t ref)
+{
+	return gnttab_interface->end_foreign_transfer_ref(ref);
+}
 EXPORT_SYMBOL_GPL(gnttab_end_foreign_transfer_ref);
 
 unsigned long gnttab_end_foreign_transfer(grant_ref_t ref)
@@ -448,8 +737,8 @@ unsigned int gnttab_max_grant_frames(void)
 EXPORT_SYMBOL_GPL(gnttab_max_grant_frames);
 
 int gnttab_map_refs(struct gnttab_map_grant_ref *map_ops,
-			struct gnttab_map_grant_ref *kmap_ops,
-			struct page **pages, unsigned int count)
+		    struct gnttab_map_grant_ref *kmap_ops,
+		    struct page **pages, unsigned int count)
 {
 	int i, ret;
 	pte_t *pte;
@@ -472,24 +761,10 @@ int gnttab_map_refs(struct gnttab_map_grant_ref *map_ops,
 				(map_ops[i].host_addr & ~PAGE_MASK));
 			mfn = pte_mfn(*pte);
 		} else {
-			/* If you really wanted to do this:
-			 * mfn = PFN_DOWN(map_ops[i].dev_bus_addr);
-			 *
-			 * The reason we do not implement it is b/c on the
-			 * unmap path (gnttab_unmap_refs) we have no means of
-			 * checking whether the page is !GNTMAP_contains_pte.
-			 *
-			 * That is without some extra data-structure to carry
-			 * the struct page, bool clear_pte, and list_head next
-			 * tuples and deal with allocation/delallocation, etc.
-			 *
-			 * The users of this API set the GNTMAP_contains_pte
-			 * flag so lets just return not supported until it
-			 * becomes neccessary to implement.
-			 */
-			return -EOPNOTSUPP;
+			mfn = PFN_DOWN(map_ops[i].dev_bus_addr);
 		}
-		ret = m2p_add_override(mfn, pages[i], &kmap_ops[i]);
+		ret = m2p_add_override(mfn, pages[i], kmap_ops ?
+				       &kmap_ops[i] : NULL);
 		if (ret)
 			return ret;
 	}
@@ -499,7 +774,7 @@ int gnttab_map_refs(struct gnttab_map_grant_ref *map_ops,
 EXPORT_SYMBOL_GPL(gnttab_map_refs);
 
 int gnttab_unmap_refs(struct gnttab_unmap_grant_ref *unmap_ops,
-		struct page **pages, unsigned int count)
+		      struct page **pages, unsigned int count, bool clear_pte)
 {
 	int i, ret;
 
@@ -511,7 +786,7 @@ int gnttab_unmap_refs(struct gnttab_unmap_grant_ref *unmap_ops,
 		return ret;
 
 	for (i = 0; i < count; i++) {
-		ret = m2p_remove_override(pages[i], true /* clear the PTE */);
+		ret = m2p_remove_override(pages[i], clear_pte);
 		if (ret)
 			return ret;
 	}
@@ -520,6 +795,77 @@ int gnttab_unmap_refs(struct gnttab_unmap_grant_ref *unmap_ops,
 }
 EXPORT_SYMBOL_GPL(gnttab_unmap_refs);
 
+static unsigned nr_status_frames(unsigned nr_grant_frames)
+{
+	return (nr_grant_frames * GREFS_PER_GRANT_FRAME + SPP - 1) / SPP;
+}
+
+static int gnttab_map_frames_v1(unsigned long *frames, unsigned int nr_gframes)
+{
+	int rc;
+
+	rc = arch_gnttab_map_shared(frames, nr_gframes,
+				    gnttab_max_grant_frames(),
+				    &gnttab_shared.addr);
+	BUG_ON(rc);
+
+	return 0;
+}
+
+static void gnttab_unmap_frames_v1(void)
+{
+	arch_gnttab_unmap(gnttab_shared.addr, nr_grant_frames);
+}
+
+static int gnttab_map_frames_v2(unsigned long *frames, unsigned int nr_gframes)
+{
+	uint64_t *sframes;
+	unsigned int nr_sframes;
+	struct gnttab_get_status_frames getframes;
+	int rc;
+
+	nr_sframes = nr_status_frames(nr_gframes);
+
+	/* No need for kzalloc as it is initialized in following hypercall
+	 * GNTTABOP_get_status_frames.
+	 */
+	sframes = kmalloc(nr_sframes  * sizeof(uint64_t), GFP_ATOMIC);
+	if (!sframes)
+		return -ENOMEM;
+
+	getframes.dom        = DOMID_SELF;
+	getframes.nr_frames  = nr_sframes;
+	set_xen_guest_handle(getframes.frame_list, sframes);
+
+	rc = HYPERVISOR_grant_table_op(GNTTABOP_get_status_frames,
+				       &getframes, 1);
+	if (rc == -ENOSYS) {
+		kfree(sframes);
+		return -ENOSYS;
+	}
+
+	BUG_ON(rc || getframes.status);
+
+	rc = arch_gnttab_map_status(sframes, nr_sframes,
+				    nr_status_frames(gnttab_max_grant_frames()),
+				    &grstatus);
+	BUG_ON(rc);
+	kfree(sframes);
+
+	rc = arch_gnttab_map_shared(frames, nr_gframes,
+				    gnttab_max_grant_frames(),
+				    &gnttab_shared.addr);
+	BUG_ON(rc);
+
+	return 0;
+}
+
+static void gnttab_unmap_frames_v2(void)
+{
+	arch_gnttab_unmap(gnttab_shared.addr, nr_grant_frames);
+	arch_gnttab_unmap(grstatus, nr_status_frames(nr_grant_frames));
+}
+
 static int gnttab_map(unsigned int start_idx, unsigned int end_idx)
 {
 	struct gnttab_setup_table setup;
@@ -551,6 +897,9 @@ static int gnttab_map(unsigned int start_idx, unsigned int end_idx)
 		return rc;
 	}
 
+	/* No need for kzalloc as it is initialized in following hypercall
+	 * GNTTABOP_setup_table.
+	 */
 	frames = kmalloc(nr_gframes * sizeof(unsigned long), GFP_ATOMIC);
 	if (!frames)
 		return -ENOMEM;
@@ -567,19 +916,65 @@ static int gnttab_map(unsigned int start_idx, unsigned int end_idx)
 
 	BUG_ON(rc || setup.status);
 
-	rc = arch_gnttab_map_shared(frames, nr_gframes, gnttab_max_grant_frames(),
-				    &shared);
-	BUG_ON(rc);
+	rc = gnttab_interface->map_frames(frames, nr_gframes);
 
 	kfree(frames);
 
-	return 0;
+	return rc;
+}
+
+static struct gnttab_ops gnttab_v1_ops = {
+	.map_frames			= gnttab_map_frames_v1,
+	.unmap_frames			= gnttab_unmap_frames_v1,
+	.update_entry			= gnttab_update_entry_v1,
+	.end_foreign_access_ref		= gnttab_end_foreign_access_ref_v1,
+	.end_foreign_transfer_ref	= gnttab_end_foreign_transfer_ref_v1,
+	.query_foreign_access		= gnttab_query_foreign_access_v1,
+};
+
+static struct gnttab_ops gnttab_v2_ops = {
+	.map_frames			= gnttab_map_frames_v2,
+	.unmap_frames			= gnttab_unmap_frames_v2,
+	.update_entry			= gnttab_update_entry_v2,
+	.end_foreign_access_ref		= gnttab_end_foreign_access_ref_v2,
+	.end_foreign_transfer_ref	= gnttab_end_foreign_transfer_ref_v2,
+	.query_foreign_access		= gnttab_query_foreign_access_v2,
+	.update_subpage_entry		= gnttab_update_subpage_entry_v2,
+	.update_trans_entry		= gnttab_update_trans_entry_v2,
+};
+
+static void gnttab_request_version(void)
+{
+	int rc;
+	struct gnttab_set_version gsv;
+
+	gsv.version = 2;
+	rc = HYPERVISOR_grant_table_op(GNTTABOP_set_version, &gsv, 1);
+	if (rc == 0) {
+		grant_table_version = 2;
+		gnttab_interface = &gnttab_v2_ops;
+	} else if (grant_table_version == 2) {
+		/*
+		 * If we've already used version 2 features,
+		 * but then suddenly discover that they're not
+		 * available (e.g. migrating to an older
+		 * version of Xen), almost unbounded badness
+		 * can happen.
+		 */
+		panic("we need grant tables version 2, but only version 1 is available");
+	} else {
+		grant_table_version = 1;
+		gnttab_interface = &gnttab_v1_ops;
+	}
+	printk(KERN_INFO "Grant tables using version %d layout.\n",
+		grant_table_version);
 }
 
 int gnttab_resume(void)
 {
 	unsigned int max_nr_gframes;
 
+	gnttab_request_version();
 	max_nr_gframes = gnttab_max_grant_frames();
 	if (max_nr_gframes < nr_grant_frames)
 		return -ENOSYS;
@@ -587,9 +982,10 @@ int gnttab_resume(void)
 	if (xen_pv_domain())
 		return gnttab_map(0, nr_grant_frames - 1);
 
-	if (!shared) {
-		shared = ioremap(xen_hvm_resume_frames, PAGE_SIZE * max_nr_gframes);
-		if (shared == NULL) {
+	if (gnttab_shared.addr == NULL) {
+		gnttab_shared.addr = ioremap(xen_hvm_resume_frames,
+						PAGE_SIZE * max_nr_gframes);
+		if (gnttab_shared.addr == NULL) {
 			printk(KERN_WARNING
 					"Failed to ioremap gnttab share frames!");
 			return -ENOMEM;
@@ -603,7 +999,7 @@ int gnttab_resume(void)
 
 int gnttab_suspend(void)
 {
-	arch_gnttab_unmap_shared(shared, nr_grant_frames);
+	gnttab_interface->unmap_frames();
 	return 0;
 }
 
diff --git a/drivers/xen/xenfs/privcmd.c b/drivers/xen/privcmd.c
index dbd3b16fd131..ccee0f16bcf8 100644
--- a/drivers/xen/xenfs/privcmd.c
+++ b/drivers/xen/privcmd.c
@@ -7,6 +7,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/string.h>
@@ -18,6 +19,7 @@
 #include <linux/highmem.h>
 #include <linux/pagemap.h>
 #include <linux/seq_file.h>
+#include <linux/miscdevice.h>
 
 #include <asm/pgalloc.h>
 #include <asm/pgtable.h>
@@ -32,6 +34,10 @@
 #include <xen/page.h>
 #include <xen/xen-ops.h>
 
+#include "privcmd.h"
+
+MODULE_LICENSE("GPL");
+
 #ifndef HAVE_ARCH_PRIVCMD_MMAP
 static int privcmd_enforce_singleshot_mapping(struct vm_area_struct *vma);
 #endif
@@ -359,7 +365,6 @@ static long privcmd_ioctl(struct file *file,
 	return ret;
 }
 
-#ifndef HAVE_ARCH_PRIVCMD_MMAP
 static int privcmd_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 {
 	printk(KERN_DEBUG "privcmd_fault: vma=%p %lx-%lx, pgoff=%lx, uv=%p\n",
@@ -392,9 +397,39 @@ static int privcmd_enforce_singleshot_mapping(struct vm_area_struct *vma)
 {
 	return (xchg(&vma->vm_private_data, (void *)1) == NULL);
 }
-#endif
 
-const struct file_operations privcmd_file_ops = {
+const struct file_operations xen_privcmd_fops = {
+	.owner = THIS_MODULE,
 	.unlocked_ioctl = privcmd_ioctl,
 	.mmap = privcmd_mmap,
 };
+EXPORT_SYMBOL_GPL(xen_privcmd_fops);
+
+static struct miscdevice privcmd_dev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "xen/privcmd",
+	.fops = &xen_privcmd_fops,
+};
+
+static int __init privcmd_init(void)
+{
+	int err;
+
+	if (!xen_domain())
+		return -ENODEV;
+
+	err = misc_register(&privcmd_dev);
+	if (err != 0) {
+		printk(KERN_ERR "Could not register Xen privcmd device\n");
+		return err;
+	}
+	return 0;
+}
+
+static void __exit privcmd_exit(void)
+{
+	misc_deregister(&privcmd_dev);
+}
+
+module_init(privcmd_init);
+module_exit(privcmd_exit);
diff --git a/drivers/xen/privcmd.h b/drivers/xen/privcmd.h
new file mode 100644
index 000000000000..14facaeed36f
--- /dev/null
+++ b/drivers/xen/privcmd.h
@@ -0,0 +1,3 @@
+#include <linux/fs.h>
+
+extern const struct file_operations xen_privcmd_fops;
diff --git a/drivers/xen/swiotlb-xen.c b/drivers/xen/swiotlb-xen.c
index 284798aaf8b1..19e6a2041371 100644
--- a/drivers/xen/swiotlb-xen.c
+++ b/drivers/xen/swiotlb-xen.c
@@ -153,7 +153,7 @@ void __init xen_swiotlb_init(int verbose)
 	char *m = NULL;
 	unsigned int repeat = 3;
 
-	nr_tbl = swioltb_nr_tbl();
+	nr_tbl = swiotlb_nr_tbl();
 	if (nr_tbl)
 		xen_io_tlb_nslabs = nr_tbl;
 	else {
diff --git a/drivers/xen/xen-pciback/pci_stub.c b/drivers/xen/xen-pciback/pci_stub.c
index 8f06e1ed028c..7944a17f5cbf 100644
--- a/drivers/xen/xen-pciback/pci_stub.c
+++ b/drivers/xen/xen-pciback/pci_stub.c
@@ -99,6 +99,7 @@ static void pcistub_device_release(struct kref *kref)
 	kfree(pci_get_drvdata(psdev->dev));
 	pci_set_drvdata(psdev->dev, NULL);
 
+	psdev->dev->dev_flags &= ~PCI_DEV_FLAGS_ASSIGNED;
 	pci_dev_put(psdev->dev);
 
 	kfree(psdev);
@@ -234,6 +235,8 @@ void pcistub_put_pci_dev(struct pci_dev *dev)
 	xen_pcibk_config_free_dyn_fields(found_psdev->dev);
 	xen_pcibk_config_reset_dev(found_psdev->dev);
 
+	xen_unregister_device_domain_owner(found_psdev->dev);
+
 	spin_lock_irqsave(&found_psdev->lock, flags);
 	found_psdev->pdev = NULL;
 	spin_unlock_irqrestore(&found_psdev->lock, flags);
@@ -331,6 +334,7 @@ static int __devinit pcistub_init_device(struct pci_dev *dev)
 	dev_dbg(&dev->dev, "reset device\n");
 	xen_pcibk_reset_device(dev);
 
+	dev->dev_flags |= PCI_DEV_FLAGS_ASSIGNED;
 	return 0;
 
 config_release:
diff --git a/drivers/xen/xen-pciback/xenbus.c b/drivers/xen/xen-pciback/xenbus.c
index 075525945e36..8e1c44d8ab46 100644
--- a/drivers/xen/xen-pciback/xenbus.c
+++ b/drivers/xen/xen-pciback/xenbus.c
@@ -241,11 +241,10 @@ static int xen_pcibk_export_device(struct xen_pcibk_device *pdev,
 		goto out;
 
 	dev_dbg(&dev->dev, "registering for %d\n", pdev->xdev->otherend_id);
-	dev->dev_flags |= PCI_DEV_FLAGS_ASSIGNED;
 	if (xen_register_device_domain_owner(dev,
 					     pdev->xdev->otherend_id) != 0) {
-		dev_err(&dev->dev, "device has been assigned to another " \
-			"domain! Over-writting the ownership, but beware.\n");
+		dev_err(&dev->dev, "Stealing ownership from dom%d.\n",
+			xen_find_device_domain_owner(dev));
 		xen_unregister_device_domain_owner(dev);
 		xen_register_device_domain_owner(dev, pdev->xdev->otherend_id);
 	}
@@ -281,7 +280,6 @@ static int xen_pcibk_remove_device(struct xen_pcibk_device *pdev,
 	}
 
 	dev_dbg(&dev->dev, "unregistering for %d\n", pdev->xdev->otherend_id);
-	dev->dev_flags &= ~PCI_DEV_FLAGS_ASSIGNED;
 	xen_unregister_device_domain_owner(dev);
 
 	xen_pcibk_release_pci_dev(pdev, dev);
@@ -707,19 +705,16 @@ static int xen_pcibk_xenbus_remove(struct xenbus_device *dev)
 	return 0;
 }
 
-static const struct xenbus_device_id xenpci_ids[] = {
+static const struct xenbus_device_id xen_pcibk_ids[] = {
 	{"pci"},
 	{""},
 };
 
-static struct xenbus_driver xenbus_xen_pcibk_driver = {
-	.name			= DRV_NAME,
-	.owner			= THIS_MODULE,
-	.ids			= xenpci_ids,
+static DEFINE_XENBUS_DRIVER(xen_pcibk, DRV_NAME,
 	.probe			= xen_pcibk_xenbus_probe,
 	.remove			= xen_pcibk_xenbus_remove,
 	.otherend_changed	= xen_pcibk_frontend_changed,
-};
+);
 
 const struct xen_pcibk_backend *__read_mostly xen_pcibk_backend;
 
@@ -735,11 +730,11 @@ int __init xen_pcibk_xenbus_register(void)
 	if (passthrough)
 		xen_pcibk_backend = &xen_pcibk_passthrough_backend;
 	pr_info(DRV_NAME ": backend is %s\n", xen_pcibk_backend->name);
-	return xenbus_register_backend(&xenbus_xen_pcibk_driver);
+	return xenbus_register_backend(&xen_pcibk_driver);
 }
 
 void __exit xen_pcibk_xenbus_unregister(void)
 {
 	destroy_workqueue(xen_pcibk_wq);
-	xenbus_unregister_driver(&xenbus_xen_pcibk_driver);
+	xenbus_unregister_driver(&xen_pcibk_driver);
 }
diff --git a/drivers/xen/xenbus/Makefile b/drivers/xen/xenbus/Makefile
index 8dca685358b4..31e2e9050c7a 100644
--- a/drivers/xen/xenbus/Makefile
+++ b/drivers/xen/xenbus/Makefile
@@ -1,4 +1,5 @@
 obj-y	+= xenbus.o
+obj-y	+= xenbus_dev_frontend.o
 
 xenbus-objs =
 xenbus-objs += xenbus_client.o
@@ -9,4 +10,5 @@ xenbus-objs += xenbus_probe.o
 xenbus-be-objs-$(CONFIG_XEN_BACKEND) += xenbus_probe_backend.o
 xenbus-objs += $(xenbus-be-objs-y)
 
+obj-$(CONFIG_XEN_BACKEND) += xenbus_dev_backend.o
 obj-$(CONFIG_XEN_XENBUS_FRONTEND) += xenbus_probe_frontend.o
diff --git a/drivers/xen/xenbus/xenbus_client.c b/drivers/xen/xenbus/xenbus_client.c
index 1906125eab49..566d2adbd6ea 100644
--- a/drivers/xen/xenbus/xenbus_client.c
+++ b/drivers/xen/xenbus/xenbus_client.c
@@ -32,15 +32,39 @@
 
 #include <linux/slab.h>
 #include <linux/types.h>
+#include <linux/spinlock.h>
 #include <linux/vmalloc.h>
 #include <linux/export.h>
 #include <asm/xen/hypervisor.h>
 #include <asm/xen/page.h>
 #include <xen/interface/xen.h>
 #include <xen/interface/event_channel.h>
+#include <xen/balloon.h>
 #include <xen/events.h>
 #include <xen/grant_table.h>
 #include <xen/xenbus.h>
+#include <xen/xen.h>
+
+#include "xenbus_probe.h"
+
+struct xenbus_map_node {
+	struct list_head next;
+	union {
+		struct vm_struct *area; /* PV */
+		struct page *page;     /* HVM */
+	};
+	grant_handle_t handle;
+};
+
+static DEFINE_SPINLOCK(xenbus_valloc_lock);
+static LIST_HEAD(xenbus_valloc_pages);
+
+struct xenbus_ring_ops {
+	int (*map)(struct xenbus_device *dev, int gnt, void **vaddr);
+	int (*unmap)(struct xenbus_device *dev, void *vaddr);
+};
+
+static const struct xenbus_ring_ops *ring_ops __read_mostly;
 
 const char *xenbus_strstate(enum xenbus_state state)
 {
@@ -436,19 +460,33 @@ EXPORT_SYMBOL_GPL(xenbus_free_evtchn);
  */
 int xenbus_map_ring_valloc(struct xenbus_device *dev, int gnt_ref, void **vaddr)
 {
+	return ring_ops->map(dev, gnt_ref, vaddr);
+}
+EXPORT_SYMBOL_GPL(xenbus_map_ring_valloc);
+
+static int xenbus_map_ring_valloc_pv(struct xenbus_device *dev,
+				     int gnt_ref, void **vaddr)
+{
 	struct gnttab_map_grant_ref op = {
 		.flags = GNTMAP_host_map | GNTMAP_contains_pte,
 		.ref   = gnt_ref,
 		.dom   = dev->otherend_id,
 	};
+	struct xenbus_map_node *node;
 	struct vm_struct *area;
 	pte_t *pte;
 
 	*vaddr = NULL;
 
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (!node)
+		return -ENOMEM;
+
 	area = alloc_vm_area(PAGE_SIZE, &pte);
-	if (!area)
+	if (!area) {
+		kfree(node);
 		return -ENOMEM;
+	}
 
 	op.host_addr = arbitrary_virt_to_machine(pte).maddr;
 
@@ -457,19 +495,59 @@ int xenbus_map_ring_valloc(struct xenbus_device *dev, int gnt_ref, void **vaddr)
 
 	if (op.status != GNTST_okay) {
 		free_vm_area(area);
+		kfree(node);
 		xenbus_dev_fatal(dev, op.status,
 				 "mapping in shared page %d from domain %d",
 				 gnt_ref, dev->otherend_id);
 		return op.status;
 	}
 
-	/* Stuff the handle in an unused field */
-	area->phys_addr = (unsigned long)op.handle;
+	node->handle = op.handle;
+	node->area = area;
+
+	spin_lock(&xenbus_valloc_lock);
+	list_add(&node->next, &xenbus_valloc_pages);
+	spin_unlock(&xenbus_valloc_lock);
 
 	*vaddr = area->addr;
 	return 0;
 }
-EXPORT_SYMBOL_GPL(xenbus_map_ring_valloc);
+
+static int xenbus_map_ring_valloc_hvm(struct xenbus_device *dev,
+				      int gnt_ref, void **vaddr)
+{
+	struct xenbus_map_node *node;
+	int err;
+	void *addr;
+
+	*vaddr = NULL;
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (!node)
+		return -ENOMEM;
+
+	err = alloc_xenballooned_pages(1, &node->page, false /* lowmem */);
+	if (err)
+		goto out_err;
+
+	addr = pfn_to_kaddr(page_to_pfn(node->page));
+
+	err = xenbus_map_ring(dev, gnt_ref, &node->handle, addr);
+	if (err)
+		goto out_err;
+
+	spin_lock(&xenbus_valloc_lock);
+	list_add(&node->next, &xenbus_valloc_pages);
+	spin_unlock(&xenbus_valloc_lock);
+
+	*vaddr = addr;
+	return 0;
+
+ out_err:
+	free_xenballooned_pages(1, &node->page);
+	kfree(node);
+	return err;
+}
 
 
 /**
@@ -489,12 +567,10 @@ EXPORT_SYMBOL_GPL(xenbus_map_ring_valloc);
 int xenbus_map_ring(struct xenbus_device *dev, int gnt_ref,
 		    grant_handle_t *handle, void *vaddr)
 {
-	struct gnttab_map_grant_ref op = {
-		.host_addr = (unsigned long)vaddr,
-		.flags     = GNTMAP_host_map,
-		.ref       = gnt_ref,
-		.dom       = dev->otherend_id,
-	};
+	struct gnttab_map_grant_ref op;
+
+	gnttab_set_map_op(&op, (phys_addr_t)vaddr, GNTMAP_host_map, gnt_ref,
+			  dev->otherend_id);
 
 	if (HYPERVISOR_grant_table_op(GNTTABOP_map_grant_ref, &op, 1))
 		BUG();
@@ -525,32 +601,36 @@ EXPORT_SYMBOL_GPL(xenbus_map_ring);
  */
 int xenbus_unmap_ring_vfree(struct xenbus_device *dev, void *vaddr)
 {
-	struct vm_struct *area;
+	return ring_ops->unmap(dev, vaddr);
+}
+EXPORT_SYMBOL_GPL(xenbus_unmap_ring_vfree);
+
+static int xenbus_unmap_ring_vfree_pv(struct xenbus_device *dev, void *vaddr)
+{
+	struct xenbus_map_node *node;
 	struct gnttab_unmap_grant_ref op = {
 		.host_addr = (unsigned long)vaddr,
 	};
 	unsigned int level;
 
-	/* It'd be nice if linux/vmalloc.h provided a find_vm_area(void *addr)
-	 * method so that we don't have to muck with vmalloc internals here.
-	 * We could force the user to hang on to their struct vm_struct from
-	 * xenbus_map_ring_valloc, but these 6 lines considerably simplify
-	 * this API.
-	 */
-	read_lock(&vmlist_lock);
-	for (area = vmlist; area != NULL; area = area->next) {
-		if (area->addr == vaddr)
-			break;
+	spin_lock(&xenbus_valloc_lock);
+	list_for_each_entry(node, &xenbus_valloc_pages, next) {
+		if (node->area->addr == vaddr) {
+			list_del(&node->next);
+			goto found;
+		}
 	}
-	read_unlock(&vmlist_lock);
+	node = NULL;
+ found:
+	spin_unlock(&xenbus_valloc_lock);
 
-	if (!area) {
+	if (!node) {
 		xenbus_dev_error(dev, -ENOENT,
 				 "can't find mapped virtual address %p", vaddr);
 		return GNTST_bad_virt_addr;
 	}
 
-	op.handle = (grant_handle_t)area->phys_addr;
+	op.handle = node->handle;
 	op.host_addr = arbitrary_virt_to_machine(
 		lookup_address((unsigned long)vaddr, &level)).maddr;
 
@@ -558,16 +638,50 @@ int xenbus_unmap_ring_vfree(struct xenbus_device *dev, void *vaddr)
 		BUG();
 
 	if (op.status == GNTST_okay)
-		free_vm_area(area);
+		free_vm_area(node->area);
 	else
 		xenbus_dev_error(dev, op.status,
 				 "unmapping page at handle %d error %d",
-				 (int16_t)area->phys_addr, op.status);
+				 node->handle, op.status);
 
+	kfree(node);
 	return op.status;
 }
-EXPORT_SYMBOL_GPL(xenbus_unmap_ring_vfree);
 
+static int xenbus_unmap_ring_vfree_hvm(struct xenbus_device *dev, void *vaddr)
+{
+	int rv;
+	struct xenbus_map_node *node;
+	void *addr;
+
+	spin_lock(&xenbus_valloc_lock);
+	list_for_each_entry(node, &xenbus_valloc_pages, next) {
+		addr = pfn_to_kaddr(page_to_pfn(node->page));
+		if (addr == vaddr) {
+			list_del(&node->next);
+			goto found;
+		}
+	}
+	node = NULL;
+ found:
+	spin_unlock(&xenbus_valloc_lock);
+
+	if (!node) {
+		xenbus_dev_error(dev, -ENOENT,
+				 "can't find mapped virtual address %p", vaddr);
+		return GNTST_bad_virt_addr;
+	}
+
+	rv = xenbus_unmap_ring(dev, node->handle, addr);
+
+	if (!rv)
+		free_xenballooned_pages(1, &node->page);
+	else
+		WARN(1, "Leaking %p\n", vaddr);
+
+	kfree(node);
+	return rv;
+}
 
 /**
  * xenbus_unmap_ring
@@ -582,10 +696,9 @@ EXPORT_SYMBOL_GPL(xenbus_unmap_ring_vfree);
 int xenbus_unmap_ring(struct xenbus_device *dev,
 		      grant_handle_t handle, void *vaddr)
 {
-	struct gnttab_unmap_grant_ref op = {
-		.host_addr = (unsigned long)vaddr,
-		.handle    = handle,
-	};
+	struct gnttab_unmap_grant_ref op;
+
+	gnttab_set_unmap_op(&op, (phys_addr_t)vaddr, GNTMAP_host_map, handle);
 
 	if (HYPERVISOR_grant_table_op(GNTTABOP_unmap_grant_ref, &op, 1))
 		BUG();
@@ -617,3 +730,21 @@ enum xenbus_state xenbus_read_driver_state(const char *path)
 	return result;
 }
 EXPORT_SYMBOL_GPL(xenbus_read_driver_state);
+
+static const struct xenbus_ring_ops ring_ops_pv = {
+	.map = xenbus_map_ring_valloc_pv,
+	.unmap = xenbus_unmap_ring_vfree_pv,
+};
+
+static const struct xenbus_ring_ops ring_ops_hvm = {
+	.map = xenbus_map_ring_valloc_hvm,
+	.unmap = xenbus_unmap_ring_vfree_hvm,
+};
+
+void __init xenbus_ring_ops_init(void)
+{
+	if (xen_pv_domain())
+		ring_ops = &ring_ops_pv;
+	else
+		ring_ops = &ring_ops_hvm;
+}
diff --git a/drivers/xen/xenbus/xenbus_comms.h b/drivers/xen/xenbus/xenbus_comms.h
index c21db7513736..6e42800fa499 100644
--- a/drivers/xen/xenbus/xenbus_comms.h
+++ b/drivers/xen/xenbus/xenbus_comms.h
@@ -31,6 +31,8 @@
 #ifndef _XENBUS_COMMS_H
 #define _XENBUS_COMMS_H
 
+#include <linux/fs.h>
+
 int xs_init(void);
 int xb_init_comms(void);
 
@@ -43,4 +45,6 @@ int xs_input_avail(void);
 extern struct xenstore_domain_interface *xen_store_interface;
 extern int xen_store_evtchn;
 
+extern const struct file_operations xen_xenbus_fops;
+
 #endif /* _XENBUS_COMMS_H */
diff --git a/drivers/xen/xenbus/xenbus_dev_backend.c b/drivers/xen/xenbus/xenbus_dev_backend.c
new file mode 100644
index 000000000000..3d3be78c1093
--- /dev/null
+++ b/drivers/xen/xenbus/xenbus_dev_backend.c
@@ -0,0 +1,90 @@
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/capability.h>
+
+#include <xen/xen.h>
+#include <xen/page.h>
+#include <xen/xenbus_dev.h>
+
+#include "xenbus_comms.h"
+
+MODULE_LICENSE("GPL");
+
+static int xenbus_backend_open(struct inode *inode, struct file *filp)
+{
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	return nonseekable_open(inode, filp);
+}
+
+static long xenbus_backend_ioctl(struct file *file, unsigned int cmd, unsigned long data)
+{
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	switch (cmd) {
+		case IOCTL_XENBUS_BACKEND_EVTCHN:
+			if (xen_store_evtchn > 0)
+				return xen_store_evtchn;
+			return -ENODEV;
+
+		default:
+			return -ENOTTY;
+	}
+}
+
+static int xenbus_backend_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	size_t size = vma->vm_end - vma->vm_start;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if ((size > PAGE_SIZE) || (vma->vm_pgoff != 0))
+		return -EINVAL;
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    virt_to_pfn(xen_store_interface),
+			    size, vma->vm_page_prot))
+		return -EAGAIN;
+
+	return 0;
+}
+
+const struct file_operations xenbus_backend_fops = {
+	.open = xenbus_backend_open,
+	.mmap = xenbus_backend_mmap,
+	.unlocked_ioctl = xenbus_backend_ioctl,
+};
+
+static struct miscdevice xenbus_backend_dev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "xen/xenbus_backend",
+	.fops = &xenbus_backend_fops,
+};
+
+static int __init xenbus_backend_init(void)
+{
+	int err;
+
+	if (!xen_initial_domain())
+		return -ENODEV;
+
+	err = misc_register(&xenbus_backend_dev);
+	if (err)
+		printk(KERN_ERR "Could not register xenbus backend device\n");
+	return err;
+}
+
+static void __exit xenbus_backend_exit(void)
+{
+	misc_deregister(&xenbus_backend_dev);
+}
+
+module_init(xenbus_backend_init);
+module_exit(xenbus_backend_exit);
diff --git a/drivers/xen/xenfs/xenbus.c b/drivers/xen/xenbus/xenbus_dev_frontend.c
index bbd000f88af7..527dc2a3b89f 100644
--- a/drivers/xen/xenfs/xenbus.c
+++ b/drivers/xen/xenbus/xenbus_dev_frontend.c
@@ -52,13 +52,17 @@
 #include <linux/namei.h>
 #include <linux/string.h>
 #include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
 
-#include "xenfs.h"
-#include "../xenbus/xenbus_comms.h"
+#include "xenbus_comms.h"
 
 #include <xen/xenbus.h>
+#include <xen/xen.h>
 #include <asm/xen/hypervisor.h>
 
+MODULE_LICENSE("GPL");
+
 /*
  * An element of a list of outstanding transactions, for which we're
  * still waiting a reply.
@@ -101,7 +105,7 @@ struct xenbus_file_priv {
 	unsigned int len;
 	union {
 		struct xsd_sockmsg msg;
-		char buffer[PAGE_SIZE];
+		char buffer[XENSTORE_PAYLOAD_MAX];
 	} u;
 
 	/* Response queue. */
@@ -583,7 +587,7 @@ static unsigned int xenbus_file_poll(struct file *file, poll_table *wait)
 	return 0;
 }
 
-const struct file_operations xenbus_file_ops = {
+const struct file_operations xen_xenbus_fops = {
 	.read = xenbus_file_read,
 	.write = xenbus_file_write,
 	.open = xenbus_file_open,
@@ -591,3 +595,31 @@ const struct file_operations xenbus_file_ops = {
 	.poll = xenbus_file_poll,
 	.llseek = no_llseek,
 };
+EXPORT_SYMBOL_GPL(xen_xenbus_fops);
+
+static struct miscdevice xenbus_dev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "xen/xenbus",
+	.fops = &xen_xenbus_fops,
+};
+
+static int __init xenbus_init(void)
+{
+	int err;
+
+	if (!xen_domain())
+		return -ENODEV;
+
+	err = misc_register(&xenbus_dev);
+	if (err)
+		printk(KERN_ERR "Could not register xenbus frontend device\n");
+	return err;
+}
+
+static void __exit xenbus_exit(void)
+{
+	misc_deregister(&xenbus_dev);
+}
+
+module_init(xenbus_init);
+module_exit(xenbus_exit);
diff --git a/drivers/xen/xenbus/xenbus_probe.c b/drivers/xen/xenbus/xenbus_probe.c
index 1b178c6e8937..3864967202b5 100644
--- a/drivers/xen/xenbus/xenbus_probe.c
+++ b/drivers/xen/xenbus/xenbus_probe.c
@@ -291,14 +291,9 @@ void xenbus_dev_shutdown(struct device *_dev)
 EXPORT_SYMBOL_GPL(xenbus_dev_shutdown);
 
 int xenbus_register_driver_common(struct xenbus_driver *drv,
-				  struct xen_bus_type *bus,
-				  struct module *owner,
-				  const char *mod_name)
+				  struct xen_bus_type *bus)
 {
-	drv->driver.name = drv->name;
 	drv->driver.bus = &bus->bus;
-	drv->driver.owner = owner;
-	drv->driver.mod_name = mod_name;
 
 	return driver_register(&drv->driver);
 }
@@ -730,6 +725,8 @@ static int __init xenbus_init(void)
 	if (!xen_domain())
 		return -ENODEV;
 
+	xenbus_ring_ops_init();
+
 	if (xen_hvm_domain()) {
 		uint64_t v = 0;
 		err = hvm_get_parameter(HVM_PARAM_STORE_EVTCHN, &v);
diff --git a/drivers/xen/xenbus/xenbus_probe.h b/drivers/xen/xenbus/xenbus_probe.h
index 9b1de4e34c64..bb4f92ed8730 100644
--- a/drivers/xen/xenbus/xenbus_probe.h
+++ b/drivers/xen/xenbus/xenbus_probe.h
@@ -53,9 +53,7 @@ extern int xenbus_match(struct device *_dev, struct device_driver *_drv);
 extern int xenbus_dev_probe(struct device *_dev);
 extern int xenbus_dev_remove(struct device *_dev);
 extern int xenbus_register_driver_common(struct xenbus_driver *drv,
-					 struct xen_bus_type *bus,
-					 struct module *owner,
-					 const char *mod_name);
+					 struct xen_bus_type *bus);
 extern int xenbus_probe_node(struct xen_bus_type *bus,
 			     const char *type,
 			     const char *nodename);
@@ -76,4 +74,6 @@ extern void xenbus_otherend_changed(struct xenbus_watch *watch,
 extern int xenbus_read_otherend_details(struct xenbus_device *xendev,
 					char *id_node, char *path_node);
 
+void xenbus_ring_ops_init(void);
+
 #endif
diff --git a/drivers/xen/xenbus/xenbus_probe_backend.c b/drivers/xen/xenbus/xenbus_probe_backend.c
index c3c7cd195c11..257be37d9091 100644
--- a/drivers/xen/xenbus/xenbus_probe_backend.c
+++ b/drivers/xen/xenbus/xenbus_probe_backend.c
@@ -232,15 +232,13 @@ int xenbus_dev_is_online(struct xenbus_device *dev)
 }
 EXPORT_SYMBOL_GPL(xenbus_dev_is_online);
 
-int __xenbus_register_backend(struct xenbus_driver *drv,
-			      struct module *owner, const char *mod_name)
+int xenbus_register_backend(struct xenbus_driver *drv)
 {
 	drv->read_otherend_details = read_frontend_details;
 
-	return xenbus_register_driver_common(drv, &xenbus_backend,
-					     owner, mod_name);
+	return xenbus_register_driver_common(drv, &xenbus_backend);
 }
-EXPORT_SYMBOL_GPL(__xenbus_register_backend);
+EXPORT_SYMBOL_GPL(xenbus_register_backend);
 
 static int backend_probe_and_watch(struct notifier_block *notifier,
 				   unsigned long event,
diff --git a/drivers/xen/xenbus/xenbus_probe_frontend.c b/drivers/xen/xenbus/xenbus_probe_frontend.c
index 2f73195512b4..9c57819df51a 100644
--- a/drivers/xen/xenbus/xenbus_probe_frontend.c
+++ b/drivers/xen/xenbus/xenbus_probe_frontend.c
@@ -230,15 +230,13 @@ static void wait_for_devices(struct xenbus_driver *xendrv)
 			 print_device_status);
 }
 
-int __xenbus_register_frontend(struct xenbus_driver *drv,
-			       struct module *owner, const char *mod_name)
+int xenbus_register_frontend(struct xenbus_driver *drv)
 {
 	int ret;
 
 	drv->read_otherend_details = read_backend_details;
 
-	ret = xenbus_register_driver_common(drv, &xenbus_frontend,
-					    owner, mod_name);
+	ret = xenbus_register_driver_common(drv, &xenbus_frontend);
 	if (ret)
 		return ret;
 
@@ -247,7 +245,7 @@ int __xenbus_register_frontend(struct xenbus_driver *drv,
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(__xenbus_register_frontend);
+EXPORT_SYMBOL_GPL(xenbus_register_frontend);
 
 static DECLARE_WAIT_QUEUE_HEAD(backend_state_wq);
 static int backend_state;
diff --git a/drivers/xen/xenbus/xenbus_xs.c b/drivers/xen/xenbus/xenbus_xs.c
index ede860f921df..d1c217b23a42 100644
--- a/drivers/xen/xenbus/xenbus_xs.c
+++ b/drivers/xen/xenbus/xenbus_xs.c
@@ -532,21 +532,18 @@ int xenbus_printf(struct xenbus_transaction t,
 {
 	va_list ap;
 	int ret;
-#define PRINTF_BUFFER_SIZE 4096
-	char *printf_buffer;
-
-	printf_buffer = kmalloc(PRINTF_BUFFER_SIZE, GFP_NOIO | __GFP_HIGH);
-	if (printf_buffer == NULL)
-		return -ENOMEM;
+	char *buf;
 
 	va_start(ap, fmt);
-	ret = vsnprintf(printf_buffer, PRINTF_BUFFER_SIZE, fmt, ap);
+	buf = kvasprintf(GFP_NOIO | __GFP_HIGH, fmt, ap);
 	va_end(ap);
 
-	BUG_ON(ret > PRINTF_BUFFER_SIZE-1);
-	ret = xenbus_write(t, dir, node, printf_buffer);
+	if (!buf)
+		return -ENOMEM;
+
+	ret = xenbus_write(t, dir, node, buf);
 
-	kfree(printf_buffer);
+	kfree(buf);
 
 	return ret;
 }
@@ -801,6 +798,12 @@ static int process_msg(void)
 		goto out;
 	}
 
+	if (msg->hdr.len > XENSTORE_PAYLOAD_MAX) {
+		kfree(msg);
+		err = -EINVAL;
+		goto out;
+	}
+
 	body = kmalloc(msg->hdr.len + 1, GFP_NOIO | __GFP_HIGH);
 	if (body == NULL) {
 		kfree(msg);
diff --git a/drivers/xen/xenfs/Makefile b/drivers/xen/xenfs/Makefile
index 4fde9440fe1f..b019865fcc56 100644
--- a/drivers/xen/xenfs/Makefile
+++ b/drivers/xen/xenfs/Makefile
@@ -1,4 +1,4 @@
 obj-$(CONFIG_XENFS) += xenfs.o
 
-xenfs-y			  = super.o xenbus.o privcmd.o
+xenfs-y			  = super.o
 xenfs-$(CONFIG_XEN_DOM0) += xenstored.o
diff --git a/drivers/xen/xenfs/super.c b/drivers/xen/xenfs/super.c
index 1aa389719846..a84b53c01436 100644
--- a/drivers/xen/xenfs/super.c
+++ b/drivers/xen/xenfs/super.c
@@ -16,6 +16,8 @@
 #include <xen/xen.h>
 
 #include "xenfs.h"
+#include "../privcmd.h"
+#include "../xenbus/xenbus_comms.h"
 
 #include <asm/xen/hypervisor.h>
 
@@ -82,9 +84,9 @@ static int xenfs_fill_super(struct super_block *sb, void *data, int silent)
 {
 	static struct tree_descr xenfs_files[] = {
 		[1] = {},
-		{ "xenbus", &xenbus_file_ops, S_IRUSR|S_IWUSR },
+		{ "xenbus", &xen_xenbus_fops, S_IRUSR|S_IWUSR },
 		{ "capabilities", &capabilities_file_ops, S_IRUGO },
-		{ "privcmd", &privcmd_file_ops, S_IRUSR|S_IWUSR },
+		{ "privcmd", &xen_privcmd_fops, S_IRUSR|S_IWUSR },
 		{""},
 	};
 	int rc;
diff --git a/drivers/xen/xenfs/xenfs.h b/drivers/xen/xenfs/xenfs.h
index b68aa6200003..6b80c7779c02 100644
--- a/drivers/xen/xenfs/xenfs.h
+++ b/drivers/xen/xenfs/xenfs.h
@@ -1,8 +1,6 @@
 #ifndef _XENFS_XENBUS_H
 #define _XENFS_XENBUS_H
 
-extern const struct file_operations xenbus_file_ops;
-extern const struct file_operations privcmd_file_ops;
 extern const struct file_operations xsd_kva_file_ops;
 extern const struct file_operations xsd_port_file_ops;